基于混合层次包围盒的水下训练机械臂碰撞检测方法研究

2022-11-09张得礼张文奇王志国杨永常韩亮亮

载人航天 2022年5期

张宇，张得礼*，张文奇，王苑，王志国，杨永常，韩亮亮

(1.南京航空航天大学机电学院，南京 210016； 2.上海宇航系统工程研究所，上海 201109；3.中国航天科技集团有限公司空间结构与机构技术实验室，上海 201109)

1 引言

空间机械臂在空间站的建设及维护中能够协助航天员完成大量复杂的舱外活动，或替代航天员执行一些高危任务，提高航天员工作效率和任务安全性[1-2]。为保证空间站任务安全、顺利完成，航天员需要提前在水下模拟失重环境中进行充分的模拟训练[3-4]。水下训练机械臂是航天员水下出舱模拟训练中必不可少的辅助设备，可以配合航天员进行出舱移动及验证在空间站的相关任务[5]。为了保证航天员及水下训练设备的安全性，水下训练机械臂的碰撞检测十分重要。

目前，机械臂碰撞检测方法主要有基于外部设备和基于3D 模型的2 种检测方法。基于外部设备的碰撞检测方法需要添加额外的传感器或相机等进行信息采集，增加了设备成本及系统复杂性；基于3D 模型的碰撞检测方法则是根据模型之间的相交关系判断是否产生碰撞，不需要额外设备且易于实现及维护，故在实际中得到了更广泛的运用[6-7]。目前，国内外已经提出了多种有效的碰撞检测算法库，如交互式精确碰撞检测( Interactive and Exact Collision Detection，I-COLLIDE)、快速搜索随机树(Rapid-exploration Random Tree， RAPID)、柔性碰撞库(Flexible Collision Library， FCL)等。 I-COLLIDE 基于扫描排序算法，利用模型轴对齐包围盒(Axis-aligned Bounding Box， AABB) 在坐标系的投影判断AABB 是否相交，然后对所有模型按相关性进行排序，减少不必要的相交测试，适用于检测物体数量巨多的场景[8-9]。 RAPID 基于方向包围盒(Oriented Bounding Box， OBB)，建立分层OBB 树，OBB 之间利用分离轴定理(Separating Axis Theorem， SAT)判断是否相交，能够准确检测多边形模型之间的碰撞，但OBB 树的更新及SAT 算法计算量都比较大[10]。 FCL 提供了一个可供各种应用程序使用的统一接口，集成了基于凸包的碰撞检测，如GJK 算法，基于边界体积层次的碰撞检测，如包围盒、空间分解算法等[11-12]。 GJK 算法[13]能够运用于各种凸体，但其迭代过程难以避免舍入误差，且迭代时间无法确定，不满足实时控制的要求。以上算法库基本解决了模型的碰撞检测问题，但存在算法复杂，计算时间长的问题，不能满足控制系统的实时性要求。

包围盒算法计算时间短，通过合理地建立包围盒，能够在满足实时性的要求下提高包裹性能。包围盒算法的精度与物体几何外形的逼近程度有关[14]。由于水下机械臂完全展开长度约为11 m，最大直径约为0.5 m，最小直径约为0.3 m。舱体如核心舱轴线长约为16.5 m，中间舱段的半径约为4.2 m，两端舱段的半径约为1.8 m，机械臂及舱体体积非常大且轴向半径不断变化，易导致包围盒内的多余体积较大，从而难以保证碰撞检测的精度。同时，由于水下机械臂的臂展长，质量大，导致其惯性大，减速时间长，为了保证航天员及训练设备的安全，对碰撞检测的实时性及准确性要求更高。干智峰等[15]通过分段建模方式建立AABB 层次树，对机械臂碰撞部位进行精准判断，但碰撞检测的精度由模型的AABB 进行多层划分保证，计算量较大；郑义等[16]采用球体及胶囊体包围盒对机械臂模型进行简化，提升了包围盒包裹性能，但采用全部遍历方式，算法计算量较大，并且采用正解对包围盒特征点进行定位，对复杂机械臂模型的适应性差；李玉虎等[17]提出一种上、下2 层的混合层次包围盒树创建方法，避免对不同种类包围盒之间进行相交测试，能够降低算法复杂度；Zhang 等[18]将2 个机械臂OBB 在地面投影，筛除不可能碰撞的OBB，但只能减少机械臂之间在与地面平行的距离较远时的碰撞检测计算量，且筛选效果较差；Safeea 等[19]基于QR 分解提出一种计算胶囊体包围盒之间最小距离的简单解析解，但计算时间并未得到改善。

