1 引言（Introduction）

近年来随着光电探测、人工智能、导航及控制等领域的快速发展，无人侦察技术发展迅猛，已成为世界各国重点发展方向。目前的无人侦察手段主要集中在空基、天基和临近空间等领域，利用无人机、人造卫星及高空气球搭载照相机、成像雷达等探测设备对地表上的目标物体进行侦察^[1]。但是，目前的无人侦察手段依赖于全球导航卫星系统（global navigation satellite system，GNSS）的辅助，当面临卫星拒止环境时，探测系统的瘫痪甚至会引起灾难性的安全事故。

地下空间作为继宇宙与海洋之外人类亟需开拓的又一大领域，近年来受到了广泛关注。但是，地下环境作为一种典型的卫星拒止复杂环境，极大地限制了人类的感知能力。在2016年至2017年的摩苏尔战役中，由于现有探测系统无法在无卫星的环境下为士兵提供可靠的战场环境信息，很多作战任务未能达成目标。掌握地下空间对于军事作战、灾后救援、基础设施监控等领域都有着十分重要的意义^[2]，因此亟需发展面向地下空间环境的无人探测系统。

移动机器人作为人工智能在应用层面的典型代表^[3]，是一种能够在复杂环境下工作的智能机器人，目前已在越来越多的行业中发挥了重要作用，如宇宙探测、精准农业等^[4-5]。利用移动机器人先行探索可以在复杂的地形环境中获取丰富的环境信息，从而引导人员成功完成任务。2018年，美国国防高级研究计划局（DARPA）开展了“地下挑战赛”项目，该项目旨在利用移动机器人为军方寻求面向地下环境的自主导航与探测新方法^[6]。2019年，美军司令部专门针对地下环境发布了《地下空间行动指南》，指出了如何在无卫星环境中进行作战：“移动机器人总是需要自主先行，为后面穿过隧道的人类探路”。但是，现有的移动机器人探测受制于诸多技术难点，例如地下复杂环境导致的定位精度恶化以及难以对目标进行精确定位等问题，因此无法在地下空间环境中实现可靠的自主探测。

本文提出了一种面向地下空间探测的移动机器人定位与感知方法。首先，构建了基于因子图的激光雷达/里程计/惯性测量单元的紧耦合融合框架，推导了高精度惯性测量单元/里程计预积分模型，能够避免惯性测量单元与里程计量测信息的重复积分，从而减小计算负担。其次，提出了基于激光雷达/红外相机融合的目标识别方法，通过端到端的训练方式，对图像进行多层次的特征提取与融合，提升了识别的准确率以及相对定位精度。

2 基于因子图的激光雷达/里程计/惯性测量单元紧耦合融合方法（Tightly-coupled LiDAR-odometer-IMU fusion method based on factor graph） 2.1 基于因子图的紧耦合融合框架

传统的基于激光雷达辅助的多源信息融合方法多是采用松耦合的方式。该类方法对各信息源分开进行解算，在此基础上采用滤波或因子图的方式进行融合。Zhang^[7]提出了一种实时的激光雷达定位与构图方法，利用惯性测量单元（IMU）的解算结果作为先验信息辅助激光雷达进行点云匹配。Shan^[8]提出了一种基于地面优化的激光雷达里程计方法，实现了算法的低漂移率和低计算复杂度。但是，以上方法都解耦了激光雷达和IMU的量测信息，并没有利用IMU做进一步的优化。Shan^[9]将IMU预积分因子添加到后端的因子图框架中，并与前端获取的激光雷达测量信息进行联合优化估计，但其仍然只是分开考虑了激光雷达和IMU的估计。Tang^[10]采用了基于扩展卡尔曼滤波器（EKF）的松耦合方式对IMU和激光雷达信息进行融合，但其只适用于2维运动的情况，无法处理3维运动和更复杂的环境。

在地下空间中，常常会出现“结构退化环境”，如长直走廊等。这时激光雷达的点云信息通常是特征缺失的，在车体运动的前后方向没有可用的有效点进行位姿约束。在松耦合的情况下，会将这种错误的激光雷达里程计信息作为后端的融合输入，从而导致最终的融合结果受到影响。本文提出了一种基于因子图的激光雷达/里程计/惯性测量单元紧耦合融合方法。相比于松耦合方式，将激光雷达扫描获取的点云信息作为观测量加入到后端融合中，能够有效地避免结构退化环境给融合结果带来的影响。

