买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工业大学

摘要：一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，涉及冗余机械臂碰撞回避和路径规划技术领域，针对现有技术中在机械臂路径规划时，并不能兼顾回避复杂障碍物的碰撞和冗余机械臂的自碰撞，进而导致规划的路径会发生机械臂碰撞的问题，本申请舍弃了实时规划的需求，通过对关节空间之间旋转矩阵的合理利用，不仅增加了碰撞障碍物的类型，使其可以进行稍复杂障碍物的碰撞检测，同时也实现了运动过程中自碰撞的检测，得以兼顾，进而避免了机械臂的碰撞。

主权项：1.一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，其特征在于包括以下步骤：步骤一：确定机械臂运动路径中的关节起点Pstart和终点Pgoal，所述起点Pstart和终点Pgoal为机械臂各个关节的角度；其中，Pstart和Pgoal都是长度为n的向量，n是冗余机械臂的关节数，n不小于7；步骤二：确定机械臂工作环境中的障碍物，并将障碍物包络为规则形状，之后将包络后的障碍物表示在机械臂基坐标系下，所述规则形状包括圆柱体、球体、圆台、立方体；步骤三：确定机械臂关节和臂杆的尺寸参数，以及机械臂上会产生自碰撞的部件，进而得到碰撞点集PSETcc；步骤四：在关节和臂杆各自所在的关节空间中表示所需进行自碰撞检测的点，进而得到自碰撞点集PSETcsc，自碰撞是指在运动过程中臂杆和关节与自碰撞部件产生碰撞；步骤五：运用正运动学将PSETcc表示在机械臂基坐标系下，运用正运动学将PSETcsc表示在自碰撞的障碍物所在的关节坐标系中，并通过快速拓展随机树RRT算法得到关节空间的路径；所述步骤五中通过快速拓展随机树RRT算法得到关节空间的路径的具体步骤为：步骤五一：构建路径随机树矩阵RRT，并将机械臂运动路径的起点Pstart表示为[Pstart0]，之后将[Pstart0]输入RRT中，其中，RRT为m行n+1列，其中m为RRT中n维向量的个数，RRT中最后一列的元素为索引号；步骤五二：以RRT中随机一行形成一个点Xbranch，并将Xbranch作为起点，其中Xbranch为1行n列的向量，之后设定概率，并通过设定的概率生成一个指向随机的n维生长向量或生成一个朝向Pgoal的n维生长向量，生成一个指向随机的n维生长向量的概率和生成一个朝向Pgoal的n维生长向量的概率之和为1，最后根据Pgoal并结合Xbranch生成生长向量的末端点Xrand，Xrand表示为： 其中，Xrand为即将生成的随机点，rand1，n为生成1行n列的大小在0-1之间的随机数，Xbranch为RRT中的随机一点，P为设定的概率；步骤五三：在RRT中寻找距离Xrand最近的点，并将距离Xrand最近的点作为新路径点的父节点Xnear，在RRT中寻找距离Xrand最近的点具体表示为： 其中，Dis为RRT中点距离Xrand的n维欧氏距离，i为RRT的行索引，将距离最近的父节点Xnear的行号记录为inear；步骤五四：以Xnear、Xrand和Step确定一个新的点Xnew，Xnew表示为：Xnew＝Xnear+Step*GrowUintGrowUint＝Xrand-Xnear|Xrand-Xnear|其中，GrowUint为以Xnear为起点，Xrand为终点的单位向量；步骤五五：以Xnew为当前各关节角度进行障碍物碰撞检测；步骤五六：以Xnew为当前各关节角度进行自碰撞检测：步骤五七：若碰撞检测和自碰撞检测并未发生碰撞，则将Xnew放入RRT中，若碰撞检测及自碰撞检测发生碰撞，则重复步骤五二到五六，直至生成无碰撞的Xnew； 步骤五八：计算Xnew与Pgoal的欧式距离，并判断得到的欧氏距离是否小于阈值Threshold，若欧式距离小于阈值Threshold，则执行步骤五九，否则，重复步骤五二到步骤五七，所述阈值Threshold用于判别是否到达终点；步骤五九：根据得到的RRT，使用最后放入的Xnew进行回溯，回溯出RRf中的完整路径，作为本次路径规划的结果path，其中，path为未知行数，列数为n的矩阵。

全文数据：

权利要求：

百度查询： 哈尔滨工业大学 一种关节空间规划的冗余机械臂碰撞回避及路径规划方法