复杂环境下软体机械臂的运动学解耦与避障方法

王 凌，张 印，谢雨鑫，蒲 伟

（四川大学 空天科学与工程学院，四川 成都 610065）

0 引言

软体机械臂相比于传统的刚性超冗余机械臂，具有刚度小、质量轻、柔性化等特点，灵活性和环境适应能力更强[1]。在深海探测、医疗手术、航空航天、工业生产等领域应用潜力巨大[2-4]。

近年来，国内外研究者围绕软体臂的设计、建模、控制和避障开展了诸多研究。例如，Shafti 等[5]提出了一种新的卡尔曼滤波算法，将姿态估计和避障相结合，实现了多段连续体机械臂的避障运动。Xiao 等提出了一种在不确定环境中抓取目标物体的在线运动规划算法，并将此算法运用在研制的平面连续体机器人上。Ataka 等[6]提出了一种基于多段连续机械臂的实时路径规划算法，并在研制的单节连续机器人上进行了实验验证。

但是由于软体臂具有无限自由度，不同构型间切换时在运动学上存在耦合。比如，从正运动学来看，当求解柔性臂正向运动学时，柔性臂每段的参考坐标系会随之改变。反之，从逆运动学来看，当已知坐标点反求柔性臂关节参数信息时，前一段柔性臂会跟随后一段进行一系列的运动。这种运动学上的耦合关系是软体机械臂在复杂环境下避障面临的巨大挑战。因为机械臂要绕过狭窄非结构化障碍物，要求柔性臂有良好的柔顺性。提出一种分段顺序建模和控制的方法，在构建运动学模型时，我们将软体臂本体的一大段视为若干小段，然后从根部到前端顺序驱动控制每一小段变形。利用这种仿藤蔓生长原理，实现软体机械臂在复杂的环境中轨迹规划。

主要贡献如下：

（1）提出了一种顺序控制方法，通过顺序控制软体机械臂的运动来避免运动学耦合。

（2）建立了软体机械臂的正逆运动学模型，并在仿真环境中验证了模型的正确性。

（3）构建了改进型快速探索随机树路径规划算法，提高路径规划算法的成功率、减少算法耗时。

1 顺序控制思想

受大自然中藤蔓植物生长过程的启发，提出了一种软体机械臂的顺序控制思想。这种控制思想的优点是在避障时，柔性臂能够仿藤蔓生长的过程，按照顺序生长运动。避免柔性机械臂每段之间存在耦合的问题，在非结构化复杂环境中能更好地完成避障任务。每段软体臂不仅有弯曲和偏转方向的自由度，并且有一个伸长方向的自由度，如图1 所示。

图1 为软体臂的顺序控制思想：每段软体臂依次进行运动，当第一段运动时其余两段将收缩在第一段的装置内，当第一段完成运动后，其余两段再依次运动。

图1 顺序控制思想示意图

2 运动学建模

建立软体机械臂的运动学模型是对实现在复杂环境中避障的重要前提。由于软体机械臂相比于传统的刚性机械臂没有自由度和关节等概念，针对传统刚性机械臂的建模方法不能直接应用。为此，有研究人员分别利用分段常曲率方法[7-8]、Cosserat 杆理论对软体机械臂进行运动学建模分析。采用分段常曲率方法对软体机械臂进行运动学分析。引入关节空间变量（θ，φ），两个参数分别对应每一段柔性机械臂的弯曲角和偏转角，k表示每段柔性臂弯曲曲率。机械臂的每一段由三个气动驱动器所驱动i为驱动器的编号。在运动学模型中如图2（b）所示，正运动学关系为驱动空间到关节空间的映射和关节空间到工作空间的映射。逆运动学关系为工作空间到关节空间的映射和关节空间到驱动空间的映射。

图2（a）软体臂在空间中的参数，其中θ为软体臂的弯曲角度，φ为软体臂在空间中绕Z轴的偏转角度，r为曲率半径，软体机械臂驱动器长度、弯曲角度、偏转角度示意图l0i表示每一段软体臂的三个驱动腔体的长度，d为中心点O到气腔表面距离。（b）为运动学映射关系。

图2 软体臂参数信息

2.1 正运动学模型

从机械臂的两种状态来分析其运动学关系。初始状态时，整个软体机械臂收缩在一起，我们认为此时臂每段的长度为零。伸长状态时，每段软体臂依次先伸长再进行弯曲，伸长量为l，弯曲角度为θ。本研究中，整个软体机械臂由三段组成，每段的运动学建模思路完全相同，为说明运动学的分析方法，下面以单段软体臂为例进行详细的正运动学分析。软体机械臂单段的简化几何模型如图2（a）所示，θ表示软体臂的弯曲角度，l表示软体臂的长度，φ表示在空间中软体臂与Z轴的偏转角度。

软体机械臂关节空间到工作空间的映射关系可由齐次变换矩阵T表示，T表示基座坐标系至末端坐标系的变换。代表从上一个基坐标系原点到下一段坐标系原点的平移。此变换方式为先绕Z轴旋转φ，再绕Y轴旋转θ，最后再绕Z旋转-φ。设定弯曲角θ，θ∊（0，π），设定偏转角度φ，φ∊（0，2π），li为软体臂各段的长度，i∊（1，2，3）。根据分段常曲率模型，可求得齐次变换矩阵为：

根据单段软体臂分析方法可求得三段式软体机械臂关节空间至工作空间的映射关系。由基座标系O0至第三段末端坐标系O3的齐次变换矩阵表示。基座标系到最末端坐标系可以由基座标系0 到第一段末端1 的齐次变换矩阵；坐标系1 到坐标系2 的齐次变换矩阵；坐标系2 到坐标系3 的齐次变换矩阵三部分相乘得到，其中：