考虑到机械式3维激光雷达内部存在旋转机构，通过采集一整圈的点云数据构成一帧雷达点云。当激光雷达运动时，扫描得到的点云数据会失真，因此得到的点云与真实环境不同。为了解决这个问题，本文利用惯性测量单元/里程计预测激光雷达的运动，并假设激光雷达在扫描期间呈线性运动，通过线性插值矫正每个点云以补偿点云畸变。

本文设计的紧耦合方法为：通过惯性递推方程更新系统状态并计算惯性测量单元/里程计预积分量测结果，对激光雷达原始数据进行处理以获得去除畸变后的点云数据。根据先前对应的优化状态，将局部窗口内先前的激光雷达特征点合并为一个局部地图，从而找出相对激光雷达测量数据。最后进行因子图优化，采用相对激光雷达测量数据和惯性测量单元/里程计预积分来获得局部窗口内状态的最大后验估计，并利用优化结果来更新系统状态。紧耦合方案如图 1所示。

因子图的状态向量设置如下：

$ \begin{align} {{\mathit{\boldsymbol{X}}}}&=[{{{\mathit{\boldsymbol{x}}}}_{0}, {{\mathit{\boldsymbol{x}}}}_{1}, \cdots, {{\mathit{\boldsymbol{x}}}}_{n}}]\\ {{\mathit{\boldsymbol{x}}}}_{k}&=[{{{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n}, {{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n}, {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k}} ^{\rm n}, {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}, {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}, s_{{\rm o}_{k} }, {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b}, {{\mathit{\boldsymbol{q}}}}_{{\rm o}_{k}} ^{\rm b}} ], \quad k\in \left[ {0, n} \right] \end{align} $

(1)

其中，$ {{\mathit{\boldsymbol{x}}}}_{k} $表示$ t_{k} $时刻的状态向量，包括位置$ {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} $、速度$ {{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} $、姿态$ {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k}} ^{\rm n} $、加速度计零偏$ {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} $、陀螺仪零偏$ {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $、里程计标度因数$ s_{{\rm o}_{k}} $、杆臂$ {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b} $（即里程计坐标系原点在机体坐标系中的3维位置）及IMU的安装角度$ {{\mathit{\boldsymbol{q}}}}_{{\rm o}_{k}} ^{\rm b} $。目标函数可以表示为

$ \begin{align} \min\limits_{{\mathit{\boldsymbol{X}}}} \Bigg( & \| {{{\mathit{\boldsymbol{r}}}}_{\rm P} -{{\mathit{\boldsymbol{H}}}}_{\rm P} {{\mathit{\boldsymbol{X}}}}}\|^{2} + \sum\limits_{k-n}^k {\| {{\mathit{\boldsymbol{r}}}}_\text{I/O} (\hat{\mathit{\boldsymbol{z}}}_{{\rm b}_{k+1} }^{{\rm b}_k}, {{\mathit{\boldsymbol{X}}}} ) \|_{{{\mathit{\boldsymbol{P}}}}_\text{I/O}} ^{2}}+ \\ &\sum\limits_{{k-n}}^{{k}} {\| {{{\mathit{\boldsymbol{r}}}}_{\rm L} ({{\hat{\mathit{\boldsymbol{z}}}}_{\rm L}, {{\mathit{\boldsymbol{X}}}}} )} \|_{{{\mathit{\boldsymbol{P}}}}_{\rm L}} ^{2}} \Bigg) \end{align} $

(2)

式(2) 中，$ \| {{{\mathit{\boldsymbol{r}}}}_{\rm P} -{{\mathit{\boldsymbol{H}}}}_{\rm P} {{\mathit{\boldsymbol{X}}}}} \|^{2} $为边缘化的先验信息，$ \| {{{\mathit{\boldsymbol{r}}}}_\text{I/O} ( {{\hat{\mathit{\boldsymbol{z}}}}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}, {{\mathit{\boldsymbol{X}}}}} )} \|_{{{\mathit{\boldsymbol{P}}}}_\text{I/O}} ^{2} $为惯性测量单元/里程计的预积分残差，$ \| {{{\mathit{\boldsymbol{r}}}}_{\rm L} ( {{\hat{\mathit{\boldsymbol{z}}}}_{\rm L}, {{\mathit{\boldsymbol{X}}}}} )} \|_{{{\mathit{\boldsymbol{P}}}}_{\rm L}} ^{2} $为激光雷达的量测残差。

2.2 高精度惯性测量单元/里程计预积分模型

在图优化过程中，优化时刻的状态量需要被频繁地调整，使得IMU的量测信息需要被重复积分，因此会导致较大的计算负担。为了解决这个问题，引入惯性预积分理论以避免重复积分问题^[11-12]。但是，以上的预积分方法多针对精度较低的微机电系统（MEMS）惯性器件，由于该类器件本身无法测量地球自转角速度以及载体运动带来的导航坐标系与地球坐标系的转动角速度，因此其对应的预积分模型不得不作一定程度的简化。但是，当应用精度较高的惯性器件时，以上的模型将会带来较大的误差。

本文提出了一种面向高精度惯性导航系统的预积分方法。考虑到地球自转角速度以及载体运动产生的有害加速度，重新构建了惯性器件输出模型。同时，考虑到里程计的递推特性，将里程计的测量信息投影到机体坐标系下，从而形成高精度惯性测量单元/里程计预积分模型。其中，IMU坐标系记为$ \varSigma_{\rm b} $，即机体坐标系。里程计坐标系记为$ \varSigma_{\rm o} $，导航坐标系采用东北天坐标系，记为$ \varSigma_{\rm n} $，地球坐标系记为$ \varSigma_{\rm e} $，惯性坐标系记为$ \varSigma_{\rm i} $，坐标系定义如图 2所示。

扣除额外角速度的陀螺模型为

$ \begin{equation} {{\mathit{\boldsymbol{w}}}}={{\mathit{\boldsymbol{w}}}}_{\rm ib}^{\rm b} -{{\mathit{\boldsymbol{R}}}}_{\rm n}^{\rm b} ({{\mathit{\boldsymbol{w}}}}_{\rm ie}^{\rm n} +{{\mathit{\boldsymbol{w}}}}_{\rm en}^{\rm n}) \end{equation} $

(3)

其中，$ {{\mathit{\boldsymbol{w}}}}_{\rm ib}^{\rm b} $为陀螺的输出，$ {{\mathit{\boldsymbol{w}}}}_{\rm ie}^{\rm n} $和$ {{\mathit{\boldsymbol{w}}}}_{\rm en}^{\rm n} $分别为地球自转角速度与载体运动带来的导航坐标系与地球坐标系的转动角速度在导航坐标系下的投影。$ {{\mathit{\boldsymbol{R}}}}_{\rm n}^{\rm b} $为导航坐标系到机体坐标系的旋转矩阵。

在载体运动过程中，会分别产生哥氏加速度以及法向加速度，因此扣除了额外加速度的加速度计完整模型为

$ \begin{equation} {{\mathit{\boldsymbol{a}}}}={{\mathit{\boldsymbol{a}}}}_{\rm ib}^{\rm b} -{{\mathit{\boldsymbol{R}}}}_{\rm n}^{\rm b} (2{{\mathit{\boldsymbol{w}}}}_{\rm ie}^{\rm n} +{{\mathit{\boldsymbol{w}}}}_{\rm en}^{\rm n}) \times {{\mathit{\boldsymbol{v}}}}_{\rm en}^{\rm n} \end{equation} $

(4)

其中，$ {{\mathit{\boldsymbol{a}}}}_{\rm ib}^{\rm b} $为加速度计的输出，$ {{\mathit{\boldsymbol{v}}}}_{\rm en}^{\rm n} $为导航坐标系相对于地球坐标系的速度在导航坐标系中的投影。

惯性测量单元/里程计在导航坐标系下的状态传播方程被建模为如下形式：

$ \begin{equation} \begin{aligned} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n} & ={{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} +{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} \Delta t_{k} +\int {\int {\left[{{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}} ^{\rm n} \left( {{\hat{\mathit{\boldsymbol{a}}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm a}} \right)-{{\mathit{\boldsymbol{g}}}}^{\rm n}} \right]{\rm d}t^{2}}} \\ {{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k+1}} ^{\rm n} & ={{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} +\int {\left[ {{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}} ^{\rm n} \left({{\hat{\mathit{\boldsymbol{a}}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm a}} \right)-{{\mathit{\boldsymbol{g}}}}^{\rm n}} \right]} {\rm d}t \\ {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k+1}} ^{\rm n} & ={{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k}} ^{\rm n} \otimes \int {\frac{1}{2}} {\boldsymbol \varOmega} \left({{\hat{\mathit{\boldsymbol{w}}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm g}} \right){{\mathit{\boldsymbol{q}}}}_{{\rm b}_{t}} ^{{\rm b}_{k}} {\rm d}t\\ {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n} & ={{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} +{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{k}} ^{\rm n} {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b} +\\ &\quad\; \int {{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}} ^{\rm n} {{\mathit{\boldsymbol{R}}}}_{{\rm o}_{t} }^{\rm b}} \left({s_{{\rm o}_{t}} {\hat{\mathit{\boldsymbol{v}}}}_{t} -{{\mathit{\boldsymbol{n}}}}_{\rm s}} \right){\rm d}t-{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{k+1}} ^{\rm n} {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k+1}}^{\rm b} \end{aligned} \end{equation} $

(5)

其中，

$ \begin{equation} \boldsymbol\varOmega ({{{\mathit{\boldsymbol{w}}}}} )= \begin{bmatrix} {-\left[{{{\mathit{\boldsymbol{w}}}}} \right]_{\times}} & {{{\mathit{\boldsymbol{w}}}}} \\ {-{{\mathit{\boldsymbol{w}}}}^{\rm T}} & 0 \\ \end{bmatrix}, \; \; \; [{{{\mathit{\boldsymbol{w}}}}} ]_{\times} = \begin{bmatrix} 0 & {-w_{{z}}} & {w_{{y}}} \\ {w_{{ z}}} & 0 & {-w_{{x}}} \\ {-w_{{y}}} & {w_{{x}}} & 0 \\ \end{bmatrix} \end{equation} $

(6)

在式(5) 中，$ {\hat{\mathit{\boldsymbol{w}}}}_{t} $和$ {\hat{\mathit{\boldsymbol{a}}}}_{t} $分别为$ t $时刻扣除额外角速度与加速度的IMU测量数据。$ {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{t}} $和$ {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{t}} $分别代表$ t $时刻陀螺仪和加速度计的常值零偏，$ {{\mathit{\boldsymbol{n}}}}_{\rm g} $和$ {{\mathit{\boldsymbol{n}}}}_{\rm a} $分别表示陀螺仪和加速度计的白噪声。$ {\hat{\mathit{\boldsymbol{v}}}}_{t} $为$ t $时刻里程计的输出，$ s_{{\rm o}_{t}} $为$ t $时刻里程计的标度因数，$ {{\mathit{\boldsymbol{n}}}}_{\rm s} $为里程计的白噪声。$ {{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}} ^{\rm n} $为$ t $时刻机体坐标系到导航坐标系的旋转矩阵，$ {{\mathit{\boldsymbol{g}}}}^{\rm n} $为导航坐标系下的重力向量。$ {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{i}}^{\rm n} $为$ t_{i} $时刻机体坐标系在导航坐标系下的位置向量，$ {{\mathit{\boldsymbol{v}}}}_{{\rm b}_{i}}^{\rm n} $为$ t_{i} $时刻机体坐标系在导航坐标系下的速度向量，$ {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{i}}^{\rm n} $为$ t_{i} $时刻机体坐标系相对于导航坐标系的四元数。$ \Delta t_{k} $为$ t_{k} $与$ t_{k+1} $的时间间隔。$ {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{i}} ^{\rm b} $为$ t_{i} $时刻里程计在机体坐标系下的位置向量，$ {{\mathit{\boldsymbol{R}}}}_{{\rm o}_{t}} ^{\rm b} $为$ t $时刻里程计坐标系到机体坐标系的旋转矩阵，$ \otimes $为四元数乘法符号。

将式(5) 中的参考系由导航坐标系转换为$ t_{k} $时刻的机体坐标系$ \varSigma_{{\rm b}_k} $，可以得到：

$ \begin{equation} \begin{aligned} &{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n}={{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} \left({{{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} +{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} \Delta t_{k} -\frac{1}{2}{{\mathit{\boldsymbol{g}}}}^{\rm n}\Delta t_{k}^{2}} \right)+{\boldsymbol \alpha}_{{\rm b}_{k+1} }^{{\rm b}_{k}} \\ &{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k+1}} ^{\rm n}={{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}}({{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} -{{\mathit{\boldsymbol{g}}}}^{\rm n}\Delta t_{k}} )+{\boldsymbol \beta} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} \\ &{{\mathit{\boldsymbol{q}}}}_{\rm n}^{{\rm b}_{k}} \otimes {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k+1}} ^{\rm n}={\boldsymbol \gamma}_{{\rm b}_{k+1} }^{{\rm b}_{k}} \\ &{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n}={{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} -{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{R}}}}_{{\rm b}_{k+1}} ^{\rm n} {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k+1}} ^{\rm b} +{{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b} +{\boldsymbol \eta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}} \end{aligned} \end{equation} $

