居鹤华, 冷舒
(北京工业大学 深空机器人研究中心,100022 北京)
摘要:
为提高传统AABB树碰撞检测的精度和效率，提出一种基于虚拟传感器的月面巡视器机械臂碰撞检测算法.建立月面巡视器机械臂的逆运动学解算模型；对地面环境点云数据进行Delaunay三角化，采用多叉树代替二叉树作为AABB树储存环境点云三角面集；利用虚拟传感器简化巡视器机械臂结构模型，通过虚拟传感器遍历AABB树中的环境点云三角面集进行碰撞检测，避免机械臂与环境发生干涉.月面巡视器就位探测任务内场实验表明：基于虚拟传感器的月面巡视器碰撞检测算法使碰撞检测精度在1 mm内，碰撞检测时间降低至10 s内.基于虚拟传感器的碰撞检测算法具有高效性和可行性.
关键词: 月面巡视器 机械臂 Delaunay三角化 虚拟传感器 AABB树 碰撞检测
DOI：10.11918/j.issn.0367-6234.2016.01.009
分类号:TP241
文献标识码:A
基金项目:国家自然科学基金(10002011200902).
A collide detection algorithm based on virtual sensors of lunar rover manipulator
Jü Hehua, LENG Shu
(Research Center of Deepspace Robot, Beijing University of Technology, 100022 Beijing,China)
Abstract:
To improve the accuracy and efficiency of classic AABB collide detection, An algorithm of the virtual sensors which is applied to the domain of Rover manipulator collide detection is proposes. At first, the inverse kinematics model of rover manipulator is established. Secondly, lunar terrain point cloud data is triangulated by Delaunay triangulation method, the triangulation face set is then stored in the leaf nodes of a multiple tree (AABB tree). In the end, the rover manipulator model is simplified by virtual sensors. By utilizing the virtual sensors to traverse the face set stored in the multiple tree, manipulator/lunar terrain collision are detected and avoided. The inner-yard experiment of lunar rover in-situ exploration task show that the accuracy of the collide detection increased to within 1mm and the time of collide detection decreased to 10 s by the algorithm proposed. The feasibility and efficiency of the collide detection algorithm is justified.
Key words: lunar rover manipulator delaunay triangulation virtual sensors AABB tree collide detection
居鹤华, 冷舒. 利用虚拟传感器的巡视器机械臂碰撞检测算法[J]. 哈尔滨工业大学学报, 2016, 48(1): 58-65. DOI: 10.11918/j.issn.0367-6234.2016.01.009.
Jü Hehua, LENG Shu. A collide detection algorithm based on virtual sensors of lunar rover manipulator[J]. Journal of Harbin Institute of Technology, 2016, 48(1): 58-65. DOI: 10.11918/j.issn.0367-6234.2016.01.009.
基金项目 国家自然科学基金（10002011200902） 作者简介 居鹤华(1969-), 男, 教授, 硕士生导师 通信作者 冷舒, ptwaixingren@126.com 文章历史 收稿日期: 2014-11-03
Contents -->Abstract Full text Figures/Tables PDF
利用虚拟传感器的巡视器机械臂碰撞检测算法
居鹤华, 冷舒
北京工业大学 深空机器人研究中心, 100022 北京
收稿日期: 2014-11-03
基金项目: 国家自然科学基金（10002011200902）
作者简介: 居鹤华(1969-), 男, 教授, 硕士生导师
通信作者: 冷舒, ptwaixingren@126.com
摘要: 为提高传统AABB树碰撞检测的精度和效率，提出一种基于虚拟传感器的月面巡视器机械臂碰撞检测算法.建立月面巡视器机械臂的逆运动学解算模型；对地面环境点云数据进行Delaunay三角化，采用多叉树代替二叉树作为AABB树储存环境点云三角面集；利用虚拟传感器简化巡视器机械臂结构模型，通过虚拟传感器遍历AABB树中的环境点云三角面集进行碰撞检测，避免机械臂与环境发生干涉.月面巡视器就位探测任务内场实验表明：基于虚拟传感器的月面巡视器碰撞检测算法使碰撞检测精度在1 mm内，碰撞检测时间降低至10 s内.基于虚拟传感器的碰撞检测算法具有高效性和可行性.
关键词: 月面巡视器 机械臂 Delaunay三角化 虚拟传感器 AABB树 碰撞检测
A collide detection algorithm based on virtual sensors of lunar rover manipulator
Jü Hehua, LENG Shu
Research Center of Deepspace Robot, Beijing University of Technology, 100022 Beijing, China
Abstract: To improve the accuracy and efficiency of classic AABB collide detection, An algorithm of the virtual sensors which is applied to the domain of Rover manipulator collide detection is proposes. At first, the inverse kinematics model of rover manipulator is established. Secondly, lunar terrain point cloud data is triangulated by Delaunay triangulation method, the triangulation face set is then stored in the leaf nodes of a multiple tree (AABB tree). In the end, the rover manipulator model is simplified by virtual sensors. By utilizing the virtual sensors to traverse the face set stored in the multiple tree, manipulator/lunar terrain collision are detected and avoided. The inner-yard experiment of lunar rover in-situ exploration task show that the accuracy of the collide detection increased to within 1mm and the time of collide detection decreased to 10 s by the algorithm proposed. The feasibility and efficiency of the collide detection algorithm is justified.
Keywords: lunar rover manipulator delaunay triangulation virtual sensors AABB tree collide detection
嫦娥三号月面巡视器是一个六轮摇臂式月面巡视器[1]，其前端携带机械臂的末端装载X射线谱仪接近月面探测目标.月面巡视器就位探测任务书中要求执行探测任务时，X射线谱仪与探测面的距离＜30 mm，且就位探测规划任务在1 min之内完成.在该任务中满足巡视器与环境不存在干涉是一个重要的约束条件，准确、高效地进行碰撞检测是其中的难点.
目前, 国内外对碰撞检测都进行了大量研究. Wong等[2]提出通过连续自我碰撞检测的方法进行避障，但这种方法适用于形状易于发生改变的物体与非变形体进行碰撞检测，与月面巡视器机械臂碰撞检测情况不同.贾庆轩等[3]提出使用几何方法进行机械臂与环境的碰撞检测，PAUL[4]等提出通过几何方法解决火星车机械臂碰撞检测问题.但上述方法都并非以月面巡视器机械臂作为碰撞检测的研究对象.王伟等[5]提出基于OBB包围盒[6]的碰撞检测算法. OBB包围盒算法可以精确进行碰撞检测计算，但在物体姿态变换时，需重新计算包围盒，导致效率不高，难以满足高效性需要. Gino[7]提出基于GJK算法[8]的碰撞检测，Cameron[9]提出增强GJK的碰撞检测算法，Mirtich[10]提出基于V-Clip的碰撞检测算法.上述3种算法对于外形复杂机构的碰撞检测十分有效，但月面巡视器机械臂外形简单，用上述方法会降低碰撞检测效率. Gino[11]还提出一种基于AABB包围盒的碰撞检测算法，该算法的效率优于OBB包围盒算法，但仍需要进行大量的包围盒计算，难以满足任务需求.
综上所述，本文提出一种基于虚拟传感器的月面巡视器机械臂碰撞检测算法:
1) 通过建立机械臂运动学模型，精确计算机械臂上的任意一点的位置，保证机械臂碰撞检测顺利进行; 2)简化机械臂模型，将机械臂杆用有限根虚拟传感器包围; 3)将月面环境点云图进行Delaunay三角化，创建点云三角面集，并通过多叉树构造的AABB树储存该面集; 4)通过虚拟传感器与AABB树内的面集进行代数分析计算.确定机械臂与环境是否发生碰撞; 5)以嫦娥三号月面巡视器进行机械臂内场探测任务为例，验证算法的正确性.
1 月面巡视器机械臂逆运动学 1.1 机械臂建模巡视器机械臂采用三自由度关节型串联构型，安装在巡视器前端，末端安装有效载荷——粒子激发X射线谱仪探头.机械臂结构如图 1所示.
Figure 1
图 1 机械臂结构图 机械臂对应的D-H坐标系如图 2所示.
Figure 2
图 2 机械臂D-H坐标系 为确定X射线谱仪中心C，建立虚运动副R3/E，其对应原点是o4，并建立相应坐标系4.
D-H参数表如表 1所示.
表 1
表 1 月面巡视器机械臂D-H参数 
表 1 月面巡视器机械臂D-H参数
符号物理意义如下：jri为体系j原点至体系i原点的位置矢量在体系j下的投影，jrS为体系j原点至空间内一点S的位置矢量在体系j下的投影，jrS[i])为体系j原点至空间内一点S的位置矢量在体系j下的投影的第i个分量，jpS为体系j原点至空间内一点S的齐次位置矢量在体系j下的投影, jQi为体系j至体系i的旋转变换阵在体系j下的表示, jQix为体系j至体系i的绕体系j的x轴进行定轴转动的旋转变换阵在体系j下的表示, jTi为体系j至体系i的齐次变换阵在体系j下的表示.
记λi=cos(αi)，μi=sin(αi)，cos(θi)=C(θi)，sin(θi)=S(θi)，其中θi为轴转角，则相邻两个DH系的旋转变换阵iQi+1为
(1) 链节位矢在根向体系表示为
(2) 1.2 机械臂逆运动学 1.2.1 机械臂逆运动学本文采用解析方法对机械臂逆运动学进行求解[12~13].
三自由度串联机械臂系统定位方程：
(3) 式中1rC= [xC, yC, zC]T为光谱仪中心C相对基座的期望位置.
将式(1)、式(2)带入式(3)中，展开得
(4) 由式(4)得三轴角度
式中：
1.2.2 机械臂正逆运动学计算结果校验机械臂正运动学与逆运动学是互逆的行为.可用表 2所示实验数据验证其计算的正确性.由表 1可知，轴转角有角度范围约束，故逆运动学解数目不固定.
表 2
表 2 巡视器机械臂正逆运动计算验证 
表 2 巡视器机械臂正逆运动计算验证
机械臂正逆运动学相互校验仿真结果如组图 3所示.
Figure 3
图 3 机械臂正逆运动学相互校验仿真 2 Delaunay三角化地面点云数据Delaunay三角化是构造三角网格的经典方法.通过该方法得到的三角网格不存在奇异性，并具有最优性和唯一性[14]. Delaunay三角化通过一个平衡二叉树结构实现.其三角面集需满足条件：在平面内，通过三角型的3个顶点的圆中不会包含平面内第4个顶点.
本文使用节点增量法构造Delaunay三角网格[15].在增加节点前，首先通过节点数量确定网格密度，若增加的节点与网格中已有的节点在XY剖分面内投影的距离小于网格密度，则过滤掉该点.否则，增加该点. Delaunay三角化流程如图 4所示.
Figure 4
图 4 Delaunay三角化流程 3 月面巡视器机械臂碰撞检测 3.1 碰撞检测步骤月面巡视器机械臂就位探测任务提出：要准确检测机械臂与地面环境是否发生碰撞.若不碰撞，需求得机械臂与地面间的最小距离，否则，在任务中提示碰撞.根据任务要求，碰撞检测执行步骤如下：
1) 简化机械臂模型：将机械臂划分为5部分，分别记为A\1, B\2, C\3, D\4, E\5.对每部分，环绕该部分杆件最大外径并平行于杆件轴线分别固接n根如同激光测距仪的虚拟传感器线段将机械臂包起来，记为dSi, i∈1, 2, …, n，如图 5所示.
Figure 5
图 5 虚拟传感器建模 2) 将地面点云数据Delaunay三角化后形成的三角网格面集以AABB包围盒的形式存储于AABB树中.
3) 使用虚拟传感器射线与地形三角面集的包围盒进行粗碰撞检测.若非碰撞，则该虚拟传感器与地面不碰撞.反之，进行射线与相应三角面的精碰撞检测.若仍碰撞，可得到一条碰撞线段记为dRi, i∈1, 2, …, 36.再判断该虚拟传感器线段是否与三角面集碰撞.若碰撞，则机械臂与地面碰撞.反之，机械臂与地面距离为
选取dRSi中最短的线段为最短距离.
碰撞检测流程图如图 6所示.
Figure 6
图 6 碰撞检测流程图 3.2 碰撞检测实现 3.2.1 碰撞检测数学模型机械臂第i部分上虚拟传感器检测线段起点A及终点B位置分别为irA、irB, 由式
得1rA、1rB间的线段方程为
(5) 故虚拟传感器检测点在机械臂底座系下的位置已完全可知.通过虚拟传感器线段方程与障碍三角面Δ[s1, s2, s3]方程联列求解，可判断虚拟传感器与三角面是否碰撞.
若障碍三角面Δ[s1, s2, s3]在机械臂底座坐标系下的坐标分别为1rs1, 1rs2, 1rs3, 则三角面平面方程为
(6) 将式(5)带入式(6)求得参数t：
(7) 若t?[0, 1]，线段与三角面不相交，否则将式(7)带入式(5)中，求得交点1rt：
通过交点是否在三角面内，判定线段与三角面是否相交.
3.2.2 虚拟传感器的碰撞检测传统基于AABB树的碰撞检测主要为通过构造二叉树存储机构的AABB包围盒，再查找AABB树结构中节点间的包围盒交叠情况.若查找到一对节点出现交叠，则根据节点所处位置，继续进行如下判断，得到最终结果，原理如图 7所示.
Figure 7
图 7 传统AABB树的碰撞检测原理 1) 一对节点都是叶节点：判定碰撞.
2) 一对节点中，一个节点为叶节点，另一个节点是内部节点：叶节点对内部节点的所有子节点进行检测，判断是否存在碰撞，直到该内部节点对应的所有叶节点检测完毕.
3) 一对节点都是内部节点：选择子节点少的节点记为A，另一节点记为B. A与B的每一个子节点都进行检测，判断是否碰撞.
在嫦娥三号内场任务中，环境点云数据量超过40万个点，Delaunay三角化产生的三角面集数量庞大，巡视器机械臂中也有1 000个以上的三角面.若采用传统AABB树方法检测碰撞，需大量遍历AABB树中的节点，导致效率降低.
本文采用多叉树将所有三角面集分别存储至AABB树叶节点，则不存在传统AABB树中的第2)、3)步，如图 8所示.通过机械臂简化模型后的虚拟传感器遍历AABB树的叶节点进行碰撞检测.只要存在一次碰撞，检测结束.否则，可检测机械臂与地面环境的最小距离，原理见图 9.
Figure 8
图 8 多叉树构成的AABB树碰撞检测原理 Figure 9
图 9 虚拟传感器碰撞检测原理 4 月面巡视器碰撞检测仿真实验月面巡视器碰撞检测通过机械臂就位探测仿真软件辅助任务进行.软件使用VC++搭建了月面巡视器机械臂就位探测仿真环境.环境点云数据由月面巡视器携带的相机获得.软件导入该数据，并通过3D仿真环境点云数据(见图 10).
Figure 10
图 10 地面环境点云图 由点云数据生成的Delaunay三角网格面和仿真图见图 11.通过月面巡视器内场实验——机械臂探测平地任务和探测石块任务的碰撞检测验证碰撞检测算法的可靠性.
Figure 11
图 11 地面环境Delaunay三角面和仿真 4.1 巡视器机械臂探测平地的碰撞检测平地探测是机械臂就位探测任务的基础.通过机械臂结构数据可知，机械臂只有E\5部分可能与地面环境发生碰撞.故主要研究E\5部分的虚拟传感器与地面环境三角面的碰撞检测问题.
机械臂探测平地时，初始状态的3个关节角为0°, 0°, 0°，如图 12所示.
Figure 12
图 12 探测平地初始状态 机械臂3个关节角为-27°, -30°, -210°时，机械臂状态如图 13所示.由图 13可知，机械臂未与地面碰撞.碰撞检测结果如表 3所示.机械臂在该位形状态时，其E\5部分的n条虚拟传感器与地面环境三角面均未发生碰撞.机械臂距地面最小距离约为0.045 m.
Figure 13
图 13 关节角为(-27°, -30°, -210°)时机械臂状态 表 3
表 3 机械臂碰撞检测结果 
表 3 机械臂碰撞检测结果
机械臂3个关节角为(-27°, -40°, -210°)时，机械臂状态如图 14所示.
Figure 14
图 14 关节角为(-27°, -40°, -210°)时机械臂状态 由图 14可知，机械臂E\5部分与地面进行碰撞.碰撞检测结果如表 4所示.机械臂在该位形状态时，其n条虚拟传感器中部分与地面环境三角面发生碰撞.
表 4
表 4 机械臂地面碰撞检测结果 
表 4 机械臂地面碰撞检测结果
4.2 巡视器机械臂探测石块的碰撞检测机械臂探测石块是探测任务中的难点.石块的形状大小不同，导致机械臂任何一杆，在执行任务时会与石块发生碰撞.为简化问题，在进行探测任务时，先计算好石块距车体的距离，保证只有E\5部分可能与石块进行碰撞，将车体移至指定位置后，再进行探测任务.
机械臂探测石块时, 初始状态3个关节角为(0°, 0°, 0°)，如图 15所示.
Figure 15
图 15 探测石块初始状态图 机械臂3个关节角为(-27°, -40°, -210°)时，机械臂状态如图 16所示.
Figure 16
图 16 关节角为(-27°, -40°, -210°)，机械臂状态图 由图 16可知，E\5的36条线段中，部分线段与石块三角面发生碰撞，碰撞检测结果如表 5所示.
表 5
表 5 机械臂与石块三角面碰撞检测结果 
表 5 机械臂与石块三角面碰撞检测结果
4.3 两种方法碰撞检测对比基于虚拟传感器算法的碰撞检测速度和基于传统AABB树的碰撞检测速度结果见图 17.碰撞点数相同时，基于虚拟传感器法进行碰撞检测的时间优于传统AABB树碰撞检测的时间.
Figure 17
图 17 两种方法碰撞检测时间对比 5 结论1) 建立了机械臂逆运动学模型，通过解析方法精确计算机械臂上各点在任意姿态下的位置.该方法利用机械臂串联构型的特点，只用小规模的计算就可完成机械臂位置姿态的确定.
2) 通过Delaunay三角化地面环境点云文件，保证点云数据的鲁棒性.
3) 用多叉树代替二叉树作为AABB树存储环境点云三角面，减少AABB树节点的存储量.
4) 简化机械臂模型，利用虚拟传感器与AABB树存储的三角面进行碰撞检测，减少了AABB树对节点的遍历时间，提高了效率，保证了精确性.
5) 内场试验验证了算法的高效性和可行性.
参考文献
[1] 孙泽洲, 张廷新, 张熇, 等. 嫦娥三号探测器的技术设计与成就[J]. 中国科学(技术科学), 2014, 44(4): 331-343.
[2] WONG S K, LIN W C, HUNG C H, et al. Radial view based culling for continuous self-collision detection of skeletal models[J]. ACM Transaction on Graphics, 2013, 32(4): 1-10.
[3] 贾庆轩, 陈钢, 孙汉旭, 等. 基于A*算法的空间机械臂避障路径规划[J]. 机械工程学报, 2010, 46(13): 109-115.
[4] PAUL B, ANTONIO D C, MATTHEW R, et al. Automated rover position and instrument placement[C]//Aerospace Conference. Piscataway: IEEE, 2005: 1-12.
[5] 王伟, 马峻, 刘伟. 基于OBB包围盒的碰撞检测研究与应用[J]. 计算机仿真, 2009, 26(9): 180-183. DOI:10.3969/j.issn.1006-9348.2009.09.049
[6] GOTTSCHALK S, LIN M C, MANOCHA D. OBBTree: a hierarchical structure for rapid Interference detection[C]//23rd annual conference on Computer graphics and interactive techniques. New York: ACM Publications, 1996: 171-180.
[7] GINO V D B. A fast and robust GJK implementation for collision detection of convex objects[J]. Journal of Graphics Tools, 1999, 4(2): 7-25. DOI:10.1080/10867651.1999.10487502
[8] GILBERT E G, JOHNSON D W, KEERTHI S S. A fast procedure for computing the distance between complex objects in three-dimensional space[J]. IEEE Journal of Robotics and Automation, 1988, 4(2): 193-203. DOI:10.1109/56.2083
[9] CAMERON S. Enhancing GJK: Computing minimum and penetration distances between convex polyhedra[C]//International Conference on Robotics and Automation. Albuquerque: IEEE Press, 1997: 3112-3117.
[10] MIRTICH B. V-Clip:fast and robust polyhedral collision detection[J]. ACM Transaction on Graphics, 1998, 17(3): 177-208. DOI:10.1145/285857.285860
[11] GINO V D B. Collision detection in interactive 3D environments[M]. Eindhoven: University Press Facilities, 1999: 98-109.
[12] 付荣, 居鹤华. 高精度解耦六自由度机械臂逆运动学解法[J]. 计算机测量与控制, 2010, 18(7): 1637-1640.
[13] JORGE Angeles. 机器人机械系统原理-理论、方法和算法[M]. 宋伟刚, 译. 北京: 机械工业出版社, 2004: 83-90.
[14] RAJAN V T. Optimality of the delaunay triangulation in Rd[J]. Discrete & Computational Geometry, 1994, 12(1): 189-202.
[15] LEE D T, SCHACHTER B J. Two algorithms for constructing a Delaunay triangulation[J]. International Journal of Computer & Information Sciences, 1980, 9(3): 219-242.
