0 引言
连续体操作臂(下文称为“连续臂”)是一种依靠柔性中心骨架，通过某种驱动方式使其弯曲成光滑连续曲线而产生运动的新型仿生操作臂[1]。连续臂常见的三种设计是连续操作臂、蛇形操作臂和同心管操作臂[2]，通常采用拉线[3]、记忆合金[4]或气压驱动。与传统机器人相比，连续型机器人具有灵巧度高、安全性好、非结构化环境适应能力强以及能够操作不同尺寸物体的特点。相对于由形状记忆聚合物[5]、导电聚合物[6]为本体材料的软体机器人，连续臂具有一定的负载能力和良好的可操控能力，在避障检测、微创手术[7]以及受限环境中的柔顺抓取[8]等场合显示出良好的实用价值。
HANNAN等[9]研制的由2组对抗线驱动的仿象鼻连续臂在弯曲运动与抓握操作中展现了其灵巧性，但中心骨架包含较多的刚性零件，在一定程度上限制了运动的灵活性、增加了臂重。耿仕能等[10]开发了以超镍钛合金为中央柔性骨架的线驱动可变刚度连续臂，样机本体由2个操作单元串联组成，具有4个自由度，碰撞时展现出良好的被动适应能力和柔顺性，但没有分析讨论冗余自由度连续臂的相关特性。方跃法等[11]设计出一种通过弹性杆驱动的非刚性连续体并联抓取机器人，该机器人能实现轴向伸缩、弯转与抓取操作，运动平台仅有3个自由度，运动灵活性有待提升。费斯托公司的轻量级气动操作臂Bionic Motion Robot[12]具有灵活操纵不同工具的能力，可安全地与人类共同加工工件，但流体驱动机器人的能源(泵站)系统有较大的工作噪声，且连续臂的设计难以实现高度集成化。
随着连续臂硬件的发展，其运动学理论模型的种类也在不断丰富。虚拟刚性链运动学和传统D-H参数法的组合方法将连续臂主干形状与工作坐标建立关联，获得了多段连续臂的运动学模型[13]。HANNAN等[9]在恒定曲率的假设下，基于Frenet-Serret(F-S)公式模型可精确积分的特点，获得了连续臂的正向运动学。但上述方法均未定义操作臂主干形状接近直线(零曲率)时的运动学映射。YANG等[14]采用有限元方法分析了柔性象鼻操作臂的变形过程，该方法具有良好的计算精度，也不存在零曲率位姿运动学模型不可靠的问题，但复杂的计算使其在高维系统模型中难以用于实时控制。MURRAY[15]从数学角度提供了刚性机器人运动学指数坐标的系统处理方法，该方法不仅可以简洁、精确地处理逆运动学问题，而且简化了多体机械系统的微分运动学分析过程。
本文以八自由度连续体内窥机械臂为例，研究冗余自由度连续臂的运动学建模和运动规划方法。该方法通过获得的操作臂全局运动学解析映射来提高多体串联连续臂运动规划的计算效率。考虑到连续臂形状接近直线(零曲率)时工作空间中微分运动学数值求解不可靠的问题，以及超冗余自由度操作臂的高维运动学模型在实时控制中的高效求解需求，提出了一种“双空间协调规划法”。八自由度连续体操作臂对小入口容器内表面的三维自动内窥操作验证了上述方法的有效性。
1 结构设计与分析
图 1所示的八自由度连续臂(下文简称“操作臂”)不仅能根据工作状态需要灵活地调节自身形状，而且允许根据不同的任务需求添加相应的执行器，完成自主窥探检测、缠绕、抓取等操作。
图1 连续体内窥操作臂样机
Fig.1 Prototype of continuum endoscopic manipulator
操作臂由4个相同的连续体模块(下文称为“操作单元”)串联组成，操作单元的机械结构如图 2所示。每个操作单元主要由中心柔性骨架(拉簧)、外部轮廓弹簧、支撑盘和驱动线组成。其中，柔性骨架为操作臂运动提供弯曲刚度；外部轮廓弹簧维持操作臂的外部轮廓；支撑盘周向均布12个穿线孔；每个操作单元的3根驱动线间隔120°；相邻两操作单元的驱动线逆时针相错30°。操作臂的支撑盘、柔性骨架及外部轮廓弹簧采用刚性连接，防止产生相对扭动；各支撑盘的穿线孔孔心保持竖直对齐，驱动线仅在所驱动单元内部裸露，其余部分套有聚乙烯材质的解耦套管。
图2 操作单元机械结构图
Fig.2 Mechanical structure diagram of manipulator’s unit
(a)弯曲运动模型
(b)周转运动模型
图3 弯转运动等效分解图
Fig.3 Equivalent exploded view of bending motion
操作臂结构设计中将操作单元串联与驱动线并联结合；每个操作单元采用冗余驱动方式，通过协调3根驱动线的长度，操作单元能完成竖直平面内的弯曲运动，如图3所示。柔性骨架的连续性和柔顺性使操作臂拥有连续变形能力、灵巧操作能力以及一定的负载能力，减小操作臂与环境意外碰触时的冲击力，提高内窥检测过程的安全性。拉线驱动使操作臂结构更紧凑，为进一步小型化、轻量化提供可能。驱动线套管解决了冗余自由度连续臂驱动线长度变化量与串联操作单元形变量之间的耦合问题，操作臂在运动过程中任意改变套管区的形状不会引起无套管区拉线长度的变化。
2 运动学分析
连续体机器人采用拉线驱动，这导致其关节空间与驱动空间不为同一空间。为方便运动学分析，将第i(i=1，2，3，4) 个操作单元的运动学参数划分至3个空间，即表征驱动线长度的驱动空间qi=[Li,1 Li,2 Li,3]T，反映机械臂形状的虚拟关节空间Θi=[θi φi]T，以及描述末端位置的工作空间Wi=[xi yi zi]T。如图 4所示，3个空间之间的映射关系分析即为操作臂的正逆运动学求解，图中的序号表示下文对操作臂各空间映射关系的介绍顺序。
图4 操作单元运动学空间关系
Fig.4 Kinematics spatial relationship description of
manipulator’s unit
如图5所示，在第1个操作单元的基部支撑盘与第i个操作单元的末端支撑盘上分别建立基础坐标系{O0}和局部坐标系{Oi}；将第i个操作单元的第j(j=1，2，3)根驱动线长度记为Li,j。
图5 定义坐标系与驱动线编号
Fig.5 Definition of coordinate system and driving
cables number
假设操作单元运动过程中，主干以恒定的曲率变形[3]，则第i个操作单元的运动模型如图6所示，其中，φi∈[0，2π]为第i个操作单元弯曲后，主干所在竖直平面与xi-1轴正方向的夹角，θi∈[0，π/2]为第i个操作单元的弯曲角度。依据操作单元柔性骨架在轴向不可伸长与压缩的特点，设第i个操作单元柔性骨架的长度为定值Si。操作单元在任意竖直平面内的弯曲运动能通过变量φi与θi描述，整体表现出2个自由度。
图6 操作单元弯转运动模型
Fig.6 Bending motion model of manipulator’s unit
2.1 关节空间到操作空间的映射
第i个操作单元发生弯曲运动时，坐标系{Oi-1}到坐标系{Oi}的变换可以通过连续的4个基本变换来实现：①将局部坐标{Oi-1}原点Oi-1平移Li至操作单元的末端坐标系{Oi}原点Oi；②将平移得到的坐标绕其Z轴旋转φi(逆时针旋转为正)；③将旋转得到的坐标系绕其Y轴旋转θi；④坐标系绕新得的Z轴旋转-φi。按上述过程将相应的旋量坐标写为
(1)
式中，ξ1为移动副的运动旋量，是节距无穷大的单位运动旋量；ξ2～ξ4为转动副的运动旋量。
依据串联机械臂运动学正解的指数积公式，可得表征坐标系{Oi}在坐标系{Oi-1}下位姿的齐次变换矩阵：
(2)
其中，算子符号“^”表示将矢量从三维空间或六维空间映射到李群SO(3)的李代数so(3) [16]，Si为第i个操作单元的柔性骨架长度，则
可表示为
(3)
那么第i个操作单元末端在基础坐标系{O0}下的位姿可写成
(4)
2.2 操作空间到关节空间的映射
雅可比矩阵反映关节空间速度与工作空间速度的映射关系，是操作臂末端实时控制的基础。描述机器人雅可比矩阵的传统方法是对其正向运动学直接进行微分求解，通常情况下的求解过程和结果都比较复杂。运用指数积公式不仅可以清晰地描述串联机器人的全局雅可比矩阵，还能突出机器人的几何特征，同时也可避免微分法采用局部参数表示的不足[16]。
第i个操作单元末端执行器在局部坐标系{Oi}中的瞬时速度为
(5)
其中，
为关节空间Θi的第n个变量对时间t的导数。由式(5)可看出
与
是一种线性关系，故第i个操作单元末端的运动旋量坐标可表示成
(6)
(7)
式中，
为第i个操作单元的雅可比矩阵。
将运动旋量由一个坐标系变换到另一个坐标系的6×6矩阵称为关于T的伴随矩阵AdT。则将局部坐标系{Oi-1}变换到基坐标系{O0}的伴随矩阵为
(8)
进一步可得操作臂关节空间到工作空间的速度级映射：
(9)
式中，
为操作臂的全局雅可比矩阵；
为操作臂全局关节角速度。
为了控制操作臂的末端轨迹，需要求出工作空间到关节空间的映射关系，
为方阵且满秩时，操作臂的关节角速度为
(10)
冗余自由度操作臂关节的角速为[17]
(11)
其中，
是
的广义逆矩阵。操作臂瞬态关节空间参数为一组实数矩阵，与之对应的瞬态雅可比矩阵也为实数矩阵，因此，实时控制过程中，可通过实矩阵运算得到
无需计算复杂的非线性方程组。I为2i维的单位阵；通过
获得的是的最小范数解；
为自运动控制项，体现了冗余自由度操作臂同一末端位姿可对应多种臂形姿态的特性；k为自运动放大系数；ω为关节角速度向量。
2.3 关节空间到驱动空间的映射
在分析操作臂虚拟关节空间到驱动空间的映射关系时，对第i个操作单元建立图 7所示的几何模型。
图7 操作单元的驱动线长度模型
Fig.7 Drive line length model of Manipulator’s unit
图8为图 7沿Xi-1Oi-1Yi-1面的横切图，图中，r为操作单元穿线孔中心到支撑盘中心的距离，ri,j为第i段操作单元基部支撑盘中心指向穿线孔i、j中心的向量，φ′i为ri,j与第i段操作单元弯曲方向Ci的夹角。
图8 Xi-1Oi-1Yi-1所在截面内的几何关系
Fig.8 Geometric relationship within section
of Xi-1Oi-1Yi-1
由图 8中的几何关系易得到向量ri,j与弯曲方向Ci的夹角：
(12)
进一步可得穿线孔中心到端面交线的距离
Ri,j=Si/θi-rcosαi,j
(13)
式中，Si/θi为弧Oi-1Oi的曲率半径，θi≠0；rcosαi,j是ri,j在Ci方向的投影长度。
由图 9得出第i段的第j根驱动线长度
Li,j=2Ri,jsin(θi/2)
(14)
图9 驱动线Li,j所在弯曲平面存在的几何关系
Fig.9 Geometry of curved plane where drive Li,j
line is located
图10中，编号i,j表征第i个操作单元第j根驱动线的穿线孔位，进一步可以得到夹角φ′i及Ci与Xi之间的夹角φi的关系：
φ′i=φi-2π(i-1)/(3i)
(15)
图10 支撑盘上穿线孔的分布及编号
Fig.10 Distribution and numbering of threading holes
on support plate
由于驱动电机安装在基座，操作单元的驱动线Li,j会穿过操作单元i-1的内部，为避免操作单元i-1形变引起Li,j长度改变，通过具有冗余长度的驱动线管进行解耦，如图 11所示。综合考虑孔位偏移与驱动线管解耦作用后，得到第i个操作单元关节空间到驱动空间的映射：
(16)
需要注意，Li,j是驱动线不含套管部分的长度。
图11 驱动线管解耦原理示意图
Fig.11 Illustration of decoupling mechanism of driving
cable tube
2.4 驱动空间到关节空间的映射
为获得第i段操作单元驱动空间qi到工作空间Θi的映射，由式(14)和图9可得
Li,j=Li-2rcosαi,jsin(θi/2)
(17)
(18)
其中，Li为第i个操作单元柔性骨架所对应的弦长。进一步得到连续体操纵臂第i个操作单元驱动空间到关节空间的映射：
(19)
3 运动规划方法
为解决上述运动学模型中操作单元形状接近直线(零曲率)时，工作空间中的微分运动学数值求解不可靠的问题，进一步提升高维运动模型的实时控制效率，本节针对冗余自由度操作臂提出了一种“双空间协调运动规划方法”。该方法首先将4个操作单元划分成两部分：第1～3个操作单元命名为“基臂组”，末端的第4个操作单元名为“探指段”。通过以下3个步骤完成容器内部状态检测：
(1)姿态预调整过程。在基臂组的关节空间中进行运动规划，探指段姿态保持不变，使基臂组末端坐标轴Z3与容器入口端面的法线平行。
(2)探指伸入过程。在工作空间中选择合适的路径，基于基臂组工作空间运动规划，使探指段轴向与瓶口轴线方向重合，然后径直伸入瓶内。
(3)协同运动过程。根据容器型腔的实际情况，通过探指段关节空间的运动规划改变连续臂末端姿态，同时依据基臂组逆运动学
调整1～3个操作单元的位姿，以保证操作臂末端达到指定位置。
该方法将操作臂分成两部分，不仅实现了高维运动学模型的降阶处理，有效提升了操作臂实时控制的计算效率，而且分组后每个操作组均少于六自由度，避免了冗余自由度操作臂的周期性 “自运动”。该方法允许在线动态调整基臂组末端的位置，使探指段的运动规划过程变得更加简单、灵活。步骤(1)通过基臂组的姿态预调整，使操作臂灵活规避了奇异臂形(零曲率)，解决了微分逆运动学在奇点附近使用不可靠的问题；步骤(3)基于微分逆运动学直接获得的是实现末端期望轨迹的最小关节角速度，简化了操作臂逆运动学存在多解时需要附加判据选取唯一解的过程[15]。在探指伸入过程保持θ4=0可降低操作臂与容器碰触的机率。另外，此方法不依靠任何由凸函数组成的优化索引，允许在线调整关键点的位姿，适用于操作臂在无障碍环境中的抓取以及充电桩与汽车充电口的自动对接。
4 运动学仿真与分析
基于设计的样机与提出的运动学算法，对八自由度连续体操作臂进行数值仿真分析。4个操作单元的长度均为150 mm，支撑盘上穿线孔所在的分度圆半径为22.5 mm；运动过程中的弯转参数范围为θi∈[0, π/2]，φi∈[0, 2π]。
4.1 工作空间仿真
为了研究操作臂的工作空间范围，绘制操作臂末端点取值范围剖切图(图 12)，图中点A～D的坐标依次为(0，0，600 mm)、(545.5 mm，0，95.49 mm)、(191 mm，0，-300 mm)和(0，0，-211.1 mm)。仿真结果表明，该机器人工作空间形状近似桃形，操作臂的在z=95.49 mm时取到的最大工作空间横截面积为0.93 m2，工作空间范围的极值为xmax=ymax=545 mm，xmin=ymin=-545 mm，zmin=-300 mm，zmax=600 mm。
图12 操作臂三维运动空间剖切图
Fig.12 Sectional view of 3D motion space
of manipulator
4.2 避障仿真
为展现操作臂的避障能力，在空间设置一个静态球状障碍物，其球心坐标为(-50 mm，0，250 mm)、半径为20 mm，设操作臂起始关节空间参数q=[-π/4 0 π/3 0 π/5 0 5π/18 0]T，与之对应的操作臂末端点坐标为(128.2 mm，0，433.6 mm)，基于式(11)控制操作臂末端在保证姿态不变情况下，沿Z负方向以28 mm/s的速度运动，该过程轨迹参数方程为
(20)
在0～10 s内每间隔1 s取一个采样位姿，得到的仿真结果如图13所示。图13a为不考虑障碍物时得到的线性轨迹控制结果。为使八自由度操作臂基于自运动完成对障碍物的躲避，在操作臂上等距均布16个观测点。当观测点与障碍物中心的距离小于设定的安全距离时，通过自运动令观测点所在操作单元的末端沿预定的逃逸方向运动，得到避障的结果，如图13b所示。由此可见，所设计的八自由度连续体操作臂在执行同一操作任务过程中具有一定的避障能力。
(a)不考虑静态障碍的线性轨迹控制
(b)考虑静态障碍物的线性轨迹控制
图13 操作臂避障运动仿真
Fig.13 Manipulator obstacle avoidance motion simulation
4.3 运动规划仿真
随着小入口容器(气瓶、油箱、发动机缸体等)在石油化工、医疗、消防、汽车等行业需求量的增加以及复杂、受限工况环境下的应用，人工安全检测该类容器内部开裂、腐蚀等状态的操作成为一项非常耗时的任务。为了验证操作臂在窥探、检测过程中的运动学特性，假定某型号的气瓶内部圆周区域内存在焊缝，并且焊缝底部存在一个漏孔P，基于给出的运动学模型与运动学规划方法对操作臂执行漏孔窥探和焊缝检测的运动位姿进行数值仿真。图14所示为仿真过程中的关节空间与工作空间的运动规划算法流程。
对连续体内窥操作臂和气瓶内部容腔进行建模，基于上文给出的运动学模型与运动规划方法，得到操作臂执行内窥检测过程的实时位姿变化结果。图15a～图15d分别展示了操作臂在时间段0～10 s、10～40 s、40～50 s、50～90 s下位姿，图15a～图15c相邻臂形位姿采样时间间隔为1 s，图15d中的采样间隔为2 s。
与图15内窥检测过程相对应的驱动空间参数如图16所示，图中各驱动线长度均以连续、平滑的规律变化，进而表明针对复杂空间内窥检测任务，用于驱动操作臂运动的电机控制率是连续、平滑的，可以避免转角突变对电机造成的损伤。
(a)关节空间运动规划算法流程图 (b)工作空间运动规划算法流程图
图14 运动规划算法流程
Fig.14 Algorithm flow chart for motion planning
(a)0～10 s (b)10～40 s
(c)40～50 s (d)50～90 s
图15 操作臂运动学规划仿真结果
Fig.15 Simulation results of kinematics planning of manipulator
(a)第1个操作单元
(b)第2个操作单元
(c)第3个操作单元
(d)第4个操作单元
图16 各操作单元驱动线长度随时间的变化量
Fig.16 Variation of the length of each manipulator’s unit driving cables with time
5 结论
(1)设计的一款具有柔性骨架、通过拉线驱动的八自由度连续体机械臂在受限环境中表现出良好的操作灵活性，其固有的柔顺性能够减小碰触时的冲击力，线驱动的设计使操作臂主体更轻、结构更紧凑，提升了操作的安全性。
(2)在分段恒定曲率假设的前提下，基于旋量理论，给出了操作臂驱动空间与工作空间完整的运动学模型。该运动模型不仅给出了雅可比矩阵的解析结果，而且基于驱动线套管解耦的方法获得了多体串联连续臂关节空间与驱动空间的解耦映射。
(3)提出了一种用于小入口腔体内表面检测的双空间协调运动规划方法。将操作臂进行分组控制不仅避免了冗余自由度操作臂的周期性“自运动”，而且降低了操作臂实时控制过程的计算维度。该方法不需要任何由凸函数组成的优化索引，能实时观测、调整关键点的位姿，提升了操作臂运动规划与控制过程的计算效率。
(4)本文以面向小入口腔体内窥操作的冗余自由度线驱动操作臂为研究对象，针对其运动学建模与运动规划问题开展了研究。未来工作将结合医疗防护、康复治疗等场景所需的样本采集(咽拭子等)、除菌消毒、按摩护理等操作，进一步开展连续体机器人末端力与位置的协调控制方法研究。
[1] 牟宗高. 面向狭小空间作业的超冗余机器人轨迹规划及控制研究[D]. 哈尔滨：哈尔滨工业大学，2017.
MOU Zonggao. Research on Trajectory Planning and Control of Hyper-redundant Robots for Operation in Narrow Spaces[D]. Harbin：Harbin Institute of Technology, 2017.
[2] LI Z, WU L, REN H, et al. Kinematic Comparison of Surgical Tendon-driven Manipulators and Concentric Tube Manipulators[J]. Mechanism and Machine Theory, 2017, 107：148-165.
[3] HE G. Motion Planning and Control for Endoscopic Operations of Continuum Manipulators[J]. Intelligent Service Robotics, 2019, 12(2)：159-166.
[4] HADI A, AKBARI H, TARVIRDIZADEH B, et al. Developing a Novel Continuum Module Actuated by Shape Memory Alloys[J]. Sensors and Actuators A：Physical, 2016, 243：90-102.
[5] LASCHI C, CIANCHETTI M, MAZZOLAI B, et al. Soft Robot Arm Inspired by the Octopus[J]. Advanced Robotics. 2012, 26(7)：709-727.
[6] SHEN Q, TRABIA S, STALBAUM T, et al. A Multiple-shape Memory Polymer-metal Composite Actuator Capable of Programmable Control, Creating Complex 3D Motion of Bending, Twisting, and Oscillation[J]. Scientific Reports, 2016, 6：24462.
[7] MA X, SONG C, CHIU P W, et al. Autonomous Flexible Endoscope for Minimally Invasive Surgery with Enhanced Safety[J]. IEEE Robotics and Automation Letters, 2019, 4(3)：2607-2613.
[8] LI M, KANG R, GENG S, et al. Design and Control of a Tendon-driven Continuum Robot[J]. Transactions of the Institute of Measurement and Control, 2018, 40(11)：3263-3272.
[9] HANNAN M W, WALKER I D. Kinematics and the Implementation of an Elephant’s Trunk Manipulator and Other Continuum Style Robots[J]. Journal of Robotic Systems, 2003, 20(2)：45-63.
[10] 耿仕能，王友渔. 变刚度连续型机械臂设计与控制[J]. 宇航学报, 2018, 39(12)：81-90.
GENG Shineng, WANG Youyu. Design and Control of Variable Stiffness Continuous Manipulator[J]. Journal of Astronautics, 2018, 39(12)：81-90.
[11] 方跃法, 林华杰. 连续体并联抓取机器人的结构设计及运动学分析[J]. 北京交通大学学报, 2019, 43(4)：81-90.
FANG Yuefa, LIN Huajie. Structural Design and Kinematics Analysis of a Continuum Parallel Grasping Robot[J]. Journal of Beijing Jiaotong University, 2019, 43(4)：81-90.
[12] Festo. Bionic Motion Robot[EB/OL]. (2017-04-05)[2020-08-01]. https:∥www.festo.com.cn/group/zh/cms/12747. htm.
[13] WEBSTER R J. Design and Kinematic Modeling of Constant Curvature Continuum Robots：a Review[J]. The International Journal of Robotics Research, 2010, 29(13)：1661-1683.
[14] YANG J, PITARCH E P, POTRATZ J, et al. Synthesis and Analysis of a Flexible Elephant Trunk Robot[J]. Advanced Robotics， 2006, 20(6)：631-659.
[15] MURRAY R M. A Mathematical Introduction to Robotic Manipulation[M]. Boca Raton：CRC Press, 2017.
[16] 于靖军. 机器人机构学的数学基础[M]. 北京：机械工业出版社, 2008.
YU Jingjun. The Mathematics Foundation of Robot Mechanism[M]. Beijing：Mechanical Industry Press, 2008.
[17] 裴九芳，许德章，王海. 基于旋量理论的三指机器人灵巧手逆运动学分析[J]. 中国机械工程, 2017, 28(24)：2975-2980.
PEI Jiufang, XU Dezhang, WANG Hai. Inverse Kinematics Analysis of Three-finger Robot Dexterous Hand Based on Spin Theory[J]. China Mechanical Engineering, 2017, 28(24)：2975-2980.
