中国科学院自动化研究所机构知识库

移动机器人的状态估计是其进行路径规划、运动控制的基础，基于多传感器融合的移动机器人定位技术利用不同传感器的互补特性取得高精度的位姿估计。本文研究基于两种传感器组合的定位系统：视觉惯性定位系统与激光惯性定位系统，并分别基于非线性优化算法与卡尔曼滤波算法实现多传感器融合。本文的主要研究内容如下：

1.提出了一种视觉惯性里程计信息矩阵部分稀疏化算法。

基于滑动窗口非线性优化的视觉惯性里程计通过边缘化的方式保证优化变量维数的固定，然而在边缘化过程中需丢弃一些观测项以保证信息矩阵稀疏性。为减小边缘化过程中的信息损失并同时保持里程计系统的实时性，本文提出了一种信息矩阵部分稀疏化算法，该算法在边缘化过程中引入视觉观测项，并通过非线性因子恢复技术从边缘化得到的稠密先验中恢复稀疏因子以近似原始信息矩阵，保证后续优化的实时性。

2.设计了一种视觉惯性定位系统协方差矩阵估计算法。

为减小视觉惯性里程计受累积误差影响产生的漂移，需建立起较早的关键帧与滑动窗口中最新的帧之间的联系，并通过位姿图优化的方法减小累积误差。为近似原始位姿图优化问题的不确定性，本文设计了一种视觉惯性定位系统协方差矩阵估计算法，以估计位姿图优化问题中各约束的协方差。该算法利用非线性因子恢复技术得到滑动窗口序列约束的最优协方差估计，并采用切换约束技术近似回环约束协方差尺度，提高位姿图优化输出轨迹精度。

3.设计并实现了一个基于迭代不变卡尔曼滤波的激光惯性里程计系统。

激光雷达作为一种外源传感器可提供丰富的场景结构信息，通过将惯性传感器信息与激光雷达信息融合，可以获得高精度的机器人状态估计。本研究设计并实现了一个基于迭代不变卡尔曼滤波的激光惯性里程计系统，以实现移动机器人的自主定位。该系统利用惯性数据进行误差状态的协方差传递，并对激光雷达输出点云数据进行特征提取与匹配以完成状态的量测更新，得到移动机器人姿态的后验估计。

State estimation is the basis of motion planning and control for mobile robots, and robot localization based on multi-sensor fusion achieves high-precision results utilizing the complementary nature of different sensors. This thesis focuses on localization with two different sensor suites: visual-inertial navigation system and Lidar-inertial navigation system, by respectively implementing nonlinear optimization and Kalman filter for multisensory fusion. The main contributions of this thesis are summarized as follows.

1. This thesis proposes a partial sparsification algorithm for visual-inertial odometry. Visual-inertial odometry based on sliding-window nonlinear optimization maintains the fixed size of optimizing variables by marginalization, during marginalization some visual measurements need to be dropped to maintain the sparsity of the information matrix. To minimize the information loss in the marginalization step while not decreasing the real-time perfomance, this research designed a partial sparsification algorithm, which includes the visual measurements in the marginalization and utilizes nonlinear factor recovery to recover sparse factors from the resultant dense prior and maintain the real-time performance of following optimization.

2. This research gives a covariance approximation method for visual-inertial navigation system. To reduce the drift of visual-inertial odometry, pose graph optimization is performed to distribute the accumulative error by establishing connections between recent frames in the sliding window and past old keyframes. To approximate the uncertainty of the original pose graph optimization problem, this research designed a covariance approximation algorithm for visual-inertial navigation system using nonlinear factor recovery to recover optimally the covariance for sequential constraints in the sliding window and switching constraints to approximate the scale of loop-closing constraints.

3. A Lidar-inertial odometry based on iterative invariant Kalman filter is designed and implemented finally. Lidar sensor can give rich structural information of the surrounding environment, and the localization system can provide high-accuracy state estimation by fusing Lidar sensor with inertial measurement unit. This research designed and implemented a Lidar-inertial odometry system based on iterative invariant Kalman filter for the autonomous localization of mobile robots, which integrates inertial measurements to propagate covariance of the error state and performs measurement update by feature extraction and matching of the Lidar point cloud data.