一种基于智能预测控制的水面无人船路径跟踪方法与流程

1.本发明涉及水面无人船路径跟踪技术领域，具体涉及一种基于智能预测控制的水面无人船路径跟踪方法。


背景技术：

2.近年来，海上搜救趋向智能化，在我国大部分近海海域、河流、湖泊、水库、运河等地区，几乎还是采用人工搜救；但是人工搜救存在一些弊端，对于一些环境比较恶劣的区域，救援人员存在人身安全问題，严重影响救援进度，水面无人船船体不大，方便的航行在各种水域，受外界约束较小，具有非常高的灵活性，对无人船的操作也非常的简单，利用水面无人船搭载救生圈和图像传输设备参与搜救，大大提高了搜救效率，在水面搜救领域发挥着极其广泛的作用；为了精确高效地完成任务，同时考虑到无人船的欠驱动性和受时变风浪流扰动对其操纵性的影响，在实际应用中，对无人船按预设期望路径跟踪的能力要求越来越高。
3.通常路径跟踪控制系统分为制导子系统和控制子系统，在制导子系统中，无人船的速度通常预定为常量，申请号为201910436861.7的专利文献公开了“一种水面无人艇路径跟踪控制方法”，文献中提出的视线制导算法将三个自由度转换为两个自由度的控制，能快速计算出期望舵角，但是，该方法不能自动调整接纳圆半径，会使无人船偏离目标路径。
4.在控制子系统中，常用控制方法有自适应控制、滑模控制、模糊控制、最优控制、预测控制等，其中，ashrafiuon h等人基于滑模控制的方法设计了一种轨迹跟踪控制器，但系统控制器受抖动的影响，很大程度上影响到系统的稳定性。 li等人考虑舵机执行器饱和约束和运动速率约束的条件下利用模型预测控制 (mpc)实现了对四自由度船舶的跟踪控制，该方法抗干扰能力好，但对参数设置、目标函数求解和模型精度要求比较高。
5.因此，一方面，海洋环境中的风浪流变化带来的未知扰动引起的不确定性使现有的一些常用的控制方法很难达到理想的路径跟踪效果；另一方面，控制器中使用传统的目标函数求解方法，精度不高，不能保证设计的路径跟随控制器的效果达到最佳。


技术实现要素：

