利用车辆运动规律提高视觉惯性里程计鲁棒性的方法及系统

本发明属于自动驾驶车辆，特别涉及利用车辆运动规律提高视觉惯性里程计鲁棒性的方法及系统。

背景技术：

1、在gps信号拒止环境下，基于机器人本体传感器的定位导航是实现智能机器人自主运动能力的关键技术。根据传感器种类的不同,其解决方案也是多种多样的。在载荷能力有限的情况下,大多使用单目相机或双目相机和惯性测量单元(inertial measurementunit,imu)两种互补传感器构建视觉惯性里程计(visual inertial odometry,vio)来解决自主运动能力中的定位导航问题。基于视觉的同时定位与地图构建(simultaneouslocalization and mapping，slam)方法已成为自动驾驶车辆(auto driving vehicle,adv)运动状态估计的主流方法，为精确的车辆位姿估计提供了一种可行且低成本的解决方案。在slam框架中，相机和惯性测量单元(inertial measurement unit，imu)的耦合为gps缺失环境下的高精度和鲁棒的自车运动估计提供了有效的解决方案，即视觉惯性里程计(visual inertial odometry,vio)。vio的动机是利用imu为vio提供车辆运动的度量尺度，并解决动态场景的无纹理和快速变化问题。

2、然而，vio部署在自动驾驶车辆(adv)中时面临着一个挑战：vio难以在车辆以恒定的速度、加速度沿圆弧、直线在平面上行驶时难以完成初始化及连续稳定的运行。车辆在常规机动过程中难以对imu进行充分激励，这导致视觉惯性里程计难以准确估计陀螺仪和加速度计的零偏及单目视觉惯性里程计的真实的尺度。此外，vio有四个不可观测的方向，包括三个在重力方向的平移和一个绕重力方向的旋转，adv中的vio在大范围室外环境中会快速累积误差并表现出较差的性能。

技术实现思路

1、本发明的目的在于利用车辆运动规律提高视觉惯性里程计鲁棒性的方法及系统，以解决上述问题。

2、为实现上述目的，本发明采用以下技术方案：

3、第一方面，本发明提供利用车辆运动规律提高视觉惯性里程计鲁棒性的方法，包括：

4、获取车辆的图像数据以及车辆惯性测量数据，对视觉惯性里程计进行初始化，初始化后的视觉惯性里程计持续输出位姿；

5、利用车辆惯性测量数据，基于虚拟传感器的群不变卡尔曼滤波器对车辆运动状态进行估计，输出的位姿作为虚拟传感器因子；

6、根据车辆惯性测量数据，通过噪声估计单元估计虚拟传感器的观测噪声；

7、利用因子图将视觉惯性里程计输出的位姿和虚拟传感器因子进行融合，输出位姿变换，并修正基于虚拟传感器的群不变卡尔曼滤波器。

8、可选的，获取车辆的图像数据以及车辆惯性测量数据，包括：

9、通过双目/单目相机捕捉图像序列，为视觉惯性里程计vio提供视觉信息，用于估计相机运动；通过惯性测量单元imu记录车辆的三轴加速度与角速度。

10、可选的，对视觉惯性里程计进行初始化，初始化后的视觉惯性里程计持续输出位姿，包括：

11、利用视觉惯性里程计vio处理来自双目/单目相机的图像和来自惯性测量单元imu的数据，首先通过初始时间的图像和惯性测量单元imu数据完成视觉惯性里程计的初始化，获得关键帧位姿t＝[r,p]，其中朝向rk、位置pk、速度vk、imu测量数据的偏置、特征点的深度信息，初始化完成后视觉惯性里程计继续接收来自双目/单目相机的图像和来自惯性测量单元imu的数据，并持续输出车辆的位姿t＝[r,p]。

12、可选的，在通用视觉惯性里程计中，视觉和惯性状态的联合优化是在一个滑动窗口中完成的，视觉惯性里程计vio待估计的状态如下：

13、χ＝[x0,x1,...,xn,λ0,λ1,...λm]

14、xk＝[pk,vk,rk,bω,ba],k∈[0,n],

15、其中xk表示第k个关键帧对应的imu状态，k∈[0,n]是关键帧的索引；xk包含位置、速度、imu在视觉坐标系w下的朝向、加速度计和陀螺仪的零偏；λl表示第1个特征点对应的逆深度，m是滑动窗口中的特征点总数。

16、可选的，利用车辆惯性测量数据，基于虚拟传感器的群不变卡尔曼滤波器对车辆运动状态进行估计，输出的位姿作为虚拟传感器因子，包括：

17、在虚拟的车辆坐标系下，的原点的垂直向上z轴和侧向y轴的速度均接近于0，的原点速度被表示为：

18、

19、其中表示在tn时刻从虚拟车辆坐标系到imu坐标系的变换关系，[rn,pn]表示在tn时刻从imu坐标系到世界坐标系的变换；

20、一个测量的虚拟传感器表示为：

21、

22、nn～n(0,nn)

23、nn＝diag(δlat,δup)2,

