1 引言（Introduction）

线驱连续型机器人作为一种类象鼻、章鱼触手的新型仿生机器人，通常采用柔性结构作为主体骨架，具有传统刚性机器人无法比拟的灵活度及柔顺性^[1]；牵引线可视为类肌肉组织，并且结构简单可靠，具有良好的操作可控性，这类机器人在微创手术、核设施检测、搜救、空间操作等领域具有广泛的应用前景^[2]，近年来备受关注。为了更好地利用这类机器人，可借助有效的动力学模型为机器人各类参数的选取和优化提供参考，而且动力学模型也可为控制算法的研究提供有效的理论基础^[3]。因此，线驱连续型机器人的动力学建模研究成为近年来国内外学者密切关注的重点方向之一。

建立线驱连续型机器人的动力学模型需要依据构形以及所研究问题的特点选取合理的系统描述变量。基于等曲率或分段等曲率假设^[4-5]的集中参数模型能够用较少的参数描述系统的变形，方便与控制算法结合进行轨迹跟踪和运动控制^[6]，因而在早期的研究中得到了广泛应用。随着研究的深入，连续型机器人的作业任务及其空间大范围运动形式也更为复杂，基于Cosserat梁理论^[7-8]的线驱连续型机器人建模理论得到了广泛的研究和应用。除此之外，不少学者也采用绝对节点坐标法^[9]以及几何精确梁理论^[10]对连续型机器人的建模及控制进行了研究。这些研究成果不断丰富着连续型机器人动力学建模的理论储备。然而，这些建模方法均需要进行单元内部点的位移计算；在进行动力学建模时，还需要进行单元惯性量的积分，不仅建模过程较为复杂，同时还增加了数值求解难度。

线驱连续型机器人的变形依赖于牵引线的驱动，对机器人的驱动进行描述应满足2个基本要求：(1) 线驱动作用的描述必须反映牵引线对系统施加的载荷；(2) 计算需求应当尽可能小，便于耦合进入动力学方程和控制系统，避免巨大的计算量。受限于这2个要求，传统的动力学模型驱动策略通常将牵引线的张力折算为外力或外力矩，施加在刚性节盘过线孔处或机器人的端部^[11-13]。但采用“力/力矩”驱动策略需要在机器人上安装力传感器，同时还需要相关硬件设备及闭环控制算法的支持，不仅使机器人结构变得复杂，同时还增加了机器人的操作难度。引入绳索单元^[14-16]描述牵引线在节盘间的流动，更符合牵引线真实的运动状态，但这会使得机器人的建模变得较为复杂，同时，也使得数值求解较为耗时。此外，已有研究针对杆驱动的连续型机器人，考虑几何约束关系，建立了静力学^{[4, 17]}及运动学^[17]模型，为线驱动连续型机器人的动力学研究提供了新的思路。

针对上述问题，本文基于集中质量矩阵法建立连续型机器人的动力学模型，将动力学的积分项等效为离散点的求和形式。通过分析系统内牵引线的驱动长度与位形的对应关系，将牵引线的驱动长度作为约束条件，进而提出一种位－力混合的驱动模式。随后，通过样机实验验证了所提模型的有效性。

2 动力学建模（Dynamic modeling） 2.1 连续型机器人运动学建模

线驱连续型机器人的主体结构由柔性脊柱、节盘以及牵引线组成，如图 1所示。机器人的根部设有基座，牵引线由基座中的电机驱动，通过改变牵引线的驱动量，从而实现连续型机器人位形的连续变化。同时控制多根牵引线的驱动长度，还可以控制连续型机器人达到期望的空间位形。

根据连续型机器人的结构特点，本文采用共旋坐标法^[18]进行建模，节盘将柔性脊骨离散为一系列梁单元，在每个梁单元上分配一个共旋坐标系，则单元的运动可分离为共旋坐标系的全局运动以及共旋坐标系内的局部变形。由于单元节点与节盘质心重合，因此梁单元的节点坐标可以同时描述共旋坐标系及节盘的全局运动。在共旋坐标系内，假设梁单元为小变形，进而可以采用成熟的线性单元描述单元体内的弹性变形，省去了复杂非线性单元的构造，简化了建模过程。

为了方便描述梁单元及刚性节盘的运动，需要定义相关坐标系。取第$ i $（$ i= 1, 2, \cdots, n $）段梁单元进行分析，如图 2所示，首先，全局坐标系由3个单位正交向量$ {\mathit{\boldsymbol{g}}}_{j} $（$ j=1, 2, 3 $）表示；其次，在节盘$ i $质心位置$ O_{i} $定义局部坐标系的基矢量$ {\mathit{\boldsymbol{n}}}_{j}^{i} $（$ j=1, 2, 3 $），用以描述节盘及单元节点$ O_{i} $所在截面的全局运动；最后，定义梁单元$ i $的共旋坐标系的基矢量$ {\mathit{\boldsymbol{e}}}_{j}^{i} $（$ j=1, 2, 3 $），以描述梁单元的全局运动。

定义节点$ O_{i} $的全局平移矢量为$ {\mathit{\boldsymbol{u}}}_{i} $，节点$ O_{i} $所在截面的全局旋转矢量为$ \mathit{\boldsymbol{\theta}}_{i} $，则定义梁单元的全局描述参数为$ {\mathit{\boldsymbol{q}}}_{i} =[{{\mathit{\boldsymbol{u}}}_{i}^{\rm T}, \mathit{\boldsymbol{\theta}}_{i}^{\rm T}, {\mathit{\boldsymbol{u}}}_{i+1}^{\rm T}, \mathit{\boldsymbol{\theta}}_{i+1}^{\rm T}}]^{\rm T} $，节点$ O_{i} $在全局坐标系下的矢径$ {\mathit{\boldsymbol{r}}}_{i} $可定义为

