在复杂的室内环境中，由于建筑物的遮蔽，往往无法获得定位卫星信号。相比于全球导航卫星系统(global navigation satellite system, GNSS)定位，惯性导航系统(inertial navigation system, INS)可以不依赖外界信号，通过测量载体在惯性参考系的加速度和角速度，将其对时间进行积分，从而得到载体在导航坐标系中的速度、姿态和位置等信息。但是由于传感器误差的影响，INS得到的位姿误差随时间不断增加。激光雷达(light detection and ranging, LiDAR)在移动过程中可以不断获得周围环境的点云扫描数据，通过对不同位置获得的数据进行匹配，可以得到LiDAR的位移，因此，可以利用LiDAR辅助惯性测量单元(inertial measurement unit, IMU)实现室内高精度定位。

目前，LiDAR和IMU的室内组合导航方法主要是将2D-LiDAR的水平位移和方位角与IMU位姿进行组合，提高室内定位精度。Kohlbrecher等^[1]设计了一套以2D-LiDAR扫描匹配结果为基础的实时定位与建图系统，通过扩展卡尔曼滤波(extended Kalman filter, EKF)融合了LiDAR、IMU、GPS和罗盘仪等传感器的观测数据，可以在复杂环境下分析载体的三维实时运动状态，证明了利用EKF融合LiDAR扫描匹配和IMU数据的理论可行性。Kumar等^[2]提出了一种室内实时定位建图方案，以无人机为载体，利用IMU和2D-LiDAR数据获得水平位移，利用第二组LiDAR的地面信息获得高度变化，将两组位移信息通过卡尔曼滤波融合，获得无人机的三维轨迹。Tang等^[3]将2D-LiDAR扫描匹配得到的航向角、二维位置和IMU数据进行滤波融合，证明在动态条件下，滤波结果比单独LiDAR扫描匹配得到的轨迹精度更高。

随着LiDAR扫描技术的发展，目前出现了大量价格低廉的3D-LiDAR，基于3D-LiDAR的配准方法也在不断成熟。然而，LiDAR配准得到的位移和姿态误差会随着时间不断累积，误差模型难以估计。针对以上问题，本文使用两组3D-LiDAR和IMU，通过基于平面特征的点云配准方法获得Li‐DAR的速度，并利用EKF滤波将其和IMU的位姿推算结果进行数据融合，从而限制IMU位姿推算的误差漂移，以提高室内导航定位的精度。

1 IMU/LiDAR组合导航算法 1.1 IMU误差模型

IMU在导航坐标系下的运动学模型公式可参考文献[4], IMU初始对准过程可以参考文献[5]。需要注意的是，本文导航坐标系采用东-北-天(E-N-U)坐标系。考虑到背包载体6自由度的运动情况，系统误差状态向量可表示如下：

$ \mathit{\boldsymbol{X}}(t) = ({\mathit{\boldsymbol{\delta }}_\varepsilon }, {\mathit{\boldsymbol{\delta }}_v}, {\mathit{\boldsymbol{\delta }}_r}, {\mathit{\boldsymbol{b}}_g}, {\mathit{\boldsymbol{b}}_a}) $

(1)

式中，${\mathit{\boldsymbol{\delta }}_\varepsilon }({\delta _{{\varepsilon _E}}}, {\delta _{{\varepsilon _N}}}, {\delta _{{\varepsilon _U}}})$为导航坐标系下3个姿态角误差；${\mathit{\boldsymbol{\delta }}_v}({\delta _{{v_E}}}, {\delta _{{v_N}}}, {\delta _{{v_U}}})$为导航坐标系下三轴速度误差；${\mathit{\boldsymbol{\delta }}_r}({\delta _\varphi }, {\delta _\lambda }, {\delta _h})$为纬度、经度和高度误差；b_g(ϵ_x，ϵ_y，ϵ_z)为陀螺仪误差；b_a(▽_x，▽_y，▽_z)为加速度计误差。IMU的系统误差传播方程可参考文献[4]。

本文使用的3DM系列航姿参考系统是一种微电子机械系统IMU，其数据误差主要考虑陀螺仪误差和加速度计误差。陀螺仪误差一般包括常值漂移、随机漂移和白噪声分量，可表示成ω(t)=ω₀+ω_d(t)+w_ω。其中，ω₀为常值漂移；ω_d(t)为随机漂移；w_ω为白噪声。通过IMU长时间静态数据分析，选择一阶马尔可夫过程描述陀螺仪的随机漂移：

