基于三维点云的定位方法、装置、设备及存储介质与流程

1.本技术涉及自动驾驶技术领域，具体而言，本技术涉及一种基于三维点云的定位方法、装置、设备及存储介质。


背景技术：

2.基于高精度地图的定位过程中，由于地图的非实时性与传感器噪声，实时采集的数据与地图之间不一致的存在问题。这一问题通常会给基于地图匹配的定位算法的精度带来影响，甚至造成局部最优或者定位无法收敛的情况。针对现有的问题，目前出现了许多融合不同传感器进行的定位，融合的方法大都基于kf(卡尔曼滤波)，然而在现实车辆运动过程中，并非属于线性系统，同时由于点云存在有噪声，使用经典的配准算法，如icp或者ndt，存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确。


技术实现要素：

3.本技术的目的旨在至少能解决上述的技术缺陷之一，特别是现有技术存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确的技术缺陷。
4.第一方面，提供了一种基于三维点云的定位方法，包括：
5.获取车辆的组合导航数据和激光雷达测量的点云数据；
6.对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；
7.基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；
8.对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；
9.采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。
10.作为本技术一种可能的实施方式，在该实施方式中，所述车辆的激光雷达位姿信息为：
11.x＝[l
t
,w
t
,v
t
]
t
[0012]
其中，l＝(l
x
,l
y
,l
z
)
t
用于表示所述激光雷达的位置信息，其中l
x
为激光雷达x轴坐标信息，l
y
为激光雷达y轴坐标信息，l
z
为激光雷达z轴坐标信息；用于表示所述激光雷达的欧拉角，其中为俯仰角，θ为横滚角，ψ为航向角，v＝(v
x
,v
y
,v
z
)
t
用于表示所述激光雷达的速度，其中v
x
为x轴速度，v
y
为y轴速度，v
z
为z轴速度。
[0013]
作为本技术一种可能的实施方式，在该实施方式中，所述车辆的当前姿态信息为：
[0014]
u＝[w
t
,a
t
]
t
[0015]
其中，w＝(w
x
,w
y
,w
z
)
t
用于表示所述车辆位姿的角速度，其中w
x
为x轴的角速度，w
y
为y轴的角速度，w
z
为z轴的角速度；a＝(a
x
,a
y
,a
z
)
t
用于表示所述车辆位姿的加速度，其中a
x
为x轴的加速度，a
y
为y轴的加速度，a
z
为z轴的加速度。
[0016]
作为本技术一种可能的实施方式，在该实施方式中，所述车辆的位姿状态计算方程为：
[0017][0018]
其中，表示车辆位姿速度状态方程，为车辆速度，表示车辆位姿角速度状态方程，其中w表示车辆位姿角速度；
[0019]
为方向余弦矩阵；为状态转移矩阵；为重力矩阵，其中g为重力加速度。
[0020]
作为本技术一种可能的实施方式，在该实施方式中，所述基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配之前，还包括：
[0021]
对所述激光雷达测量的点云数据进行降采样处理，并对降采样处理后的所述点云数据进行特征提取。
[0022]
作为本技术一种可能的实施方式，在该实施方式中，所述采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，包括：
[0023]
根据公式(p

)
‑1＝k
·
p
‑1 (1
‑
k)
·
c
t
r
‑1c计算先验估计协方差；
[0024]
确定姿态预测方程为
[0025]
确定卡尔曼增益为k＝pc
t
(k
·
r c
t
pc)
‑1；
[0026]
采用公式对激光雷达位姿新进行更新；
[0027]
确定更新后的协方差为p

＝p
‑
(1
‑
k)
‑1·
kcp；
[0028]
其中，p

