基于脊线模态法和RRT算法的蛇形机械臂避障控制研究 ①

王 轸 常 健 李 斌 王 聪 刘 春 张良全

(*中国科学院沈阳自动化研究所机器人学国家重点实验室 沈阳 110016)(**中国科学院机器人与智能制造创新研究院 沈阳 110169)(***中国科学院大学 北京 100049)(****东北大学信息科学与工程学院 沈阳 110819)

(收稿日期：2019-11-13)

0 引 言

随着科学技术的进步，新一代的仿生机器人将遇到一系列的挑战[1]。替代人在恶劣环境下工作，仿生机器人表现出了良好的应用前景[2]。现今越来越复杂的工作环境，如太空探索、资源勘探、管道检查、水下工作等，对仿生机器人的工作内容有了更高的要求。目前，研究者通过模仿蛇的生理机构，研究出具有高自由度的蛇形机械臂。为了突出蛇形机械臂的高灵活性，降低机械臂主体的重量，多采用绳索驱动[3-4]。与传统机械臂相比，蛇形机械臂对复杂的工作环境具有更强的避障能力和适应性。

蛇形机械臂属于多冗余机器人，其逆运动学求解和轨迹规划具有一定难度，被国内外学者广泛研究。目前运动学求逆通常采用雅可比迭代法[5-7]，但是对多冗余机械臂，存在求解雅可比伪逆困难、计算量巨大的问题。王俊刚等人[8]针对多冗余蛇形机械臂提出了一种末端跟随运动的逆解算法。根据末端的运动，依次计算出其他关节的运动。与雅可比迭代法相比，末端跟随算法计算量得到了降低，但它只适用于基座能够平移的蛇形机械臂。董云等人[9]将遗传算法与解析法结合提出了计算量小、适应性强的逆解算法，但是其针对的是平面三自由度冗余机械臂，对于三维空间高自由度机械臂无法快速获得最优解。相比于传统求解方法，脊线模态法可以得到机械臂末端满足期望位姿的脊线，同时反映机械臂的宏观几何构型，具有效率高、计算简单的优点，且对末端姿态有精确的定义，适用于非闭环构型的冗余机械臂的逆解求解。Chirikjian等人[10-11]提出了使用“脊线”对多冗余机械臂的运动学进行研究，用空间脊线来表示机械臂的几何特征。Fahimi等人[12]改进了Chirikjian脊线模型的模态参数方程组，提高了脊线模态法的效率。

而对于机械臂的轨迹规划问题，祁若龙等人[13]将轨迹规划转化成多目标优化求解问题，并用遗传算法求解。但是随着自由度的增加，遗传算法的待求解参数数量增加，碰撞检测的计算成本也会增加，计算时间随之成倍增加。Zhou等人[14]利用蚁群算法规划出机械臂末端的无碰撞最短路径，但是对于更复杂的障碍布局易出现无解的情况。此外，轨迹规划的方法还有人工势场法[15]、快速扩展随机树(rapidly-exploring random tree， RRT)算法[16]、强化学习算法[17]等。其中，RRT算法是基于采样查询的规划方法，通过随机采样，不断查询，最终生成一条不发生碰撞的路径，同时避免了对空间的模型建立，具有无需精确的数学模型、适用高维空间等优点，尤为适用于多冗余机械臂高维空间的轨迹规划。

结合脊线模态法和RRT的控制算法的优点，本文提出适合于复杂作业空间的蛇形机械臂避障的算法。算法主要利用脊线模态法实现多冗余蛇形机械臂的逆运动学求解，并在此基础之上进行关节自适应调整，最后利用RRT算法进行轨迹生成。

本文首先对蛇形机械臂进行结构特性设计及运动模型的建立，其次利用脊线模态法实现机械臂的运动学逆解以及研究避障轨迹规划，最后在Matlab环境下进行仿真分析。

1 机械臂结构设计及运动学模型建立

1.1 结构设计

蛇形机械臂采用蜿蜒型[18]如图1所示。整体机械臂由若干段刚性臂杆构成，各臂杆之间由关节连接，具有很好的灵活性及刚度。

图1 蜿蜒型机器人