本文针对航天员水下训练超大作业空间的人机安全问题，提出一种快速、准确的机械臂碰撞检测方法。首先，基于最小包络和二分法实现球体及胶囊体包围盒的包裹性能优化及自动生成；然后，基于二叉树思想，采用AABB-球体及胶囊体包围盒2 层结构，建立混合层次包围盒树，通过同层优先的遍历算法对碰撞部位进行快速定位。

2 机械臂及舱体模型包围盒创建

为了提高包围盒的包裹性能及测试效率，提高碰撞检测的精度及效率，需要对机械臂及舱体模型进行简化建模[20]。考虑到机械臂整体结构由2 根细长的圆柱连杆和一些近似短圆柱体的零件组成，舱体主要形状为圆柱体及球体，且圆柱体及球体包围盒能够方便、准确计算最小距离，故选用球体及胶囊体包围盒对机械臂及舱体模型进行简化建模。而在机械臂执行任务时，航天员通过手臂对舱体上的关键部件进行操作，为了满足航天员的操作空间，舱体上的部件与人体需要保持一定距离，故实际包围盒需要在最小包围盒的基础上扩大一定范围，此时机械臂与舱体表面细节已经在实际包围盒内，故在包围盒创建时可将这些表面细节进行简略。

机械臂球体及胶囊体包围盒的几何特征点可以利用机械臂DH 参数对运动过程中的机械臂进行正运动学分析求得，但该方法对机械臂模型几何结构要求比较严格，对于零部件结构复杂的机械臂通常还需要进行基于模型的修正计算，其包裹性能难以得到保证，且不易实现包围盒的自动生成。本文基于最小包络和二分法的思想提出了一种模型的最优球体及胶囊体包围盒的自动化生成方法。

2.1 胶囊体包围盒创建

最优胶囊体包围盒的创建旨在使胶囊体左、右半球内的点分别到左、右半球球心的最大距离恰好等于胶囊体半径，从而得到最小包络胶囊体，具体方法如下:

1)在三维模型导入阶段，对模型进行OBB 建模，从而获得位于模型最长边方向上的左右端点P1、P2及中心线P1P2，如图1 所示，为了简化计算，设定向量P1P2方向为模型的x轴方向。

图1 模型中心轴的创建Fig.1 Creation of model central axis

2)计算模型上所有三角片的顶点到中心线P1P2的最大距离dmax作为胶囊体包围盒的半径radius，模型三角片上的顶点(x，y，z) 到中心线P1P2的距离d如式(1)所示:

3)接下来的目标是将胶囊体左右半球的球心尽可能往中心移动。假设点P1(xmin，0，0)、P2(xmax，0，0) 为求得的OBB 中心线的端点。首先，令胶囊体包围盒左右半球的球心P3(x3，0，0)，P4(x4，0，0) 的初始值等于点P1、P2，以线段P3P4作为胶囊体包围盒的中心轴线；然后，确定点P3的x3的取值范围为xmin～(xmin＋radius)，点P4的x4的取值范围为(xmax-radius) ～xmax；最后，通过二分法快速查找P3、P4的最优解。

以点P3为例，算法流程如图2 所示，其中Pmax为模型在P3点左侧(x ＜x3) 所有点到P3的最大距离，P3.x为P3点的x轴坐标值，SD为给定的P3到最佳点(Pmax＝radius)的误差可接受范围，SD越小，包围盒包裹性能越好，耗费的计算时间相应增长。

图2 最优解算法流程图Fig.2 Flow chart of optimal solution algorithm

图3 为用最优解算法获得的满足精度要求的模型最优胶囊体包围盒。

图3 胶囊体包围盒Fig.3 Capsule enclosure

2.2 球体包围盒创建

球体包围盒的创建旨在找到一个最小包络球面能够使模型内所有点到球心的距离小于球体的半径，具体方法如下:

1)在模型导入阶段，对模型进行OBB 建模，从而获得模型的中心点center(x0，y0，z0)，如图4中点P所示。

图4 模型中心点的创建Fig.4 Creation of model center point

2)计算模型所有点到中心点center的距离，模型三角片上的顶点(x，y，z) 到中心点center的距离d如式(2)所示:

3)比较得到所有顶点到球心距离的最大值dmax，则球体包围盒的半径radius等于dmax。

