[发明专利]一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法

权利要求书

1.一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法，其特征在于，该方法包括以下步骤：

步骤一：根据目标点的位置，利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态；

步骤二：以最优目标状态为目标节点，在关节空间构建搜索树，规划一条无碰路径；

步骤三：从步骤二规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第k个节点更新为根节点；

步骤四：根据障碍物的位置变化，实时更新环境地图，并在新的根节点附近，自定义更新时间和更新半径，对搜索树重新布线；

步骤五：若关节空间上的目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状态作为新的目标节点。

2.根据权利要求1所述的方法，其特征在于，步骤一中利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态，具体如下：

首先定义评价函数用于评价机械臂所处状态的优劣，其值越小表示机械臂所处状态越优；

然后对该评价函数进行优化求解可求得唯一θ值；

其中θ为自运动变量，当机械臂末端位姿确定且不考虑翻肩现象时，给定θ值便可唯一确定机械臂状态，n为障碍物个数，F_i(θ)为第i个障碍物对机械臂产生的排斥力，F_i(θ)计算公式如下：

其中，D_i(θ)为第i个障碍物与机械臂的最近距离，ρ为障碍物排斥势场作用范围，α为形变因子。

3.根据权利要求1所述的方法，其特征在于，步骤二中以最优目标状态为目标节点构建搜索树，规划一条无碰路径，具体如下：

设置当前机械臂所处状态q_init＝[q₁，q₂，…，q₇]^T为搜索树根节点q_root，求解得到的最优目标状态q_best为目标节点q_goal，其中q₁，q₂，…，q₇分别为机械臂的7个关节角；

然后在机械臂关节角空间中随机采样一个点x_rand，连接搜索树中与该随机点距离最近的节点x_nearest，然后在x_rand与x_nearest的连线上以一定的步长Δt截取得到新的节点x_new；判断节点x_nearest与x_new的连线是否与障碍物碰撞，如果碰撞则重新采样，否则遍历x_new的近邻点集X_near，找到使离线代价函数cost(x_new)最小且连线不与障碍物碰撞的节点作为x_new的父节点，将x_new添加到搜索树中；

当达到最大设定迭代次数后结束搜索树的构建，从目标节点的近邻点集中选择代价函数cost(q_goal)最小的节点，向上回溯到根节点，得到一条无碰路径(x₀，x₁，…，x_goal)；

其中，

(1)x_rand按如下公式计算：

Uniform(X_goal)＝f(θ_r)θ_r∈[0，2π]

其中，Uniform(·)表示均匀采样，P_r∈[0，1]表示随机数，α为自定义常数，X_goal表示目标解空间，X_free表示机械臂无障碍空间，θ_r为在[0，2π]范围内的随机数，f(θ_r)表示给定θ_r角后求解机械臂每个关节角的过程；

(2)x_new按如下公式计算：

其中，x_nearest为搜索树中离x_rand距离最近点，Δt为自定义步长；

(3)X_near按如下公式计算：

X_near＝{x_i∈X_tree：||x_i-x_new||＜D_near}

其中，X_tree表示搜索树中的节点集合，x_i表示搜索树中的第i个节点，D_near表示自定义近邻距离；

(4)cost(x_i)按如下公式计算：

其中，x_parent表示节点x_i的父节点，root表示根节点的序号。