$ \begin{align} {\mathit{\boldsymbol{r}}}_{i} ={\mathit{\boldsymbol{r}}}_{i}^{0} +{\mathit{\boldsymbol{u}}}_{i} \end{align} $

(1)

式中：$ {\mathit{\boldsymbol{r}}}_{i}^{0} $为初始状态下$ O_{i} $在全局坐标系的矢径。

节点$ O_{i} $所在截面坐标系相对于全局坐标系的旋转矩阵$ {\mathit{\boldsymbol{R}}}_{i} $可写为

$ \begin{align} {\mathit{\boldsymbol{R}}}_{i} =\cos \theta_{i} {\mathit{\boldsymbol{I}}}+\frac{1-\cos \theta_{i}} {\theta_{i}^{2}} \mathit{\boldsymbol{\theta}}_{i} \mathit{\boldsymbol{\theta}}_{i}^{\rm T} +\frac{\sin \theta_{i}} {\theta_{i}} \widetilde{\mathit{\boldsymbol{\theta}}_{i}} \end{align} $

(2)

式中：$ {\mathit{\boldsymbol{I}}} $为3$ \times $3单位矩阵，$ \theta_{i} =\| {\mathit{\boldsymbol{\theta}}_{i}} \| $，$ \widetilde{(\cdot)} $为相应矢量的反对称张量形式。

式(1)(2)对时间$ t $求导，结合$ \dot{\mathit{\boldsymbol{R}}}_{i} =\widetilde{{\mathit{\boldsymbol{\omega}}}_{i}} {\mathit{\boldsymbol{R}}}_{i} $，可得节点$ O_{i} $的全局平移速度$ \dot{\mathit{\boldsymbol{r}}}_{i} $及旋转角速度$ {\mathit{\boldsymbol{\omega}}}_{i} $：

$ \begin{align} \dot{\mathit{\boldsymbol{r}}}_{i} =\dot{\mathit{\boldsymbol{u}}}_{i}, \; \; \; {\mathit{\boldsymbol{\omega}}}_{i} ={\mathit{\boldsymbol{T}}}_{i} \dot{\mathit{\boldsymbol{\theta}}}_{i} \end{align} $

(3)

式中：$ {\mathit{\boldsymbol{T}}}_{i} ={\mathit{\boldsymbol{T}}}({\mathit{\boldsymbol{\theta}}_{i}})={\mathit{\boldsymbol{I}}}\!+\!\dfrac{1-\cos \theta_{i}} {\theta_{i}^{2}} \widetilde{\mathit{\boldsymbol{\theta}}_{i}} \!+\!\dfrac{\theta_{i} \!-\!\sin \theta_{i}} {\theta_{i}^{3}}\widetilde{\mathit{\boldsymbol{\theta}}_{i}} \widetilde{\mathit{\boldsymbol{\theta}}_{i}} $。

在共旋坐标系下，定义节点$ O_{i} $的局部平移矢量为$ \bar{\mathit{\boldsymbol{u}}}_{i} $，节点$ O_{i} $所在截面的局部旋转矢量为$ \bar{\mathit{\boldsymbol{\theta}}}_{i} $，单元的局部描述参数可定义为$ \bar{\mathit{\boldsymbol{q}}}_{i} =[{\bar{\mathit{\boldsymbol{u}}}_{i}^{\rm T}, \bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm T}, \bar{\mathit{\boldsymbol{u}}}_{i+1}^{\rm T}, \bar{\mathit{\boldsymbol{\theta}}}_{i+1}^{\rm T}}]^{\rm T} $，则变形后，单元形心线上任意弧长$ s $处的局部位移矢量$ \bar{\mathit{\boldsymbol{u}}}_{i}^{s} $及局部旋转矢量$ \bar{\mathit{\boldsymbol{\theta}}}_{i}^{s} $可由插值得到：

$ \begin{align} \bar{\mathit{\boldsymbol{u}}}_{i}^{s} ={\mathit{\boldsymbol{N}}}_{i}^{\rm u} \bar{\mathit{\boldsymbol{q}}}_{i}, \; \; \; \bar{\mathit{\boldsymbol{\theta}}}_{i}^{s} ={\mathit{\boldsymbol{N}}}_{i}^\mathtt{θ} \bar{\mathit{\boldsymbol{q}}}_{i} \end{align} $

(4)

式中：$ {\mathit{\boldsymbol{N}}}_{i}^{\rm u} $及$ {\mathit{\boldsymbol{N}}}_{i}^\mathtt{θ} $为多项式插值函数矩阵^[19]。

由式(4)可得，变形后的质心点$ O_{i}^{\rm c} $在共旋坐标系下的局部位移矢量$ \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm c} $及局部旋转矢量$ \bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm c} $可写为

$ \begin{align} \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm c} =\frac{1}{m_{i}} \int_0^{l_{i}^{0}} {\bar{\mathit{\boldsymbol{u}}}_{i}^{s} \rho_{\rm A} {\rm d}s}, \; \; \; \bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm c} = {{\mathit{\boldsymbol{N}}}_{i}^\mathtt{θ}} |_{\xi =\frac{1}{2}} \bar{\mathit{\boldsymbol{q}}}_{i} \end{align} $

(5)

式中，$ m_{i} $为单元$ i $的质量，$ l_{i}^{0} $为单元初始长度，$ \rho_{\rm A} $为单元线密度，$ \xi =s/l_{i}^{0} $。

根据局部描述参数$ \bar{\mathit{\boldsymbol{q}}}_{i} $与全局描述参数$ {\mathit{\boldsymbol{q}}}_{i} $的转换关系式^[18]，式(5)可由单元的全局描述参数$ {\mathit{\boldsymbol{q}}}_{i} $表达。

