李 扬，张 蕾，李鹏飞，王晓华，王文杰

(西安工程大学 电子信息学院，陕西 西安 710600)

伴随着全球工业与智能化制造的发展趋势以及“中国制造2025”国家行动纲领的实施，我国机器人技术正面临着从低端制造向智能化制造发展的阶段。工业机械臂作为智能机器人中的执行终端,逐渐应用于各工业领域，由于我国社会老龄化问题日益严重，将会导致未来中国劳动生产力逐步减少，必须提高平均劳动生产力，防止GDP增速减缓，而机械臂是代替人力的有效手段，因此对机械臂运动规划的研究愈发重要[1]。在机械臂系统执行任务的工作区内通常会有一些物体，它们对机械臂的运动形成了阻碍空间。传统示教路径的方式在避障等操作上会增加很多负担，因此引入智能避障路径规划算法，节省作业时间的同时，也能够避免磨损,具有提高生产质量的现实意义[2]。机械臂的运动规划是以碰撞检测为基础并规划出一条从初始位姿到最终位姿的路径，使机械臂能沿着这条路径无碰撞地完成工作任务[3]。

运动规划问题是机械臂规划过程中的基础。传统的运动规划方法主要分为两大类：基于搜索的运动规划方法以及基于采样的运动规划方法。基于搜索的运动规划方法多针对二维空间中机器人的运动规划问题，通常使用A*、Dijkstra等算法，虽然其搜索能力强但当目标为机械臂且其关节数增多、自由度较高时，这些算法复杂度会大幅提升，即并不适用于高维空间下的运动规划[4]，而基于采样的运动规划方法则巧妙地解决了这些弊端。

基于采样的运动规划方法主要针对高维空间下的机械臂运动规划问题。1988年，LAVALLE[5]提出一种基于采样的运动规划方法即快速拓展随机树(Rapidly exploring Random Tree，RRT)算法。此类算法具有概率完整性、快速的探索速度，且无需将障碍物从任务空间映射到配置空间的优点[6]。同时，传统的RRT算法也存在一些不足，例如在随机树搜索过程中的收敛速度较低，导向性不稳定，在复杂环境中寻迹速度慢且不必要节点过多以及障碍物约束使其随机采样生成的路径包含许多不必要的断点，从而导致路径不平滑且不连续等[7-9]。因此，机械臂的运动规划通常是不稳定的。

为了解决上述问题，本文提出一种融合目标偏置策略以及自适应步长策略的RRT(P-Adaptive Variable Step size—RRT, PAVS-RRT)算法，通过目标偏置策略在采样过程中引导算法以一定概率向目标点快速拓展，实现尽快抵达目标点的任务以减少无用区域搜索[10]，然后通过自适应步长策略搜集地图环境中的信息来加速算法收敛并达到探索空间的目的[11]。最终在其规划出来的路径上采用三次B样条函数优化其平滑性，来生成机械臂可跟踪的光滑路径[12]。通过仿真实验分析，验证了所提算法的快速性和有效性。

1.1 机械臂运动学分析及其模型建立

本文研究对象为六轴机械臂，其Urdf模型如图1所示，其相应的D-H参数如表1所示。

表1 D-H参数表

表中各参数分别为：假定当前关节标号为i-1，下一关节为i，di为连杆i相对于连杆i-1的偏置；
ai为连杆i的长度；
αi为连杆i的扭角；
θi为关节角。

连杆i相对于连杆i-1坐标系变换通式为：

(1)

(2)

式中：n,o,a三个列向量表示机械臂姿态;x,y,z为三维空间内的基于右手定则的坐标表示;p表示其三维坐标。

1.2 机械臂连杆与障碍物空间占位关系简化描述

避障路径规划的前提是要对机械臂本体以及障碍物建立数学模型以便于设计碰撞检测算法[13]。

一般工业中，障碍物具有随机性分布、形状不确定性的特点，难以对其精确建模，针对障碍物建模方法一般包括包络盒、包络球等[14-17]。障碍物包络球建模方法具有计算简单且在三维空间内旋转角度和位置不会影响碰撞检测性能等的优点，因此，本文采用包络球方法进行障碍物建模，其半径可表示为：

R={(x,y,z)|(x-a)2+

(y-b)2+(z-c)2≤r2}。