图5 为得到的最优球体包围盒。球体及胶囊体包围盒只需在模型导入时创建一次，之后可以通过旋转平移坐标变换矩阵实现与机械臂模型的同步运动。

图5 球体包围盒Fig.5 Sphere bounding box

3 包围盒碰撞检测算法

3.1 AABB 碰撞检测算法

AABB 的碰撞检测只需要将2 个包围盒投影到x，y，z坐标轴上，若2 个包围盒在3 个坐标轴上的投影均相交，则说明2 个AABB 相交，相交测试只需要进行6 次比较运算[21]。

3.2 OBB 碰撞检测算法

OBB 之间采用分离轴算法进行碰撞检测，即给定2 个凸物体，若这2 个物体不相交，则一定存在一个平面能够将2 个物体隔开，分离轴即为该平面的法线，将2 个物体上的点投影到分离轴上，2 个物体的投影不重叠，则能够判定2 个物体不相交[22]。 OBB 总共有3 个面法矢以及3 个边法矢，2 个包围盒总共有6 个面法矢，加上2 个包围盒之间的边法矢叉乘得到的9 个面法矢，总共进行15 次分离轴测试[23]。

3.3 胶囊体及球体包围盒碰撞检测算法

胶囊体与球体包围盒之间的碰撞检测是基于最小距离的原理，即如果2 个物体之间的最小距离大于0，则能够说明2 个物体未相交，基于这种思想，可得到胶囊体及球体包围盒碰撞检测算法[24]。球体与球体之间的最小距离简化为求解2 个球心点之间的距离，球体与胶囊体之间的最小距离简化为求解球心点到胶囊体中心线段之间的最小距离，胶囊体与胶囊体之间的最小距离简化为求解胶囊体中心线段到胶囊体中心线段之间的最小距离。本文胶囊体及球体包围盒碰撞检测方法采用文献[6]中计算线段与线段以及点与线段之间最小距离的方法。

4 水下机械臂碰撞检测算法

层次包围树是基于二叉树，按照自顶向下的顺序对物体进行剖分，从而建立层次包围盒树，自顶向下的节点与包围盒紧密程度相对应[25]。层次包围盒树随着机械臂的运动进行实时更新，其碰撞检测遍历遵循同层优先的原则，对机械臂包围盒进行快速筛选，最终确定机械臂实际碰撞位置。

4.1 混合层次包围盒树创建

为给定模型创建OBB 树所需的时间大约是创建AABB 树所需时间的3 倍[26]。上层层次包围盒相交测试次数最多，需要构建简单、测试速度快，从而实现对碰撞部位快速定位；底层则为了满足碰撞监测精度要求，需要包裹性好的包围盒[27]。因此，虽然AABB 紧密性较差，但创建简单，相交测试计算量小，能够实现层次树的快速刷新及对碰撞部位的快速定位。球体及胶囊体包围盒计算量比AABB 大，比OBB 小，并且对于水下机械臂和舱体模型而言，包裹性能优于AABB 和OBB，故层次树上层节点选用AABB，层次树叶子节点则选用球体及胶囊体包围盒。而不同种类的包围盒之间的相交检测算法复杂，难以满足机械臂碰撞检测的实时性要求，所以对于机械臂模型及舱体模型均采用AABB 球体及胶囊体2 层结构，上层结构为AABB，下层结构为球体及胶囊体包围盒。机械臂和舱体层次结构图如图6 所示。

图6 机械臂和舱体层次结构图Fig.6 Hierarchy diagram of manipulator and cabin

其中AABB 层只与AABB 层进行相交检测，球体与胶囊体层只与球体与胶囊体层进行相交检测，降低了相交检测算法复杂度。 AABB 层可以实现包围盒的实时刷新，能够快速排除掉没有相交的物体，提升算法计算效率。球体及胶囊体层不仅能够满足算法效率的要求，还能够保证包围盒的紧密性。假设二叉树层数为n(n＞0)，则其叶子节点最大数量为2n-1。故假设叶子节点数为m，则层次树层数D如式(3)所示:

针对水下训练机械臂及航天员模型的结构特点，对模型的零部件进行拆分，拆分后得到的零部件去除水下机械臂基座、关节1 等不可能产生碰撞的部分，获得机械臂模型层次包围盒树的叶子节点，叶子节点数为13，故层次树实际层数为5。