24、其中表示状态xn,和虚拟传感器的测量值yn之间的转换关系；xn表示imu状态；假设噪声n为中心化高斯分布，协方差矩阵为nn；测量值被送入iekf以估计xn,

25、可选的，根据车辆惯性测量数据，通过噪声估计单元估计虚拟传感器的观测噪声，包括：

26、利用基于cnn的噪声估计单元来估计噪声的分布，噪声估计单元由卷积神经网络和全连接组成，具体结构为一个具有2层的1d卷积神经网络，输出为噪声对应的协方差矩阵nn+1计算如下：

27、

28、其中，和分别表示横向和纵向速度的初始方差；

29、对虚拟传感器因子中高斯噪声的方差进行如下的动态调整：

30、

31、其中是由虚拟传感器因子中平移部分和旋转部分对应的高斯噪声的经验方差向量。

32、可选的，利用因子图将视觉惯性里程计输出的位姿和虚拟传感器因子进行融合，输出位姿变换，并修正基于虚拟传感器的群不变卡尔曼滤波器，包括：

33、使用基于虚拟传感器的群不变卡尔曼滤波器iekf得到的位姿来计算两个关键帧状态χi,χj之间的相对位姿变换：

34、

35、其中和是来自iekf对关键帧i,j对应时刻的imu位姿的估计；δti,j通过最小化如下的相对位姿变换残差来约束vio对状态的估计：

36、

37、通过最小化由相对变换、imu和相机的观测值以及先验形成的联合残差来获得χ的最大后验估计：

38、

39、其中表示相机观测对应的重投影误差，‖rp-hpχ‖2表示滑动窗口外关键帧提供的先验信息；将位姿估计(记为tk＝[rk,pk])作为vio因子集成到因子图中；

40、利用因子图给出的位姿变换每隔设定时间对基于虚拟传感器的群不变卡尔曼滤波器的状态进行修正，即利用对基于虚拟传感器的群不变卡尔曼滤波器iekf的状态的平移和旋转部分进行修正，这种修正等价于利用vio的视觉观测来约束iekf位姿估计的累积误差。

41、第二方面，本发明提供利用车辆运动规律提高视觉惯性里程计鲁棒性的系统，包括：

42、数据获取模块，用于获取车辆的图像数据以及车辆惯性测量数据，对视觉惯性里程计进行初始化，初始化后的视觉惯性里程计持续输出位姿；

43、虚拟传感器因子输出模块，用于利用车辆惯性测量数据，基于虚拟传感器的群不变卡尔曼滤波器对车辆运动状态进行估计，输出的位姿作为虚拟传感器因子；

44、降噪模块，用于根据车辆惯性测量数据，通过噪声估计单元估计虚拟传感器的观测噪声；

45、融合修正模块，用于利用因子图将视觉惯性里程计输出的位姿和虚拟传感器因子进行融合，输出位姿变换，并修正基于虚拟传感器的群不变卡尔曼滤波器。

46、第三方面，本发明提一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现利用车辆运动规律提高视觉惯性里程计鲁棒性的方法的步骤。

47、第四方面，本发明提一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现利用车辆运动规律提高视觉惯性里程计鲁棒性的方法的步骤。

48、与现有技术相比，本发明有以下技术效果：

49、本发明通过结合车辆的图像数据和惯性测量数据，利用车辆的运动规律来提高视觉惯性里程计(vio)的鲁棒性。

50、视觉惯性里程计(vio)通过双目/单目相机和惯性测量单元(imu)的数据进行初始化，并持续输出车辆的位姿。这种初始化方法能够确保vio在后续运行过程中具有准确的起点和稳定的输出。

51、本发明基于虚拟传感器的群不变卡尔曼滤波器(iekf)利用车辆惯性测量数据对车辆运动状态进行估计，并输出虚拟传感器因子。这种估计方法考虑了车辆的运动特性，如垂直和侧向速度接近于零，从而提高了运动状态估计的精确性。

52、本发明噪声估计单元通过卷积神经网络(cnn)对iekf包含的噪声进行估计，并根据实际情况对噪声方差进行动态调整。这有助于更准确地反映实际情况中的噪声分布，从而增强滤波器的鲁棒性。

53、本发明通过将vio输出的位姿与iekf输出的虚拟传感器因子进行融合，并利用因子图计算位姿变换，可以对iekf的状态进行修正。这种修正方法能够利用vio的视觉观测来约束iekf位姿估计的累积误差，从而提高整个系统的鲁棒性和准确性。

54、该技术方案综合了vio和iekf的优势，通过结合车辆的图像数据和惯性测量数据，以及利用车辆的运动规律，实现了对车辆位姿的精确估计和滤波器的有效修正。这不仅能够提高系统的鲁棒性，使其在面对复杂环境或恶劣条件时仍能保持稳定和准确的输出，还能够为后续的导航、定位等应用提供可靠的数据支持。

55、综上所述，该技术方案通过结合车辆的图像数据和惯性测量数据，以及利用车辆的运动规律来提高视觉惯性里程计的鲁棒性，具有显著的技术效果和实际应用价值。