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

工矿自动化 Ind ustr ya nd MineAuto ma tio n Vo l . 45 No . 9 Sep.2019 第45卷第9期 2019年9月 文章编号1671-251X201909-0025-05DOI10. 13272/j. issn. 1671-251x. 17441 基于迭代最近点的井下无人机实时位姿估计 王岩马宏伟王星杨林12 （1.西安科技大学机械工程学院％陕西西安710054； 2.陕西省矿山机电装备智能监测重点实验室％陕西 西安710054； 3.中煤能源研究院有限责任公司陕西西安710054） 扫码移动阅读 摘要针对煤矿井下无GPS信号、低照度、结构化环境等特点，提出了 一种基于迭代最近点ICP的井下 无人机实时位姿估计方法％通过建立四旋翼无人机运动模型与机载激光雷达观测模型，将煤矿井下四旋翼 无人机位姿估计问题转换为机载激光点云数据的扫描匹配问题。用三维激光雷达作为四旋翼无人机机载环 境测量传感器，得到无人机当前位姿下的观测点云数据；以第1帧位置为初始位，通过ICP方法得到连续 2帧点云数据之间的相对变换矩阵，迭代求解连续关键帧点云数据，得到煤矿井下四旋翼无人机实时位姿估 计结果％采用滤波与下采样方法对点云数据进行优化，加速变换矩阵的求解，满足四旋翼无人机位姿估计实 时性需求。实验结果表明，基于ICP的井下无人机实时位姿估计方法能够快速、有效地求解四旋翼无人机 位姿，相比于正态分布变换方法，ICP方法更适用于煤矿井下四旋翼无人机的实时位姿估计％ 关键词井下四旋翼无人机；位姿估计；迭代最近点；三维激光雷达；点云数据；变换矩阵 中图分类号TD67 文献标志码A Rea l -time po se estima tio n o f und er g r o und unma nned a er ia l veh ic l e b a sed o n ICP meth o d WANG Ya n12 - MA Ho ng wei12 - WANG Xing3 - YANG Lin1- 1. Co l l eg e o f Mec h a nic a l Eng ineer ing , X\n Univer sity o f Sc ienc e a nd Tec h no l o g y, X\n 710054, Ch ina; 2. Sh a a nxi Key La b o r a to r y o f Mine El ec tr o mec h a nic a l Equipment Intel l ig ent Mo nito r ing , X\n 710054, Ch ina; 3. Ch ina Co a l Ener g y Resea r c h Institute Co . , Ltd . , X\n 710054, Ch ina AbstractInviewo fc h a r a c ter is tic s o fno GPSs ig na l ,l o wilumina tio na nd str uc tur ed envir o nmentin c o a l mine,a r ea l -timepo seestima tio n meth o d fo r und er g r o und unma nned a er ia l veh ic l eUAVb a sed o n iter a tive c l o sest po int ICP wa s pr o po sed . By esta b l ish ing qua d r o to r UAV mo tio n mo d el a nd a ir b o r ne l a ser r a d a r o b ser va tio n mo d el , th e pr o b l em o f po sitio n estima tio n o f und er g r o und qua d r o to r UAV is c o nver ted 1nto sc a nn1ng ma tc h 1ng pr o b l em o f a 1r b o r ne l a ser po 1nt c l o ud d a ta . 3D l a ser r a d a r 1s used a s a ir b o r ne envir o nment mea sur ement senso r o f qua d r o to r UAV, a nd o b ser va tio n po int c l o ud d a ta in c ur r ent po s1t1o no fth eUAV1so b ta 1ned .Ta k1ng th efr stfr a mepo st1o na s1n1t1a l po s1to n,r el a tvetr a nsfo r ma t1o n ma tr 1xb etweentwo c o nsec utvepo 1ntso fpo 1ntc l o ud d a ta 1so b ta 1ned b yICP meth o d ,a nd th ec o nt1nuo us key fr a me po int c l o ud d a ta is so l ved iter a tivel y to o b ta in th e r ea l -time po se estima tio n r esul t o f qua d r o to r UAVin und e-g -o und c o a l mine. Fil te-ing a nd d o wnsa mpl ing meth o d s a -e used to o ptimize po int c l o ud d a ta , a nd th e so l utio n o f tr a nsfo r ma tio n ma tr ix is a c c el er a ted to meet th e r ea l -time r equir ements o f po sitio n estima tio n o f qua d r o to r UAV. Th e exper imenta l r esul ts sh o w th a t th e ICP-b a sed r ea l -time po se estima tio n meth o d fo r und er g r o und UAV c a n quic kl y a nd effec tivel y so l ve th e po se o f qua d r o to r UAV, a nd c o mpa r ed with no r ma l d istr ib utio ntr a nsfo r m meth o d ,th eICP meth o d ismo r esuita b l efo r r ea l -timepo seestima tio n 收稿日期收稿日期019-05-21；修回日期修回日期019-08-17；责任编辑责任编辑胡娴。 基金项目基金项目国家自然科学基金项目50674075；陕西省科技统筹创新工程计划项目2013KJTCL01-02。 作者简介作者简介王岩1991 ,男，陕西西安人，博士研究生，主要研究方向为机器人导航技术,E-ma il1245994265qq. c o m。 引用格式引用格式王岩，马宏伟，王星，等.基于迭代最近点的井下无人机实时位姿估计工矿自动化，2019,4592529. WANG Ya nMA Ho ng weiWANG Xingeta l 8Rea l -timepo s ees tima tio no fund er g r o und unma nned a er ia l veh ic l eb a s ed o nICP meth o d*. Ind us tr y a nd Mine Auto ma tio n,2019,459 25-29. ・26・ 工矿自动化2019年第45卷 o f qua d r o to r UAV in und er g r o und c o a l mine. Key words und er g r o und qua d r o to r UAV; po se estima tio n; iter a tive nea r est po int; 3D l a ser r a d a r; po int c l o ud d a ta; tr a nsfo r ma tio n ma tr ix 0引言 传统的煤矿井下地面移动式巡检设备存在以下 问题一是 道，二是面对 、地面 或障 碍 等情况时空间运动能力 。 人 机技术 ，利 进 人机技术协助煤 矿 相关工作成为可能「T。四 人机体积 、质量轻，相 固 式无人机，具有优 垂 与定点悬停能力，同 他类型 人机 涵道无人机、直升机、超多 人机等更易控 制3*。因此，本文 四 人机作为研究 对象。 实时自主导航功能是衡量煤矿井下无人机自动 化、智能 业能力的重要指标。无人机实 行 程中6自由 与姿态解算是解决无人机自主 导航 题 键。 人机需要在GPS Gl o b a l Po sitio ning System，全球定位系统信号良 好的开阔室外 ，由 控飞行「5*。GPS提 供经 ，气压计提供高 ， 为无人机 入， 与 计 积分 计算出欧拉 为无人机 入「6*。但煤矿 没有GPS信号，因此，需要增加相 器来实现 无人机实 估计。 本文针对煤矿 GPS信号、低照度、结构 化环境等特点7*，提出了基于ICPIter a tive Cl o sest Po int，迭代最近点 人机实时位姿估计方 法。 激光雷 为四 人机机载环境 测量 器,通过对2个 点云数据进行扫描 解算，得到2帧点 间的相对变换矩 阵⑻，经 代求解 键帧点云数据, 矿 四 人机实 估计结果。 1四旋翼无人机运动模型与激光雷达观测模型 1. 1 四旋翼无人机坐标系与三维激光雷达坐标系 选用16线激光雷达，固 四旋翼无人机顶部 中 ，单次扫 360m 。选取四 人机机 与激光雷达扫 中心，分别 为 人机 标 点 与 激光雷 标 点， 图1所示，其中红、绿、蓝分别对应轴、夕轴、轴o 由于激光雷达固 人机机架，属于刚 ，所 以，两坐标 间无相对旋转与位移。激光雷 i 扫 算 法 解 算 键 点 间 换 矩 ， 换矩 进行四 人机 标 人机运动 估计。 图1四旋翼无人机与三维激光雷达坐标系 Fig .1 Qua d c o pter UAVc o o r d ina tea nd l id a r c o o r d ina te 1. 2 煤矿井下四旋翼无人机运动模型 矿 四 人机 动 间 的刚体运动，由 与平移组成，不同 刚体坐标 间 动由 换描述9*。 拉角、 向量、旋转矩阵、四元数等描述方式，本文 J 学 更 矩 为 述符。平移由 向量描述10*。 矩阵与平移 向量联立 构成齐 标 换矩阵To 人机某 特殊欧氏群 SE3空间下的po se表示，它由特殊正交群SO3 空间 矩阵SO3“与平移向量v组 成，对R，v构成的矩阵进行齐 处 寻到 SEOpo s e*11* SE3[po se* SE3[[SO3R*,v* E R4X4 1 自定义位姿估计周期下四旋翼无人机 换 如图2所示。设时刻无人机处于初始位置，其位 姿为0，则 01 0 2 01 式中E为三维单位矩阵。 1 为 1 T10 0 3 Rv 式中T10为变换矩阵，T10 。 01 依此类推可得2时刻的位姿2为 2 T21 T10 0 T21 1 4 式中T21为变换矩阵。 求解自 估计 期 2 个 键 换矩 T， 矿 四 人机 估计。 2019年第9期王岩等基于迭代最近点的井下无人机实时位姿估计・27・ SE39J ［叽 SE3p0 _1 0 0 0_ 0 10 0 0 0 10 0 0 0 1」4x4 图2四旋翼无人机位姿变换 Fig . 2 Po se c h a ng e o f qua d r o to r UAV 1. 3 煤矿井下三维激光雷达观测模型 激光雷达每 测 当前位姿下的观测 点云数据,2帧点云数据之间的匹配关系是未知的。 用三维激光雷达Vel o d yne-16观测到的一帧点云 图3 ，其包含约30 000个 点。 图3 点 图 Fig 3 3D po int c l o ud 记t时刻和t时刻观测到的2个相关帧点云 数据分另I］为 m , n为L 0, U,VT为奇异值对角阵对 正交矩阵。 由R结果计算出v的最优解v v 00 Ro1 1 * I0 R 1 * 17 至此完成了 2组点云之间的变换矩阵计算，整 个过程 为以下4步①2组点云分别去质心 -② 对应点对 误 型- ③用SVD分解法优先估计 矩阵R-④ 吾质 换方程计算平移向量v。 3实验分析实验分析 3. 1 位姿估计结果对比分析 实验 择与煤矿 相似的走廊 与房房间，分别对 矿 巷道与工控 室。四 人机携带 激光雷达进行 。采用 ICP方法 与房房间 果如果如图4、图5 。每幅图中 两边 点云为上 点 据，左边 点云为当前帧点 ，右边红色点云 为匹配变换后的当前帧点云。由图4中走廊的A， B处与图5中房间的C,D处可看出，2 良 ，说 四 人机 估估计的正 确性。 图4采用ICP方法得到的走廊匹配结果 Fig . 4 Co r r id o r ma tc h ing r esul t o b ta ined b y ICP meth o d 室外复杂非 点云扫描匹配通常 采用 NDTNo r ma l Distr ib utio n Tr a nsfo r m，正态分 换方法「14*,但煤矿井下多为 ，用 NDT方法求解 入局局部 ，导 失败败。 用NDT方法对同样条件 与房房间进行点云 图5采用ICP方法得到的房间匹配结果 Fig .5 Ro o m ma tc h ing r esul to b ta ined b yICP meth o d 扫描匹配，结果如图6、图7所示，从图中可明显看 出匹配失败。 图6采用NDT方法得到的走廊匹配结果 Fig .6 Co r r id o r ma tc h ing r esul to b ta ined b y NDT meth o d 图 7 NDT 方法 房间 果 Fig .7 Ro o m ma tc h ing r esul t o b ta ined b y NDT meth o d 进行多 ICP迭代扫描匹配，得到 四 人机 键帧位姿估计，走廊中的一组 关键帧点 图8 。图8中每个坐标轴代 表关键帧所显示的四旋翼无人机位姿。实验时四旋 人机定高匀 行，位姿估计结果与运动输入 基本一致。 3.2位姿解算耗时分析 为 四 人机实时性需求，每次ICP扫 计算 能大于四 人机控制周 期，常用四旋翼无人机控制周期为100 ms。在处理 点 ，进行滤波与 ， 中分别提 取10组房间与 点 ，统计 解算平均 耗时，结果 1- ，经过滤波与 处 i 解算 ， 人机实 需求。 2 019年第9期王岩等基于迭代最近点的井下无人机实时位姿估计・2 9・ 图8连续位姿估计结果 Fig .8 Co ntinuo uspo seestima tio nr esul t 表1位姿解算平均耗时 Ta b l e1 Aver a g e time c o nsuming o f po se c a l c ul a tio n 地点 平均耗时/ms 加速前加速后 走廊 35576 房间 14743 4结语 建立了煤矿井下四旋翼无人机与机载激光雷达 的坐标系，通过对四 人机运动模型与激光雷 测模型 论分析，将煤矿 四 人机 位姿估计问题转换为激光点 扫〔配问 题。以第1 为初 ，通过ICP方法连续计 算出2帧点 换 ，迭代求解出每 匸 - 滤波与 方法对点据 进行优化，提高 换矩 求解 , 四旋 人机 估计实 需求。实验结果表明，相 比于NDT方法，ICP方法更适用于求解煤矿井下环 境中的激光扫 题。 参考文献References 1 *秦玉鑫，杜翠洁，王红旗，等.一种新型煤矿灾害信息 探测机器人系统设计［J*.工矿自动化，2 015,41 2 17- 0. QIN Yuxin,DU Cuijie,WANG Ho ng qi,et a l . Desig n o f a no vel info r ma tio n expl o r a tio n r o b o t system fo r mine d isa ster J*. Ind ustr y a nd Mine Auto ma tio n， 2015,412 1720. 2 *王川伟，马宏伟，马琨，等.救援探测机器人越障能力 研究［J*.煤炭技术,2018,371 230-231. WANG Ch ua nweiMA Ho ng weiMA Kunet a l . Stud y o n c a pa b il ity o f o b sta c l e sur mo unting fo r r esc ue d etec ting r o b o t J*. Co a l Tec h no l o g y, 2018 , 37 1 230-231. 3 *段洪伟，赵长辉，王琦.新型垂直起降无人机的发展 ［C*//2014第五届中国无人机大会论文集.北京 航空工业出版社2014. 4 *李尧.四旋翼飞行器控制系统设计［D*.大连大连理 工大学，2013. 5 * 丁团结，方威，王锋.无人机遥控驾驶关键技术研究与 飞行品质分析［J*.飞行力学,2011,2921719. DING Tua njie FANG Wei WANG Feng Devel o pment o f UAV r emo te-pil o ted key tec h no l o g y a nd fl ig h t qua l ities〕*. Fl ig h t Dyna mic s,2011,292 17-198 6 *张航，卢小平，郝波，等.无人机在露天矿开采监管中 的应用研究J*.采矿技术2018186111114. 7 *孙硕，陈岩.煤矿 仿真分析研究J*.计算机 仿真,2011,284145148. SUN Sh uoCHEN Ya n8Simul a tio na na l ysisr esea r c h o f c o a l mine l o c a tio nJ*. Co mputer Simul a tio n, 2011， 284145-148. 8 *沈雅琼，叶伯生，熊烁.基于齐次变换矩阵的机器人轨 迹规划方法J*.组合机床与自动化加工技术， 201415-9. SHEN Ya qio ng，YE Bo sh eng，XIONG Sh uo . A meth o d o f r o b o t\s tr a jec to r y pl a nning b a sed o n h o mo g eneo us tr a nsfo r ma tio n ma tr ix J *8 Mo d ul a r Ma c h ine TdoI Auto ma tic Ma nufa c tur ing Tec h nique， 2014159. 9 *郭勇，汪大伟，邓宇.四旋翼飞行器建模及其运动控制 J*.传感器与微系统2017361138-41. GUO Yo ngWANG Da weiDENG Yu Mo d el ing a nd fl ig h t c o ntr o l o f qua d r o to r J *. Tr a nsd uc er a nd Mic r o system Tec h no l o g ies2017 361138-418 10* CHOU J8 Qua ter nio n kinema tic a nd d yna mic d iffer entia l equa tio ns J *. IEEE Tr a nsa c tio ns o n Ro b o tic sa nd Auto ma tio n 1992 8153-648 11* ISERLES A，MUNTHE-KAAS H 乙 N0RSETT S P， eta l 8Lie-g r o up meth o d sJ*8Ac ta Numer ic a 2000 9 215-3658 12* BESLPJMCKAY H D8A meth o d fo r r eg istr a tio no f 3-D sh a pes J *8IEEE Tr a nsa c tio ns o n Pater n Ana l ysis Ma c h ine Intel l ig enc e， 1992， 14 2 239-256. 13* GOLUB G H RENSCH C. Sing ul a r va l ue d ec o mpo sitio n a nd l ea st squa r es so l utio ns J *. Numer isc h e Ma th ema tic 1970 14 5 403-420. 14* GELMAN ARUBIN D B.Infer enc efr o m iter a tive simul a tio n using mul tipl e sequenc es J *. Sta tistic a l Sc ienc e,1992,74457472.