本发明涉及一种联合横滚机构与扑翼非对称相位差的航向控制方法，通过姿态传感器获取当前航向信息，计算出偏航值；根据当前航行器纵平面速度快慢，利用模糊PID控制，在航行器滑翔纵平面速度较快时，采用横滚机构进行航向控制，速度较慢时，如下潜上浮转换阶段，采用非对称的扑翼相位差进行定航。横滚机构与扑翼协同控制既可降低功耗，解决了仿生航行器长期扑动功耗大的问题，能在航行器纵向速度大时仅转动横滚块这单独一个机构来控制航向，减小能耗；又能在纵向速度减小，横滚机构不能有效控制航向时，采用扑翼的非对称相位差控制。使得航行器在整个航行的航向控制过程中响应更快、机动性更高，更能适应海况复杂多变的海域。
1.一种联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于：航行器左侧包含2个舵机，分别命名为第一舵机(1)和第二舵机(2)，右侧命名为第三舵机(3)和第四舵机(4)；每个舵机由一个转换公式的输出控制，一个舵机为一个单元；航向控制步骤如下：步骤1：通过姿态传感器获取水下航行器当前航向角 任务设定的目标航向角为
步骤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}，
步骤4：将模糊化后的偏航角和航向偏差变化率给到模糊规则表中进行查表，以ec值作为纵坐标，e值作为横坐标进行表格查询，对原有PID参数进行自整定，根据模糊规则表得出Δkp、Δki和kd值，整正后的PID参数为：其中kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；Δkp为模糊控制器查表得到的比例系数整定调整量，Δki为模糊控制器查表得到的积分系数整定调整量，Δkd为模糊控制器查表得到的微分系数整定调整量；kpF为经过模糊控制器查表得到的比例系数整定调整量Δkp与原始比例系数kp相加后的比例系数，kiF为经过模糊控制器查表得到的积分系数整定调整量Δki与原始积分系数ki相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数kd相加后的微分系数；
步骤5：以模糊控制器整定后PID控制器计算出的航向控制量yaw(t)为：
式中yaw(n)为第n周期控制器的控制输出；T为离散的时间间隔；
步骤6：根据航行器从姿态传感器获取的纵向速度v为判断条件，将定航任务分为以下两种情况：a)当v>a时，水下航行器处于迅速下潜/上浮阶段，采用横滚机构控制航向,依据控制量计算出横滚机构的横滚块偏移的角度的控制量为：式中：R为横滚块偏移的角度，Amax为横滚块的最大偏移角度，横滚块的偏转范围为[‑Amax Amax],yaw(n)控制器的控制量输出，M为控制量转换系数；
b)当v≤a时，水下航行器处于下潜与上浮转换阶段，采用改变扑翼的非对称相位差来控制航向；
步骤7：将控制量经相位差计算公式线性转化为两侧扑翼的相位差变化量输入到CPG网络中，同时输入设置的扑翼的幅值Ri、频率νi，经CPG网络迭代计算得到扑翼扑动的角度θi，从而实现用非对称相位差完成航向控制；相位差计算公式为：式中：yaw(n)为控制器的控制量输出，Q为控制量转换系数，A为直游时期望相位差，为舵机1、2的期望相位差， 为舵机2、3的期望相位差， 为舵机3、4的期望相位差；
所述CPG网络由相位方程、振幅方程和输出方程构成：
各方程分别为相位方程、振幅方程和输出方程，式中φi表示第i个单元的相位，νi表示固有频率，ωij表示第j个单元对第i个单元的耦合权重， 表示期望相位差；ri表示幅值，ai表示控制幅值收敛速度的正常数，Ri表示期望振幅；θi表示输出值。
2.根据权利要求1所述的联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于所述CPG网络的拓扑结构为：左侧第一单元(1)与第二单元(2)联系，右侧单元(3)与单元(4)联系；通过第二单元(2)和第三单元(3)建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω13、ω23三种形式，可取值为ω12＝ω34＝4、ω23＝3。
3.根据权利要求1所述的联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于所述CPG网络的拓扑结构为：左侧第一单元(1)与第二单元(2)联系，右侧单元(3)与单元(4)联系；通过第一单元(1)和第三单元(3)建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω13、ω34三种形式，可取值为ω12＝ω34＝2、ω13＝1。
4.根据权利要求1所述的联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于所述CPG网络的拓扑结构为：左侧第一单元(1)与第二单元(2)联系，右侧单元(3)与单元(4)联系；通过第一单元(1)和第三单元(3)以及第二单元(2)和第四单元(4)建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω34、ω13、ω24四种形式，取值为ω12＝ω34＝4、ω13＝ω24＝2。
5.根据权利要求1所述的联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于所述CPG网络的拓扑结构为：左侧第一单元(1)与第二单元(2)联系，右侧单元(3)与单元(4)联系；通过第二单元(2)和第三单元(3)以及第一单元(1)和第四单元(4)建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω23、ω34、ω41四种形式，取值为ω12＝ω23＝ω41＝ω34＝3。
6.根据权利要求1所述的联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于所述CPG网络的拓扑结构为：左侧第一单元(1)与第二单元(2)联系，右侧单元(3)与单元(4)联系；通过第一单元(1)和第四单元(4)建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω34、ω14、三种形式，取值为ω12＝ω34＝2、ω14＝1。
一种联合横滚机构与扑翼非对称相位差的航向控制方法
技术领域
[0001] 本发明属于仿蝠鲼航行器航向控制的方法，涉及一种联合横滚机构与扑翼非对称相位差的航向控制方法，具体来说涉及的是一种利用横滚机构和扑翼相位差协同作用进行仿蝠鲼航行器航向控制的方法。
背景技术
[0002] 水下滑翔机是一种在海洋中作滑翔运动并在运动过程中采集观测海洋环境的新型观测平台，利用自身重力与浮力差驱动运动，实现海洋的大区域多尺度监测，适用于海洋环境观测和军事领域的探测。仿蝠鲼水下航行器在水下滑翔机原理基础上，结合滑翔机的滑翔推进和仿海洋生物蝠鲼的柔性推进，实现滑扑一体化，具有更好的机动性和更高的隐蔽性，可以实现广域粗维度、定点细维度的水下观测，更适应于各种复杂海域。
[0003] 为了解决水下航行器执行任务时的偏航问题，传统的水下航行器多是依靠螺旋桨推进，推进速度快，但航行姿态难以灵活改变。且已有多种针对水下航行器航向控制的方法，如模糊控制算法、PID控制算法等，大多数是基于航行器建立的运动学和动力学模型进行控制算法设计。而仿蝠鲼航行器推进方式与传统的航行器不同，且至今没有比较准确的模型，故采用传统的方法实现其精确航向控制十分困难，需要设计一种新的航向控制的方法。而在公开的文献中，还未有用横滚机构和扑翼相位差协同作用来实现仿蝠鲼航行器航向控制的例子。
发明内容
[0004] 要解决的技术问题
[0005] 为了避免现有技术的不足之处，本发明提出一种联合横滚机构与扑翼非对称相位差的航向控制方法，针对滑扑一体的仿蝠鲼航行器，将定航任务根据纵向速度的大小分为两种工况进行控制方案设计，结合模糊PID控制算法对横滚机构和经CPG神经网络控制的扑翼系统进行调节，进而完成定航游动的任务。
[0006] 为实现上述任务，本发明采用的技术方案是通过姿态传感器获取当前航向信息，计算出偏航值；根据当前航行器纵平面速度快慢，利用模糊PID控制，在航行器滑翔纵平面速度较快时，采用横滚机构进行航向控制，速度较慢时，如下潜上浮转换阶段，采用非对称的扑翼相位差进行定航。
[0007] 本发明通过调节横滚机构、扑翼的非对称相位差实现仿蝠鲼水下航行器控制的基本原理是：
[0008] 以仿蝠鲼航行器滑行过程为例来进行分析说明，当仿蝠鲼航行器以一定速度下潜滑翔的时候，调节横滚机构发生偏转，此时仿蝠鲼航行器将沿着航行器的弦向发生偏转，使仿蝠鲼航行器呈现偏转状态。由于航行器机体与水平面间存在一个夹角，这时垂直于航行器表面的流体动力将产生一个使航行器绕过重心的竖直轴的偏航力矩，导致仿蝠鲼航行器航向发生变化。利用这个偏航力矩就可以对仿蝠鲼航行器的航向进行矫正。
[0009] 纵平面速度较慢时，通过两侧扑翼以非对称的相位差扑动，也可以实现仿蝠鲼水下航行器的姿态调整。通过对生物蝠鲼的研究表明，生物蝠鲼胸鳍扑动产生的推进力与波动传递的方向相反，当生物蝠鲼两侧胸鳍扑动产生的波动传递方向相反时，会形成转弯力矩，从而完成转弯。同样当仿蝠鲼水下航行器两侧扑翼以非对称相位差扑动时，可以在主体两侧形成同方向而不同大小甚至不同方向的波动传递，从而形成转弯力矩，实现航行器的姿态调整。
[0010] 技术方案
[0011] 一种联合横滚机构与扑翼非对称相位差的航向控制方法，其特征在于：航行器左侧包含2个舵机，分别命名为第一舵机1和第二舵机2，右侧命名为第三舵机3和第四舵机4；每个舵机由一个转换公式的输出控制构成一个单元；航向控制步骤如下：
[0012] 步骤1：通过姿态传感器获取水下航行器当前航向角 任务设定的目标航向角为则偏航角e：
[0013]
[0014] 步骤2：对偏航角求导，得到航向角偏差变化率ec：
[0015]
[0016] 其中t为水下航行器的姿态传感器信息更新时间；
[0017] 步骤3：将求得的偏航角和航向偏差变化率模糊化处理，其模糊化的语言变量用负大即NB、负中即NM、负小即NS、零即ZO、正小即PS、正中即PM、正大即PB表述，。模糊集合为：
[0018] ec＝{NB,NM,NS,ZO,PS,PM,PB}，
[0019] e＝{NB,NM,NS,ZO,PS,PM,PB}，
[0020] 步骤4：将模糊化后的偏航角和航向偏差变化率给到模糊规则表中进行查表，以ec值作为纵坐标，e值作为横坐标进行表格查询，对原有PID参数进行自整定，根据模糊规则表得出Δkp、Δki和kd值，整正后的PID参数为：
[0021]
[0022] 其中kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；Δkp为模糊控制器查表得到的比例系数整定调整量，Δki为模糊控制器查表得到的积分系数整定调整量，Δkd为模糊控制器查表得到的微分系数整定调整量；kpF为经过模糊控制器查表得到的比例系数整定调整量Δkp与原始比例系数kp相加后的比例系数，kiF为经过模糊控制器查表得到的积分系数整定调整量Δki与原始积分系数相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数Δkd相加后的微分系数；
[0023] 模糊规则表1：
[0024] kp模糊控制系数修正表
[0025]
[0026] 模糊规则表2：
[0027] ki/kd模糊控制系数修正表
[0028]
[0029] 步骤5：以模糊控制器整定后PID控制器计算出的航向控制量yaw(t)为：
[0030]
[0031] 式中dt为微分时间；
[0032] 离散化得：
[0033]
[0034] 式中yaw(t)为当前时刻控制器的控制输出；T为离散的时间间隔；
[0035] 步骤6：根据航行器从姿态传感器获取的纵向速度v为判断条件，将定航任务分为以下两种情况：
[0036] a)当v>a时，水下航行器处于迅速下潜/上浮阶段，采用横滚机构控制航向,依据控制量计算出横滚机构的横滚块偏移的角度的控制量为：
[0037]
[0038] 式中：R为横滚块偏移的角度，Amax为横滚块的最大偏移角度，横滚块的偏转范围为[‑Amax Amax],yaw(t)控制器的控制量输出，M为控制量转换系数；
[0039] b)当v≤a时，水下航行器处于下潜与上浮转换阶段，采用改变扑翼的非对称相位差来控制航向；
[0040] 步骤7：将控制量经下式做线性转化为的两侧扑翼的相位差变化量输入到CPG网络中，同时输入设置的扑翼的幅值Ri、频率νi，经CPG网络迭代计算得到扑翼扑动的角度θi，从而实现用非对称相位差完成航向控制；
[0041]
[0042] 式中：yaw(t)为控制器的控制量输出，Q为控制量转换系数。A为直游时期望相位差。 为舵机1、2的期望相位差， 为舵机2、3的期望相位差， 为舵机3、4的期望相位差；
[0043] 所述CPG网络由相位方程、振幅方程和输出方程构成：
[0044]
[0045]
[0046] θi＝ri(1+cosφi)
[0047] 各方程分别为相位方程、振幅方程和输出方程。式中φi表示第i个单元的相位，νi表示固有频率，ωij表示第j个单元对第i个单元的耦合权重， 表示期望相位差；ri表示幅值，ai表示控制幅值收敛速度的正常数，Ri表示期望振幅；θi表示输出值。
[0048] 所述CPG网络的拓扑结构为：左侧第一单元1与第二单元2联系，右侧单元3与单元4联系；通过第二单元2和第三单元3建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω13、ω23三种形式，可取值为ω12＝ω34＝4、ω23＝3。
[0049] 所述CPG网络的拓扑结构为：左侧第一单元1与第二单元2联系，右侧单元3与单元4联系；通过第一单元1和第三单元3建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω13、ω34三种形式，可取值为ω12＝ω34＝2、ω13＝1。
[0050] 所述CPG网络的拓扑结构为：左侧第一单元1与第二单元2联系，右侧单元3与单元4联系；通过第一单元1和第三单元3以及第二单元2和第四单元4建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω34、ω13、ω24四种形式，取值为ω12＝ω34＝4、ω13＝ω24＝2。
[0051] 所述CPG网络的拓扑结构为：左侧第一单元1与第二单元2联系，右侧单元3与单元4联系；通过第二单元2和第三单元3以及第一单元1和第四单元4建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω23、ω34、ω41四种形式，取值为ω12＝ω23＝ω41＝ω34＝3。
[0052] 所述CPG网络的拓扑结构为：左侧第一单元1与第二单元2联系，右侧单元3与单元4联系；通过第一单元1和第四单元4建立左右侧之间的联系；其中的第j个单元对第i个单元的耦合权重ωij只存在ω12、ω34、ω14、三种形式，取值为ω12＝ω34＝2、ω14＝1。
[0053] 有益效果
[0054] 本发明提出的一种联合横滚机构与扑翼非对称相位差的航向控制方法，通过姿态传感器获取当前航向信息，计算出偏航值；根据当前航行器纵平面速度快慢，利用模糊PID控制，在航行器滑翔纵平面速度较快时，采用横滚机构进行航向控制，速度较慢时，如下潜上浮转换阶段，采用非对称的扑翼相位差进行定航。
[0055] 采用本发明具有如下的有益效果：
[0056] 1.对于仿蝠鲼航行器很难建模的对象，传统的PID控制算法难以有效地控制，模糊控制划分的模糊规则是有限的，降低了其控制精度，将二者结合起来既具有模糊控制的响应快、灵活等特点，又具有PID控制的精度高的特点。
[0057] 2.仿蝠鲼航行器与传统的采用螺旋桨推进航行器在驱动模式上存在很大差别，无法采用传统的控制方法。而本发明提出了在扑翼布局的水下航行器领域，通过横滚机构与扑翼协同控制来实现定航，为滑扑一体的仿生水下航行器航向控制提供了一种新思路。
[0058] 3.横滚机构与扑翼协同控制既可降低功耗，解决了仿生航行器长期扑动功耗大的问题，能在航行器纵向速度大时仅转动横滚块这单独一个机构来控制航向，减小能耗；又能在纵向速度减小，横滚机构不能有效控制航向时，采用扑翼的非对称相位差控制，在主体两侧形成同方向而不同大小甚至不同方向的波动传递，从而形成转弯力矩，使得仿蝠鲼水下航行器航向调整速度更快,转弯效果更明显，机动性更高，且横滚稳定性更好。使得航行器在整个航行的航向控制过程中响应更快、机动性更高，更能适应海况复杂多变的海域。
附图说明
[0059] 图1为本发明仿蝠鲼航行器的模糊PD控制的定航原理图；
[0060] 图2为本发明构建的CPG拓扑1连接示意图；
[0061] 图3为本发明构建的CPG拓扑2连接示意图；
[0062] 图4为本发明构建的CPG拓扑3连接示意图；
[0063] 图5为本发明构建的CPG拓扑4连接示意图；
[0064] 图6为本发明构建的CPG拓扑4连接示意图；
[0065] 图7为本发明定航流程图；
[0066] 图8为本发明航行器横滚机构和扑翼协同控制的航向控制曲线图；t1‑t2深度无明显变化，采用扑翼的非对称相位差控制航向；其余段采用横滚机构控制航向。
具体实施方式
[0067] 现结合实施例、附图对本发明作进一步描述：
[0068] 本发明通过姿态传感器获得当前航向角与当前航行器纵向速度，根据纵向速度的大小选用不同的执行机构来调整航向，其具体步骤为：
[0069] 1.通过姿态传感器获取水下航行器当前航向角 任务设定的目标航向角为则偏航角e，则
[0070]
[0071] 2.对偏航角求导，得到航向角偏差变化率ec，则：
[0072]
[0073] 其中t为水下航行器的姿态传感器信息更新时间。
[0074] 将上式离散化为：
[0075]
[0076] 其中e(t)为当前时刻的航向角偏差，e(t‑1)为上一时刻的航向角偏差。
[0077] 3.将求得的偏航角和航向偏差变化率模糊化处理，其模糊化的语言变量用负大(NB)、负中(NM)、负小(NS)、零(ZO)、正小(PS)、正中(PM)、正大(PB)来表述。模糊集合如下所示：
[0078] ec＝{NB,NM,NS,ZO,PS,PM,PB} (4)
[0079] e＝{NB,NM,NS,ZO,PS,PM,PB} (5)
[0080] 将模糊化的偏航角和航向偏差变化率给到模糊规则表中进行查表，即以ec值作为纵坐标，e值作为横坐标进行表格查询，对原有PID参数进行自整定，模糊规则表如表3、4所示：
[0081] 附表3：
[0082] kp模糊控制系数修正表
[0083]
[0084] 附表4：
[0085] ki/kd模糊控制系数修正表
[0086]
[0087] 根据模糊规则表得出Δkp、Δki和kd值，整正后的PID参数为：
[0088]
[0089] 其中kp为原始比例系数，ki为原始积分系数，kd为原始微分系数；Δkp为模糊控制器查表得到的比例系数整定调整量，Δki为模糊控制器查表得到的积分系数整定调整量，Δkd为模糊控制器查表得到的微分系数整定调整量；kpF为经过模糊控制器查表得到的比例系数整定调整量Δkp与原始比例系数kp相加后的比例系数，kiF为经过模糊控制器查表得到的积分系数整定调整量Δki与原始积分系数相加后的积分系数，kdF为经过模糊控制器查表得到的微分系数整定调整量Δkd与原始微分系数Δkd相加后的微分系数；
[0090] 利用模糊控制器整定后PID控制器计算出的航向控制量为：
[0091]
[0092] 式中dt为微分时间。
[0093] 离散化可得：
[0094]
[0095] 式中yaw(t)为当前时刻控制器的控制输出；T为离散的时间间隔。
[0096] 由流体动力的相关知识可得，航行器在纵向速度小的情况下，因横滚角产生的偏航力矩很小，转弯效果不明显。基于此设计了横滚机构与扑翼协同作用来实现航行器定航。根据航行器从姿态传感器获取的纵向速度v为判断条件，将定航任务分为以下两种情况：
[0097] a)迅速上浮、下潜：当v>a时，水下航行器处于迅速下潜/上浮阶段，采用横滚机构控制航向,根据计算所得的控制量调节航行器内部的横滚机构左右滚动，改变航行器的横滚角，航行器需左转时，横滚机构左高右低，使得航行器呈左偏航姿态；航行器需右转时，横滚机构左低右高，使得航行器呈右偏航姿态，最终完成定航任务。
[0098] 其中，依据控制量计算出横滚机构的横滚块偏移的角度，横滚块偏转范围是‑45°到45°，具体公式为：
[0099]
[0100] 式中：R为横滚块偏移的角度，Amax为横滚块的最大偏移角度，横滚块的偏转范围为[‑Amax Amax],yaw(t)控制器的控制量输出，M为控制量转换系数。
[0101] 如模糊PID计算所得当前θ(t)为6，M为0.1，即横滚机构块需要移动到39°的位置。
[0102] b)下潜、上浮转换时：当v≤a时，水下航行器处于下潜与上浮转换阶段，纵向速度较小，采用改变扑翼的非对称相位差来控制航向；
[0103] 为使扑翼顺滑扑动，本专利构建了一种人工CPG神经控制网络实现仿蝠鲼航行器的扑动控制，采用最简连接的CPG相位振荡器模型，仿蝠鲼航行器左侧胸鳍包含2个舵机，分别命名为舵机1、2，右侧胸鳍舵机命名为舵机3、4。每个舵机由一个相位振荡器模型控制，舵机间的相互联系通过耦合项实现。左侧胸鳍单元1与单元2联系；右侧胸鳍单元3与单元4联系；通过胸鳍中间单元1和3建立左右侧胸鳍之间的联系。其连接方式如图2所示，该模型主要由相位方程、振幅方程和输出方程构成，具体如下：
[0104]
[0105] 其中，各方程分别为相位方程、振幅方程和输出方程。式中φi表示第i个单元的相位，νi表示固有频率，ωij表示第j个单元对第i个单元的耦合权重， 表示期望相位差；ri表示幅值，ai表示控制幅值收敛速度的正常数，Ri表示期望振幅；θi表示输出值。
[0106] 舵机间的相互联系通过耦合项实现，ωij表示第j个单元对第i个单元的耦合项，此种连接方式ωij只存在ω12、ω13、ω23三种形式，可取值为ω12＝ω34＝4、ω23＝3。
[0107] 经模糊PID计算所得的控制量经公式(21)做线性转化为的两侧扑翼的相位差变化量输入到CPG网络中，同时输入设置的扑翼的幅值Ri、频率νi，经CPG网络迭代计算得到扑翼扑动的角度θi，从而实现用非对称相位差完成航向控制。
[0108]
[0109] 式中：yaw(t)为模糊PID控制器的控制量输出，Q为控制量转换系数，A为直游时期望相位差。 为舵机1、2的期望相位差， 为舵机2、3的期望相位差， 为舵机3、4的期望相位差。
[0110] 当航行器需要左转来调整航向时，扑翼左侧两鳍条间的相位差 减小或反向，右侧两鳍条间的相位差 增大，左侧扑翼推进力小于右侧扑翼推进力或左侧扑翼推进力转向，完成左转达到目标航向；当航行器需要右转来调整航行时，扑翼左侧两鳍条间的相位差 增大，右侧两鳍条间的相位差 增大减小或反向，左侧扑翼推进力大于右侧扑翼推进力或右侧扑翼推进力转向，完成右转达到目标航向；
[0111] 如航行器需要左转来调整航向时，输入设置的扑翼的幅值30°、频率0.3hz，A为20，经模糊PID计算所得值θ(t)为6，Q为5，航行的扑翼将以 振幅30°，频率0.3hz的角度进行定航扑动。
[0112] 具体其程序流程图如图7所示，试验定航曲线如图8所示。
[0113] 当CPG相位振荡器模型拓扑采用图3时，振荡器模型中单元连接为单元1与单元2连接；单元1与单元3连接；单元3与单元4连接。ωij表示第j个单元对第i个单元的耦合项，此种连接方式ωij只存在ω12、ω13、ω34三种形式，可取值为ω12＝ω34＝2、ω13＝1。
[0114] 当CPG相位振荡器模型拓扑采用图4时，振荡器模型中单元连接为单元1与单元2连接；单元3与单元4连接；单元1与单元3连接；单元2与单元4连接。ωij表示第j个单元对第i个单元的耦合项，此种连接方式ωij只存在ω12、ω34、ω13、ω24四种形式，可取值为ω12＝ω34＝4、ω13＝ω24＝2。
[0115] 当CPG相位振荡器模型拓扑采用图5时，振荡器模型中单元连接为单元1与单元2连接；单元2与单元3连接；单元3与单元4连接；单元4与单元1连接。ωij表示第j个单元对第i个单元的耦合项，此种连接方式ωij只存在ω12、ω23、ω34、ω41四种形式，可取值为ω12＝ω23＝ω41＝ω34＝3。
[0116] 当CPG相位振荡器模型拓扑采用图6时，振荡器模型中单元连接为单元1与单元2连接；单元3与单元4连接；单元1与单元4连接。ωij表示第j个单元对第i个单元的耦合项，此种连接方式ωij只存在ω12、ω34、ω14、三种形式，可取值为ω12＝ω34＝2、ω14＝1。