(7)

其中，

$ \begin{equation} \begin{aligned} {\boldsymbol \alpha} _{{\rm b}_{k+1}}^{{\rm b}_{k}}&=\int {\int {{{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}} ^{{\rm b}_{k}} ({\hat{\mathit{\boldsymbol{a}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm a}} )} }} {\rm d}t^{2} \\ {\boldsymbol \beta} _{{\rm b}_{k+1}}^{{\rm b}_{k}}&=\int {{{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}}^{{\rm b}_{k}} ({\hat{{{\mathit{\boldsymbol{a}}}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm a}} )} {\rm d}t} \\ {\boldsymbol \gamma} _{{\rm b}_{k+1}}^{{\rm b}_{k}}&=\int {\frac{1}{2}} {\boldsymbol \varOmega} ( {\hat{\mathit{\boldsymbol{w}}}_{t} -{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{t}} -{{\mathit{\boldsymbol{n}}}}_{\rm g}} ){\boldsymbol \gamma} _{{\rm b}_{t}}^{{\rm b}_{k} } {\rm d}t \\ {\boldsymbol \eta}_{{\rm b}_{k+1}}^{{\rm b}_{k}}&=\int {{{\mathit{\boldsymbol{R}}}}_{{\rm b}_{t}}^{{\rm b}_{k}} {{\mathit{\boldsymbol{R}}}}_{{\rm o}_{t} }^{\rm b} ({s_{{\rm o}_{t}} \hat{{{\mathit{\boldsymbol{v}}}}}_{t} -{{\mathit{\boldsymbol{n}}}}_{\rm s}} ){\rm d}t} \end{aligned} \end{equation} $

