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

摘要

本发明提出一种自主水下航行器四通道耦合控制方法，首先分别获取姿态传感器和深度传感器数据，将各数据进行滤波处理；然后分别进行纵平面控制计算、水平面控制计算和横滚控制计算；之后进行舵角分配计算，将控制力矩分配到四个舵面上，得到操舵角度；然后获取速度传感器信息，进行滤波之后进行速度控制计算，得到控制转速指令；最后将舵角和转速指令发送到执行机构执行。本发明通过控制算法，使单桨加十字舵型AUV通过周期性控制舵角和转速，实现了AUV稳定航行，而且本发明中采用的舵角分配算法考虑到了AUV横滚带来的影响，对任意横滚角下的AUV都适用，特别是当AUV横滚角非零时，比传统的静态舵分配方法具有更好的控制效果。

主权项

1.一种自主水下航行器四通道耦合控制方法，其特征在于：包括以下步骤：/n步骤1：分别计算纵平面控制力矩、水平面控制力矩和横滚控制力矩：/nAUV纵平面运动控制采用AUV深度控制，根据AUV每个控制周期测量的深度、俯仰角和俯仰角速度，按照以下公式进行混合计算得到纵平面控制力矩：/n /n其中depth、depth_ref和depth_err分别为深度、参考深度和深度误差；θ和ω_z分别为俯仰角和俯仰角速度；α_f为AUV自由角；k_pd、k_id、k_pθ和k_dθ分别为深度比例系数、深度积分系数、俯仰角比例系数和俯仰角微分系数；L_e为纵平面控制力矩；/nAUV水平面运动控制采用AUV航向角控制，根据AUV每个周期测量的航向角和航向角速度，按照以下公式经过混合计算得出水平面控制力矩：/n /n其中ψ、ψ_ref和ψ_err分别为航向角、参考航向角和航向角误差；ω_y为航向角速度；k_pψ和k_dψ分别为航向角比例系数和航向角微分系数；L_r为水平面控制力矩；/nAUV横滚控制采用AUV横滚角控制，根据AUV每个周期测量的横滚角和横滚角速度，按照以下公式经过混合计算得出横滚角控制力矩：/n /n其中和ω_x分别为横滚角和横滚角速度；和分别为横滚角比例系数和横滚角微分系数；L_d为横滚控制力矩；/n步骤2：根据步骤1计算得到的纵平面控制力矩、水平面控制力矩和横滚控制力矩，采用以下公式进行AUV舵角分配，将纵平面控制力矩、水平面控制力矩和横滚控制力矩分配到AUV的四个舵面上，通过舵面产生需要的控制力矩：/n /n其中AUV舵面采用十字形舵面布局，D＝[d_u d_d d_l d_r]^T为AUV上、下、左、右四个舵角值；k_LD为力矩舵角转换系数；/n /n为加入横滚角修正的舵角分配矩阵；/n步骤3：AUV速度控制采用以下公式计算转速指令，调节AUV主推进电机的转速控制AUV的速度：/n /n其中F为速度与转速对应关系映射；v_r为参考速度；为第k次速度误差；k_e为误差积分系数；R为转速指令；/n步骤4：将步骤2和步骤3得到的舵角和转速指令发送到AUV执行机构执行。/n