李明，赵金龙，袁逸萍，晁永生
（新疆大学机械工程学院，新疆 乌鲁木齐 830047）
近年来，不同类型的机械臂广泛的应用于各个行业。然而，随着复杂作业、智能作业的要求不断提高，机械臂面临的工作环境越来越复杂，与工作环境中的障碍物碰撞的可能性大大增加。因此，机械臂避障路径规划成为研究热点：（1）机械臂避障路径规划是机械臂按时完成操作的前提；
（2）避障路径规划能提高机械臂的安全性。针对机械臂避障路径规划，大量学者做了相关研究。文献[1]为提升机械臂在障碍环境下运动时的安全性，先使用A*算法规划一条避障路径，在考虑平稳性的基础上，使用2次B样条平滑避障路径。文献[2]提出了一种基于改进A*算法的变步长搜索方法。文献[3]将障碍物转化到机械臂关节空间，并利用A*算法完成六自由度机械臂关节空间无碰撞路径规划。文献[4]提出了一种改进A*算法，在缩短路径长度的同时，提高了路径规划的成功率。文献[5]针对双臂机器人避障路径规划问题，提出一种改进快速扩展随机树算法，应用于协同避障路径规划。文献[6]提出一种自适应步长RRT 方法，通过建立构型空间与工作空间的范数相容不等式，实现有效的碰撞检测，完成双机器人协同路径规划。文献[7]针对荔枝采摘机器人的避障问题，提出了一种改进的RRT算法。文献[8]提出一种适用于装配任务系统的改进快速扩展随机树算法。文献[9]提出一种将高斯分布用于RRT算法的方法。文献[10]针对RRT算法搜索效率低的问题，利用高斯分布采样的方式提出一种GB_RRT算法。
上述学者针对机械臂在障碍环境避障的问题，应用和改进A*算法、RRT算法、人工势场法等算法完成了避障路径的规划，但在改进RRT算法的过程中忽略了该算法在避障路径规划过程中会出现耗时长、路径长度不是最优、且转折点多的问题。针对这些问题，首先建立障碍物模型，其次针对RRT算法在三维避障路径规划过程中会出现耗时长的问题，应用双向RRT算法完成避障路径规划，降低路径规划耗时。针对RRT算法规划的避障路径存在路径长、且转折点多的问题，使用A*算法选取关键节点，并建立三次B样条插值函数，完成对避障路径的平滑处理，并缩短避障路径的长度。
2.1 障碍模型
在实际的工作场景中，障碍物的形式以及形状难以确定。因此，为避免机械臂本体及各连杆在避障时与障碍物发生碰撞，利用便于计算的空间规则体近似包络障碍物，虽然形成的规则体障碍物体积较大，但是使用空间规则模型提高了碰撞检测及搜索路径的效率，同时也降低了末端执行器在实际运行过程中与障碍物发生碰撞的可能性。障碍物可以采用长方体包络，如图1所示。将障碍物近似包络为长方体的方法，简化了距离的计算，提升路径规划效率。
图1 长方体包络的障碍物Fig.1 Cuboid Envelope Obstacle
2.2 机械臂模型
以满足Pieper准则的六自由度机械臂为研究对象，其三个相邻关节轴交于一点或三轴线平行。根据结构特点，将机械臂各杆件使用圆柱体包络，关节使用球形包络。六自由度机械臂结构简图，如图2所示。
图2 机械臂模型Fig.2 The Model of Manipulator
3.1 双向RRT算法
快速扩展搜索树（RRT）算法是Steven M.LaValle 于1998 年首次提出的。该算法通过采样点在二维或三维障碍环境进行碰撞检测，能在高维空间、复杂约束的条件下解决路径规划问题。基本的RRT算法的搜索过程，如图3所示。
图3 基本RRT算法原理图Fig.3 Basic RRT Algorithm Schematic
设qinit为随机树的起始点，qgoal随机树的目标点，qrand为每次迭代产生的随机点，qnear为扩展时离qrand最近的节点，设每次扩展的步长为s，当qnear与qrand之间的距离大于s时，按照步长s产生新节点qnew，之后需要对新产生的节点进行碰撞检测，如果发现未发生碰撞，保留新节点加入到随机树，反之舍去。
如果qnear与qrand之间的距离小于s时，取qrand等于qnew，在进行碰撞检测，如果未发生碰撞，保留新节点加入到随机树，反之舍去。直到随机树新产生的节点qnew与qgoal小于设置的阈值，随机树停止生长。
针对基本RRT算法搜索效率低，搜索时间长的问题，采用双向RRT算法来降低搜索耗时。双向RRT算法的主要思想是两颗随机树Ta和Tb同时从起始点和目标点开始扩展，假设Ta从qinit开始扩展，Tb从qgoal开始扩展直到Ta和Tb新产生的点之间的距离小于设定的阈值时，随机树停止生长。双向RRT算法的搜索过程，如图4所示。
图4 双向RRT算法原理Fig.4 Principle of RRT−Connect Algorithm
3.2 A＊算法
A*算法是一种典型的启发式方法，可在平面或空间障碍环境中完成避障路径的搜索，是在障碍信息已知的条件下较为有效的路径搜索方法。
f(n)函数是避障路径搜索时的评价指标，每进行一次路径搜索都需计算所有子节点的f(n)值，并将下一个父节点定义为子节点中估价值最小的点，直至搜索到目标位置。A*算法在路径规划过程中只需按照f(n)函数值选取子节点，解决了需要遍历空间点的难题。A*算法公式如下：
式中：f(n)—实际代价与预估代价的和；
g(n)—起始位置到节点n之间的实际代价；
h(n)—节点xn到目标位置的预估代价。
节点xn到终点xd的预估代价值会影响A*算法的路径搜索速度，如果计算节点xn到终点xd的预估代价较小，会导致A*算法在搜索过程中形成的子节点数量过多，降低计算速度和搜索效率。若h(n)计算的预估代价值与节点xn到终点xd实际耗费的代价相等，则A*算法搜索时不会形成额外的子节点，搜索效率也会相应的提高。预估代价函数h(n)为从节点xn到终点xd之间的欧式距离，即：
RRT−A*融合算法的搜索流程，如图5 所示。先使用双向RRT算法完成在障碍环境下避障路径的规划，再利用A*算法选取该避障路径上的关键节点，完成对避障路径的优化。
图5 RRT−A*融合算法规划避障路径流程图Fig.5 Flow Chart of Obstacle Avoidance Path Planning by RRT−A* Fusion Algorithm
机械臂在避障过程中，为避免机械臂在起始、终止以及避障路径转折点出现急加速的情况，就要保证避障路径的平滑性。
首先取出A*算法选取的RRT算法规划的避障路径上的关键节点，并在关键节点之间使用三次B样条函数平滑优化。
对于空间位置—时间序{θi，ti}，i=0，1，…，n其一般形式如下：
式中：Qi（i=0，1，…，n）—控制顶点；
Ni，k(t)（i=0，1，…，n）—是k次规范B样条基函数，且有：
式中：k—B样条函数的阶数；
i—B样条函数的序列号，记0/0=0。
通过障碍空间中对应的避障位置—时间序列点，将其作为型值点去反算控制点，并建立三次B样条插值函数，保证平滑后的避障轨迹经过A*算法选取的避障路径上的关键节点，其第i段曲线的函数表达式为：
仿真试验的空间机械臂是以工业上最常用的PUMA560 型机器人为研究对象，其D−H参数，如表1所示。利用Matlab中机器人工具箱建立的模型，如图6所示。
表1 机械臂D-H参数Tab.1 D-H Parameters of Manipulator
图6 PUM560机器人模型Fig.6 The Model of PUM560 Robot
避障路径规划仿真实验是在长宽高为100cm的环境中进行的，双向RRT算法和RRT−A*算法规划的避障路径对比结果，如图7 所示。折线为双向RRT 算法规划的避障路径，路径长度为168cm，有16个转折点，虽然双向RRT算法和传统RRT算法相比可有效的提高搜索速度，但不能解决路径长度较长，转折点多的问题。因此，使用A*算法对避障路径的节点进行选取，A*算法对该避障路径选取的关键节点构成的避障路径，如图7虚线所示。经过A*算法对避障路径节点的选取，路径长度缩短为136cm，但路径仍然有3个转折点。故使用三次B样条函数完成进一步的避障路径优化，经过三次B样条平滑后的避障路径，如图8所示。从图中可以看出，经过三次B样条曲线平滑后，生成了一条平滑的避障路径，从而可提升机械臂在避障运动过程中的位移、速度、加速度曲线的平滑性。
图7 双向RRT算法和RRT−A*算法规划避障路径对比Fig.7 The Comparison of RRT−connect Algorithm and RRT−A* Algo⁃rithm Planning Obstacle Avoidance Path
图8 三次B样条平滑避障路径Fig.8 Obstacle Avoidance Path Smoothed by Cubic B−Spline
通过对机械臂避障运动过程中的位移、速度、加速度的计算求解，得到其位移、速度、加速度变化图，如图9所示。从图中可以看出，机械臂在避障运动过程中位移、速度、加速度曲线均平滑变化、且速度、加速度无突变，这保证了机械臂在运动过程中的稳定性和准确性，同时可提高机械臂的安全性以及完成避障运动的速度。
图9 三次B样条平滑路径各关节位移、速度、加速度曲线Fig.9 Displacement，Velocity，Acceleration Curve of Each Joint of Cubic B−Spline Smooth Path
针对RRT算法在三维避障路径规划过程中会出现耗时长、路径长度不是最优、且转折点多的问题，提出并实现了一种基于RRT−A*融合算法和三次B样条的机械臂避障路径规划方法。首先建立空间障碍物三维模型，在构建的障碍环境中应用双向RRT算法规划避障路径，降低搜索避障路径的耗时。然后使用A*算法选取该避障路径的关键节点，建立三次B样条插值函数，生成平滑的机械臂避障路径，解决路径长度不是最优且转折点多的问题。从仿真结果可以看出，通过该方法可规划出平滑的避障路径且保证机械臂避障时位移、速度、加速度曲线的平滑性，验证了该方法的有效性。
猜你喜欢样条障碍物双向双向度的成长与自我实现出版人(2022年11期)2022-11-15一元五次B样条拟插值研究安徽师范大学学报（自然科学版）(2022年3期)2022-07-14高低翻越动漫界·幼教365(中班)(2020年3期)2020-04-20SelTrac®CBTC系统中非通信障碍物的设计和处理铁道通信信号(2020年9期)2020-02-06三次参数样条在机床高速高精加工中的应用制造技术与机床(2017年7期)2018-01-19三次样条和二次删除相辅助的WASD神经网络与日本人口预测软件(2017年6期)2017-09-23基于样条函数的高精度电子秤设计计算机测量与控制(2017年6期)2017-07-01一种软开关的交错并联Buck/Boost双向DC/DC变换器通信电源技术(2016年5期)2016-03-22一种工作频率可变的双向DC-DC变换器电源技术(2015年9期)2015-06-05基于双向预测的图像去噪河南科技(2014年19期)2014-02-27