(8)

在式(7) 中，$ {\boldsymbol \alpha} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} $为利用陀螺仪与加速度计解算的$ \varSigma_{{\rm b}_{k+1}} $相对于$ \varSigma_{{\rm b}_k} $的预积分位置变化量，$ {\boldsymbol \beta} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} $为利用陀螺仪与加速度计解算的$ \varSigma_{{\rm b}_{k+1}} $相对于$ \varSigma_{{\rm b}_k} $的预积分速度变化量，$ {\boldsymbol \gamma} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} $为利用陀螺仪解算的$ \varSigma_{{\rm b}_{k+1}} $相对于$ \varSigma_{{\rm b}_k} $的预积分旋转变化量，$ {\boldsymbol \eta }_{{\rm b}_{k+1}} ^{{\rm b}_{k}} $为利用陀螺仪与里程计解算的$ \varSigma_{{\rm b}_{k+1}} $相对于$ \varSigma_{{\rm b}_k} $的预积分位置变化量。惯性测量单元/里程计预积分的修正模型可以表示为

$ \begin{equation} \begin{aligned} {\boldsymbol \alpha} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} &={\hat{\mathit{\boldsymbol{\alpha}}}} _{{\rm b}_{k+1} }^{{\rm b}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}} ^{\delta {\boldsymbol \alpha}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} \delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta {\boldsymbol \alpha}_{{\rm b}_{k+1}} ^{{\rm b}_{k} }} \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} \\ {\boldsymbol \beta} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} &={\hat{\mathit{\boldsymbol{\beta}}}} _{{\rm b}_{k+1} }^{{\rm b}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}} ^{\delta {\boldsymbol \beta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} \delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta {\boldsymbol \beta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}} } \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} \\ {\boldsymbol \gamma} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} &={\hat{\mathit{\boldsymbol{\gamma}}}} _{{\rm b}_{k+1} }^{{\rm b}_{k}} \otimes \begin{bmatrix} 1 \\ {\dfrac{1}{2}{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}}^{\delta {\boldsymbol \theta}_{{\rm b}_{k+1}}^{{\rm b}_{k}}} \delta {\mathit{\boldsymbol{b}}}_{{\rm g}_{k}}} \\ \end{bmatrix} \\ {\boldsymbol \eta} _{{\rm b}_{k+1}} ^{{\rm b}_{k}} &={\hat{\mathit{\boldsymbol{\eta}}}} _{{\rm b}_{k+1} }^{{\rm b}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta {\boldsymbol \eta}_{{\rm b}_{k+1}}^{{\rm b}_{k}}} \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} + \\ &\quad\; {{\mathit{\boldsymbol{J}}}}_{\delta s_{{\rm o}_{k}}}^{\delta {\boldsymbol \eta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}} } \delta s_{{\rm o}_{k}} +{{\mathit{\boldsymbol{J}}}}_{\delta {\boldsymbol \theta}_{{\rm o}_{k}}^{\rm b}}^{\delta {\boldsymbol \eta}_{{\rm b}_{k+1} }^{{\rm b}_{k}}} \delta {\boldsymbol \theta} _{{\rm o}_{k}} ^{\rm b} \end{aligned} \end{equation} $

