基于优化A制造技术

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

Based on optimized A

The invention relates to a three dimensional obstacle avoidance path planning method of artificial potential field manipulator based on optimized A*, including the following steps: establishing a three-dimensional obstacle avoidance environment and initialization of the manipulator; avoiding the local minimum of the artificial potential field method with A*, and improving the efficiency of the minimum estimate cost of the A* search by the minimum two fork heap. The path planning of three dimensional obstacle avoidance for the manipulator is carried out, and the inverse kinematics of the planned path is obtained, and the joint angles of the manipulator are obtained, thus a path of the end of the manipulator is planned to avoid the obstacles. The minimum two fork sequencing method is used to sort the OPEN table in the artificial potential field path search algorithm of A* by the least two fork heap. It can quickly search the nodes with the least estimated cost, thus can quickly find the obstacle avoidance path with the minimum estimate cost, and improve the efficiency of the algorithm. The least squares method is used to smooth the obstacle avoidance path, so that the obstacle avoidance path is smoothed and the motion jitter of the manipulator is avoided.

【技术实现步骤摘要】
基于优化A*的人工势场机械臂三维避障路径规划方法
本专利技术涉及一种机械臂避障路径规划技术，特别涉及一种基于二叉堆优化A*的人工势场机械臂三维避障路径规划方法。
技术介绍
机械臂避障路径规划指的是，在给定环境与障碍条件，起始点位姿和目标点位姿的情况下，要规划一条从起始点至目标点的路径，使得机械臂能无碰撞的规划一条路径。二维环境中有移动机器人的避障路径规划，三维环境中有机械臂的避障路径规划。传统的路径规划的方法有模拟退火算法、人工势场法等，较好的路径搜索算法有fallback算法、Floyd算法、Dijkstra算法、A*算法等。人工势场法的算法结构简单，运算速度快，但易于陷入局部极小值点，A*是一种全局最优的规划算法，可以有效地避免人工势场法陷入局部极小值点。但是，在三维空间环境下搜索路径过程中，每次都从A*的OPEN表中取出估计代价最小的节点用于下一次计算，存在运算速度慢的问题。
技术实现思路
本专利技术是针对三维环境中有机械臂的避障路径规划存在的问题，提出了一种基于优化A*的人工势场机械臂三维避障路径规划方法，利用最小二叉堆排序方法对A*的人工势场法搜索路径算法中的OPEN表进行最小二叉堆排序，能够更加快速搜索出估计代价最小的节点，从而能快速寻找最小估计代价的避障路径，提高算法的效率。本专利技术的技术方案为：一种基于优化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的机器人工具箱RoboticsToolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。所述步骤第二步具体包括如下步骤：S1：创建空的OPEN表和CLOSED表，定义步长stepL；S2：将起始点START插入OPEN表，计算启发函数f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,对于起始点START，其中实际代价g(n)＝0，估计代价本文使用的估计代价函数h(n)为机械臂末端点位置(xn、yn、zn)与目标点(xd、yd、zd)之间的欧氏距离,接着把起始点STRAT插入CLOSED表，记当前节点为i＝START；S3：对当前节点i进行碰撞检测，若发生碰撞，规划失败，，停止规划；若未发生碰撞，则执行下一步S4；S4：检测当前节点i对应的是否是目标节点，若不是目标节点，则执行步骤S5；若是目标节点，则执行步骤S9；S5：使用人工势场法计算目标点对机械臂末端的斥力、引力、合力，再根据合力计算下一节点i+1的受力情况，其中当前节点i的实际代价g(n)为上一节点的实际代价加上步长stepL；S6：判断当前节点i是否是极小值点，及当前节点i机械臂所受合力是否为零，若不是极小值点，则执行下一步S7；若是极小值点，则执行IV，IV具体步骤包括：SS1：对当前节点i进行上、下、左、右、前、后共6个方向同步长stepL扩展；SS2：通过扩展得到后继节点next，计算后继节点NEXT的代价f(next)；SS3：判断后继节点next是否在OPEN表中，若在，则执行SS4；若不在，则执行SS5；SS4：把f(next)与当前节点i对应的最小根节点的f(n)进行比较，使用最小二叉堆排序更新OPEN表中的根节点或叫最小父节点f(n)；SS5：判断后继节点next是否在CLOSED表中，若在，则执行SS6；若不在，则把后继节点next插入OPEN表中；SS6：判断OPEN表是否为空，若不为空，则执行SS7；若为空，则结束规划；SS7：用最小二叉堆法对OPEN表进行排序，更新最小根节点f(n),把更新后的最小根节点作为避障规划的下一节点i+1，此时避障规划跳出了局部极小值点；SS8：判断节点i+1处机械臂是否发生碰撞，若发生碰撞，则选取次最小节点的f(n)作为下一节点i+1，并转到SS7重新判断；若不发生碰撞，则执行SS9；SS9：把下一节点i+1插入CLOSED表，并使用最小二叉堆法对该CLOSED表排序，赋值i＝i+1，即把下一节点i+1作为当前节点i，更新符合要求的下一节点i+1为当前节点i；SS10：判断更新后的节点i是否在障碍物影响的范围之内，若是，则转到IV重新进行扩展；若不是，则转到II重新判断更新后的当前节点i是否是目标节点；S7：把由人工势场法计算的下一节点i+1插入OPEN表与CLOSED表，并使用最小二叉堆对插入新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点f(n)最小，同理对CLOSED表使用最小二叉堆排序更新，再把当前节点i保存为父节点，保存使用最小二叉堆排序更新后的OPEN表和CLOSED表；S8：将下一节点i+1当作当前节点i，转到步骤S3进行碰撞检测；S9：当前节点i是目标节点，记目标节点为END，则说明机械臂寻找到避障路径，从END反向回寻至START，得到规划的避障路径；S10：使用最小二乘法，对规划得到的避障路径进行拟合，使该路径更加平滑；S11：规划结束。本专利技术的有益效果在于：本专利技术基于优化A*的人工势场机械臂三维避障路径规划方法，最小二叉堆排序提高了A*的人工势场法搜索路径的寻找速度，使用最小二本文档来自技高网...

【技术保护点】
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组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。...

【技术特征摘要】
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的机器人工具箱RoboticsToolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。2.根据权利要求1所述基于优化A*的人工势场机械臂三维避障路径规划方法，其特征在于，所述步骤第二步具体包括如下步骤：S1：创建空的OPEN表和CLOSED表，定义步长stepL；S2：将起始点START插入OPEN表，计算启发函数f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,对于起始点START，其中实际代价g(n)＝0，估计代价本文使用的估计代价函数h(n)为机械臂末端点位置(xn、yn、zn)与...

【专利技术属性】
技术研发人员：李玉齐，林森阳，鲍海锋，王玉林，王博，肖洒，
申请(专利权)人：上海理工大学，
类型：发明
国别省市：上海,31