为先验估计协方差矩阵，p为协方差矩阵，为先验估计位姿，r为系统误差矩阵，c为点云初始配准时的姿态估计信息投影矩阵，f为激光雷达的姿态信息矩阵，k为帧间点云的匹配度，帧间点云的匹配度，其中，p
j
表示当前帧点云中的一个点，pi表示地图与中pj最近的对应点。
[0029]
第二方面，提供了一种基于三维点云的定位装置，该装置包括：
[0030]
数据获取模块，用于获取车辆的组合导航数据和激光雷达测量的点云数据；
[0031]
矩阵确定模块，用于对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；
[0032]
位姿确定模块，用于基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；
[0033]
方程确定模块，用于对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；
[0034]
定位模块，用于采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。
[0035]
第三方面，提供了一种电子设备，该电子设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述的基于三维点云的定位方法。
[0036]
第四方面，提供了一种计算机可读存储介质，所述存储介质存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述的基于三维点云的定位方法。
[0037]
本技术实施例通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的形式规律，保证车辆定位的准确性。
附图说明
[0038]
为了更清楚地说明本技术实施例中的技术方案，下面将对本技术实施例描述中所需要使用的附图作简单地介绍。
[0039]
图1为本技术实施例提供的一种基于三维点云的定位方法的流程示意图；
[0040]
图2为本技术实施例提供的一种定位方法的流程框图；
[0041]
图3为本技术实施例提供的一种基于三维点云的定位装置的结构示意图；
[0042]
图4为本技术实施例提供的一种电子设备的结构示意图。
[0043]
结合附图并参考以下具体实施方式，本技术各实施例的上述和其他特征、优点及方面将变得更加明显。贯穿附图中，相同或相似的附图标记表示相同或相似的元素。应当理解附图是示意性的，原件和元素不一定按照比例绘制。
具体实施方式
[0044]
下面详细描述本技术的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本技术，而不能解释为对本技术的限制。
[0045]
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本技术的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措
辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
[0046]
为使本技术的目的、技术方案和优点更加清楚，下面将结合附图对本技术实施方式作进一步地详细描述。
[0047]
本技术提供的基于三维点云的定位方法、装置、电子设备和计算机可读存储介质，旨在解决现有技术的如上技术问题。
[0048]
下面以具体地实施例对本技术的技术方案以及本技术的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本技术的实施例进行描述。
[0049]
本技术实施例中提供了一种基于三维点云的定位方法，如图1所示，该方法包括：
[0050]
步骤s101，获取车辆的组合导航数据和激光雷达测量的点云数据；
[0051]
步骤s102，对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；
[0052]
步骤s103，基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；
[0053]
步骤s104，对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；
[0054]
步骤s105，采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。
[0055]
本技术实施例中，提供的基于三维点云的定位方法适用于自动驾驶或导航技术领域，基于高精度地图的定位过程中，由于地图的非实时性与传感器噪声，实时采集的数据与地图之间不一致的存在问题。这一问题通常会给基于地图匹配的定位算法的精度带来影响，甚至造成局部最优或者定位无法收敛的情况。针对现有的问题，目前出现了许多融合不同传感器进行的定位，融合的方法大都基于kf(卡尔曼滤波)，然而在现实车辆运动过程中，并非属于线性系统，同时由于点云存在有噪声，使用经典的配准算法，如icp或者ndt，存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确，为此提出一种基于ekf(扩展卡尔曼滤波)点云的定位方法。
[0056]
为方便说明，以一个具体实施例为例，激光雷达和gps组合导航安装在自动驾驶的车辆上，或者具有自动导航功能的车辆上，车上传感器接收组合导航数据和激光雷达测量的点云数据，并分别对组合导航数据和激光雷达测量的点云数据进行处理，其中，对于组合导航数据，先进行解析，得到当前时刻车辆的位姿信息，其中该位姿信息z0＝(x0,y0,z0,yaw0,pitch0,roll0)作为车辆的初始定位位姿信息，然后根据当前时刻的位姿信息计算得到激光雷达测量的点云数据中该车辆的点云定位中点云变换矩阵r0和平移矩阵t0，然后基于该变换矩阵和平移矩阵将当前帧点云与预先建立的地图进行匹配，确定所述车辆的激光雷达位姿信息
[0057]
x＝[l
t
,w
t
,v
t
]
t
[0058]
其中，l＝(l
x
,l
y
,lz)
t
用于表示所述激光雷达的位置信息，用于表示所述激光雷达的俯仰角、横滚角、以及航向角的欧拉角，v＝(v
x
,v
y
,vz)
t
用于表示所述激光雷达的速度。
[0059]
对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程：
[0060]
为方向余弦矩阵；为状态转移矩阵；为重力矩阵。
[0061]
采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，包括：根据公式(p

)
‑1＝k
·
p
‑1 (1
‑
k)
·
c
t
r
‑1c计算先验估计协方差；确定姿态预测方程为确定卡尔曼增益为k＝pc
t
(k
·
r c
t
pc)
‑1；采用公式对激光雷达位姿新进行更新；确定更新后的协方差为p