(3)

式中：(a，b，c)为包络球的中心在xyz坐标系上对应的坐标；
r为包络球半径；
(x，y，z)为障碍物表面任意一点的坐标。

建模过程如图2所示，其中灰色部分为障碍物，以O为球心，以R1为半径的球面为包络面,考虑到需要为后续碰撞检测做基础，将包络球表面进行膨化处理，具体膨化距离约等于机械臂连杆半径，膨化后包络球半径为R2。

其次，需要对简化后机械臂连杆与障碍物之间的空间占比关系进行描述。一般情况下，六自由度工业机械臂的连杆多为长方体或圆柱体，在碰撞检测过程中必须考虑其中心到边缘的最大包络半径。为简化碰撞检测设计过程，本文采用一种机械臂建模简化方法[18]，如图3所示。

在已知末端位姿的情况下，根据工具箱中逆运动学求解函数求得一组最优解，随后求出正解即可得到连杆AB的方程。

设点O到连杆AB的最短距离为dmin，根据连杆与包络球的位置关系分为以下几种情况：

(1)dmin>r,不发生碰撞,如图4a所示。

(2)dmin

(3)dmin

(4)dmin

由图4可见，图4a和图4b所示两种情况分别为机械臂连杆与障碍物未发生碰撞的两种表现形式，图4c和图4d所示分别表示机械臂连杆与障碍物发生两个交点、一个交点碰撞的表现形式，根据该原理可便于计算机械臂的碰撞检测问题。

2.1 基本原理简介

快速拓展随机树(RRT)算法是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过随机构建Space Filling Tree实现对非凸高维空间快速搜索的算法。该算法可以很容易地处理包含障碍物和差分运动约束的场景，因而被广泛地应用在各种机器人的运动规划场景中[19-20]。该算法示意图如图5所示。

RRT算法虽然在高维空间中很适用，但也存在一些弊端，比如它是一种纯粹的随机搜索算法，对环境类型不敏感，当搜索空间中存在大量障碍物时，该算法缺乏导向性从而会导致搜索速度慢、效率大幅下降以及该算法规划出来的路径尖点较多不平滑等，针对这类问题本文提出了一些优化方法。

2.2 传统RRT算法优化(PAVS-RRT)

2.2.1 基于概率的目标偏向寻优

考虑到传统的RRT算法在复杂环境中存在搜索时间长、采样点随机性高等问题，本文采用基于概率的目标偏向寻优的RRT算法(P-RRT)[21-22]，其核心思想主要是在迭代过程中引入目标偏置策略来优化采样过程，在其采样过程中对扩展函数引入目标偏置策略来优化采样点：

(4)

如式(4)所示，假定一个目标偏置阈值Ptarget，然后按均匀概率分布随机获取一个取值范围在0～1内的概率值P，若该值大于阈值，则树的扩展方向为向目标点xgoal生长，若该值小于阈值，则会采用RandomState函数产生的随机点作为采样节点。基于概率P的目标偏置策略的RRT算法伪代码如下：

Input:M,xinit,xgoal

Result:APathTfromxinittoxgoal

fori=1tondo

xrand←ChooseTarget(xrand,xgoal,P)

ifP>Ptargetthen

xrand←x;

else

xrand←RandomState();

xnear←Near(xrand,T);

xnew←Steer(xrand,xnear,Stepsize) ;

Ei←Edge(xnew,xnear);

ifCollision_Free(M,Ei)then

T.addNode(xnew);

T.addEdge(Ei);

ifxnew=xgoalthen

Success();

上述代码中：M为地图环境,xinit为起始位置,xgoal为目标位置，P为概率函数。路径空间搜索过程从起点开始，先随机撒点xrand；
然后查找距离xrand最近的节点xnear并根据概率P的值来选择树的生长方向；
然后沿着xnear到xrand方向前进Stepsize的距离得到xnew；
Collision_Free(M,Ei)方法检测Edge(xnew,xnear)是否与地图环境中的障碍物有碰撞，若没有碰撞则可以完成一次成功的空间拓展搜索。

2.2.2 局部扩展机制

