一种路面信息增强的城市场景车载自主导航方法、程序、设备及存储介质

摘要: 本发明属于导航定位,涉及一种车载导航方法,具体涉及一种路面信息增强的城市场景车载自主导航方法、程序、设备及存储介质。背景技术:1、随着城市车辆数量激增,交通事故...
一种路面信息增强的城市场景车载自主导航方法、程序、设备及存储介质

本发明属于导航定位,涉及一种车载导航方法,具体涉及一种路面信息增强的城市场景车载自主导航方法、程序、设备及存储介质。

背景技术:

1、随着城市车辆数量激增,交通事故频发,尤其是在北方一些居住人数较少或不发达地区对冰雪路面的清理不及时、不充分,使得交通事故发生率显著提高。因此在城市的复杂环境下,尤其是在冰雪路面、卫星信号拒止环境及动态干扰等复杂场景中对高精度、高可靠导航的迫切需求,能够提升驾驶安全的车载自主导航和智能驾驶技术成为了关键技术。

2、一方面,由于在车载大尺度场景、冰雪覆盖环境、卫星拒止和动态干扰等复杂场景中,存在着车载智能感知测量点离群、定位性能下降和构图有效性及精度不足等问题。另一方面,由于无人车平台整体性价比、体积等限制了所选用车载传感器性能无法满足上述特殊场景高性能定位及构图需求,影响了无人车平台全天候全陆地应用能力,增大了极端环境天气和夜视场景下的车载平台应用风险。迫切需要考虑车载多传感器信息和道路模型信息间内在联系、数据关联质量和信息利用率,不仅降低车载定位与构图对工作场景、先验地图及卫星信号限制,还增强极端环境天气和夜视场景中车载目标感知与定位及构图精度和鲁棒性,促进车载自主导航解决方案进一步完善。

3、综上所述,本发明提出了一种基于微惯性导航系统(strapdown inertialnavigation system, sins)、视觉传感器和三维激光雷达(light detection andranging, lidar)的多模态融合定位与动态场景表征方法。其核心在于充分利用不同传感器的特点,克服单一传感器在复杂、动态城市环境下的局限性,同时结合路面信息(十字路口、三岔路口、弧形拐弯、t型路口、直行路段等道路模型和冰雪覆盖路面)辅助车载导航系统实现全天候、精准、稳定的导航定位,为车辆在城市全域导航提供持续稳定的位姿信息,实现全地形的高精度高可靠性定位及构图的目标。

4、本发明能够降低车载定位与构图对工作场景、先验地图及卫星信号限制,增强冰雪覆盖路面场景、无先验地图且卫星拒止场景和动态干扰等复杂场景中车载目标感知与定位及构图精度和鲁棒性,促进车载自主导航解决方案进一步完善。

技术实现思路

1、本发明为解决在复杂场景下导航定位精度低的问题,提供一种路面信息增强的城市场景车载自主导航方法。

2、本发明提供一种路面信息增强的城市场景车载自主导航方法,包括以下步骤:

3、采集相机环境图像和激光雷达点云观测数据;针对车辆处于非匀速直线运动场景,采用scan context算法对关键帧点云进行场景特征匹配,完成回环检测;

4、对非地面边缘点聚类并拟合直线,选取最长直线作为时间点云参考线;同步采用激光雷达与sins分别解算当前点位置至参考线距离,获得距离差值作为激光雷达的观测向量;

5、视觉里程计获取载体位姿,结合外参转换得到相机位姿;将其与sins的估计姿态差值作为相机的观测向量;

6、若检测到当前载体位置为回环点时,根据视觉里程计和sins计算的历史关键帧到当前关键帧的相对旋转矩阵与平移向量构建回环观测向量;

7、融合多源传感器观测信息,在卡尔曼滤波框架下基于对应的观测方程执行量测更新,对先验状态估计进行修正,输出载体运动系统后验状态。

8、进一步的,所述回环检测为:

9、当激光雷达点云识别到路口、相机识别到冰雪路面、相机识别到交通标志和相机识别不到两侧车道线时,提取关键帧点云与历史帧进行场景特征匹配;

10、将点云按径向距离分为nr个环、按方位角分为ns个扇区,环与扇区相交得到bin网格单元;

11、提取各个bin内全部点云的最大高度作为单元表征值完成bin编码,所有环与扇区组合形成尺寸为nr×ns的二维场景矩阵,矩阵内各像素一一对应空间划分后的bin单元;

12、统计每个环形区间内有效像素占比得到环密度,串联各环密度构造nr维环形键向量;依托当前帧对应的环形键向量在kd 树内实施快速检索,筛选相似关键帧候选集,计算候选帧与当前关键帧之间的相似度得分,依据匹配相似度完成回环检测。

13、进一步的,所述建立sins的观测方程为:

14、通过激光雷达和捷联惯导同时计算当前点位置与参考线之间的距离,将激光雷达实际测量的距离值与sins估计位置距参考线的距离值差值作为观测向量;通过激光雷达直接获得当前点位置到参考线的距离,解算获得的第时刻位置真值为;观测位置由载体位置真值叠加测量扰动误差构成:

15、

