一种基于科瑟拉弹性杆模型的线缆的建模方法及装置

摘要

本发明的实施例提供了一种基于科瑟拉弹性杆模型的线缆的建模方法及装置，其中该建模方法包括：对待建模线缆的中心线进行离散处理，得到多条中心线段；建立待建模线缆的弹性势能与每条中心线段中点的四元数和每条中心线段的端点的坐标的第一函数关系式；根据第一函数关系式，确定出待建模线缆的每条中心线段的端点的坐标，本发明的实施例能真实、稳定、高效地对线缆进行物理建模。

说明书

技术领域

本发明涉及机械工程领域，特别涉及一种基于科瑟拉弹性杆模型的线缆的 建模方法及装置。

背景技术

作为能量和信号的传输通道，线缆在机电产品中应用广泛，而线缆的布局 和敷设安装质量直接影响机电产品的质量及可靠性。近年来，在虚拟环境中进 行线缆布局设计与敷设过程仿真逐渐成为国内外的研究热点。

柔性线缆的模型表达是进行虚拟环境下线缆敷设过程仿真的基础，该模型 不仅要能支持电缆的几何和拓扑特性，还要能支持线缆的物理特性表达，并满 足虚拟现实系统特有的实时性要求。与布料和人体组织等柔性体不同，线缆最 大的特点在于其长度通常远大于直径，工程中通常以线缆的中心线为扫描路径， 以一定的截面信息扫描来形成线缆的三维模型，因此中心线的确定是线缆建模 的关键，最早提出的方法有采用折线段，或自由曲线如贝塞尔曲线等表示线缆 的中心线，但这些方法属于线缆的几何建模，没有考虑线缆的物理属性，进行 线缆敷设过程仿真时缺乏真实性。

由于柔性物体的形状不像刚体那样固定不变，而是随着外界条件的不同发 生改变，因此建模难度很大。线缆物理建模是典型的柔性体的物理建模，由于 线缆敷设过程中，线缆整体的运动与机器人机械臂的运动具有相似性，基于该 思想，魏发远等提出了一种基于蛇形机器人的线缆建模与敷设仿真方法，将来 源于机器人理论的逆运动学方法应用于线缆建模中。德国Hergenrother等开发 的虚拟装配系统中采用了类似的模型，通过一系列相连接的刚性杆表示线缆， 每个连接处称为关节，在关节处加入扭簧和质点以考虑抗弯刚度和重力这两个 物理属性，当线缆端部移动到某一位置时，采用能量优化法和逆运动学方法， 求解出每一关节所应有的变量并且系统具有的能量最小。以上建模方法能够体 现线缆在敷设仿真过程中“长度不变”的性质，但考虑的线缆物理参数较少， 真实性不足。

又由于线缆在敷设过程中的局部应变很小，基本保持在材料的弹性范围内， 因此有学者将其本构关系抽象为弹性关系，并用弹性杆模型进行线缆物理建模。 弹性细杆静力学理论是Kirchhoff在1859年建立的，工程中绳索、钻杆、纤维 等都曾将弹性细杆作为其力学模型。刘检华等提出的以Kirchhoff弹性细杆非 线性力学理论为基础的虚拟环境下活动线缆建模与运动仿真方法，对活动线缆 的仿真取得了较好效果，但该方法需要求解微分方程组的边界值问题，并利用 “打靶法”在已知两端的位置和朝向基础上对线缆的整体外形进行求解，其存在 问题是求解所需要的初值往往难以给定，而且求解时间所需较长，无法满足线 缆敷设过程仿真的实时性需求。

科瑟拉理论作为Kirechhoff理论的改进，考虑了弹性杆的轴向线应变和弯 曲剪应变等物理参数，建立更精确的平衡方程。科瑟拉理论首先由Pai等用于 弹性杆这类细长的可变形结构的建模，该方法同样通过“打靶法”数值求解微分 方程得到杆的静态外形；由于数值解法在初始条件上难以确定，同时在处理自 接触和与其他物体接触的问题上也存在困难，之后的方法多关注于模型的离散 表达。Jonas Spillmann等提出用于柔性杆动力学模型，该模型以科瑟拉理论为 基础导出杆件的连续能量表达式，通过对杆的离散计算出杆的每个单元具有的 能量，进而由拉格朗日方程得到杆的动力学微分方程，数值求解该微分方程得 到杆的动态变化。由于动力学模型需要较长时间进入稳定状态，黄劲等提出了 一个稳定快速的优化策略对线缆进行形变模拟，并着重研究了大步长、准静态 模拟过程中的接触处理这一关键问题。MireilleGr′egoire等提出了通用弹簧质 点模型表达线缆这类柔性零件的外形，并以科瑟拉理论对其弯曲和扭转进行建 模，通过能量最小化过程求解线缆的平衡状态，这种方法避免了动力学模型中 的振动现象，因此具有较好的稳定性。但该方法采用比例积分控制实现模型的 定长约束，引入了约束力变量，通过迭代的方法确定约束力，这使得模型变得 复杂也降低了求解效率；四元数标准化约束中，为了避免线缆外形出现“V” 形的异常形变，改变了约束表达式，将四元数的模变为其倒数，这使得雅克比 矩阵变得复杂，降低了求解效率；在优化问题求解中级联使用了多种优化方法 以保证算法收敛，但初值的选择仍对求解效率和算法收敛性影响较大。

综上所述，目前不能真实、稳定、高效地对线缆进行物理建模。

发明内容

本发明实施例的目的在于提供一种基于科瑟拉弹性杆模型的线缆的建模 方法及装置，能真实、稳定、高效地对线缆进行物理建模。

