CN114012726B - 一种航天机械臂碰撞检测方法 - Google Patents

一种航天机械臂碰撞检测方法 Download PDF

Info

Publication number
CN114012726B
CN114012726B CN202111315578.2A CN202111315578A CN114012726B CN 114012726 B CN114012726 B CN 114012726B CN 202111315578 A CN202111315578 A CN 202111315578A CN 114012726 B CN114012726 B CN 114012726B
Authority
CN
China
Prior art keywords
bounding box
capsule
sphere
model
mechanical arm
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111315578.2A
Other languages
English (en)
Other versions
CN114012726A (zh
Inventor
张宇
张得礼
王志国
周星雨
秦臻
曾思力
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202111315578.2A priority Critical patent/CN114012726B/zh
Publication of CN114012726A publication Critical patent/CN114012726A/zh
Application granted granted Critical
Publication of CN114012726B publication Critical patent/CN114012726B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种航天机械臂碰撞检测方法，先基于最小包络和二分法算得模型最小包络球体及胶囊体，再基于二叉树思想采用AABB和球体及胶囊体包围盒两层结构，按照自顶向下顺序对机械臂及舱体模型建立混合层次包围盒树，遵循同层优先的原则对层次包围盒树节点进行遍历，利用上层AABB层实现了碰撞部位的快速定位，利用下层球体及胶囊体包围盒层保证了碰撞检测的精度，本发明的方法提高了碰撞检测的精度，满足了超大作业空间碰撞检测实时性及准确性要求。

Description

一种航天机械臂碰撞检测方法
技术领域
本发明涉及机械臂技术领域，尤其是一种航天机械臂碰撞检测方法。
背景技术
空间机械臂在空间站的建设及维护中，起到了关键作用，能够协助航天员完成大量复杂的舱外活动或替代航天员执行一些高危任务等，提高航天员工作效率和任务安全性。为保证航天员空间站任务安全、顺利完成，航天员需要提前在水下模拟失重环境中进行充足的模拟训练。水下训练机械臂是航天员水下出舱模拟训练中必不可少的辅助设备，需要配合航天员进行出舱移动及验证在空间站的相关任务；为了保证航天员及水下训练设备的安全性，需要对水下训练机械臂的碰撞进行检测，同时，由于水下机械臂的臂展长，质量大，导致其惯性大，减速时间长，为了航天员及训练设备安全性考虑，对碰撞检测的实时性及准确性提出很高要求。
目前，机械臂碰撞检测方法主要有基于外部设备和基于模型包围盒的碰撞检测方法，基于外部设备的碰撞检测方法需要添加额外的传感器或相机等进行信息采集，增加了设备成本及系统复杂性。基于模型包围盒的碰撞检测方法目前有如下几种：第一种，采用AABB(轴对齐包围盒)，通过分段建模的方式建立包围盒层次树对机械臂碰撞部位进行精准判断，但碰撞检测的精度是通过对模型的AABB进行多层划分保证，要获得足够精度，需要提高划分层次数，计算量较大；第二种，采用球体包围盒及胶囊体包围盒对机械臂模型进行简化然后进行碰撞检测，此方法提升了包围盒包裹性能，但在碰撞检测过程中采用全部遍历的方式，算法计算量较大，并且采用正解对球体包围盒及胶囊体包围盒特征点进行定位，对复杂机械臂模型的适应性差；第三种，采用检测两个机械臂在地面投影是否相交来判断碰撞产生的可能性，在可能发生碰撞的情况下才对OBB(方向包围盒)进行相交测试，但该方法只能减少机械臂之间在与地面平行的方向上距离较远时的碰撞检测计算量，且只向地面投影进行碰撞位置筛选，筛选效果较差；第四种，采用基于层次包围盒以AABB(轴对齐包围盒)、OBB(方向包围盒)、三角面片为基础提出一种空间碰撞快速验证方法，通过二分法配合计算相交距离，提高了碰撞检测效率，但是空间距离验证耗费时间仍较长，难以满足水下机械臂碰撞检测实时性要求。
发明内容
一种航天机械臂碰撞检测方法，其特征在于，包括：
基于最小包络和二分法创建机械臂模型及舱体模型的球体包围盒和胶囊体包围盒；
球体包围盒及胶囊体包围盒碰撞检测分析；
基于二叉树，建立混合层次包围盒树，混合层次包围盒树上层节点用轴对齐包围盒，混合层次包围盒树叶子节点用球体包围盒及胶囊体包围盒；
按从叶子节点到根节点的顺序更新混合层次包围盒树；
采用同层优先的遍历算法对混合层次包围盒树进行相交检测，快速筛选和准确定位碰撞部位。
进一步地，所述的航天机械臂碰撞检测方法，其特征在于，所述胶囊体包围盒创建方法：
在三维模型导入阶段，对模型进行方向包围盒建模，从而获得模型沿最长边方向上的左右端点P1、P2及中心线P1P2，设定中心线P1P2方向为模型的X轴方向；计算模型上所有三角片的顶点到中心线P1P2的最大距离dmax作为胶囊体包围盒的半径radius，模型三角片的顶点(x，y，z)到中心线P1P2的距离d为：
Figure BDA0003343501850000021
接下来的运算将胶囊体包围盒左右半球的球心，尽可能往中心移动；假设点P1(Xmin，0，0)，P2(Xmax，0，0)为求得的方向包围盒中心线的端点；首先，令胶囊体包围盒左右半球的球心P3(X3，0，0)，P4(X4，0，0)初始值等于点P1，P2的值，以线段P3P4作为胶囊体包围盒的中心轴线；然后，确定点P3的X3的取值范围为:Xmin～Xmin+radius，点P4的X4的取值范围为范围为：Xmax-radius～Xmax；最后，通过二分法来快速查找P3，P4的最优解，可创建出胶囊体包围盒；
球体包围盒创建方法：
在三维模型导入阶段，对模型进行方向包围盒建模，从而获得模型的中心点P(x0，y0，z0)；计算模型所有点到中心点P的距离，从而得到距离最大值d′max，则球体包围盒的半径等于d′max，可创建出球体包围盒。
进一步地，所述的航天机械臂碰撞检测方法，其特征在于，所述胶囊体包围盒与球体包围盒之间碰撞检测是基于最小距离的原理，其中，球体包围盒与球体包围盒之间的最小距离简化为求两个球心点之间的距离，球体包围盒与胶囊体包围盒之间的最小距离简化为求球心点到胶囊体包围盒中心线段之间的最小距离，胶囊体包围盒与胶囊体包围盒之间的最小距离简化为求胶囊体包围盒中心线段到胶囊体包围盒中心线段之间的最小距离。
进一步地，所述的航天机械臂碰撞检测方法，其特征在于，所述建立混合层次包围盒树，层次树上层节点选用轴对齐包围盒，层次树叶子节点则选用球体包围盒及胶囊体包围盒，对于机械臂模型及舱体模型均采用轴对齐包围盒球体及胶囊体包围盒两层结构，上层结构为轴对齐包围盒，下层结构为球体包围盒及胶囊体包围盒，其中轴对齐包围盒层只与轴对齐包围盒层进行相交检测，球体包围盒与胶囊体包围盒层只与球体包围盒与胶囊体包围盒层进行相交检测；针对水下训练机械臂及航天员模型的结构特点，排除水下机械臂基座、关节1等不可能产生碰撞的部位，将机械臂从整体到局部划分成小块，将所有小块以自顶向下方式，按照从末端航天员到关节2的左右顺序的排列，建立起基于轴对齐包围盒和球体包围盒及胶囊体包围盒两层结构的水下机械臂碰撞检测混合层次包围盒树，水下机械臂除了层次树末端的叶子节点，其余节点均采用轴对齐包围盒进行包围盒建模，叶子节点则采用球体包围盒及胶囊体包围盒的方式进行包围盒建模。
进一步地，所述的航天机械臂碰撞检测方法，其特征在于，所述更新混合层次包围盒树，采用由叶子节点到根节点的更新顺序；混合层次包围盒树叶子节点的轴对齐包围盒和球体包围盒及胶囊体包围盒在模型导入阶段已经创建完成，在机械臂运动期间通过旋转平移变换矩阵跟随模型运动，实现了叶子节点包围盒的更新；其余层次的轴对齐包围盒更新则是通过下一层次的左、右子节点的轴对齐包围盒来创建上一层次的父节点的轴对齐包围盒，逐层创建直到创建完成最高层次的根节点的轴对齐包围盒，则实现了整个混合层次包围盒树的更新；假设父节点的左、右子节点的轴对齐包围盒的16个顶点分别为Pi(xi，yi，zi)(i＝1～16)，通过比较得到xi、yi、zi的最大值和最小值，将最大最小值组成两个点P′max(X′max，Y′max，Z′max)和P′min(X′min，Y′min，Z′min)，这两个点即为父节点的轴对齐包围盒的两个顶点，父节点的轴对齐包围盒通过顶点P′max以及P′min来创建，父节点包围盒的长L、宽W、高H为：
Figure BDA0003343501850000031
Figure BDA0003343501850000032
Figure BDA0003343501850000033
通过4号和5号叶子节点的轴对齐包围盒创建2号节点的轴对齐包围盒，再通过6号和7号叶子节点的轴对齐包围盒创建3号节点的轴对齐包围盒，最后由2号和3号节点的轴对齐包围盒创建1号根节点的轴对齐包围盒，从而完成混合层次包围盒树的更新。
进一步地，所述的航天机械臂碰撞检测方法，其特征在于，所述混合层次包围盒树相交检测，采用同层优先的遍历算法，首先从层次树的根节点出发进行遍历，如果父节点检测到相交，则进入到左右子节点的相交检测中，直到对所有相交的包围盒的左右子节点完成相交测试。在相交检测过程中若检测到叶子节点相交，则表明机械臂跟舱体之间产生了碰撞。若未检测到叶子节点相交，则表明机械臂与舱体之间未产生碰撞，开始对机械臂下一位置的层次树进行更新重建。
本发明的有益效果：本方法提高了机械臂碰撞检测精度，减少了机械臂在任意位置的碰撞检测计算量，能够满足水下训练机械臂超大尺寸空间的碰撞检测的实时性及准确性要求；本方法已集成并运用于自主研发的水下训练机械臂操控系统，并经过长时间运行验证了本方法具有优良的稳定性和可靠性。
附图说明
图1为模型中心轴创建的示意图；
图2为胶囊体包围盒运算过程流程图；
图3为用最优解算法获得的满足精度要求的模型最优胶囊体包围盒示意图；
图4为模型中心点示意图；
图5为球体包围盒示意图；
图6为机械臂和舱体层次结构图；
图7为机械臂碰撞检测混合层次包围盒树示意图；
图8为混合层次包围盒树更新示意图；
图9为层次树遍历示意图；
图10为三种算法计算时间对比示意图；
图11为包围盒最小距离图像；
图12为碰撞信号图；
图13为机械臂末端操作台与舱体碰撞位置示意图。
图中：1-三角片。
具体实施方式
在三维模型导入阶段，对模型进行OBB(方向包围盒)建模，从而获得模型沿最长边方向上的左右端点P1、P2及中心线P1P2，如图1所示，为了简化计算，设定中心线P1P2方向为模型的X轴方向；计算模型上所有三角片1的顶点到中心线P1P2的最大距离dmax作为胶囊体包围盒的半径radius，模型三角片1上的顶点(x，y，z)到中心线P1P2的距离d为：
Figure BDA0003343501850000041
接下来的目标是在运算中将胶囊体包围盒左右半球的球心，尽可能往中心移动；假设点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的最大距离，SD为给定的P3到最佳点(Pmax等于radius)的误差可接受范围，SD越小，包围盒包裹性能越好，耗费的计算时间相应增长,用最优解算法获得的满足精度要求的模型最优胶囊体包围盒示意图如图3所示。
在模型导入阶段，对模型进行OBB建模，从而获得模型的中心点P(x0，y0，z0)，如图4中点P所示；计算模型所有点到中心点P的距离，从而得到距离最大值d′max，则球体包围盒的半径radius等于d′max，得到的最优球体包围盒示意图如图5所示。
AABB(轴对齐包围盒)的碰撞检测只需要将两个包围盒投影到X，Y，Z坐标轴上，若两个包围盒在三个坐标轴上的投影均相交，则说明两个AABB相交，相交测试只需要进行6次比较运算。
OBB(方向包围盒)之间采用分离轴算法进行碰撞检测，分离轴算法即给定两个凸物体，若这两个物体不相交，则一定存在一个平面能够将两个物体隔开，分离轴即为该平面的法线，将两个物体上的点投影到分离轴上，两个物体的投影不重叠则能够判定两个物体不相交。OBB总共有3个面法矢以及3个边法矢，两个包围盒总共有6个面法矢，加上两个包围盒之间的边法矢叉乘得到的9个面法矢，总共进行15次分离轴测试。
胶囊体包围盒与球体包围盒之间的碰撞检测是基于最小距离的原理，如果两个物体之间的最小距离大于0，则能够说明两个物体未相交，基于这种思想，可得到胶囊体包围盒及球体包围盒碰撞检测算法。球体包围盒与球体包围盒之间的最小距离简化为求两个球心点之间的距离，球体包围盒与胶囊体包围盒之间的最小距离简化为求球心点到胶囊体中心线段之间的最小距离，胶囊体包围盒与胶囊体包围盒之间的最小距离简化为求胶囊体中心线段到胶囊体中心线段之间的最小距离。
层次包围树的思想是基于二叉树，按照自顶向下的顺序对物体进行剖分，从而建立层次包围盒树，自顶向下的节点与包围盒紧密程度相对应。层次包围盒树需要随着机械臂的运动进行实时更新，其碰撞检测遍历遵循同层优先的原则，对机械臂包围盒进行快速筛选，最终确定机械臂实际碰撞位置。
为给定模型创建OBB树所需的时间大约是创建AABB树所需时间的三倍。上层层次包围盒相交测试次数最多，需要有构建简单和测试速度快的特点，从而对碰撞部位进行快速定位；底层则为了满足碰撞监测精度要求，需要包裹性好的包围盒。基于此，虽然AABB紧密性较差，但创建简单，相交测试计算量小，能够实现层次树的快速刷新及对碰撞部位的快速定位。球体包围盒及胶囊体包围盒计算量比AABB大，比OBB小，并且其对于水下机械臂和舱体模型而言，包裹性能优于AABB和OBB。故层次树上层节点选用AABB，层次树叶子节点则选用球体包围盒及胶囊体包围盒。而不同种类的包围盒之间的相交检测算法复杂，难以满足机械臂碰撞检测的实时性要求，所以对于机械臂模型及舱体模型均采用AABB球体包围盒及胶囊体包围盒两层结构，上层结构为AABB，下层结构为球体包围盒及胶囊体包围盒，机械臂和舱体层次结构如图6所示；其中AABB层只与AABB层进行相交检测，球体包围盒与胶囊体包围盒层只与球体包围盒与胶囊体包围盒层进行相交检测，降低了相交检测算法复杂度，减少算法计算时间。其中AABB层可以实现包围盒的实时刷新，能够快速排除掉没有相交的物体，提升算法计算效率。球体包围盒及胶囊体包围盒层不仅能够满足算法效率的要求，还能够保证包围盒的紧密性。
针对水下训练机械臂及航天员模型的结构特点，排除水下机械臂基座、关节1等不可能产生碰撞的部位，将机械臂从整体到局部划分成小块；将所有小块以自顶向下方式，按照从末端航天员到关节2的左右顺序的排列，建立起基于AABB球体包围盒及胶囊体包围盒两层结构的水下机械臂碰撞检测混合层次包围盒树，机械臂混合层次包围盒树如图7所示；水下机械臂除了层次树末端的叶子节点，其余节点均采用AABB进行包围盒建模，叶子节点则采用球体包围盒及胶囊体包围盒的方式进行包围盒建模。
混合层次包围盒树采用由叶子节点到根节点的更新顺序；混合层次包围盒树叶子节点的AABB和球体包围盒及胶囊体包围盒在模型导入阶段已经创建完成，在机械臂运动期间通过旋转平移变换矩阵跟随模型运动，实现了叶子节点包围盒的更新；其余层次的AABB更新则是通过下一层次的左、右子节点的AABB来创建上一层次的父节点的AABB，逐层创建直到创建完成最高层次的根节点的AABB，则实现了整个混合层次包围盒树的更新。
假设父节点的左、右子节点的AABB的16个顶点分别为Pi(xi，yi，zi)(i＝1～16)，通过比较得到xi、yi、zi的最大值和最小值，将最大最小值组成两个点P′max(X′max，Y′max，Z′max)和P′min(X′min，Y′min，Z′min)，这两个点即为父节点的AABB的两个顶点，父节点的AABB包围盒通过顶点P′max以及P′min来创建，父节点包围盒的长L、宽W、高H为：
Figure BDA0003343501850000061
Figure BDA0003343501850000062
Figure BDA0003343501850000063
更新过程总体示意图如图8所示，通过4号和5号叶子节点的AABB创建2号节点的AABB，再通过6号和7号叶子节点的AABB创建3号节点的AABB，最后由2号和3号节点的AABB创建1号根节点的AABB，从而完成混合层次包围盒树的更新。
混合层次包围盒树的相交检测采用同层优先的遍历算法；首先从层次树的根节点出发进行遍历，如果父节点检测到相交，则进入到左右子节点的相交检测中，直到对所有相交的包围盒的左右子节点完成相交测试；在相交检测过程中若检测到叶子节点相交，则表明机械臂跟舱体之间产生了碰撞。若未检测到叶子节点相交，则表明机械臂与舱体之间未产生碰撞，开始对机械臂下一位置的层次树进行更新重建。如图9所示，假设带竖线的方块代表检测到相交的包围盒，空白的方块代表未检测到相交的包围盒，则从根节点开始遍历寻找产生相交的准确位置的过程如图中箭头所示。
采用同层优先的遍历算法，只要父节点没有检测到相交，则跳到同层次的节点进行下一次相交检测，能够快速排除未相交的部位，大幅减少相交检测时间，提高碰撞检测算法效率，实现碰撞部位的快速定位；同时，在机械臂距离舱体较远时，能够将遍历过程锁定在AABB层的前几层，能够减少大量无意义的相交检测计算。
实施例1
为了验证本文机械臂碰撞检测算法，选用航天训练水下机械臂作为实验对象，为满足机械臂碰撞检测实时性要求，该机械臂碰撞检测总计算时间要求小于0.5ms；将本文碰撞检测算法集成到自主研发的水下训练机械臂操控系统，该操控系统利用OpenGL和MFC实现虚拟环境和人机交互，能够实现机械臂及舱体模型的导入及位置标定、机械臂的仿真运动轨迹规划以及机械臂模型跟随机械臂实体进行同步运动等功能，虚拟平台代码在倍福高性能嵌入式工控机:CX2040上运行，机械臂及舱体模型导入后通过本文算法自动创建包围盒树并在模型上显示球体包围盒及胶囊体包围盒。
首先，验证碰撞检测算法的精度；包围盒的紧密性越好，碰撞检测算法精度越高；机械臂和舱体的OBB、球体包围盒及胶囊体包围盒的总体积如表1所示，球体及胶囊体包围盒总体积比OBB的总体积减少了28％，说明采用球体及胶囊体包围盒作为层次包围盒树的叶子节点，能够显著减少包围盒的无效体积，提高碰撞检测算法的精度。
表1
包围盒类型 OBB 球体及胶囊体包围盒
包围盒总体积/m3 489.8294 352.6508
体积比％ 100％ 352.6508/489.8294*100％＝72％
接下来验证碰撞检测算法的实时性是否能够满足要求，由于机械臂及舱体设备成本昂贵，而且机械臂的质量大、臂展长，导致其惯性大，所需要的减速时间长，为了航天员及实验设备的安全性考虑，将AABB及OBB长宽高以中心为基准增大50mm，球体及胶囊体包围盒半径提升50mm，即将碰撞检测的阈值设为100mm，100mm的阈值既可以保证安全性，又在航天员手动操作空间允许的范围内，不会影响航天员进行训练操作；机械臂以0.1m/s的最大速度进行S型加减速直线运动。图10为三种算法的计算时间曲线图的对比，表2为对图10中的几种算法计算时间的统计分析得到的数据，可以看到，采用传统OBB遍历所需时间平均值为1.095ms，最大值为1.883ms，最小值为1.077ms，不能满足碰撞检测的实时性要求；采用球体及胶囊体包围盒遍历所需时间平均为0.248ms，最大值0.819ms，最小值为0.243ms也无法满足碰撞检测实时性要求，但是其计算时间优于传统OBB遍历，故采用球体及胶囊体作为混合层次树的叶子节点，能够减少算法计算时间；采用本文碰撞检测算法，碰撞检测平均时间为0.087ms，最大值为0.215ms，最小值为0.087ms，能够满足水下机械臂碰撞检测实时性要求，且计算时间较球体及胶囊体包围盒遍历算法缩短了60％以上。结合图10及表2可得，本文算法能够大幅减少碰撞检测计算时间，提高碰撞检测效率，满足水下机械臂碰撞检测实时性要求。
表2
算法 计算时间平均值/ms 计算时间最大值/ms 计算时间最小值/ms
OBB 1.095 1.883 1.077
球体及胶囊体 0.248 0.819 0.243
本文算法 0.087 0.215 0.087
最后验证算法的准确性是否满足要求，当将包围盒扩大100mm时，图13为机械臂末端航天员的训练用操作台与舱体之间产生碰撞时的位置，可以看到操作台包围盒在此时做出了碰撞反应，此时计算得到的机械臂与舱体包围盒之间的最小距离为62.14mm，小于碰撞检测的阈值100mm，说明包围盒已经产生了相交。
结合图11机械臂及宇航员包围盒与舱体的最小距离图像及图12碰撞信号图像可得，当将碰撞检测距离设置为100mm时，在产生碰撞信号的部位，其最小距离均小于100mm，故本文算法可以准确定位碰撞部位，满足碰撞检测准确性要求。

Claims (4)

1.一种航天机械臂碰撞检测方法，其特征在于，包括：
基于最小包络和二分法创建机械臂模型及舱体模型的球体包围盒和胶囊体包围盒；
球体包围盒及胶囊体包围盒碰撞检测分析；
基于二叉树，建立混合层次包围盒树，混合层次包围盒树上层节点用轴对齐包围盒，混合层次包围盒树叶子节点用球体包围盒及胶囊体包围盒；对于机械臂模型建立混合层次包围盒树，机械臂模型的混合层次包围盒树上层节点选用轴对齐包围盒，叶子节点选用球体包围盒及胶囊体包围盒；对舱体模型建立混合层次包围盒树，舱体模型的混合层次包围盒树上层节点选用轴对齐包围盒，叶子节点选用球体包围盒及胶囊体包围盒；轴对齐包围盒层只与轴对齐包围盒层进行相交检测，球体包围盒与胶囊体包围盒层只与球体包围盒与胶囊体包围盒层进行相交检测；
按从叶子节点到根节点的顺序更新混合层次包围盒树；混合层次包围盒树叶子节点的球体包围盒及胶囊体包围盒在模型导入阶段已经创建完成，在机械臂运动期间通过旋转平移变换矩阵跟随模型运动，实现了叶子节点包围盒的更新；其余层次的轴对齐包围盒更新则是通过下一层次的左、右子节点的轴对齐包围盒来创建上一层次的父节点的轴对齐包围盒，逐层创建直到创建完成最高层次的根节点的轴对齐包围盒，则实现了整个混合层次包围盒树的更新；假设父节点的左、右子节点的轴对齐包围盒的16个顶点分别为Pi(xi，yi，zi)(i＝1～16)，通过比较得到xi、yi、zi的最大值和最小值，将最大最小值组成两个点P′ max(X′ max，Y′ max，Z′ max)和P′ min(X′ min，Y′ min，Z′ min)，这两个点即为父节点的轴对齐包围盒的两个顶点，父节点的轴对齐包围盒通过顶点P′ max以及P′ min来创建，父节点包围盒的长L、宽W、高H为：
Figure FDA0004267846130000011
Figure FDA0004267846130000012
Figure FDA0004267846130000013
通过4号和5号叶子节点的轴对齐包围盒创建2号节点的轴对齐包围盒，再通过6号和7号叶子节点的轴对齐包围盒创建3号节点的轴对齐包围盒，最后由2号和3号节点的轴对齐包围盒创建1号根节点的轴对齐包围盒，从而完成混合层次包围盒树的更新；
采用同层优先的遍历算法对混合层次包围盒树进行相交检测，快速筛选和准确定位碰撞部位。
2.根据权利要求1所述的航天机械臂碰撞检测方法，其特征在于，所述胶囊体包围盒创建方法：
在三维模型导入阶段，对模型进行方向包围盒建模，从而获得模型沿最长边方向上的左右端点P1、P2及中心线P1P2，设定中心线P1P2方向为模型的X轴方向；计算模型上所有三角片的顶点到中心线P1P2的最大距离dmax作为胶囊体包围盒的半径radius，模型三角片的顶点(x，y，z)到中心线P1P2的距离d为：
Figure FDA0004267846130000021
接下来的运算将胶囊体左右半球的球心，尽可能往中心移动；假设点P1(Xmin，0，0)，P2(Xmax，0，0)为求得的方向包围盒中心线的端点；首先，令胶囊体包围盒左右半球的球心P3(X3，0，0)，P4(X4，0，0)初始值等于点P1，P2的值，以线段P3P4作为胶囊体包围盒的中心轴线；然后，确定点P3的X3的取值范围为:Xmin～Xmin+radius，点P4的X4的取值范围为范围为：Xmax-radius～Xmax；最后，通过二分法来快速查找P3，P4的最优解，可创建出胶囊体包围盒；
球体包围盒创建方法：
在三维模型导入阶段，对模型进行方向包围盒建模，从而获得模型的中心点P(x0，y0，z0)；计算模型所有点到中心点P的距离，从而得到距离最大值d′ max，则球体包围盒的半径等于d′ max，可创建出球体包围盒。
3.根据权利要求1所述的航天机械臂碰撞检测方法，其特征在于，所述胶囊体包围盒与球体包围盒之间碰撞检测是基于最小距离的原理，其中，球体包围盒与球体包围盒之间的最小距离简化为求两个球心点之间的距离，球体包围盒与胶囊体包围盒之间的最小距离简化为求球心点到胶囊体包围盒中心线段之间的最小距离，胶囊体包围盒与胶囊体包围盒之间的最小距离简化为求胶囊体包围盒中心线段到胶囊体包围盒中心线段之间的最小距离。
4.根据权利要求1所述的航天机械臂碰撞检测方法，其特征在于，所述混合层次包围盒树相交检测，采用同层优先的遍历算法，首先从层次树的根节点出发进行遍历，如果父节点检测到相交，则进入到左右子节点的相交检测中，直到对所有相交的包围盒的左右子节点完成相交测试；在相交检测过程中若检测到叶子节点相交，则表明机械臂跟舱体之间产生了碰撞；若未检测到叶子节点相交，则表明机械臂与舱体之间未产生碰撞，开始对机械臂下一位置的层次树进行更新重建。
CN202111315578.2A 2021-11-08 2021-11-08 一种航天机械臂碰撞检测方法 Active CN114012726B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111315578.2A CN114012726B (zh) 2021-11-08 2021-11-08 一种航天机械臂碰撞检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111315578.2A CN114012726B (zh) 2021-11-08 2021-11-08 一种航天机械臂碰撞检测方法

Publications (2)

Publication Number Publication Date
CN114012726A CN114012726A (zh) 2022-02-08
CN114012726B true CN114012726B (zh) 2023-07-04

Family

ID=80062507

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111315578.2A Active CN114012726B (zh) 2021-11-08 2021-11-08 一种航天机械臂碰撞检测方法

Country Status (1)

Country Link
CN (1) CN114012726B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114918913B (zh) * 2022-03-31 2024-04-19 杭州芯控智能科技有限公司 一种碰撞检测方法、装置、计算机设备和存储介质
CN115048824B (zh) * 2022-08-15 2022-12-06 北京华航唯实机器人科技股份有限公司 一种碰撞检测方法、装置及计算机可读介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2008091958A2 (en) * 2007-01-23 2008-07-31 Mental Images, Gmbh Terminating spatial partition hierarchies by a priori bounding memory
CN106197928A (zh) * 2016-06-30 2016-12-07 哈尔滨理工大学 一种基于虚拟球的包围盒碰撞检测方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2915826B2 (ja) * 1995-07-11 1999-07-05 富士通株式会社 干渉チェック装置
CN101593366A (zh) * 2009-06-24 2009-12-02 北京航空航天大学 一种基于平衡二叉树的大规模虚拟场景碰撞检测方法
CN102368280A (zh) * 2011-10-21 2012-03-07 北京航空航天大学 一种面向虚拟装配的基于aabb-obb混合包围盒的碰撞检测方法
CN108052703A (zh) * 2017-11-22 2018-05-18 南京航空航天大学 基于混合层次包围盒的快速碰撞检测方法
CN109465835A (zh) * 2018-09-25 2019-03-15 华中科技大学 一种动态环境下双臂服务机器人作业的事前安全预测方法
CN111546376A (zh) * 2020-04-22 2020-08-18 哈尔滨工业大学 一种空间组合机械臂的快速自碰撞检测方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2008091958A2 (en) * 2007-01-23 2008-07-31 Mental Images, Gmbh Terminating spatial partition hierarchies by a priori bounding memory
CN106197928A (zh) * 2016-06-30 2016-12-07 哈尔滨理工大学 一种基于虚拟球的包围盒碰撞检测方法

Also Published As

Publication number Publication date
CN114012726A (zh) 2022-02-08

Similar Documents

Publication Publication Date Title
CN114012726B (zh) 一种航天机械臂碰撞检测方法
CN111360824B (zh) 一种双臂自碰撞检测方法和计算机可读存储介质
US5056031A (en) Apparatus for detecting the collision of moving objects
CN106444861B (zh) 一种基于三维手势的空间机器人遥操作系统
CN107972034B (zh) 一种基于ros平台的复杂工件轨迹规划仿真系统
CN112669434B (zh) 一种基于网格与包围盒的碰撞检测方法
CN103236079B (zh) 一种基于三维模型体素化的内部球改进构造方法
JPH0927046A (ja) 干渉チェック方法
CN113211495A (zh) 一种机械臂碰撞检测方法、系统、存储介质和机械臂
CN109376383A (zh) 一种基于碰撞检测的爆炸视图生成方法
GB2227106A (en) Detecting collision
Wang et al. Digital twin implementation of autonomous planning arc welding robot system
CN116822100B (zh) 一种数字孪生建模方法及其仿真试验系统
CN116049995B (zh) 级间分离试验中基于虚拟样机的特种机构碰撞预测方法
CN111462330B (zh) 一种基于平面法向投影的测量视点规划方法
CN115008475A (zh) 一种基于混合几何表征的双机械臂协同避障运动规划优化方法
CN102393827A (zh) 一种基于连续法向锥剔除的柔性场景连续碰撞检测方法
CN114742944A (zh) 面向工业机器人路径规划的保守碰撞检测方法
JP3425760B2 (ja) 干渉チェック装置
Pudney Surface Modelling And Surface Following For Robots Equipped With Range Sensors
Liang et al. Model-based 3d pose estimation for pick-and-place application
CN117422810B (zh) 结构与参数引导的室内要素规则化与关系推理方法及终端
CN112917477B (zh) 多自由度机器人静态环境运动规划方法
CN112330804B (zh) 一种局部可变形三维模型接触检测方法
CN116985151B (zh) 一种约束桁架内机械臂强化学习避障规划与训练方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant