一种基于激光雷达和先验地图的实时定位系统及方法

1.本发明涉及无人驾驶车辆定位技术领域，特别是涉及一种基于激光雷达和先验地图的实时定位系统及方法。


背景技术：

2.智能网联汽车及自动驾驶技术是《中国制造2025》的重要领域，近年来其相关领域和技术得到了快速发展，其中就包括车辆定位技术。作为车辆环境感知的重要组成部分，同时也是一切自动驾驶技术的出发点，车辆定位技术面临着越来越高的要求。
3.现有车辆可选的定位方案包括：全球卫星导航系统gnss、地面站定位系统、车辆惯性导航系统imu、车轮定位技术、二维码定位技术、wifi指纹定位技术、基于激光雷达的建图和定位技术(lidar
‑
slam)、地图配准(mm)定位技术等方案。各种方法的优缺点描述如下：
4.(1)全球卫星导航系统gnss、地面站定位系统在开阔地带可以实现车辆的厘米级的高精度实时定位；但是不适用在高楼群、隧道、地下停车场等相对封闭的环境中，因信号遮挡问题。
5.(2)车载惯性导航系统imu依靠内置的加速计和陀螺仪得出车辆的实时位置，但imu比较容易受到外界环境干扰，如磁场、温度，此外imu的漂移误差问题也同样严重。
6.(3)车轮定位技术可以依靠车轮转速和行驶中的车轮角度变化来计算车辆位置，对路面平整情况要求严格，在凹凸不平的地面环境中误差较大，此外还存在车轮打滑、漂移误差等难以解决的难题。
7.(4)二维码定位技术依靠二维码图片来实现车辆定位，技术简单快速，车辆扫描到二维码的同时，即可快速计算车辆的真实位置；但如果要满足车辆定位实时性的要求，则需要保证驾驶场景中有足够的二维码图片密度，同时，二维码图片的识别也不是完美可靠的，易受障碍物遮挡、环境光照因素影响。
8.(5)wifi指纹定位技术同样需要大量的前期布置计算，并且同时为所有非开阔地带布置wifi是不切实际的。
9.(6)w基于激光雷达的建图和定位技术(lidar
‑
slam)由机器人领域扩展而来，可以同时实现车辆定位和周围地图构建，但是缺点在车辆领域十分致命：它只能保证短距离或短时间内定位的实时性和定位精度，系统运行的时间越长其漂移误差越大，通常需要轨迹闭环来进行矫正；
10.(7)地图配准(mm)定位可以保证车辆长时间内的定位精度，但是其短时间内的实时性和定位精度无法保证。
11.面向未来智能网联汽车和自动驾驶技术应用需求达到“车辆在封闭环境下定位保证10hz及以上的定位频度以及至少车道级以内的定位精度”这一技术问题，目前尚不存在有效的解决方案。


技术实现要素：

