[发明专利]考虑多体耦合的水下机器人控制方法

权利要求书

1.考虑多体耦合的水下机器人控制方法，其特征在于，包括以下步骤：

1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；

2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；

3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力τ_arm；

步骤1)-3)的具体实现方法如下：

首先，运动学控制模块IK根据下述公式计算出基座和机械臂的参考轨迹：

式(1.1)中，是计算出的参考轨迹，其中，前六个元素是基座的参考轨迹，后六个元素是机械臂的参考轨迹，将它们分别传给机械臂速度环和基座控制器；与基座雅克比矩阵相关，代表机械臂的线速度，ω_ee代表机械臂的角速度；K_a是误差反馈系数；代表机械臂位置和姿态误差列向量；

机械臂的速度环根据伺服电机的不同，采用PI控制或者PID控制；

基座控制器按照下式进行设计：

其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；M_v＝M_RB+M_A，M_RB是基座的广义质量矩阵，M_A是基座附加质量矩阵；是基座加速度；C_v＝C_RB+C_A，C_RB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，C_A是基座附加质量引起的柯氏力和向心力项；v代表基座速度；D_RB(v)代表基座的粘性水动力，由摩擦力引起；代表惯性坐标系下的恢复力，等于重力和浮力的合力；τ_v＝B_vu_v代表基座的控制推力，B_v是推力分配矩阵，u_v是推力器的推力阵列；τ_arm代表机械臂对基座的作用力；

这里，应用反馈线性化进行基座控制律设计，所设计的控制律如下：

上三式中，代表推力分配矩阵B_v的逆矩阵；是控制方程中的基座加速度；是一个变化矩阵，是惯性系到体坐标系的旋转矩阵，J_k,o是基座雅克比矩阵；η是基座位置和姿态组成的列向量；下标d代表期望值；下标e代表存在误差的量；K_pv、K_vv和K_iv分别是PID控制中的比例系数矩阵、微分系数矩阵和误差积分系数矩阵；

式(1.3)中，机械臂对基座的耦合力和力矩由牛顿欧拉迭代动力学计算，计算公式为：

式(1.5)～(1.6)中，M_i是包含附加质量的质量矩阵，是包含附加惯量的、相对于质心的惯性张量，是坐标系i的原点到连杆i的质心的矢径，d_i表示作用在连杆上的升力和阻力，表示坐标系i-1的原点到坐标系i的原点的矢径，是坐标系i-1的原点到连杆i的质心的矢径，表示坐标系i-1的原点到连杆i的浮心的矢径；在坐标系中，表示连杆i受到的浮力，gⁱ表示连杆i受到的重力，f_iⁱ表示连杆i-1作用在连杆i上的力，表示连杆i-1作用在连杆i上的力矩；默认升力、阻力和重力作用在质心；实际计算时，先用式(1.5)，从基座开始外推，计算机械臂连杆所受的惯性力和惯性力矩，然后利用式(1.6)进行内推，依次计算各个关节之间的作用力和力矩，直到计算出机械臂与基座之间的耦合力和耦合力矩；

4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出应该对基座施加的控制力τ_v；具体计算如下：

其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；M_v＝M_RB+M_A，M_RB是基座的广义质量矩阵，M_A是基座附加质量矩阵；是基座加速度；C_v＝C_RB+C_A，C_RB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，C_A是基座附加质量引起的柯氏力和向心力项；v代表基座速度；D_RB(v)代表基座的粘性水动力，由摩擦力引起；代表惯性坐标系下的恢复力，等于重力和浮力的合力；τ_v＝B_vu_v代表基座的控制推力，B_v是推力分配矩阵，u_v是推力器的推力阵列；τ_arm代表机械臂对基座的作用力。