2.2 基于集中质量矩阵方法的动力学建模

(1) 集中质量矩阵方法

对梁单元进一步使用集中质量矩阵法^[19]进行离散化，进而可以通过离散点求和来等效计算单元的动能，避免了梁单元内位移及速度量的解算，简化了建模过程；同时，还避免了积分带来的数值求解困难。

按照集中质量矩阵法的原则，离散前后单元的动能等效，即：1) 梁单元总质量不变，满足平移动能协调条件；2) 梁单元转动惯量不变，满足旋转动能协调条件。

为满足上述条件，对于单元$ i $，引入辅助点$ O_{i}^{\rm a} $，将梁单元的质量按照$ m_{i} /6 $、$ 2 m_{i} /3 $、$ m_{i} /6 $原则^[18]分配至$ O_{i} $、$ O_{i}^{\rm a} $及$ O_{i+1} $上，同时离散点的局部位移矢量与质心$ O_{i}^{\rm c} $的局部位移矢量需满足：

$ \begin{align} \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm c} =\frac{1}{6}({\bar{\mathit{\boldsymbol{u}}}_{i} +4\bar{\mathit{\boldsymbol{u}}}_{i}^{\rm a} +\bar{\mathit{\boldsymbol{u}}}_{i+1}})\Rightarrow \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm a} =\frac{1}{4}({6\bar{\mathit{\boldsymbol{u}}}_{i}^{\rm c} -\bar{\mathit{\boldsymbol{u}}}_{i} -\bar{\mathit{\boldsymbol{u}}}_{i+1}}) \end{align} $

(6)

式中：$ \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm a} $为$ O_{i}^{\rm a} $在共旋坐标系下的位移矢量。

由式(6)，辅助点$ O_{i}^{\rm a} $在全局坐标系的矢径$ {\mathit{\boldsymbol{r}}}_{i}^{\rm a} $为

$ \begin{align} {\mathit{\boldsymbol{r}}}_{i}^{\rm a} =\frac{1}{2}({{\mathit{\boldsymbol{r}}}_{i} +{\mathit{\boldsymbol{r}}}_{i+1}})+{\mathit{\boldsymbol{R}}}_{i}^{\rm r} \bar{\mathit{\boldsymbol{u}}}_{i}^{\rm a} \end{align} $

(7)

式中：$ {\mathit{\boldsymbol{R}}}_{i}^{\rm r} =[{{\mathit{\boldsymbol{e}}}_{j}^{1}, {\mathit{\boldsymbol{e}}}_{j}^{2}, {\mathit{\boldsymbol{e}}}_{j}^{3}} ] $，为共旋坐标系相对于全局坐标系的旋转矩阵。

式(7)对时间$ t $求导，可得$ O_{i}^{\rm a} $的平移速度$ \dot{\mathit{\boldsymbol{r}}}_{i}^{\rm a} $：

$ \begin{align} \dot{\mathit{\boldsymbol{r}}}_{i}^{\rm a} =\frac{1}{2}({\dot{\mathit{\boldsymbol{r}}}_{i} +\dot{\mathit{\boldsymbol{r}}}_{i+1}})+{\mathit{\boldsymbol{R}}}_{i}^{\rm r} \dot{\bar{\mathit{\boldsymbol{u}}}}_{i}^{\rm a} -{\mathit{\boldsymbol{R}}}_{i}^{\rm r} \widetilde{\bar{\mathit{\boldsymbol{u}}}_{i}^{\rm a}} ({{\mathit{\boldsymbol{R}}}_{i}^{\rm r}})^{\rm T}{\mathit{\boldsymbol{\omega}}}_{i}^{\rm r} \end{align} $

(8)

式中：$ {\mathit{\boldsymbol{\omega}}}_{i}^{\rm r} $为共旋坐标系相对于全局坐标系的旋转角速度。

辅助点$ O_{i}^{\rm a} $的局部旋转矢量$ \bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm a} $满足：

$ \begin{align} \bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm a} =\bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm c} \end{align} $

(9)

由角速度叠加原理，可得辅助点$ O_{i}^{\rm a} $在全局坐标系下的角速度$ {\mathit{\boldsymbol{\omega}}}_{i}^{\rm a} $为

$ \begin{align} {\mathit{\boldsymbol{\omega}}}_{i}^{\rm a} ={\mathit{\boldsymbol{\omega}}}_{i}^{\rm r} +{\mathit{\boldsymbol{R}}}_{i}^{\rm r} \bar{\mathit{\boldsymbol{\omega}}}_{i}^{\rm a} \end{align} $

(10)

式中：$ \bar{\mathit{\boldsymbol{\omega}}}_{i}^{\rm a} ={\mathit{\boldsymbol{T}}}({\bar{\mathit{\boldsymbol{\theta}}}_{i}^{\rm a}})\dot{\bar{{\mathit{\boldsymbol{\theta}}}}}_{i}^{\rm a} $。

(2) 连续型机器人的动力学建模

连续型机器人按照结构的变形特点，可分为刚性部件及柔性部件。刚性部件包含基座、节盘及末端执行器（选装），视为一系列刚体；柔性部件为柔性脊柱，建模为一系列梁单元。

机器人系统的动能$ T $由梁单元动能$ T_{\rm b} $及刚体动能$ T_{\rm d} $两部分组成，即：

$ \begin{align} T=T_{\rm b} +T_{\rm d} \end{align} $

(11)

梁单元的动能$ T_{\rm b} $可写为

$ \begin{align} T_{\rm b} =T_{\rm b}^{\rm u} +T_{\rm b}^\mathtt{θ} \end{align} $

(12)

