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

申请/专利权人：电子科技大学

摘要：本发明公开了一种基于合作博弈的多目标优化RRT*机器人路径规划方法，首先初始化机器人环境信息，设置起始点、目标点与算法参数，然后在机器人关节空间中选取采样点并生成新节点，对新节点进行碰撞检测，再选择通过碰撞检测的新节点附近已有节点计算优化指标，定义多目标优化问题求解帕累托面，在帕累托面中选择均衡节点，并将新节点加入搜索路径树并更新均衡节点为新节点的父节点，最后判断新节点是否接近目标点，若新节点接近目标节点一定范围内则路径规划成功，若重复次数达到最大迭代次数则算法失败。本发明的方法有效改善了路径质量，避免了机器人运动过程中的奇异性问题，并减小了机器人运行过程中的能量损耗。

主权项：1.一种基于合作博弈的多目标优化RRT*机器人路径规划方法，具体步骤如下：S1、初始化环境信息，设置起始点、目标点与算法参数；初始化机器人环境信息，规定机器人起始节点qstart，目标节点qgoal，设定算法参数，包括：规定步长step，重选范围r，截止阈值domain，最大迭代次数iterMax，路径搜索树Tree；S2、在机器人关节空间中选取采样点，并生成新节点；在机器人关节空间中选取采样点qsample，且采样点将有一定概率直接选为目标点qgoal；在路径搜索树Tree中搜寻离qsample距离最近的已有节点qnear，在qnear连接至qsample的方向上从qnear处生成步长step的新节点qnew，若qnear与qsample之间的距离小于step，则直接取qsample为新节点qnew，则新节点生成表达式如下： 其中，qrand表示关节空间中的随机节点，p表示0到1之间的随机概率；S3、对步骤S2得到的新节点进行碰撞检测，若发送碰撞则回到步骤S2，若未发送碰撞则进入步骤S4；对新节点qnew进行碰撞检测，多关节串联机器人由多个连杆组成，各连杆与障碍物进行碰撞检测；将连杆以胶囊体包络，将障碍物以球体与胶囊体包络，对机器人建立正向运动学，获取不同节点下各连杆空间坐标信息，表达式如下： 其中，n表示机器人关节数，表示机器人末端位姿相对于世界坐标系的变换矩阵，px,py,pz表示机器人末端三维坐标在世界坐标系下的表示，nx,ny,nz、ox,oy,oz和ax,ay,az表示末端坐标系三个轴在世界坐标系下的单位方向向量，表示连杆1相对于世界坐标系的变换矩阵，表示连杆2相对于连杆1的变换矩阵，表示连杆n相对于连杆n-1的变换矩阵；障碍物碰撞检测分为胶囊体与球体的距离检测以及胶囊体与胶囊体的距离检测两种情况，这两种情况分别转换为点与线段之间的距离及线段与线段之间的距离；若该距离小于连杆半径与障碍物半径之和，则视为节点与障碍物碰撞，删除新节点qnew并回到步骤S2；否则将新节点视为有效节点并加入路径搜索树Tree，进入步骤S4；S4、选择新节点附近已有节点，并计算附近节点的优化指标，然后定义多目标优化问题求解帕累托面，在帕累托面中选择均衡节点，最后将新节点加入搜索路径树并更新均衡节点为新节点的父节点；S41、选择新节点附近已有节点；在路径搜索树Tree中选择新节点qnew在r范围内的已有节点，即在路径搜索树Tree搜寻满足||qnew-qnear||≤r的所有节点并放入节点组qnearArray；S42、计算附近节点的优化指标；其中，所述优化指标包括：路径总长度、机器人可操作性和能量消耗；定义qcount表示qnearArray中任意节点，计算新节点qnew到qnearArray中任意节点之间的三个优化指标，具体如下：1计算路径总长度；计算qnew和qcount之间的距离及qcount到起始点qstart的距离，表达式如下：Clengthqnew＝||qnew-qcount||+||qcount-qcount.parent||+…+||qcount.parent.….parent-qstart||3其中，Clengthqnew表示qnew到qstart的路径总长度指标，qcount.parent表示qcount的父节点，qcount.parent.….parent表示qcount在节点树中通过父节点及父节点的父节点不断连接直到离起始点qstart最近的节点；2计算机器人可操作性；计算机器人雅可比矩阵Jq，其表示关节速度与笛卡尔空间末端速度之间的映射关系，雅可比的条件数可有效表示机器人的可操作性，其计算表达式如下： 其中，KJ表示雅可比条件数，||·||Fro表示矩阵的Frobenius范数，q表示机器人关节节点，σmin和σmax分别表示雅可比矩阵的最大和最小奇异值，J+表示雅可比矩阵的伪逆；当σmin等于零时，条件数是无限的，使用条件数的倒数代替条件数，条件数倒数KRecq的界限在0和1之间，定义其为机器人可操作性指标，表达式如下： 计算qnew和qcount之间的可操作性指标以及qcount到起始点qstart的可操作性指标，表达式如下：CJcnqnew＝-KRecqnew-KRecqcount-KRecqcount.parent-…-KRecqstart6其中，CJcnqnew表示qnew到qstart的可操作性总和；3计算能量消耗；使用关节的最大输入扭矩作为衡量机器人能量消耗的指标，对机器人能量消耗引入标准，表达式如下： 其中，Wq0,qf表示机器人从节点q0运动到节点qf能量消耗，τiq表示机器人第i个关节的扭矩；计算qnew和qcount之间的能量消耗以及qcount到起始点qstart的能量消耗，表达式如下：Cenergyqnew＝Wqnew,qcount+Wqcount,qcount.parent+…+Wqcount.parent.….parent,qstart8其中，Cenergyqnew表示表示qnew到qstart的能量消耗总和，Wqnew,qcount表示机器人从节点qcount运动到节点qnew能量消耗，qcount.parent.….parent表示qcount在节点树中通过父节点及父节点的父节点不断连接直到离起始点qstart最近的节点；S43、定义多目标优化问题求解帕累托面，并采用合作博弈理论从帕累托面中选择均衡节点作为新节点的父节点qnew.parent；基于步骤S42计算的优化指标对qnearArray中所有节点进行多目标优化获得非支配解的帕累托面，即将Clength、CJcn和Cenergy作为多目标优化对象，定义多目标优化问题表达式如下：minCqnew,qcountqcount∈qnearArray9其中，Cqnew,qcount＝[Clength,CJcn,Cenergy]表示优化指标向量；对该多目标优化问题进行求解得到一组帕累托解，且qnew只能选择一个父节点，则在帕累托解中采用合作博弈的方式选择一个均衡节点qNash作为最优解；在帕累托解中分别取Clength、CJcn和Cenergy的最大值组成均衡参考点CN，在帕累托解中离CN最远的一组解即为均衡节点qNash；将qNash作为新节点qnew的父节点qnew.parent并更新路径搜索树Tree；S5、判断步骤S4得到的新节点是否接近目标点，若是则生成运动路径，算法结束，若否，则判断迭代次数是否超过最大值，若未超过，则重复步骤S2-S4，直到新节点接近目标点，若迭代次数超过最大值，则路径规划失败；其中，当最新生成的节点满足||qnew-qgoal||≤domain，即qnew接近目标点qgoal，连接两点后路径规划算法得到起始点到目标点的运动路径，算法结束；若重复次数超过最大迭代次数iterMax，则路径规划失败。

全文数据：

权利要求：

百度查询： 电子科技大学 基于合作博弈的多目标优化RRT*机器人路径规划方法