6.本发明旨在提出一种基于智能预测控制的水面无人船路径跟踪方法，目的在于提高无人船按预设期望路径跟踪的能力，本发明提出改进的自适应视线法、人工鱼群算法和模型预测控制算法结合的方法进行路径跟踪控制；在数据融合方面，本发明提出互补滤波算法，并设置阈值增量约束对互补滤波算法进行改进，解决上电期间，数据偏差较大和无人船在高速航行时，陀螺仪数据漂移的问题；在制导方面，考虑到无人船在恶劣环境中转向时，容易受外界干扰，船身发生较大的抖动，进入接纳圆时，会发生转向提前或滞后，本发明提出自适应视线法，该算法能自动调整接纳圆半径，快速求解出期望航向角，使无人船有充分时间调整航向，平缓的完成转向动作，避免偏离目标路径；在控制方面，本发明提出了模型预测控制的方法来提高控制系统的鲁棒性和抗干扰能力，为了提高控制器的路径跟踪精
度，本发明提出改进的人工鱼群优化算法对模型预测控制器中的目标函数进行全局寻优，寻找出最优控制序列，将控制序列中的第一个控制分量作用到无人船；本发明设计的路径跟踪控制方法能使水面无人船在复杂多变的海域上开展安全、高效、精准的搜救工作。
7.本发明的目的通过以下技术方案予以实现：
8.一种基于智能预测控制的水面无人船路径跟踪的控制方法，包括以下步骤：
9.步骤1：下位机接收北斗、惯性导航传感器的数据，并进行解析，得到无人船的当前经纬度坐标、航速、艏向角等信息，设置阈值增量约束改进互补滤波算法，对艏向角进行滤波处理，将滤波后的数据将进行转化，得到北东地坐标系下的状态信息；
10.步骤2：按照无人船作业任务预先规划出航行路径点，然后连接各个航行路径点形成一条期望路径曲线作为跟踪的路径，根据无人船初始状态，计算出每个路径点的最优接纳圆半径；
11.步骤3：根据状态信息进行航向规划并利用自适应视线法解算出期望航向角；
12.步骤4：进行路径段的更新，在当前路径点时，若无人船在自适应的接纳圆内，则跟踪下一路径点；
13.步骤5：在航速给定的情况下，将当前艏向角与期望航向角的误差作为模型预测控制器的输入，设计基于模型预测控制的路径跟踪控制器，通过构建预测模型，以滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，再进行反馈校正，解算出对应推进器的转速指令来调整动作，来缩小并消除误差；
14.步骤6：本发明引入人工鱼群算法，把目标函数倒数作为人工鱼群算法的适应度函数，并对该算法进行改进，在觅食阶段，将视野范围扩大后，在新的视野范围内，再次进行搜索，寻找适应度高的人工鱼，并向该方向前进一步；
15.步骤7：通过改进的人工鱼群算法解算出最优控制增量序列中的第一个控制增量分量，将其作用到无人船，在下一时刻，根据导航传感器的实时状态，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制。
16.本发明的目的还可以通过以下技术措施来进一步实现：
17.进一步的，步骤(1)具体包括：
18.步骤(1.1)：下位机通过rs232通信协议接收北斗、惯性导航传感器的数据，并进行解析；
19.步骤(1.2)：初始化参数；
20.步骤(1.3)：获取加速度计实测值，并进行归一化处理，转换成单位向量；
21.步骤(1.4)：利用陀螺仪更新方向余弦矩阵：
22.23.式中，为加速度分量，为四元素分量；
24.步骤(1.5)：将标准重力加速度向量与加速度测量值向量进行叉乘，得到误差向量：
[0025][0026]
式中，为误差向量，为角速度分量，为加速度分量；
[0027]
步骤(1.6)：通过pid控制器对误差进行消除，得到当前时刻无人船艏向角的滤波值，控制器公式如下：
[0028][0029]
式中，e
z
(t)为艏向角误差，δe
z
(t)为艏向角补偿量，k
p
为比例系数，t
i
为积分系数，t
d
为微分系数；
[0030]
步骤(1.7)：采用一阶龙格库塔法进行四元素更新：
[0031][0032]
式中，为四元素分量，为角速度分量，t为采样时间；
[0033]
步骤(1.8)：保留上一时刻的滤波值；
[0034]
步骤(1.9)：本发明对互补滤波算法进行了改进：设置阈值增量约束将当前时刻和上一时刻滤波差值与阈值增量约束进行比较，若满足则输出结果，否则重新执行，判定公式如下：
[0035]
|x
now
‑
x
last
|≤β
ꢀꢀ
(5)
[0036]
式中，x
now
为当前滤波值，x
last
为上一时刻滤波值，β为阈值增量约束,选取范围为[0,1]；
[0037]
步骤(1.10)：将机体坐标系下的当前状态信息进行转化处理，得到北东地坐标系下的状态信息。
[0038]
进一步的，步骤(2)具体包括：
[0039]
步骤(2.1)：将路径离散为按顺序散布在路径上的路径点，提取每个直线路径单元上的实时参考路径：
[0040][0041][0042]
式中，(x
r
,y
r
)是实时参考路径点坐标，(x
n
,y
n
)为预定的第n个航迹点坐标， (x,y)为无人船实时位置，θ
n
为连接航迹点(x
n
,y
n
)与(x
n 1
,y
n 1
)的路径方位角，为自适应接纳圆半径；
[0043]
步骤(2.2)：根据无人船初始状态，计算出每个路径点的最优接纳圆半径，以下是自适应接纳圆半径的选取方法：
[0044][0045]
式中，l为船舶的长度，η为待定系数，选取范围为[1,5]，为自适应接纳圆的半径，r
min
为待定系数，选取船舶长度的1～2倍，r
max
为待定系数，选取船舶长度的3～5倍，θ为相邻路径夹角，γ为缓冲系数，选取范围为[
‑
1,1]。
[0046]
进一步的，步骤(3)具体包括：
[0047]
利用自适应视线法，以当前位置为中心，选取无人船船长的3～5倍为半径，确定视距点p
los
，解算出期望航向角，公式如下：
[0048][0049]
式中，(x
los
,y
los
)为视距位置，(x,y)为当前位置，ψ
los
为期望航向角。
[0050]
进一步的，步骤(4)具体包括：
[0051]
在选择路径p
n 1
时，判断无人船是否在自适应接纳圆内，如果满足公式，将选择跟踪下一路径p
n 1
，公式如下：
[0052][0053]
式中：为自适应接纳圆的半径，(x
n
,y
n
)为预定的第n个航迹点坐标，(x,y) 为当前位置。
[0054]
进一步的，步骤(5)具体包括：
[0055]
步骤(5.1)：建立3自由度的非线性欠驱动无人船操纵模型：
[0056][0057][0058][0059][0060][0061][0062]
式中，x,y,ψ是船舶在北东坐标下的位置，u,v,r是船体坐标系下的速度，τ
u
是推进器的力矩，τ
δ
是舵机或双推进器差速下的偏航力矩，m是船舶的质量，izz 是绕o
z
轴的转动惯量，是船舶在纵荡、横荡、艏摇3个方向上加速度引起的附加质量，定义为负数，x
u
,y
v
,n
r
分别为船舶在各个方向上的水动力阻尼系数；
[0063]
步骤(5.2)：对欠驱动无人船操纵模型在期望路径点(x
r
,u
r
)处进行一阶泰勒展开线性化处理，得到如下的预测模型：
[0064][0065]
式中，a、b为雅可比矩阵；
[0066]
步骤(5.3)：对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：
[0067][0068][0069]
式中，
[0070]
步骤(5.4)：进行约束条件设置，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：
[0071]
δu
min
≤δu(k i)≤δu
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0072]
u
min
≤u(k i)≤u
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0073]
y
min
≤y(k i)≤y
max
,i＝0,1,2,...,n
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0074]
式中，n
c
是控制时域，n
p
是预测时域，δu(k i)为k i时刻的控制增量， u(k i)为k i时刻的控制量，y(k i)为k i时刻的输出量，δu min
是控制增量最小值，δu
max
是控制增量最大值，u
min
是控制量最小值，u
max
是控制量最大值，根据推进器机械性能选取。y
min
是输出量
最小值，y
max
是输出量最大值；
[0075]
步骤(5.5)：滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，构建关于系统的状态量偏差和控制增量的目标函数，通过改进的人工鱼群算法优化最优控制序列，输出第一个控制分量作用到无人船上，目标函数如下：
[0076]
j＝(y
‑
y
ref
)
t
q(y
‑
y
ref
) δu
t
rδu
ꢀꢀ
(23)
[0077]
式中，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；
[0078]
步骤(5.6)：等待无人船接收下一时刻的定位、姿态数据作为反馈信息。
[0079]
进一步的，步骤(6)具体包括：
[0080]
步骤(6.1)：选取目标函数的倒数作为适应度函数，对鱼群进行初始化，设置最大尝试次数为20，初始视野感知距离为1，拥挤因子δ为0.6，初始移动步长为0.1，最大迭代次数为50，适应度函数如下：
[0081][0082]
式中，h(δu)为适应度值，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；
[0083]
步骤(6.2)：计算初始鱼群个体的适应值，取最优人工鱼状态及其值赋予给公告牌；
[0084]
步骤(6.3)：进行觅食行为，本发明对人工鱼群算法进行了改进，在觅食阶段，当人工鱼用完尝试次数时，将视野范围扩大，增加寻优的机会，改进后的具体觅食方法如下：将视野扩大前的人工鱼适应度值与其视野感知范围内随机一条人工鱼的适应度值进行比较，如果比当前人工鱼的适应度高，则向适应度高的人工鱼方向移动一步，否则，随机选择另一条人工鱼进行比较，反复尝试20次后，如果仍不满足前进条件，将视野范围扩大λ倍，新的视野公式如下：
[0085]
visual
new
＝(1 λ)visual
ꢀꢀ
(25)
[0086]
式中：visual
new
是扩大以后的视野，visual是扩大前的视野，λ为待定系数，一般取50％～80％，
[0087]
在新的视野范围内，再次进行搜索，尝试次数用完后，如果搜索到适应度更高的位置，则向该方向移动一步，若没有搜索到适应度更高的位置，则执行随机移动一步；
[0088]
步骤(6.4)：进行聚群行为，在探索当前人工鱼视野范围内，满足下面公式时，表明中心不太拥挤，朝着中心位置前进一步，否则，进行觅食行为，公式如下：
[0089]
y
c
/n
f
＞δy
i1
ꢀꢀ
(26)
[0090]
式中，y
c
是中心位置人工鱼的适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i1
是人工鱼的当前状态适应度值；
[0091]
步骤(6.5)：进行追尾行为，在探索当前人工鱼视野范围内，满足下面公式时，表明视野范围内随机选择的人工鱼周围不太拥挤，朝着该方向前进一步，否则，进行觅食行为，公式如下：
[0092]
y
j2
/n
f
＞δy
i2
ꢀꢀ
(27)
[0093]
式中，y
j2
是视野范围内随机选择的人工鱼适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i2
是人工鱼的当前状态适应度值；
[0094]
步骤(6.6)：进行随机行为，在新的视野范围中随机选择一个状态，然后向该方向移动，公式如下：
[0095]
x
i|next
＝x
i3
round
·
visual
new
ꢀꢀ
(28)
[0096]
式中：x
i|next
为x
i3
的下一个状态，x
i3
为当前人工鱼的状态，visual
new
是扩大以后的视野，round是[
‑
1,1]区间的随机数；
[0097]
步骤(6.7)：对所有人工鱼个体进行评价，若某个体优于公告牌，则将公告牌更新为该个体；
[0098]
步骤(6.8)：当公告牌上最优解达到满意误差界内或者达到迭代次数上限时算法结果时，输出最优控制增量序列：
[0099]
δu
*
＝[δu(t|t)
* δu(t 1|t)
* ··· δu(t n
c
‑
1|t)
*
]
ꢀꢀ
(29)
[0100]
式中，δu(t i|t)
*
为t i时刻的最优控制增量分量，i＝1,2,
···
,n
c
1，δu
*
为最优控制增量序列。
[0101]
进一步的，步骤(7)具体包括：
[0102]
将控制增量序列中的第一个控制增量分量作用无人船，在下一时刻，对无人船的状态信息进行采集，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制，如果到达最后一个期望路径点，则终止循环。
[0103]
与现有技术相比，本发明的有益效果是：
[0104]
1、本发明提出改进的自适应视线法、人工鱼群算法和模型预测控制相结合的方法(adaptive los
‑
afsa
‑
mpc)进行路径跟踪，该方法能使系统快速响应并收敛，提高了控制器的动态特性，具有良好的跟踪效果。
[0105]
2、针对姿态解算数据不稳定和精度不高的问题，本发明提出互补滤波算法，该方法简单，不需要建立模型，能消除高频噪声和低频误差的累积，滤波效果好，提高传感器的测量精度。
[0106]
3、本发明提出数据融合算法，设置阈值增量约束，对互补滤波算法进行改进，解决了上电期间，数据偏差较大和无人船在高速航行时，陀螺仪数据漂移的问题，改进后的方法能避免艏向角出现较大的波动，使数据融合后得到的曲线更加平滑。
[0107]
4、针对无人船在恶劣环境中转向时，容易受外界干扰，船身发生较大的抖动，进入接纳圆时，会出现转向提前或滞后的问题，本发明提出自适应视线算法，该算法能自动调整接纳圆半径，使无人船有充分时间调整航向，平缓的完成转向动作，避免偏离目标路径，提高制导精度。
[0108]
5、本发明提出模型预测控制方法设计跟踪艏向角的控制器，该方法采用滚动优化的策略，有较好的动态控制性能，且该方法设计的闭环控制系统有很强的抗扰动能力。
[0109]
6、在设计基于模型预测控制的路径跟踪控制器时，考虑到用传统的目标函数求解方法，会带来精度不高的问题，导致控制器路径跟踪效果不佳，本发明提出改进的人工鱼群算法对目标函数进行全局寻优，该方法对参数设定要求不高，能快速得到可行解，能有效提高控制精度。
[0110]
7、在人工鱼群算法的各个行为中都涉及到了视野，当视野范围越大，人工鱼越容易发现全局极值并收敛，本发明对人工鱼群算法进行了改进，在觅食阶段，当人工鱼用完尝试次数时，将视野范围扩大，增加寻优的机会，提高整个算法的收敛速度。
附图说明
[0111]
图1是无人船的控制系统结构框图；
[0112]
图2是数据融合算法流程图；
[0113]
图3是坐标系转换图；
[0114]
图4是自适应视线法原理图；
[0115]
图5是模型预测控制算法流程图；
[0116]
图6是人工鱼视觉描述图；
[0117]
图7是人工鱼群算法最优化求解算法流程图。
具体实施方式
[0118]
下面结合附图对本发明的具体实施方式进行说明。
[0119]
如图1所示,是无人船的控制系统结构框图，在无人船路径跟踪过程中，首先，通过惯性导航传感器测得姿态数据，提出数据融合算法，设置阈值增量约束，改进互补滤波算法，对艏向角进行滤波处理；然后，根据期望路径点的位置和由北斗测得的无人船实时位置，提出自适应视线法，计算出最优接纳圆半径，解算出无人船的期望航向角；接着，在速度给定的条件下，将滤波反馈值与期望航向角的误差作为模型预测控制器的输入；最后，提出改进的人工鱼群算法对模型预测控制器中的目标函数进行全局寻优，解算出的第一个控制分量作为控制器的输出，从而实现对无人船的路径跟踪控制。
[0120]
步骤1：如图2所示，是数据融合算法流程图，首先，下位机通过rs232通信协议接收北斗、惯性导航传感器的数据，进行解析，得到无人船的当前经纬度坐标、航速、艏向角等信息，接着，对艏向角进行数据融合处理，最后，进行转化得到北东地坐标系下的状态信息，如图3所示，坐标系转换图，对艏向角的融合滤波算法具体包括以下步骤：
[0121]
步骤1.1：下位机通过rs232通信协议接收北斗、惯性导航传感器的数据，并进行解析；
[0122]
步骤1.2：初始化姿态参数；
[0123]
步骤1.3：获取加速度计实测值，并进行归一化处理，转换成单位向量；
[0124]
步骤1.4：利用陀螺仪更新方向余弦矩阵得到标准重力加速度在机体坐标中的表示：
[0125]
[0126]
式中，为加速度分量，为四元素分量；
[0127]
步骤1.5：将机体坐标系中的标准重力加速度向量与处理后的加速度测量值向量进行叉乘，得到误差向量：
[0128][0129]
式中，为误差分量，为角速度分量，为加速度分量；
[0130]
步骤1.6：通过pid控制器对误差进行消除，得到当前时刻无人船艏向角的滤波值，控制器公式如下：
[0131][0132]
式中，e
z
(t)为艏向角误差，δe
z
(t)为艏向角补偿量，k
p
为比例系数，t
i
为积分系数，t
d
为微分系数；
[0133]
步骤1.7：采用一阶龙格库塔法进行四元素更新：
[0134][0135]
式中，为四元素分量，为角速度分量，t为采样时间；
[0136]
步骤1.8：保留上一时刻的滤波值；
[0137]
步骤1.9：针对初始上电期间，数据偏差较大和无人船高速航行时，陀螺仪数据漂移的问题，本发明对互补滤波算法进行改进，设置阈值增量约束，将当前时刻和上一时刻滤波差值与阈值增量约束进行比较，若满足则输出结果，否则重新执行，该方法能避免艏向角出现较大的波动，使数据融合后的曲线更加平滑，提高导航传感器的测量精度，判定公式如下：
[0138]
|x
now
‑
x
last
|≤β
ꢀꢀ
(5)
[0139]
式中，x
now
为当前滤波值，x
last
为上一时刻滤波值，β为阈值增量约束,选取范围为[0,1]；
[0140]
步骤1.10：将机体坐标系下的当前状态信息进行转化处理，得到北东地坐标系下的状态信息。
[0141]
步骤2：如图4所示，是自适应视线法原理图，根据无人船作业任务预先规划出航行路径点，然后连接各个航行路径点形成一条期望路径曲线作为跟踪的路径，考虑到无人船在恶劣环境中转向时，容易受外界干扰，船身发生较大的抖动，进入接纳圆时，接纳圆半径过小，会发生转向滞后；接纳圆半径过大，会过早跟踪下一路径，本发明提出自适应视线法，根据无人船初始状态，计算出每个路径点的自动接纳圆半径，该方法能使无人船有充分时间调整航向，平缓的完成转向动作，避免偏离目标路径，自适应调整接纳圆半径具体包括以下步骤：
[0142]
步骤2.1：将路径离散为按顺序散布在路径上的路径点，提取每个直线路径单元上的实时参考路径：
[0143][0144][0145]
式中，(x
r
,y
r
)是实时参考路径点坐标，(x
n
,y
n
)为预定的第n个航迹点坐标， (x,y)为无人船实时位置，θ
n
为连接航迹点(x
n
,y
n
)与(x
n 1
,y
n 1
)的路径方位角，为自适应接纳圆的半径；
[0146]
步骤2.2：根据无人船初始状态，计算出每个路径点的最优接纳圆半径，以下是自适应接纳圆半径的选取方法：
[0147][0148]
式中，l为船舶的长度，η为待定系数，选取范围为[1,5]，为自适应接纳圆的半径，r
min
为待定系数，选取船舶长度的1～2倍，r
max
为待定系数，选取船舶长度的3～5倍，θ为相邻路径夹角，γ为缓冲系数，选取范围为[
‑
1,1]。
[0149]
步骤3：利用自适应视线法，以当前位置为中心，选取无人船船长的3～5倍为半径，确定视距点p
los
，解算出期望航向角，公式如下：
[0150][0151]
式中，(x
los
,y
los
)为视距位置，(x,y)为当前位置，ψ
los
为期望航向角。
[0152]
步骤4：路径段更新，在选择路径p
n 1
时，判断无人船是否在自适应接纳圆内，如果满足公式，将选择跟踪下一路径p
n 1
。公式如下：
[0153][0154]
式中：为自适应接纳圆的半径，(x
n
,y
n
)为预定的第n个航迹点坐标，(x,y) 为当前位置。
[0155]
步骤5：如图5所示，是模型预测控制算法流程图，在航速给定的情况下，将当前艏向角与期望航向角的误差作为模型预测控制器的输入，设计基于模型预测控制的路径跟踪控制器，通过构建预测模型，以滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，再进行反馈校正，解算出对应推进器的转速指令来调整动作，来缩小并消除误差，提出的模型预测控制方法包括以下步骤：
[0156]
步骤5.1：本发明考虑坐标原点在重心处，忽略无人船的横摇、纵摇以及升降运动带来的影响，忽略在运动过程中的水流附加质量与水动力带来的附加科里奥利向心力，忽略非线性阻尼的影响，得到3自由度的非线性欠驱动无人船操纵模型如下：
[0157][0158][0159][0160][0161][0162][0163]
式中，x,y,ψ是船舶在北东坐标下的位置，u,v,r是船体坐标系下的速度，τ
u
是推进器的力矩，τ
δ
是舵机或双推进器差速下的偏航力矩，m是船舶的质量，izz 是绕o
z
轴的转动惯量，是船舶在纵荡、横荡、艏摇3个方向上加速度引起的附加质量，定义为负数，x
u
,y
v
,n
r
分别为船舶在各个方向上的水动力阻尼系数；
[0164]
步骤5.2：对欠驱动无人船操纵模型在期望路径点(x
r
,u
r
)处进行一阶泰勒展开线性化处理，得到如下的预测模型：
[0165][0166]
式中，a、b为雅可比矩阵；
[0167]
步骤5.3：为了得到离散化状态方程，本发明采用前向欧拉的方法对该模型进行离散化处理，并化简为用控制增量形式表示的状态空间模型：
[0168]
[0169][0170]
式中，
[0171]
步骤5.4：进行约束条件设置，一方面，无人船的推进器受机械性能的影响，转速过小或过大都可能导致推进器无法正常工作；另一方面，为了减少能量的消耗和确保状态量偏差不宜过大；在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：
[0172]
δu
min
≤δu(k i)≤δu
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0173]
u
min
≤u(k i)≤u
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0174]
y
min
≤y(k i)≤y
max
,i＝0,1,2,...,n
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0175]
式中，n
c
是控制时域，n
p
是预测时域，δu(k i)为k i时刻的控制增量， u(k i)为k i时刻的控制量，y(k i)为k i时刻的输出量，δu
min
是控制增量最小值，δu
max
是控制增量最大值，u
min
是控制量最小值，u
max
是控制量最大值，根据推进器机械性能选取。y
min
是输出量最小值，y
max
是输出量最大值；
[0176]
步骤5.5：滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，构建关于系统的状态量偏差和控制增量的目标函数，通过改进的人工鱼群算法优化最优控制序列，输出第一个控制分量作用到无人船上，目标函数如下：
[0177]
j＝(y
‑
y
ref
)
t
q(y
‑
y
ref
) δu
t
rδu
ꢀꢀ
(23)
[0178]
式中，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；
[0179]
步骤5.6：等待无人船接收下一时刻的定位、姿态数据作为反馈信息。
[0180]
步骤6：考虑到人工调整预测和控制时域可能带来路径跟踪效果不佳的问题，本发明引入人工鱼群算法，人工鱼群算法模拟鱼群的觅食行为而实现全局寻优，如图6所示，是人工鱼视觉描述图，将目标函数倒数作为人工鱼群算法的适应度函数，进行全局寻优求解，本发明对该算法进行改进，在觅食阶段，将视野范围扩大后，在新的视野范围内，再次进行搜索，寻找适应度高的人工鱼，并向该方向前进一步，改进后的方法能提高整个算法的收敛速度，如图7所示，是人工鱼群算法最优化求解算法流程图；
[0181]
具体改进的人工鱼群算法包括以下步骤：
[0182]
步骤6.1：选取目标函数的倒数作为适应度函数，对鱼群进行初始化。设置最大尝试次数为20，初始视野感知距离为1，拥挤因子δ为0.6，初始移动步长为0.1，最大迭代次数为50。适应度函数如下：
[0183][0184]
式中，h(δu)为适应度值，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；
[0185]
步骤6.2：计算初始鱼群各个个体的适应度值，取最优人工鱼的状态及其适应度值赋予给公告牌；
[0186]
步骤6.3：进行觅食行为，将视野扩大前人工鱼的适应度值与其视野感知范围内随
机一条人工鱼的适应度值进行比较，如果比当前人工鱼的适应度高，则向适应度高的人工鱼方向移动一步，否则，随机选择另一条人工鱼进行比较，反复尝试20次后，如果仍不满足前进条件，将视野范围扩大λ倍，新的视野公式如下：
[0187]
visual
new
＝(1 λ)visual
ꢀꢀ
(25)
[0188]
式中：visual
new
是扩大以后的视野，visual是扩大前的视野，λ为待定系数，一般取50％～80％；
[0189]
在新的视野范围内，再次进行搜索，尝试次数用完后，如果搜索到适应度更高的位置，则向该方向移动一步；若没有搜索到适应度更高的位置，则执行随机移动一步；
[0190]
步骤6.4：进行聚群行为，在探索当前人工鱼视野范围内，满足下面公式时，表明中心不太拥挤，朝着中心位置前进一步，否则，进行觅食行为，公式如下：
[0191]
y
c
/n
f
＞δy
i1
ꢀꢀ
(26)
[0192]
式中，y
c
是中心位置人工鱼的适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i1
是人工鱼的当前状态适应度值；
[0193]
步骤6.5：进行追尾行为，在探索当前人工鱼视野范围内，满足下面公式时，表明视野范围内随机选择的人工鱼周围不太拥挤，朝着该方向前进一步，否则，进行觅食行为，公式如下：
[0194]
y
j2
/n
f
＞δy
i2
ꢀꢀ
(27)
[0195]
式中，y
j2
是视野范围内随机选择的人工鱼适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i2
是人工鱼的当前状态适应度值；
[0196]
步骤6.6：进行随机行为，在新的视野范围中随机选择一个状态，然后向该方向移动，公式如下：
[0197]
x
i|next
＝x
i3
round
·
visual
new
ꢀꢀ
(28)
[0198]
式中：x
i|next
为x
i3
的下一个状态，x
i3
为当前人工鱼的状态，visual
new
是扩大以后的视野，round是[
‑
1,1]区间的随机数；
[0199]
步骤6.7：对所有人工鱼个体进行评价，若某个体优于公告牌，则将公告牌更新为该个体；
[0200]
步骤6.8：当公告牌上最优解达到满意误差界内或者达到迭代次数上限时算法结果时，输出最优控制增量序列：
[0201]
δu
*
＝[δu(t|t)
* δu(t 1|t)
* ··· δu(t n
c
‑
1|t)
*
]
ꢀꢀ
(29)
[0202]
式中，δu(t i|t)
*
为t i时刻的最优控制增量分量，i＝1,2,
···
,n
c
1，δu
*
为最优控制增量序列。
[0203]
步骤7：解算出最优控制增量序列中第一个控制增量分量对应的推进器转速值，将其作用到无人船的推进系统，进行航向调整，在下一时刻，通过北斗和惯性导航传感器接收无人船的状态信息，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制，使得控制系统具有很强的抗扰动和克服系统不确定性的能力，如果到达最后一个期望路径点，则终止循环。
[0204]
综上所述，本发明所提到的方法能有效提高导航精度和控制器的抗干扰能力，具有较好的路径跟踪效果和现实意义。
[0205]
除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。