式中：$ T_{\rm b}^{\rm u} $为梁单元平移动能，$ T_{\rm b}^\mathtt{θ} $为单元的旋转动能。

由集中质量矩阵法的原则，结合式(3)(8)(10)，$ T_{\rm b}^{\rm u} $及$ T_{\rm b}^\mathtt{θ} $可写为等价的三点求和形式：

$ \begin{align} T_{\rm b}^{\rm u} & =\frac{1}{12}\sum _{i=1}^n {m_{i} ({\dot{\mathit{\boldsymbol{r}}}_{i}^{\rm T} \dot{\mathit{\boldsymbol{r}}}_{i} +4({\dot{\mathit{\boldsymbol{r}}}_{i}^{\rm a}})^{\rm T}\dot{\mathit{\boldsymbol{r}}}_{i}^{\rm a} +\dot{\mathit{\boldsymbol{r}}}_{i+1}^{\rm T} \dot{\mathit{\boldsymbol{r}}}_{i+1}})} \end{align} $

(13)

$ \begin{align} T_{\rm b}^\mathtt{θ} & =\frac{1}{12}\sum _{i=1}^n {({{\mathit{\boldsymbol{\omega}}}_{i}^{\rm T} {\mathit{\boldsymbol{J}}}_{i}^{\rm b} {\mathit{\boldsymbol{\omega}}}_{i} +4({{\mathit{\boldsymbol{\omega}}}_{i}^{\rm a}})^{\rm T}{\mathit{\boldsymbol{J}}}_{i}^{\rm b} {\mathit{\boldsymbol{\omega}}}_{i}^{\rm a} +{\mathit{\boldsymbol{\omega}}}_{i+1}^{\rm T} {\mathit{\boldsymbol{J}}}_{i}^{\rm b} {\mathit{\boldsymbol{\omega}}}_{i+1}})} \end{align} $

(14)

式中：$ {\mathit{\boldsymbol{J}}}_{i}^{\rm b} =m_{i} \bar{\mathit{\boldsymbol{J}}}_{i}^{\rm b} $，$ \bar{\mathit{\boldsymbol{J}}}_{i}^{\rm b} $为梁单元的截面惯性矩。

由式(3)，刚体动能$ T_{\rm d} $可写为

$ \begin{align} T_{\rm d} =\frac{1}{2}\sum _{i=1}^{n+1} {({m_{i}^{\rm d} \dot{\mathit{\boldsymbol{r}}}_{i}^{\rm T} \dot{\mathit{\boldsymbol{r}}}_{i} +{\mathit{\boldsymbol{\omega}}}_{i}^{\rm T} {\mathit{\boldsymbol{J}}}_{i}^{\rm d} {\mathit{\boldsymbol{\omega}}}_{i}})} \end{align} $

(15)

式中：$ m_{i}^{\rm d} $为节盘$ i $的质量，$ {\mathit{\boldsymbol{J}}}_{i}^{\rm d} $为节盘$ i $的转动惯量。

由于梁单元的弹性势能在全局坐标系及共旋坐标系内等价，因此系统的弹性势能$ U $可写为

$ \begin{align} U=\sum _{i=1}^n \iiint\limits_{V_{i}} {\mathit{\boldsymbol{\varepsilon}}_{i}^{\rm T} {\mathit{\boldsymbol{\sigma}}}_{i} {\rm d}y{\rm d}z{\rm d}s} =\frac{1}{2}\sum _{i=1}^n {\bar{\mathit{\boldsymbol{q}}}_{i} {\mathit{\boldsymbol{K}}}_{i} \bar{\mathit{\boldsymbol{q}}}_{i}} \end{align} $

(16)

其中，$ {\mathit{\boldsymbol{\varepsilon}}}_{i} $为梁单元的应变矢量，$ {\mathit{\boldsymbol{\sigma}}}_{i} $为应力矢量，$ {\mathit{\boldsymbol{K}}}_{i} $为单元刚度阵^[18]。

定义系统的全局参数坐标为

$ \begin{align} {\mathit{\boldsymbol{q}}}=[{{\mathit{\boldsymbol{u}}}_{1}^{\rm T}, \mathit{\boldsymbol{\theta}}_{1}^{\rm T}, \cdots, {\mathit{\boldsymbol{u}}}_{n+1}^{\rm T}, \mathit{\boldsymbol{\theta}}_{n+1}^{\rm T}}]^{\rm T} \end{align} $

(17)

根据第二类拉格朗日方程，可得动力学方程：

$ \begin{align} \frac{\rm d}{{\rm d}t} \left({\frac{\partial T}{\partial \dot{\mathit{\boldsymbol{q}}}}}\right)^{\rm T}- \left({\frac{\partial T}{\partial {\mathit{\boldsymbol{q}}}}}\right)^{\rm T}=- \left({\frac{\partial U}{\partial {\mathit{\boldsymbol{q}}}}}\right)^{\rm T}+{\mathit{\boldsymbol{Q}}}_{\rm F} \end{align} $

(18)

式中：$ \dfrac{\rm d}{{\rm d}t} \left({\dfrac{\partial T}{\partial \dot{\mathit{\boldsymbol{q}}}}}\right)^{\rm T}=\mathit{\boldsymbol{M\ddot{q}}}+{\mathit{\boldsymbol{\dot{M}\dot{q}}}} $，$ {\mathit{\boldsymbol{M}}} $为广义质量矩阵，$ {\mathit{\boldsymbol{Q}}}_{\rm F} $为外力对应的广义力。

展开式(18)可得系统的动力学模型为

$ \begin{align} \mathit{\boldsymbol{M\ddot{q}}}={\mathit{\boldsymbol{Q}}}_{\rm F} -{\mathit{\boldsymbol{Q}}}_{\rm U} -{\mathit{\boldsymbol{Q}}}_{\rm I} \end{align} $

