0 引言

传统的煤矿井下地面移动式巡检设备存在以下问题：一是占用通道，二是面对水域、地面泥泞或障碍不可逾越等情况时空间运动能力不足。随着无人机技术的快速发展，利用先进的无人机技术协助煤矿井下相关工作成为可能[1-2]。四旋翼无人机体积小、质量轻，相比于固定翼式无人机，具有优秀的垂直起降与定点悬停能力，同时比其他类型的无人机(涵道无人机、直升机、超多旋翼无人机等)更易控制[3-4]。因此，本文选用四旋翼无人机作为研究对象。

实时自主导航功能是衡量煤矿井下无人机自动化、智能化作业能力的重要指标。无人机实时运行过程中6自由度位置与姿态解算是解决无人机自主导航时定位问题的关键。通常无人机需要在GPS(Global Positioning System，全球定位系统)信号良好的开阔室外环境下，由飞手遥控飞行[5]。GPS提供经纬度信息，气压计提供高度信息，这些信息可作为无人机的位置输入，陀螺仪与加速度计通过积分计算出欧拉角作为无人机姿态输入[6]。但煤矿井下没有GPS信号，因此，需要增加相应传感器来实现无人机实时位姿估计。

本文针对煤矿井下无GPS信号、低照度、结构化环境等特点[7]，提出了基于ICP(Iterative Closest Point，迭代最近点)的井下无人机实时位姿估计方法。采用三维激光雷达作为四旋翼无人机机载环境测量传感器，通过对2个时刻的点云数据进行扫描和匹配解算，得到2帧点云数据之间的相对变换矩阵[8]，经过迭代求解连续关键帧点云数据，得到煤矿井下四旋翼无人机实时位姿估计结果。

1 四旋翼无人机运动模型与激光雷达观测模型

1.1 四旋翼无人机坐标系与三维激光雷达坐标系

选用16线激光雷达，固定于四旋翼无人机顶部中心位置，单次扫描可获得360°范围数据。选取四旋翼无人机机架质心与激光雷达扫描位中心，分别作为无人机坐标系原点与激光雷达坐标系原点，如图1所示，其中红、绿、蓝分别对应x轴、y轴、z轴。由于激光雷达固定于无人机机架，属于刚性连接，所以，两坐标系之间无相对旋转与位移。激光雷达通过扫描匹配算法解算关键帧点云数据间的变换矩阵，通过变换矩阵进行四旋翼无人机坐标系下的无人机运动位姿估计。

图1 四旋翼无人机与三维激光雷达坐标系
Fig.1 Quadcopter UAV coordinate and lidar coordinate

1.2 煤矿井下四旋翼无人机运动模型

煤矿井下四旋翼无人机的运动属于三维空间下的刚体运动，由旋转与平移组成，不同时刻刚体坐标系之间的运动由欧氏变换描述[9]。旋转有欧拉角、旋转向量、旋转矩阵、四元数等描述方式，本文采用数学性质更好的旋转矩阵作为旋转描述符。平移由三维向量描述[10]。旋转矩阵与平移三维向量联立构成齐次坐标系下的变换矩阵T。

设无人机某一时刻的位姿采用特殊欧氏群SE(3)空间下的pose表示，它由特殊正交群SO(3)空间下的三维旋转矩阵SO(3)R与平移向量v组成，对R，v构成的矩阵进行齐次化处理得到SE(3)[pose][11]：

SE(3)[pose]=SE(3)[[SO(3)R],v]∈R4×4

(1)

自定义位姿估计周期下四旋翼无人机位姿变换如图2所示。设t0时刻无人机处于初始位置，其位姿为p0，则

(2)

式中E为三维单位矩阵。

t1时刻的位姿为

p1=T10p0

(3)

式中T10为变换矩阵，

依此类推可得t2时刻的位姿p2为

p2=T21T10p0=T21p1

(4)

式中T21为变换矩阵。

通过求解自定义位姿估计周期下2个关键帧的位姿变换矩阵T，可得煤矿井下四旋翼无人机的位姿估计。

图2 四旋翼无人机位姿变换
Fig.2 Pose change of quadrotor UAV

1.3 煤矿井下三维激光雷达观测模型

激光雷达每次观测得到一组当前位姿下的观测点云数据，2帧点云数据之间的匹配关系是未知的。用三维激光雷达(Velodyne-16)观测到的一帧点云数据如图3所示，其包含约30 000个数据点。

