基于能观测性分析的机器人EKF-SLAM算法

EKF-SLAM algorithm for robot based on observability analysis

针对传统EKF-SLAM算法中存在状态估计不一致的问题,从系统能观测性角度分析,提出一种增加观测性约束条件的算法,利用补偿矩阵U最优化求解约束条件,得到新的线性点,并通过优化系统的雅克比矩阵重构系统能观测矩阵,使得EKF-SLAM系统与非线性SLAM系统观测方程能观矩阵的秩保持一致.结果表明,所提出算法在状态估计的精确性和协方差一致性方面明显优于传统的EKF-SLAM算法,研究工作和结论对车辆自主驾驶有一定的参考价值.

In order to solve the problem that the state estimation inconsistency exists in the traditional EKF-SLAM （extended Kalman filter-simultaneous localization and mapping） algorithm, an algorithm which could increase the observability constraint condition was proposed from the perspective of system observability. In addition, the compensation matrix U was optimized to solve the constrained condition, and the new linear points were obtained. Through optimizing the Jacobi matrix of system, the observability matrix of system was reconstructed, which could make the rank of local observability matrix of EKF-SLAM system be consistent with that of non-linear SLAM system. The results show that the proposed algorithm is superior to the traditional EKF-SLAM algorithm in terms of both state estimation accuracy and covariance consistency. The research work and conclusions have certain reference value for the vehicle autonomous driving.