(19)

式中：$ {\mathit{\boldsymbol{Q}}}_{\rm I} ={\mathit{\boldsymbol{\dot{M}\dot{q}}}}- \left({\frac{\partial T}{\partial {\mathit{\boldsymbol{q}}}}}\right)^{\rm T} $，$ {\mathit{\boldsymbol{Q}}}_{\rm U} = \left({\frac{\partial U}{\partial {\mathit{\boldsymbol{q}}}}}\right)^{\rm T} $。

为了方便驱动载荷的对比分析，本节中外力的广义力$ {\mathit{\boldsymbol{Q}}}_{\rm F} $中不包含驱动载荷的贡献，故式(19)为连续型机器人无驱动载荷动力学方程。

3 线驱动（Cable actuation）

本节分别建立线力驱动与线几何约束模式的动力学模型，并对两种形式的动力学模型进行分析，得到线驱动力与拉氏乘子的关系。本文中有如下假设：

(1) 忽略牵引线的应变；

(2) 不考虑牵引线与节盘间的摩擦。

3.1 线力驱动模式

基于力驱动模式的动力学模型是将牵引线的拉力折算到每个过线孔处，相当于在过线孔两侧施加了外力，如图 3所示。

节盘$ i $上过孔点$ O_{i}^{j} $在全局坐标系下的矢径为

$ \begin{align} {\mathit{\boldsymbol{r}}}_{i}^{j} ={\mathit{\boldsymbol{r}}}_{i} +{\mathit{\boldsymbol{\rho}}}_{i}^{j}, \; \; j=1, 2, 3 \end{align} $

(20)

式中：$ {\mathit{\boldsymbol{\rho}}}_{i}^{j} $是节点坐标系原点到过孔点$ O_{i}^{j} $的矢径。

由式(20)，可计算过孔点$ O_{i}^{j} $与过孔点$ O_{i+1}^{j} $间牵引线的单位方向矢量$ \hat{\mathit{\boldsymbol{L}}}_{i}^{j} $：

$ \begin{align} \hat{\mathit{\boldsymbol{L}}}_{i}^{j} =\frac{{\mathit{\boldsymbol{L}}}_{i}^{j}} {L_{i}^{j}}, \; \; \; {\mathit{\boldsymbol{L}}}_{i}^{j} ={\mathit{\boldsymbol{r}}}_{i+1}^{j} -{\mathit{\boldsymbol{r}}}_{i}^{j}, \; \; \; L_{i}^{j} =\| {{\mathit{\boldsymbol{L}}}_{i}^{j}} \| \end{align} $

(21)

由于不考虑牵引线与节盘的摩擦，各段牵引线张紧力相同，则每个过孔点$ O_{i}^{j} $处驱动力所做虚功为

$ \begin{align} {\delta} W_{i}^{j} = \begin{cases} ({{\delta} {\mathit{\boldsymbol{r}}}_{i}^{j}})^{\rm T}T_{j} \hat{\mathit{\boldsymbol{L}}}_{i}^{j}, & i=1 \\ ({{\delta} {\mathit{\boldsymbol{r}}}_{i}^{j}})^{\rm T}({T_{j} \hat{\mathit{\boldsymbol{L}}}_{i}^{j} -T_{j} \hat{\mathit{\boldsymbol{L}}}_{i-1}^{j}}), & i=2, 3, \cdots, n \\ -({{\delta} {\mathit{\boldsymbol{r}}}_{i}^{j}})^{\rm T}T_{j} {\mathit{\boldsymbol{L}}}_{i-1}^{j}, & i=n+1 \end{cases} \end{align} $

(22)

式中，$ T_{j} $为牵引线$ j $的驱动力，$ \delta {\mathit{\boldsymbol{r}}}_{i}^{j} $为第$ i $个节盘上过孔点$ O_{i}^{j} $的虚位移，结合式(3)，$ \delta {\mathit{\boldsymbol{r}}}_{i}^{j} $的具体形式为

$ \begin{align} {\delta} {\mathit{\boldsymbol{r}}}_{i}^{j} ={\delta} {\mathit{\boldsymbol{u}}}_{i} -\widetilde{\mathit{\boldsymbol{\rho}}}_{i}^{j} {\mathit{\boldsymbol{T}}}_{i} {\delta} \mathit{\boldsymbol{\theta}}_{i} ={\mathit{\boldsymbol{K}}}_{i}^{j} {\mathit{\boldsymbol{\varGamma}}}_{i} {\delta} {\mathit{\boldsymbol{q}}} \end{align} $

(23)

其中：$ {\mathit{\boldsymbol{K}}}_{i}^{j} =[{{\mathit{\boldsymbol{I}}}, \; -\widetilde{\mathit{\boldsymbol{\rho}}}_{i}^{j} {\mathit{\boldsymbol{T}}}_{i}}] $，$ {\mathit{\boldsymbol{\varGamma}}}_{i} $是节点$ O_{i} $的全局参数与广义坐标的变换矩阵，即：

$ \begin{align} [{{\mathit{\boldsymbol{u}}}_{i}^{\rm T}, \mathit{\boldsymbol{\theta}}_{i}^{\rm T}} ]^{\rm T}={\mathit{\boldsymbol{\varGamma}}}_{i} {\mathit{\boldsymbol{q}}} \end{align} $

(24)

结合式(23)，可将牵引线$ j $的张紧力的虚功写为每个节盘两侧张紧力虚功之和：