为了达到上述目的，本发明的实施例提供了一种基于科瑟拉弹性杆模型的 线缆的建模方法，该建模方法包括：

对待建模线缆的中心线进行离散处理，得到多条中心线段；

建立待建模线缆的弹性势能与每条中心线段中点的四元数和每条中心线 段的端点的坐标的第一函数关系式；

根据第一函数关系式，确定出待建模线缆的每条中心线段的端点的坐标。

其中，在对待建模线缆的中心线进行离散处理，得到多条中心线段之前， 建模方法还包括：

获取待建模线缆的长度、截面形状及尺寸、杨氏模量、切变模量以及用于 固定待建模线缆的各接头的安装位置。

其中，建立待建模线缆的弹性势能与每条中心线段中点的四元数和每条中 心线段的端点的坐标的第一函数关系式，具体包括：

根据待建模线缆的长度和各接头的安装位置，分别建立每条中心线段的拉 伸形变的弹性势能与每条中心线段的端点的坐标的第二函数关系式以及每相 邻两条中心线段的两个中点之间的中心线段的弯曲及扭转变形的弹性势能与 每条中心线段的中点的四元数的第三函数关系式；

根据第二函数关系式和第三函数关系式，计算得到第一函数关系式。

其中，分别建立每条中心线段的拉伸形变的弹性势能与每条中心线段的端 点的坐标的第二函数关系式以及每相邻两条中心线段的两个中点之间的中心 线段的弯曲及扭转变形的弹性势能与每条中心线段的中点的四元数的第三函 数关系式，具体包括：

通过公式

$V_s\left[i\right]=\frac{1}{2}{l}_i{k}_s{\left(\frac{\sqrt{(r_{i+1}-r_i)·(r_{i+1}-r_i)}}{l_i}-1\right)}^2$

建立每条中心线段的拉伸形变的弹性势能与每条中心线段的端点的坐标 的第二函数关系式，其中V_s[i]表示第i条中心线段的拉伸形变的弹性势能，l_i表 示当待建模线缆处于自然状态时，第i条中心线段的长度，k_s表示抗拉刚度， r_i+1和r_i分别表示在参考坐标系中第i条中心线段的两个端点的坐标；

通过公式计算得到每相邻两条中心线段的两个中点之间的中 心线段的任一点处的四元数对弧坐标s的微分，其中λ′_j表示第j+1条中心线段 的中点与第j条中心线段的中点之间的中心线段的任一点处的四元数对弧坐标 s的微分，λ_j+1表示第j+1条中心线段的中点的四元数，λ_j表示第j条中心线 段的中点的四元数，l_j表示当待建模线缆处于自然状态时，第j条中心线段和 第j+1条中心线段的和的一半；

通过公式计算得到每相邻两条中心线段的两个中点之间的中 心线段的任一点处的四元数，其中表示第j+1条中心线段的中点与第j条中 心线段的中点之间的中心线段的任一点处的四元数；

通过公式

$\left(\begin{array}{c}{V}_b\left[j\right]=\frac{l_j}{2}[k_{b1}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{3,j}^{\prime }+\overline{{\lambda }_{3,j}}{\lambda }_{2,j}^{\prime }\right)-{\hat{u}}_{1,j}\right)}^2\\ +k_{b2}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{2,j}^{\prime }+\overline{{\lambda }_{1,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{1,j}^{\prime }\right)-{\hat{u}}_{2,j}\right)}^2\\ +k_{b3}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{2,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{0,j}^{\prime }\right)-{\hat{u}}_{3,j}\right)}^2]\end{array}\right)$

建立每相邻两条中心线段的两个中点之间的中心线段的弯曲及扭转变形 的弹性势能与每条中心线段的中点的四元数的第三函数关系式，其中V_b[j]表示 第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的弯曲及扭 转变形的弹性势能，k_b1、k_b2以及k_b3分别表示在三个坐标轴方向上的抗弯和 抗扭刚度，且当待建模线缆的截面为圆形时，E表示杨氏模量，G表示切变模量，r表示待建模线缆的截面半径，表示第 j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的 四元数的第一个参数，表示第j+1条中心线段的中点与第j条中心线段的 中点之间的中心线段的任一点处的四元数的第二个参数，表示第j+1条中 心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的四元数的 第三个参数，表示第j+1条中心线段的中点与第j条中心线段的中点之间 的中心线段的任一点处的四元数的第四个参数，λ′_0,j表示第j+1条中心线段的 中点与第j条中心线段的中点之间的中心线段的四元数的第一个参数对弧坐标 s的微分，λ′_1,j表示第j+1条中心线段的中点与第j条中心线段的中点之间的中 心线段的四元数的第二个参数对弧坐标s的微分，λ′_2,j表示第j+1条中心线段 的中点与第j条中心线段的中点之间的中心线段的四元数的第三个参数对弧坐 标s的微分，λ′_3,j表示第j+1条中心线段的中点与第j条中心线段的中点之间的 中心线段的四元数的第四个参数对弧坐标s的微分，以及分别表示 第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段在三个坐标 轴方向上的原始弯扭度。

其中，根据第二函数关系式和第三函数关系式，计算得到第一函数关系式， 具体为：

通过公式

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

计算得到第一函数关系式，其中V₁表示待建模线缆的弹性势能，i和j均表 示中心线段的序号，N表示在对待建模线缆的中心线进行离散处理时的节点数。

其中，根据第一函数关系式，确定出待建模线缆的每条中心线段的端点的 坐标，具体包括：

通过罚函数对公式

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

的约束条件进行转换，得到第一罚函数ds和第二罚函数其中V_c和V_n分别表示第一罚 函数和第二罚函数，s表示弧坐标，k_c和k_n均表示罚因子系数，r’表示参考坐 标系中的向量r对弧坐标s的微分，d₃表示主坐标系中z轴方向的单位矢量， λ表示四元数；

