[发明专利]一种自主水下航行器四通道耦合控制方法

权利要求书

1.一种自主水下航行器四通道耦合控制方法，其特征在于：包括以下步骤：

步骤1：分别计算纵平面控制力矩、水平面控制力矩和横滚控制力矩：

AUV纵平面运动控制采用AUV深度控制，根据AUV每个控制周期测量的深度、俯仰角和俯仰角速度，按照以下公式进行混合计算得到纵平面控制力矩：

其中depth、depth_ref和depth_err分别为深度、参考深度和深度误差；θ和ω_z分别为俯仰角和俯仰角速度；α_f为AUV自由角；k_pd、k_id、k_pθ和k_dθ分别为深度比例系数、深度积分系数、俯仰角比例系数和俯仰角微分系数；L_e为纵平面控制力矩；

AUV水平面运动控制采用AUV航向角控制，根据AUV每个周期测量的航向角和航向角速度，按照以下公式经过混合计算得出水平面控制力矩：

其中ψ、ψ_ref和ψ_err分别为航向角、参考航向角和航向角误差；ω_y为航向角速度；k_pψ和k_dψ分别为航向角比例系数和航向角微分系数；L_r为水平面控制力矩；

AUV横滚控制采用AUV横滚角控制，根据AUV每个周期测量的横滚角和横滚角速度，按照以下公式经过混合计算得出横滚角控制力矩：

其中和ω_x分别为横滚角和横滚角速度；和分别为横滚角比例系数和横滚角微分系数；L_d为横滚控制力矩；

步骤2：根据步骤1计算得到的纵平面控制力矩、水平面控制力矩和横滚控制力矩，采用以下公式进行AUV舵角分配，将纵平面控制力矩、水平面控制力矩和横滚控制力矩分配到AUV的四个舵面上，通过舵面产生需要的控制力矩：

其中AUV舵面采用十字形舵面布局，D＝[d_u d_d d_l d_r]^T为AUV上、下、左、右四个舵角值；k_LD为力矩舵角转换系数；

为加入横滚角修正的舵角分配矩阵；

步骤3：AUV速度控制采用以下公式计算转速指令，调节AUV主推进电机的转速控制AUV的速度：

其中F为速度与转速对应关系映射；v_r为参考速度；为第k次速度误差；k_e为误差积分系数；R为转速指令；

步骤4：将步骤2和步骤3得到的舵角和转速指令发送到AUV执行机构执行。