[发明专利]基于人工势场法的工业机器人安全避障方法

权利要求书

1.一种基于人工势场法的工业机器人安全避障方法，其特征在于，包括：

采用改进的人工势场法，避免工业机器人在运动过程中出现目标不可达现象，在含有障碍物的工作环境中为所述工业机器人规划出一条从起始点到目标点的安全路径，并判断工业机器人是否陷入局部极小点，

若工业机器人陷入局部极小点，则通过RRT算法添加虚拟目标点，使工业机器人逃离局部极小点，并继续向目标点运动。

2.如权利要求1所述的基于人工势场法的工业机器人安全避障方法，其特征在于，采用改进的人工势场法，避免工业机器人在运动过程中出现目标不可达现象，在含有障碍物的工作环境中为所述工业机器人规划出一条从起始点到目标点的安全路径，并判断工业机器人是否陷入局部极小点，包括：

步骤1：选取关节空间用以搜索路径，确定所述工业机器人的起始位姿和最终目标点位姿，通过传感器搜寻障碍物的位置并构建相应包围盒；

步骤2：依据当前关节角组合θ搜索相邻的关节角组合，所述相邻的关节角组合取(θ-λ，θ，θ+λ)中的一种情况，其中，λ为搜索步长；

步骤3：求解所有关节角组合对应的工业机器人的关节点与末端点位置；

步骤4：舍弃工业机器人与障碍物发生碰撞的各对应的关节角组合，选取工业机器人未发生碰撞的各对应的关节角组合，并计算未发生碰撞时工业机器人的各连杆与障碍物的最短距离，以及工业机器人的末端点和目标点之间的距离；

步骤5-1：所述改进的人工势场法中，目标点对于工业机器人的引力势函数如下：

式中，k_a为引力势函数系数，X为工业机器人的末端点位置，X_g为目标点位置，k_θ为关节角距离系数，θ_i为工业机器人当前各关节角度，θ_i+1为工业机器人下一状态各关节角度，θ_i-1为工业机器人上一状态各关节角度；

步骤5-2：所述改进的人工势场法中，工业机器人的各连杆所受斥力势函数如下：

式中，k_r为斥力势函数系数，d_n为工业机器人的各个连杆和障碍物之间的最短距离，d₀是障碍物斥力的影响范围，d_safe是障碍物安全距离，当机器人与障碍物的距离小于d_safe时，直接认为斥力势场无穷大；

步骤5-3：所述工业机器人所受总的斥力势函数为：

式中，N表示工业机器人的连杆数量；

步骤5-4：所述述工业机器人所受的总势能为：

U＝U_att+U_rep；

步骤6：计算步骤4中选取的各关节角组合对应的总势能，选出其中总势能最小的关节角组合及其对应的工业机器人末端位置；

步骤7：判断步骤6中选出工业机器人的末端位置是否为目标点位置，如果是，工业机器人运动到目标点位置；若不是，进入步骤8进行判段；

步骤8：判断总势能最小的关节角组合是否为工业机器人当前的关节角组合，若不是，工业机器人运动到该关节角组合对应的位置，然后转至步骤2重新计算，直至工业机器人的末端运动到目标点位置，即根据总势能下降的方向规划出一条无碰撞工作路径；若是，工业机器人则陷入了局部极小点。

3.如权利要求1所述的基于人工势场法的工业机器人安全避障方法，其特征在于，若工业机器人陷入局部极小点，则通过RRT算法添加虚拟目标点，使工业机器人逃离局部极小点，并继续向目标点运动，包括：

步骤81：对随机树T_init进行扩展，将局部极小点设置为X_init，作为本次扩展的根节点；

步骤82：在工业机器人工作空间中避开障碍物，随机确定一点X_rand，并添加参数μ表示点X_rand与点X_g重合的概率，点X_g表示目标点位置；

步骤83：遍历随机树中的每一个节点，找出距离X_rand最近的节点X_near，定义步长L，找出X_near向X_near与X_rand连线方向扩展L的新节点X_new；

步骤84：判断节点X_near与新节点X_new的连线段是否与障碍物碰撞，若是，则舍弃并返回步骤1，若不是，新节点X_new加入随机树；

步骤85：更新随机树T_init，以新节点X_new作为下一次扩展的根节点，迭代N次；

步骤86：迭代结束，找出随机树中除根节点X_init以外距离障碍物最远的一个节点，设置为人工势场法的虚拟目标点，工业机器人跳出局部极小点，返回所述步骤2，直至最终到达真正目标点位置，完成避障与路径规划。