蛇形机械臂的驱动方式为绳索驱动，即驱动器电机与机械臂本体分离，驱动器通过绳索进行驱动力的传递。绳索驱动有效地减少了机械臂本体的质量与体积。蛇形机械臂的整体结构[19]如图2所示，机械臂关节为万向节类型，单个万向节关节具有2个自由度，整个机械臂通过8个关节模块串联在一起。基座包含电机和丝杠，电机带动丝杠旋转拉动绳索。

图2 蛇形机械臂整体结构图

1.2 运动学模型建立

蛇形机械臂单个关节通过3根绳索驱动，整个机械臂共有24根绳索。绳子在底端圆盘的分布如图3所示。

图3 绳索圆盘分布

图2中关节的序号从右向左依次为1、2、3、4、5、6、7、8，圆盘半径为r。根据右手定则，在图3中建立的坐标系，第i个关节对应的3根绳索的坐标矩阵为

(1)

对单个关节进行分析，关节角度变化与绳索变化的示意图如图4所示。l1h1、l2h2、l3h3即表示3根绳索，万向节关节的2个角度与3根绳索的长度是相对应的。

图4 关节角度与绳长示意图

通过坐标变换法由角度变化得到绳索变化。根据图4利用改进型D-H法建立单个关节的坐标系，如图5所示。

万向节中心到关节的上平面和下平面的距离均设为l，对于第i个关节旋转的2个角度为θzi和θyi，相对应的变换矩阵为

(2)

通过式(1)求得关节上平面3点h1、h2、h3相对自身坐标系的坐标，然后根据变换矩阵，求出相对图5中基坐标系的坐标。

(3)

图5 单关节坐标系

通过两点距离求出当前角度状态下l1h1、l2h2、l3h3的距离。其他关节绳索的长度同理可得。同时蛇形机械臂的运动是存在耦合的。例如1号关节对应的3根绳索长度变化是所有关节对这3根绳索长度变化之和。所以最终给定16个角度，则24根绳长变化如式(4)所示，其中j为图3中的绳索序号。

(4)

2 机械臂的逆运动学

蛇形机械臂属于多冗余机械臂，本文首先采用脊线模态法确定蛇形机械臂的空间脊线，其次通过二分法将各个关节拟合在空间脊线上，最后通过几何法计算出各个关节的角度，实现逆解。

2.1 脊线模态法

脊线模态法基于微分几何，用来确定机械臂的空间脊线，如图6所示，其函数表达式为

(5)

其中，s表示脊线归一化的长度参数，s∈[0, 1]；L为脊线的实际长度；u(σ,t)为脊线在σ处的单位化的切线向量。其中，脊线的起始坐标就是x(0,t)，终点坐标为x(1,t)。脊线上的点对应的切向量用式(6)表示为

μ=[x′(s)y′(s)z′(s)]

=[sinKcosTcosKcosTsinT]

(6)

图6 空间脊线

K和T的定义如图7所示。定义空间脊线的起始点为坐标原点，所以空间脊线上的任一点可以表示为

(7)

由此，通过确定K与T得到空间脊线的整体构

图7 K与T角度的定义

型。K与T可表示为

K(s,t)=a1sin(2πs)+a2(1-cos(2πs))

(8)

式(8)中a1、a2、a3为模态协同参数，[b1k,b1t]=[K(0,t),T(0,t)]为起始位置的2个角度，[b2k,b2t]=[K(1,t),T(1,t)]为脊线末端的2个角度。b1k、b1t、b2k、b2t是根据期望目标确定的。得到合适的模态协同参数就可以确定K与T。本文采用数值解析法来求解模态协同参数。在s=1情况下将式(7)对t求导，整理得：

(9)

(10)

其中：

J31=0

J32=0

根据式(9)建立迭代公式：

am+1=am+αJ-1(am)[xd-xm]

(11)

其中，α为控制收敛率的常数，m为迭代次数，J-1(am)为式(10)中3×3的模态雅可比矩阵的逆矩阵，xd为脊线末端期望位置，xm为当前求解的位置。收敛系数α和迭代总次数根据经验选择，直到期望位置与当前求解的位置的误差满足精度要求，则得出合适的模态协同参数。模态协同参数确定之后，即得到一条末端满足期望位姿的空间脊线。

2.2 关节拟合

