[发明专利]一种改进型D*机械臂动态避障路径规划方法

摘要

本发明实施例提供了一种改进型D^*机械臂避障路径规划方法，实现机械臂动态避障路径规划，包括：通过层次包围盒法将机械臂所有部件和环境中的障碍物均转化为长方体包络，并设计了相关算法实现机械臂与机械臂以及机械臂与环境之间的快速碰撞检测；根据空间机械臂特征，获得基于启发函数的路径搜索算法；将应用于七自由度机械臂D^*避障路径规划进行改进，完成七自由度机械臂动态避障路径规划。根据本发明实施例提供的技术方案，以实现七自由度机械臂基于改进型D^*动态避障路径规划的方法。

主权项

1.一种改进型D*的机械臂动态避障路径规划方法：依据长方体包络的相交测试思想，获得了快速并较为精确的碰撞检测算法；依据空间机械臂特性，获得启发式函数搜索路径规划算法；依据改进型D*动态搜索算法，获得机械臂动态避障路径规划算法：其中，快速碰撞检测算法至少包括：依据简单的长方体包络，进行空间机械臂和环境的包络；依据包络长方体特征，将其进行重叠式旋转变换，使其形成的包络区域近似为圆柱体或球体；依据长方体间位姿关系，进行长方体之间的碰撞检测，完成快速碰撞检测过程；长方体之间的碰撞检测包括：通过旋转将长方体近似等价为圆柱体或球体的过程为，以长方体中心为原点，到各表面中心为坐标轴建立空间直角坐标系，以长方体构建圆柱体过程为例，将包络的长方体绕坐标轴按照设置角度旋转，以旋转过程中产生的多个长方体所构成的叠加图形近似代替圆柱体，同理，绕三个坐标轴旋转近似代替球体；简单长方体的碰撞检测过程，利用如下公式进行长方体中心距离检测，若满足则返回碰撞，否则，继续；其中，Dist(G_C1,G_C2)为表示长方体一G_C1与长方体二G_C2之间的距离，||O_cO_c'||表示长方体G_C1中心O_c与长方体G_C2中心O_c'之间距离，s₁、s₂、s₃、s₁'、s₂'、s₃'为起点为长方体中心坐标，终点为三个互不相对面中心的方向向量，d_S为碰撞检测的安全裕度；第二步：判断长方体一的顶点是否在长方体二内，此即点‑长方体碰撞检测，若有1个顶点碰撞则返回碰撞，否则继续判断下一个顶点，若都不碰撞则进入第三步；第三步：判断长方体一的边是否与长方体二碰撞，此即线段‑长方体碰撞检测，若有1条边碰撞则返回碰撞，否则继续判断下一条边，若都不碰撞则返回不碰撞。