一种基于模糊自适应算法的无人艇航向航速协同控制方法
技术领域
本发明属于船舶自主运动控制技术领域，具体地指一种基于模糊自适应算法的无人艇航向航速协同控制方法。
背景技术
自主运动控制是无人艇(Unmanned Surface Vehicle，简称USV)区别于有人操纵船舶的核心技术之一。在自主运动控制问题中，实用有效的航向航速控制算法是USV平台在各种不确定水面环境中完成复杂航行任务的关键。
目前的USV航向航速控制方法主要包括：(1)将航向保持作为自动控制目标，使船舶能够自动跟随给定的航向，但是航向给定不在自主控制的范围内。相应的控制算法主要包括：基于精确运动模型的PID控制、Lyapunov控制、Backstepping控制等常规控制算法，基于在线辨识模型参数的变结构控制等自适应控制算法，基于模糊规则的模糊控制算法等。(2)在航向保持的基础上，将航向给定纳入自主控制的范围，由艇载计算机通过控制算法自主得出航向给定值，进行闭环控制，但航速给定不包括在自主控制的范围内。
以上控制方法存在以下问题：(1)控制算法方面，基于精确数学模型的常规控制算法中，船体运动模型较难精确确定，而且在不确定和强非线性的水面环境中，线性时不变的运动模型参数难以实时和准确的描述船体运动状态的变化；在自适应控制算法中，自适应函数和精确干扰模型较难确定；在模糊控制算法中，算法结构，输入输出变量的模糊域，模糊规则的确定需要丰富的工程实践经验。(2)控制策略方面，航向航速的协同控制程度较弱，主要考虑航向对运动控制结 果的贡献，仅将航向纳入自主控制算法的范围，而较少考虑航速对运动控制快速性，鲁棒性和自主性的影响。
发明内容
本发明的主要目的是针对现有技术的不足，提供一种基于模糊自适应算法的无人艇航向航速协同控制算法，避免了常规方法中的船体运动建模问题，改进了模糊控制策略和算法，融入了自适应因素，提高了控制方法在不确定水面环境中的适应性和鲁棒性。
为实现上述目的，本发明所设计的一种基于模糊自适应算法的无人艇航向航速协同控制方法，包括如下步骤：
包括如下步骤：
步骤1：获取并设定无人艇自主航行目标点经纬度坐标；
步骤2：采集无人艇的经纬度、航向角：
步骤3：比对所述采集的经纬度与所述无人艇自主航行目标点经纬度是否相同，相同则返回步骤1，不相同则继续下一步骤；
步骤4：根据所述采集的经纬度和航向角计算无人艇与目标点的航向角偏差量P和直线距离偏差量D；
步骤5：对所述航向角偏差量P和直线距离偏差量D进行模糊自适应控制算法的解算，获得所述航向角偏差量P和直线距离偏差量D在模糊集合中的隶属度函数；
步骤6：由所述航向角偏差量P和直线距离偏差量D在模糊集合中的隶属度函数计算并输出舵角偏转控制量U和油门开度控制量V，所述舵角偏转控制量U和油门开度控制量V分别对应于对无人艇的航向和航速控制；
步骤7：按一定周期时间循环操作步骤2至6。
优选地，所述步骤4的具体步骤包括：
步骤4.1：根据所述采集的经纬度航向角得到经度方向上的距离偏差量X，以东为正方向，纬度方向上的距离偏差量Y，以北为正方 向，航向角θ，以真北方向为参考；
步骤4.2：由以下公式得到所述航向角偏差量P和直线距离偏差量D：
P=θ+arctan(YX)+90······X<0θ+arctan(YX)-90······X>0]]>
D=(X2+Y2).]]>
优选地，所述步骤5的具体步骤包括：
步骤5.1：对所述航向角偏差量P引入航向角偏差量隶属度函数
Pj：
Pj={x,μPj(x)|x∈[0,360]}]]>
式中所述航向角偏差量隶属度函数Pj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数；
步骤5.2：对所述直线距离偏差量D引入直线距离偏差量隶属度
函数Dj：
Dj={x,μDj(x)|x∈[0,∞]}]]>
式中所述直线距离偏差量隶属度函数Dj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数。
优选地，所述步骤5.1的航向角偏差量隶属度函数Pj中，令j＝-2，-1，0，1，2分别代表负向大NB，负向小NS，零偏差ZE，正向小PS，正向大PB，Pj的隶属度函数簇![]()
为：
μP-2(x)=1-|x-5050|,|x-50|<500,|x-50|≥50]]>
μP-1(x)=1-|x-12060|,|x-120|<600,|x-120|≥60]]>
μP0(x)=1-|x-18030|,|x-180|<300,|x-180|≥30]]>
μP1(x)=1-|x-24060|,|x-240|<600,|x-240|≥60]]>
μP2(x)=1-|x-31050|,|x-310|<500,|x-310|≥50;]]>
所述步骤5.2的直线距离偏差量隶属度函数Dj中，令j＝0，1，2分别代表零偏差Z，小偏差S，大偏差B，Dj的隶属度函数簇![]()
为：
μD0(x)=1-x50,0<x≤500,x>50]]>
μD1(x)=1-|x-5050|,|x-50|<500,|x-50|≥50]]>
μD2(x)=0,x≤50x-5050,50<x≤1001,x>100.]]>
优选地，所述步骤6的具体步骤包括：
步骤6.1：对所述舵角偏转控制量U引入舵角偏转控制量隶属度函数
Uj,Uj={x,μUj(x)|x∈[-25,25]}]]>
式中所述舵角偏转控制量隶属度函数Uj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数，令j＝-2，-1，0，1，2分别代表了负向大NB，负向小NS，零偏差ZE，正向小PS，正向大PB，Uj的隶属度函数簇![]()
为：
μU-2(x)=1-|x+205|,|x+20|<50,|x+20|≥5]]>
μU-1(x)=1-|x+1010|,|x+10|<100,|x+10|≥10]]>
μU0(x)=1-|x5|,|x|<50,|x|≥5;]]>
μU1(x)=1-|x-1010|,|x-10|<100,|x-10|≥10]]>
μU2(x)=1-|x-205|,|x-20|<50,|x-20|≥5;]]>
步骤6.2：对所述油门开度控制量V引入油门开度控制量隶属度函数Vj，
Vj={x,μVj(x)|x∈[0,60]}]]>
式中所述油门开度控制量隶属度函数Vj是一个标识号为j的模糊集合，j∈自整数，![]()
为偏差量属于各个模糊集合的隶属度函数，令j＝0，1，2分别代表零油门Z，小油门S，大油门B，Vj的隶属度函数簇![]()
为：
μV0(x)1-x10,0<x≤100,x>10]]>
μV1(x)1-|x-3030|,|x-30|<300,|x-30|≥30]]>
μV2(x)=0,x≤30x-3030,30<x≤601,x>60;]]>
步骤6.3：引入中间变量ωi、ωk，i∈整数，k∈整数，计算公式为：
ωi=μPi(x)^μDi(x)]]>
ωk=μPk(x)^μDk(x)]]>
其中符号“∧”指取小，即取交集；
步骤6.4：根据所述中间变量ωi、ωk计算舵角偏转控制量U，计算公式为：
μU(x)=[ωi^μUi(x)]∪[ωk^μUk(x)]]]>
U=∫xμU(x)dx∫μU(x)dx;]]>
步骤6.5：根据所述中间变量ωi、ωk计算油门开度控制量V，计算公式为：
μV(x)=[ωi^μVi(x)]∪[ωk^μVk(x)]]]>
V=∫xμV(x)dx∫μV(x)dx.]]>
优选地，所述步骤6.5之后还包括：
步骤6.6：计算单位时间航向角偏差率Pt＝P/t，单位取角度/秒；
步骤6.7：当所述航向角偏差率Pt＞30角度/秒时，控制间隔为1秒；当30角度/秒＞所述航向角偏差率Pt＞5角度/秒时，控制间隔为2秒；当所述航向角偏差率Pt<5角度/秒时，控制间隔为5秒。
针对常规控制方法中运动模型较难精确确定，动态响应不理想、抗干扰能力较弱；自适应控制算法中自适应函数和精确干扰模型较难确定；模糊控制算法中算法结构，输入输出变量的模糊域，模糊规则较难确定和优化；航向航速协同程度较弱等问题。本发明提出了一种基于模糊自适应算法的无人艇航向航速协同控制方法。
本发明与现有技术相比其优点在于：(1)采用模糊控制算法改善了USV控制方法在不确定水面条件下的适应度，避免了常规方法中的 船体运动建模问题；(2)以经典模糊控制为基础，提出了改进的两输入两输出USV模糊控制算法，增加了距离模糊域函数作为算法输入，并将航向和航速都纳入自主控制范围，进行协同控制，提高了USV运动控制的收敛速度，增强了控制的鲁棒性；(3)采用航向角偏差率作为自变量，自动调节控制间隔参数，使控制算法能够根据控制效果的变化，自适应的调整控制参数，进一步增强了自主控制系统的智能性。
附图说明
图1无人艇模糊自适应航向航速协同控制算法流程图；
图2航向角偏差量P隶属度函数曲线；
图3直线距离偏差量D隶属度函数曲线；
图4舵角偏转控制量U隶属度函数曲线；
图5油门开度控制量V隶属度函数曲线；
图6舵角偏转控制量模糊推理规则；
图7油门开度控制量模糊推理规则；
图8试验艇航行试验结果曲线。
具体实施方式
以下结合附图和具体实施例对发明作进一步的详细说明。无人艇模糊自适应航向航速协同控制算法流程图如图1所示，包括如下步骤：
步骤1：获取并设定无人艇自主航行目标点经纬度坐标；
步骤2：采集无人艇的经纬度、航向角：
步骤3：比对采集的经纬度与无人艇自主航行目标点经纬度是否相同，相同则返回步骤1，不相同则继续下一步骤；
步骤4：根据采集的经纬度和航向角计算无人艇与目标点的航向角偏差量P和直线距离偏差量D；具体步骤包括：
步骤4.1：根据采集的经纬度和航向角得到经度方向上的距离偏差量X(以东为正方向)，纬度方向上的距离偏差量Y(以北为正方 向)，目标航向角θ(以真北方向为参考)；
步骤4.2：由以下公式得到航向角偏差量P和直线距离偏差量D：
P=θ+arctan(YX)+90······X<0θ+arctan(YX)-90······X>0]]>
D=(X2+Y2).]]>
步骤5：对航向角偏差量P和直线距离偏差量D进行模糊自适应控制算法的解算，获得航向角偏差量P和直线距离偏差量D在模糊集合中的隶属度函数；具体步骤包括：
步骤5.1：对航向角偏差量P引入航向角偏差量隶属度函数Pj：
Pj={x,μPj(x)|x∈[0,360]}]]>
式中航向角偏差量隶属度函数Pj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数；
在航向角偏差量隶属度函数Pj中，令j＝-2，-1，0，1，2分别代表了NB(负向大)，NS(负向小)，ZE(零偏差)，PS(正向小)，PB(正向大)，Pj的隶属度函数簇![]()
为：
μP-2(x)=1-|x-5050|,|x-50|<500,|x-50|≥50]]>
μP-1(x)=1-|x-12060|,|x-120|<600,|x-120|≥60]]>
μP0(x)=1-|x-18030|,|x-180|<300,|x-180|≥30]]>
μP1(x)=1-|x-24060|,|x-240|<600,|x-240|≥60]]>
μP2(x)=1-|x-31050|,|x-310|<500,|x-310|≥50]]>
隶属度函数簇![]()
如图2所示，具体参数由试验和专家经验得出。
步骤5.2：对直线距离偏差量D引入直线距离偏差量隶属度函数
Dj：
Dj={x,μDj(x)|x∈[0,∞]}]]>
式中直线距离偏差量隶属度函数Dj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数。
在直线距离偏差量隶属度函数Dj中，令j＝0，1，2分别代表了Z(零偏差)，S(小偏差)，B(大偏差)，Dj的隶属度函数簇![]()
为：
μD0(x)=1-x50,0<x≤500,x>50]]>
μD1(x)=1-|x-5050|,|x-50|<500,|x-50|≥50]]>
μD2(x)=0,x≤50x-5050,50<x≤1001,x>100]]>
隶属度函数簇![]()
如图3所示，具体参数由试验和专家经验得出。
步骤6：由航向角偏差量P和直线距离偏差量D在模糊集合中的隶属度函数计算并输出舵角偏转控制量U和油门开度控制量V，舵角偏转控制量U和油门开度控制量V分别对应于对无人艇的航向和航速控制。具体步骤包括：
步骤6.1：对舵角偏转控制量U引入舵角偏转控制量隶属度函数Uj，
Uj={x,μUj(x)|x∈[-25,25]}]]>
式中舵角偏转控制量隶属度函数Uj是一个标识号为j的模糊集合，j∈自整数，![]()
即为偏差量属于各个模糊集合的隶属度函数，舵角偏转控制量U的模糊推理规则如图6所示。令j＝-2，-1，0，1，2分别代表NB(负向大)，NS(负向小)，ZE(零偏差)，PS(正向小)，PB(正向大)，Uj的隶属度函数簇![]()
为：
μU-2(x)=1-|x+205|,|x+20|<50,|x+20|≥5]]>
μU-1(x)=1-|x+1010|,|x+10|<100,|x+10|≥10]]>
μU0(x)=1-|x5|,|x|<50,|x|≥5;]]>
μU1(x)=1-|x-1010|,|x-10|<100,|x-10|≥10]]>
μU2(x)=1-|x-205|,|x-20|<50,|x-20|≥5;]]>
步骤6.2：对油门开度控制量V引入油门开度控制量隶属度函数Vj，
Vj={x,μVj(x)|x∈[0,60]}]]>
式中油门开度控制量隶属度函数Vj是一个标识号为j的模糊集合，j∈整数，![]()
为偏差量属于各个模糊集合的隶属度函数。油门开度控制量V的模糊推理规则如图7所示。j＝0，1，2分别代表了Z(零油门)，S(小油门)，B(大油门)，Vj的隶属度函数簇![]()
为：
μV0(x)=1-x10,0<x≤100,x>10]]>
μV1(x)=1-|x-3030|,|x-30|<300,|x-30|≥30]]>
μV2(x)=0,x≤30x-3030,30<x≤601,x>60;]]>
制定模糊推理规则表，如图6和图7所示，由航向角偏差量P和直线距离偏差量D计算输出舵角偏转控制量U和油门开度控制量V，U和V对应着无人艇运动角速度和线速度的控制量，其中图6为舵角偏转控制量U的模糊推理规则，图7为油门开度控制量V的模糊推理规则，最后控制量U和V的去模糊化具体步骤包括：
步骤6.3：引入中间变量ωi、ωk，i∈整数，k∈整数，计算公式为：
ωi=μPi(x)^μDi(x)]]>
ωk=μPk(x)^μDk(x)]]>
其中符号“∧”指取小，即取交集；
步骤6.4：根据中间变量ωi、ωk计算舵角偏转控制量U，计算公式为：
μU(x)=[ωi^μUi(x)]∪[ωk^μUk(x)]]]>
U=∫xμU(x)dx∫μU(x)dx;]]>
步骤6.5：根据中间变量ωi、ωk计算油门开度控制量V，计算公式为：
μV(x)=[ωi^μVi(x)]∪[ωk^μVk(x)]]]>
V=∫xμV(x)dx∫μV(x)dx.]]>
控制间隔自适应调整的具体步骤包括：
步骤6.6：计算单位时间航向角偏差率Pt＝P/t，单位取角度/秒(°/S)；
步骤6.7：将航向角偏差率Pt根据运行环境的不确定性和运行状态的偏差程度划分为3档。当航向角偏差率Pt＞30角度/秒时，表示大偏差状态，采用高控制频率，控制间隔为1秒；当30角度/秒＞航向角偏差率Pt＞5角度/秒时，表示一般偏差状态，采用中控制频率，控制间隔为2秒；当偏差率Pt<5角度/秒时，表示小偏差状态，采用低控制频率，控制间隔为5秒。具体参数由试验和专家经验得出。
步骤7：按一定周期时间循环操作步骤2至6。
测量结果分析
为了验证本发明提出的无人艇模糊自适应航向航速协同控制算法的有效性和鲁棒性，在实际无人艇试验平台上进行了风、浪干扰条件下，静止目标跟踪的自主航行试验，得到了如附图8所示的部分实验结果。
结果表明，所述算法能够在未进行无人艇运动建模、干扰条件复杂且未知的情况下，驱动试验艇自主跟踪目标，航迹平滑，抗干扰性强。在存在横向风浪干扰的航行过程中，能够根据实时的航向偏差和距离偏差，自主的调整试验艇航向和航速，快速准确的抵达目标点。