[发明专利]基于优化A*的人工势场机械臂三维避障路径规划方法

摘要

本发明涉及一种基于优化A*的人工势场机械臂三维避障路径规划方法包括以下步骤：建立三维避障环境及机械臂初始化；用A*避免人工势场法陷入局部极小值的同时，通过最小二叉堆排序提高A*搜索最小估计代价的效率，以此进行机械臂三维避障路径规划；对规划出的路径求运动学逆解，得到机械臂各关节角，由此规划一条避开障碍物的机械臂末端路径。利用最小二叉堆排序方法对A*的人工势场法路径搜索算法中的OPEN表进行最小二叉堆排序，能够更加快速搜索出估计代价最小的节点，从而能快速寻找最小估计代价的避障路径，提高算法的效率。使用最小二乘法拟合对避障路径做平滑处理，使所得避障路径平滑，避免机械臂运动抖动。

主权项

1.一种基于优化A*的人工势场机械臂三维避障路径规划方法，其特征在于，第一步：在三维空间环境中建立障碍物的模型，使用球体包络障碍物，并对机械臂末端执行器的起始点位置，目标点位置，每个关节的初始关节角均进行初始化；第二步：使用最小二叉堆优化A*的人工势场法来规划机械臂的三维避障路径，定义有序数组存储节点信息，即定义OPEN表用来存储已生成而待考察的节点，CLOSED表用来存储访问过的节点，使用最小二叉堆对OPEN表排序，寻找最小估计代价节点避障路径，根据每一步判断节点的情况，把符合要求的节点存入OPEN表，每运行完该节点后，再把该节点从OPEN表存入CLOSED表，每个节点里面存储的是位置信息，并使用最小二叉堆对插入新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点对应的启发函数f(n)最小，f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,机械臂末端每一走一步均需计算该启发函数f(n)的值，所得f(n)最小的节点就是机械臂末端下一步到达的位置；第三步：判断第二步中规划出来的机械臂末端三维避障路径是否存在运动学逆解：求逆解是通过机械臂末端的坐标点位置信息确定机械臂各连杆关节角，确定机械臂是否存在可运行的避障路径，若存在逆运动学解，则执行第四步；若求逆运动学解失败，说明机械臂达到奇异限位或所求解的路径点超出机械臂末端执行器的工作空间，则返回第二步重新规划路径；第四步：借助MATLAB的机器人工具箱Robotics Toolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。