层次树的创建流程按照自顶向下的顺序逐层将机械臂的父节点划分为子节点，按照从末端航天员到关节2 的左右顺序的排列，建立起基于AABB-球体及胶囊体包围盒2 层结构的水下机械臂碰撞检测混合层次包围盒树。机械臂混合层次包围盒树如图7 所示。水下机械臂叶子节点采用球体及胶囊体包围盒进行包围盒建模，其余节点均采用AABB 进行包围盒建模。水下机械臂叶子节点球体及胶囊体包围盒如图8 所示。

图7 机械臂碰撞检测混合层次包围盒树Fig.7 Hybrid hierarchical bounding box tree for collision detection of manipulator

图8 叶子节点球体及胶囊体包围盒示意图Fig.8 Schematic diagram of leaf nodes’ sphere and capsule bounding box

4.2 混合层次包围盒树的更新

混合层次包围盒树采用由叶子节点到根节点的更新顺序。混合层次包围盒树叶子节点的AABB 和球体及胶囊体包围盒在模型导入阶段已经创建完成，在机械臂运动期间通过旋转平移变换矩阵实现叶子节点包围盒的更新。其余层次的AABB 更新则是通过下一层次的左、右子节点的AABB 来创建上一层次的父节点的AABB，逐层创建，直到创建完成最高层次根节点的AABB，则实现了整个混合层次包围盒树的更新。

假设父节点的左、右子节点的AABB 的16 个顶点分别为Pi(xi，yi，zi)(i＝1，2，…，16)，比较得到xi、yi、zi的最大值和最小值，将最大、最小值组成点Pmax(Xmax，Ymax，Zmax) 和Pmin(Xmin，Ymin，Zmin)，这2 个点即为父节点的AABB 的2 个顶点，父节点的AABB 包围盒通过顶点Pmax以及Pmin来创建，父节点包围盒的长L、宽W、高H如式(4)所示:

更新过程总体示意图如图9 所示，通过4 号和5 号叶子节点的AABB 创建2 号节点的AABB，再通过6 号和7 号叶子节点的AABB 创建3 号节点的AABB，最后由2 号和3 号节点的AABB 创建1 号根节点的AABB，完成混合层次包围盒树的更新。

图9 混合层次包围盒树更新示意图Fig.9 Schematic diagram of update of hybrid hierarchical bounding box tree

4.3 混合层次包围盒树相交检测

混合层次包围盒树的相交检测采用同层优先的遍历算法。首先从层次树的根节点出发进行遍历，如果父节点检测到相交，则进入到左右子节点的相交检测中，直到对所有相交包围盒的左右子节点完成相交测试。在相交检测过程中若检测到叶子节点相交，则表明机械臂跟舱体之间产生了碰撞。若未检测到叶子节点相交，则表明机械臂与舱体之间未产生碰撞，开始对机械臂下一位置的层次树进行更新重建。假设带竖线的方块代表检测到相交的包围盒，空白的方块代表未检测到相交的包围盒，则从根节点开始遍历寻找产生相交的准确位置的过程如图10 中箭头所示。

图10 层次树遍历示意图Fig.10 Schematic diagram of hierarchical tree traversal

采用同层优先的遍历算法，即针对未发生碰撞的部位，只要父节点没有检测到相交，则跳到同层次的节点进行下一次相交检测，能够快速排除未相交的部位，大幅减少相交检测时间，实现碰撞部位的快速定位。同时，在机械臂距离舱体较远时，能够将遍历过程锁定在AABB 层的前几层，减少大量无意义的相交检测计算。

4.4 碰撞检测流程

碰撞检测算法流程图如图11 所示。 ①程序开始运行后，进行机械臂及舱体模型的导入；②通过模型三角片的顶点信息创建层次包围盒树叶子节点的球体及胶囊体包围盒和AABB；③利用创建好的叶子节点的包围盒进行一次混合层次包围盒树的创建；④对层次包围盒树进行相交检测遍历；⑤遍历完成后，输出机械臂碰撞状态，如果检测到最小距离低于碰撞检测阈值，则给操作人员发送预警信号；⑥若程序不关闭，则读取机械臂下一步的每个关节的运动位置；⑦更新下一位置的层次包围盒树；⑧最后重复④～⑦，实现机械臂的实时碰撞检测。

图11 碰撞检测算法流程图Fig.11 Flow chart of collision detection algorithm

5 试验验证

