X Μ Δ S+ξ (公式4.7);
[0067] St为滤波周期，Μ为里程计参数误差向量的系数矩阵，ξ为等效观测噪声。Μ的具体 形式戈
（公式4.9);
[006引公式4.8中，5f。为一个里程计采样间隔[n-l，n]内里程计在b系X轴方向上的增量， 及^为Y轴方向上的增量;公式4.9中，为b系至导航坐标系η系的旋转矩阵。
[0069] 运里取状态向量X'去掉位置误差向量后的子向量r为IMU/里程计组时的状态向 量巧'矩阵中也需要去掉相应的行和列，记为口'），则观测方程为。=!1沪+弓(公式4.10);
[0070] ξ为观测噪声，观测矩阵Η为:H= [ l3X3St，- Δ Ps X，03X6，M](公式4.11);
[0071] 其中，13X3表示Ξ阶单位矩阵，值得注意的是滤波周期St的选择。如果St过小，观 测噪声会被放大，而如果周期过长，则速度误差不能被视为常量。因此，滤波周期St可介于 0.2s至2s之间，一般采用0.5秒或1秒的滤波周期较佳。
[0072] 卡尔曼滤波过程时间更新：
[0076] 状态更新：
[0077] X" k+i=X" k+i I k+Xk+i (Zk-HX" k+i I k)(公式 4.15);
[007引增益矩阵:Kk+i = Dk+i|kHT(HPk+i|kHT+R)-i (公式 4.16);
[0079] 协方差矩阵:护片1 = (1寸=扳+曲护片化(公式4.17);
[0080] 式中，At为IMU更新周期，R为观测噪声协方差矩阵，Q为等效系统噪声协方差矩 阵。得到IMU的速度误差修正量后，采用公式4.19计算位置误差修正量：ΔPl=ΔVlδt (公 式 4.18)。
[0081] 注意到在卡尔曼状态更新中，位置误差方差并没有参与更新。下面进一步考虑 IMU/里程计组合时位置误差方差的计算模型。在位置递推过程中，位置误差被建模为一阶 马尔科夫过程，其模型如下：
[0084]其中，Γρ为系数矩阵，ξρ为系统模型噪声，如为系统噪声方差矩阵。在滤波时刻，由 式(4.19)计算的位置误差修正量被反馈到惯性滤波器，然后被置零。在递推过程中，位置误 差保持为零，位置误差方差会呈指数不断增加。考虑IMU/里程计组合的W下Ξ个方面:1)滤 波修正后，位置误差不确定性减小，此时方差应该减小;2) IMU/里程计组合系统定位误差不 确定性会随着时间增长;3)位置方差的主要作用在于反向平滑时准确地将位置误差改正量 分配到其他时刻。因此，在滤波更新时刻，本发明设计了一种随时间平滑增长，但增长速度 远小于指数函数的位置方差函数(一种对数模型来计算位置误差的方差）；换而言之，本发 明的惯性滤波器在滤波更新时刻，通过对数模型来计算位置误差的方差，具体公式如下：
[0088] 其中，《7^.为东方向方差基值，灯^为北方向方差基值，贫/^为高程方差基值。因此， IMU/里程计组合定位方差整体趋势将如图5所示:在一个滤波周期St内，位置方差随着时间 快速增长，在里程计数据修正时刻，位置误差不确定降低，方差也随之降低，但仍比上个滤 波时刻要大，既总体上仍呈增长趋势。
[0089] 最终，IMU/里程计组合的整体流程如图6所示，即，所述步骤S10具体包括：
[0090] S110、对惯性测量单元（IMU)测得的数据进行纯惯性推算，得到惯性位置、速度和 姿态。
[0091] S120、惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置 增量。
[0092] S130、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩 展卡尔曼滤波器中，得到惯性测量单元的误差修正量(加速度计零偏和巧螺仪漂移）、W及 速度误差修正量和姿态误差修正量，根据速度误差改正向量A Vi和滤波周期St得到位置误 差修正量Δ Pi，即，利用公式4.1的十算得到位置误差修正量Δ Pi。
[0093] S140、将惯性测量单元的误差修正量反馈到惯性滤波器(纯惯性滤波器）中，而位 置、速度和姿态误差修正量则与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿 态(即，得到初始滤波位置、初始滤波速度和初始滤波姿态）。
[0094] 而本发明提供的则是LiDAR/IMU/里程计组合模型。如果没有激光雷达的辅助，误 差累积速度会随时间增长。经过一段时间后，误差可能发散到超过测量要求。为了获得长期 高精度轨迹，需要引入外部高精度位置信息对累积误差进行修正。因此，所引入的绝对位置 精度将决定整个定位定姿系统的绝对精度。本发明提供的移动物体的定位定姿方法，通过 在地下空间的测量场地中布设LiDAR控制标祀（测量控制点），然后利用激光雷达对运些 LiDAR控制标祀进行观测，将观测值与真值之间的残差观测值带入到卡尔曼滤波方差中，从 而修正IMU/里程计系统的累积误差。
[00M]所述步骤S20具体包括:根据公式4.22,可W将激光雷达测量的距离和角度与IMU/ 里程计组合定位定姿计算出的初始滤波位置与初始滤波姿态数据融合得到初始激光点云。
[0096]
[0097] 运里，上标g，b，1分别表示地理空间坐标系、IMU坐标系和激光扫描坐标系，XS是地 理空间坐标系坐标下LiDAR点的坐标向量，Τ?是地理空间坐标系下导航中屯、的坐标向量，巧 是从IMU坐标系到地理空间坐标系的旋转矩阵，由IMU计算出得姿态械计算得到。χ?是激光 扫描观测值在激光坐标系下的的坐标向量，巧6是从激光扫描仪坐标系到IMU坐标系的的平 移向量。及；^是从激光扫描仪到^1]坐标系到旋转矩阵，由标定得到的空间同步姿态角计算 得到。
[0098] 所述步骤S30具体包括:利用其它高精度测量手段测得激光雷达控制标祀高精度 坐标，用X康示，将该值作为真实值，激光雷达控制标祀高精度坐标Xl就是测量控制点已知 的实际坐标。壯与初始激光点云中对应标祀观测量XS相减得到坐标残差观测量A Xg，即Δ Xg = XL-χg公式（4.23)。
[0099] 所述步骤S40具体包括:一方面，考虑到经过里程计修正后，IMU/里程计组合解算 的姿态精度较高；另一方面，从点云中提取控制标祀中屯、点时，可能会存在较小的偏差，而 姿态误差的影响可能被控制点提取误差的影响所掩盖。因此，控制点残差观测值将主要由 于定位误差造成。从而，我们得到A Xe与IMU/里程计定位定姿误差Δ P2, Δ 02之间的关系Δ 拍=ΔP2+ξ(公式4.24)。
[0100] 令观测值Z2= AXg，系统状态向量X为IMU误差状态向量，即：
[0101]
:公式 4.25);
[0102] 则观测方程为：
[0103] Z2 =出 Χ+ξ
[0104] 出=[Ι3Χ3,03Χ12](公式 4.26);
[0105] 状态方程为：
（公式4.27)。
[0106] 控制点残差观巧尴的噪声主要来自于控制点坐标误差W及从点云中提取控制点时的 提取误差。控制点误差和提取误差分别服从高斯分布（0，和高斯分布（0, L e 5
则观测值噪声的分布巧 在实际作业中，我们可W利用测量机器人将 0 控制点坐标精度控制在0.02m，利用可视化软件将提取精度控制在0.01m，因此，控制点噪声 的方程为0.0005m2。
[0107] 所述步骤S50具体包括:LiDAR/IMU滤波过程与公式4.12~4.15类似。如果^0八尺/ IMU滤波时间间隔不等长的，在采用控制点改正后，里程计推算量应清零，重新开始IMU/里 程计的滤波周期。否则会出现错误的速度改正量，造成滤波扰乱。图7为LiDAR/IMU/里程计 组合时的位置方差曲线示意图：在一个LiDAR/IMU/里程计滤波周期内，位置方差随着时间 而增长，在滤波时刻，累积位置误差得到修正，位置方差随之减小。
[0108] 所述步骤S60具体包括:无论是IMU/里程计组合还是LiDAR/IMU/里程计组合，在滤 波修正处位置、速度和姿态都会出现跳动，造成他们在时间上的不连续。对于实时定位导航 而言，运种跳动可W被忽略。而对于移动测图而言，运些跳动则需要被消除，W保证轨迹的 时间连续性，即需要对轨迹进行平滑处理。平滑的另一个目的在于利用滤波。
[0109] 如步骤S60所述，无论是IMU/里程计组合还是LiDAR/IMU/里程计组合，在滤波修正 处位置、速度和姿态都会出现跳动，造成他们在时间上的不连续。对于实时定位导航而言， 运种跳动可W被忽略。而对于移动测图而言，运些跳动则需要被消除，W保证轨迹的时间连 续性，即需要对轨迹进行平滑处理。平滑的另一个目的在于利用滤波信息计算其他时刻的 位置、速度和姿态误差改正量，从而提高整体轨迹精度。常用的平滑算法有正反向滤波和固 定滞后区间反向平滑。运里采用固定区间反向平滑R-T-S算法，其数学模型为：
[0113] 其中，if为k时刻平滑改正量，为滤波改正量，Kks平滑增益矩阵，Dks为平滑后的 协方差矩阵。
[0114] 由此可知，本发明构建了统一的融合LiDAR控制标祀数据、IMU数据W及里程计数 据的扩展卡尔曼滤波模型。该模型建立在IMU动力学模型和误差模型基础上，通过将LiDAR 控制标祀数据带入到