$ {\dot \omega _d}(t) = - \frac{1}{\tau }{\omega _d}(t) + w(t) $

(2)

式中，τ指相关时间，是由长时间静态数据分析获得的固定参数；w (t)指白噪声。加速度计误差与陀螺仪误差类似，采用同样的模型估计。

1.2 IMU/LiDAR组合导航

IMU的测量坐标系和LiDAR的仪器坐标系存在旋转平移偏差，在进行组合导航前首先进行IMU和LiDAR之间相对位置关系的校准。通过文献[6]中的外参标定方法可获得IMU和LiDAR坐标系相对旋转关系$\mathit{\boldsymbol{R}}_{{\rm{LiDAR}}}^{{\rm{IMU}}}$。考虑到两者之间的平移偏差对组合导航精度的影响较小，所以采用手工量测的方式获得IMU和LiDAR坐标中心的平移偏差$T_{{\rm{LiDAR}}}^{{\rm{IMU}}}$。

以往研究证明EKF是一种相对高效且稳定的非线性误差分析方法^[7-9]，因此本文采用该滤波算法实现IMU和LiDAR的组合导航，流程见图 1。其中，$\mathit{\boldsymbol{R}}_{b{\rm{IMU}}}^n$、${\mathit{\boldsymbol{v}}^n}_{{\rm{IMU}}}$、${\mathit{\boldsymbol{r}}^n}_{{\rm{IMU}}}$分别表示当前时刻IMU滤波之前相对于导航坐标系的姿态、速度和位置；${\mathit{\boldsymbol{v}}^n}_{{\rm{LiDAR}}}$表示当前时刻由点云配准得到的LiDAR速度；$\mathit{\boldsymbol{R}}_b^n$、${\mathit{\boldsymbol{v}}^n}$、${\mathit{\boldsymbol{r}}^n}$分别表示经过滤波后的姿态、速度和位置。

每次滤波过程都包括系统状态更新和观测值更新，具体在组合导航系统中指IMU误差状态的预测更新和基于LiDAR平面配准结果的观测值更新。IMU数据的更新频率和LiDAR数据的更新频率不一致，本文中IMU数据的更新频率是100 Hz，点云数据更新频率是20 Hz。当IMU数据更新时，进行系统位姿推算和系统误差更新；当LiDAR数据更新时，首先利用IMU得到的系统位姿为LiDAR数据提供初始姿态，然后使用文献[10]中的基于平面特征的点云配准方法进行两帧点云之间的旋转平移矩阵计算。在得到相邻三帧点云之间的相对位移后，计算得到中间时刻LiDAR的运动速度${\mathit{\boldsymbol{v}}^n}_{{\rm{LiDAR}}}$，将其作为滤波的观测值，计算系统误差改正。相比于文献[3, 11, 12]中以LiDAR相对于轨迹起始点坐标系的姿态和位移为观测值，本文使用LiDAR的速度作为滤波器的观测值，这是考虑到假如某一时刻LiDAR数据配准出现了较大误差，采用LiDAR位姿作为观测值的更新方式会将该部分误差引入滤波系统，且该误差无法在后续的观测值更新过程中消除，最终可能导致滤波系统的崩溃。而${\mathit{\boldsymbol{v}}^n}_{{\rm{LiDAR}}}$的误差只和每次平面配准的精度相关，将其作为滤波器的观测值可以有效避免观测值的误差累积。

将LiDAR速度作为观测值的系统观测方程为：

$ {\mathit{\boldsymbol{Z}}_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{V}}_k} $

(3)

式中，${\mathit{\boldsymbol{Z}}_k} = \left[ {{\mathit{\boldsymbol{v}}^n}_{{\rm{IMU}}} - {\mathit{\boldsymbol{v}}^n}_{{\rm{LiDAR}}}} \right]$; ${\mathit{\boldsymbol{V}}_k} \sim N(0, {\mathit{\boldsymbol{R}}_k})$，表示观测值噪声，其中，${\mathit{\boldsymbol{R}}_k} = {\rm{diag}}(\mathit{\boldsymbol{\delta }}_{{v^n}_{{\rm{LiDAR}}}}^2)$，$\mathit{\boldsymbol{\delta }}_{{v^n}_{{\rm{LiDAR}}}}^2$可以基于LiDAR设备的特性及点云配准算法的精度进行估计；${\mathit{\boldsymbol{H}}_k} = \left[ {\begin{array}{*{20}{c}} {{{\bf{0}}_{3 \times 3}}}&{\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{I}}_{3 \times 3}}}&{{{\bf{0}}_{3 \times 3}}}&{\begin{array}{*{20}{c}} {{{\bf{0}}_{3 \times 3}}}&{{{\bf{0}}_{3 \times 3}}} \end{array}} \end{array}} \end{array}} \right]$。