(9)

其中，$ \delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} $和$ \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $分别代表加速度计和陀螺的零偏误差，$ \delta s_{{\rm o}_{k}} $和$ \delta{\boldsymbol \theta} _{{\rm o}_{k}} ^{\rm b} $分别代表里程计标度因数误差以及安装角度误差。$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}}^{\delta {\boldsymbol \alpha}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} $和$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}}^{\delta {\boldsymbol \alpha}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} $分别为惯性预积分位置变化量误差对应$ \delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} $和$ \delta{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $的雅可比矩阵，$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}} ^{\delta{\boldsymbol \beta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} $和$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta{\boldsymbol \beta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} $分别为惯性预积分速度变化量误差对应$ \delta{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}} $和$ \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $的雅可比矩阵，$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta {\boldsymbol \theta}_{{\rm b}_{k+1}}^{{\rm b}_{k}}} $为惯性预积分旋转变化量误差对应$ \delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $的雅可比矩阵，$ {{\mathit{\boldsymbol{J}}}}_{\delta {{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} ^{\delta {\boldsymbol \eta}_{{\rm b}_{k+1}}^{{\rm b}_{k}}} $、$ {{\mathit{\boldsymbol{J}}}}_{\delta s_{{\rm o}_{k}}} ^{\delta {\boldsymbol \eta}_{{\rm b}_{k+1} }^{{\rm b}_{k}}} $和$ {{\mathit{\boldsymbol{J}}}}_{\delta {\boldsymbol\theta}_{{\rm o}_{k}} ^{\rm b}} ^{\delta {\boldsymbol\eta}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} $分别为里程计预积分位置变化量误差对应$ \delta{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}} $、$ \delta s_{{\rm o}_{k}} $和$ \delta {\boldsymbol \theta} _{{\rm o}_{k}}^{\rm b} $的雅可比矩阵。上述雅可比矩阵均对应$ t_{k+1} $时刻雅可比矩阵$ {{\mathit{\boldsymbol{J}}}}_{{\rm b}_{k+1}} $的子块矩阵，可以从对应位置中获取。

因此，惯性测量单元/里程计预积分残差可以表示为

$ \begin{align} &\; {{\mathit{\boldsymbol{r}}}}_\text{I/O} ({{\hat{\mathit{\boldsymbol{z}}}}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}, {{\mathit{\boldsymbol{X}}}}} ) \\ =& \begin{bmatrix} {{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} \bigg({{{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n} -{{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} -{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} \Delta t_{k} +\dfrac{1}{2}{{\mathit{\boldsymbol{g}}}}^{\rm n}\Delta t_{k}^{2}} \bigg)-\hat{\mathit{\boldsymbol{\alpha}}}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} \\ {2[{ ({\hat{\mathit{\boldsymbol{\gamma}}} _{{\rm b}_{k+1}} ^{{\rm b}_{k}}} )^{-1}\otimes ({{{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k}} ^{\rm n}} )^{-1}\otimes {{\mathit{\boldsymbol{q}}}}_{{\rm b}_{k+1} }^{\rm n}} ]_{xyz}} \\ {{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} ({{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k+1}} ^{\rm n} -{{\mathit{\boldsymbol{v}}}}_{{\rm b}_{k}} ^{\rm n} +{{\mathit{\boldsymbol{g}}}}^{\rm n}\Delta t_{k}} )-\hat{\mathit{\boldsymbol{\beta}}}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} \\ {{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k+1}} -{{\mathit{\boldsymbol{b}}}}_{{\rm a}_{k}}} \\ {{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k+1}} -{{\mathit{\boldsymbol{b}}}}_{{\rm g}_{k}}} \\ { ({{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k+1}} ^{\rm n} -{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{p}}}}_{{\rm b}_{k}} ^{\rm n} +{{\mathit{\boldsymbol{R}}}}_{\rm n}^{{\rm b}_{k}} {{\mathit{\boldsymbol{R}}}}_{{\rm b}_{k+1}} ^{\rm n} {{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k+1}} ^{\rm b} -{{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b} } )-\hat{\mathit{\boldsymbol{\eta}}}_{{\rm b}_{k+1}} ^{{\rm b}_{k}}} \\ {s_{{\rm o}_{k+1}}-s_{{\rm o}_{k}}} \\ {{{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k+1}} ^{\rm b} -{{\mathit{\boldsymbol{p}}}}_{{\rm o}_{k}} ^{\rm b}} \\ {2[{ ({{{\mathit{\boldsymbol{q}}}}_{{\rm o}_{k}} ^{\rm b}} )^{-1}\otimes {{\mathit{\boldsymbol{q}}}}_{{\rm o}_{k+1}} ^{\rm b}} ]_{xyz}} \\ \end{bmatrix} \end{align} $

