[发明专利]一种机械臂避障路径规划方法

权利要求书

1.一种机械臂避障路径规划方法，其特征在于，包括如下步骤：

步骤一：对机械臂模型以及障碍物进行简化，确定机械臂关节点，采用虚拟关节点插补对相邻关节间的连杆进行插补；在每个关节点处采用球形包围盒进行包络以代替机械臂模型，使得机械臂处于任何姿态使均能完整替代用以距离计算；

步骤二：采用眼在手外的布置方式放置深度相机，实时获取工作空间的机械臂以及障碍物点云信息，并采用机器人实时过滤包将机械臂本体的点云信息进行滤除，仅保留障碍物点云信息；

步骤三：在机械臂运动前使用RRT-Connect算法对机械臂所处的静态环境进行路径规划，依托其完备性获得一条可行的全局规划路径；

步骤四：控制机械臂沿全局规划路径运动，并实时获取机械臂所在空间的障碍物点云信息，在机械臂坐标下对障碍物点云与机械臂关节点进行距离计算，以获得最短距离；

步骤五：通过改进人工势场法进行局部路径规划实现避障；实时确定机械臂末端与目标点的距离与位置向量，根据机械臂末端与目标点的距离与位置向量确定目标点对机械臂末端的吸引力大小；

所述目标点为控制机械臂到达的目标位置；所述实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引力大小的函数公式为：

其中，ρ(q,q_goal)为机械臂末端与目标点之间的欧氏距离；U_att为改进后的引力场函数，ξ'为引力的动态增益系数，其表达方式为其中动态增益系数与机械臂到目标点之间距离成反比，起始点的增益系数最小，从而使引力大小得以调节，防止引力过大使机械臂和障碍物之间发生碰撞，保证机械臂获得一条安全且平滑的路径；

实时确定机械臂关节点与障碍物点云之间的最短距离，当所述最短距离大于设定安全距离时，障碍物对机械臂的排斥力为0，而当最短距离小于设定安全距离时，根据机械臂上的关节点和最近障碍物点确定排斥力的大小，所述排斥力大小的函数公式为：

式中U_rep为障碍物产生的斥力势场；ρ₀为安全距离半径；ρ_i(q,q_obst)为机械臂末端与第i个障碍物之间的欧氏距离；

F_rep1i为斥力分量，方向由障碍物指向机械臂末端，大小为：

F_rep2i为引力分量，方向是机械臂末端指向目标点，大小为：

当机械臂使用局部规划器规划路径时，由所述吸引力和排斥力产生的合力引导机械臂运动；

步骤六：对机械臂末端引入一个指引力，该指引力在机械臂陷入局部最小值时使其运动进入下一位置进行重新规划；

所述指引力的函数公式为：

进入局部极小值时的合力的函数公式：

其中s为障碍物个数；a为指引力系数；x的取值范围为[1,10]；a的取值随障碍物个数以及机械臂与障碍物之间的距离变化而变化，当障碍物个数为1，若此时机械臂与障碍物的距离大于安全距离的1/2，则a的取值为1，反之则a取值为0.5；当障碍物个数大于1时，则采用单调递增函数作为指引函数对a进行赋值，直到机械臂脱离局部最小值点，若下一位置为依旧陷入局部最小值，则重复以上方法；

步骤七：在局部路径规划中引入预规划机制；

步骤八：在局部路径规划中加入跟随机制。

2.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述RRT-Connect算法是基于随机采样方法对随机采样点进行连接以形成一条可行路径，其连接过程类似树的生长，因此称为随机树，该算法通过从起始点与目标点同时生成两棵随机树进行交汇获得可行路径以提升其路径规划速度；

为避免采样时过于随机，提升路径的方向性，RRT-Connect算法还采用交替方式进行随机树的生长。