根据第一罚函数和第二罚函数，建立待建模线缆的罚函数的能量函数；

根据罚函数的能量函数和第一函数关系式，确定出待建模线缆的每条中心 线段的端点的坐标。

其中，根据第一罚函数和第二罚函数，计算待建模线缆的罚函数的能量函 数，具体包括：

根据第一罚函数，通过公式$V_c\left[i\right]=\frac{1}{2}{\int }_0^{l_i}{k}_c({r^{,}}_i-d_3({\lambda }_i))·({r^{,}}_i-$$d_3\left({\lambda }_i\right))$ds计算得到每条中心线段的第一罚函数的能量函数，其中V_c[i]表示第 i条中心线段的第一罚函数的能量，r’_i表示参考坐标系中第i条中心线段的向 量r对弧坐标s的微分，d₃(λ_i)表示用第i条中心线段的四元数表达主坐标系 中z轴方向的单位矢量；

根据第二罚函数，通过公式计算得到每条 中心线段的第二罚函数的能量函数，其中V_n[i]表示第i条中心线段的第二罚函 数的能量，λ_i表示第i条中心线段的四元数；

根据公式

$V_2=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_c\left[i\right]+\underset{i=1}{\overset{N-1}{\Sigma }}{V}_n\left[i\right]$

计算得到待建模线缆的罚函数的能量函数，其中V₂表示待建模线缆的罚函 数的能量。

其中，根据罚函数的能量函数和第一函数关系式，确定出待建模线缆的每 条中心线段的端点的坐标，具体包括：

计算出罚函数的能量函数和弹性势能的和值；

通过非线性最小二乘法，计算出和值的最小值；

根据最小值，确定出待建模线缆的每条中心线段的端点的坐标。

本发明的实施例还提供了一种基于科瑟拉弹性杆模型的线缆的建模装置， 该建模装置包括：

第一获取模块，用于对待建模线缆的中心线进行离散处理，得到多条中心 线段；

第一确定模块，用于建立待建模线缆的弹性势能与每条中心线段中点的四 元数和每条中心线段的端点的坐标的第一函数关系式；

第二确定模块，用于根据第一函数关系式，确定出待建模线缆的每条中心 线段的端点的坐标。

其中，建模装置还包括：

第二获取模块，用于获取待建模线缆的长度、截面形状及尺寸、杨氏模量、 切变模量以及用于固定待建模线缆的各接头的安装位置。

本发明的上述方案至少包括以下有益效果：

在本发明的实施例中，通过对待建模线缆的中心线进行离散处理，得到多 条中心线段，并建立待建模线缆的弹性势能与每条中心线段中点的四元数和每 条中心线段的端点的坐标的第一函数关系式，进而根据第一函数关系式，确定 出当待建模线缆处于最小弹性势能状态下时，待建模线缆的每条中心线段的端 点的坐标，解决了不能真实、稳定、高效地对线缆进行物理建模的问题，达到 了真实、稳定、高效地对线缆进行物理建模的效果。

附图说明

图1为本发明第一实施例中基于科瑟拉弹性杆模型的线缆的建模方法的 流程图；

图2为本发明第一实施例中弹性杆坐标系的示意图；

图3为本发明第一实施例中Frenet坐标系的示意图；

图4为本发明第一实施例中求解时间与离散点数目的关系图；

图5为本发明第二实施例中基于科瑟拉弹性杆模型的线缆的建模装置的 结构示意图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了 本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被 这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本 公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

第一实施例

如图1所示，本发明的第一实施例提供了一种基于科瑟拉弹性杆模型的线 缆的建模方法，该建模方法包括：

步骤S11，对中心线进行离散处理，得到多条中心线段；

在本发明的第一实施例中，对中心线进行离散处理的方式可以为：从中心 线上取N个节点，即将中心线分成N-1条中心线段，其中N可以根据实际需 要进行取值。且此时待建模线缆的状态参数(包括每条中心线段两端点的坐标 和每条中心线段中点的四元数)是已知的。

步骤S12，建立待建模线缆的弹性势能与每条中心线段中点的四元数和每 条中心线段的端点的坐标的第一函数关系式；

在本发明的第一实施例中，可以基于科瑟拉弹性杆理论，根据每条中心线 段的中点的四元数和每条中心线段的端点的坐标，建立待建模线缆的弹性势能 与每条中心线段中点的四元数和每条中心线段的端点的坐标的第一函数关系 式，具体的建立过程会在后文详细阐述。

步骤S13，根据第一函数关系式，确定出待建模线缆的每条中心线段的端 点的坐标。

在本发明的第一实施例中，可通过对获取到的待建模线缆的中心线进行离 散处理后，得到多条(例如N-1)中心线段，然后再根据每条中心线段的中点 的四元数和每条中心线段的端点的坐标，建立待建模线缆的弹性势能与每条中 心线段中点的四元数和每条中心线段的端点的坐标的第一函数关系式，最后根 据该第一函数关系式，确定出当待建模线缆处于最小弹性势能状态下时，待建 模线缆的每条中心线段的端点的坐标，从而真实、稳定、高效地对待建模线缆 进行物理建模。

其中，在本发明的第一实施例中，在执行步骤S12之前，上述建模方法还 包括：获取待建模线缆的长度、截面形状及尺寸(当截面为圆形时，对应的尺 寸为圆形的半径)、杨氏模量(包括拉伸变形中的杨氏模量和弯曲变形时的杨 氏模量)、切变模量以及用于固定待建模线缆的各接头的安装位置。以便后续 计算第一函数关系式。