考虑到在RRT算法中引入目标偏执的函数，从而使采样的概率分布性发生变化，新节点总是选择离目标近的节点，为保证收敛性、完备性以及避免扩展树易陷入到局部极小值的问题，本文引入一种局部扩展机制[23]。其核心思想为：对于陷入局部最优的节点选择是以局部RRT为主的，剔除已遍历过的区域。若树中新的扩展节点xnew与其父节点xf的距离小于与树上其他节点的最近距离，即distance(xnew,xf)

如图6a所示，虚线连接并向外扩展的节点是可以进行下一步扩展的节点，而与实心节点圈在一起的节点为不满足约束而不被实例化的节点，图6b中所有节点均为待扩展节点，即可以实例化的节点。

算法实现过程如下：

(1)算法使陷入局部极值的节点xf通过expansion_Node()函数延展出一个局部扩展集合x；
(2)对集合中各节点及边进行碰撞检测；
(3)对未发生碰撞的节点求它们与树T中除父节点xf外所有节点的最短距离，若集合中某些节点与树中节点的最短距离大于与其父节点的距离，则保留这些节点x，并将它们加入树T中。若当前节点无法扩展，则令其父节点通过expansion_Node()函数扩展。

(4)树的扩展陷入局部极值时，算法会使陷入局部极值部分的各节点向外扩展一个步长，随后再对每个待扩展节点进行分析，直至完成一次成功扩展。

2.2.3 自适应步长策略

采用固定步长的RRT算法，其节点在扩展过程中，无法充分利用工作空间和障碍物的信息。本文采用自适应步长策略，利用扩展过程中收集到的信息来适应树中节点的扩展，使得随机树可以更快遍历障碍物较少的区域[24]。其具体步骤为：在算法开始时，给定一个初始步长εinit和最短步长εmin，初始步长为最短步长的n倍。首先使用初始步长对随机树进行拓展，在每一次随机树拓展生成新节点xnew时，进行障碍物碰撞检测Collision_Free(M,Ei)，如无检测到障碍物则调整步长。

本文所采用的步长调整策略：

(1)随机树拓展生成新节点xnew，判断该新节点与其父节点xf之间的路径是否会存在障碍物，如不存在障碍物则将xnew加入到随机树中。

(2)若xnew在障碍物范围内，则将xnew与其父节点xf的连线平分成n个线段，其中节点为x1,x2,…,xm,xm+1,…,xn-1。

(3)遍历这些节点找到满足要求的两个节点xm，xm+1，其中xm不在障碍物内而xm+1在障碍物内且必须满足m>1。

(4)若存在这两个节点xm，xm+1，则拓展后新的节点为xm，否则判断此次拓展失败。

xnew与其父节点xf的连线分段如图7所示。

RRT算法得到的路径是由随机树中的节点依次连接的连续线段，并不是一条平滑的曲线，从而会影响机械手的稳定性，加剧磨损。因此，需要对计划中的连续线段进行平滑处理。准均匀B样条函数具有连续性的特点，它可以传递起始位置和方向以及目标位置和方向。考虑到该函数的这些优点，对本文提出的改进RRT(PAVS-RRT)算法所得到的路径进行平滑优化处理。首先，k阶B样条函数表达式为:

(5)

式中:di(i=0,1,2,3,…)表示控制点，Ni,k(u)表示k次B样条基函数,且有：

(6)

(7)

三次B样条基函数：

(8)

三次B样条曲线段：

c0,3(u)=c0×N0,3(u)+c1×N1,3(u)+

c2×N2,3(u)+c3×N3,3(u)，u∈[0,1]。

(9)

当给定控制点di的时候，利用式(9)可以求出一系列满足三次B样条的曲线点。

4.1 RRT寻迹

本文在Inteli5-9500台式电脑上使用MATLABR2020a对本文提出的算法在三维环境下进行仿真验证。

在本次仿真实验中，搜索步长设定为5，迭代次数上限设定为4 000，由于类似于RRT算法这样的基于采样的算法存在随机性，执行实验次数为100。路径规划起点设置为[5,5]，目标点设置为[95,95]，实验环境设置为[100×100×100]的三维空间，障碍物为随机布置。如图9所示为传统RRT[5]，RRT-Connect[25]，RRTstar[26]以及本文所提出的改进RRT算法(PAVS-RRT)的寻迹结果。