2 实验与分析 2.1 实验方案

本文利用组合导航算法进行实验，使用的IMU型号是3DM-GX5-25, LiDAR型号是Velodyne公司生产的VLP-16系列。测量范围为100 m，测量距离精度为±3 cm，竖直方向测量角分辨率为2°，水平方向测量角分辨率为0.1°~0.4°。将两组LiDAR和IMU安装在同一个刚体支架背包上，由实验人员背负支架在实验场景内步行采集数据。

为了给组合导航轨迹提供参考信息，在获得所有点云平面配准关系后，利用图优化(general graph optimization, g2o)对所有关系进行非线性优化，得到一组近似真实轨迹的姿态位移信息，见图 2(a)；并且利用这组姿态位移恢复了所有点云的相互位置姿态关系，得到了整个会议室的稠密点云模型，见图 2(b)。从点云模型可以看出，实验场景有足够的平面特征为点云配准提供便利。

2.2 实验结果分析

图 3为IMU、LiDAR、参考轨迹的姿态角对比图。可以看出，IMU推算的俯仰角、横滚角相比于LiDAR平面配准得到的姿态角误差更小；IMU推算出的航向角和LiDAR平面配准的航向角误差近似，但是一段时间后，明显还是后者的累积误差更大。出现这种情况是因为实验选取的IMU陀螺仪精度较高，而平面配准的姿态精度容易受所提取平面的噪声影响；而且平面配准存在误匹配和中断的可能性，而陀螺仪的姿态输出可以始终保持不间断。基于这种情况，在EKF的设计中，使用LiDAR的姿态来更正系统姿态误差的方法并不可取。

以参考轨迹的水平位置为参考，图 4给出了水平方向上滤波输出的位置误差。LiDAR观测量的加入有效地抑制了滤波的误差漂移，滤波轨迹的水平误差始终控制在0.25 m内。另外，滤波轨迹的水平误差随着轨迹呈周期变化，每次回到起始点时水平误差都趋于零。在图 4框出的最后一段轨迹中，当单独LiDAR配准误差出现较大误差时，滤波误差仍然保持在较小范围内，与参考轨迹保持一致，并没有被观测值的误差所影响。

图 5给出了垂直地面方向上滤波输出的位置误差。在轨迹高度误差方面，相比IMU单独导航，虽然组合导航高度误差得到了一定控制，轨迹最后高度误差仍维持在1 m内，但是相比于LiDAR轨迹高度的来回震荡，滤波的轨迹高度始终往一个方向偏离，且高度误差与时间呈线性关系。这是因为相比于水平面的轨迹计算，IMU在竖直方向受到更多重力有害加速度影响，LiDAR的高度观测值无法完全抑制INS的漂移。一般这个问题的解决方式是在垂直地面方向上添加其他约束来进一步抑制INS在高度上的误差，有待进一步研究。

以上实验证明，以LiDAR速度为观测值的EKF可以有效地抑制IMU的水平定位误差，并且在LiDAR配准出现较大误差时，滤波也可以将水平误差控制在0.25 m内。

3 结束语

本文提出了一种LiDAR平面配准方法辅助的IMU室内定位方法，将LiDAR速度和IMU位姿利用EKF融合，有效约束了误差漂移。实验结果表明：①在动态运动条件下，无论周围环境特征是否丰富，本文采用的IMU姿态比点云配准的姿态更准确，所以即使在一些特征不明显的区域，整个系统始终可以输出准确的姿态；② LiDAR配准得到的LiDAR速度确实限制IMU轨迹的漂移，并且在一些点云配准误差较大的场景，滤波后的结果仍然可以保持一定的水平位移精度；③相较于采用LiDAR的位移和姿态作为观测值的方式，本文将LiDAR速度作为观测值的方案更适合解决实时数据采集中可能遇到的点云配准失效问题。在缺少闭环检测的情况下，LiDAR点云配准获得的位移和姿态都存在误差累积的问题，但是LiDAR的速度误差只和点云配准算法及LiDAR测量精度相关。本文提出的方法可被应用于无GPS信号的室内实时定位导航。