其中，在本发明的第一实施例中，上述步骤S12具体包括：首先根据待建 模线缆的长度和各接头的安装位置，分别建立每条中心线段的拉伸形变的弹性 势能与每条中心线段的端点的坐标的第二函数关系式以及每相邻两条中心线 段的两个中点之间的中心线段的弯曲及扭转变形的弹性势能与每条中心线段 的中点的四元数的第三函数关系式；然后再根据第二函数关系式和第三函数关 系式，计算得到第一函数关系式。

在本发明的第一实施例中，在详细阐述建立第一函数关系式之前，先简单 阐述一下科瑟拉弹性杆理论。

在科瑟拉弹性杆理论中，弹性杆(例如上述线缆)为细长的可变形体，它 可以呈现出弯曲、扭转等复杂的形态。弹性杆截面的几何中心连成空间曲线C 称为弹性杆的中心线，在刚性截面假定基础上，弹性杆的几何形态由截面沿中 心线的移动和转动所体现的，因此可以通过弹性杆中心线构建出弹性杆的外形。

为了方便描述弹性杆的空间姿态，如图2～图3所示，在此建立多个坐标 系(即参考坐标系、Frenet坐标系以及主轴坐标系)。对于弹性杆的中心线C， 以曲线上的一固定点P_n为原点建立弧坐标系s，以空间中一固定点O建立参考 坐标系(O-ξηζ)，O点到空间中一点的向量为r。在曲线上任意一点P可定义一 个依附于曲线的右手坐标系(P-NBT)称为Frenet坐标系。其中T为该点处切线 方向上的单位矢量，N为该点处的法线方向上的单位矢量，矢量B由B＝T×N得 到。

由于弹性杆的几何形态由截面沿中心线的移动和转动所体现，因此在截面 中心点P建立与刚性截面相固连的主轴坐标系(P-xyz)，各坐标轴的单位矢量称 为基矢量为d₁、d₂、d₃，其中z轴与P点处的切线轴T重合，即d₃＝T。主轴 坐标系x轴与Frenet坐标系的N轴的夹角为θ，夹角θ为截面扭转变形的体现。

在此采用四元数表示旋转，主轴坐标系的姿态通过参考坐标系的旋转量表 示。主轴坐标系基矢量为d₁、d₂、d₃，参考坐标系基矢量为d₁⁰、d₂⁰、d₃⁰， 即四元数λ＝(λ₀λ₁λ₂λ₃)^T表示的旋转矩阵为

$R=\left(\begin{array}{ccc}2\left({\lambda }_0^2+{\lambda }_1^2\right)-1& 2\left({\lambda }_1{\lambda }_2-{\lambda }_0{\lambda }_3\right)& 2\left({\lambda }_1{\lambda }_3-{\lambda }_0{\lambda }_2\right)\\ 2\left({\lambda }_2{\lambda }_1-{\lambda }_0{\lambda }_3\right)& 2\left({\lambda }_0^2+{\lambda }_2^2\right)-1& 2\left({\lambda }_2{\lambda }_3-{\lambda }_0{\lambda }_1\right)\\ 2\left({\lambda }_3{\lambda }_1-{\lambda }_0{\lambda }_2\right)& 2\left({\lambda }_3{\lambda }_2-{\lambda }_0{\lambda }_1\right)& 2\left({\lambda }_0^2+{\lambda }_3^2\right)-1\end{array}\right)$

其中四元数满足约束并且四元数参数λ_k(0≤k≤3)都为实 数。

矢量d₁⁰、d₂⁰、d₃⁰经过转动得到主轴的坐标系的基矢量为

$d_1=\left(\begin{array}{c}2\left({\lambda }_0^2+{\lambda }_1^2\right)-1\\ 2\left({\lambda }_2{\lambda }_1+{\lambda }_0{\lambda }_3\right)\\ 2\left({\lambda }_3{\lambda }_1-{\lambda }_0{\lambda }_2\right)\end{array}\right),d_2=\left(\begin{array}{c}2\left({\lambda }_1{\lambda }_2-{\lambda }_0{\lambda }_3\right)\\ 2\left({\lambda }_0^2+{\lambda }_2^2\right)-1\\ 2\left({\lambda }_3{\lambda }_2+{\lambda }_0{\lambda }_1\right)\end{array}\right),d_3\left(\begin{array}{c}2\left({\lambda }_1{\lambda }_3+{\lambda }_0{\lambda }_2\right)\\ 2\left({\lambda }_2{\lambda }_3-{\lambda }_0{\lambda }_1\right)\\ 2\left({\lambda }_0^2+{\lambda }_3^2\right)-1\end{array}\right)$

此外，弹性杆的弯曲、扭转变形的剧烈程度通过弯扭度矢量u表示，中心 线上任意一点P处的弯扭度是该点处主轴坐标系的角位移对弧坐标s的变化率。 与角速度的概念类似，不同的是角速度是角位移相对于时间t的变化率。弯扭 度等价于动点P沿着中心线沿弧坐标正向以单位速度做匀速运动时主轴坐标 系的角速度。

在计算中使用四元数对弯扭度矢量进行表达，其中四元数与旋转的角速度 矢量的关系为

