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

申请/专利权人：长春工业大学

摘要：本发明公开了一种改进APF‑RRT算法的机器人避障路径规划方法，在已知起始位置，目标位置和障碍物环境信息的条件下，利用改进APF‑RRT算法，引入障碍物数目参数和算法切换策略避免传统APF算法无法到达目标点和陷入局部最小值，引入双边最近节点选择策略、节点两次拓展策略、圆锥体子集约束采样和目标引导结合的混合采样策略，加快改进RRT算法的收敛效率，采用去除冗余节点和3次非均匀有理B样条曲线平滑策略对避障路径进行裁剪和平滑；提出的改进APF‑RRT算法减少了路径长度，提升了算法效率，可以很好地适用于机器人避障路径规划问题。

主权项：1.一种改进APF-RRT算法的机器人避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物环境信息的条件下，利用改进APF-RRT算法，引入障碍物数目参数和算法切换策略避免传统APF算法无法到达目标点和陷入局部最小值，引入双边最近节点选择策略、节点二次扩展策略、圆锥体子集约束采样和目标引导结合的混合采样策略，加快改进RRT算法的收敛效率，采用去除冗余点和3次非均匀有理B样条曲线平滑策略对避障路径进行裁剪和平滑，减少路径长度，满足机器人运动要求；包括以下步骤：步骤1：获取工作空间和障碍物环境信息，设置机器人在关节空间中的起始位置qinit、目标位置qgoal、步长L、路径节点集S；步骤2：计算当前节点与障碍物之间的距离；如果当前节点与最近的障碍物之间的距离大于步长的两倍，则执行步骤3；如果当前节点与最近的障碍物之间的距离小于步长的两倍，则执行步骤4；步骤3：引入障碍物数目作为势场参数，采用修改后的人工势场函数使qgoal对当前节点qnearest产生吸引力，使障碍物对qnearest生成排斥力，qnearest按照给定的步长L沿着两个力的合力方向生成新的节点qnew；使用改进后的APF算法在目标点和障碍物的联合作用下进行搜索和前进，1起始位置qinit为初始节点，计算路径点受到的吸引力和排斥力，根据合力方向生成距离当前节点步长L的新节点；2确定新节点所受的吸引力和排斥力是否相等且方向相反，如果相等且方向相反，证明新节点陷入局部最小值，执行步骤4，使用改进后的RRT算法生成下一节点，直到有节点跳出局部最小值，将该节点添加到路径节点集S中，并在该节点处重新执行步骤3进行路径搜索；如果吸引力和排斥力不是相等且方向相反，则将新节点保存到路径节点集S中，继续执行步骤；3计算节点与最近障碍物的距离，如果节点与最近障碍物之间的距离小于步长的两倍，则跳转到步骤4，否则，继续执行步骤；4判断当前节点与目标位置qgoal距离是否小于步长L，如果节点与目标位置qgoal之间的距离小于步长L，则将目标位置qgoal添加到路径节点集S中，并结束算法，保存路径节点集S；否则，重新执行步骤3，继续路径搜索；步骤4：使用改进的RRT算法进行路径搜索；1初始化随机搜索树，获取起始位置qinit、目标位置qgoal、步长L、路径节点集S，起始位置qinit作为随机搜索树的根节点，此时树中只有一个节点qinit；2利用均匀采样法，在区间[0，1]中随机生成概率Prand，设定P＝0.5，如果随机概率Prand小于目标偏置率P，将采样点qrand选择为目标位置qgoal；如果大于等于目标偏置率P，则在工作空间中的随机生成采样点qrand；3使用双边最近节点选择策略选取最近节点，在节点qnearest指向采样点qrand的方向进行步长L展开，得到一个新的节点qnew，进行碰撞检测，判断新节点与最近节点的连线是否与障碍物碰撞，如果发生碰撞，舍弃该新节点qnew，重新执行2；如果没有碰撞，则将新节点qnew添加到随机搜索树中，并将新节点qnew的父节点分配给最近节点qnearest；在生成的新节点qnew1上，利用节点二次扩展策略生成新节点qnew2，生成新节点qnew2后进行碰撞检测，如果qnew1和qnew2的连线与障碍物发生碰撞，舍弃新节点qnew2，继续执行4，否则，如果qnew1和qnew2的连线与障碍物没有碰撞，则将新节点qnew2添加到随机搜索树中，并将节点qnew1设定为新节点qnew2的父节点，继续执行4；4当随机搜索树中的节点与目标位置qgoal的距离小于步长L时，结束算法，将目标位置qgoal添加到随机搜索树中并将随机搜索树中的节点保存到路径节点集S中；当随机搜索树中的节点与最近障碍物的距离大于步长的两倍时，跳转到步骤3，执行改进后的APF算法，否则，跳转到2，继续进行路径搜索；步骤5：改进APF-RRT算法执行完毕后，将路径节点集S中的节点进行连接，去除冗余节点，得到处理后的路径Snew；步骤6：使用3次非均匀有理B样条曲线对路径Snew进行平滑，最终得到机器人的避障路径；引入障碍物数目作为势场参数，采用修改后的人工势场函数使qgoal对当前节点qnearest产生吸引力，使障碍物对qnearest生成排斥力，qnearest按照给定的步长L沿着两个力的合力方向生成新的节点qnew；改进的人工势场包括引力场函数Uattq如式1所示、斥力场函数Urepq如式2所示，则合力势场Usum函数如式3所示： Usum＝Uattq+ΣUrepq3其中，ε为引力势场的系数，σ为斥力势场的系数，dq，qgoal表示当前位置与目标位置的欧氏距离，dq，qobs表示当前位置与障碍物中心位置的欧氏距离，d0为障碍物影响半径，n为障碍物数目；引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式4和5，合力Fsum如式6所示，斥力的分力Frep1和Frep2如式7和8所示： Fsum＝Fattq+ΣFrepq6 其中，▽dq，qgoal表示dq，qgoal的导数，Frep1方向由障碍物指向当前位置，Frep2方向由当前位置指向目标点；当目标点附近存在障碍物时，机器人受到的斥力较大，引力较小，存在无法到达目标点的现象，通过改进的斥力场函数，机器人在靠近障碍物时，受到的斥力随着距离越近变得越小，避免产生目标不可达问题；确定新节点所受的吸引力和排斥力是否相等且方向相反，如果相等且方向相反，证明新节点陷入局部最小值，使用改进后的RRT算法生成下一节点，直到有节点跳出局部最小值，将该节点添加到路径节点集中，并在该节点处重新使用改进APF算法进行路径搜索；利用均匀采样法，在区间[0，1]中随机生成概率Prand，如果随机概率Prand小于目标偏置率PP＝0.5，将采样点qrand选择为目标位置qgoal；如果大于等于目标偏置率P，则在工作空间中的随机生成采样点qrand，采样点qrand的选取公式如式9所示； 使用双边最近节点选择策略，具体过程为：当采样点不是目标位置qgoal时，在工作空间内随机选取采样点，将随机采样点与随机搜索树中的节点连线作为一条边，随机采样点与目标点的连线作为另一条边，计算双边距离之和作为选取最近节点的代价函数，代价函数如式10所示； λ和μ分别是比例因子，||qrand–q||表示随机采样点与随机搜索树上节点的欧式距离，||q-qgoal||表示随机采样点与目标点的欧氏距离，应用黄金比例分割系数，可以得出λ：μ＝1.618，最近节点的选取为：qnearest＝minWq；在新节点扩展时采用节点二次扩展策略，具体过程为：当新节点qnew1采样成功后，扩展下一个新节点qnew2时，采用了圆锥体子集约束采样和目标引导结合的混合采样策略，即将第一次节点扩展得到的新节点qnew1指向目标位置qgoal的方向作为圆锥体子集高的方向，圆锥体子集的底面半径等于算法中的步长L，圆锥体子集的母线长度也等于步长L，圆锥体子集的顶点为节点qnew1，二次扩展的新节点qnew2在圆锥子集底面边线中随机选取；生成新节点qnew2后进行碰撞检测，如果qnew1和qnew2的连线与障碍物发生碰撞，则放弃新节点qnew2，如果没有碰撞，则将新节点qnew2添加到随机搜索树中，并将节点qnew1设定为新节点qnew2的父节点；使用改进APF-RRT算法得到机器人避障路径后，去除路径中的冗余节点，具体过程为：从起始节点qinit开始，连接下一个节点的子节点，即第三个节点，将连接的路径进行碰撞检测，如果路径没有与障碍物发生碰撞，则删掉中间节点，即第二个节点；如果发生碰撞，则保留中间节点并将第三个节点看做初始节点qinit，重复上述步骤，直到路径到达最后一个路径点qend；使用3次非均匀有理B样条曲线对去除冗余节点路径进行平滑，最终得到机器人的避障路径。

全文数据：

权利要求：

百度查询： 长春工业大学 一种改进APF-RRT算法的机器人避障路径规划方法