[发明专利]一种滑扑一体航行器的航向控制方法

权利要求书

1.一种滑扑一体航行器的航向控制方法，其特征在于：滑扑一体航行器左侧包含2个舵机，分别命名为第一舵机单元(1)和第二舵机单元(2)，右侧命名为第三舵机单元(3)和第四舵机单元(4)；每个舵机单元由一个转换公式的输出控制构成一个单元；航向控制步骤如下：

步骤1：通过姿态传感器获取水下航行器当前航向角任务设定的目标航向角为则偏航角e：

步骤2：对偏航角求导，得到航向角偏差变化率ec：

其中t为水下航行器的姿态传感器信息更新时间；

步骤3：将求得的偏航角和航向偏差变化率模糊化处理，其模糊化的语言变量以负大即NB、负中即NM、负小即NS、零即ZO、正小即PS、正中即PM、正大即PB表述，模糊集合为：

ec＝{NB,NM,NS,ZO,PS,PM,PB}

e＝{NB,NM,NS,ZO,PS,PM,PB}

将模糊化的偏航角和航向偏差变化率给到模糊规则表中，以ec值作为纵坐标，e值作为横坐标，对原有PD数进行自整定，根据模糊规则表1和表2得出Δk_p、k_d值，整正后的PD参数为：

其中k_p为原始比例系数，k_d为原始微分系数；Δk_p为模糊控制器查表得到的比例系数整定调整量，Δk_d为模糊控制器查表得到的微分系数整定调整量；k_pF为经过模糊控制器查表得到的比例系数整定调整量Δk_p与原始比例系数k_p相加后的比例系数，k_dF为经过模糊控制器查表得到的微分系数整定调整量Δk_d与原始微分系数Δk_d相加后的微分系数；

所述模糊规则表1

k_p模糊控制系数修正表

所述模糊规则表2

k_d模糊控制系数修正表

步骤4：利用模糊控制器整定后PID控制器计算出的航向控制量为：

式中dt为微分时间；

离散化得：

式中y(t)为当前时刻控制器的控制输出；t为离散的时间间隔；

步骤5：根据航行器从姿态传感器获取的纵向速度v为判断条件，将定航任务分为以下两种情况：

a)当va时，水下航行器处于迅速下潜/上浮阶段，采用横滚机构控制航向,根据计算所得的控制量调节航行器内部的横滚机构左右滚动，改变航行器的横滚角，从而实现偏航姿态，完成定航任务；

依据控制量计算出横滚机构的横滚块偏移的角度：

式中：roll为横滚块偏移的角度，B_l横滚块的左转最大偏移角度，为正值；B_r横滚块的右转最大偏移角度，为负值；B_z为横滚块的零位；横滚块的偏转范围为[B_r B_l],y(t)控制器的控制量输出，A为控制量转换系数,M为控制量限定值，防止输入y(t)的值过大导致机构偏转超过限位；

b)当v≤a时，水下航行器处于下潜与上浮转换阶段，纵向速度较小，采用改变扑翼的非对称振幅来控制航向；

步骤6：将模糊PD计算所得的控制量经转化后作为振幅变化量输入到CPG网络中，同时输入设置的扑翼的相位差、频率，经CPG网络迭代计算得到扑翼扑动的角度，从而实现用非对称振幅完成航向控制；

转化公式为：

式中e偏航角,Q为直游时期望振幅，y(t)为控制器的控制量输出，R₁、R₂、R₃、R₄分别为舵机1、2、3、4期望相位差；

所述滑扑一体航行器的CPG网络为：

其中，各方程分别为相位方程、振幅方程和输出方程；式中φ_i表示第i个单元的相位，ν_i表示固有频率，ω_ij表示第j个单元对第i个单元的耦合权重，表示期望相位差；r_i表示幅值，a_i表示控制幅值收敛速度的正常数，R_i表示期望振幅；θ_i表示输出值。