或$\left(\begin{array}{c}0\\ {\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right)=2{\Lambda }^T\left(\begin{array}{c}{\stackrel{·}{\lambda }}_0\\ {\stackrel{·}{\lambda }}_1\\ {\stackrel{·}{\lambda }}_2\\ {\stackrel{·}{\lambda }}_3\end{array}\right)$

其中ω是角度速度矢量在主轴坐标系中的表达，为四元数λ的共轭四元 数，为四元数λ对时间t的微分。为了便于进行四元数运算，用矩阵相乘代 替四元数相乘，其中矩阵

$\Lambda =\left(\begin{array}{cccc}{\lambda }_0& -{\lambda }_1& -{\lambda }_2& -{\lambda }_3\\ {\lambda }_1& {\lambda }_0& -{\lambda }_3& {\lambda }_2\\ {\lambda }_2& {\lambda }_3& {\lambda }_0& -{\lambda }_1\\ {\lambda }_3& -{\lambda }_2& {\lambda }_1& {\lambda }_0\end{array}\right)$

计算得到在主轴坐标系中角速度矢量的各个分量为

$\left(\begin{array}{c}{{\omega }^{\prime }}_1\\ {{\omega }^{\prime }}_2\\ {{\omega }^{\prime }}_3\end{array}\right)=2\left(\begin{array}{c}{\lambda }_0{\stackrel{·}{\lambda }}_1-{\lambda }_1{\stackrel{·}{\lambda }}_0-{\lambda }_2{\stackrel{·}{\lambda }}_3+{\lambda }_3{\stackrel{·}{\lambda }}_2\\ {\lambda }_0{\stackrel{·}{\lambda }}_2+{\lambda }_1{\stackrel{·}{\lambda }}_3-{\lambda }_2{\stackrel{·}{\lambda }}_0-{\lambda }_3{\stackrel{·}{\lambda }}_1\\ {\lambda }_0{\stackrel{·}{\lambda }}_3-{\lambda }_1{\stackrel{·}{\lambda }}_2+{\lambda }_2{\stackrel{·}{\lambda }}_1-{\lambda }_3{\stackrel{·}{\lambda }}_0\end{array}\right)$

由于弯扭度矢量与角速度都是角位移的变化率，将

$\left(\begin{array}{c}{{\omega }^{\prime }}_1\\ {{\omega }^{\prime }}_2\\ {{\omega }^{\prime }}_3\end{array}\right)=2\left(\begin{array}{c}{\lambda }_0{\stackrel{·}{\lambda }}_1-{\lambda }_1{\stackrel{·}{\lambda }}_0-{\lambda }_2{\stackrel{·}{\lambda }}_3+{\lambda }_3{\stackrel{·}{\lambda }}_2\\ {\lambda }_0{\stackrel{·}{\lambda }}_2+{\lambda }_1{\stackrel{·}{\lambda }}_3-{\lambda }_2{\stackrel{·}{\lambda }}_0-{\lambda }_3{\stackrel{·}{\lambda }}_1\\ {\lambda }_0{\stackrel{·}{\lambda }}_3-{\lambda }_1{\stackrel{·}{\lambda }}_2+{\lambda }_2{\stackrel{·}{\lambda }}_1-{\lambda }_3{\stackrel{·}{\lambda }}_0\end{array}\right)$

中对时间变量t的微分改写为对弧坐标s的微分，可以得到弯扭度矢量u 在主轴坐标系中的表达

$\left(\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right)=2\left(\begin{array}{c}{\lambda }_0{\lambda }_1^{\prime }-{\lambda }_1{\lambda }_0^{\prime }-{\lambda }_2{\lambda }_3^{\prime }+{\lambda }_3{\lambda }_2^{\prime }\\ {\lambda }_0{\lambda }_2^{\prime }+{\lambda }_1{\lambda }_3^{\prime }-{\lambda }_2{\lambda }_0^{\prime }-{\lambda }_3{\lambda }_1^{\prime }\\ {\lambda }_0{\lambda }_3^{\prime }-{\lambda }_1{\lambda }_2^{\prime }+{\lambda }_2{\lambda }_1^{\prime }-{\lambda }_3{\lambda }_0^{\prime }\end{array}\right)$

其中u_k(1≤k≤3)为弯扭度矢量的分量，为四元数分量λ_k对弧坐标s的微 分。

由于处于平衡状态的曲杆只具有弹性势能，所以无需考虑运动过程中的动 能以及耗散能。弹性势能包括拉伸形变具有的弹性势能(V_s)和弯曲、扭转形 变具有的弹性势能(V_b)。并通过弹性杆的科瑟拉理论中弹性势能表达，得到V_s的表达式为

$V_s=\frac{1}{2}{\int }_0^L{k}_s{\left(\right|\left|\frac{dr\left(s\right)}{ds}\right||-1)}^{2}ds$

其中，ks表示抗拉刚度，它与拉伸杨氏模量E_s有关，通过式子k_s＝E_sπr²计 算得到，r表示截面半径，L表示曲杆的长度，s表示弧坐标，r(s)为用弧坐标 s表示的参考坐标系中的向量。

V_h的表达式为

$V_b=\frac{1}{2}{\int }_0^L\underset{k=1}{\overset{3}{\Sigma }}{k}_{bk}{(u_k-{\hat{u}}_k)}^{2}ds$

其中u_k为曲杆的弯扭度，且当曲杆在松弛状态下时u_k(1≤k≤3)都为0， 与曲杆的初始弯曲和扭转变形有关，当都为零时为无扭转的直杆。k_bk为 抗弯和抗扭刚度。在截面的均匀假设下，圆形截面的抗弯抗扭刚度为

$k_{b1}=k_{b2}=E\frac{{\mathrm{\pi r}}^4}{4},k_{b3}=G\frac{{\mathrm{\pi r}}^4}{4}$

其中E为弯曲变形中的杨氏模量，G为扭转变形时的切变模量，r为截面 半径。对于理想杆，拉伸变形中的杨氏模量Es等于弯曲变形时的杨氏模量E， 由于实际线缆中存在多条线缆组成的线束，其截面并不均匀而且存在空隙，在 实际测量中线缆拉伸时的杨氏模量Es相比于弯曲时的杨氏模量E大。

由于中心线上任意一点处的切线方向与该点处主轴坐标系的基矢量d₃的方 向相同，所以存在方向一致约束

$\frac{dr\left(s\right)}{ds}=d_3\left(\lambda \right)$

对于四元数λ，只有当它的模等于一时，才能表示一个纯转动，因此存在 四元数的标准化约束||λ||＝1。

其中，在本发明的第一实施例中，分别建立每条中心线段的拉伸形变的弹 性势能与每条中心线段的端点的坐标的第二函数关系式以及每相邻两条中心 线段的两个中点之间的中心线段的弯曲及扭转变形的弹性势能与每条中心线 段的中点的四元数的第三函数关系式，具体包括：首先通过公式 计算每一条中心线段的任一点处的空间向量对弧坐 标s的微分，其中r₁′表示第i条中心线段的空间向量对弧坐标s的微分，r_i(s)为 第i条中心线段用弧坐标s表示的参考坐标系中的向量，r_i+1和r_i分别表示在参 考坐标系中第i条中心线段的两个端点的坐标，l_i表示当待建模线缆处于自然 状态时，第i条中心线段的长度。然后将该表达式代入上述V_s的表达式中，得 到公式

$V_s\left[i\right]=\frac{1}{2}{l}_i{k}_s{\left(\frac{\sqrt{(r_{i+1}-r_i)·(r_{i+1}-r_i)}}{l_i}-1\right)}^2$

并通过公式

$V_s\left[i\right]=\frac{1}{2}{l}_i{k}_s{\left(\frac{\sqrt{(r_{i+1}-r_i)·(r_{i+1}-r_i)}}{l_i}-1\right)}^2$

建立每条中心线段的拉伸形变的弹性势能与每条中心线段的端点的坐标 的第二函数关系式，其中V_s[i]表示第i条中心线段的拉伸形变的弹性势能，l_i表 示当待建模线缆处于自然状态时，第i条中心线段的长度，k_s表示抗拉刚度， r_i+1和r_i分别表示在参考坐标系中第i条中心线段的两个端点的坐标。

同时通过公式${\lambda }_j^{\prime }=\frac{{\mathrm{d\lambda }}_j\left(s\right)}{ds}=\frac{{\lambda }_{j+1}-{\lambda }_j}{\frac{1}{2}\left(\left|\right|r_{j+2}-r_{j+1}\left|\right|+\left|\right|r_{j+1}-r_j\left|\right|\right)}\approx \frac{{\lambda }_{j+1}-{\lambda }_j}{l_j}$计算得到每相邻 两条中心线段的两个中点之间的中心线段的任一点处四元数对弧坐标s的微 分，其中，λ_j(s)为用弧坐标s表示的第i条中心线段的四元数，λ′_j表示第j+1 条中心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的四元 数对弧坐标s的微分，λ_j+1表示第j+1条中心线段的中点的四元数，λ_j表示第 j条中心线段的中点的四元数，l_j表示当待建模线缆处于自然状态时，第j条中 心线段和第j+1条中心线段的和的一半。同时通过公式计算得到 每相邻两条中心线段的两个中点之间的中心线段的任一点处的四元数，其中表示第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的任一 点处的四元数；然后再用四元数代替上述V_b的表达式是中的u_k，得到公式

$\left(\begin{array}{c}{V}_b\left[j\right]=\frac{l_j}{2}[k_{b1}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{3,j}^{\prime }+\overline{{\lambda }_{3,j}}{\lambda }_{2,j}^{\prime }\right)-{\hat{u}}_{1,j}\right)}^2\\ +k_{b2}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{2,j}^{\prime }+\overline{{\lambda }_{1,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{1,j}^{\prime }\right)-{\hat{u}}_{2,j}\right)}^2\\ +k_{b3}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{2,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{0,j}^{\prime }\right)-{\hat{u}}_{3,j}\right)}^2]\end{array}\right)$