(10)

其中，$ [\cdot]_{xyz} $代表四元数的虚部。

3 基于激光雷达/红外相机融合的目标识别方法（Object recognition method based on LiDAR and infrared camera）

在目标识别方面，现有的目标识别方法往往是通过手动设计特征提取器，例如Haar特征^[13]、方向梯度直方图（HOG）特征^[14]和尺度不变特征变换（SIFT）特征^[15]。在此基础上使用支持向量机（SVM）模型^[16]以及可形变部件模型（deformable part model，DPM）等模型^[17]进行图像分类，但是其检测速度与准确度都相对较低。近年来基于深度学习的目标检测方法逐渐崛起，往往使用卷积神经网络进行图像特征提取，主要包括两阶段目标检测算法^[18-20]和单阶段目标检测算法^[21-22]。

本文提出了基于激光雷达/红外相机融合的目标识别方法。针对单目红外相机无法获取物体尺度信息的问题，引入了激光雷达点云信息进行补充。因此需要对红外图像数据与激光雷达点云数据进行联合校准，即将点云投影到红外图像上获取对应的深度信息，再对目标的相对位置进行解算。同时，设计了基于神经网络架构的单阶段目标检测算法，在保证检测速度与检测精度的同时，能够对图像中感兴趣的物体进行检测标注，获取其在图像中的位置。融合目标检测的方案如图 3所示。