$ \begin{align} {\delta} W^{j}=\sum _{i=1}^n {({({\delta {\mathit{\boldsymbol{r}}}_{i}^{j}})^{\rm T}T_{j} \hat{\mathit{\boldsymbol{L}}}_{i}^{j} -({\delta {\mathit{\boldsymbol{r}}}_{i+1}^{j}})^{\rm T}T_{j} \hat{\mathit{\boldsymbol{L}}}_{i}^{j}})} \end{align} $

(25)

将式(21)(23)代入式(25)，展开可得：

$ \begin{align} {\delta} W^{j}=\delta {\mathit{\boldsymbol{q}}}^{\rm T}{\mathit{\boldsymbol{T}}}_{\rm A}^{j} T_{j} \end{align} $

(26)

式中：$ {\mathit{\boldsymbol{T}}}_{\rm A}^{j} =-\sum\limits_{i=1}^n {({{\mathit{\boldsymbol{K}}}_{i+1}^{j} {\mathit{\boldsymbol{\varGamma}}}_{i+1} -{\mathit{\boldsymbol{K}}}_{i}^{j} {\mathit{\boldsymbol{\varGamma}}}_{i}})^{\rm T}\hat{\mathit{\boldsymbol{L}}}_{i}^{j}} $。

由式(19)(26)，含有牵引线驱动力的动力学模型可写为

$ \begin{align} \mathit{\boldsymbol{M}}\ddot{\mathit{\boldsymbol{q}}}={\mathit{\boldsymbol{Q}}}_{\rm F} -{\mathit{\boldsymbol{Q}}}_{\rm U} -{\mathit{\boldsymbol{Q}}}_{\rm I} +{\mathit{\boldsymbol{Q}}}_{\rm A} \end{align} $

(27)

式中：$ {\mathit{\boldsymbol{Q}}}_{\rm A} =\sum\limits_j {{\mathit{\boldsymbol{T}}}_{\rm A}^{j} T_{j}} $，为牵引线张紧力的广义力向量。

3.2 牵引线对系统的几何约束

线驱连续型机器人的位形由穿线的方式、牵引线的驱动量确定，对不同的牵引线驱动长度组合，连续型机器人都有唯一的位形与之对应，因此，从力学角度来看，牵引线的驱动量实际上代表了牵引线对系统位形的几何约束。

在机器人运动过程中的$ t $时刻，驱动电机带动牵引线$ j $（$ j=1, 2, 3 $）绕出一定长度后，保留在系统中的牵引线$ j $的长度$ S_{\text{in}}^{j} $与其所流经的节盘过孔点$ O_{i}^{j} $间距$ L_{i}^{j} $（$ i=1, 2, \cdots, n $）之和相等；在$ t $时刻，牵引线$ j $的驱动长度为$ \Delta S^{j}(t) $，则牵引线$ j $对系统的约束作用可由约束方程表示：

$ \begin{align} \varPhi^{j}(\mathit{\boldsymbol{q}}, t)=S_{0}^{j} -\Delta S^{j}(t)-S_{\text{in}}^{j} \geqslant 0, \; \; \; j=1, 2, 3 \end{align} $

(28)

式中：$ S_{\text{in}}^{j} =\sum\limits_{i=1}^n {L_{i}^{j}} $，$ S_{0}^{j} $为牵引线在初始时刻留在系统中的长度。当牵引线张紧时，式(28)取等号。

当系统由多根牵引线驱动时，可由式(28)判断每根牵引线是否张紧，并写出对应的系统约束方程组$ \boldsymbol{\varPhi} ={\bf{0}} $，$ \varPhi^{j} $为$ \boldsymbol{\varPhi} $中对应的列元素。当式(28)取等号时，对时间$ t $求导数，得到：