并通过该公式建立每相邻两条中心线段的两个中点之间的中心线段的弯 曲及扭转变形的弹性势能与每条中心线段的中点的四元数的第三函数关系式， 其中V_b[j]表示第j+1条中心线段的中点与第j条中心线段的中点之间的中心线 段的弯曲及扭转变形的弹性势能，k_b1、k_b2以及k_b3分别表示在三个坐标轴方 向上的抗弯和抗扭刚度，且当待建模线缆的截面为圆形时，E表示杨氏模量(具体为弯曲变形中的杨氏模量)，G表示切变 模量，r表示待建模线缆的截面半径，表示第j+1条中心线段的中点与第j 条中心线段的中点之间的中心线段的任一点处的四元数的第一个参数，表 示第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的任一点 处的四元数的第二个参数，表示第j+1条中心线段的中点与第j条中心线 段的中点之间的中心线段的任一点处的四元数的第三个参数，表示第j+1 条中心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的四元 数的第四个参数，λ′_b,j表示第j+1条中心线段的中点与第j条中心线段的中点 之间的中心线段的四元数的第一个参数对弧坐标s的微分，λ′_1,j表示第j+1条 中心线段的中点与第j条中心线段的中点之间的中心线段的四元数的第二个参 数对弧坐标s的微分，λ′_2,j表示第j+1条中心线段的中点与第j条中心线段的中 点之间的中心线段的四元数的第三个参数对弧坐标s的微分，λ′_3,j表示第j+1 条中心线段的中点与第j条中心线段的中点之间的中心线段的四元数的第四个 参数对弧坐标s的微分，以及分别表示第j+1条中心线段的中点与 第j条中心线段的中点之间的中心线段在三个坐标轴方向上的原始弯扭度。

