[发明专利]一种基于局部搜索算法的机械臂自主避障路径生成方法

权利要求书

1.一种基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，包括如下步骤：

步骤S1，初始化障碍物相关信息，根据初始化后的信息生成障碍物的边界框；

步骤S2，将当前关节角度一个邻域内的关节点离散化；

步骤S3，从当前角度出发，依据预测的关节角度变化律获得下一步的搜索路径点；判断从当前关节角度出发能否获得可行路径点，如果能够获得可行路径点，则从可行路径点中筛选最优路径点，进而判断是否到达终点；如果不能获得可行路径点则进行路径回溯，返回前一个路径点重新进入S2向前搜索；

步骤S4，如果判断到达终点，则将所有最优路径点平滑连接起来，生成最终的平滑路径，如果判断没有到达终点，则前进一步返回步骤S2，搜索下一个路径点。

2.如权利要求1所述的基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，在所述步骤S1，所述初始化障碍物相关信息，包括：初始化路径起始点、终点、角度极限、障碍物边界信息。

3.如权利要求1所述的基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，所述生成障碍物的边界框，包括：选择由圆形和矩形元素描述障碍物的边界框；其中圆形由球心O_i(xo_i,yo_i,zo_i)和半径ro_i组成，矩形由长方体的各个顶点(xc_i,yc_i,zc_i)表示，障碍物的边界框由上述元素组成的圆柱形，球形和多面体进行描述。

4.如权利要求1所述的基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，在所述步骤S2中，

首先，每个关节角度满足约束：

其中，q_i,分别表示第i个关节的关节角度，关节角度上限和下限；m表示关节总个数；

式子(1)表示每个关节角度都必须处于其限定范围内，是机械臂处于任何姿态，均需要遵守的固有约束；

依据机械臂的具体状态，对其关节角做出了进一步的约束：

其中，q_i,c,分别表示第i个关节第c步关节角的角度，角度上限和下限，c表示从起始开始的步数序号；

r_i,c分别表示第i个关节初始关节角度上限，第c-1步的关节角度上限，关节角极限变化范围；

分别表示第i个关节初始关节角度下限，第c-1步的关节角度下限；

确定离散关节点时的离散步长：

其中，n_s表示离散关节点时划分的步数，dq_i,c表示第i个关节角第c步的离散步长。

5.如权利要求1所述的基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，在所述步骤S3中，

步骤S31，每次搜索只选择距离当前点位一个离散步长的离散点位作为目标路径点，每次搜索时每个关节角度可能的结果为：

其中，表示第i个关节角第c步可能的搜索结果，q_i,c-1表示第i个关节角第c-1步的最优路径点；

步骤S32，对于上式中可能的搜索结果，确定搜索规则，寻找最优路径点。

6.如权利要求5所述的基于局部搜索算法的机械臂自主避障路径生成方法，其特征在于，所述确定搜索规则，包括：

式子(6)可以进一步地表示为：

其中，sn_i,c为符号标志位，其可能的值为-1，0，1，其中i表示关节角序号，c表示步数序号；

此时，每次路径点的搜索过程转化为sn_i,c的遍历过程，采用基于预测关节角度变化律的方法确定sn_i,c的遍历准则，使得能够优先遍历到最优路径点，加快搜索速度。