[发明专利]变刚度软体蛇形臂的运动学建模方法

权利要求书

1.一种变刚度软体蛇形臂的运动学建模方法，其特征在于，该方法包括以下步骤：

步骤一、选取已知变刚度软体蛇形臂的三维模型，该三维模型包括若干节刚柔单元，刚柔单元一端为基座盘、另一端为末端盘；

步骤二、调节导向杆的长度，获取预期的弯曲方向和弯曲角度，并建立各节刚柔单元的基座盘坐标系和末端盘坐标系；

步骤三、求出末端盘坐标系相对基座盘坐标系的变换矩阵；

步骤四、确立变刚度软体蛇形臂末端达到预期位置和姿态时的目标位姿矩阵，即可根据基座盘坐标系、变换矩阵和目标位姿矩阵求解每节刚柔单元的弯曲方向和弯曲角度。

2.如权利要求1所述的变刚度软体蛇形臂的运动学建模方法，其特征在于：在所述步骤一中，所述基座盘和所述末端盘均为圆盘，在所述基座盘和所述末端盘之间设有两对长度可调的导向杆，每对导向杆的连接点位于同一直径上。

3.如权利要求2所述的变刚度软体蛇形臂的运动学建模方法，其特征在于：所述步骤二中，先调节导向杆的长度，来获取预期的弯曲方向和弯曲角度θ，

然后建立基座盘坐标系o₀-x₀y₀z₀时，先在基座盘所在平面建立坐标系x₀o₀y₀，o₀为基座盘圆心，x₀轴穿过一对导向杆的固连点，y₀轴穿过另一对导向杆的固连点，z₀轴指向末端盘的圆心，然后将基座盘坐标系o₀-x₀y₀z₀绕z₀轴旋转角度θ₁，θ₁＝θ，得坐标系o₁-x₁y₁z₁；

再建立末端盘坐标系o₂-x₂y₂z₂时，先在基座盘所在平面建立坐标系x₂o₂y₂，o₂为末端盘圆心，x₂轴穿过x₀对应一对导向杆穿过的固连点，y₂轴穿过另一对导向杆的固连点，z₂轴朝向背离基座盘的圆心，然后将末端盘坐标系o₂-x₂y₂z₂绕z₂轴旋转角度θ₃，θ₃＝-θ，得坐标系o₃-x₃y₃z₃。

4.如权利要求3所述的变刚度软体蛇形臂的运动学建模方法，其特征在于：所述步骤三中，

求解变形后基座盘圆心与末端盘圆心连线与z₁轴的夹角θ₂，R为变形后刚柔单元的曲率半径，α为y₂轴与y₁轴的夹角；

再以表示o₀-x₀y₀z₀到o₁-x₁y₁z₁的变换矩阵，表示o₁-x₁y₁z₁到o₂-x₂y₂z₂的变换矩阵，表示o₂-x₂y₂z₂到o₃-x₃y₃z₃的变换矩阵，得：

则有末端坐标系相对于基座坐标系的变换矩阵为：

5.如权利要求4所述的变刚度软体蛇形臂的运动学建模方法，其特征在于：所述步骤四中，已知初始矩阵T和目标位姿矩阵T₃，即可求得θ₁、θ₂、θ₃，即可得出刚柔单元的弯曲方向和弯曲角度。