3.1 基于YOLOv3模型的目标检测算法

YOLOv3模型采用Darknet-53网络作为基本的特征提取网络，对输入图像进行下采样以获取不同尺度的特征图，并在特征图的每个网格上生成3个不同宽高比的先验框。对于每个先验框，网络会输出$ t_{x}, t_{y}, t_{m}, t_{n} $来表示其修正量。通过式(11) 可获取输出的预测框：

$ \begin{equation} \begin{aligned} b_{x} &=\sigma ({t_{x}} )+c_{x} , \quad b_{y} =\sigma ({t_{y}} )+c_{y} \\ b_{m} &=p_{m} {\rm e}^{t_{m}} , \qquad\; \; \; b_{n} =p_{n} {\rm e}^{t_{n}} \end{aligned} \end{equation} $

(11)

其中$ (c_{x}, c_{y} ) $与$ p_{m}, p_{n} $分别表示先验框的位置与宽、高，$ (b_{x}, b_{y} ) $与$ b_{m}, b_{n} $分别表示预测框的位置与宽、高。最小化损失函数可以表示为

$ \begin{align} r_{C} &=\lambda_\text{coord} \sum\limits_{i=0}^{S^{2}} {\sum\limits_{j=0}^B {I_{ij}^\text{obj}}} P +\lambda_\text{coord} \sum\limits_{i=0}^{S^{2}} {\sum\limits_{j=0}^B {I_{ij}^\text{obj}}} {W} + \\ &\quad\; \sum\limits_{i=0}^{S^{2}} {\sum\limits_{j=0}^B {I_{ij}^\text{obj}}} c_{i} + \lambda_\text{noobj} \sum\limits_{i=0}^{S^{2}} {\sum\limits_{j=0}^B {I_{ij}^\text{noobj}}} c_{i} + \\ &\quad\; \sum\limits_{i=0}^{S^{2}} {I_{i}^\text{obj}} \sum\limits_{c\in L} {p_{i} (c)} \end{align} $

(12)

其中$ P $和$ W $分别代表预测框的中心坐标损失量和宽高坐标损失量，$ c_{i} $表示优化目标的置信度，$ p_{i} (c) $表示优化类别的置信度。$ S^{2} $和$ B $分别表示提取特征图的网格数以及每个网格内的先验框数量，$ L $为候选检测类别。若第$ i $个网格的第$ j $个先验框负责预测该物体，则$ I_{ij}^\text{obj} $为0；若第$ i $个网格的第$ j $个先验框不负责预测该物体，则$ I_{ij}^\text{noobj} $为0。通过调整$ \lambda_\text{coord} $以及$ \lambda_\text{noobj} $来调整两部分的损失权重，使网络更快收敛。

