可伸缩蛇形臂机器人的设计及运动学建模

Design and Kinematics Modeling of Extensible Snake-like Manipulators

摘要

提出一种新型蛇形臂机器人，该机器人模块化关节具有主动的可伸缩自由度，增加了整机运动的灵活性和复杂空间适应能力。建立了蛇形臂机器人的运动学模型，分析了驱动空间、关节空间与末端操作空间的映射关系。针对关节具有伸缩自由度的构型特点，提出了两种关节-末端逆运动学算法，即结合传统FABRIK算法与基于雅可比矩阵迭代法的组合算法、杆长迭代更新的改进FABRIK算法。为了验证两种算法的计算精度和效率，进行了数值仿真对比，结果表明两种算法均具有良好的运算精度，改进的FABRIK算法具有更高的运算效率。最后搭建了物理样机并开展了实验验证，证明了所提出蛇形臂机器人具有良好的弯曲性能和伸缩运动性能。

Abstract

A novel snake-like manipulator was proposed， the modular joints of the manipulator were designed with an active extensible degree of freedom， which increased the overall motion flexibility and adaptability in complex spaces. Kinematics modeling of the snake-like manipulators was carried out. The mappings among actuator space， joint space and end task space were analyzed. Considering the characteristics of the extensible joints， two novel joint-end inverse kinematics algorithms were proposed， i.e. the integrated method by combing the conventional FABRIK algorithm with Jacobian-based iterative algorithm， and the modified FABRIK algorithm with iteratively updated link lengths. Numerical simulations were conducted to verify computational accuracy and efficiency. The results show that both methods have better accuracy， while the modified FABRIK algorithm has higher computational efficiency. Finally， a prototype was constructed， and experiments were carried out to validate motion capabilities of the proposed snake-like manipulators.

Graphical abstract

关键词

蛇形臂机器人 / 结构设计 / 逆运动学 / 改进FABRIK算法 / 实验验证

Key words

snake-like manipulator / structure design / inverse kinematics / improved FABRIK algorithm / experimental validation

Author summay

王旭浩，男，1989年生，博士，讲师。研究方向为机器人机构学和民航特种智能装备。E-mail：[e-mail]。

0 引言

蛇形臂机器人是基于仿生学的超冗余特种机器人，具有运动灵活、结构紧凑和工作空间大等优点，适于非结构化和受限空间作业，在医疗手术、核管道检测、飞机/航空发动机检测与维修等领域具有广阔的应用前景^［1-3］。

蛇形臂机器人通常由多个模块化关节和连杆串联组成，其结构形式可以划分为连续型^［4-5］和离散型^［6-7］两种。其中，连续型机器人采用柔性材料作为关节与连杆，可以模拟蛇的连续变形特性，具有良好的运动灵活性和柔韧性^［8］。但是由于自由度无限和固有的柔性，连续型机器人的承载能力、定位精度有限。离散型蛇形臂机器人则由刚性的关节（如虎克铰、球铰等）和连杆组成，具有更高的静刚度、运动精度以及承载能力^［9］，因此，OC Robotics和新松公司的商业化蛇形臂机器人均采用刚性虎克铰关节与连杆的构型方案^［10］。为了提高离散型机器人的运动柔性，学者们通过集成刚性关节与柔性骨架，提出了多种具有刚柔耦合关节的蛇形臂机器人，兼具连续型和离散型蛇形臂机器人的特性^［11-13］。但是，如何权衡机器人的运动灵活性和承载能力依然有待研究。此外，关节和连杆的尺寸影响机器人的狭小空间通过能力和工作空间大小，即较短的连杆尺寸有利于提高狭小空间适应性，而较长的连杆尺寸有利于增大工作空间范围。XU等^［14］设计了一种具有三自由度主体模块的蛇形机械臂，采用弹簧和驱动绳实现连杆的被动伸缩。ZHUANG等^［15］采用气动肌肉和弹性杆复合驱动设计了可伸缩的连续型机器人。受弹簧等柔性件刚度的影响，现有的被动伸缩蛇形臂机器人的负载能力和运动精度受限制。设计具有主动伸缩自由度的蛇形臂机器人已成为研究热点^［16-17］。

运动学是机器人控制的基础。超冗余的蛇形臂机器人逆运动学求解比较困难。目前，常用的逆运动学求解算法主要包括基于雅可比矩阵的数值迭代法^［18］、智能搜索法^［19］和几何法^［20］等。其中，基于雅可比矩阵的迭代法广泛用于超冗余串联机器人的逆运动学，并可以通过零空间运动解决避障等问题^［21］，但是该方法对初始解和奇异位姿较为敏感。基于启发式智能搜索法将机器人逆运动学求解问题转化为求极值问题，容易陷入局部极值。FABRIK算法是一种广泛应用的几何法^［22］，可以很好地解决初始解敏感以及奇异问题。目前，FABRIK算法主要用于求解不可伸缩或固定杆长的蛇形臂机器人的逆运动学，针对新型可伸缩蛇形臂机器人的逆运动学求解方法尚待进一步研究。

本文提出了一种主动可伸缩蛇形臂机器人的结构方案，建立了机器人的运动学模型，提出了两种面向可伸缩蛇形臂机器人的关节-末端逆运动学算法，并进行了仿真分析与对比。搭建了实验样机并对蛇形臂机器人的运动性能进行了实验验证。

1 可伸缩蛇形臂机器人结构设计