图3 三维点云图
Fig.3 3D point cloud

记t0时刻和t1时刻观测到的2个相关帧点云数据分别为其中Lt0,Lt1表示当前帧点云集合；表示三维激光雷达观测到的每一个点在激光雷达坐标系下的三维坐标；m,n为Lt0,Lt1中的点数。则位姿估计问题可转换为求解2帧数据之间的欧氏变换矩阵T(R,v)，使得式(5)成立。

Lt0=TLt1=RLt1+v

(5)

2 基于ICP的变换矩阵求解方法

由激光雷达传感器特性可知，激光雷达的扫描匹配属于未知对应点的求解问题，需要首先确定2对点云数据之间的对应关系，即寻找一致点对[12]。使用最近邻原则作为点对一致性的评价标准。假设Lt0与Lt1中一个点的数据分别为li与lj，则li与lj之间的欧氏距离Dij为

(6)

通过计算点对的欧氏距离找到每组中最小的Dij，得到2组点云之间的对应点对，将其定义为与

根据式(5)的点云变换关系，定义点对匹配误差ei为

(7)

构建最小二乘问题，计算使误差平方和e达到极小值的R，v：

(8)

该问题可通过线性SVD(Singular Value Decomposition，奇异值分解)的方法一次求解[13]，也可通过非线性优化的方式迭代求解，后者更加耗时。出于实时性考虑，选用线性SVD求解方式。

定义t0与t1时刻2组点云的质心分别为

(9)

(10)

将式(9)、式(10)代入式(8)中进行去质心化操作,得

(11)

由于所以目标函数等价于

(12)

式(12)中旋转矩阵R的最优解R*可表示为

(13)

根据SVD方法，定义矩阵W为

(14)

对式(14)进行SVD分解可得

W=UΣVT

(15)

R*=UVT

(16)

式中：Σ为SVD分解得到的奇异值所构成的对角阵；U，VT为奇异值对角阵Σ对应的正交矩阵。

最后由R*结果计算出v的最优解v*：

(17)

至此完成了2组点云之间的变换矩阵计算，整个过程可总结为以下4步：① 2组点云分别去质心化。② 构造去质心化对应点对匹配误差模型。③ 用SVD分解法优先估计旋转矩阵R。④ 根据质心变换方程计算平移向量v。

3 实验分析

3.1 位姿估计结果对比分析

实验环境选择与煤矿井下环境结构相似的走廊与房间，分别对应煤矿井下的巷道与工控硐室。四旋翼无人机携带三维激光雷达进行数据采集。采用ICP方法得到的走廊与房间匹配结果如图4、图5所示。每幅图中左右两边白色点云为上一帧点云数据，左边绿色点云为当前帧点云数据，右边红色点云为匹配变换后的当前帧点云。由图4中走廊的A，B处与图5中房间的C，D处可看出，2帧数据获得了良好的匹配，说明了四旋翼无人机位姿估计的正确性。

图4 采用ICP方法得到的走廊匹配结果
Fig.4 Corridor matching result obtained by ICP method

室外复杂非结构化环境下的点云扫描匹配通常采用NDT(Normal Distribution Transform，正态分布变换)方法[14]，但煤矿井下多为结构化环境，用NDT方法求解容易陷入局部极小，导致匹配失败。用NDT方法对同样条件下的走廊与房间进行点云扫描匹配，结果如图6、图7所示，从图中可明显看出匹配失败。

图5 采用ICP方法得到的房间匹配结果
Fig.5 Room matching result obtained by ICP method

图6 采用NDT方法得到的走廊匹配结果
Fig.6 Corridor matching result obtained by NDT method

图7 采用NDT方法得到的房间匹配结果
Fig.7 Room matching result obtained by NDT method

连续进行多帧数据的ICP迭代扫描匹配，得到四旋翼无人机连续关键帧位姿估计，走廊中的一组关键帧点云数据如图8所示。图8中每个坐标轴代表关键帧所显示的四旋翼无人机位姿。实验时四旋翼无人机定高匀速飞行，位姿估计结果与运动输入基本一致。

3.2 位姿解算耗时分析