其中，在本发明的第一实施例中，可以进一步根据V_s[i]和V_b[j]，通过公式

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

计算得到第一函数关系式，其中V₁表示待建模线缆的弹性势能，i和j均表 示中心线段的序号，N表示在对待建模线缆的中心线进行离散处理时的节点数。

其中，在本发明的第一实施例中，上述步骤S13具体包括：首先基于科瑟 拉弹性杆理论的约束条件(即方向一致约束和四元数的标准化约束)，通过罚 函数对

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

的约束条件进行转换，得到第一罚函数ds和第二罚函数其中V_c和V_n分别表示第一罚 函数和第二罚函数，s表示弧坐标，k_c和k_n均表示罚因子系数，r’表示参考坐 标系中的向量r对弧坐标s的微分，d₃表示主坐标系中z轴方向的单位矢量， λ表示四元数；然后再根据第一罚函数和第二罚函数，计算待建模线缆的罚函 数的能量函数；最后再根据罚函数的能量函数和第一函数关系式，确定出当待 建模线缆处于最小弹性势能状态下时，待建模线缆的每条中心线段的端点的坐 标。

其中，根据第一罚函数和第二罚函数，建立待建模线缆的罚函数的能量函 数的步骤具体包括：根据第一罚函数，通过公式ds计算得到每条中心线段的第一罚函数的能量函数，其中V_c[i] 表示第i条中心线段的第一罚函数的能量，r’_i表示参考坐标系中第i条中心线 段的向量r对弧坐标s的微分，d₃(λ_i)表示用第i条中心线段的四元数表达主 坐标系中z轴方向的单位矢量；同时根据第二罚函数，通过公式计算得到每条中心线段的第二罚函数的能量函数，其中 V_n[i]表示第i条中心线段的第二罚函数的能量，λ_i表示第i条中心线段的四元 数；最后再根据公式

$V_2=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_c\left[i\right]+\underset{i=1}{\overset{N-1}{\Sigma }}{V}_n\left[i\right]$

计算得到待建模线缆的罚函数的能量函数，其中V₂表示待建模线缆的罚函 数的能量。

其中，在本发明的第一实施例中，上述根据罚函数的能量函数和第一函数 关系式，确定出待建模线缆的每条中心线段的端点的坐标的步骤具体包括：首 先通过公式V_总＝V₁+V₂计算出罚函数的能量函数和弹性势能的和值，其中V_总表示总能量，即上述和值；然后通过非线性最小二乘法(即高斯-牛顿法)，计 算出和值的最小值；最后根据最小值，确定出当和值取最小值时待建模线缆的 每条中心线段的端点的坐标，并根据确定出的每条中心线段的端点的坐标确定 出待建模线缆的中心线。其中通过非线性最小二乘法，计算出和值的最小值的 方法对于本领域的技术人员而言属于公知常识，在此不再赘述。

其中，在本发明的第一实施例中，非线性最小二乘法对线缆能量函数最小 值的求解具有较高的效率及稳定性。如图4所示的求解时间与离散点数目的关 系图，求解时间与离散点数目呈线性关系，一般情况下采用30至50离散点对 线缆中心线进行离散处理，其精度已经能够满足大部分应用的要求，其求解时 间平均15至18毫秒，因此满足了实时性要求，同时确保了建模的真实性。

第二实施例

如图5所示，本发明的第二实施例提供了一种基于科瑟拉弹性杆模型的线 缆的建模装置，该建模装置包括：

第一获取模块51，用于对待建模线缆的中心线进行离散处理，得到多条 中心线段；

第一确定模块52，用于建立待建模线缆的弹性势能与每条中心线段中点 的四元数和每条中心线段的端点的坐标的第一函数关系式；

第二确定模块53，用于根据第一函数关系式，确定出待建模线缆的每条 中心线段的端点的坐标。

其中，建模装置还包括：

第二获取模块，用于获取待建模线缆的长度、截面形状及尺寸、杨氏模量、 切变模量以及用于固定待建模线缆的各接头的安装位置。

其中，第一确定模块52包括：

第一计算单元，用于根据待建模线缆的长度和各接头的安装位置，分别建 立每条中心线段的拉伸形变的弹性势能与每条中心线段的端点的坐标的第二 函数关系式以及每相邻两条中心线段的两个中点之间的中心线段的弯曲及扭 转变形的弹性势能与每条中心线段的中点的四元数的第三函数关系式；

第二计算单元，用于根据第二函数关系式和第三函数关系式，计算得到第 一函数关系式。

其中，第一计算单元包括：

第一计算子单元，用于通过公式

$V_s\left[i\right]=\frac{1}{2}{l}_i{k}_s{\left(\frac{\sqrt{(r_{i+1}-r_i)·(r_{i+1}-r_i)}}{l_i}-1\right)}^2$

建立每条中心线段的拉伸形变的弹性势能与每条中心线段的端点的坐标 的第二函数关系式，其中V_s[i]表示第i条中心线段的拉伸形变的弹性势能，l_i表 示当待建模线缆处于自然状态时，第i条中心线段的长度，k_s表示抗拉刚度， r_i+1和r_i分别表示在参考坐标系中第i条中心线段的两个端点的坐标；

第二计算子单元，用于通过公式计算得到每相邻两条中心线 段的两个中点之间的中心线段的任一点处的四元数对弧坐标s的微分，其中λ′_j表示第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的任一 点处的四元数对弧坐标s的微分，λ_j+1表示第j+1条中心线段的中点的四元数， λ_j表示第j条中心线段的中点的四元数，l_j表示当待建模线缆处于自然状态时， 第j条中心线段和第j+1条中心线段的和的一半；

