Positioning method for underground unmanned aerial vehicles in coal mines based on global point cloud map
-
摘要: 即时定位与建图 （SLAM）技术应用于煤矿井下无人机自主定位时，由于采用特征点构建地图，易出现退化问题，导致定位不准确，且因其以机体作为参考坐标系，无法实现全局定位。针对该问题，提出了一种基于全局点云地图的煤矿井下无人机定位方法。以Fast−LIO2算法作为激光SLAM算法，获得无人机位姿估计；采用迭代最近邻算法，对获取的激光雷达实时点云和全局点云地图进行两步匹配，实现无人机位姿校正；针对因点云数量过多导致点云匹配速度无法保证定位实时性的问题，设计了基于时间的位姿输出策略，提高了无人机位姿数据输出频率。在1 000 m煤矿井下巷道中测试无人机定位方法的SLAM精度和位姿校正效果，结果表明：在长距离巷道环境中，Fast−LIO2算法的定位累计误差小于1 m，在600 m以上范围内小于0.3 m，明显小于LOAM−Livox算法和LIO−Livox算法；Fast−LIO2算法输出的位姿估计经校正算法校正后，飞行路径全部位于全局点云地图中，验证了位姿校正算法有效；单次SLAM算法运行耗时14.83 ms，单次位姿校正耗时883 ms，位姿数据输出频率为10 Hz，满足无人机定位实时性要求。Abstract: When simultaneous localization and mapping (SLAM) technology is applied to autonomous positioning of unmanned aerial vehicles in coal mines, the use of feature points to construct maps can easily lead to degradation issues, resulting in inaccurate positioning. Moreover, due to its use of the body as a reference coordinate system, global positioning cannot be achieved. In order to solve the problems, a positioning method for underground unmanned aerial vehicles (UAV) in coal mines based on global point cloud map is proposed. The method uses Fast-LIO2 algorithm as the lidar SLAM algorithm to obtain UAV position and attitude estimation. An iterative nearest-neighbor algorithm is used for two-step matching of the acquired real-time lidar point cloud and the global point cloud map to achieve UAV position and attitude correction. To address the issue of point cloud matching speed not ensuring real-time positioning due to the excessive number of point clouds, a time-based position and attitude output strategy is designed to increase the frequency of outputting UAV position and attitude data. The SLAM precision and position and attitude correction effect of the UAV positioning method are tested in a 1 000 m underground coal mine roadway. The results show that in long-distance roadway environments, the cumulative positioning error of the Fast-LIO2 algorithm is less than 1 m, and is less than 0.3 m in the range of 600 m or more, which is significantly smaller than the cumulative positioning errors of LOAM-Livox algorithm and LIO-Livox algorithm. The position and attitude estimation output by the Fast-LIO2 algorithm has been corrected by the correction algorithm, and all flight paths are located in the global point cloud map, verifying the effectiveness of the position and attitude correction algorithm. The time consumption of single SLAM algorithm operation is 14.83 ms, the one of single position and attitude correction is 883 ms, and the output frequency of position and attitude data is 10 Hz, meeting the real-time requirements of UAV positioning.
-
0. 引言
近年来，无人机在煤矿灾后救援[1-2]、井下探测[3]、设备巡检[4]等领域得到了研究和应用。精确定位是无人机开展各项应用的重要前提。目前民用无人机定位和导航依赖全球导航卫星系统（Global Navigation Satellite System，GNSS）实现。而煤矿井下无法接收到GNSS信号，这对无人机在井下的应用造成了很大困难。近年来，随着即时定位与建图 (Simultaneous Localization and Mapping，SLAM) 技术的发展，无人机已经能在限定条件的无GNSS环境中实现自动巡航飞行，如文献[5-7]将SLAM技术应用于煤矿井下环境中的无人机定位，其中文献[7]采用激光雷达实时建图（Lidar Odometry and Mapping in Real-time，LOAM）算法[8-9]，在精确定位基础上实现了无人机自主避障。为提高SLAM算法性能，文献[10]采用回环检测方法优化了SLAM建图效果；文献[11]引入基于关键帧的滑动窗口及因子图，提高了SLAM定位精度；文献[12]采用卡尔曼滤波器实现惯性测量单元(Inertial Measurement Unit, IMU)数据和激光点云数据的紧耦合，进一步提高了SLAM定位精度。上述研究可在一定程度上提升基于SLAM算法的煤矿井下移动平台自主定位效果[13-16]。但井下巷道是一类典型的缺乏结构约束的环境， 基于特征点云匹配的方法易出现退化问题，导致定位不准确。另外，SLAM算法以机体作为参考坐标系，无法得到全局坐标信息，但实际应用中需要全局坐标信息，用于提供不同架次飞行中获取的具有一致坐标信息的任务数据。鉴此，本文提出一种煤矿井下无人机定位方法，结合激光SLAM和基于全局点云地图的位姿校正，实现了无人机在井下巷道较长范围内的稳定定位。
1. 井下无人机定位方法架构
煤矿井下无人机定位方法主要包括前端激光SLAM算法和后端基于点云地图的位姿校正算法，如图1所示。SLAM处理激光雷达点云数据和IMU输出的角速度、加速度数据，通过激光SLAM算法得到里程计信息，即无人机当前位姿估计。基于点云地图的位姿校正算法使用点云地图确定井下全局坐标系，对无人机的位姿估计误差进行校正，并基于当前点云帧和点云地图计算无人机的全局位姿矩阵。
![]()
图 1 煤矿井下无人机定位方法架构Figure 1. Structure of unmanned aerial vehicles (UAV) positioning method in underground coal mine2. 激光SLAM算法
以LOAM为代表的传统激光SLAM算法常在原始点云中提取边缘特征点和平面特征点，且使用特征点构建地图，提高了运算效率，但在非结构化场景中定位精度较差。Fast−LIO2算法[17-18]采用IMU预积分方法在点云帧采集时间内进行状态传播，使用传播后的状态校正点云失真，进而使用点云构造残差，通过迭代扩展卡尔曼滤波器（Iterated Extended Kalman Filter，IEKF）将激光雷达特征点云和IMU预积分状态进行融合。该算法使用原始点云直接计算局部地图，因此在非结构化或复杂场景中依然保持较高的定位精度；采用新的卡尔曼增益计算公式进行滤波，使计算量依赖于状态量的维度而不是观测量的维度，在保证计算精度的同时，降低了IEKF的运算复杂度；在近邻点搜索及地图构建过程中使用ikd−tree算法，进一步降低了算法运行时间。因此，采用Fast−LIO2算法作为激光SLAM算法，计算井下无人机位姿估计，流程如图2所示。需要说明的是，本节提到的点云地图是激光SLAM算法维护的点云地图，与本文使用的全局点云地图不同。
![]()
图 2 激光SLAM算法的无人机位姿估计流程Figure 2. Flow of UAV position and attitude estimation of lidar simultaneous localization and mapping (SLAM) algorithm3. 基于点云地图的位姿校正算法
3.1 两步匹配算法
为了解决SLAM应用于长廊形巷道易导致无人机位姿漂移的问题，提出了两步匹配算法，依靠激光扫描点云和全局点云地图进行匹配，进而实现无人机位姿校正，具体流程如图3所示。
1） 采用离线方式扫描并建立巷道全局点云地图$ {{{\boldsymbol{P}}}_{{\text{map}}}} $，其包含无人机的全部作业场景。设SLAM坐标系为G，全局点云地图坐标系为${{M}}$，激光雷达坐标系为L，当前时刻tk扫描的激光点云为${{\boldsymbol{P}}}_k^{{L}}$。将激光点云${{\boldsymbol{P}}}_k^{{L}}$和全局点云地图$ {{{\boldsymbol{P}}}_{{\text{map}}}} $进行第1次体素降采样，以降低点云数量和密度，提高后续匹配速度。具体过程：将原始点云空间按边长${d_1}$分割为小正方体（即体素），对每个体素中的点做平均，得到均值点，最终只保留均值点。令第1次体素降采样后的激光点云和全局点云地图分别为${{\boldsymbol{P}}}_k^{{ \prime} {L}}$，${{{\boldsymbol{P}}}_{{\text{map}}}^{ \prime}}$。
2） 通过Fast–LIO2算法获取位姿变换矩阵${\overline {\boldsymbol{T}}_k}$，将$ {{\boldsymbol{P}}}_k^{{ \prime}{L}}$按${\overline {\boldsymbol{T}}_k}$进行变换，得到点云$ {{\boldsymbol{P}}}_k^{{ \prime}{M}}$。对$ {{\boldsymbol{P}}}_k^{{ \prime} {M}}$和${ {{\boldsymbol{P}}}_{{\text{map}}}^{ \prime}}$进行点云粗匹配，得到位姿变换矩阵${\overline {\boldsymbol{T}}_k^\prime} $。点云匹配采用迭代最近点（Iterative Closest Point，ICP）算法，具体过程在3.2节给出。
3） 对激光点云$ {{\boldsymbol{P}}}_k^{{L}}$和全局点云地图$ { {{\boldsymbol{P}}}_{{\text{map}}}} $进行第2次体素降采样，边长为$ {d_2} $，${d_2} ＜ {d_1}$，得到点云$ {{\boldsymbol{P}}}_k^{{\prime \prime } {L}}$和全局点云地图${ {{\boldsymbol{P}}}_{{\text{map}}}^{{\prime \prime }}}$。将$ {{\boldsymbol{P}}}_k^{{\prime \prime } {L}}$按$ {\overline {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k^{ \prime}} $进行变换，得到点云$ {{\boldsymbol{P}}}_k^{{\prime \prime } {M}}$。对$ {{\boldsymbol{P}}}_k^{{\prime \prime } {M}}$和${ {{\boldsymbol{P}}}_{ \text{map}}^{{\prime \prime }}}$进行点云精匹配，得到位姿变换矩阵${\overline {\boldsymbol{T}}_k^{\prime \prime } }$。最终得到全局位姿变换矩阵${{\boldsymbol{T}}_k} = {\overline {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k^\prime} {\overline {\boldsymbol{T}}_k}^{\prime \prime }$。
3.2 基于ICP的点云匹配方法
设待匹配的2组点云分别为$\left\{ {{{{\boldsymbol{s}}}}_1^{{L}}，{{{\boldsymbol{s}}}}_2^{{L}}， \cdots ，{{{\boldsymbol{s}}}}_{{n}}^{{L}}} \right\} $，$\left\{{{\boldsymbol{s}}}_1^{{M}}， {{\boldsymbol{s}}}_2^{{M}}， \cdots， {{\boldsymbol{s}}}_{{n}}^{{M}}\right\} $，${{\boldsymbol{s}}}_{{i}}^{{L}} $为从激光雷达获取的第i个点云，${{\boldsymbol{s}}}_{{i}}^{{M}} $为全局点云地图中${{\boldsymbol{s}}}_{{i}}^{{L}} $的最近邻点云，i=1，2，…，n，n为待匹配点云个数。则ICP目标公式为
$$ \min E\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \min \frac{1}{n}\sum\limits_{i=1}^n {{{\left\| {{\boldsymbol{s}}_i^{{M}} - {\boldsymbol{R}}{'}{\boldsymbol{s}}_i^{{L}} - {\boldsymbol{x}}{'}} \right\|}^2}} $$ (1) 式中：$E\left( {{\boldsymbol{R}}^{ \prime},{\boldsymbol{x}}^{ \prime}} \right)$为代价函数；${\boldsymbol{R}}^{ \prime}$为旋转矩阵；${\boldsymbol{x}}^{ \prime}$为平移向量。
设${\overline {\boldsymbol{s}}^{{M}}}$，${\overline {\boldsymbol{s}}^{{L}}} $分别为全局点云地图和激光雷达点云的质心，令${\boldsymbol{A}} = {\overline {\boldsymbol{s}}^{{M}}} - {\boldsymbol{R}}{'}{\overline {\boldsymbol{s}}^{{L}}}$，则$E\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right)$可进一步表示为
$$\begin{split} & E\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \frac{1}{n}\sum\limits_{i = 1}^n {\left\| {{\boldsymbol{s}}_i^{{M}} - {\boldsymbol{R}}{'}{\boldsymbol{s}}_i^{{L}} - {\boldsymbol{x}}{'} + {\boldsymbol{A}} - {\boldsymbol{A}}} \right\|} = \\ & \qquad \frac{1}{n}\sum\limits_{i = 1}^n {\left( {{{\left\| {{\boldsymbol{s}}_i^{{M}} - {{\overline {\boldsymbol{s}}}^{{M}}} -{\boldsymbol{ R}}{'}\left( {{\boldsymbol{s}}_i^{{L}} - {{\overline {\boldsymbol{s}}}^{{L}}}} \right)} \right\|}^2}} \right)} + {\left\| {{{\overline {\boldsymbol{s}}}^{{M}}} - {\boldsymbol{R}}{'}{{\overline {\boldsymbol{s}}}^{{L}}} - {\boldsymbol{x}}{'}} \right\|^2} \end{split} $$ (2) 由于${\overline {\boldsymbol{s}}^{{M}}} = \dfrac{1}{n}\displaystyle \sum\limits_{i = 1}^n {{\boldsymbol{s}}_i^{{M}}}$，${\overline {\boldsymbol{s}}^{{L}}} = \dfrac{1}{n}\displaystyle \sum\limits_{i = 1}^n {{\boldsymbol{s}}_i^{{L}}}$，所以$E\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right)$可简化为
$$ E\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = {E_1}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) + {E_2}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) $$ (3) $$ {E_1}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \frac{1}{n}\sum\limits_{i = 1}^n {\left( {{{\left\| {{\boldsymbol{s}}_i^{{M}} - {{\overline {\boldsymbol{s}}}^{{M}}} - {\boldsymbol{R}}{'}\left( {{\boldsymbol{s}}_i^{{L}} - {{\overline {\boldsymbol{s}}}^{{L}}}} \right)} \right\|}^2}} \right)} $$ (4) $$ {E_2}\left( {{\boldsymbol{R}}',{\boldsymbol{x}}{'}} \right) = {\left\| {{{\overline {\boldsymbol{s}}}^{{M}}} - {\boldsymbol{R}}{'}{{\overline {\boldsymbol{s}}}^{{L}}} - {\boldsymbol{x}}{'}} \right\|^2} $$ (5) ${E_1}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right)$只与旋转有关，${E_2}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right)$只与平移有关。
设${\boldsymbol{s}} _i^{\prime {M}} = {\boldsymbol{s}}_i^{{M}} - {\overline {\boldsymbol{s}}^{{M}}}$，${\boldsymbol{s}} _i^{ \prime {L}} = {\boldsymbol{s}}_i^{{L}} - {\overline {\boldsymbol{s}}^{{L}}}$，则有
$$\begin{split} & {E_1}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \frac{1}{n}\sum\limits_{i = 1}^n {\left( {{{\left\| {{\boldsymbol{s}}{'}_{ i}^{{M}} - {\boldsymbol{R}}{'}{\boldsymbol{s}}{'}_{ i}^{{L}}} \right\|}^2}} \right)} = \\& \quad \frac{1}{n}\sum\limits_{i = 1}^n {\left( {{{\left( {{\boldsymbol{s}}{'}_{ i}^{{M}}} \right)}^{{{\rm{T}}}}}{\boldsymbol{s}}{'}_{ i}^{{M}} - {{\left( {{\boldsymbol{s}}{'}_{ i}^{{L}}} \right)}^{{{\rm{T}}}}}{{{\boldsymbol{R}}{'}}^{{{\rm{T}}}}}{\boldsymbol{R}}{'}{\boldsymbol{s}}{'}_{ i}^{{L}} - 2{{\left( {{\boldsymbol{s}}{'}_{ i}^{{M}}} \right)}^{{{\rm{T}}}}}{\boldsymbol{R}}{'}{\boldsymbol{s}}{'}_{ i}^{{L}}} \right)} \end{split} $$ (6) ${\left( {{\boldsymbol{s}} _i^{\prime {M}}} \right)^{\text{T}}}{\boldsymbol{s}} _i^{\prime {M}}$与${\boldsymbol{R}}{'}$无关，${{\boldsymbol{R}} ^{\prime \text{T}}}{\boldsymbol{R}}{'}$为三阶单位矩阵。令${E_1'}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) =\displaystyle \sum\limits_{i = 1}^n {{{\left( {{\boldsymbol{s}} _i^{\prime {M}}} \right)}^{\text{T}}}{\boldsymbol{R}}{'}{\boldsymbol{s}}{'}_{ i}^{{L}}}$，则有
$$ {\text{arg}}\mathop {{\text{min}}}\limits {E_1}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \arg \mathop {\max }\limits {E_1'}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) $$ (7) 又因为
$$ \begin{split} & {E_1'}\left( {{\boldsymbol{R}}{'},{\boldsymbol{x}}{'}} \right) = \sum\limits_{i = 1}^n {{{\left( {{\boldsymbol{s}}_i^{\prime{M}}} \right)}^{\text{T}}}{\boldsymbol{R}}{'}{\boldsymbol{s}}_i^{\prime{L}}} = \sum\limits_{i = 1}^n {{\text{tr}}\left( {{{\left( {{\boldsymbol{s}}_i^{\prime{M}}} \right)}^{\text{T}}}{\boldsymbol{R}}{'}{\boldsymbol{s}}_i^{\prime{L}}} \right)}= \\& \quad \sum\limits_{i = 1}^n {{\text{tr}}\left( {{\boldsymbol{R^ \prime s}}_i^{\prime{L}}{{\left( {{\boldsymbol{s}}_i^{\prime{M}}} \right)}^{\text{T}}}} \right)} = {\text{tr}}\sum\limits_{i = 1}^n {{\boldsymbol{R}}{'}{\boldsymbol{s}}_i^{\prime{L}}{{\left( {{\boldsymbol{s}}_i^{\prime{M}}} \right)}^{\text{T}}}} = {\text{tr}}\left( {{\boldsymbol{R}}{'}{\boldsymbol{Z}}} \right) \end{split} $$ (8) $${\boldsymbol{ Z }}= \sum\limits_{i = 1}^n {{\boldsymbol{s}}_i^{\prime{L}}{{\left( {{\boldsymbol{s}}_i^{\prime{M}}} \right)}^{\text{T}}}} $$ (9) 由此将求解2组点云的位姿变换问题转换为求使${\text{tr}}\left( {{\boldsymbol{R}}{'}{\boldsymbol{Z}}} \right)$最大的${\boldsymbol{R}}{'}$。
对Z 进行奇异值分解(Singular Value Decomposition，SVD)，得
$${\boldsymbol{ Z}} = {\boldsymbol{U}}{\boldsymbol{\varSigma}} {{\boldsymbol{V}}^{\text{T}}} $$ (10) 式中：$ {\boldsymbol{\varSigma }}$为由SVD得到的奇异值构成的对角矩阵；$ {\boldsymbol{U }}$，$ {{\boldsymbol{V}}} $为${\boldsymbol{ \varSigma}} $对应的正交矩阵。
取${\boldsymbol{R}}{'} = {\boldsymbol{V}}{{\boldsymbol{U}}^{\text{T}}}$，将${\boldsymbol{R}}{'}$代入式（5）得到平移向量${\boldsymbol{x}}{'}$，进而得到全局点云地图和当前时刻激光点云的全局位姿变换矩阵$ {{\boldsymbol{T}}_k} $。
3.3 基于时间的位姿输出策略
由于点云匹配耗费的时间和点的数量呈正相关，当点数量过多时，点云匹配算法的运行速度无法保证定位数据的实时性。对此，设计了基于时间的位姿输出策略，在满足位姿精度要求的前提下，保证位姿的输出频率，具体流程如下。
1） 设定点云匹配算法的运行频率f，单次匹配算法运行时间t应满足$ t ＞ {1}/{f} $。
2） 获取激光SLAM算法得到的位姿变换矩阵${\overline {\boldsymbol{T}}_k}$。
3） 通过点云匹配得到当前时刻全局位姿变换矩阵$ {{\boldsymbol{T}}_k} $，则SLAM坐标系到点云地图坐标系的位姿变换矩阵${\boldsymbol{T}}_k^{{{{{G}} - {{M}}}}} = {{\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k}$。
4） 设下一次运行点云匹配算法的时刻为tm，将tk到tm之间所有的位姿变换矩阵$\left\{ {{{\overline{\boldsymbol{ T}}}_{k + 1}},{{\overline{\boldsymbol{ T}}}_{k + 2}}, \cdots ,{{\overline {\boldsymbol{T}}}_m}} \right\}$全部按${\boldsymbol{T}}_k^{{{{{G}} - {{M}}}}}$进行位姿变换，得到全局位姿变换矩阵$\left\{ {{{\hat {\boldsymbol{T}}}_{k + 1}},{{\hat {\boldsymbol{T}}}_{k + 2}}, \cdots ,{{\hat {\boldsymbol{T}}}_m}} \right\}$。之后转步骤3）。
需要注意的是，定位系统初始运行时全局位姿变换矩阵的初始经验值$ {\hat {\boldsymbol{T}}_1} $需人为给出。
位姿变换算法伪代码如下。
算法1：位姿变换算法 输入：点云地图$ {{{\boldsymbol{P}}}_{{\text{map}}}} $， 当前时刻的扫描点云${{\boldsymbol{P}}}_k^{{L}}$， SLAM位姿变换矩阵${\overline {\boldsymbol{T}}_k}$， 当前时刻${t_k}$， 上一次运行位姿校正程序的时间${t_j}$， SLAM坐标系到全局点云地图坐标系的位姿变换矩阵${{\boldsymbol{T}}_k ^{{{G - M}}}}$。输出：当前时刻的全局位姿变换矩阵${{\boldsymbol{T}}_k}$。 1 预测位姿变换矩阵${\hat {\boldsymbol{T}}_k} = {{\boldsymbol{T}}_k^{G - M}}{{\boldsymbol{T}}_k}$； 2 If ${t_k} - {t_j} ＞ 1/f$ then 3 将${{{\boldsymbol{P}}}_{{\text{map}}}}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^\prime} $和${{\boldsymbol{P}}}_k^{ \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime {L}}$按${\hat {\boldsymbol{T}}_k}$变换为${{\boldsymbol{P}}}_k^{ \prime {M}}$； 4 使用ICP算法对$ {{{\boldsymbol{P}}}_{ {\text{map}}}^ \prime} $和${{\boldsymbol{P}}}_k^{ \prime{M}}$进行粗匹配，得到${\overline {\boldsymbol{T }}_k^\prime}$； 5 将${M}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime \prime{L}}$按${\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}{}_k^{\prime }}$变换为${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$； 6 使用ICP算法对$ {{{\boldsymbol{P}}} _{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$进行精匹配，得到${\overline {\boldsymbol{T}}_k^{\prime \prime}}$； 7 全局位姿变换矩阵${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k^{\prime }}{\overline {\boldsymbol{T}}{}_k^{\prime \prime}}$； 8 ${\boldsymbol{T}}_k^{{{G } -{ M}}} = {{\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k}$ 9 else 10 ${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}$ 11 final 12 return ${{\boldsymbol{T}}_k}$ 4. 试验验证
4.1 硬件架构
试验用无人机硬件包括无人机平台、机载计算机、三维激光雷达，如图4所示。无人机平台包括飞行控制器及动力系统，采用X型四旋翼布局，如图5所示。无人机平台对称轴距为600 mm，最大起飞质量为4 kg，搭载Livox–AVIA雷达采集环境点云数据和IMU数据，通过网口将点云数据和IMU数据发送至khadas–vim3机载计算机。机载计算机对接收数据进行定位程序处理，计算无人机当前位姿，并实时规划无人机飞行轨迹，将当前位姿和飞行路径发送至飞行控制器，实现无人机精准定位及位置控制。
4.2 SLAM精度试验对比
受井下环境条件的限制，难以布置测量设备得到无人机飞行位置的坐标真值，因此直接评估定位精度成本和代价非常高，也不是本文重点。巷道是典型的长廊形结构，由于其在长度方向上缺乏明显的结构特征，所以激光SLAM容易出现定位漂移。为了分析和验证本文算法在长廊形巷道中的定位精度，设计以下试验过程。
以某矿辅助运输大巷为试验环境，试验巷道长度为1 000 m，巷道较平直，无明显转弯，无照明，无大型设备。标记无人机起飞位置a和降落位置b，用100 m钢卷尺从位置a开始，沿巷道中线每100 m测量标记1个位置点，记为$ \left\{ {{a_1}，{a_2}， \cdots， {a_9}，b} \right\} $。无人机从位置a起飞至距地面1 m高度时记录时间戳t0，控制无人机沿巷道平飞。飞行过程中记录激光雷达点云扫描数据、IMU原始数据，以及到达标记位置和终点的时间戳$\left\{ {{t_0}，{t_1}，\cdots ，{t_9}，{t_b}} \right\}$。采用不同的SLAM算法处理飞行过程中记录的数据，主要步骤如下：
1） 记录SLAM算法在$\left\{ {{t_0}，{t_1}， \cdots ，{t_9}，{t_b}} \right\}$时刻得到的位姿数据$\left\{ {{{\boldsymbol{T}}_0}，{{\boldsymbol{T}}_1}， \cdots， {{\boldsymbol{T}}_9}，{{\boldsymbol{T}}_b}} \right\}$。
2） 依次计算$\left\{ {{t_1}，{t_2}， \cdots ，{t_9}，{t_b}} \right\}$时刻的里程计数据与${t_0}$时刻里程计数据差值$d = \left\{ {{d_1}， {d_2}，\cdots ，{d_9}，{d_b}} \right\}$。
3） 计算d和各标记点真实里程的误差。
为验证本文算法在巷道环境中定位的稳定性，同时消除试验设置的特殊性，分别从试验巷道200，400，600 m处作为起点，以b点为终点进行试验，记录途经标记点的真实里程误差。试验场景如图6所示。
采用LOAM−Livox算法[19]、LIO−Livox算法[20]和本文算法进行里程精度对比。LOAM−Livox算法仅使用激光点云作为输入，是LOAM算法针对Livox激光雷达的改进版本；LIO−Livox是一种激光点云和IMU数据紧耦合的算法。
3种SLAM算法的建图效果如图7所示，在4组试验中的定位误差见表1，误差曲线如图8所示。可看出3种算法的定位误差总体上随飞行距离不断累计，在标记a点起飞试验100 m标记处、标记200 m处起飞试验400 m和500 m标记处，LIO−Livox算法的误差较小，但在600 m以上范围内本文算法的累计误差明显小于LOAM−Livox算法和LIO−Livox算法，且在长距离定位场景中误差绝对值始终小于1 m，验证了本文算法在长距离巷道环境中具有较高的定位精度。特别指出的是，在大于600 m距离的试验巷道场景中，本文算法的定位误差始终小于0.3 m，满足实际应用要求。
表 1 不同SLAM算法定位误差对比Table 1. Positioning error comparison of different SLAM algorithms试验条件 误差/m 100 m处 200 m处 300 m处 400 m处 500 m处 600 m处 700 m处 800 m处 900 m处 1 000 m处 从标记a点起飞 LOAM−Livox 0.16 −0.31 0.20 −0.25 −6.18 −11.90 −27.84 −60.76 −62.10 −63.82 LIO−Livox −0.04 −0.41 −0.50 −1.15 −2.08 −2.36 −2.25 −3.31 −5.47 −7.28 本文算法 0.27 0.10 0.38 0.36 −0.18 −0.02 0.35 0.38 0.27 −0.76 从标记200 m处起飞 LOAM−Livox − − 0.69 0.32 −5.27 −11.17 −26.51 −62.41 −63.91 −65.91 LIO−Livox − − 0.64 0.05 −0.04 −1.20 −1.02 −1.82 −4.78 −6.80 本文算法 − − 0.29 0.27 −0.28 −0.12 0.25 0.29 0.18 −0.86 从标记400 m处起飞 LOAM−Livox − − − − −9.32 −13.84 −28.72 −64.62 −66.22 −68.52 LIO−Livox − − − − −1.50 −1.97 −2.03 −3.08 −7.15 −9.30 本文算法 − − − − −0.28 −0.40 −0.03 0.01 −0.10 −0.58 从标记600 m处起飞 LOAM−Livox − − − − − − −17.48 −50.84 −51.72 −53.58 LIO−Livox − − − − − − −0.19 −1.61 −4.72 −6.76 本文算法 − − − − − − −0.13 0.02 −0.13 −0.55 4.3 位姿校正试验
由于SLAM算法定位以起飞时刻的位姿作为定位坐标系，导致每次飞行中使用的坐标系不同。为验证提出的位姿校正算法的有效性，设计了以下试验：使用Fast−LIO2算法建立巷道的全局点云地图并保存，使用该地图确定井下全局坐标系；将无人机起飞位置偏转一定角度，重新沿巷道飞行，记录Fast−LIO2算法输出的位置数据和本文位姿校正算法输出的位置数据。试验结果如图9所示，蓝色为校正前的路径，红色为校正后的路径。可看出校正前的路径偏离至全局点云地图之外，校正后的路径全部位于全局点云地图中，验证了算法的有效性。10个标记点坐标见表2。
![]()
图 9 位姿校正前后无人机飞行路径对比Figure 9. Comparison of UAV flight routes before and after position and attitude correction表 2 位姿校正前后标记点坐标Table 2. Coordinate of label points before and after position and attitude correction位置 校正前坐标/m 校正后坐标/m X Y Z X Y Z 100 m处 100.193 −3.810 −0.672 99.967 −0.326 3.497 200 m处 199.877 −7.530 −5.523 200.106 −0.513 2.860 300 m处 299.978 −11.356 −10.658 300.008 −0.856 2.036 400 m处 399.728 −14.856 −16.975 400.102 −0.845 0.071 500 m处 499.157 −18.536 −17.829 500.070 −1.053 3.640 600 m处 599.201 −22.067 −21.024 600.035 −1.172 4.899 700 m处 699.390 −24.916 −26.742 700.184 −0.703 3.668 800 m处 799.140 −28.294 −34.422 800.051 −0.821 0.595 900 m处 898.886 −30.942 −39.222 900.065 −0.124 0.395 1000 m处 997.655 −34.066 −44.636 998.930 −0.014 −0.082 4.4 算法复杂度分析和实时性试验
Fast−LIO2算法复杂度分析详见文献[17-18]，算法中主要包括IEKF算法和ikd−tree算法。传统的IEKF算法的时间复杂度和观测量相关，设观测量维度为$r$，时间复杂度为$\mathcal{O}\left( {{r^2}} \right)$，而Fast−LIO2算法采用新的卡尔曼增益计算公式，时间复杂度和状态量维度直接相关，在保证运算精度的同时，降低了时间复杂度。ikd−tree算法主要实现地图的增量操作、地图重建和K近邻搜索，设ikd−tree的尺寸为${N_{{\text{tree}}}}$，则增量操作的时间复杂度为$\mathcal{O}\left( {\log_2 {N_{{\text{tree}}}}} \right)$，地图重建的时间复杂度为$\mathcal{O}\left( {{N_{{\text{tree}}}}} \right)$，K近邻搜索的时间复杂度为$\mathcal{O}\left( {{N_{{\text{tree}}}}\log_2 {N_{{\text{tree}}}}} \right)$。
ICP算法的时间复杂度依赖于源点云和目标点云的大小，设${N_{{\rm{lidar}}}}$为激光雷达某一点云帧中的三维点数，${N_{{\rm{map}}}}$为全局点云地图中三维点数，则ICP算法单次迭代的时间复杂度为$\mathcal{O}\left( {{N_{{\rm{lidar}}}}{N_{{\rm{map}}}}} \right)$，详细分析见文献[21]。
机载计算机CPU为Amlogic−A311D（ARM架构、4核2.2 GHz+双核1.8 GHz），激光雷达数据帧为10 Hz，根据实测，位姿校正算法每20 s运行1次即可满足要求。4.2节和4.3节的试验过程中，记录各步骤运行的平均时间，结果见表3。单次SLAM算法运行耗时14.83 ms，单次位姿校正耗时883 ms。位姿校正算法和激光SLAM算法分别在2个线程中运行，最终整体计算输出的位姿数据频率为10 Hz，而低速无人机位置控制频率不低于5 Hz即可满足实时性，因此本文方法能够满足实时性要求。
表 3 算法单个步骤单次运行耗时Table 3. Time consumption of single operation in single step of the algorithm步骤 运行频率/Hz 运行耗时/ms SLAM 预处理 10 0.05 位姿估计 10 14.35 建图 10 0.43 位姿校正 0.05 883 5. 结论
1） 提出的井下无人机定位方法采用基于IEKF的激光雷达和IMU融合SLAM获得无人机位姿估计，基于点云地图和ICP点云匹配对无人机位姿进行校正，从而得到无人机井下全局位姿数据。
2） 在1 000 m井下巷道环境中开展无人机定位试验，比较了LOAM−Livox算法、LIO−Livox算法和本文算法在相同数据集上的结果差异，验证了本文方法具有更高的定位精度和稳定性。
-
![]()
图 1 煤矿井下无人机定位方法架构
Figure 1. Structure of unmanned aerial vehicles (UAV) positioning method in underground coal mine
![]()
图 2 激光SLAM算法的无人机位姿估计流程
Figure 2. Flow of UAV position and attitude estimation of lidar simultaneous localization and mapping (SLAM) algorithm
![]()
图 9 位姿校正前后无人机飞行路径对比
Figure 9. Comparison of UAV flight routes before and after position and attitude correction
算法1：位姿变换算法 输入：点云地图$ {{{\boldsymbol{P}}}_{{\text{map}}}} $， 当前时刻的扫描点云${{\boldsymbol{P}}}_k^{{L}}$， SLAM位姿变换矩阵${\overline {\boldsymbol{T}}_k}$， 当前时刻${t_k}$， 上一次运行位姿校正程序的时间${t_j}$， SLAM坐标系到全局点云地图坐标系的位姿变换矩阵${{\boldsymbol{T}}_k ^{{{G - M}}}}$。输出：当前时刻的全局位姿变换矩阵${{\boldsymbol{T}}_k}$。 1 预测位姿变换矩阵${\hat {\boldsymbol{T}}_k} = {{\boldsymbol{T}}_k^{G - M}}{{\boldsymbol{T}}_k}$； 2 If ${t_k} - {t_j} ＞ 1/f$ then 3 将${{{\boldsymbol{P}}}_{{\text{map}}}}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^\prime} $和${{\boldsymbol{P}}}_k^{ \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime {L}}$按${\hat {\boldsymbol{T}}_k}$变换为${{\boldsymbol{P}}}_k^{ \prime {M}}$； 4 使用ICP算法对$ {{{\boldsymbol{P}}}_{ {\text{map}}}^ \prime} $和${{\boldsymbol{P}}}_k^{ \prime{M}}$进行粗匹配，得到${\overline {\boldsymbol{T }}_k^\prime}$； 5 将${M}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime \prime{L}}$按${\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}{}_k^{\prime }}$变换为${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$； 6 使用ICP算法对$ {{{\boldsymbol{P}}} _{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$进行精匹配，得到${\overline {\boldsymbol{T}}_k^{\prime \prime}}$； 7 全局位姿变换矩阵${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k^{\prime }}{\overline {\boldsymbol{T}}{}_k^{\prime \prime}}$； 8 ${\boldsymbol{T}}_k^{{{G } -{ M}}} = {{\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k}$ 9 else 10 ${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}$ 11 final 12 return ${{\boldsymbol{T}}_k}$ 
下载: 导出CSV
表 1 不同SLAM算法定位误差对比
Table 1 Positioning error comparison of different SLAM algorithms
试验条件 误差/m 100 m处 200 m处 300 m处 400 m处 500 m处 600 m处 700 m处 800 m处 900 m处 1 000 m处 从标记a点起飞 LOAM−Livox 0.16 −0.31 0.20 −0.25 −6.18 −11.90 −27.84 −60.76 −62.10 −63.82 LIO−Livox −0.04 −0.41 −0.50 −1.15 −2.08 −2.36 −2.25 −3.31 −5.47 −7.28 本文算法 0.27 0.10 0.38 0.36 −0.18 −0.02 0.35 0.38 0.27 −0.76 从标记200 m处起飞 LOAM−Livox − − 0.69 0.32 −5.27 −11.17 −26.51 −62.41 −63.91 −65.91 LIO−Livox − − 0.64 0.05 −0.04 −1.20 −1.02 −1.82 −4.78 −6.80 本文算法 − − 0.29 0.27 −0.28 −0.12 0.25 0.29 0.18 −0.86 从标记400 m处起飞 LOAM−Livox − − − − −9.32 −13.84 −28.72 −64.62 −66.22 −68.52 LIO−Livox − − − − −1.50 −1.97 −2.03 −3.08 −7.15 −9.30 本文算法 − − − − −0.28 −0.40 −0.03 0.01 −0.10 −0.58 从标记600 m处起飞 LOAM−Livox − − − − − − −17.48 −50.84 −51.72 −53.58 LIO−Livox − − − − − − −0.19 −1.61 −4.72 −6.76 本文算法 − − − − − − −0.13 0.02 −0.13 −0.55
下载: 导出CSV
表 2 位姿校正前后标记点坐标
Table 2 Coordinate of label points before and after position and attitude correction
位置 校正前坐标/m 校正后坐标/m X Y Z X Y Z 100 m处 100.193 −3.810 −0.672 99.967 −0.326 3.497 200 m处 199.877 −7.530 −5.523 200.106 −0.513 2.860 300 m处 299.978 −11.356 −10.658 300.008 −0.856 2.036 400 m处 399.728 −14.856 −16.975 400.102 −0.845 0.071 500 m处 499.157 −18.536 −17.829 500.070 −1.053 3.640 600 m处 599.201 −22.067 −21.024 600.035 −1.172 4.899 700 m处 699.390 −24.916 −26.742 700.184 −0.703 3.668 800 m处 799.140 −28.294 −34.422 800.051 −0.821 0.595 900 m处 898.886 −30.942 −39.222 900.065 −0.124 0.395 1000 m处 997.655 −34.066 −44.636 998.930 −0.014 −0.082
下载: 导出CSV
表 3 算法单个步骤单次运行耗时
Table 3 Time consumption of single operation in single step of the algorithm
步骤 运行频率/Hz 运行耗时/ms SLAM 预处理 10 0.05 位姿估计 10 14.35 建图 10 0.43 位姿校正 0.05 883
下载: 导出CSV
-
[1] 郑学召,童鑫,张铎,等. 矿井危险区域多旋翼侦测无人机关键技术探讨[J]. 工矿自动化,2020,46(12):48-56. DOI: 10.13272/j.issn.1671-251x.17653 ZHENG Xuezhao,TONG Xin,ZHANG Duo,et al. Discussion on key technologies of multi-rotor detection UAVs in mine dangerous area[J]. Industry and Mine Automation,2020,46(12):48-56. DOI: 10.13272/j.issn.1671-251x.17653
[2] 吕文红,夏双双,魏博文,等. 基于改进A*算法的灾后井下无人机航迹规划[J]. 工矿自动化,2018,44(5):85-90. LYU Wenhong,XIA Shuangshuang,WEI Bowen,et al. Route planning of unmanned aerial vehicle in post-disaster underground based on improved A* algorithm[J]. Industry and Mine Automation,2018,44(5):85-90.
[3] 张铎,吴佩利,郑学召,等. 矿井侦测无人机研究现状与发展趋势[J]. 工矿自动化,2020,46(7):76-81. ZHANG Duo,WU Peili,ZHENG Xuezhao,et al. Research status and development trend of mine detection unmanned aerial vehicle[J]. Industry and Mine Automation,2020,46(7):76-81.
[4] 李标. 基于无人机技术的煤矿带式输送机巡检方案[J]. 煤矿安全,2020,51(7):128-131. LI Biao. Inspection scheme of coal mine belt conveyor based on UAV technology[J]. Safety in Coal Mines,2020,51(7):128-131.
[5] 王岩,马宏伟,王星,等. 基于迭代最近点的井下无人机实时位姿估计[J]. 工矿自动化,2019,45(9):25-29. WANG Yan,MA Hongwei,WANG Xing,et al. Real-time pose estimation of underground unmanned aerial vehicle based on ICP method[J]. Industry and Mine Automation,2019,45(9):25-29.
[6] 夏双双,殷立杰. 煤矿井下无人机SLAM定位算法研究[J]. 电子质量,2017(12):56-61,66. DOI: 10.3969/j.issn.1003-0107.2017.12.015 XIA Shuangshuang,YIN Lijie. Research on SLAM location algorithm of downhole UAV[J]. Electronics Quality,2017(12):56-61,66. DOI: 10.3969/j.issn.1003-0107.2017.12.015
[7] 江传龙,黄宇昊,韩超,等. 井下巡检无人机系统设计及定位与避障技术[J]. 机械设计与研究,2021,37(4):38-42,48. JIANG Chuanlong,HUANG Yuhao,HAN Chao,et al. Design of underground inspection UAV system and studyof positioning and obstacle avoidance[J]. Machine Design & Research,2021,37(4):38-42,48.
[8] ZHANG Ji, SINGH S. Visual-lidar odometry and mapping: low-drift, robust, and fast[C]. IEEE International Conference on Robotics and Automation, Piscataway, 2015: 2174-2181.
[9] ZHANG Ji,SINGH S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots,2017,41(2):401-416. DOI: 10.1007/s10514-016-9548-2
[10] SHAN Tixiao, ENGLOT B. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, Piscataway, 2018: 4758-4765.
[11] SHAN Tixiao, ENGLOT B, MEYERS D, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping[C]. IEEE International Conference on Intelligent Robots and Systems, Las Vegas, 2020: 5135-5142.
[12] CHAO Qin, YE Haoyang, PRANATA C E, et al. LINS: a lidar-inertial state estimator for robust and efficient navigation[C]. IEEE International Conference on Robotics and Automation, Piscataway, 2020: 8899-8906.
[13] 杨林,马宏伟,王岩. 基于激光惯性融合的煤矿井下移动机器人SLAM算法[J]. 煤炭学报,2022,47(9):3523-3534. DOI: 10.13225/j.cnki.jccs.2022.0506 YANG Lin,MA Hongwei,WANG Yan. LiDAR-inertial SLAM for mobile robot in underground coal mine[J]. Journal of China Coal Society,2022,47(9):3523-3534. DOI: 10.13225/j.cnki.jccs.2022.0506
[14] 邹筱瑜,黄鑫淼,王忠宾,等. 基于集成式因子图优化的煤矿巷道移动机器人三维地图构建[J]. 工矿自动化,2022,48(12):57-67,92. DOI: 10.13272/j.issn.1671-251x.2022100041 ZOU Xiaoyu,HUANG Xinmiao,WANG Zhongbin,et al. 3D map construction of coal mine roadway mobile robot based on integrated factor graph optimization[J]. Journal of Mine Automation,2022,48(12):57-67,92. DOI: 10.13272/j.issn.1671-251x.2022100041
[15] 李猛钢,胡而已,朱华. 煤矿移动机器人LiDAR/IMU紧耦合SLAM方法[J]. 工矿自动化,2022,48(12):68-78. DOI: 10.13272/j.issn.1671-251x.2022100061 LI Menggang,HU Eryi,ZHU Hua. LiDAR/IMU tightly-coupled SLAM method for coal mine mobile robot[J]. Journal of Mine Automation,2022,48(12):68-78. DOI: 10.13272/j.issn.1671-251x.2022100061
[16] 马艾强,姚顽强,蔺小虎,等. 面向煤矿巷道环境的LiDAR与IMU融合定位与建图方法[J]. 工矿自动化,2022,48(12):49-56. DOI: 10.13272/j.issn.1671-251x.2022070007 MA Aiqiang,YAO Wanqiang,LIN Xiaohu,et al. Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method[J]. Journal of Mine Automation,2022,48(12):49-56. DOI: 10.13272/j.issn.1671-251x.2022070007
[17] XU Wei,ZHANG Fu. FAST-LIO:a fast,robust LiDAR-Inertial odometry package by tightly-coupled iterated Kalman filter[J]. IEEE Robotics and Automation Letters,2021,6(2):3317-3324. DOI: 10.1109/LRA.2021.3064227
[18] XU Wei,CAI Yixi,HE Dongjiao,et al. FAST-LIO2:fast direct LiDAR-Inertial odometry[J]. IEEE Transactions on Robotics,2022,38(4):2053-2073. DOI: 10.1109/TRO.2022.3141876
[19] LIN Jiarong, ZHANG Fu. LOAM Livox: a fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV[C]. IEEE International Conference on Robotics and Automation, Paris, 2020: 3126- 3131.
[20] GitHub-Livox-SDK/LIO-Livox: a robust LiDAR-inertial odometry for Livox LiDAR[EB/OL]. [2022-12-22]. https://github.com/Livox-SDK/LIO-Livox.
[21] BESL P,MCKAY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256. DOI: 10.1109/34.121791
-
期刊类型引用(2)
1. 赵亚东，马腾飞，思旺斗，王猛. 煤矿井下移动机器人同步定位关键技术研究. 煤矿机械. 2024(02): 48-51 . 
百度学术
2. 蒋水华，余琦，黄河，常志璐，孟京京. 岩质高边坡结构面识别及产状统计信息采集方法. 工矿自动化. 2024(07): 156-164 . 本站查看
其他类型引用(0)
计量
- 文章访问数: 666
- HTML全文浏览量: 56
- PDF下载量: 44
- 被引次数: 2