为满足四旋翼无人机实时性需求，每次ICP扫描和匹配计算耗时不能大于四旋翼无人机控制周期，常用四旋翼无人机控制周期为100 ms。在处理点云时，进行滤波与下采样，从采集的数据中分别提取10组房间与走廊的点云数据，统计位姿解算平均耗时，结果见表1。可见，经过滤波与下采样处理后位姿解算耗时明显下降，满足无人机实时性需求。

图8 连续位姿估计结果
Fig.8 Continuous pose estimation result

表1 位姿解算平均耗时

Table 1 Average time consuming of pose calculation

地点平均耗时/ms加速前加速后走廊35576房间14743

4 结语

建立了煤矿井下四旋翼无人机与机载激光雷达的坐标系，通过对四旋翼无人机运动模型与激光雷达观测模型的理论分析，将煤矿井下四旋翼无人机位姿估计问题转换为激光点云数据的扫描匹配问题。以第1帧位置为初始位，通过ICP方法连续计算出2帧点云数据的变换关系，迭代求解出每一时刻的位姿。最后采用滤波与下采样方法对点云数据进行优化，提高了变换矩阵的求解速度，满足了四旋翼无人机位姿估计实时性需求。实验结果表明，相比于NDT方法，ICP方法更适用于求解煤矿井下环境中的激光扫描匹配问题。

参考文献：(References)

[1] 秦玉鑫,杜翠洁,王红旗,等.一种新型煤矿灾害信息探测机器人系统设计[J].工矿自动化,2015,41(2):17-20.

QIN Yuxin,DU Cuijie,WANG Hongqi,et al.Design of a novel information exploration robot system for mine disaster[J].Industry and Mine Automation,2015,41(2):17-20.

[2] 王川伟,马宏伟,马琨,等.救援探测机器人越障能力研究[J].煤炭技术,2018,37(1):230-231.

WANG Chuanwei,MA Hongwei,MA Kun,et al.Study on capability of obstacle surmounting for rescue detecting robot[J].Coal Technology,2018,37(1):230-231.

[3] 段洪伟,赵长辉,王琦.新型垂直起降无人机的发展[C]//2014(第五届)中国无人机大会论文集.北京:航空工业出版社,2014.

[4] 李尧.四旋翼飞行器控制系统设计[D].大连:大连理工大学,2013.

[5] 丁团结,方威,王锋.无人机遥控驾驶关键技术研究与飞行品质分析[J].飞行力学,2011,29(2):17-19.

DING Tuanjie,FANG Wei,WANG Feng.Development of UAV remote-piloted key technology and flight qualities[J].Flight Dynamics,2011,29(2):17-19.

[6] 张航,卢小平,郝波,等.无人机在露天矿开采监管中的应用研究[J].采矿技术,2018,18(6):111-114.

[7] 孙硕,陈岩.煤矿井下定位仿真分析研究[J].计算机仿真,2011,28(4):145-148.

SUN Shuo,CHEN Yan.Simulation analysis research of coal mine location[J].Computer Simulation,2011,28(4):145-148.

[8] 沈雅琼,叶伯生,熊烁.基于齐次变换矩阵的机器人轨迹规划方法[J].组合机床与自动化加工技术,2014(1):5-9.

SHEN Yaqiong,YE Bosheng,XIONG Shuo.A method of robot's trajectory planning based on homogeneous transformation matrix[J].Modular Machine Tool &Automatic Manufacturing Technique,2014(1):5-9.

[9] 郭勇,汪大伟,邓宇.四旋翼飞行器建模及其运动控制[J].传感器与微系统,2017,36(11):38-41.

GUO Yong,WANG Dawei,DENG Yu.Modeling and flight control of quadrotor[J].Transducer and Microsystem Technologies,2017,36(11):38-41.

[10] CHOU J.Quaternion kinematic and dynamic differential equations[J].IEEE Transactions on Robotics and Automation,1992,8(1):53-64.

[11] ISERLES A,MUNTHE-KAAS H Z,NØRSETT S P,et al.Lie-group methods[J].Acta Numerica,2000,9:215-365.

[12] BESL P J,MCKAY H D.A method for registration of 3-D shapes[J].IEEE Transactions on Pattern Analysis &Machine Intelligence,1992,14(2):239-256.

[13] GOLUB G H,REINSCH C.Singular value decomposition and least squares solutions[J].Numerische Mathematic,1970,14(5):403-420.

[14] GELMAN A,RUBIN D B.Inference from iterative simulation using multiple sequences[J].Statistical Science,1992,7(4):457-472.