[发明专利]基于超椭球映射解析算法并联冗余柔索机构伺服控制方法

权利要求书

1.基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于，该方法的步骤包括：

步骤(1)、确定并联冗余柔索机构运动部件的质量m、转动惯量I_xx、I_yy与I_zz，定义状态向量为x＝(p^T,Θ^T)^T，其中p＝(p_x,p_y,p_z)^T为并联冗余柔索机构运动部件质心位移向量，为并联冗余柔索机构运动部件姿态角向量，为滚动角、θ为俯仰角、ψ为偏航角；定义PID控制参数矩阵K_p、K_I与K_D，控制参数矩阵维度为6×6；

步骤(2)、通过传感器提取与处理信息，获得并联冗余柔索机构运动部件状态信息x，确定该时刻目标轨迹x_d，求解运动误差e＝x_d-x；

步骤(3)、根据步骤(2)所获得的运动部件状态信息向量x，求解并联冗余柔索机构的雅可比矩阵J，雅可比矩阵维度为6×m，m≥6为控制柔索数量；

步骤(4)、通过步骤(1)所获得的m、I_xx、I_yy、I_zz与步骤(2)所获得的并联冗余柔索机构状态信息x、运动误差e，求解伺服控制伪指令f，伺服控制伪指令的维度为6×1；

步骤(5)、通过步骤(2)所获得的并联冗余柔索机构状态信息x与步骤(3)所获得的雅可比矩阵J求解控制张力最小超球直径r；

步骤(6)、根据步骤(4)所获得的伺服控制伪指令与步骤(5)所获得的控制张力超球最小直径r，求解控制张力分配向量T，控制张力分配向量维度为m×1，将控制张力分配向量T分配到柔索驱动电机执行；

步骤(7)、运行时间每间隔控制步长Δt，重复步骤(2)至(6)直至并联冗余柔索机构完整生成目标运动轨迹；

步骤(4)中的伺服控制伪指令f通过下式进行求解：

其中M(x)为质量矩阵，为柯氏力与向心力矩阵，x为状态变量，为状态变量速度，为目标轨迹加速度，g为外部力与扭矩所组成的外载荷向量；为运动误差对时间的一阶导；

步骤(5)中的控制张力最小超球半径r通过下式进行求解

其中a、b与c为中间变量；

步骤(6)中的控制张力分配向量T通过下式进行求解

T＝-J^T(JJ^T)f+rηη^TI₃₁

其中η为矩阵-J的零空间。

2.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：步骤(3)中的雅可比矩阵J通过下式进行求解

上式中u_i为全局坐标系下第i根柔索的单位方向向量，b_i为并联冗余柔索机构运动部件随体坐标系下运动部件质心与第i根柔索连接点间的向量，R为姿态角转换矩阵。

3.根据权利要求2所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：R通过任意转换方法，以3-2-1转换顺序，通过下式进行求解

其中c_α与s_α分别表示cos(α)与sin(α)。

4.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：

其中Q为姿态角速度转换矩阵，m为质量，I_xx、I_yy、I_zz为x、y、z三个方向上的转动惯量；为目标姿态角向量对时间的一阶导。

5.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：以3-2-1转换顺序，通过下式进行求解，

6.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：b＝I₃₁J^T(JJ^T)f,c＝f^T(JJ^T)f,