六自由度机械臂避障路径规划方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113799141A(43)申请公布日2021.12.17(21)申请号202111200867.8(22)申请日2021.10.14(71)申请人福州大学地址350108福建省福州市闽侯县福州大学城乌龙江北大道2号福州大学(72)发明人陈志勇黄泽麟曾德财(74)专利代理机构福州元创专利商标代理有限公司35100代理人丘鸿超蔡学俊(51)Int.Cl.B25J9/16(2006.01)权利要求书3页说明书9页附图4页(54)发明名称六自由度机械臂避障路径规划方法(57)摘要本发明提出一种六自由度机械臂避障路径规划方法，在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，加快算法收敛于目标位置，采用动态步长，根据节点扩展情况来评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境，并采用势力场修正策略，加快算法远离起始点，收敛目标位置，有效提高了算法避障路径规划的实时性，最后用重选父节点优化路径节点，减少路径长度。前述设计的路径规划算法可以很好的适用于六自由度机械臂路径规划问题，在不同的障碍物环境中都可以完成路径规划任务。CN113799141ACN113799141A权利要求书1/3页1.一种六自由度机械臂避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，以加快算法收敛于目标位置；并依据势力场修正策略，调整节点扩展位置，加快算法远离起始点，收敛目标位置；采用动态步长，根据节点扩展情况评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境；最后采用重选父节点优化路径节点，减少路径长度。2.根据权利要求1所述的六自由度机械臂避障路径规划方法，其特征在于，包括以下步骤：步骤S1：设六自由度机械臂末端操作器在系统基坐标系O‑xyz下的位置矢量为P＝(x,Ty,z)；其末端操作器连体坐标系Oe‑xeyeze的姿态通过基坐标系O‑xyz进行Z‑Y‑X欧拉角q＝(γ,β,α)T旋转操作确定，即以基坐标系O‑xyz为起始坐标系，先绕基坐标系O‑xyz的z轴旋转α角，获得坐标系O'‑x'y'z'；再绕旋转后的坐标系O'‑x'y'z'的y'轴旋转β角，获得坐标系O”‑x”y”z”；最后绕坐标系O”‑x”y”z”的x”轴旋转γ角，最终确定出机械臂末端操作器的连体坐标系Oe‑xeyeze；由此，将机械臂末端操作器位姿相对于系统基坐标系O‑xyz的齐次变换矩阵描述为：其中，为末端操作器连体坐标系Oe‑xeyeze姿态相对于基坐标系O‑xyz的旋转变换矩阵，O＝(0,0,0)为零向量，为机械臂连杆i连体坐标系相对于连杆i‑1连体坐标系的齐次变换矩阵，θi表示机械臂连杆i相对于连杆i‑1的关节转角；利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θi(i＝1,2,…,6)；利用符号表示机械臂在位置*时的系统节点；其中，表示机械臂各连杆在位置*时所对应的关节角；将系统起始位置节点用符号Qinit表示，目标位置节点用Qgoal表示，相应的以机械臂关节角行程作为系统的路径代价度量；在此基础上，获取包括尺寸、形状、起始位置Qinit、目标位置Qgoal的机械臂信息及包括障碍物的尺寸、形状及位置的机械臂工作环境信息；步骤S2：以机械臂起始位置Qinit作为根节点构建起始搜索树StartT，以机械臂目标位置Qgoal作为根节点构建目标搜索树GoalT；所述起始搜索树是一个以Qinit为根节点且可以不断扩展的节点集motionS；所述目标搜索树是一个以Qgoal为根节点且可以不断扩展的节点集motionG；起始搜索树的扩展步长StartT_step和目标搜索树的扩展步长GoalT_step初始时都设为相同的步长值init_step；2CN113799141A权利要求书2/3页步骤S3：采用目标偏置和metropolis接受准则在工作空间筛选随机采样点Qrand；设rand为在[0,1]内随机生成的数值，Pgoal为目标偏置概率；若rand＜Pgoal，则直接将目标点Qgoal作为随机采样点Qrand；若rand≥Pgoal，则采用metropolis接受准则来筛选随机采样点Qrand，即若采样点满足P＝1，则接受此采样点，继续执行步骤S4；若P＝0，则此采样点不被算法接受，重新进行随机采样，直至采样点满足P＝1的情况；其中，metropolis接受准则如下：其中，Qnearest表示起始搜索树StartT中距离采样点Qrand最近