同时定位与地图构建（Simultaneous Localization and Mapping, SLAM）作为自主机器人应用的基本先决条件，近年来取得了蓬勃的发展，优秀的研究成果逐步实现从实验室向工业界的落地。层出不穷的数据集和实际场景的多样性，对SLAM系统的定位精度和鲁棒性提出了更高的要求。一些复杂的外界环境，比如快速光照变化、弱纹理、高遮挡、场景结构退化、剧烈运动等，会导致SLAM系统性能下降甚至运行失败。特别是对于使用单一传感器的算法，由于其固有的缺陷和局限性，很难适应多样化的环境。融合多源信息的SLAM方法能够结合不同传感器的优势，使其在复杂多样现实场景中适应性和鲁棒性地提高成为可能。本文围绕激光雷达、惯性测量单元和相机三种模态的传感器信息源，针对视觉-惯性、激光雷达-惯性和激光雷达-惯性-视觉定位与建图三种具体的任务展开研究。本文主要的研究内容与贡献总结如下：

一、提出一种基于非线性因子估计位姿图优化（Pose Graph Optimization，PGO）的视觉-惯性定位与建图方法。本方法综合考虑了位姿图中序列约束（时间上相近）和闭环约束（空间上相似）的协方差估计，以更准确地表示系统真实不确定性。对于序列约束，本方法引入绝对位姿和相对位姿因子，并利用非线性因子提取方法从累积的里程计中获取最优的协方差矩阵；对于闭环约束，本方法引入尺度因子，并提出动态尺度估计方法以近似其协方差的尺度。得益于所提尺度估计算法，本方法在面对由动态物体干扰造成的假阳性闭环场景时，展现出较强的鲁棒性。所提方法的有效性在公开数据集、动态物体干扰数据集和自采数据集上通过实验进行了验证。

二、提出一种基于不变扩展卡尔曼滤波（Invariant Extended Kalman Filter，InvEKF）的激光雷达-惯性定位与建图方法。本方法为激光雷达-惯性定位与建图设计了一种不变EKF状态估计器。考虑到系统会引入IMU偏置和传感器噪声，本方法利用不变观测器原理和李群理论，得到近似对数-线性的误差动力学方程，以改善系统中存在的非一致性问题。将该不变EKF状态估计器应用到激光雷达-惯性定位与建图中，本文分别实现了以机器人为中心（Inv-LIO1）和以世界为中心（Inv-LIO2）的两个独立系统，并对其可观性进行了理论分析。Inv-LIO1中局部坐标系下的状态在每帧激光雷达时间移动，以防止滤波器发散；Inv-LIO2直接在全局坐标系下融合激光雷达与IMU测量。此外，Inv-LIO1的里程计和建图模块分别运行在两个不同的线程中，而Inv-LIO2则采用一种集成里程计和建图模块的架构。在室内仿真数据集、室外公共数据集和自采数据集上的实验表明了所提方法的有效性。

三、提出一种基于多状态颜色一致性约束的激光雷达-惯性-视觉定位与建图方法。本方法紧耦合了激光雷达-惯性里程计（LiDAR-Inertial Odometry，LIO）和视觉-惯性里程计（Visual-Inertial Odometry，VIO）两个子系统，并定义了带有颜色信息的全局地图表示形式。LIO子系统中点云经过运动补偿后，直接用于构建点到面的残差；VIO子系统利用全局地图中点的深度信息，根据滑动窗口中多个相机状态观测到同一地图点颜色的不变性，构建光度误差约束；最后通过不变EKF状态估计器进行系统状态更新。本文还对系统的可观性进行了理论分析。所提方法在公共数据集和自采数据集上进行实验和鲁棒性测试，验证了方法的有效性。