3.2 激光雷达/红外相机数据融合方法

对于在3维激光雷达坐标系中的某一点云坐标$ (x, y, z) $，通过

$ \begin{equation} \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} {f_{u}} & 0 & {u_{0}} \\ 0 & {f_{v}} & {v_{0}} \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} R & t \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix} \end{equation} $

(13)

即可求出红外相机所捕获图像中对应点$ (u, v) $的深度值$ f(u, v)=z $。其中，$ f_{u} $、$ f_{v} $、$ u_{0} $和$ v_{0} $可通过相机内部参数得到，$ R $和$ t $为激光雷达与红外相机之间的外部参数，可通过联合标定得到。由于激光雷达点云较为稀疏，导致投影得到的深度图也较为稀疏，因此采用均值滤波的方式获取稠密深度图。

$ \begin{equation} d(u, v)=\frac{1}{M}\sum\limits_{f\in s} f (u, v) \end{equation} $

(14)

其中，$ M $表示滑动窗口内激光雷达点云投影的个数，$ s $代表均值滤波的当前窗口。

若通过目标识别算法获取到的物体检测框为$ (x_{1}, y_{1}, x_{2}, y_{2}) $，取其中心作为物体在2维平面中的位置，则可计算出其对应深度值，进而通过相机内参矩阵反计算出其在相机坐标系下的位置，实现相对定位功能。

4 试验验证（Experimental verification）

在室内长廊夜晚环境中开展了试验测试，用于验证本文所提出的定位与感知方法。测试系统包含移动机器人平台、IMU、里程计、3维激光雷达、红外相机，如图 4所示。其中，IMU的采样频率为100 Hz，里程计为50 Hz，激光雷达为2 Hz，红外相机为25 Hz。惯性传感器的参数如表 1所示。控制移动机器人在场地中行进，通过在移动机器人上安装棱镜，并利用全站仪对棱镜进行自动跟踪以提供毫米级的定位信息作为移动机器人的轨迹基准。同时，利用全站仪对待检测目标的位置进行标定，用于衡量相对定位精度。试验方案如图 5所示。

4.1 移动机器人自主定位试验

自主定位试验主要用于评估移动机器人的自主定位性能。试验场景为教学楼内的光滑长直走廊，用于模拟地下空间的结构退化环境。本文将松耦合融合方法与所提出的方法进行了对比，$ X $轴向与$ Y $轴向的定位误差如图 6和图 7所示。表 2为$ X $轴向与$ Y $轴向定位的均方根误差对比。可以看出，相比松耦合融合方法，本文方法的定位精度更优，从而验证了本文方法在结构退化环境中的有效性。

图 8~图 10为传感器参数的估计结果，可以看出本文方法能够对传感器参数进行同步估计。

4.2 目标探测识别定位试验

目标探测识别定位试验选择在夜晚环境中进行，用于模拟地下空间的弱光照环境。在试验过程中，首先在场地的不同位置放置待探测目标，如背包、对讲机等。并利用全站仪对每个目标的绝对位置进行标定。利用红外相机采集目标物体图像进行目标识别，并与激光雷达数据融合实现对目标的相对定位。在检测出目标的同时，会得到目标检测的置信度与相对移动机器人的3维位置。由图 11可以看出，基于红外图像的目标检测算法能实现较清晰的成像并识别出待探测目标。同时，对目标检测的相对定位精度进行衡量，结合移动机器人的绝对位置真值以及目标的真值，可以得到目标在移动机器人坐标系的位置真值，通过与检测得到的位置进行对比，3组测试的相对定位误差分析结果如表 3、表 4和表 5所示。可以看出，对目标的相对定位精度可以达到厘米级。

5 结论（Conclusion）

以结构退化与弱光照为主要特征的地下空间环境会给现有的移动机器人探测方法带来新的挑战。本文通过构建基于因子图的激光雷达/里程计/惯性测量单元紧耦合融合方法，能够充分利用原始点云信息进行全局优化，相比传统松耦合融合方法，能够提升定位精度50{%} 以上。同时，提出了基于激光雷达/红外相机融合的目标识别方法，能够对弱光照环境下的目标实现厘米级的相对定位精度，从而为地下空间环境的无人探测提供了一种有效的解决手段。