$ \begin{align} \dot{\varPhi}^{j}={\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{j} \dot{\mathit{\boldsymbol{q}}}+\varPhi_{t}^{j} =0, \; \; j=1, 2, 3 \end{align} $

(29)

其中：$ \varPhi_{t}^{j} =\partial \varPhi^{j}/\partial t $，$ {\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{j} $为约束雅可比矩阵$ {\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} $对应的行向量：

$ \begin{align} {\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{j} =-\sum _{i=1}^n {\hat{\mathit{\boldsymbol{L}}}_{i}^{\rm T} ({{\mathit{\boldsymbol{K}}}_{i+1}^{j} {\mathit{\boldsymbol{\varGamma}}}_{i+1} -{\mathit{\boldsymbol{K}}}_{i}^{j} {\mathit{\boldsymbol{\varGamma}}}_{i}})} \end{align} $

(30)

将线的驱动建模为牵引线对系统的几何约束，则系统的全局描述参数不相互独立，本文从动力学普遍方程出发，结合第一类拉格朗日方程（引入拉氏乘子$ {\mathit{\boldsymbol{\lambda}}} $）及第二类拉格朗日方程的推导过程，将线驱连续型机器人的动力学方程表示为约束系统动力学方程：

$ \begin{align} \begin{cases} \mathit{\boldsymbol{M}}\ddot{\mathit{\boldsymbol{q}}}-{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{\rm T} {\mathit{\boldsymbol{\lambda}}} ={\mathit{\boldsymbol{Q}}}_{\rm F} -{\mathit{\boldsymbol{Q}}}_{\rm U} -{\mathit{\boldsymbol{Q}}}_{\rm I} \\ {\mathit{\boldsymbol{\varPhi}}} =\mathit{\boldsymbol{0}} \end{cases} \end{align} $

(31)

3.3 驱动力－几何约束的联系

经上述推导，受动力学影响相同的2组量，其对动力学模型的贡献等价，比较方程(27)与式(31)的第1式可得：

$ \begin{align} {\mathit{\boldsymbol{Q}}}_{\rm A} ={\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{\rm T} {\mathit{\boldsymbol{\lambda}}} \end{align} $

(32)

对比式(26)与式(30)，$ {\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{j} $的转置向量与广义力变换向量$ {\mathit{\boldsymbol{T}}}_{\rm A}^{j} $的表达形式相同。因此，式(31)中拉氏乘子就是牵引线上的张紧力，亦即驱动电机对牵引线的拉力。由此也论证了将牵引线的驱动等效建模为一组电机驱动参数的约束方程是准确的。

通过上述分析，建立了驱动力与牵引线对系统的几何约束的联系。动力学方程(31)中的约束方程可以直接使用电机的驱动参数建立，不需进行线张紧力的测定，更方便机器人的实际应用，并且计算得到的拉氏乘子$ {\mathit{\boldsymbol{\lambda}}} $代表了牵引线的驱动张力，然而，式(31)属于微分代数方程，增大了数值求解难度，同时需对违约进行抑制^[20]。

4 位－力混合驱动（Hybrid position-force actuation）

本节提出一种位－力混合的连续型机器人动力学模型的驱动模式，不仅考虑牵引线对系统的约束，同时还可以直接解得线张紧力。

将约束方程对时间$ t $求2阶导数，得到派生的约束方程的2阶变化率形式，即：

$ \begin{align} \ddot{\mathit{\boldsymbol{\varPhi}}}={\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} \ddot{\mathit{\boldsymbol{q}}}+\dot{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} \dot{\mathit{\boldsymbol{q}}}+\dot{\mathit{\boldsymbol{\varPhi}}}_{t} =\mathit{\boldsymbol{0}} \end{align} $

(33)

式中，$ \ddot{\mathit{\boldsymbol{q}}} $可由式(31)的第1式求得：

$ \begin{align} \ddot{\mathit{\boldsymbol{q}}}=({{\mathit{\boldsymbol{M}}}^{-1}{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{\rm T}}){\mathit{\boldsymbol{\lambda}}} +{\mathit{\boldsymbol{M}}}^{-1}({{\mathit{\boldsymbol{Q}}}_{\rm F} -{\mathit{\boldsymbol{Q}}}_{\rm U} -{\mathit{\boldsymbol{Q}}}_{\rm I}}) \end{align} $

(34)

将式(34)代入式(33)，可得$ {\mathit{\boldsymbol{\lambda}}} $：

$ \begin{align} {\mathit{\boldsymbol{\lambda}}} ={\mathit{\boldsymbol{A}}}^{-1}{\mathit{\boldsymbol{b}}} \end{align} $

(35)

式中：

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{A}}}={\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} ({{\mathit{\boldsymbol{M}}}^{-1}{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}}^{\rm T}}) \\ {\mathit{\boldsymbol{b}}}=-{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} {\mathit{\boldsymbol{M}}}^{-1}({{\mathit{\boldsymbol{Q}}}_{\rm F} -{\mathit{\boldsymbol{Q}}}_{\rm U} -{\mathit{\boldsymbol{Q}}}_{\rm I}})-\dot{\mathit{\boldsymbol{\varPhi}}}_{\mathit{\boldsymbol{q}}} \dot{\mathit{\boldsymbol{q}}}-\dot{\mathit{\boldsymbol{\varPhi}}}_{t} \end{cases} \end{align} $

(36)

然而式(35)仅考虑了约束的2阶变化率，在数值求解时，约束方程及约束方程的1阶变化率可能会偏离零点，并不能精确满足牵引线对系统的约束，因此，使用式(35)进行求解，将受到很大的限制。

本文借鉴文[21]的模型光滑化思想，将$ [ t, \; t+ $ $ \Delta t ] $时间微段的约束方程进行2阶泰勒展开，得到：

$ \begin{align} {\mathit{\boldsymbol{\varPhi}}} (\tau)\approx {\mathit{\boldsymbol{\varPhi}}} (t)+({\tau -t})\dot{\mathit{\boldsymbol{\varPhi}}}(t)+\frac{1}{2} ({\tau -t})^{2}\ddot{\mathit{\boldsymbol{\varPhi}}}(t) \end{align} $

(37)

由式(37)，在$ [t, \; t+\Delta t ] $时间微段内，约束方程的平均值可写为

$ \begin{align} \bar{\mathit{\boldsymbol{\varPhi}}} & \triangleq \frac{1}{\Delta t}\int_t^{t+{\Delta} t} {\mathit{\boldsymbol{\varPhi}}} (\tau){\rm d}\tau \\ & \approx {\mathit{\boldsymbol{\varPhi}}} (t)+\frac{\Delta t}{2}\dot{\mathit{\boldsymbol{\varPhi}}}(t)+\frac{\Delta t^{2}}{6}\ddot{\mathit{\boldsymbol{\varPhi}}}(t) \end{align} $

(38)

将式(34)代入式(33)，进而式(38)可改写为

$ \begin{align} {\mathit{\boldsymbol{\lambda}}} =\hat{\mathit{\boldsymbol{A}}}^{-1}\hat{\mathit{\boldsymbol{b}}} \end{align} $

(39)

式中：

$ \begin{align} \begin{cases} \hat{\mathit{\boldsymbol{A}}}=\dfrac{\Delta t^{2}}{6}{\mathit{\boldsymbol{A}}} \\[5pt] \hat{\mathit{\boldsymbol{b}}}=\dfrac{\Delta t^{2}}{6}{\mathit{\boldsymbol{b}}}-\dfrac{\Delta t}{2}\dot{\mathit{\boldsymbol{\varPhi}}} (t)-{\mathit{\boldsymbol{\varPhi}}} (t) \end{cases} \end{align} $

(40)

由式(39)可解得拉氏乘子$ {\mathit{\boldsymbol{\lambda}}} $，即求得牵引线的张紧力。当牵引线的驱动作用有效地施加到系统中时，解得的张紧力为非负唯一解。将其代入动力学模型(27)，系统动力学方程可解。该方法不仅考虑了约束方程，还考虑了1阶及2阶变化率的派生约束方程，因此使用该方法可以满足牵引线对系统的约束，同时，该方法可避免微分代数方程的数值求解以及违约的抑制工作。

5 仿真与实验验证（Simulation and experimental validation） 5.1 原型样机

线驱连续型机器人的样机及实验平台如图 4所示。样机由基座内的直线步进电机组（赛菱智能科技公司28H045H-0674型）驱动，在步进电机上安装有拉力传感器（蚌埠传感器系统工程有限公司JLBS-M2型），使用超弹Ni-Ti合金作为柔性脊骨，节盘为PLA（聚乳酸）材质，由3D打印制备，节盘直径为30 mm，牵引线为高强度的轻质超高分子量聚乙烯纤维复合线。在节盘及步进电机上贴有反光测量点，由运动捕捉系统的红外感应相机测量样机的运动数据。

选取样机所用材料参数建立等效的仿真模型，其中，超弹Ni-Ti合金的力学参数如表 1所示。

5.2 实验与仿真结果对比分析

(1) 单线驱动验证

连续型机器人样机由牵引线1驱动，线1依次穿过节盘上对应的过线孔1，样机的变形及运动捕捉系统检测的机器人位形如图 5所示，样机最大的弯曲角度为110$ ^{\circ} $。

图 6为牵引线1的驱动长度及驱动力的实验结果与仿真结果对比曲线，仿真计算按照样机的实际拉线长度曲线进行驱动，将数值计算出的线驱动力与拉力传感器记录的数据进行对比，可以看出仿真计算得到的驱动力数值与实验结果基本吻合，以实验结果为准确值，仿真驱动力的最大误差值为0.237 N。

图 7为拉线过程中仿真与实验结果的位形对比曲线，从图中可以看出，仿真与实验中机器人的动态位形曲线基本一致。仿真与实验结果的节盘轨迹的误差定义为

$ \begin{align} \delta_{\rm A} =| {r_{i, k}^\text{Sim} -r_{i, k}^\text{Exp}} |\bigg/\sum _{j=1}^{i-1} {l_{j}^{0}} \times 100\%, \; \; k=x, y, z \end{align} $

(41)

式中：$ r_{i, k}^\text{Sim} $及$ r_{i, k}^\text{Exp} $分别为仿真和实验结果的节盘$ i $形心点的矢径在$ k $坐标轴上的分量，$ | r_{i, k}^\text{Sim} -r_{i, k}^\text{Exp} | $为节盘$ i $的轨迹误差量。

节盘的轨迹误差曲线如图 8所示，$ x $向轨迹最大误差为5.91%，$ y $向轨迹最大误差为4.61%；末端点（即节盘11）的$ x $向轨迹最大误差为3.85%，$ x $向轨迹的最大误差量为24.5 mm，末端点$ y $向轨迹最大误差为1.97%，$ y $向轨迹最大误差量为12.54 mm。

(2) 双线异步驱动验证

为了进一步验证本文方法的有效性，进行了牵引线1与线2异步驱动验证，样机的位形及运动捕捉系统检测的位形如图 9所示。由于牵引线1与线2的驱动区间存在时间间隔，机器人不仅会发生弯曲变形，还会出现绕$ y $轴的扭转动作。

仿真按照牵引线1及牵引线2的实际驱动长度进行驱动，仿真计算的线1及线2的驱动力与力传感器记录的结果如图 10所示。可以看出，仿真计算的驱动力与实验结果的趋势基本相同，线1驱动力的仿真与实验结果的最大误差量为0.245 N，线2驱动力的最大误差量为0.194 N。

仿真与实验结果中机器人在不同时刻的位形如图 11所示，仿真计算的机器人位形在3~8 s区间内绕$ y $轴出现了扭转运动，对应牵引线2的驱动区间，与实验结果的位形曲线吻合。仿真与实验的节盘轨迹误差如图 12所示，节盘轨迹在$ x $、$ y $、$ z $坐标轴分量的最大误差分别为3.23%、1.852%、2.44%；末端点在$ x $、$ y $、$ z $坐标上的最大轨迹误差值为13.60 mm、11.40 mm、15.37 mm，对应的末端点轨迹最大误差分别为2.32%、1.79%、2.42%。

由上述的样机单线驱动及双线异步驱动的实验及仿真结果对比可见，采用本文所提出的位－力混合驱动的线驱连续型机器人动力学模型进行仿真，仿真与实验结果较为吻合，可以较为准确地计算出机器人的动态位形，而且适用于单/多线驱动情况。

6 结论（Conclusion）

提出了一种位－力混合的线驱连续型机器人动力学驱动方法，其中动力学模型采用集中质量矩阵法进行建模，将单元的动能等效离散到单元两节点及辅助点，可避免单元内位移及速度量的求解，提升数值计算效率；位－力混合驱动模式相比于传统的力驱动模式，不仅考虑了牵引线对系统的几何约束，而且可以在不使用拉力传感器的条件下计算出牵引线的驱动力，简化了机器人的结构与实际操作过程；相比于线的几何约束驱动模式，避免了动力学模型的微分代数方程求解以及违约抑制工作。

本文的线驱连续型机器人位－力混合动力学驱动方法，未考虑牵引线与节盘的摩擦，在机器人变形较大时，将会对机器人的线驱动力计算精度产生一定的影响，因此，下一步将引入摩擦模型进行研究。此外，由于摩擦的引入，机器人的动力学模型将体现出非光滑性，会对数值仿真的计算效率产生较大的影响，后续需针对非光滑系统研究如何提升计算效率。