蛇形臂机器人通常采用多个模块化的虎克铰关节和刚性连杆依次串联组成，每个虎克铰关节可以实现两个转动自由度，刚性连杆的长度一般是固定的。需要指出的是，在一些特定的场合，需要蛇形臂具有关节伸缩自由度来提高运动的灵活性和适应性。例如，较短的连杆长度有利于提高狭小空间通过能力，而较长的连杆尺寸有利于增大工作空间范围。因此，本文在虎克铰关节蛇形臂机器人的基础上提出了一种可主动伸缩的蛇形臂机器人，主要包括蛇形臂和绳驱动系统两个部分，如图1所示。

蛇形臂由多个模块化关节单元串联组成，是机器人的核心运动部分。其中，每个关节单元在虎克铰的基础上增加一个主动伸缩自由度，形成了可伸缩三自由度关节单元（两个转动自由度和一个伸缩自由度），如图2a所示。由图2b可知，主动伸缩自由度通过将传统的刚性连杆改为前后分体式，并增加直线轴承和导柱实现。此外，为了主动控制三个运动自由度，每个关节单元需要设置4根驱动绳。其中，3根驱动绳协调控制实现虎克铰的两个转动自由度，定义这3根驱动绳为转动驱动绳。另外1根驱动绳通过滑轮转向，并配合3根转动驱动绳，可以实现关节的伸缩自由，定义该驱动绳为伸缩驱动绳。

综上所述，每个可伸缩关节单元具有三个运动自由度，需要4根驱动绳。因此，为实现n（n=3）关节蛇形臂的精确控制，设计的绳驱动系统包含12个驱动模块，每个驱动模块包含驱动电机、联轴器、直线模组和滑轮。通过控制12根驱动绳运动，实现蛇形臂9个转动/伸缩运动自由度。

2 蛇形臂机器人运动学建模

可伸缩蛇形臂机器人的运动学模型旨在建立机器人驱动空间（绳长）、关节空间（关节转角和伸缩量）与末端操作空间（末端位姿）的映射关系。为表述清晰，将可伸缩蛇形臂机器人的运动学模型分为两个子运动学模型，即驱动-关节运动学模型与关节-末端运动学模型进行推导。

2.1 驱动-关节运动学模型

驱动-关节运动学模型要求建立驱动空间与关节空间映射关系，即给定关节转角和伸缩量求绳长（逆运动学），或已知绳长求关节转角和伸缩量（正运动学）。蛇形臂为多个关节单元的串联，因此，首先对单个关节单元i（i=\mathrm{1,2},\mathrm{3}）进行分析。可伸缩关节单元包括转动自由度和伸缩自由度。

对于关节转动自由度，需要3根转动驱动绳进行驱动，建立转动关节的局部坐标系｛2i-1｝和｛2i｝，如图3所示。其中，x{}_{\mathrm{2}i-\mathrm{1}}轴平行于回转关节θ{}_{\mathrm{2}i-\mathrm{1}}轴线，z{}_{\mathrm{2}i-\mathrm{1}}轴垂直于下连杆端面，y{}_{\mathrm{2}i-\mathrm{1}}轴由右手定则确定。y_2i 轴平行于回转关节θ_2i 的轴线，z_2i 轴垂直于上连杆端面，x_2i 轴由右手定则确定。这样转动驱动绳长可以由关节前后端面的绳孔距离l_{A_{i,j}{B}_{i,j}}表示，其中，j为绳的编号，j=\mathrm{1,2},\mathrm{3}。

根据矩阵变换，坐标系｛2i-1｝到｛2i｝的齐次变换矩阵为

{}_{\mathrm{2}i-\mathrm{1}}{}^{\mathrm{2}i}\mathit{T}=\mathrm{T}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}(z,h)\mathrm{R}\mathrm{o}\mathrm{t}(x,{\theta }_{\mathrm{2}i-\mathrm{1}})\mathrm{R}\mathrm{o}\mathrm{t}(y,{\theta }_{\mathrm{2}i})\mathrm{T}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}(z,h)（1）

式中：Trans（z，h）表示沿z轴平移距离h；Rot（x，θ{}_{\mathrm{2}i-\mathrm{1}}）表示绕x轴转动θ{}_{\mathrm{2}i-\mathrm{1}}；θ{}_{\mathrm{2}i-\mathrm{1}}、θ_2i 为两个关节转角。

点A_i，j 在坐标系｛2i-1｝中的坐标表示为

{\mathit{A}}_{i,j}^{o_{\mathrm{2}i-\mathrm{1}}}=(r\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_{i,j},r\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{i,j}{,\mathrm{0,1})}^{\mathrm{{\rm T}}}（2）

式中：φ_i，j 为第j个驱动绳与坐标系｛2i-1｝的x轴的夹角；r为绳孔分布圆半径。

同理，点B_i，j 在坐标系｛2i｝中的坐标表示为

{\mathit{B}}_{i,j}^{o_{\mathrm{2}i}}=(r\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_{i,j},r\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_{i,j}{,\mathrm{0,1})}^{\mathrm{T}}（3）

根据坐标变换关系，点B_i，j 在坐标系｛2i-1｝中的坐标表示为

{\mathit{B}}_{i,j}^{o_{\mathrm{2}i-\mathrm{1}}}={}_{\mathrm{2}i-\mathrm{1}}{}^{\mathrm{2}i}\mathit{T}{\mathit{B}}_{i,j}^{o_{\mathrm{2}i}}（4）

则联立式（2）和式（4）可得关节转动引起转动驱动绳长变化量为