16、其中,,,和为mems-sins估计位置与激光雷达实测位置的误差,和为子午线半径和卯酉圈半径;为大地高度;为局部纬度;为尺度对角变换矩阵;

17、当远小于或时,观测向量为:

18、

19、其中,为激光雷达实际测量的距离值;为sins估计位置距参考线的距离值;为标准化的向量;、和分别为在,,坐标系下的三个分量;为从当前位置到第l条参考线引一条垂线,垂线和参考线的交点;

20、激光雷达的观测向量矩阵及其对应观测矩阵为:

21、

22、

23、其中,为激光雷达中在当前时刻选择的参考线总数量;

24、激光雷达点云噪声的协方差对角矩阵根据当前位置距第条参考线的距离计算;

25、

26、

27、其中,为激光雷达的水平面分离角;为激光雷达的垂直分离角。

28、进一步的,所述建立相机观测方程具体为:

29、通过相邻时刻相对齐次变换矩阵依次相乘,由初始时刻位姿累积得到时刻载体相对于初始时刻的全局位姿;

30、

31、其中,为视觉里程计获取的时间段内旋转增量矩阵;为视觉里程计获取的时间段内平移增量向量;

32、根据相邻时刻相对平移量计算该时段平均速度;

33、

34、其中,为时间间隔;

35、根据视觉里程计输出的全局位姿与相邻时段平均速度,结合载体与相机之间标定的刚体外参,解算出导航坐标系下相机对应的位置、速度以及姿态;

36、利用相机的位姿与sins的估计位姿之差,得到相机的观测向量及其对应观测矩阵为:

37、

38、

39、其中,、和分别为捷联惯导估计的位置、速度和姿态;、和分别相机的载体位置、速度和姿态误差;

40、相机观测噪声的协方差对角矩阵根据位置协方差、速度协方差和姿态协方差计算;

41、

42、其中,为位置的东北天方向协方差;为速度的东北天方向协方差;为姿态的东北天方向协方差。

43、进一步的,所述建立回环观测方程具体为:

44、以视觉里程计测的候选回环帧相对位姿作为初值,通过icp算法对候选回环帧点云与当前帧点云将进行几何配准,求解历史关键帧m到当前关键帧q的变换矩阵,得到相对旋转矩阵和平移向量;

45、根据mems-sins在历史关键帧m和当前关键帧q的估算位置和姿态矩阵得到相对旋转矩阵和平移向量;

46、回环观测向量及其对应观测矩阵为:

47、

48、

49、

50、其中,和分别为平移残差和旋转残差;和分别为历史关键帧m和当前关键帧的估计位置相对于导航系的旋转矩阵;和分别为历史关键帧m和当前关键帧由sins估计的三维位姿;

51、回环观测噪声包括平移向量在历史关键帧m载体坐标系上的随机观测误差和旋转矩阵在历史关键帧m载体坐标系上的随机观测误差,其协方差矩阵通过平移噪声观测方差和旋转噪声观测方差计算;

52、。

53、进一步的,车辆处于低速运行状态下,回环无效时,载体运动系统的观测方程采用相机的观测向量及其对应观测矩阵修正后的激光雷达的观测向量矩阵及其对应观测矩阵构建;所述观测方程为:

54、

55、其中,;;;;

56、车辆处于低速运行状态下,回环有效时,载体运动系统的观测方程采用回环观测修正为:

57、

58、其中,;;;;;

59、车辆处于高速运行状态下,回环无效时,载体运动系统的观测方程直接采用激光雷达的观测向量矩阵及其对应观测矩阵构建;

60、车辆处于高速运行状态下,回环有效时,载体运动系统的观测方程采用回环观测修正激光雷达的观测向量矩阵及其对应观测矩阵构建。

61、本发明还提供了一种计算机设备,包括存储器、处理器及存储在存储器上的计算机程序,所述处理器执行所述计算机程序时实现上述任一项所述路面信息增强的城市场景车载自主导航方法的步骤。

62、本发明还提供了一种计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现上述任一项所述路面信息增强的城市场景车载自主导航方法的步骤。

63、本发明还提供了一种计算机程序产品,包括计算机程序,该计算机程序被处理器执行时实现上述任一项所述路面信息增强的城市场景车载自主导航方法的步骤。

64、本发明的有益效果在于:

65、本发明针对城市复杂环境下,尤其是在冰雪路面、卫星信号拒止环境及动态干扰等复杂场景中对高精度、高可靠导航的迫切需求,提出了一种基于sins、视觉传感器和lidar的多模态融合定位与动态场景表征方法。该方法充分利用不同传感器的特点,克服单一传感器在复杂、动态城市环境下的局限性,同时结合路面信息(十字路口、三岔路口、弧形拐弯、t型路口、直行路段等道路模型、车道线和冰雪覆盖路面)辅助车载导航系统实现全天候、精准、稳定的导航定位,为车辆在城市全域导航提供持续稳定的位姿信息,实现全地形的高精度高可靠性定位及构图的目标。本发明方法降低车载定位与构图对工作场景、先验地图及卫星信号限制,增强冰雪覆盖路面场景、无先验地图且卫星拒止场景和动态干扰等复杂场景中车载目标感知与定位及构图精度和鲁棒性,促进车载自主导航解决方案进一步完善。