12.针对上述现有技术存在的问题，本发明提出了一种基于激光雷达和先验地图的实时定位系统及方法，实现了基于激光雷达里程计和先验地图就能车道级的车辆定位，从而改善了在封闭环境下的车辆定位问题。
13.本发明的一种基于激光雷达和先验地图的实时定位系统，该系统包括点云注册模块100、激光雷达里程计模块200、地图配准定位模块300和定位融合模块400；其中：
14.所述点云注册模块100，该模块用于提取激光雷达点云帧中的特征点云；
15.所述激光雷达里程计模块200，该模块利用激光雷达计算两帧特征点云之间的变换关系进行车辆航迹推算，得到10hz频率的航迹推算结果；
16.所述地图配准定位模块300，该模块用于从点云注册模块100产生的特征点云中选取特征点最多的一帧作为关键点云帧；利用将该关键点云帧与已建好的先验地图进行配准，得到配准频率为1hz的矫正定位结果；
17.定位融合模块400，该模块将定位频率为10hz的激光雷达里程计航迹推算结果与1hz的地图配准定位结果进行融合计算，得到定位频率为10hz以及车道级定位精度的车辆定位结果。
18.本发明的一种基于激光雷达和先验地图的实时定位方法，该方法包括以下步骤：
19.步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧注册，提取为特征点云数据；
20.步骤2：进行第t个时间戳激光雷达里程计，具体为在第t个时间戳激光雷达里程计模块接收到特征点云，之后，计算连续两帧特征点的变换关系进而计算得到相对于初始位置的航迹推算
21.步骤3：进行地图配准定位，具体为以连续帧周期进行地图配准定位，在第k周期，获取地图片段关键帧特征点云和里程计的航迹推算结果后，通过ndt配准得出车辆相对准确的定位
22.步骤4：进行定位融合，即在第t个时间戳将定位频率为10hz的航迹推算结果与1hz的地图定位结果融合计算，得到定位频率为10hz的车辆定位
23.与现有技术相比，本发明能够达成以下技术效果：
24.1)在无卫星信号和基站信号的封闭环境中，实现车道级的车辆定位；
25.2)车辆定位的频率在10hz左右，与激光雷达固有频率持平，满足自动驾驶实时性的要求；
26.3)实现长距离和长时间行驶环境下仍维持在车道级以内的漂移误差；
27.4)前期工作只需制作一份先验地图即可，无需环境布置工作。
附图说明
28.图1为本发明的基于激光雷达和先验地图的实时定位系统架构示意图；
29.图2为第t个时间戳时的点云注册流程示意图；
30.图3为第t个时间戳激光雷达里程计流程示意图；
31.图4为通过kd
‑
tree寻找相邻帧间的特征点对应示意图；
32.图5为点线距离与点面距离示意图；
33.图6为第t个时间戳地图配准定位流程示意图；
34.图7为第t个时间戳定位融合流程示意图。
具体实施方式
35.下面结合附图和具体实施例对本发明技术方案作进一步详细描述。
36.如图1所示，为本发明的基于激光雷达和先验地图的实时定位系统架构示意图。该系统包括四个节点，分别是点云注册模块100、激光雷达里程计模块200、地图配准定位模块300和定位融合模块400。其中：
37.点云注册模块100，该模块用于提取激光雷达点云帧中的特征点云，所述特征点云用于近似表示车辆周围环境，降低原始点云帧中的点云规模以及减少计算量；每秒产生的10帧特征点云，特征点云的产生频率为10hz；
38.激光雷达里程计模块200，该模块利用激光雷达计算两帧特征点云之间的变换关系进行车辆航迹推算，得到基于10hz频率的航迹推算结果(该航迹推算结果的漂移误差会不断增大)；10hz的频率代表此模块1秒产生10次航迹推算结果；
39.地图配准定位模块300，该模块用于从点云注册模块100每秒产生的10帧特征点云中选取特征点最多的一帧作为关键点云帧；然后将该关键点云帧与已建好的先验地图通过正态分布变换(ndt)进行配准，得到基于配准频率为1hz的地图配准定位结果，该定位结果具有车道级的定位精度；1hz的频率代表此模块1秒只产生1次地图配准定位结果；
40.定位融合模块400，该模块将10hz的激光雷达里程计航迹推算结果与1hz的地图配准定位结果进行融合计算，得到频率为10hz以及车道级定位精度的车辆定位结果。
41.本发明的基于激光雷达和先验地图的实时定位方法根据激光雷达传感器数据和先验地图数据，获取频率为10hz以及车道级精度的车辆定位，具体实现包括以下流程：
42.步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧提取为特征点云数据，如图2所示，为第t个时间戳时的点云注册过程示意图。该步骤具体包括以下处理：
43.步骤1.1：去除无效点，即去除激光雷达传感器产生的点云帧中的无效点，记录原始数据时间戳t(t＝1,2,
…
)，以此时间戳作为航迹推算结果和最终车辆定位结果的时间戳；
44.步骤1.2：划分点云层id，即根据点云中每个点的俯仰角α，将点云中具有相同俯仰角α的点划分到同一个层id中；设激光雷达的垂直视角为
±
θ，垂直分辨率为β，线数为n，它们满足关系对其中任意一点对应的坐标值(x,y，z)，计算其俯仰角度α以及对应的层id，表达式如下：
[0045][0046]
[0047]
如果求得一个点的id不为正整数，视其为无效点予以丢弃；
[0048]
步骤1.3：计算曲率，以曲率作为提取特征点云的依据，即为降低计算复杂度，只利用当前点p
i
(x
i
，y
i
，z
i
)，及在同一层id中其前后各5个点(p
i
‑5，
…
，p
i
‑1,p
i+1
,
…
,p
i+5
)的坐标差的平方和来近似估计曲率k，表达式如下：
[0049]
k
x
＝x
i
‑5+
…
+x
i
‑1+x
i+1
+
…
+x
i+5
‑
10x
i
[0050]
k
y
＝y
i
‑5+
…
+y
i
‑1+y
i+1
+
…
+y
i+5
‑
10y
i
[0051]
k
z
＝z
i
‑5+
…
+z
i
‑1+z
i+1
+
…
+z
i+5
‑
10z
i
[0052][0053]
其中，k
x
,k
y
k
z
分别表示x,y,z方向的曲率，x
i
‑5,
…
,x
i
‑1,x
i+1
,
…
,x
i+5
分别表示当前点前后各5个点的x坐标，y
i
‑5,
…
,y
i
‑1,y
i+1
,
…
,y
i+5
分别表示当前点前后各5个点的y坐标，z
i
‑5,
…
,z
i
‑1,z
i+1
,
…
,z
i+5
分别表示当前点前后各5个点的z坐标；
[0054]
步骤1.4：提取特征点云，即根据每个点对应曲率k的大小，提取四类点云包括轮廓点云次级轮廓点云平面点云和次级平面点云每一个层id分组中，曲率最大的前12个点标记为轮廓点，曲率最大的前120个点标记为次级轮廓点，曲率最小的24个点标记为平面点，剩下的点经过下采样降低到原点云数量的20％，标记为次级平面点。之后将所有层id分组中的特征点分别合并，形成对应的特征点云，发布给激光雷达里程计节点和地图配准定位节点，用于后续计算。
[0055]
步骤2：进行第t个时间戳激光雷达里程计，即在第t个时间戳激光雷达里程计模块接收到特征点云，之后，根据连续两帧特征点的对应关系计算连续两帧特征点的变换关系进而计算得到相对于初始位置的航迹推算如图3所示，为第t个时间戳激光雷达里程计流程示意图。该步骤具体包括以下处理：
[0056]
步骤2.1：构建kd
‑
tree(kd
‑
tree是一种分割k维数据空间的数据结构，在本发明中用于最近点的查找)。kd
‑
tree
‑
ic和kd
‑
tree
‑
if分别为由第t
‑
1个时间戳的次级轮廓点云和次级平面点云构建成的kd
‑
tree结构；
[0057]
步骤2.2：依靠kd
‑
tree寻找不同时间戳的特征点的对应关系，再求得它们之间的几何距离；
[0058]
如图4所示，为通过kd
‑
tree寻找相邻帧间的特征点对应点示例图。通过该示例表明：
[0059]
通过kd
‑
tree
‑
ic找到第t个时间戳轮廓点云中的每一个点(如轮廓点a)在第t
‑
1个时间戳的次级轮廓点云中的2个对应点(最近点i、相邻层id的最近点j)；
[0060]
通过kd
‑
tree
‑
if找到第t个时间戳平面点云中的每一个点(如平面点b)在第t
‑
1个时间戳次级平面点云中的3个对应点(最近点k、次近点l、相邻层id的最近点m)。
[0061]
然后分别计算特征点a、b分别到最近点k组成的线ij、面klm的距离d1及d2。如图5所示，为点线距离与点面距离示意图。
[0062]
距离d1的计算公式如下：
[0063][0064]
其中，和为特征点a到两个对应点i、j的向量，为两个对应点形成的向量。
[0065]
距离d2的计算公式如下：
[0066][0067]
其中，为特征点b到对应点k的向量，为三个对应点间形成的向量；在时间戳序列中，重复进行步骤2.1和2.2，获取对应时间戳t的和
[0068]
步骤2.3：计算相邻时间戳t
‑
1和t之间车辆位姿的变换关系通过牛顿梯度下降法来迭代最小化距离函数和的值，求出变换关系具体推导过程如下：
[0069]
首先构建雅可比矩阵列出高斯牛顿方程：
[0070]
j
t
jδt
l
＝
‑
j
t
d
[0071]
求得的增量δt
l
后，更新及a、b的坐标值(x
a
,y
a
,z
a
)、(x
b
,y
b
,z
b
)，迭代过程如下：
[0072][0073][0074][0075]
之后，使用步骤2.2中距离公式重新计算和构建新的雅可比矩阵j和高斯牛顿方程，迭代进行一定次数后便得到第t个时间戳的位姿变换关系
[0076]
步骤2.4：计算第t个时间戳的航迹推算结果表达式如下：
[0077][0078]
其中，代表在时间戳序列中，第i(i＝1,2,
…
,t)个车辆位姿变换关系。
[0079]
步骤3：进行地图配准定位，即该定位系统以10个连续帧为一个周期进行地图配准定位。在第k周期，获取地图片段关键帧特征点云和里程计的航迹推算结果后，通过ndt配准得出车辆相对准确的定位如图6所示，为第t个时间戳地图配准定位流程示意图。
[0080]
步骤3.1：预测车辆位置根据k
‑
1周期的地图定位结果以及两次地图配准定位之间航迹推算结果的几何变换过程推算关键帧被采集时车辆所在的预测位置，作为ndt配准的原初变换矩阵。表达式为：
[0081][0082]
其中，表示第k
‑
1周期所选关键帧对应的航迹推算结果，表示第k周期所选关键帧对应的航迹推算结果。
[0083]
步骤3.2：获取地图片段根据预测的车辆在地图中的位置，截取地图内的点云片段作为参与ndt配准的地图片段
[0084]
步骤3.3：合并特征点云。将关键帧中的轮廓点云、次级轮廓点云、平面点云和次级平面点云合并，得到一个新的特征点云作为ndt配准的源点云。
[0085]
步骤3.4：ndt配准。根据特征点云、地图点云和预测的车辆位置，使用ndt配准算法得出矫正定位结果即为地图配准定位结果。ndt算法会借助牛顿梯度下降法，最小化特征点云与地图间的差异，同时将更新为此结果与此时刻的里程计结果会被发布给定位融合节点。
[0086]
步骤4：进行定位融合，即在第t个时间戳，将10hz的航迹推算结果与1hz的地图定位结果融合计算，得到10hz的车辆定位如图7所示，为第t个时间戳定位融合流程示意图。
[0087]
接收里程计模块得到的第t个时间戳的航迹推算结果接收地图配准定位模块得到的第k个周期地图定位结果和航迹推算采取推定的方式计算车辆定位结果以地图定位节点的结果为基准，计算地图定位节点的航迹推算数据和激光雷达里程计节点的航迹推算数据之间的几何变换过程再推算得出车辆的实际位置表达式如下：
[0088]