[发明专利]一种快速渐进最优的机械臂避障路径规划方法

说明书

一种快速渐进最优的机械臂避障路径规划方法，获取工作空间障碍物环境信息，建立机械臂碰撞模型和运动学模型，确定关节空间形式的起始状态和目标状态；通过双树相向生长的策略快速获的初始路径；然后建立启发式超椭球采样子空间，并在该子空间内进行采样，双树生长，路径优化；路径收缩算法进一步优化了已有路径，也加快压缩了超椭球空间；多次采样生长优化配合一次更新最短路径的策略也使得搜索树可以充分探索空间和优化路径，避免进行过多无用的路径收缩。本发明融合多种启发式思想，对RRT路径规划算法的快速性和最优性进行了平衡，能够快速渐进最优的获得一条无碰撞路径。

技术领域

本发明涉及机械臂运动规划领域，具体而言涉及高维机械臂在静态障碍物环境中快速寻找一条最短的可行无碰撞路径。

背景技术

机械臂路径规划是机器人研究领域的基本问题，即给定起始状态和目标状态，求出一条可行无碰撞的路径θ(s)(0≤s≤1)，其中θ(0)表示起始状态，θ(1)表示目标状态，无碰撞指的是对于路径上的任意状态，机械臂自身之间以及机械臂和周围环境之间都没有发生碰撞。

传统移动机器人的路径规划算法如人工势场法、A*等都需要在配置空间显示表达障碍物，然而机械臂路径规划中维数高，这些算法的计算量将显著增加，严重降低算法的成功率和实时性。针对该问题，学者们提出了基于采样的规划方法，如PRM(概率路标法)、RRT(快速扩展随机树)。该类算法在状态空间采样，通过碰撞检测判断采样点的可行性，从而避免了对环境约束的显示表达，在高位配置空间表现出了相对高效的路径规划能力。

RRT(rapidly-exploring random tree)算法在状态空间随机采样，将未发生碰撞的可行采样点按照一定的扩展方式加入随机探索树中，随着采样的进行，整个状态空间会被充分的探索，随机树也一定会生长到目标状态或目标状态的附近，从而形成一条从起始状态(树的根节点)到目标状态的可行无碰撞路径，因此，RRT算法是一种概率完备的算法。

各种启发思想的加入，可以改善RRT算法的规划效果。RRT-Conect从起始状态和目标状态分别生成一颗随机树，两棵树相遇则形成可行路径，大大提高了规划的速度，但路径随机性很大，往往不是最优；RRT*不断调整新生成节点与其附近节点的父子关系，逐步降低路径代价，实现路径的渐进优化，但规划时间很长；informed-RRT*让RRT*算法在一个启发式超椭球子空间内探索采样，提高了RRT*的渐进优化速度，但获取初始路径仍然慢，超椭球空间收缩较慢；可以看出，基于快速随机搜索树的算法仍然存在快速性和最优性难以平衡，优化速度较慢的问题。

发明内容

针对机械臂路径规划快速性和最优性难以平衡的问题，本发明提供一种快速渐进最优的机械臂避障路径规划方法，对RRT算法进行变种，能够快速渐进优化的寻找到一条机械臂无碰撞运动路径。

本发明的快速渐进最优的机械臂避障路径规划方法，具体包括以下步骤：

步骤1：获取工作空间障碍物环境信息，建立机械臂碰撞模型和运动学模型，确定关节空间形式的起始状态和目标状态；

建立包含碰撞模型的机械臂空间运动学模型，获取环境中的所有机械臂工作空间内的障碍物位置和大小信息，为算法中的碰撞检测提供碰撞信息；

步骤2：获取初始路径；

确定起始状态s_init和目标状态s_goal，并以关节空间中的形式(q₁,q₂,...,q_n)表示，n代表机械臂的自由度；如果已经是关节空间形式，则进入下一步；如果是以笛卡尔空间形式表示，则通过串联机械臂逆运动学和碰撞检测求出一组无碰撞的关节角，设置为状态点；如果无法求出无碰撞的关节空间起始或目标状态，则需重新设置起始状态或者目标状态，直到求出无碰撞的状态为止；生成初始路径：