得到满足期望目标的空间曲线之后，通过关节拟合，将蛇形机械臂的各个关节点尽可能地拟合到空间曲线上。本蛇形机械臂有8个关节，起点为坐标原点，终点即为所求空间脊线的终点，需要在曲线上拟合7个点。同时保证相邻两点之间的距离为单个臂杆的长度，本文采用二分法从终点倒推，依次查找其他关节点的空间坐标，流程图如图8所示。

图8 二分法算法流程图

通过二分法计算得出蛇形机械臂所有关节点的空间坐标，之后利用坐标变换法依次求出每个关节对应的2个角度值，将所有角度带入式(4)获得24根绳索的长度。

3 机械臂的避障轨迹规划

在各类非结构化的工作环境中，要使蛇形机械臂从初始位置到达期望位置，机械臂需要具有良好的避障能力。根据脊线模态法得到一条期望的空间脊线。蛇形机械臂的轨迹规划问题就可以转化为从初始位置的脊线生成到期望脊线的轨迹，同时充分考虑障碍的影响。以下对不同位置的障碍展开研究。

3.1 障碍在脊线上的避障策略

本文用圆球来表示障碍，设定障碍圆球的半径为r0。对于碰撞检测，脊线上存在某点到障碍圆球的圆心的距离小于r0，则说明碰撞。利用脊线模态法生成的空间脊线上存在障碍，则可以利用蛇形机械臂高自由度的特性，通过旋转部分关节，从而绕过障碍，形成一条新的空间脊线。

如图9所示，当障碍半径较小时，通过计算，只需要调整2个关节即可绕过障碍。即以关节5的起点、关节6的终点连线为旋转轴，对两关节进行旋转，直到关节曲线避过障碍。图中障碍存在这个脊线上，脊线模态法初始生成的空间脊线与障碍发生碰撞，障碍上方的脊线表示调整2个关节之后的脊线。该脊线与其他未发生碰撞的脊线构成了一个新的绕过障碍的空间脊线。调整2个关节，计算简便，同时各个关节角变化量小，但对于障碍半径较大的情况，旋转2个关节无法避开障碍，则可以选择调整4个关节，以此类推。

图9 障碍在关节曲线上

关节调整分为3种情况进行研究。(1)障碍靠近期望终点，那么在这个期望位姿的空间脊线是无法调整的。在满足要求的情况下，可以更换终点的姿态，生成新的关节曲线。(2)障碍靠近起始点，则只能对2个关节进行调整。(3)其他情况，首先进行2个关节调整，如果对2个关节调整后无法避开障碍，则进行4个关节的调整。流程图如图10所示，图中n为机械臂的关节总数。通过分析距离障碍圆心最近的关节序号的大小进行分类讨论。

图10 关节调整流程图

调整4个关节的主要思路是先将中间关节调整成能够拟合一条弧线，之后进行旋转避障。5个关节点设为L1、L2、L3、L4和L5。首先选择L1、L3、L5所在的平面建立坐标系，L1、L5点的绝对位置始终保持不变。设单个连杆长度为L，先构造以L1、L3、L5为顶点，两边为2L的等腰三角形如图11所示，同时求出当前坐标系相对基坐标系的变换矩阵。

根据等腰三角形的性质求得L3的坐标，之后进行循环操作，即L3沿着z轴负方向减小一个步长。直至L3L5的距离等于L2L4的距离，即保证这5个关节点在弧线上，如图12所示。

图11 L1、L3、L5构成等腰三角形

图12 压缩、拟合圆弧

对L2、L3、L43点进行初步调整之后，以L1和L5连线为旋转轴，其他3点绕旋转轴进行旋转，直到关节线避开障碍。最后得到5个关节点的绝对坐标，与其他关节点形成新的空间脊线，如图13所示。原始脊线被调整后生成无碰撞的空间脊线。

图13 调整4个关节避障

3.2 障碍在其他位置的避障策略

通过上节的分析，解决了期望目标空间脊线上存在障碍的问题。而对于其他位置的障碍，即机械臂从初始位置到达最终位置的过程中存在的障碍，本文采取RRT算法生成绕过障碍的路径轨迹。RRT算法中初始角度矩阵与期望角度矩阵维度为16。算法在十六维的空间中快速探寻出无碰撞的轨迹。蛇形机械臂RRT算法的流程图如图14所示。

图14 RRT算法流程图

4 仿真分析

