CN110125933A - 软体机械臂的力学模型的建立方法及控制方法 - Google Patents

Abstract

本发明公开了一种软体机械臂的力学模型的建立方法及控制方法，包括以下步骤：在每个软体驱动器的底端建立三维坐标系，第一个软体驱动器底端的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，建立软体机械臂的力学模型。

Description

软体机械臂的力学模型的建立方法及控制方法

技术领域

本发明涉及软体机械臂技术领域，特别地，涉及一种软体机械臂的力学模型的建立方法，此外，还涉及软体机械臂空间运动姿态的开环控制方法和闭环控制方法。

背景技术

软体机械臂的研究主要受到自然界中的软体动物(如章鱼、象鼻等)的运动模式启发，通过软性材料搭配气动控制系统，实现超冗余和无限自由度的柔顺运动。软体机械臂因其柔性结构特性，使其在复杂未知环境探测、人机友好交互等方面，都具有刚性机械臂难以匹敌的优势。现有的软体机械臂通过控制气囊腔的充气和放气以实现软体机械臂的运动，但由于其结构的柔性，导致控制精度很难保证，并且负载能力差，无法负载质量大的物体。对于一种气控型刚柔耦合模块化的软体机械臂，通过将多个波纹管状的气囊体沿周向等距排布构成软体驱动器，软体驱动器上设有用于沿径向拉结多个气囊体的固定结构，多个固定结构沿气囊模块轴向间隔排布，用于提高机械臂的负载能力并使软体驱动器的所有气囊体相互做耦合运动，再通过连接结构将多个软体驱动器沿轴向首尾密封连接，气囊体充气后膨胀延长，气囊体放气后收缩缩短，通过向气囊体内充气和/或放气以控制气囊体中压力值，从而控制软体驱动器的空间运动姿态，目前可采用开环控制方法对软体机械臂的空间运动姿态进行控制，通过向各个气囊体中对应输入准确的压力值，以使软体机械臂形成指定的运动姿态，还可采用闭环控制方法对软体机械臂的空间运动姿态进行控制，通过向各个气囊体中输入初始的压力值后使软体机械臂形成运动姿态，再通过将运动姿态与指定运动姿态进行比较后对输入气囊体中的压力值进行调整，直至软体机械臂形成指定运动姿态，但由于目前无法对运动姿态进行准确计算，导致软体机械臂空间运动姿态的开环控制和闭环控制均难以实现。

发明内容

本发明提供了一种软体机械臂的力学模型的建立方法及控制方法，以解决气控型刚柔耦合模块化的软体机械臂的空间运动姿态无法精确控制的技术问题。

根据本发明的一个方面，提供一种软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，软体机械臂包括M个软体驱动器，软体驱动器通过连接结构沿轴向首尾密封连接，软体驱动器包括沿周向等距排布的多个波纹管状的气囊体，每个气囊体包括多个波纹小节，波纹小节的分布中心为特征点，M个软体驱动器从软体机械臂的固定端向自由端端方向依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器上波纹小节的个数依次为N₁，N₂，……，N_M；M个软体驱动器1上特征点的个数依次为N₁，N₂，……，N_M，软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤N_q)个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。

软体驱动器的底端建立的坐标系，以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B。

确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。

确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的前(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。

将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：当软体驱动器排第一时，则无需转换；当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。

特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示。

获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角。

当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，包括以下步骤：第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(x_q,i y_q,i z_q,i)^T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x₀ y₀ z₀)^T，第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：

H_u(a_u,θ_u)＝M_z(α_u)M_y(θ_u)M_z(-α_u)，u＝1，2，……，(q-1)，

其中，

α_u为第u个软体驱动器的弯曲方向，θ_u为第u个软体驱动器的弯曲角度，