如图2（c）所示，软体臂的第一段的驱动空间与关节空间的映射关系如式（6）-（8）所示：

驱动器长度与弯曲角、偏转角之间的关系为：

2.2 逆运动学模型

为建立起机械臂从工作空间到关节空间的映射，需要构建软体臂的逆运动学模型。通过求解齐次变换矩阵中非线性方程组实现工作空间到关节空间的映射，但具有一定挑战性。为了提高求解可靠性，降低求解难度，在求解逆运动学方程时设定约束条件来简化求解逆运动学方程难度，从而避免在求解逆运动学过程中可能会产生奇异解甚至无解的情况。假设在求解软体臂逆运动学参数时，每段长度固定不变，只用考虑弯曲和偏转两个变量。约束条件和求解如公式10-13。

为验证运动学模型的正确性，在Matlab 仿真环境中设定了系列目标点位置，然后让机械臂每段处于已伸长完成的状态，从初始位置（0，0，0）运动到指定目标点位置（123，259.4，421）。通过逆运动学求解各段弯曲角为（0.63rad，0.63rad，0.63rad），各段偏转角为（0.45rad，3.58rad，0.45rad）。利用逆运动学求解软体臂运动到目标点仿真,结果见图3（a），图中显示软体臂成功运动到指定点。运动到指定点所运动过程中各段驱动器长度的变化量，如图3（b）所示，从图能够发现各驱动器的长度变化量之间是解耦合关系。

图3 逆运动学模型验证

3 避障

3.1 改进型RRT 避障算法

快速探索随机树算法（Rapidly Exploring Random Trees，RRT）是一种基于随机采样的路径规划算法。该算法的工作原理是在空间中从起点向终点进行采样，将满足要求的采样点依次连接形成一条像树枝的路径。RRT 算法在无人机、智能车避障领域中有着广泛的应用。传统的RRT 算法在空间中随机采样，存在采样数据点过多、随机性较大和整体算法耗时长等问题。针对这些问题，提出了一种RRT 算法的优化方法。

3.2 概率偏向目标点

因为RRT 算法在空间中寻找采样点完全随机，所以在寻找路径时会存在耗时较高和导向性不稳定等缺点。因此，在采样时利用一种基于概率的采样方法。假设关系如下：

如式（14）所示，设定概率Pt，按照概率均匀随机分布，在采样前获得一个概率值P，如果P小于Pt，则采样点为目标点，路径则向目标点方向生长。如果大于Pt，将空间中随机产生的点作为采样点。

3.3 分段寻迹

RRT 算法在生成节点时存在一定随机性，生成的最终路径不连续。此外，起点距离终点太远容易导致路径规划失败。因此，为提高算法的成功率，将路径找寻任务分为若干子任务。首先，在寻找路径时，对整条路径信息进行预先细分。然后，在仿真环境中依次设置“分段终点”。最后，RRT 算法在寻找路径时将根据预先设置的“分段终点”依次到达目标点位。这种方法能够有效地提高路径寻找的成功率，渐少生成路径的随机性。

3.4 路径寻找实验

仿真使用的计算机软硬件配置为：IntelR CoreTM i5 6300HQ，内存（RAM）16GB，显卡为Nvidia GeForce GTX 960M，MATLAB 使用版本为2016b。

在本次仿真实验中，设置搜索步长为6，最高迭代次数为7000，实验空间环境设置为[800，800，1000]路径规划的起点设置为[0，0，0]，目标终点设置为[500，.600，150]，实验中的分段终点为[400，320，100]，[420，410，250]。路径规划实验，如图4 所示。其中长方体表示障碍物，在复杂环境中生成的路径如图4 所示，由图可见该路径成功避开障碍物。

图4 路径寻找实验

4 避障实验分析

为验证软体臂自生长工作模式的避障效果，进行了软体臂在非结构化复杂环境中的避障实验。为模拟软体臂在工作时需要穿越狭小空间的情况在仿真环境中设置长方体障碍物布局如图4 所示。为了保证机械臂能够成功避开障碍物，到达目标点位，首先将上一章节提前规划完成的路径信息进行分段，确定软体臂段数。其次，利用2.2 节所提出的自生长模式逆运动学，使每一段运动到指定位置。通过每一段的生长运动使机械臂到达终点。如图5 所示，根据起点距离终点的长度信息，在实验中只需要软体臂前面两大段。表1 为避障实验中各段的关节信息。这种工作模式的优点在于可根据任务要求决定使用具体的软体臂段数，每一段之间独立控制不存在耦合情况。

图5 自生长工作模式下避障实验

表1 避障实验中各段参数信息

5 结语

针对软体机械臂提出一种顺序建模和控制方法，使机械臂能够模仿藤蔓生长的过程，每段先伸长后弯曲，避免机械臂运动学上的强耦合，从而实现在非结构化环境中避障。具体地，利用改进型RRT 算法在非结构化障碍物空间中寻找一条路。软体机械臂将按照此路径，分段顺序运动进而实现避障。在仿真环境中实现了运动学分析及避障算法的验证。

由于软体臂在伸长过程中是采取的先伸长后弯曲的控制策略，在今后的工作中我们希望能够在伸长的同时实现弯曲，使得整个软体机械臂运动地更加柔顺，在未来的工作里我们将搭建实物软体臂来进行仿真和实验的相互验证。