具有全局速度约束的惯性/编码器/视觉/激光融合定位方法:IEVL-Fusion

IEVL-Fusion:An Inertial/Encoder/Vision/Laser Fusion Positioning Method with Global Velocity Constraint

针对卫星拒止环境中移动机器人高精度定位需求,提出一种基于误差状态扩展卡尔曼滤波(ES-EKF)的激光定位子系统/视觉定位子系统/全局速度测量子系统松耦合融合定位方法,并设计了一个误差低漂移的组合定位系统。首先,根据向量加法和矩阵乘法对系统状态的误差进行最小表示,建立误差形式的卡尔曼滤波模型,以误差状态的最优估计对系统状态的估计值进行补偿。然后,针对激光和视觉定位子系统位姿不确定度未知的问题,将位姿输出依时间戳转化为位姿增量,并建立位姿增量观测模型。其次,针对组合定位系统缺少全局速度约束的问题,利用姿态航向参考系统(AHRS)和正向运动学模型构建全局速度测量子系统,并建立全局速度观测模型。最后,在街道和野外两个场景中进行测试,本文算法的相对定位误差小于0.4%,相较于仅受局部速度约束的EKF和ES-EKF融合定位算法降低了约40%。实验结果表明,所提出的算法有效提升了定位系统的精度。

Answering the high-precision positioning requirements of mobile robot in satellite-denied environments,an error-state extended Kalman filter(ES-EKF)based positioning method is proposed with loosely coupled fusion of subsystems of laser positioning,visual positioning,and global velocity measurement,and an integrated positioning system with low error drift is designed.Firstly,the error of system state is represented with vector addition and matrix multiplication in a minimum form,and a Kalman filter model in the error form is established,in which the optimal estimation of error state is used to compensate the estimated value of system state.Then,the pose output is transformed into the pose increment according to the time stamp,and the pose increment observation model is established,to deal with the problem of unknown pose uncertainty of the laser and visual positioning subsystems.Secondly,a global velocity measurement subsystem is constructed by using the attitude heading reference system(AHRS)and forward kinematics model,and a global velocity observation model is established,to make up the lack of global velocity constraint in the integrated positioning system.Finally,tests are carried out in street and field scenes,and results show that the relative positioning error of the proposed algorithm is less than 0.4%,which is about 40%lower than that of EKF and ES-EKF positioning algorithms with local velocity constraint.Experimental results demonstrate that the proposed method effectively improves the accuracy of the positioning system.