M_z(α_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转α_u的旋转矩阵，

M_y(θ_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转a_u的旋转矩阵，

M_z(-a_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-a_u的旋转矩阵，

R_z(a₀)R_y(β₀)R_x(γ₀)为固定底端坐标系按照321顺序分别旋转欧拉角a₀、β₀、γ₀后与第q个软体驱动器的底端的坐标系重合的旋转矩阵。

根据本发明的另一方面，还提供一种软体机械臂空间运动姿态的开环控制方法，包括以下步骤：采用上述软体机械臂的力学模型对软体机械臂的指定的空间运动姿态进行模拟，从而确定输入软体机械臂的各个气囊体中的气压值；分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态。

根据本发明的另一方面，还提供一种软体机械臂空间运动姿态的闭环控制方法，包括以下步骤：分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动；采用上述软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态；根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态。

本发明具有以下有益效果：

本发明的软体机械臂的力学模型的建立方法，首先在每个软体驱动器的底端建立B-xyz三维坐标系，由于位于软体机械臂固定端处的第一个软体驱动器的底端在软体机械臂的运动过程中位置固定，因此为固定底端坐标系，通过以波纹管状的气囊体的伸长变形的线弹性假设和弯曲变形的弹性梁假设为基础，从软体驱动器的底端依次确定软体驱动器上的特征点在所在的软体驱动的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立软体驱动器的力学模型，由于除了位于软体机械臂固定端处的第一个软体驱动器的底端的位置固定不变，软体机械臂的其他部位的位置均会在运动过程中发生改变，因此通过将特征点在自身所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中，以固定底端坐标系作为软体机械臂的定位基准，最后根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。确定了输入气囊体的压力值、软体机械臂位置、空间运动姿态之间的关系，一方面可通过软体机械臂的力学模型模拟出软体机械臂的指定的空间运动状态，从而确定软体机械臂形成指定的空间运动状态时输入各个气囊体中的气压值，再将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态，从而实现对软体机械臂的开环控制；另一方面可通过分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动，采用软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态，再根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态，从而实现对软体机械臂的精确闭环控制。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明优选实施例的软体机械臂的结构示意图；

图2是本发明优选实施例的软体机械臂的力学模型的建立方法的流程示意图。

图例说明：

1、软体驱动器；11、气囊体；2、连接结构。

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以由下述所限定和覆盖的多种不同方式实施。

图1是本发明优选实施例的软体机械臂的结构示意图；图2是本发明优选实施例的软体机械臂的力学模型的流程示意图。

如图1和图2所示，本实施例的软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，软体机械臂包括M个软体驱动器1，软体驱动器1通过连接结构2沿轴向首尾密封连接，软体驱动器1包括沿周向等距排布的多个波纹管状的气囊体11，每个气囊体11包括多个波纹小节，波纹小节的分布中心为特征点，M个软体驱动器1从软体机械臂的固定端向自由端依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器上波纹小节的个数依次为N₁，N₂，……，N_M；M个软体驱动器1上特征点的个数依次为N₁，N₂，……，N_M，建立方法包括以下步骤：软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤N_q)个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。本发明的软体机械臂的力学模型，首先在每个软体驱动器的底端建立B-xyz三维坐标系，由于位于软体机械臂固定端处的第一个软体驱动器的底端在软体机械臂的运动过程中位置固定，因此为固定底端坐标系，通过以波纹管状的气囊体的伸长变形的线弹性假设和弯曲变形的弹性梁假设为基础，从软体驱动器的底端依次确定软体驱动器上的特征点在所在的软体驱动的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立软体驱动器的力学模型，由于除了位于软体机械臂固定端处的第一个软体驱动器的底端的位置固定不变，软体机械臂的其他部位的位置均会在运动过程中发生改变，因此通过将特征点在自身所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中，以固定底端坐标系作为软体机械臂的定位基准，最后根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型，确定了输入气囊体的压力值、软体机械臂位置、空间运动姿态之间的关系。

软体驱动器的底端建立坐标系，以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B。由于特征点为气囊体上同节的波纹小节的分布中心，而特征点周围的波纹小节的分布位置不相同，为了更加准确地获得特征点的位置坐标，需要获得周围的波纹小节的中心点的位置坐标，通过以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B，使得该软体驱动器上所有的波纹小节的中心点的位置坐标易于确定。

确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。

确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的第1～(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。

在本实施例中，每个软体驱动器包括沿周向等距布设的三个气囊体。以建立第一个软体驱动器的力学模型为例，第一个软体驱动器上包括N₁个特征点，从第1个软体驱动器的底端依次确定第i(1≤i≤N₁)个特征点在第一个软体驱动器的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，包括以下步骤：

确定第一个软体驱动器中三个气囊体上第i个波纹小节与气囊体底端之间的长度：

确定第一个软体驱动器的第i个特征点处的弯曲方向，弯曲方向为软体驱动器第i个特征点处从X轴沿逆时针方向转动的角度a₁：

且a₁与L₁₁、L₁₂以及L₁₃之间需满足以下关系：

其中，L₁₁、L₁₂以及L₁₃分别为第一个软体驱动器中三个气囊体第i个波纹小节与气囊体底端之间的长度；

确定第一个软体驱动器第i个特征点处的弯曲角度θ₁，软体驱动器假设为弹性梁，弯曲角度为弹性梁第i个特征点处的转角：S₁′＝π(r_out,1 ²-r_in,1 ²)，

I₁为第一个软体驱动器中气囊体截面的惯性矩，

W₁的计算方法为：

当L₁₁≠L₁₃时，

当L₁₁＝L₁₃≠L₁₂时，

根W₁中包含两个正数解，取其较小者为方程的实际解。

当L₁₁＝L₁₂＝L₁₃时，W₁＝0，

其中，S₁为第一个软体驱动器中气囊体内腔的横截面积，S₁‘为第一个软体驱动器中气囊体壁体的横截面积，a₁为气囊体之间的间距，r_out,1为第一个软体驱动器中气囊体的外径，r_in,1为第一个软体驱动器中气囊体的内径，k₁为第一个软体驱动器中气囊体中的弹性系数；

三个气囊体第i个波纹小节的中心点分别为A’、B’、C’，其中B’与第一个软体驱动器的底端的坐标系的原点位于同一个气囊体中，确定第一个软体驱动器的底端的坐标系的原点分别到与第i个特征点同节的三个气囊体的第i个波纹小节的中心点的位置矢量r_1,A′、r_1,B′、r_1,C′：

其中，P₁₁、P₁₂、P₁₃分别为A’、B’、C’所在的气囊体的压力值，L₀₁为第一个软体驱动器中波纹小节的原始长度；

确定第一个软体驱动器上第i个特征点在第一个软体驱动器的底端的坐标系中的位置矢量r_1,i：

其中，

(H₁ ^T)^v＝M_z(a₁)M_y(θ₁)M_z(-a₁)，(v＝1,2,…,q-1),

M_z(a₁)为第一个软体驱动器的第v个特征点的位置矢量绕z轴转动a₁的旋转矩阵，

M_y(θ₁)为第一个软体驱动器的第v个特征点的位置矢量绕y轴转动θ₁的旋转矩阵，

M_z(-a₁)为第一个软体驱动器的第v个特征点的位置矢量绕z轴转动-a₁的旋转矩阵，

(H₁ ^T)^v为第一个软体驱动器的第v个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵；

采用上述方式算出第一个软体驱动器的前(i-1)个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵之和[(H₁ ^T)^i-2+(H₁ ^T)^i-3+…+(H₁ ^T)¹+(H₁ ^T)⁰]以及第一个软体驱动器的第(i-1)个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵(H₁ ^T)^i-1，从而确定第一个软体驱动器上第i个特征点在第一个软体驱动器的底端的坐标系中的位置矢量r_1,i。

将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：当软体驱动器排第一时，则无需转换；当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。

特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示。获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角。固定底端坐标系依次绕z轴、y轴、x轴旋转至与特征点所在的软体驱动器底端的坐标系重合。

当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，包括以下步骤：第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(x_q,i y_q,i z_q,i)^T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x₀ y₀ z₀)^T，第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：

其中，

(H_u ^T)^v＝M_z(a_u)M_y(θ_u)M_z(-a_u)，(u＝1,2,…,M)，(v＝1,2,…,Nu)，

(H_u ^T)^v为第u个软体驱动器上第v个特征点在第u个软体驱动器底端的坐标系中位置矢量的旋转矩阵，

a_u为第u个软体驱动器的第v个特征点处的弯曲方向，θ_u为第u个软体驱动器的第v个特征点处的弯曲角度，

M_z(a_u)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转a_u的旋转矩阵，

M_y(θ_u)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转a_u的旋转矩阵，

M_z(-a_u)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-a_u的旋转矩阵，

采用上述方式计算出前(q-1)个软体驱动器上末端的特征点在所在的软体驱动器底端的坐标系中位置矢量的旋转矩阵的乘积以及第q个软体驱动器上第(i-1)个特征点在第q个软体驱动器底端的坐标系中位置矢量的旋转矩阵[(H_q ^T)^i-1]，从而确定固定底端坐标系按照321顺序分别旋转欧拉角a₀、β₀、γ₀后与第q个软体驱动器的底端的坐标系重合的旋转矩阵[R_z(a₀)R_y(β₀)R_x(γ₀)]，进而确定旋转欧拉角a₀、β₀、γ₀。

本实施例的软体机械臂空间运动姿态的开环控制方法，包括以下步骤：采用上述软体机械臂的力学模型对软体机械臂的指定运动姿态进行模拟，从而确定软体机械臂的各个气囊体的气压值；分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定运动姿态。

本实施例的软体机械臂空间运动姿态的闭环控制方法，包括以下步骤：分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂形成运动姿态；采用上述软体机械臂的力学模型确定软体机械臂的运动姿态的姿态信息；根据软体机械臂的运动姿态的姿态信息与指定运动姿态的姿态信息之间的差异对输入的压力值进行调整，直至软体机械臂形成的运动姿态为指定运动姿态。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，

软体机械臂包括M个软体驱动器(1)，软体驱动器(1)通过连接结构(2)沿轴向首尾密封连接，

软体驱动器(1)包括沿周向等距排布的多个波纹管状的气囊体(11)，每个气囊体(11)包括多个波纹小节，波纹小节的分布中心为特征点，

M个软体驱动器(1)从软体机械臂的固定端向自由端方向依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器(1)上波纹小节的个数依次为N₁，N₂，……，N_M；M个软体驱动器(1)上特征点的个数依次为N₁，N₂，……，N_M，

其特征在于，建立方法包括以下步骤：

软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；

从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤N_q)个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；

将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；

根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。

2.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，软体驱动器的底端建立的坐标系，以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B。

3.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：

分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；

根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；

根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。

4.根据权利要求3所述的软体机械臂的力学模型的建立方法，其特征在于，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：

根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；

根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的前(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。

5.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：

当软体驱动器排第一时，则无需转换；

当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。

6.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，

特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示。

7.根据权利要求6所述的软体机械臂的力学模型的建立方法，其特征在于，获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：

确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；

根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角。

8.根据权利要求7所述的软体机械臂的力学模型的建立方法，其特征在于，

当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，

包括以下步骤：

第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(x_q,i y_q,i z_q,i)^T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x₀y₀ z₀)^T，

第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：

H_u(a_u,θ_u)＝M_z(α_u)M_y(θ_u)M_z(-α_u)，u＝1，2，……，(q-1)，

其中，

a_u为第u个软体驱动器的弯曲方向，θ_u为第u个软体驱动器的弯曲角度，

M_z(α_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转α_u的旋转矩阵，

M_y(θ_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转α_u的旋转矩阵，

M_z(-α_u)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-α_u的旋转矩阵，

R_z(a₀)R_y(β₀)R_x(γ₀)为固定底端坐标系按照321顺序分别旋转欧拉角a₀、β₀、γ₀后与第q个软体驱动器的底端的坐标系重合的旋转矩阵。

9.一种软体机械臂空间运动姿态的控制方法，其特征在于，包括以下步骤：

采用权利要求1-8任一所述的软体机械臂的力学模型对软体机械臂的指定的空间运动姿态进行模拟，从而确定输入软体机械臂的各个气囊体中的气压值；

分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态。

10.一种软体机械臂空间运动姿态的控制方法，其特征在于，包括以下步骤：

分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动；

采用权利要求1-8任一所述的软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态；

根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态。