[发明专利]软体机械臂的力学模型的建立方法及控制方法

权利要求书

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

特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的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(α_u,θ_u)＝M_z(α_u)M_y(θ_u)M_z(-α_u)，u＝1，2，……，(q-1)，

其中，

为前(q-1)个软体驱动器上末端的特征点在所在的软体驱动器底端的坐标系中位置矢量的旋转矩阵的乘积，

为第q个软体驱动器上第(i-1)个特征点在第q个软体驱动器底端的坐标系中位置矢量的旋转矩阵，

中每个H_u(u＝1,2,...,q)矩阵的定义表达式，α_u,θ_u是H_u矩阵的自变量，α_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(α₀)R_y(β₀)R_x(γ₀)为固定底端坐标系按照3-2-1顺序分别旋转欧拉角α₀、β₀、γ₀后与第q个软体驱动器的底端的坐标系重合的旋转矩阵。