本发明属于机器人及遥操作技术领域，基于七自由度拟人机械臂验证优化的a*路径规划规划，涉及一种基于优化的机器人a*避障路径规划方法。
背景技术：
随着空间探索的深入,空间在轨服务和维修越来越加频繁,其中空间机械臂在空间站的建设和维护中显得尤其重要，空间机械臂应用技术已经成为空间技术的重要研究方向。空间机械臂代替宇航员完成空间作业任务，如组装与搭建空间站、释放与回收卫星、维护空间设备以及完成空间科学试验等，大大减小了宇航员舱外作业的风险，因此空间机械臂应用技术受到国内外专家的高度重视。在微重力环境下，空间机械臂系统处于自由漂浮状态，使得机械臂控制变量与非独立变量之间存在强烈的运动耦合，运动控制难度加大，从而空间机械臂的路径规划变得特别复杂此外，由于空间环境中的空间碎片，空间舱体外设试验装置等都有可能成为空间机械臂在轨操作过程中的障碍，因此为了顺利完成在轨操作任务，开展空间机械臂避障路径规划研究十分重要，但空间机械臂设计具有工作空间大、运动灵活、使用寿命长、可靠性和控制精度要求高等要求，本专利基于此背景提出了一种改进的长方体包络法结合圆柱包络法进行碰撞检测,此方法增加了碰撞检测的计算速度,提高了路径规划的实时性，同时提出一种优化的a*算法可以有效增加机械臂末端运动轨迹的平滑性，不但减少机械臂关节角速度变化的频率，而且可以减少机械臂关节运动停止与启动次数，有效降低机械臂的磨损，延长了机械臂使用寿命；最后在unity与visualstudio2010平台上进行仿真研究,验证了该方法的有效性和实用价值。
技术实现要素：
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种基于优化的机器人a*避障路径规划方法
技术方案
一种基于优化的机器人a*避障路径规划方
法，其特征在于步骤如下：
步骤1、采用改进a*路径规划算法计算出路径：
a1：首先采用栅格法对环境进行建模，分成n×n×n个边长为适当长度的立方体，然后以估价函数为每一个立方体赋值f(n)＝g(n)+h(n)，并计算f(n)、g(n)、h(n)值；所述环境为起始点至目标节点的一个立体空间；所述g(n)为起始点至当前节点的欧式距离；所述h(n)当前节点至目标节点的欧式距离；
a2：将起始点加入到open链表中，并把起始点当作当前节点；
a3：将立方体中当前节点的26临域加入open链表中，并把当前节点作为其父节点；
在open集中选f(n)值最小的节点作为当前节点；
并且将当前节点添加到close链表中，且在open链表中移除；
若终点添加到open链表中即结束搜索，否则转下一步骤；
a4：若当前节点的26临域不在open链表中时，将当前节点的26临域加入open链表中，并计算f(n)＝g(n)+h(n)中的f(n)、g(n)、h(n)值；若在open链表中，重新计算g(n)值，若比先前g(n)值小，则更新g(n)值和父节点，重复步骤a3；
将搜索过程中的当前节点设为搜索路径，作为pi集合
步骤2、采用优化算法进行路径优化：
设pi集合为原始的a*算法搜索到的路径点，po为优化后的路径节点，表达式如下：
优化过程如下：
b1：把
b2：连接
b3：若
b4：当n＝a-1时，优化结束，若b＜a则优化节点序列为
有益效果
本发明提出的一种基于优化的机器人a*避障路径规划方法，为降低空间在轨服务机械臂长期运行而造成磨损，提出一种优化的a*路径规划，因为在工程中机械臂末段每经过一个节点机械臂需要停止和启动一次，这样加大了对机械臂的磨损，本方法提到的方法可以减少搜索路径中的节点，增加机械臂末端路径的平滑性，减少机械臂停止与开始的频率，该方法映射到关节空间，减少关节速度变化的频率，减少关节的磨损，增加机械臂的使用寿命。同时其中还提出一种改进的长方体包络障碍物碰撞检测方法和圆柱型包络机械臂连杆自身碰撞检测方法，该方法减少计算复杂度，提高计算效率，减少碰撞检测时间，优先提高碰撞检测的实时性。
综上该方法与现有的技术相比有如下优点：
(1)增加机械臂末端路径的平滑性；
(2)减少路径中的节点，在工程中有效减少机械臂开始和停止的次数，降低了机械臂的磨损程度；
(3)该方法搜索出来的路径映射到关节空间时，减少关节速度变化的频率。有效降低关节的磨损，延长机械臂使用寿命；
(4)提到的碰撞检测策略，降低了计算复杂度，改善了碰撞检测效率。
附图说明
图1：碰撞检测简化模型
图2：机械臂运动过程中的一帧图片
具体实施方式
现结合实施例、附图对本发明作进一步描述：
具体实现步骤如下：
1、首先用传统的a*路径规划算法计算出路径：
(1)首先栅格法对环境进行建模，分成n×n×n个边长为适当长度的立方体，然后估价函数为每一个立方体赋值，代价函数为f(n)＝g(n)+h(n)；并计算f(n)、g(n)、h(n)值；所述环境为起始点至目标节点的一个立体空间；所述g(n)为起始点至当前节点的欧式距离；所述h(n)当前节点至目标节点的欧式距离；
(2)将起始点加入到open链表中，并把起始点当作当前节点；
(3)把当前节点的26临域加入open链表中，并把当前节点作为其父节点，在open集中选f值最小的节点作为当前节点；
并且将当前节点添加到close链表中，且在open链表中移除；
若终点添加到open链表中，则就结束搜索，否则转到c；
(4)c当前节点的26临域不在open链表中时，加入open链表中，并重新计算f、g、h值，若在open链表中，重新计算g值，若比先前g值小，则更新g值和父节点，重复步骤(3)。
将搜索过程中的当前节点设为搜索路径，作为pi集合
2、采用优化算法进行路径优化
设pi集合为原始的a*算法搜索到的路径点，po为优化后的路径节点，表达式如下所示：
优化过程如下所示：
(1)把
(2)连接
(3)若
(4)当n＝a-1时，优化结束，若b＜a则优化节点序列为
所述障碍物碰撞方法：
根据搜索出来的路径(路径为末端位置，末端姿态设置为[0,0,0],)即末端位姿根据逆解求出关节角，并用本专利提出的碰撞检测法选择出一组不发生碰撞的逆解；碰撞检测方法如下：
(1)首先要进行机械臂模型简化，把每个连杆简化成一条线段，然后把七个连杆中最大的半径长度叠加到障碍物的厚度上，如图1所示
(2)设关节i与关节i+1形成空间线段的两个端点坐标分别为p1(x1,y1,z1)与p2(x2,y2,z2)，面abcd的法向量为n(nx,ny,nz)，空间线段向量为h(x2-x1,y2-y1,z2-z1)，点abcd的坐标分别为a(x1,y1,z1)、b(x2,y2,z2)、c(x3,y3,z3)、d(x4,y4,z4)，且e(x5,y5,z5)(1)若n·h＝0，且线段中的任意一点没有在平面中，则线段与平面平行，没有交点。
(3)若nh≠0，则线段有可能与平面相交，如满足交点在线段上且交点在平面内，设交点为p(x0,y0,z0)，即满足
min(a(z1),b(z2))≤z0≤max(a(z1),b(z2))
min(a(x1),c(x3))≤x0≤max(a(x1),c(x3))
min(a(y1),e(y5))≤y0≤max(a(y1),e(y5))
则空间线段与空间平面相交；同理判断空间线段与其他五个面之间的关系。
4、仿真验证
本文以如下表所示的机械臂参数为模型进行仿真验证，机械臂末端以[1.5m,2.5m,-1.0m,0,0,0]为起点以[2.5m,0.5m,2.0m,0,0,0]为终点仿真验证，其中障碍物有三处，中心坐标分别为[2.5m,2.5m,0.5m],[3.5m,0.5m,1.0m],[1.5m,1.5m,1.5m],大小都为1m×1m×1m立方体，下面将给出机械臂末端在路径中一点[1.75m,0.75m,1.0m,0,0,0]处的32种逆解机械臂模型，同时给出路径算法优化前后从起点到终点的机械臂末端路径与各个关节角的变化。机械臂d-h参数如下表1所示：
表1仿真机械臂d-h参数
图2为仿真验证结果，其中灰色先线为优化算法搜索的路径，黑色为传统a*算法搜索到的路径。
从图2分析，优化算法搜索出来的路径更加平滑，验证优化算法的有效性和实用性。