\mathrm{\Delta }{l}_{i,j}^{\mathrm{r}}=‖{\mathit{B}}_{i,j}^{o_{\mathrm{2}i-\mathrm{1}}}-{\mathit{A}}_{i,j}^{o_{\mathrm{2}i-\mathrm{1}}}‖-\mathrm{2}h（5）

对于关节伸缩自由度，需要1根伸缩驱动绳和3根转动驱动绳耦合驱动，如图4所示。绳长变化量\mathrm{\Delta }{l}_{i,j}^{\mathrm{e}}与关节伸缩量s_i间的关系为

\mathrm{\Delta }{l}_{i,j}^{\mathrm{e}}=\left\{\begin{array}{l}{s}_{i}j=\mathrm{1,2},\mathrm{3}\\ -s_{i}j=\mathrm{4}\end{array}\right.（6）

根据蛇形臂的结构特点，第i个关节单元的驱动绳也会通过第i-1个关节单元的绳孔，即各关节单元驱动绳长度存在耦合关系。考虑耦合影响，各关节单元驱动绳的长度可以表示为

l_{i,j}=\left\{\begin{array}{l}{l}_{i,j}^{\mathrm{0}}+{\displaystyle \sum _{m=\mathrm{1}}^i}(\mathrm{\Delta }{l}_{m,j}^{\mathrm{r}}+\mathrm{\Delta }{l}_{m,j}^{\mathrm{e}})j=\mathrm{1,2},\mathrm{3}\\ l_{i,j}^{\mathrm{0}}+\mathrm{\Delta }{l}_{i,j}^e+{\displaystyle \sum _{m=\mathrm{0}}^{i-\mathrm{1}}}(\mathrm{\Delta }{l}_{m,j}^{\mathrm{r}}+\mathrm{\Delta }{l}_{m,j}^{\mathrm{e}})j=\mathrm{4}\end{array}\right.（7）

式中：l_{i,j}^{\mathrm{0}}为驱动绳的初始长度，\mathrm{\Delta }{l}_{\mathrm{0},j}^{\mathrm{r}}=\mathrm{\Delta }{l}_{\mathrm{0},j}^{\mathrm{e}}=\mathrm{0}。

式（7）为驱动-关节逆运动学模型。对于驱动-关节正运动学模型，各驱动绳长为已知，通过求解式（7），可以从第一关节单元至第三关节单元依次递推求得关节转角和伸缩量。

2.2 关节-末端运动学模型

关节-末端运动学模型要求建立关节空间（关节转角和伸缩量）与末端操作空间（末端位姿）间的映射关系。为了推导方便，建立图5所示的关节坐标系，则相邻关节单元间变换矩阵为：坐标系｛2i-2｝沿z轴移动d+s_i 到坐标系｛2i-1｝，坐标系｛2i-1｝经变换（式（1））到坐标系｛2i｝，其中，d为伸缩连杆的初始长度。

根据矩阵变换原理，关节-末端正运动学即末端坐标系｛6｝在基础坐标系｛0｝中的位姿矩阵为

{\mathit{T}}_{\mathrm{e}\mathrm{n}\mathrm{d}}={}_{\mathrm{0}}{}^{\mathrm{1}}\mathit{T}{}_{\mathrm{1}}{}^{\mathrm{2}}\mathit{T}{}_{\mathrm{2}}{}^{\mathrm{3}}\mathit{T}{}_{\mathrm{3}}{}^{\mathrm{4}}\mathit{T}{}_{\mathrm{4}}{}^{\mathrm{5}}\mathit{T}{}_{\mathrm{5}}{}^{\mathrm{6}}\mathit{T}=\mathit{T}(\mathit{q})（8）

其中，{}_{\mathrm{2}i-\mathrm{2}}{}^{\mathrm{2}i-\mathrm{1}}\mathit{T}=\mathrm{T}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}(z,d+s_i)，{}_{\mathrm{2}i-\mathrm{1}}{}^{\mathrm{2}i}\mathit{T}即式（1），i=\mathrm{1,2},\mathrm{3}， q 为独立关节向量， q =（q₁，q₂，q₃，q₄，q₅，q₆，q₇，q₈，q₉）^T=（s₁，θ₁，θ₂，s₂，θ₃，θ₄，s₃，θ₅，θ₆）^T。

对于超冗余蛇形臂机器人的逆运动学，目前主要有基于雅可比矩阵的迭代法和FABRIK算法。其中，基于雅可比矩阵的迭代法广泛应用于串联机器人的逆运动学，但它对初始解比较敏感，无法解决多解和奇异问题。FABRIK算法则是一种典型的几何算法，计算效率高，不存在奇异问题，但现有FABRIK算法主要针对固定杆长蛇形臂机器人的逆运动学，因此，针对可伸缩蛇形臂机器人，本文提出两种关节-末端逆运动学算法：FABRIK算法与雅可比矩阵迭代法的组合算法和杆长更新的改进FABRIK算法。

2.2.1 逆运动学组合算法

如图6所示，逆运动学组合算法是一种两步求解法：第一步，采用FABRIK算法计算逆运动学近似解 q₀；第二步，以 q₀为初始解，采用基于雅可比矩阵的数值迭代法求解逆运动学精确解。由于连杆伸缩自由度只影响机器人末端位置精度，并可以根据位置误差对杆长进行近似补偿，因此，FABRIK算法能为雅可比迭代法提供较好的初始解，提高组合算法的计算精度和效率。

2.2.1.1 FABRIK算法求近似解

FABRIK算法将逆运动学转化为迭代寻找关节中心节点的问题，其核心迭代流程如图7所示，主要包括向前接近迭代（图7a）和向后接近迭代（图7b）。需要指出的是，为了满足FABRIK算法的假设条件，这里忽略连杆的伸缩自由度，即杆长为定值。图7a的①中给出了蛇形臂的初始臂型（其中{\mathit{N}}_i(i=\mathrm{1,2},\mathrm{3})为关节中心节点，{\mathit{N}}_{\mathrm{0}}为基础坐标系原点，{\mathit{N}}_{\mathrm{4}}为末端参考点）和期望的末端目标位置向量 P_t和方向向量{\mathit{e}}_{\mathrm{t}}（ P_t、{\mathit{e}}_{\mathrm{t}}可以由给定末端位姿矩阵 T_t求得）。

对于向前接近迭代，首先将蛇形臂末端节点移动到目标位置 P_t，并记作 N'₄。末端连杆的单位方向向量与{\mathit{e}}_{\mathrm{t}}平行，可以表示为

{\mathit{e}}_{\mathrm{4}}^{\text{'}}={\mathit{e}}_{\mathrm{t}}（9）

由末端向前更新其他节点位置向量：

{\mathit{N}}_i\mathrm{\text{'}}={\mathit{N}}_{i+\mathrm{1}}^{\text{'}}-L_{i+\mathrm{1}}{\mathit{e}}_{i+\mathrm{1}}^{\text{'}},\mathrm{1}\le i\le n（10）

其中，n为蛇形臂关节单元数量，n=3；L_i为连杆i的长度，L_i=‖{\mathit{N}}_i-{\mathit{N}}_{i-\mathrm{1}}‖；{\mathit{e}}_i^{\text{'}}为更新后连杆i的单位方向向量，具体表示为

{\mathit{e}}_i^{\text{'}}=\left\{\begin{array}{l}{(\mathrm{0,0},\mathrm{1})}^{T}i=\mathrm{1}\\ ({\mathit{N}}_i^{\text{'}}-{\mathit{N}}_{i-\mathrm{1}})/‖{\mathit{N}}_i^{\text{'}}-{\mathit{N}}_{i-\mathrm{1}}‖i=\mathrm{2,3}\\ {\mathit{e}}_{t}i=n+\mathrm{1}\end{array}\right.（11）

如图7a中④所示，向前接近迭代完成后，{\mathit{N}}_{\mathrm{1}}^{\text{'}}和{\mathit{N}}_{\mathrm{1}}不重合，需进一步完成向后接近迭代。向后接近迭代由第一关节点向末端逐一更新节点坐标和连杆方向向量。首先，将第一关节点与{\mathit{N}}_{\mathrm{1}}重合，并记作{\mathit{N}}_{\mathrm{1}}^{″}，如图7b中①所示。连杆1的单位方向向量为{\mathit{e}}_{\mathrm{1}}^{″}={(\mathrm{0,0},\mathrm{1})}^{\mathrm{T}}。则其他节点位置和连杆单位方向向量可以迭代计算为

{\mathit{N}}_i^{″}={\mathit{N}}_{i-\mathrm{1}}^{″}+L_i{\mathit{e}}_i^{″}\mathrm{1}\le i\le n+\mathrm{1}（12）

{\mathit{e}}_i^{″}=\left\{\begin{array}{l}({\begin{array}{ccc}\mathrm{0}& \mathrm{0}& \mathrm{1})\end{array}}^{T}i=\mathrm{1}\\ ({\mathit{N}}_i^{\text{'}}-{\mathit{N}}_{i-\mathrm{1}}^{″})/‖{\mathit{N}}_i^{\text{'}}-{\mathit{N}}_{i-\mathrm{1}}^{″}‖i=\mathrm{2,3}\\ {\mathit{e}}_{t}i=n+\mathrm{1}\end{array}\right.（13）

至此，完成一次向前接近迭代和向后接近迭代循环。随着迭代循环次数的增加，蛇形臂各节点和连杆方向向量逐渐收敛得到最终臂型。FABRIK算法迭代终止条件为：关节转角增量小于给定阈值或达到最大迭代次数，即

（14）

为计算关节转角增量并最终求解蛇形臂机器人运动学逆解，需要根据蛇形臂的臂型即节点位置向量和连杆方向向量计算各关节转角。首先，{\mathit{e}}_{\mathrm{2}}^{″}可以表示在基础坐标系｛0｝下z₂轴的方向向量，根据矩阵变换可得

{\mathit{e}}_{\mathrm{2}}^{″}=\left[\begin{array}{c}{e}_{\mathrm{2}x}^{″}\\ e_{\mathrm{2}y}^{″}\\ e_{\mathrm{2}z}^{″}\end{array}\right]={}_{\mathrm{1}}{}^{\mathrm{2}}\mathit{R}\left[\begin{array}{c}\mathrm{0}\\ \mathrm{0}\\ \mathrm{1}\end{array}\right]=\left[\begin{array}{c}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{2}}\\ -\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{1}}\\ \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{1}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{2}}\end{array}\right]（15）

式中：{}_{\mathrm{1}}{}^{\mathrm{2}}\mathit{R}为矩阵{}_{\mathrm{1}}{}^{\mathrm{2}}\mathit{T}（即式（1））的旋转子矩阵。

由式（15）左右对应元素相等，求得关节1转角为

\left.\begin{array}{l}{\theta }_{\mathrm{1}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}(-e_{\mathrm{2}y}^{″},e_{\mathrm{2}\mathrm{z}}^{″})\\ {\theta }_{\mathrm{2}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{s}\mathrm{i}\mathrm{n}(e_{\mathrm{2}x}^{″})\end{array}\right\}（16）

根据{\theta }_{\mathrm{1}}、{\theta }_{\mathrm{2}}和式（1），可以求得{}_{\mathrm{1}}{}^{\mathrm{2}}\mathit{R}，坐标系｛4｝的z₄轴相对坐标系｛2｝可以表示为

{}_{\mathrm{4}}^{\mathrm{2}}{\mathit{e}}^{″}={(}_{\mathrm{4}}^{\mathrm{2}}{e}_x^{″}{,}_{\mathrm{4}}^{\mathrm{2}}{e}_y^{″}{,}_{\mathrm{4}}^{\mathrm{2}}{e}_z^{″}{)}^{\mathrm{{\rm T}}}{=}_{\mathrm{1}}^{\mathrm{2}}{\mathit{R}}^{-\mathrm{1}}{\mathit{e}}_{\mathrm{3}}^{″}（17）

根据矩阵变换，{}_{\mathrm{4}}{}^{\mathrm{2}}{\mathit{e}}^{″}还可以表示为关节2转角的函数形式，即

{}_{\mathrm{4}}^{\mathrm{2}}{\mathit{e}}^{″}=(\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{4}},-\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{4}}\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{\mathrm{3}},\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{3}}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{\mathrm{4}}{)}^{\mathrm{{\rm T}}}（18）

联立式（17）和式（18），求得关节2转角为

\left.\begin{array}{l}{\theta }_{\mathrm{3}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}({-}_{\mathrm{4}}^{\mathrm{2}}{e}_y^{″}{,}_{\mathrm{4}}^{\mathrm{2}}{e}_z^{″})\\ {\theta }_{\mathrm{4}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{s}\mathrm{i}\mathrm{n}{(}_{\mathrm{4}}^{\mathrm{2}}{e}_x^{″})\end{array}\right\}（19）

同理，可以求得关节3转角{\theta }_{\mathrm{5}}和{\theta }_{\mathrm{6}}。

此外，由于受虎克铰关节结构的限制，关节转角约束在一定范围，即迭代过程中关节偏转角满足约束关系{\gamma }_i\le {\gamma }_{\mathrm{m}\mathrm{a}\mathrm{x}}。其中，关节偏转角可以由连杆方向向量求得：

{\gamma }_i=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{c}\mathrm{o}\mathrm{s}(\frac{{\mathit{e}}_i^{″}\cdot {\mathit{e}}_{i+\mathrm{1}}^{″}}{‖{\mathit{e}}_i^{″}‖‖{\mathit{e}}_{i+\mathrm{1}}^{″}‖})i=\mathrm{1,2},\mathrm{3}（20）

以向前接近迭代为例，关节转角极限回避方法如图8所示。当关节偏转角{\gamma }_{\mathrm{3}}超过极限值时，应将其调整到γ_max。修正后连杆3方向向量{\tilde{\mathit{e}}}_{\mathrm{3}}^{\mathrm{\text{'}}}可视为{\mathit{e}}_{\mathrm{4}}^{\text{'}}绕 μ 旋转γ_max的结果，即

{\tilde{\mathit{e}}}_{\mathrm{3}}^{\mathrm{\text{'}}}=\mathrm{t}\mathrm{r}\mathrm{o}\mathrm{t}(\mathit{\mu },{\gamma }_{\mathrm{m}\mathrm{a}\mathrm{x}}){\mathit{e}}_{\mathrm{4}}^{\text{'}}（21）

\mathit{\mu }={\mathit{e}}_{\mathrm{4}}^{\text{'}}\times {\mathit{e}}_{\mathrm{3}}^{\text{'}}

2.2.1.2 基于雅可比矩阵迭代求精确解

以FABRIK算法求得逆运动学近似解为初始解 q₀，由式（8）求得当前末端位姿矩阵 T_a。根据微分原理，关节增量与末端位姿误差间存在关系：

\mathrm{\delta }\mathit{T}=\mathit{J}\mathrm{\Delta }\mathit{q}（22）

\mathrm{\Delta }\mathit{q}=(\mathrm{\Delta }{q}_{\mathrm{1}},\mathrm{\Delta }{q}_{\mathrm{2}},\cdots ,\mathrm{\Delta }{q}_{\mathrm{9}}{)}^{\mathrm{T}}

\mathrm{\delta }\mathit{T}=(\mathrm{\delta }x,\mathrm{\delta }y,\mathrm{\delta }z,\mathrm{\delta }{\omega }_x,\mathrm{\delta }{\omega }_y,\delta {\omega }_z{)}^{\mathrm{{\rm T}}}

式中：\mathrm{\Delta }\mathit{q}为关节变量的增向量； J 为雅可比矩阵； \mathrm{\delta }\mathit{T}为末端位姿误差向量，可由 T_a和 T_t求解^［18］。

当 J 可逆时，\mathrm{\Delta }\mathit{q}表示为

\mathrm{\Delta }\mathit{q}={\mathit{J}}^{-\mathrm{1}}\mathrm{\delta }\mathit{T}（23）

关节变量 q 的迭代公式为

\mathit{q}={\mathit{q}}_{\mathrm{0}}+\mathrm{\Delta }\mathit{q}（24）

迭代终止条件为：末端位姿误差小于给定阈值{\epsilon }_{\mathrm{2}}或达到最大迭代次数，即

（25）

\mathrm{d}\mathit{p}={(\mathrm{\delta }x,\mathrm{\delta }y,\mathrm{\delta }z)}^{\mathrm{T}}\mathrm{d}\mathit{r}=(\mathrm{\delta }{\omega }_x,\mathrm{\delta }{\omega }_y{)}^{\mathrm{T}}

式中：\mathrm{d}\mathit{p}、\mathrm{d}\mathit{r}分别为位置和姿态误差向量。

需要指出的是，虎克铰关节蛇形臂末端只有5个自由度，因此不考虑绕z轴转角误差。

2.2.2 杆长更新的改进FABRIK算法

采用常规FABRIK算法求解可伸缩蛇形臂机器人逆运动学时，由于无法考虑杆长变化，最终迭代结果可能会存在位置误差，即{\mathit{N}}_{\mathrm{4}}^{″}与 P_t不重合。对此，本文提出了一种杆长迭代更新的改进FABRIK算法，算法流程见图9。

相较常规FABRIK算法，改进的FABRIK算法在完成一个向前接近迭代和向后接近迭代循环后，需要根据末端位置误差d p 对连杆长度或伸缩量进行修正补偿，补偿量满足关系式：

\mathrm{\delta }{s}_{\mathrm{1}}+\mathrm{\delta }{s}_{\mathrm{2}}+\mathrm{\delta }{s}_{\mathrm{3}}=‖\mathrm{d}\mathit{p}‖（26）

式中：\mathrm{\delta }{s}_i(i=\mathrm{1,2},\mathrm{3})为可伸缩连杆的伸缩量。

需要指出的是，根据式（26），杆长总补偿量为末端位置误差，各连杆的具体伸缩量要根据避障等需求确定。为了推导简单，这里设各杆长补偿量相等。

此外，得到补偿量后，各连杆是伸长还是缩短需要根据蛇形臂末端节点与目标点的位置关系确定。如图10所示，当末端位置误差向量{\mathit{e}}_{\mathrm{m}}={\mathit{P}}_{\mathrm{t}}-{\mathit{N}}_{\mathrm{4}}^{″}与末端目标方向向量{\mathit{e}}_{\mathrm{t}}的夹角ξ为锐角时，连杆需要伸长；当{\mathit{e}}_{\mathrm{m}}与{\mathit{e}}_{\mathrm{t}}的夹角ξ为钝角时，连杆需要缩短；当\xi =\mathrm{90}°时，连杆长度不变。修正后的杆长伸缩量表示为

s_i=s_{i,\mathrm{0}}+\mathrm{s}\mathrm{g}\mathrm{n}({\mathit{e}}_{\mathrm{m}}\cdot {\mathit{e}}_{\mathrm{t}})‖\mathrm{d}\mathit{p}‖/\mathrm{3}（27）

式中：s_{i,\mathrm{0}}为连杆初始伸缩量。

迭代终止条件为：末端位置误差小于给定阈值或达到最大迭代次数，即

（28）

3 运动学仿真分析与验证

3.1 驱动-关节运动学

为验证驱动-关节运动学模型的正确性，在Solidworks软件中建立关节单元仿真模型。考虑到关节伸缩自由度引起绳长的计算简单（式（6）），因此，仅分析关节1的两转角{\theta }_{\mathrm{1}}、{\theta }_{\mathrm{2}}与3根转动驱动绳长间的映射关系（式（5））。为了分析方便，令{\theta }_{\mathrm{1}}={\theta }_{\mathrm{2}}，它们随关节转角由0°逐渐增加到最大值30°，软件仿真测得绳长变化量\mathrm{d}{l}_{i,j}^{\mathrm{s}}与理论计算绳长变化量\mathrm{d}{l}_{i,j}^{\mathrm{t}}关系如图11所示。

由图11可得，理论计算各驱动绳长变化量与仿真测量值一致性较高，最大误差E_i，j 小于5 nm。仿真结果验证了所提出驱动-关节运动学模型即式（5）的正确性。

3.2 关节-末端运动学

对于关节-末端运动学，正运动学模型（式（8））相对简单，在下述仿真中认为是准确的。为了验证逆运动学模型的正确性，首先给定末端目标位姿矩阵{\mathit{T}}_{\mathrm{t}}，并采用逆运动学模型求解关节变量 q；然后采用运动学正解模型求解理论末端位姿矩阵{\mathit{T}}_{\mathrm{c}}；最后对比{\mathit{T}}_{\mathrm{t}}与{\mathit{T}}_{\mathrm{c}}，得到逆运动学算法的计算误差。

为了方便分析，首先给定目标关节向量{\mathit{q}}_{\mathrm{t}}={(\mathrm{9}\mathrm{m}\mathrm{m},\mathrm{6}°,\mathrm{17}°,\mathrm{14}\mathrm{m}\mathrm{m},\mathrm{18}°,\mathrm{20}°,\mathrm{12}\mathrm{m}\mathrm{m},\mathrm{14}°,\mathrm{10}°)}^{\mathrm{T}}，目标位姿矩阵 T_t由式（8）求得。作为对比，采用基于雅可比矩阵的数值迭代法、组合算法和改进的FABRIK算法进行逆运动学求解。在相同终止条件下，各算法的计算时间t如表1所示。其中，基于雅可比矩阵数值迭代法的初始解一般随机选择，为了方便，这里以蛇形臂的零位作为迭代的初始解，组合算法则以第一步FABRIK算法求得近似解为初始解。

由表1可知，本文提出的组合算法和改进FABRIK算法的运算效率比常规基于雅可比矩阵的迭代法高。其中，组合算法相较基于雅可比矩阵的迭代法具有更好的初始解，而改进FABRIK算法可以避免雅可比矩阵求逆具有更高效率。

不失一般性，随机选取1000组目标关节向量来验证所提出的逆运动学算法在机器人整个可用工作空间内的精度和效率。采用组合算法和改进FABRIK算法进行逆运动学求解，其位置误差\mathrm{d}p=‖{(\mathrm{\delta }x,\mathrm{\delta }y,\mathrm{\delta }z)}^{\mathrm{T}}‖、姿态误差\mathrm{d}r=‖(\mathrm{\delta }{\omega }_x,\mathrm{\delta }{\omega }_y{)}^{\mathrm{T}}‖以及计算时间t分别如图12～图14所示。

由图12和图13可知，组合算法和改进FABRIK算法的最大位置误差为\mathrm{0.01}\mathrm{m}\mathrm{m}，最大姿态误差分别小于3×10^-3（°）和2.5×10^-13（°）。在相同终止条件下，组合算法和改进FABRIK算法均具有较高的计算精度，且改进FABRIK算法可以获得更高的姿态精度。由图14可知，组合算法和改进FABRIK算法的平均计算时间分别为7.7 ms和1.4 ms，改进FABRIK算法具有更高的计算效率。

4 样机研制与运动性能实验

为了进一步验证所提出的可伸缩蛇形臂机器人结构设计的合理性及运动学模型的正确性，制作了具有三个关节单元的可伸缩蛇形臂机器人样机，如图15所示。蛇形臂最大直径为55 mm，最小长度为353 mm，关节单元的最大伸长量为30 mm，最大关节转角为30˚。

基于蛇形臂机器人样机开展运动性能实验，并采用NOKOV光学三维动作捕捉系统进行测量，系统最大测量误差为0.2 mm。如图16所示。为了同时测量位置和姿态，在每个关节单元末端设置了3个反光标志点，关节单元末端坐标系原点及姿态可由3个反光标志点中心位置求得^［18］。

首先进行蛇形臂在o_{\mathrm{0}}{x}_{\mathrm{0}}{z}_{\mathrm{0}}平面内的均匀弯曲实验。蛇形臂末端绕y₀轴弯曲角从0˚~90˚的运动过程如图17所示。实验测量蛇形臂末端位置误差及姿态误差如图18所示。由图18可得，最大末端位置误差为3.56 mm，占总臂长的1.10%；最大姿态误差为1.05˚，占总弯曲角的1.17%。实验结果证明所设计蛇形臂机器人具有良好的弯曲性能。

然后进行蛇形臂的伸缩实验，蛇形臂沿z_{\mathrm{0}}轴方向由初始臂长L_total伸长54 mm，如图19所示。运动过程中，测量蛇形臂末端位置误差如图20所示。由图20可得，最大末端位置误差为3.77 mm，占总伸长量的6.98%。实验结果表明，所设计的蛇形臂机器人具有良好的伸缩性能。

最后进行了蛇形臂的弯曲与伸缩运动结合实验，如图21所示。最大末端位置误差为3.85 mm，最大姿态误差为0.75°。

以上实验结果证明本文设计的可伸缩蛇形臂机器人具有良好的弯曲性能和伸缩性能。蛇形臂的运动误差主要源于加工制造误差、绳长弹性变形、关节间摩擦以及绳孔间隙等。

5 结论

1）设计了一种可伸缩蛇形臂机器人，通过在虎克铰关节基础上引入主动可伸缩自由度提高了蛇形臂机器人的运动灵活性和适应性。

2）建立了蛇形臂机器人的运动学模型，分析了驱动空间、关节空间与末端操作空间的映射关系。特别是针对关节具有伸缩自由度的构型特点，提出了两种关节-末端逆运动学算法。通过仿真证明所提出的组合算法和改进FABRIK算法均具有良好的计算精度，最大位置误差小于0.01 mm，最大姿态误差小于3.0×10{}^{-\mathrm{3}}（°）；改进的FABRIK算法具有更高的运算效率，平均运算时间为1.4 ms。

3）搭建了具有三个关节单元的可伸缩蛇形臂机器人样机，通过实验验证了本文设计的蛇形臂机器人具有良好的弯曲性能与伸缩运动性能。

参考文献

[1] RUSSO M， SADATI S M H， DONG Xin. Continuum Robots： an Overview［J］. Advanced Intelligent Systems， 2023， 5（5）：2200367.

[2] WANG Fengxu， DONG Haodai， YAN Lei， et al. Development of a Linear Decoupling Cable-driven Manipulator with Independent Driving Joints［J］. Mechatronics， 2024， 100：103192.

[3] 潘琪琪， 罗静静， 王富豪， 等. 切口式单孔腹腔镜连续体手术机器人构型设计和优化［J］. 机械工程学报， 2023， 59（23）：55-67.

[4] PAN Qiqi， LUO Jingjing， WANG Hongbo， et al. Design and Optimization of Continuum Robot Configuration for Single-port Laparoscopic Surgery［J］. Journal of Mechanical Engineering， 2023， 59（23）：55-67.

[5] 李法民， 郑天江， 沈雯钧， 等. 绳驱动连续体机器人标定方法［J］. 中国机械工程， 2022， 33（2）：202-208.

[6] LI Famin， ZHENG Tianjiang， SHEN Wenjun， et al. Calibration Method for Cable-driven Continuum Robots［J］. China Mechanical Engineering， 2022， 33（2）：202-208.

[7] 吉爱红， 刘荣兴， 陈辉， 等. 多关节连续体机器人的运动分析与遥操作技术［J］. 中国机械工程， 2021， 32（23）：2883-2889.

[8] JI Aihong， LIU Rongxing， CHEN Hui， et al. Kinematics Analysis and Teleoperation Technology of Multi-joint Continuum Robots［J］. China Mechanical Engineering， 2021， 32（23）：2883-2889.

[9] ZHENG Yang， WU Baibo， CHEN Yuyang， et al. Design and Validation of Cable-driven Hyper-redundant Manipulator with a Closed-loop Puller-follower Controller［J］. Mechatronics， 2021， 78：102605.

[10] LI Junxiang， XU Wenfu， LI Wenshuo， et al. Design and Development of Composite Linkage Mechanism for Cable-driven Segmented Manipulator to Increase Synchronous Accuracy and Transmission Distance［J］. IEEE Robotics and Automation Letters， 2023， 9（1）：25-32.

[11] 齐飞， 张恒， 裴海珊， 等. 基于力传递模型的连续体机器人驱动误差补偿研究［J］. 农业机械学报， 2023， 54（1）：402-411.

[12] QI Fei， ZHANG Heng， PEI Haishan， et al. Drive Error Compensation of Continuum Manipulator Based on Force Transfer Model［J］. Transactions of the Chinese Society of Agricultural Machinery， 2023， 54（1）：402-411.

[13] YANG Taiwei， JIAN Huang， XU Wenfu， et al. Development of a Cable-driven Redundant Space Manipulator with Large Bending Angle by Combining Quaternion Joints and Segmented Coupled Linkages Mechanism［J］. Chinese Journal of Aeronautics， 2023， 36（11）：483-499.

[14] LIU Tong， WANG Xiaojie. Modeling and Analysis of Oblique-chamber and Symmetric Oblique-chamber Pneu-net Soft Actuators［J］. IEEE Robotics and Automation Letters， 2024， 9（10）：8682-8689.

[15] HONG Wuzhou， FENG Fan， XIE Lei， et al. A Two-segment Continuum Robot with Piecewise Stiffness for Maxillary Sinus Surgery and Its Decoupling Method［J］. IEEE/ASME Transactions on Mechatronics， 2022， 27（6）：4440-4450.

[16] WANG Mingfeng， DONG Xin， BA Weiming， et al. Design， Modelling and Validation of a Novel Extra Slender Continuum Robot for In-situ Inspection and Repair in Aeroengine［J］. Robotics and Computer-Integrated Manufacturing， 2021， 67：102054.

[17] 雷飞， 刘思宇， 廖峻北，等. 大负载作用下绳驱连续型机器人静力学建模分析［J］. 机械工程学报， 2024， 60（15）：28-37.

[18] LEI Fei， LIU Siyu， LIAO Junbei， et al. Static Modeling and Analysis of Cable-driven Continuum Robots under Large Load［J］. Journal of Mechanical Engineering， 2024， 60（15）：28-37.

[19] XU Dawei， LI En， LIANG Zize. Kinematics and Statics Analysis of a Novel Cable-driven Snake Arm Robot［C］∥2017 Chinese Automation Congress （CAC）. Jinan， 2017：439-444.

[20] ZHUANG Zheming， ZHANG Ze， GUAN Yuntao， et al. Design and Control of SLPM-based Extensible Continuum Arm［J］. Journal of Mechanisms and Robotics， 2022， 14（6）：061003.

[21] WANG Pengyuan， DENG Binchen， HE Zongbo， et al. Extensible Continuum Manipulator Toward In-situ Explosive Ordnance Disposal［J］. Journal of Mechanisms and Robotics， 2023， 15（5）：051013.

[22] MAVINKURVE U， KANADA A， TAFRISHI S A， et al. Geared Rod-driven Continuum Robot with Woodpecker-inspired Extension Mechanism and IMU-based Force Sensing［J］. IEEE Robotics and Automation Letters， 2023， 9（1）：135-142.

[23] WANG Xuhao， WANG Chengfa， WU Mengli， et al. Design and Kinematics of a Novel Continuum Robot Connected by Unique Offset Cross Revolute Joints［J］. Journal of Mechanisms and Robotics， 2024， 16：121003-1.

[24] WANG Xuhao， ZHANG Dawei， ZHAO Chen. The Inverse Kinematics of a 7R 6-degree-of-freedom Robot with Non-spherical Wrist［J］. Advances in Mechanical Engineering， 2017， 9（8）：1-11.

[25] MU Zonggao， YUAN Han， XU Wenfu， et al. A Segmented Geometry Method for Kinematics and Configuration Planning of Spatial Hyper-redundant Manipulators［J］. IEEE Transactions on Systems， Man， and Cybernetics：Systems， 2018， 50（5）：1746-1756.

[26] 万俊， 姚佳烽， 余亮， 等. 基于伪距离的冗余机器人避障算法［J］. 机械工程学报， 2020， 56（17）：59-70.

[27] WAN Jun， YAO Jiafeng， YU Liang， et al. Obstacle Avoidance Algorithm for Redundant Manipulators Based on Pseudo-distance Method ［J］. Journal of Mechanical Engineering， 2020， 56（17）：59-70.

[28] NIU Pingan， HAN Liang， HUANG Yunzhi， et al. Shape-controllable Inverse Kinematics of Hyper-redundant Robots Based on the Improved FABRIK Method［J］. Robotica， 2024， 42（1）：225-241.

基金资助

天津市自然科学基金多元投入项目(23JCQNJC00140)

中央高校基本科研业务费专项资金(3122024051)