基于多传感器融合无人车的相对定位方法、系统及车辆
阅读说明：本技术 基于多传感器融合无人车的相对定位方法、系统及车辆 (Relative positioning method and system based on multi-sensor fusion unmanned vehicle and vehicle ) 是由 闫耀威 王宽 林鑫余 彭祥军 于 2021-08-05 设计创作，主要内容包括：本发明公开了一种基于多传感器融合无人车的相对定位方法、系统及车辆,包括以下步骤：(1)数据接入；(2)定位初始化；(3)IMU积分推算；(4)地图数据库匹配；(5)融合定位建模；(6)融合定位求解。本发明降低了累计误差对定位结果的影响,以及有效地减少了匹配过程中出现的误匹配结果。(The invention discloses a relative positioning method, a relative positioning system and a relative positioning vehicle based on a multi-sensor fusion unmanned vehicle, wherein the relative positioning method comprises the following steps of: (1) data access; (2) positioning initialization; (3) IMU integral calculation; (4) map database matching; (5) fusing, positioning and modeling; (6) and (5) fusion positioning solving. The invention reduces the influence of accumulated errors on the positioning result and effectively reduces the mismatching result in the matching process.)
技术领域
本发明属于定位技术领域，具体涉及一种基于多传感器融合无人车的相对定位方法、系统及车辆。
背景技术
对于自主移动无人车来说，它需具备的一个核心能力是运用传感器来感知周围的环境。无人车从事自主导航工作时，在环境中的实时定位一直是一个热门的研究问题。传统的定位方法是通过安装在无人车轮子上的编码器所检测的数据来实时推算出无人车的当前位姿。这种方法简单易用但是存在其固有的累积误差。机器人在地面移动时不可避免地会出现打滑情况，以及系统设计制造过程中存在的误差，这些误差随着机器人的移动会不断累积，运行一段时间后位姿的计算结果将会出现无法校正和应用的情况，因而这种方法有相应的局限性。
在机器人定位的研究过程中，人们逐渐采用激光雷达扫描匹配的技术来修正里程计在位姿计算过程中所产生的偏差。扫描匹配是将当前时刻的激光雷达扫描结果同前一时刻的扫描结果相匹配，通过匹配可得到两组激光点云的位姿变化关系，进而确定机器人本体在环境中的位姿。这种方法较好地弥补了里程计定位所存在的累积误差结果，通常在雷达具有本身精度较高的情况下，定位结果精度也较高。然而激光扫描匹配的方法也有一定的局限性，对于扫描的环境必须包含较多的路标特征点，同时路标特征不能高度相似，否则会出现误匹配的情况。
因此，有必要开发一种新的基于多传感器融合无人车的相对定位方法、系统及车辆。
发明内容
本发明的目的是提供一种基于多传感器融合无人车的相对定位方法、系统及车辆，能降低累计误差对定位结果的影响，以及能有效减少匹配过程中出现的误匹配结果。
第一方面，本发明所述的一种基于多传感器融合无人车的相对定位方法，包括以下步骤：
步骤1.数据接入；
实时接收GPS、IMU、轮速计和激光雷达所输出的数据，对每一帧的数据附上当前时刻的时间戳信息，并分别存入对应的缓存区中；
步骤2.定位初始化；
从GPS缓存区弹出两帧GPS数据，基于两帧GPS数据来判断车辆是否发生移动，若是，则计算出当前车辆的位置P、速度V和航向角；
步骤3.IMU积分推算；
根据IMU动力学公式，通过使用IMU的线速度和角速度积分得到每个IMU时刻的IMU状态(R、p、V、ba、bg)，其中R表示姿态旋转量，p表示平移量，ba表示陀螺仪的偏差，bg表示加速度计的偏差；
根据历史定位姿态和IMU积分得出的位姿(R、p)对实时的激光雷达点云做畸变矫正；
步骤4.地图数据库匹配；
根据IMU积分推算的平移量p，使用KNN查找地图数据库中与其最近的地图帧，使用NDT匹配算法计算出实时雷达帧和查找到的地图帧之间对应的位姿(R,p)，并将其作为一元因子添加到因子图模型计算图中；
步骤5.融合定位建模；
根据接收到的GPS信号，并将其作为一元Factor添加到因子图模型计算图中；
根据接收到的轮速信号，并将其作为一元Factor添加到因子图模型计算图中；
将IMU状态(R，p，v，ba，bg)作为二元Factor添加到因子图模型计算图中；
步骤6.融合定位求解；
将所有数据的因子发送到iSAM2模块中,得到最后的本车在地图中的横纵向坐标及航向角。
可选地，使用两帧大于0.1米的GPS数据，通过前一帧减去后一帧的位移再除以时间，得到速度，通过X方向和Y方向上的速度向量求出航向角；将计算出的速度用作IMU的初始化速度，并把GPS坐标发送到NDT数据库，用于搜索距离最近一帧NDT数据的bin文件；搜索到以后，使用当前帧和地图做配准，得出当前车辆的位置P、速度V和航向角，同时将计算出的位置P及航向角作为IMU的初始化位姿,同时速度V一起作为主线的IMU初始化参数。
可选地，求每个时刻的IMU状态向量，具体为：
首先查找两个时刻之间的IMU数据，创建一个6*1的矩阵，包括加速度和角速度，并将两帧数据的时间差送入因子图模型中的imu_preintegrated函数来做积分,根据每进来一帧的IMU数据来预测IMU的状态(R、p、V、ba、bg)。
可选地，所述步骤4，具体为：
建图过程：输入一条轨迹，即通过imufactor计算得出的定位结果{Ri，pi}i＝1....N；其中，Ri表示旋转向量，pi表示平移向量；以及对应时刻的雷达点云帧：{Li}i＝1....N，其中，Li表示局部坐标系下的激光雷达每一帧的位姿信息，将每一帧的点云位姿转换到世界坐标系下，即其中，表示局部坐标系转换到世界坐标系下的雷达每一帧的位姿信息，pi表示坐标转换时的平移量，最终得到激光雷达的点云地图；
地图匹配：使用NDT来匹配，即以当前点为中心，将参考点云所占的空间划分成指定大小的网格或者体素；将激光雷达的扫描点落在小格子当中的统计下来，当每个格子收集到的激光点不少于4个时，计算每个网格的多维正态分布参数，求出对应格子的均值和协方差；然后通过IMU得到的里程计数据赋值给变换参数，对于要配准的点云，通过变换矩阵T将其转换到参考点云的网格中，即其中，表示配准后坐标转换到地图下的点云坐标，表示未转换坐标的激光雷达点云坐标，Lnew表示局部坐标系下的激光雷达点云坐标；计算位于格子内的点云的正态分布PDF参数：p(k)＝exp[-0.5(k-q)t∑-1(k-q)]，其中，k表示一个网格内所有的扫描点，q表示正态分布的均值；对每个网格计算出的概率密度相加得到NDT配准的得分，再根据牛顿优化算法对目标函数score进行优化，即寻找变换参数使得得分值最优，得到最后的定位NDT定位结果；接到GPS第一帧的坐标信息以后，开始寻找距离这帧坐标最近的地图帧，找到后将地图帧保存到容器中，然后用去除点云畸变坐标转换后的当前激光雷达数据同地图帧做配准，得到配准后的相对位姿，并将得到后的位姿作为一元因子传到因子图模型计算图中。
第二方面，本发明所述的一种基于多传感器融合无人车的相对定位系统，包括存储器和控制器，所述控制器与存储器连接，所述存储器内存储有计算机可读程序，所述控制器调用该计算机可读程序时，能执行如本发明所述的基于多传感器融合无人车的相对定位方法的步骤。
第三方面，本发明所述的一种车辆，采用如本发明所述的基于多传感器融合无人车的相对定位系统。
本发明具有以下优点：本发明能够对以往里程计系统定位所存在的固有约束进行提高，降低了累计误差对定位结果的影响；本发明所运用的激光点云扫描匹配与IMU、GPS、里程计定位相结合，针对大范围环境或者环境路标特征相似度较高的情况，能够有效地减少匹配过程中出现的误匹配结果。
附图说明
图1为本实施例中各传感器相互关系以及处理流程；
图2为本实施例中算法模块中各传感器数据在因子图模型中的处理方式；
图3为本实施例中基于GPS轨迹使用NDT算法建成的点云地图；
图4为本实施例中定位轨迹与高精度GPS轨迹对比图(已经全部重合)；
图5为现有单激光雷达相对定位结果与高精度GPS轨迹对比图(误差较大)。
具体实施方式
下面结合附图对本发明作进一步说明。
如图1所示，本实施例中，一种基于多传感器融合无人车的相对定位方法，包括以下步骤：
(1)数据接入；
分别接入GPS、IMU、轮速计和激光雷达所输出的数据，对每一帧的数据附上当前时刻的时间戳信息，并分别储存到对应的缓存区中。
(2)定位初始化；
IMU初始化，车辆首先静止50秒以上。GPS初始化，初始化使用两帧大于0.1米的GPS数据，前一帧减去后一帧的位移除以时间，可以算出来速度，通过X和Y方向上的速度向量，即可求出航向角yaw。如果没有轮速速度传入的话，会使用GPS速度来替换轮速加入。同时计算出的速度用作IMU的初始化速度，并把GPS坐标发送到NDT(正态分布变换建图)数据库，用来搜索距离最近一帧NDT数据的bin文件。搜索到以后，使用当前帧和地图做配准，从而得出当前车辆的位置P(即横纵向定位结果x,y)、速度V和航向角，同时将计算出的位置P及航向角作为IMU的初始化位姿,同时将前面GPS计算出的速度V一起作为主线的IMU初始化参数。
(3)IMU积分推算；
IMU得到初始化参数以后，就要开始求每个时刻的IMU状态向量。由于IMU在瞬时状态精准度比较高，但是积分误差非常大，IMU推算位姿方法，首先查找两个时刻之间的IMU数据，创建一个6*1的矩阵，包括加速度和角速度，并将两帧数据的时间差送入因子图模型中的imu_preintegrated函数来做积分，根据每进来一帧的IMU数据来预测IMU的状态(R、p、V、ba、bg)，同时需要其他传感器作为观测量来不断地校准，使用因子图模型中的imufactor函数来计算IMU累加值即得到里程计信息。根据历史定位姿态和IMU积分得出的位姿(R、p)对实时的激光雷达点云做畸变矫正。
(4)地图数据库匹配；
建图过程：输入一条轨迹，也即通过imufactor计算得出的定位结果{Ri，pi}i＝1....N；其中，Ri表示旋转向量，pi表示平移向量；以及对应时刻的雷达点云帧：{Li}i＝1....N，其中，Li表示局部坐标系下的激光雷达每一帧的位姿信息，将每一帧的点云位姿转换到轨迹的坐标系下，即其中，表示局部坐标系转换到世界坐标系下的雷达每一帧的位姿信息，pi表示坐标转换时的平移量，最终得到激光雷达的点云地图。
地图匹配：使用NDT即正态分布变换算法来匹配；NDT的大致思想是以当前点为中心，将参考点云所占的空间划分成指定大小的网格或者体素；例如(10cm)×(10cm)×(10cm)的小格子，激光雷达的扫描点落在小格子当中的被统计下来，只要满足每个格子收集到的激光点不少于4个，计算每个网格的多维正态分布参数，求出对应格子的均值和协方差。然后使用上面通过IMU得到的里程计数据赋值给变换参数，对于要配准的点云，通过变换矩阵T将其转换到参考点云的网格中，即其中，表示配准后坐标转换到地图下的点云坐标，表示未转换坐标的激光雷达点云坐标，Lnew表示局部坐标系下的激光雷达点云坐标。计算位于格子内的点云的正态分布PDF参数：p(k)＝exp[-0.5(k-q)t∑-1(k-q)]，其中，k表示一个网格内所有的扫描点，q表示正态分布的均值。每个PDF可以认为是局部平面的近似，描述了平面位置、朝向、形状和平滑性等特征。在对每个网格计算出的概率密度相加得到NDT配准的得分(score)，再根据牛顿优化算法对目标函数score进行优化，即寻找变换参数使得score值最最优，得到最后的定位NDT定位结果。接到GPS第一帧的坐标信息以后，开始寻找距离这帧坐标最近的地图帧，找到后将地图帧保存到容器中，然后用去除点云畸变坐标转换后的当前激光雷达数据同地图帧做配准，过程就是求解当前帧和NDT地图中每个格子中的高斯分布关系。得到配准后得到的相对位姿。将得到后的位姿作为一元因子传到因子图模型中，作为调整IMU数据的一元因子。
(5)融合定位建模；
GPS接收到的数据也作为的IMU的位置观测量添加到因子图模型计算图中，同时自建一个一元因子来用于轮速计的数据，轮速计信息主要用做观测量，来推断本车的位置信息，同GPS信息做融合，作为一个参数不断的纠正IMU的位置信息。
(6)融合定位求解；
将所有数据的因子发送到iSAM2模块(长期滑动窗),得到最后的本车在地图中的横纵向坐标及航向角。
如图3所示，为本实施例中基于GPS轨迹使用NDT算法建成的点云地图。
如图4所示，为本实施例中定位轨迹与高精度GPS轨迹对比图，从图4中可看出，定位轨迹和高精度GPS轨迹已经全部重合，说明采用本方法的定位精度高，且稳定。
如图5所示，为现有单激光雷达相对定位结果与高精度GPS轨迹对比图，从图中可看出，单激光雷达相对定位结果的误差较大。
本实施例中，一种基于多传感器融合无人车的相对定位系统，包括存储器和控制器，所述控制器与存储器连接，所述存储器内存储有计算机可读程序，所述控制器调用该计算机可读程序时，能执行如本实施例中所述的基于多传感器融合无人车的相对定位方法的步骤。
本实施例中，一种车辆，采用如本实施例中所述的基于多传感器融合无人车的相对定位系统。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：地图检测方法和装置