＝p
‑
(1
‑
k)
‑1·
kcp；其中，p为先验估计协方差矩阵，r为系统误差矩阵，c为点云初始配准时的姿态估计信息投影矩阵，f为激光雷达的姿态信息矩阵，k为帧间点云的匹配度，其中，p
j
表示当前帧点云中的一个点，p
i
表示地图与中p
j
最近的对应点。
[0062]
本技术实施例通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的形式规律，保证车辆定位的准确性。
[0063]
作为本技术一种可能的实施方式，在该实施方式中，对所述组合导航数据进行解析得到所述车辆的当前姿态信息，得到车辆的当前姿态信息为
[0064]
u＝[w
t
,a
t
]
t
[0065]
其中，w＝(w
x
,w
y
,w
z
)
t
用于表示所述车辆位姿的角速度，a＝(a
x
,a
y
,a
z
)
t
用于表示所述车辆位姿的加速度。
[0066]
作为本技术一种可能的实施方式，在该实施方式中，所述基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配之前，还包括：
[0067]
对所述激光雷达测量的点云数据进行降采样处理，并对降采样处理后的所述点云数据进行特征提取。
[0068]
在本技术实施例中，车辆获取激光雷达测量的点云数据和组合导航数据，并且对这两组数据进行不同处理。其中对获取的点云数据进行降采样处理，进行降采样的目的是滤除掉距离激光雷达过远的点以及距离激光雷达中心过远的点，提高配准的精度以及效率。接着对点云数据进行特征点提取：一次扫描的点通过曲率值来分类，特征点曲率大于阈值的为边缘点，特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中，将一
次扫描划分为4个独立的子区域，每个子区域最多提供2个边缘点和4个平面。此外，将不稳定的特征点(瑕点)排除。同时对获取的组合导航的数据进行数据解析，得到gps数据，对gps数据进行utm变换，即根据gps数据进行经纬度到本地坐标的转换，变换得到的结果用于计算初始点云配准的旋转矩阵r和平移矩阵t。待当前帧点云已经匹配到预先构建的地图时，就可以计算出当前车辆的激光雷达位姿测量值，同时也不再需要gps数据。
[0069]
在本技术实施例中，如图2所示，在对激光雷达测量的点云数据和组合导航数据进行处理之后，需要进行ekf融合，建立扩展卡尔曼(ekf)状态和观测状态模型，车辆的激光雷达姿态信息x＝[l
t
,w
t
,v
t
]
t
，其中，l＝(l
x
,l
y
,l
z
)
t
用于表示所述激光雷达的位置信息，用于表示所述激光雷达的俯仰角、横滚角、以及航向角的欧拉角，v＝(v
x
,v
y
,v
z
)
t
用于表示所述车辆的速度。经过组合导航原始数据解析后可以得到姿态信息u＝[w
t
,a
t
]
t
，其中，w＝(w
x
,w
y
,w
z
)
t
用于表示所述激光雷达的角速度，a＝(a
x
,a
y
,a
z
)
t
用于表示所述激光雷达的加速度。接着可以建立车辆位姿估计状态方程：
[0070][0071]
其中e
w
为状态转移矩阵，r
w
为方向余弦矩阵，g代表重力，在车辆运动过程中，组合导航和激光雷达导航x轴的方向保持一致，所以相应的矩阵为：
[0072][0073][0074][0075]
最后进行ekf的预测和更新：先验估计协方差：(p

)
‑1＝k
·
p
‑1 (1
‑
k)
·
c
t
r
‑1c；激光雷达姿态预测方程：雷达姿态预测方程：卡尔曼增益：k＝pctk
·
r ctpc
‑
1；lidar姿态更新：更新后验估计协方差：p