表2 各类RRT算法性能对比

图9中：蓝色的线是各种算法最终寻找到的路径，红色线条是拓展树的分支，绿点为三维空间中随机采样点。由图9可见，本文所提出的PAVS-RRT算法在空间中采样节点数明显少于其他3种方法,且规划出来的路径曲折程度较低，结合表2中具体数据可得，PAVS-RRT算法相较于传统RRT算法在规划时间层面上由0.634 s降低到0.168 s，提高了约73.49%，平均路径长度由247.49cm缩短到183.84 cm，减少了约25.72%。虽然RRTstar找到了相对较小代价的路径但是其规划时间过长，且不必要节点数过多，而PAVS-RRT算法的路径搜索更有方向性，同时也大大减少了不必要的探索节点，从而节省时间，提升了搜索效率。

4.2 路径平滑性优化效果分析

考虑到基于随机采样规划的RRT算法所规划出来的路径都是在三维空间中随机生成的采样点连接成的路径，会导致其路径平滑度差，因此本文采用三次B样条曲线对所规划出来的路径映射到关节空间后进行平滑性优化处理，细节处理效果如图10和图11所示。

图10为在局部空间内的路径规划。图11为在图10所规划出来的路径的基础上，导入本文上述的六自由度机械臂模型，在保证其能平稳运行的同时，在其关节空间内采用三次B样条曲线拟合优化末端轨迹前后的对比效果。其中实线表示未插值前路径，虚线代表优化后机械臂末端轨迹，从图中可以明确地看出相较于插值前的轨迹有了一个明显平滑的趋势，且在机械臂运行过程中并未与障碍物发生碰撞。

在图12中关于角度曲线的圆圈表示各关节在之前采用随机采样算法规划出来的由离散点组成的路径上的插值优化节点，且各个关节的速度和加速度的初始值都为0，符合实际情况。从图12的机械臂各个关节的角度、角速度以及角加速度的曲线可以看出，在机械臂的末端轨迹拟合优化后得到更平滑的轨迹的同时，也保证了机械臂的各个关节的角度(pos)、角速度(vel)以及角加速度(acc)并没有发生突变，减少了机械臂各个关节的抖振情况，保障了机械臂在所规划路径上运行状态平稳且成功避障。

本文针对六轴工业机械臂在复杂环境中采用传统RRT算法进行运动规划时产生的导向性差、规划时间长、无效节点过多以及平滑性较差等情况，提出一种基于目标偏置策略融合自适应步长策略的RRT算法，其结果表明改进后的算法在其规划时间以及各种路径代价上都优于传统RRT算法，并在规划出来的路径上采用三次B样条函数对路径进行插值优化，在保证机械臂各关节平稳运行的同时提高了末端轨迹的平滑度。而且在机械臂仿真中有效提高了轨迹的平滑性以及各项参数的性能，降低了规划时间，可以快速地对机械臂进行运动规划，具有较好的发展与应用空间。后续考虑在Ubuntu 16.04系统中采用机器人操作系统(Robot Operating System, ROS)中的Gazebo仿真软件搭建模型，并将本文所设计的算法嵌入至ompl运动规划库中，通过Moveit对其调用，并通过所搭建的物理仿真环境，进一步验证本文所提算法有效性。

猜你喜欢样条连杆障碍物一元五次B样条拟插值研究安徽师范大学学报（自然科学版）(2022年3期)2022-07-14压裂泵虚拟试验连杆疲劳性能研究石油矿场机械(2022年3期)2022-06-17某发动机连杆螺栓拧紧工艺开发装备制造技术(2020年2期)2020-12-14高低翻越动漫界·幼教365(中班)(2020年3期)2020-04-20SelTrac®CBTC系统中非通信障碍物的设计和处理铁道通信信号(2020年9期)2020-02-06赶飞机创新作文(1-2年级)(2019年4期)2019-10-15三次参数样条在机床高速高精加工中的应用制造技术与机床(2017年7期)2018-01-19三次样条和二次删除相辅助的WASD神经网络与日本人口预测软件(2017年6期)2017-09-23连杆的设计及有限元分析汽车实用技术(2017年16期)2017-09-21基于样条函数的高精度电子秤设计计算机测量与控制(2017年6期)2017-07-01