基于迭代最近点的井下无人机实时位姿估计

Real-time pose estimation of underground unmanned aerial vehicle based on ICP method

:针对煤矿井下无GPS信号、低照度、结构化环境等特点,提出了一种基于迭代最近点(ICP)的井下无人机实时位姿估计方法。通过建立四旋翼无人机运动模型与机载激光雷达观测模型,将煤矿井下四旋翼无人机位姿估计问题转换为机载激光点云数据的扫描匹配问题。用三维激光雷达作为四旋翼无人机机载环境测量传感器,得到无人机当前位姿下的观测点云数据;以第1帧位置为初始位,通过ICP方法得到连续2帧点云数据之间的相对变换矩阵,迭代求解连续关键帧点云数据,得到煤矿井下四旋翼无人机实时位姿估计结果。采用滤波与下采样方法对点云数据进行优化,加速变换矩阵的求解,满足四旋翼无人机位姿估计实时性需求。实验结果表明,基于ICP的井下无人机实时位姿估计方法能够快速、有效地求解四旋翼无人机位姿,相比于正态分布变换方法,ICP方法更适用于煤矿井下四旋翼无人机的实时位姿估计。

In view of characteristics of no GPS signal, low illumination and structured environment in coal mine, a real-time pose estimation method for underground unmanned aerial vehicle (UAV) based on iterative closest point (ICP) was proposed. By establishing quadrotor UAV motion model and airborne laser radar observation model, the problem of position estimation of underground quadrotor UAV is converted into scanning matching problem of airborne laser point cloud data. 3D laser radar is used as airborne environment measurement sensor of quadrotor UAV, and observation point cloud data in current position of the UAV is obtained. Taking the first frame position as initial position,relative transformation matrix between two consecutive points of point cloud data is obtained by ICP method, and the continuous key frame point cloud data is solved iteratively to obtain the real-time pose estimation result of quadrotor UAV in underground coal mine.Filtering and downsampling methods are used to optimize point cloud data, and the solution of transformation matrix is accelerated to meet the real-time requirements of position estimation of quadrotor UAV. The experimental results show that the ICP-based real-time pose estimation method for underground UAV can quickly and effectively solve the pose of quadrotor UAV, and compared with normal distribution transform method, the ICP method is more suitable for real-time pose estimation of quadrotor UAV in underground coal mine.