[发明专利]一种基于ESKF的紧耦合定位方法、设备及存储介质

说明书

本发明涉及一种基于ESKF的紧耦合定位方法、设备及存储介质，方法具体包括以下步骤：获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；基于ESKF算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成ESKF融合，获得最终的定位结果。本发明的多传感器组合的定位方案相较于单一传感器的定位方案，理论上结果会更准确、鲁棒性更强；同时使用ESKF算法，在保持较高定位精度的情况下能有效降低定位算法的计算资源需求。

技术领域

本发明属于自动驾驶车辆定位技术领域，具体涉及一种基于ESKF的紧耦合定位方法、设备及存储介质。

背景技术

在定位算法方面，基于先验地图的定位方法是比较常用的定位方法之一，该方法将在线观测的环境数据与先验地图匹配，得到差异最小的最佳匹配位置。先验地图需要通过即时定位与地图构建(simultaneous localization and mapping，SLAM)进行构建。

基于点云地图的匹配定位方法是目前定位精准度最高的方法。点云地图匹配精度虽然高，但是资源占用率和计算复杂度也很高，如何对地图进行维护更新以应对环境的动态变化也是该方法需要解决的难点问题之一。将IMU数据与轮速数据和视觉定位数据进行融合能有效提高定位算法的可靠性和运行效率。为此Martinelli研究了视觉-惯性结构在运动问题中的应用，Mourikis提出使用多状态约束的卡尔曼滤波(multi-stateconstraint Kalman filter，MSCKF)方法来融合视觉-惯性导航，在提高运行效率方面，Biber等使用里程计信息来加速定位迭代的收敛速度。

现有技术中：如CN107221007A——《一种基于图像特征降维的无人车单目视觉定位方法》，使用ORB-slam框架进行无人车定位，但是该方法中最终融合结果的鲁棒性较差，有待进一步改进。

发明内容

针对现有技术的上述不足，本发明要解决的技术问题是提供一种基于ESKF的紧耦合定位方法、设备及存储介质，避免单一传感器的定位方案带来的结果不够准确、鲁棒性差的问题，取得保持较高定位精度的情况下能有效降低定位算法的计算资源需求的效果。

为解决上述技术问题，本发明采用如下技术方案：

一种基于ESKF的紧耦合定位方法，具体包括以下步骤：

获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；

通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；

基于ESKF算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成ESKF融合，获得最终的定位结果。

进一步完善上述技术方案，基于惯性测量单元测量值和轮速计读数的融合结果构建系统状态，并推断错误状态，完成状态传递。

进一步地，构建系统状态时，状态量x的表达式如下：

其中，p为当前时刻车辆位姿；R为旋转；b_g为惯性测量单元中陀螺仪的偏差；a为轮速因子；

通过下列各式得到错误状态δx_t和系统状态协方差矩阵p_t：

δx_t＝(I+F_tΔt)δx_t-1

p_t＝(I+F_t)p_t-1(I+F_t)^T+(G_t)Q(G_t)^T