定位是移动机器人自主导航的核心技术问题，当机器人在已知/未知环境中导航行走时，需要实时知道"我在哪里、面朝何方"，这是机器人执行任何任务的前提。但在实际场景中，传感器会有噪声、环境会发生变化，长通道环境会导致激光退化等。如何在重重干扰下，给出准确可靠的定位信息，是机器人能够自主执行任务的能力基础。

在机器人定位领域，现有研究依据状态估计的数学范式可将定位方法划分为滤波方法与优化方法两大类别。滤波方法以贝叶斯滤波为理论基础，通过预测-更新两步递归计算维持当前时刻状态的后验概率分布，典型代表包括基于粒子滤波的自适应蒙特卡洛定位（AMCL）以及基于卡尔曼滤波（EKF、UKF）的多传感器融合方案。此类方法的共同特征在于递推计算与固定的计算复杂度，但其历史信息一旦被边缘化即不可回溯修正。优化方法则以优化建模为核心，将多个时刻的状态变量作为节点、传感器测量作为因子约束，通过非线性最小二乘求解全局最优的轨迹估计，以基于iSAM2的增量平滑优化为代表。此类方法允许新信息的加入来回溯修正历史状态，理论上具有更高的精度上限。此外Cartographer也是一个优秀的SLAM框架，但其的的纯定位方法，本质上也是在建图，而建图所存在的后端优化对于定位而言，是不必要的；回环甚至可能产生定位跳变的干扰；此外其前端的扫描匹配不是与当前环境地图进行匹配，便有可能导致定位精度降低。

综合业内现状，并且为了进一步提高机器人定位精度，采用基于因子图优化的紧耦合框架来实现机器人的导航定位。使用优化的方法，来维护长时间的轨迹估计，当新测量加入时，该约束可通过因子图反向传播，修正历史时刻的位姿估计，这一特性是因子图方法精度优势的理论根源。采用紧耦合框架，在原始测量层面融合，信息无损，可以有效保证定位融合精度。

因子图优化将定位问题建模为概率图模型上的最大后验估计，其理论基础为图模型与非线性最小二乘优化，通过iSAM2（增量平滑与建图）实现高效的增量式求解。

算法原理：构建因子图，其中变量节点包括各时刻的位姿SE_k、速度v_k及IMU偏置B_k。因子边编码各类传感器约束：IMU预积分因子连接相邻时刻的位姿、速度与偏置，实现紧耦合的惯性约束；轮式里程计因子提供帧间相对位姿约束；激光匹配因子可分层设计（局部匹配对滑动窗口地图、全局匹配对先验地图），分别提供帧间平滑约束与全局绝对约束；先验因子固定初始状态以消除系统自由度。求解时，每加入新的变量与因子，iSAM2仅对贝叶斯树中受影响的局部区域进行重线性化与更新，避免全局批处理的计算开销。

算法流程图见图1。其中imu参数估计、多传感器时间同步、激光点云畸变矫正均是从传感器层面来提高最终的融合定位精度。

因子图节点与约束见图2。不同传感器测量对应一个因子节点，机器人状态对应变量节点，iSAM2 实现增量式优化求解。

变量节点：SE₁ → SE₂ → SE₃ → ... → SEₖ    ← 位姿序列

      v₁→  v₂→  v₃ → ... → vₖ    ← 速度序列

      B₁ → B₂ → B₃ → ... → Bₖ   ← IMU 偏置序列（包含加速度计ba与陀螺仪bg）

因子边：

 ● IMU 预积分因子 —— 连接 SEk-1, vk-1, SEk, vk, Bk-1, Bk

 ● 轮式里程计因子 —— 连接 SEk-1, SEk

 ● 激光 PL-ICP 因子 —— 连接 SEk（与局部地图的约束）

 ● 全局点云匹配因子 —— 连接 SEk（与先验地图的约束）

 ● 先验因子 —— 固定初始状态

a) 纠偏测试

当实际位置不对时，因子图定位基于激光信息自动纠偏。（下图激光点云与地图的最终将自动变为一致）

因子图优化方法在定位精度上有着显著优势，能够有效融合异步、异构传感器数据，并利用激光雷达的观测信息对累积误差进行修正，显著提升定位的精度与可靠性。同时系统具备良好的扩展性，因子图框架对新传感器的接入表现为因子节点的增加，模块化程度高。后续可以更方便地引入其他观测源（如视觉特征约束），以进一步提高定位的鲁棒性。