第三计算子单元，用于通过公式计算得到每相邻两条中心线 段的两个中点之间的中心线段的任一点处的四元数，其中表示第j+1条中心 线段的中点与第j条中心线段的中点之间的中心线段的任一点处的四元数；

第四计算子单元，用于通过公式

$\left(\begin{array}{c}{V}_b\left[j\right]=\frac{l_j}{2}[k_{b1}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{3,j}^{\prime }+\overline{{\lambda }_{3,j}}{\lambda }_{2,j}^{\prime }\right)-{\hat{u}}_{1,j}\right)}^2\\ +k_{b2}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{2,j}^{\prime }+\overline{{\lambda }_{1,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{0,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{1,j}^{\prime }\right)-{\hat{u}}_{2,j}\right)}^2\\ +k_{b3}{\left(2·\left(\overline{{\lambda }_{0,j}}{\lambda }_{3,j}^{\prime }-\overline{{\lambda }_{1,j}}{\lambda }_{2,j}^{\prime }-\overline{{\lambda }_{2,j}}{\lambda }_{1,j}^{\prime }-\overline{{\lambda }_{3,j}}{\lambda }_{0,j}^{\prime }\right)-{\hat{u}}_{3,j}\right)}^2]\end{array}\right)$

建立每相邻两条中心线段的两个中点之间的中心线段的弯曲及扭转变形 的弹性势能与每条中心线段的中点的四元数的第三函数关系式，其中V_b[j]表示 第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的弯曲及扭 转变形的弹性势能，k_b1、k_b2以及k_b3分别表示在三个坐标轴方向上的抗弯和 抗扭刚度，且当待建模线缆的截面为圆形时，E表示杨氏模量，G表示切变模量，r表示待建模线缆的截面半径，表示第 j+1条中心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的 四元数的第一个参数，表示第j+1条中心线段的中点与第j条中心线段的 中点之间的中心线段的任一点处的四元数的第二个参数，表示第j+1条中 心线段的中点与第j条中心线段的中点之间的中心线段的任一点处的四元数的 第三个参数，表示第j+1条中心线段的中点与第j条中心线段的中点之间 的中心线段的任一点处的四元数的第四个参数，λ′_0,j表示第j+1条中心线段的 中点与第j条中心线段的中点之间的中心线段的四元数的第一个参数对弧坐标 s的微分，λ′_1,j表示第j+1条中心线段的中点与第j条中心线段的中点之间的中 心线段的四元数的第二个参数对弧坐标s的微分，λ′_2,j表示第j+1条中心线段 的中点与第j条中心线段的中点之间的中心线段的四元数的第三个参数对弧坐 标s的微分，λ′_3,j表示第j+1条中心线段的中点与第j条中心线段的中点之间的 中心线段的四元数的第四个参数对弧坐标s的微分，以及分别表示 第j+1条中心线段的中点与第j条中心线段的中点之间的中心线段在三个坐标 轴方向上的原始弯扭度。

其中，第二计算单元包括：

第五计算子单元，用于通过公式

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

计算得到第一函数关系式，其中V₁表示待建模线缆的弹性势能，i和j均表 示中心线段的序号，N表示在对待建模线缆的中心线进行离散处理时的节点数。

其中，第二确定模块53包括：

第三计算单元，用于通过罚函数对公式

$V_1=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_s\left[i\right]+\underset{j=1}{\overset{N-2}{\Sigma }}{V}_b\left[j\right]$

的约束条件进行转换，得到第一罚函数ds和第二罚函数其中V_c和V_n分别表示第一罚 函数和第二罚函数，s表示弧坐标，k_c和k_n均表示罚因子系数，r’表示参考坐 标系中的向量r对弧坐标s的微分，d₃表示主坐标系中z轴方向的单位矢量， λ表示四元数；

第四计算单元，用于根据第一罚函数和第二罚函数，建立待建模线缆的罚 函数的能量函数；

确定单元，用于根据罚函数的能量函数和第一函数关系式，确定出待建模 线缆的每条中心线段的端点的坐标。

其中，第四计算单元包括：

第六计算子单元，用于根据第一罚函数，通过公式计算得到每条中心线段的第一罚函数的能量函 数，其中V_c[i]表示第i条中心线段的第一罚函数的能量，r’_i表示参考坐标系中 第i条中心线段的向量r对弧坐标s的微分，d₃(λ_i)表示用第i条中心线段的 四元数表达主坐标系中z轴方向的单位矢量；

第七计算子单元，用于根据第二罚函数，通过公式计算得到每条中心线段的第二罚函数的能量函数，其中V_n[i]表示第i条 中心线段的第二罚函数的能量，λ_i表示第i条中心线段的四元数；

第八计算子单元，用于根据公式

$V_2=\underset{i=1}{\overset{N-1}{\Sigma }}{V}_c\left[i\right]+\underset{i=1}{\overset{N-1}{\Sigma }}{V}_n\left[i\right]$

计算得到待建模线缆的罚函数的能量函数，其中V₂表示待建模线缆的罚函 数的能量。

其中，确定单元包括：

第九计算子单元，用于计算出罚函数的能量函数和第一函数关系式的和值；

第十计算子单元，用于通过非线性最小二乘法，计算出和值的最小值；

确定子单元，用于根据最小值，确定出待建模线缆的每条中心线段的端点 的坐标。

需要说明的是，本发明实施例提供的线缆的建模装置是应用上述建模方法 的装置，即上述建模方法的所有实施例均适用于该装置，且均能达到相同或相 似的有益效果。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技 术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰， 这些改进和润饰也应视为本发明的保护范围。