通过Matlab对蛇形机械臂进行仿真，主要包括角度变化与绳长变化映射关系、脊线模态算法、二分法关节拟合效果以及蛇形机械臂轨迹规划，进而验证这些方法的有效性。

对单个关节进行研究，分析角度变化与绳长变化的关系，根据前文图2所示，以1号关节为例，两个旋转角度为θz和θy。圆盘半径r为6 cm，万向节中心到上下端面的距离l为5 cm。两个角度变化的范围均为[-60 °,60 °]。单个角度变化影响的绳长变化如图15所示。从图15中上图可以看出，关节1在θy为0的情况下θz变化对3根绳索的影响，根据图3,2号绳索与3号绳索位置关于y轴对称，关节绕z轴旋转，2号绳索与3号绳索长度变化是相同的，θz变化过程中1号绳索伸长，2号、3号绳索缩短。从图15中下图可以看出，关节1在θz为0的情况下θy变化对3根绳索的影响，1号绳索在y轴的负方向上，整体变化不大，2号绳索逐渐伸长，3号绳索逐渐缩短。仿真计算分析符合实际情况。

图15 单关节角度绳长变化图

通过例子对脊线模态法和二分法关节拟合进行分析。单个臂杆的长度为20 cm，根据脊线模态法，假设起始位置的两个角度[K(0,t),T(0,t)]=[π/2, 0]切线方向平行于x轴，脊线末端的两个角度[K(1,t),T(1,t)]=[0, π/6],末端的位置设为[60， 80， 80]，根据2.1节的计算步骤，得到a1为0.40、a2为0.13、a3为0.20。计算生成一条空间脊线，如图16所示。曲线为脊线模态法生成的空间脊线。通过二分法进行关节拟合，每个折线段表示单个臂杆。

图16 脊线模态法及关节拟合

对存在脊线上的障碍和其他位置的障碍进行分析，通过调整关节和RRT算法生成使末端到达期望位姿且无碰撞的轨迹，如图17所示。一簇脊线从右下到左上是算法生成的轨迹。两个圆球代表两种不同类型的障碍。对于存在初始期望脊线上的障碍，脊线调整4个关节生成绕过障碍的空间脊线，之后利用RRT算法生成图中的轨迹，使蛇形机械臂到达期望的目标。

图17 蛇形机械臂轨迹规划

从数值上分析图17中的轨迹无碰撞发生。图中两个障碍的半径设定为10 cm,蛇形机械臂从当前轨迹运动到下一个轨迹的时间为0.1 s,则两障碍圆心到每个轨迹的最短距离变化如图18所示。实曲线为存在脊线上的障碍圆心到机械臂的最短距离，从初始位置到最终位置，距离呈减小趋势。调整关节时考虑的是理想情况，不发生碰撞就停止调整

图18 两障碍到所有轨迹的最短距离

关节，脊线上存在某点在障碍的临界位置，所以最终轨迹机械臂到圆心的距离逼近10 cm。虚曲线为过程中障碍圆心到机械臂的最短距离，数值先减小后增加。只要满足虚曲线每点对应的值大于10 cm即表明无碰撞发生，算法生成的路径中有一段靠近障碍，如图17所示，因此虚曲线存在一部分距离接近于10 cm。

对图17中机械臂运动中绳长变化进行分析。通过式(4)计算得出在蛇形机械臂运动过程中每条脊线对应的绳索长度，24条绳索长度变化如图19所示。图17中每条脊线末端两条线段变化较小，即代表关节7、关节8角度变化较小，对应图19中绳索19～24长度变化较小。

图19 绳长变化

5 结 论

本文以蛇形机械臂为研究对象，研究得到机械臂各角度变化到绳索长度的函数关系。提出了一种基于脊线模态法和RRT算法的避障轨迹规划算法。采用脊线模态法得到满足期望位姿的空间脊线，通过二分法对空间脊线进行关节拟合。在脊线模态法的基础之上主要对存在于脊线上的障碍进行分析，通过调整关节得到不经过障碍的空间脊线，然后利用RRT算法生成无碰撞的轨迹。最后，通过Matlab仿真分析了单关节角度绳长的变化关系，并验证了基于脊线模态法与RRT的避障控制算法的可行性。在未来的研究中，将进一步改进蛇形机械臂的避障轨迹规划算法，优化调整关节的算法，探寻最优路径，降低蛇形机械臂的工作代价。