＝p
‑
(1
‑
k)
‑1·
kcp；其中p为先验估计协方差矩阵，r为系统误差矩阵，c为点云初始配准时的姿态估计信息投影矩阵，f为激光雷达输出的姿态信息矩阵，k为帧间点云的匹配度，p
j
表示当前帧点云中的一个点，p
i
表示地图中与p
j
最近的对应点；根据ekf最后得到激光雷达测量的姿态的最优估计值经过坐标变换得到当前车辆的定位信息。
[0076]
本技术实施例通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确
定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的形式规律，保证车辆定位的准确性。
[0077]
本技术实施例提供了一种基于三维点云的定位方法、装置、设备及存储介质装置，如图3所示，该基于三维点云的定位装置30可以包括：数据获取模块301、矩阵确定模块302、位姿确定模块303、方程确定模块304、以及定位模块305，其中，
[0078]
数据获取模块301，用于获取车辆的组合导航数据和激光雷达测量的点云数据；
[0079]
矩阵确定模块302，用于对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；
[0080]
位姿确定模块303，用于基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；
[0081]
方程确定模块304，用于对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；
[0082]
定位模块305，用于采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。
[0083]
作为本技术一种可能的实施方式，在该实施方式中，所述车辆的当前姿态信息为：
[0084]
u＝[w
t
,a
t
]
t
[0085]
其中，w＝(w
x
,w
y
,w
z
)
t
用于表示所述车辆位姿的角速度，a＝(a
x
,a
y
,a
z
)
t
用于表示所述车辆位姿的加速度。
[0086]
作为本技术一种可能的实施方式，在该实施方式中，所述车辆的位姿状态计算方程为：
[0087][0088]
其中，
[0089]
为方向余弦矩阵；为状态转移矩阵；为重力矩阵。
[0090]
作为本技术一种可能的实施方式，在该实施方式中，所述基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配之前，还包括：
[0091]
对所述激光雷达测量的点云数据进行降采样处理，并对降采样处理后的所述点云
integrated circuit，专用集成电路)，fpga(field programmable gate array，现场可编程门阵列)或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本技术公开内容所描述的各种示例性的逻辑方框，模块和电路。处理器4001也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，dsp和微处理器的组合等。
[0104]
总线4002可包括一通路，在上述组件之间传送信息。总线4002可以是pci(peripheral component interconnect，外设部件互连标准)总线或eisa(extended industry standard architecture，扩展工业标准结构)总线等。总线4002可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
[0105]
存储器4003可以是rom(read only memory，只读存储器)或可存储静态信息和指令的其他类型的静态存储设备，ram(random access memory，随机存取存储器)或者可存储信息和指令的其他类型的动态存储设备，也可以是eeprom(electrically erasable programmable read only memory，电可擦可编程只读存储器)、cd
‑
rom(compact disc readonly memory，只读光盘)或其他光盘存储、光碟存储(包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等)、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。
[0106]
存储器4003用于存储执行本技术方案的应用程序代码，并由处理器4001来控制执行。处理器4001用于执行存储器4003中存储的应用程序代码，以实现前述方法实施例所示的内容。
[0107]
本技术实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，当其在计算机上运行时，使得计算机可以执行前述方法实施例中相应内容。与现有技术相比，本技术实施例通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的形式规律，保证车辆定位的准确性。
[0108]
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
[0109]
以上所述仅是本技术的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本技术原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本技术的保护范围。