为了验证本文机械臂碰撞检测算法，选用航天训练水下机械臂作为试验对象，为满足碰撞检测实时性要求，碰撞检测总计算时间要求小于0.5 ms。实验平台如图12 所示，左图为水下机械臂及舱体实物，右图为水下机械臂操控台，操控台控制器为倍福高性能嵌入式工控机CX2040，设备配置为CPU: Intel® Corei7，2.1 GHz，双核；内存4 GB。

图12 水下训练环境和操控台Fig.12 Underwater training environment and console

将本文碰撞检测算法集成到自主研发的水下训练机械臂操控系统，该操控系统利用OpenGL和MFC 实现虚拟环境和人机交互，集成了机械臂运动学模型，能够实现机械臂及舱体模型的导入及位置标定、机械臂的仿真运动轨迹规划以及机械臂模型跟随机械臂实体进行同步运动等功能，操控台控制器运行虚拟平台代码，机械臂及舱体模型导入后通过本文算法自动创建包围盒树并在模型上显示球体及胶囊体包围盒，效果图如图13所示。

图13 模型导入效果图Fig.13 Effect drawing of model import

5.1 精度验证

包围盒的紧密性越好，碰撞检测算法精度越高。机械臂和舱体的OBB、球体及胶囊体包围盒的总体积如表1 所示，球体及胶囊体包围盒总体积比OBB 的总体积减少了28%，说明采用球体及胶囊体包围盒作为层次包围盒树的叶子节点能够显著减少包围盒的无效体积，提高碰撞检测算法的精度。

表1 不同包围盒体积对比Table 1 Volume comparison of various bounding boxes

表2 为模型真实体积与球体及胶囊体包围盒体积和两者之间的体积比，机械臂整体体积比为49.2%，核心舱体积比为82.8%，节点舱体积比为96.9%，实验舱体积比为93.1%，可以看到球体及胶囊体模型整体包络效果较好，特别是对于舱体模型包络效果非常好，对于机械臂部件模型，组装成整体后，胶囊体包围盒部分重叠部位较多，故实际体积比大于49.2%。

表2 模型体积和包围盒体积对比Table 2 Comparison of model volume and bounding box volume

5.2 实时性验证

由于机械臂及舱体设备成本昂贵，而且机械臂的质量大、臂展长，导致其惯性大，所需要的减速时间长，为航天员及试验设备的安全性考虑，将AABB 及OBB 长宽高以中心为基准增大50 mm，球体及胶囊体包围盒半径提升50 mm，即将碰撞检测的阈值设为(100 mm，100 mm)，这样既可以保证安全性，又在航天员手动操作空间允许的范围内，不会影响其进行训练操作。机械臂以0.1 m/s 的最大速度进行S 型加减速直线运动。表3 为4 种算法计算时间t的统计数据，图14 为4 种算法的计算时间曲线图。可以看到本文算法能够满足水下机械臂碰撞检测实时性要求，且计算时间较其他算法缩短了45%以上，大幅减少了碰撞检测计算时间，提高了碰撞检测效率。

图14 4 种算法计算时间对比Fig.14 Comparison of calculation time of four algorithms

表3 算法计算时间统计分析Table 3 Statistical analysis of algorithm calculation time

5.3 准确性验证

当将包围盒碰撞检测阈值设置为100 mm时，图15 为机械臂末端航天员训练用操作台与舱体之间产生碰撞时的位置，可以看到操作台包围盒在此时做出了碰撞反应，操作台模型及其包围盒变成了红色，此时计算得到的机械臂与舱体包围盒之间的最小距离dmin为68.00 mm，小于碰撞检测的阈值100 mm，说明包围盒已经产生了相交。

图15 包围盒碰撞位置Fig.15 Collision position of bounding box

通过在机械臂末端安装超声测距传感器，令机械臂末端以水平姿态，沿竖直方向接近舱体，测量机械臂运动过程中机械臂末端与舱体的最小距离。结果如图16、图17 所示。图16 为通过算法计算得到的机械臂包围盒与舱体包围盒之间的最小距离和通过传感器实际测得的最小距离，图17为本文算法得出的碰撞信号和通过实际距离确定的碰撞信号。图17 中出现一个周期的延迟是由于传感器测距本身存在误差。结合图16、图17可得，当将碰撞检测距离设置为100 mm 时，在产生碰撞信号的部位，其最小距离均小于100 mm，故本文算法可以准确定位碰撞部位，满足碰撞检测准确性要求。