本发明提出了一种多段线驱连续体机械臂动力学建模方法，首先确定连续体机械臂的每个关节段上各单元的质心位置坐标，并结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程和势能方程；结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；最后，结合多段线驱连续体机械臂的动能、势能以及广义力方程，建立多段线驱连续体机械臂的动力学模型。本发明的多段线驱连续体机械臂动力学建模方法，能够对一种存在强耦合效应的多段式线驱连续体机械臂建立具有高效率和精度的动力学模型，解决梁理论力学模型计算效率低的问题，同时该动力学模型利于机械臂控制系统的设计，具有很好的实时控制效果。
1.一种多段线驱连续体机械臂动力学建模方法，其特征在于，包括以下步骤：步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，基于分段常曲率假设，确定连续体机械臂的每个关节段上各单元的质心坐标，具体为：
第n个关节段中心弹性杆上任意弧长为sn的点相对于机械臂基底盘坐标系的位置坐标T
为rn1＝[xn yn zn 1] ，其相对于第n‑1个关节段受力盘末端坐标系的位置坐标为rn2＝[xn1 T
yn1 zn1 1]，则基于关节空间到任务空间的正运动学分析得到：
其中， Ti为第i个关节段机械臂的基坐标系O‑Xi‑1Yi‑1Zi‑1至末端坐标系O‑XiYiZi的齐次变换矩阵，βi、γi分别为第i个关节段机械臂末端弯曲角度和弯曲平面转角度，cβi、cγi表示余弦函数cosβi、cosγi缩写，sβi、sγi表示正弦函数sinβi、sinγi缩写；
式(2)表示第n‑1关节段机械臂关节空间至操作空间的运动学映射关系，Tn‑1为第n‑1个关节段末端坐标系相对于机械臂基底盘坐标系的齐次变换矩阵；式(4)为第1个关节段中心弹性杆上任意弧长为s1的点相对于机械臂基底盘的位置坐标，sk表示各关节段上第k个单元质心点处的弧长，K为各关节段上间隔盘的个数，l为各关节段的长度，结合式(1)～(5)，得到多段线驱连续体机械臂的每个关节段上各单元的质心坐标；
步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉‑拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
2.根据权利要求1所述的多段线驱连续体机械臂动力学建模方法，其特征在于，所述步骤2中的构建多段线驱连续体机械臂的动能方程具体包含以下步骤：步骤2‑1、求解多段线驱连续体机械臂的每个关节段上各单元的质心速度：由步骤1得到的第n个关节段上第k个单元质心点相对于机械臂基底盘坐标系的位置坐T
标表示为rnk1＝[xnk ynk znk 1] ，将其对时间t进行求导，得到k单元的质心速度步骤2‑2、计算第n个关节段上第k个单元上的动能：mnk＝m0+mp+3·(N‑n+1)·mc (7)其中，mnk表示第n个关节段上第k个单元的总质量，m0、mp、mc分别表示单元上中心弹性杆、间隔盘以及驱动线缆的质量，N表示多段线驱连续体机械臂的关节段数；
由式(6)、(7)，得到整个多段线驱连续体机械臂上的动能方程：其中，K表示各关节段上间隔盘的个数。
3.根据权利要求1所述的多段线驱连续体机械臂动力学建模方法，其特征在于，所述步骤3中的构建多段线驱连续体机械臂的势能方程具体为：第n个关节段上第k个单元上的弹性势能为：其中，EI1为中心弹性杆的刚度，EI2为驱动线缆的刚度，βn为第n个关节段机械臂末端弯曲角度，l为关节段的长度，N表示线驱连续体机械臂的段数；
4.根据权利要求1所述的多段线驱连续体机械臂动力学建模方法，其特征在于，所述步骤4中的构建多段线驱连续体机械臂的广义力方程具体为：由驱动线缆的几何关系，得到连续体机械臂第n个关节段的第m根驱动线缆长度变化公式为：
式中，r表示间隔盘的驱动线缆圆孔与中心杆圆孔之间的孔间距，θnm为第n个关节段的第m根驱动线缆在间隔盘分布角度，βq为第q个关节段机械臂末端弯曲角度，γq为第q个关节段机械臂末端弯曲平面转角；
驱动空间与关节空间的实时运动学关系为：式中，J为描述驱动空间与关节空间的雅可比矩阵，T
ΔL＝[ΔL11 ΔL12 ΔL13 … ΔLn1 ΔLn2 ΔLn3]为多段线驱连续体机械臂驱动线缆长度， 为多段线驱连续体机械臂的关节角度；
依据达朗贝尔原理，多段线驱连续体机械臂的广义力方程为：T
其中，F＝[F11 F12 F13 … FN1 FN2 FN3]为机械臂各驱动线缆所施加的驱动力。
5.根据权利要求1所述的多段线驱连续体机械臂动力学建模方法，其特征在于，所述步骤5中的构建多段线驱连续体机械臂的动力学模型具体为：多段线驱连续体机械臂系统拉格朗日第二类方程为：式中， 为多段线驱连续体机械臂的关节角度，βi、γi分别表示为第i关节段机械臂末端弯曲角度和弯曲平面转角，E为多段线驱连续体机械臂的动能方程，Ep为多段线驱连续体机械臂的势能方程，Qj为多段线驱连续体机械臂的广义力方程；
将上述步骤得到动能方程、势能方程和广义力方程带入公式(13)，得到多段线驱连续体机械臂的动力学微分方程：
其中，M为机械臂的惯性矩阵，C为科里奥利力矩阵和离心力矩阵，D为广义刚度矩阵，JtT
为描述驱动空间与关节空间的雅可比矩阵，F＝[F11 F12 F13 … FN1 FN2 FN3]为机械臂各驱动线缆所施加的驱动力。
6.一种多段线驱连续体机械臂动力学建模系统，其特征在于，包括以下模块：动能方程构建模块：用于推导连续体机械臂的每个关节段上各单元的质心速度，并结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
势能方程构建模块：推导连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
广义力方程构建模块：用于结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
动力学模型构建模块：用于结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉一拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
7.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1‑5中任一项所述方法的步骤。
8.一种计算机可存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1‑5中任一项所述的方法的步骤。
一种多段线驱连续体机械臂动力学建模方法
技术领域
[0001] 本发明属于机械臂建模技术领域，尤其涉及一种多段线驱连续体机械臂的动力学建模方法。
背景技术
[0002] 连续体机器人是一种模仿章鱼触手、大象鼻子等结构的新型仿生机器人，它不具有刚性连杆和关节形式，而是通过柔性本身的弹性变形，沿长度方向的连续弯曲运动，这种
结构形式使机械臂具有良好的柔顺性与灵活性，适应于狭小空间非结构环境下的作业要
求。连续体机器人在医疗微创手术、抢险搜救、核电站检修、深海水下作业、航空航天等领域
具有广阔的应用前景。
[0003] 目前关于连续体机械臂动力学建模的分析方法主要是：1)基于Cosserat杆理论的动力学建模分析方法，多是用于肌腱驱动与气、液压驱动的连续体机械臂上，并且该情况下
的弹性杆是变曲率变化的。基于Cosserat杆理论的精确动态建模需对一类二阶偏微分方程
进行数值求解，其求解过程是十分费时的，从而无法进行高效求解的实时控制。 2)基于常
曲率假设的虚功(凯恩)原理的动力学建模方法，主要用于肌腱驱动类的连续体机械臂，一
般还需在力学模型基础上进行数值模拟以利于控制系统设计。3)基于欧拉—拉格朗日的集
总参数建模方法，可建立简单的连续体机器人动力学模型，现已有学者将其应用于平面以
及单段式的线驱连续体机械臂的动力学建模。目前学者们在线驱动连续体机械臂的动力学
研究方面取得了一些成果，但对于存在耦合效应的多段式线驱连续体机械臂系统，尚不存
在一种能保证模型计算效率和精度，同时能满足控制器设计的动力学模型。
[0004] 基于上述情况，目前迫切需要一种新的多段线驱连续体机械臂动力学建模方法，能够对存在强耦合效应的多段式线驱连续体机械臂建立具有高效率和精度的动力学模型，
并且该动力学模型利于对机械臂控制系统进行设计。但是现有技术中尚无相关描述。
发明内容
[0005] 本发明所解决的技术问题在于提供一种多段线驱连续体机械臂动力学建模方法，从而对存在强耦合效应的多段式线驱连续体机械臂建立具有高效率和精度的动力学模型，
且该动力学模型利于对连续体机械臂控制系统进行设计。
[0006] 实现本发明目的的技术解决方案为：一种多段线驱连续体机械臂动力学建模方法，包括以下步骤：
[0007] 步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，基于分段常曲率假设，确定连续体机械臂的每个关节段上各单元
的质心坐标；
[0008] 步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
[0009] 步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
[0010] 步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
[0011] 步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
[0012] 本发明与现有技术相比，其显著优点：
[0013] (1)本发明的多段线驱连续体机械臂动力学建模方法，能够对一种存在强耦合效应的多段式线驱连续体机械臂建立具有高效率和精度的动力学模型；
[0014] (2)本发明的多段线驱连续体机械臂动力学建模方法基于伪刚体建模的思想简化机械臂模型，解决梁理论力学模型具有积分项导致计算效率低的问题；
[0015] (3)本发明的建模方法结合欧拉—拉格朗日的集总参数建模方法，将线驱连续体机械臂动力学模型拓宽至三维空间的多段式结构；同时该动力学模型利于机械臂控制系统
的设计，具有很好的实时控制效果，弥补了现有技术的空缺。
[0016] 下面结合附图对本发明作进一步详细描述。
附图说明
[0017] 图1为本发明的二段式线驱动连续体机械臂结构示意图。
[0018] 图2为本发明的二段式线驱动连续体机械臂的间隔盘分布示意图。
[0019] 图3为本发明的多段线驱连续体机械臂的一关节段单元划分示意图。
具体实施方式
[0020] 一种多段线驱连续体机械臂动力学建模方法，包括以下步骤：
[0021] 步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，利用伪刚体建模的思想，将中心弹性杆等效为轻质连杆，每个单元
的质心落在刚性盘的中心位置，基于分段常曲率假设，确定连续体机械臂的每个关节段上
各单元的质心坐标，具体为：
[0022] 第n个关节段中心弹性杆上任意弧长为sn的点相对于机械臂基底盘坐标系的位置T
坐标为rn1＝[xn yn zn 1] ，其相对于第n‑1个关节段受力盘末端坐标系的位置坐标为 rn2＝
T
[xn1 yn1 zn1 1]，则基于关节空间到任务空间的正运动学分析得到：
[0023]
[0024] 0Tn‑1＝0T1·1T2····n‑2Tn‑1 (2)
[0025] rn1＝0Tn‑1·rn2 (3)
[0026]
[0027]
[0028] 其中，i‑1Ti为第i个关节段机械臂的基坐标系O‑Xi‑1Yi‑1Zi‑1至末端坐标系O‑XiYiZi的齐次变换矩阵，βi、γi分别为第i个关节段机械臂末端弯曲角度和弯曲平面转角度，cβi、
cγi表示余弦函数cosβi、cosγi缩写，sβi、sγi表示正弦函数sinβi、sinγi缩写；
[0029] 式(2)表示第n‑1关节段机械臂关节空间至操作空间的运动学映射关系，0Tn‑1为第 n‑1个关节段末端坐标系相对于机械臂基底盘坐标系的齐次变换矩阵；式(3)所表示的第n
个关节段中心弹性杆上任意弧长为sn的点相对于机械臂基底盘坐标系的位置坐标由式
(1)、(2)计算得到；式(4)为第1个关节段中心弹性杆上任意弧长为s1的点相对于机械臂基
底盘的位置坐标，sk表示各关节段上第k个单元质心点处的弧长，K为各关节段上间隔盘的
个数，l为各关节段的长度，结合式(1)～(5)，得到多段线驱连续体机械臂的每个关节段上
各单元的质心坐标。
[0030] 步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程，具体包括以下步
骤：
[0031] 步骤2‑1、求解多段线驱连续体机械臂的每个关节段上各单元的质心速度：
[0032] 由步骤1得到的第n个关节段上第k个单元质心点相对于机械臂基底盘坐标系的位T
置坐标表示为rnk1＝[xnk ynk znk 1] ，将其对时间t进行求导，得到k单元的质心速度
[0033] 步骤2‑2、计算第n个关节段上第k个单元上的动能：
[0034]
[0035] mnk＝m0+mp+3·(N‑n+1)·mc (7)
[0036] 其中，mnk表示第n个关节段上第k个单元的总质量，m0、mp、mc分别表示单元上中心弹性杆、间隔盘以及驱动线缆的质量，N表示多段线驱连续体机械臂的关节段数；
[0037] 由式(6)、(7)，得到整个多段线驱连续体机械臂上的动能方程：
[0038]
[0039] 其中，K表示各关节段上间隔盘的个数。
[0040] 步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程，具体为：
[0041] 相较于连续体机械臂的弹性势能，其重力势能可以忽略，第n个关节段上第k个单元上的弹性势能为：
[0042]
[0043] 其中，EI1为中心弹性杆的刚度，EI2为驱动线缆的刚度，βn为第n个关节段机械臂末端弯曲角度，l为关节段的长度，N表示线驱连续体机械臂的段数；
[0044] 得到整个多段线驱连续体机械臂的势能方程：
[0045]
[0046] 步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程，具体为：
[0047] 由驱动线缆的几何关系，得到连续体机械臂第n个关节段的第m根驱动线缆长度变化公式为：
[0048]
[0049] 式中，r表示间隔盘的驱动线缆圆孔与中心杆圆孔之间的孔间距，θnm为第n个关节段的第m根驱动线缆在间隔盘分布角度，βq为第q个关节段机械臂末端弯曲角度，γq为第q个
关节段机械臂末端弯曲平面转角；
[0050] 驱动空间与关节空间的实时运动学关系为：
[0051]
[0052] 式中，Jt为描述驱动空间与关节空间的雅可比矩阵，ΔL＝[ΔL11 ΔL12 ΔL13 … T
ΔLn1 ΔLn2 ΔLn3] 为多段线驱连续体机械臂驱动线缆长度， 为
多段线驱连续体机械臂的关节角度；
[0053] 在不考虑机械臂受外力的情况下，依据达朗贝尔原理，多段线驱连续体机械臂的广义力方程为：
[0054] Q＝JtTF (12)
[0055] 其中，F＝[F11 F12 F13 … FN1 FN2 FN3]T为机械臂各驱动线缆所施加的驱动力。
[0056] 步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型，具体为：
[0057] 多段线驱连续体机械臂系统拉格朗日第二类方程为：
[0058]
[0059] 式中， 为多段线驱连续体机械臂的关节角度，βi、γi分别表示为第i关节段机械臂末端弯曲角度和弯曲平面转角，E为多段线驱连续体机械臂的动能
方程，Ep为多段线驱连续体机械臂的势能方程，Qj为多段线驱连续体机械臂的广义力方程；
[0060] 将上述步骤得到动能方程、势能方程和广义力方程带入公式(13)，得到多段线驱连续体机械臂的动力学微分方程：
[0061]
[0062] 其中，M为机械臂的惯性矩阵，C为科里奥利力矩阵和离心力矩阵，D为广义刚度矩T
阵，Jt为描述驱动空间与关节空间的雅可比矩阵， F＝[F11 F12 F13 … FN1 FN2 FN3]为机械
臂各驱动线缆所施加的驱动力。
[0063] 一种多段线驱连续体机械臂动力学建模系统，包括以下模块：
[0064] 动能方程构建模块：用于推导连续体机械臂的每个关节段上各单元的质心速度，并结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
[0065] 势能方程构建模块：推导连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
[0066] 广义力方程构建模块：用于结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
[0067] 动力学模型构建模块：用于结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
[0068] 一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
[0069] 步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，基于分段常曲率假设，确定连续体机械臂的每个关节段上各单元
的质心坐标；
[0070] 步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
[0071] 步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
[0072] 步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
[0073] 步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
[0074] 一种计算机可存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
[0075] 步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，基于分段常曲率假设，确定连续体机械臂的每个关节段上各单元
的质心坐标；
[0076] 步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程；
[0077] 步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程；
[0078] 步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程；
[0079] 步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型。
[0080] 下面结合实施例对本发明作进一步描述。
[0081] 实施例
[0082] 结合图1、2，以一个处于驱动线缆放松状态的两段式线驱连续体机械臂来介绍多段线驱连续体机械臂的机械结构。
[0083] 机械臂每段长度为l，两段式机械臂总长度为2l，每段有末端弯曲角度β和弯曲平面转角γ两个自由度，整体机械结构形状是圆台形，由中心弹性杆、驱动线缆和支撑圆盘构
成，每段机械臂由K个间隔盘和1个受力盘组成；
[0084] 两段式机械臂共有6根驱动绳缆，均分为两组，第1组驱动绳缆在通过K个间隔盘后固结在第一段的受力盘上，第2组驱动绳缆则通过所有间隔盘并固结在第二段的受力盘上。
第一段机械臂的间隔盘上均匀分布6个圆孔，用于穿过6根驱动绳缆，第二段机械臂的间隔
盘上均匀分布3个圆孔，圆孔i、j(i＝1,2,j＝1,2…6)表示第i段机械臂的第j根驱动绳缆穿
过该孔，所有间隔盘的驱动绳缆圆孔与中心杆圆孔之间的孔间距r相等。以此类推，通过串
联若干段关节，便可得到更多自由度的连续体机器人。
[0085] 结合图3，一种多段线驱连续体机械臂动力学建模方法，包括以下步骤：
[0086] 步骤1、将多段线驱连续体机械臂的每个关节段上的某一刚性盘与其相邻的中心弹性杆划分为一个单元，利用伪刚体建模的思想，将中心弹性杆等效为轻质连杆，每个单元
的质心落在刚性盘的中心位置，基于分段常曲率假设，确定连续体机械臂的每个关节段上
各单元的质心坐标，具体为：
[0087] 第n个关节段中心弹性杆上任意弧长为sn的点相对于机械臂基底盘坐标系的位置T
坐标为rn1＝[xn yn zn 1] ，其相对于第n‑1个关节段受力盘末端坐标系的位置坐标为 rn2＝
T
[xn1 yn1 zn1 1]，则基于关节空间到任务空间的正运动学分析得到：
[0088]
[0089] 0Tn‑1＝0T1·1T2…n‑2Tn‑1 (2)
[0090] rn1＝0Tn‑1·rn2 (3)
[0091]
[0092]
[0093] 其中，i‑1Ti为第i个关节段机械臂的基坐标系O‑Xi‑1Yi‑1Zi‑1至末端坐标系O‑XiYiZi的齐次变换矩阵，βi、γi分别为第i个关节段机械臂末端弯曲角度和弯曲平面转角度，cβi、
cγi表示余弦函数cosβi、cosγi缩写，sβi、sγi表示正弦函数sinβi、sinγi缩写；
[0094] 式(2)表示第n‑1关节段机械臂关节空间至操作空间的运动学映射关系，0Tn‑1为第 n‑1个关节段末端坐标系相对于机械臂基底盘坐标系的齐次变换矩阵；式(3)所表示的第n
个关节段中心弹性杆上任意弧长为sn的点相对于机械臂基底盘坐标系的位置坐标由式
(1)、(2)计算得到；式(4)为第1个关节段中心弹性杆上任意弧长为s1的点相对于机械臂基
底盘的位置坐标，sk表示各关节段上第k个单元质心点处的弧长，K为各关节段上间隔盘的
个数，l为各关节段的长度，结合式(1)～(5)，得到多段线驱连续体机械臂的每个关节段上
各单元的质心坐标。
[0095] 步骤2、基于质心坐标，确定连续体机械臂的每个关节段上各单元的质心速度，结合各关节段的速度和质量信息，构建多段线驱连续体机械臂的动能方程，具体包括以下步
骤：
[0096] 步骤2‑1、求解多段线驱连续体机械臂的每个关节段上各单元的质心速度：
[0097] 由步骤1得到的第n个关节段上第k个单元质心点相对于机械臂基底盘坐标系的位T
置坐标表示为rnk1＝[xnk ynk znk 1] ，将其对时间t进行求导，得到k单元的质心速度
[0098] 步骤2‑2、计算第n个关节段上第k个单元上的动能：
[0099]
[0100] mnk＝m0+mp+3·(N‑n+1)·mc (7)
[0101] 其中，mnk表示第n个关节段上第k个单元的总质量，m0、mp、mc分别表示单元上中心弹性杆、间隔盘以及驱动线缆的质量，N表示多段线驱连续体机械臂的关节段数；
[0102] 由式(6)、(7)，得到整个多段线驱连续体机械臂上的动能方程：
[0103]
[0104] 其中，K表示各关节段上间隔盘的个数。
[0105] 步骤3、确定连续体机械臂的各关节段的弹性势能，构建多段线驱连续体机械臂的势能方程，具体为：
[0106] 相较于连续体机械臂的弹性势能，其重力势能可以忽略，第n个关节段上第k个单元上的弹性势能为：
[0107]
[0108] 其中，EI1为中心弹性杆的刚度，EI2为驱动线缆的刚度，βn为第n个关节段机械臂末端弯曲角度，l为关节段的长度，N表示线驱连续体机械臂的段数；
[0109] 得到整个多段线驱连续体机械臂的势能方程：
[0110]
[0111] 步骤4、结合驱动空间与关节空间的映射关系，构建多段线驱连续体机械臂的广义力方程，具体为：
[0112] 由驱动线缆的几何关系，得到连续体机械臂第n个关节段的第m根驱动线缆长度变化公式为：
[0113]
[0114] 式中，r表示间隔盘的驱动线缆圆孔与中心杆圆孔之间的孔间距，θnm为第n个关节段的第m根驱动线缆在间隔盘分布角度，βq为第q个关节段机械臂末端弯曲角度，γq为第q个
关节段机械臂末端弯曲平面转角；
[0115] 驱动空间与关节空间的实时运动学关系为：
[0116]
[0117] 式中，Jt为描述驱动空间与关节空间的雅可比矩阵，ΔL＝[ΔL11 ΔL12 ΔL13 … T
ΔLn1 ΔLn2 ΔLn3] 为多段线驱连续体机械臂驱动线缆长度， 为
多段线驱连续体机械臂的关节角度；
[0118] 在不考虑机械臂受外力的情况下，依据达朗贝尔原理，多段线驱连续体机械臂的广义力方程为：
[0119] Q＝JtTF (12)
[0120] 其中，F＝[F11 F12 F13 … FN1 FN2 FN3]T为机械臂各驱动线缆所施加的驱动力。
[0121] 步骤5、结合多段线驱连续体机械臂的动能、势能以及广义力方程，基于欧拉—拉格朗日建模原理，构建多段线驱连续体机械臂的动力学模型，具体为：
[0122] 多段线驱连续体机械臂系统拉格朗日第二类方程为：
[0123]
[0124] 式中， 为多段线驱连续体机械臂的关节角度，βi、γi分别表示为第i关节段机械臂末端弯曲角度和弯曲平面转角，E为多段线驱连续体机械臂的动能
方程，Ep为多段线驱连续体机械臂的势能方程，Qj为多段线驱连续体机械臂的广义力方程；
[0125] 将上述步骤得到动能方程、势能方程和广义力方程带入公式(13)，得到多段线驱连续体机械臂的动力学微分方程：
[0126]
[0127] 其中，M为机械臂的惯性矩阵，C为科里奥利力矩阵和离心力矩阵，D为广义刚度矩T
阵，Jt为描述驱动空间与关节空间的雅可比矩阵， F＝[F11 F12 F13 … FN1 FN2 FN3]为机械
臂各驱动线缆所施加的驱动力。
[0128] 由上述实施例可知，本发明的建模方法可以构建立具有高效率和精度的动力学模型，基于伪刚体建模的思想简化机械臂模型，解决梁理论力学模型具有积分项导致计算效
率低的问题；再结合欧拉—拉格朗日的集总参数建模方法，将线驱连续体机械臂动力学模
型拓宽至三维空间的多段式结构；同时该动力学模型利于机械臂控制系统的设计，具有很
好的实时控制效果，弥补了现有技术的空缺。
