[发明专利]基于改进的天牛须搜索算法的机械臂避障路径规划方法

权利要求书

1.基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，包括以下步骤：

建立姿态规划结果的优化评估模型；包括用于优化机械臂关节角度的函数，其表达式为：，式中，为机械臂关节经过N次搜索迭代后的角度差之和；变量表示在路径点i上采样时刻t处的关节角度；M表示路径规划过程中所有路径点的总数；为包含6个元素的向量，其中每个元素是实数；

用于优化运行路径长度的函数，其表达式为：式中，表示机械臂末端执行器在空间中的移动距离，变量表示在路径点i上采样时刻t处的机械手端点位置；k表示路径中包含的路径点个数；为包含3个元素的向量，其中每个元素是实数；

获取机械臂末端起点和终点位姿，利用逆运动学解析法求解起点关节角和终点关节角，确定机械臂在空间中的位置；

建立机械臂和障碍物碰撞检测模型；

根据碰撞检测结果，结合优化评估模型，利用改进后的BAS4-AO算法生成机械臂避障路径，包括以下步骤：

（1）、初始化各项参数；

（2）、生成两个相互正交的路径点搜索方向，其中搜索方向的计算公式如下：

；

和是两个比例常数，为均匀分布于上的随机值；为随机单位向量，为目标点相对于当前路径点的方向向量；表示从左到右的方向向量；表示从下到上的方向向量；

计算搜索方向之前，需要将方向向量归一化，公式如下：

；

其中，为具有目标点处关节信息的目标导向部分，为三维坐标转换为关节角的逆运动学方程；其中原始随机方向仍然保留，常数向量表示目标点的坐标；

（3）、路径点空间位置生成

进行第t时刻迭代时，围绕空间坐标P的四个判断点的空间位置坐标为：

；

其中，搜索步长是一个动态调整因子，随着搜索迭代循环衰减；

然后，生成下一次迭代的位姿信息：；

其中，作为四个判断点的适应度评价标准，符号函数是用于提取四个判断点中适应度函数值最小的点；矩阵为目标点的旋转矩阵，为当前点的旋转矩阵，以作为匹配系数，作为到的逼近步长；

（4）、碰撞检测及搜索步长调整

进行机械臂轴体与障碍物的碰撞检验，若无碰撞，则直接转入步骤（2）执行下一次迭代循环；若发生碰撞，则须衰减全局搜索步长，清除此次路径点信息并再次搜索新的路径点；

（5）、进行关节角度寻优判断

根据机械臂末端执行器空间坐标求解机械臂关节角度，判断当前机械臂末端执行器关节角度和目标点关节角度之间的角度差是否小于阈值，若不小于阈值，则返回步骤（2）继续生成路径点；若小于阈值，则转入步骤（6）；

（6）关节角度局部变步长搜索：

;

;

其中，作为关节角度的适应度评价标准；

局部变步长的更新如下所示：；

其中，为最小搜索步长，m为该局部衰减迭代的最大迭代次数；

（7）、路径点精度判断

判断当前路径点与目标路径点的空间位置距离，若该距离不小于精度阈值，则返回步骤（6），若小于精度阈值，则进入步骤（8）；

（8）、路径修剪

计算每个路径点各关节角度变化下的模量，确定最小关节变化所在的点，具体如下式所示：

；

其中，表示在对规划路径点进行正序或逆序计算过程中，与当前点关节角度最接近的规划路径中的第i个点；

将当前计算的路径点和具有最小关节变化的点进行标注，对标注点之间的路径点进行平滑处理，并使用当前最小关节变化的点的关节角替换标注点之间路径点的关节角。

2.根据权利要求1所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，算法完成的时间优化函数表达式为：

；

其中，为算法优化的起始点，为机械手操作完成的时间点；搜索迭代次数，为采样区间。

3.根据权利要求1所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，碰撞检测模型中，使用凸包简化模型作为机械臂模型，使用轴体包络模型作为障碍物模型。