一种多无人船编队安全优化控制系统的制作方法

1.本发明涉及无人船协同控制领域，尤其涉及一种多无人船编队安全优化控制系统。


背景技术：

2.无人船是船舶智能化发展的产物，作为一种现代工具已广泛应用于情报收集、监视侦察、搜寻救助、水文地理勘察、中继通信等任务中。为提高作业效率，形成规模化效应，多无人船编队协同作业得到广泛研究。多无人船编队协同关键技术领域中，编队安全控制技术是重要组成部分，探索多无人船编队安全控制技术具有重要意义。
3.目前无人船编队运动控制方法存在以下不足：
4.一、现有的无人船编队控制方法大多只关注编队控制的鲁棒性，未系统的考虑复杂海洋环境中的动态与静态障碍物，以及编队中无人船之间的碰撞问题。若忽略实际海洋环境航行过程中的障碍及碰撞规避问题，必然会引起编队安全问题，导致编队任务失败。
5.二、现有的无人船编队防碰撞控制方法大多只关注编队控制的避碰功能，未系统全面的考虑无人船实际状态约束、执行机构约束及能量优化、控制输入的平滑性。此外现有的无人船编队控制方法大多依赖固定精确的无人船数学模型进行控制器的设计，未考虑实际海洋环境下外部干扰及模型不确定性。若忽略这些因素进行控制方法的设计，必然会降低控制方法的最优性和工程适用性。


技术实现要素：

6.本发明提供一种多无人船编队安全优化控制系统，以克服现有的无人船控制系统未考虑复杂海洋环境中的动态与静态障碍物、无人船实际状态约束、执行机构约束及能量优化、控制输入的平滑性等技术问题。
7.为了实现上述目的，本发明的技术方案是：
8.一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；
9.所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τi、受控无人船输出地球坐标系下的速度信息qi、无人船艏向角速度信息ri和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
；其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τ
iu
和艏向角速度τ
ir
；
10.所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储
到数据集s
iq
和s
ir
中；
11.所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集s
iq
和s
ir
、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri，并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息
12.所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块；
13.所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块；
14.所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算安全纵向速度控制输入τ
iu
和参考艏向角并将安全纵向速度控制输入τ
iu
输出至所述受控无人船，将参考艏向角输入至所述艏向角跟踪预测控制模块；
15.所述艏向角跟踪预测控制模块根据所述参考艏向角无人船艏向角速度信息ri以及艏向角方向待求解估计信息得出艏向角速度控制输入值τ
ir
，并将艏向角速度控制输入值τ
ir
输入至所述受控无人船。
16.进一步的，所述受控无人船的运动学与动力学模型具体为：
[0017][0018]
其中，为无人船的位置姿态信息，xi、yi、为无人船在地球坐标系下x轴方向的位置信息、y轴方向的位置信息以及艏向角信息；为ηi的导数信息；ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；和为ui、vi和ri的导数；f
iu
、f
iv
和f
ir
为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解； m
iu
和m
ir
分别为船体纵向和艏摇方向的惯量系数；t为时间。
[0019]
进一步的，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：
[0020][0021][0022]
其中：pi＝[xi,yi]、qi＝[q
ix
,q
iy
]为受控无人船在地球坐标系下的位置信息和速度信息，为受控无人船在地球坐标系下x轴和y轴方向的速度信息；分别为pi、qi导数；f
iq
＝[f
ix
,f
iy
]为无人船在地球坐标系下的位置运动模型的待求解，f
ix
、f
iy
为无人船在地球坐标系下x轴和y轴方向的位置运动模型的待求解，其具体转化方式如下：
[0023][0024]
进一步的，计算地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
具体公式为：
[0025][0026]
其中：g
iq
＝[g
ix
,g
iy
]，g
ix
为x轴方向位置控制输入滤波信息，g
iy
为y轴方向位置控制输入滤波信息，n
iq
＝[n
ix
,n
iy
]，n
ix
为x轴方向位置回归向量滤波信息，n
iy
为y轴方向位置回归向量滤波信息，τ
iq
＝[τ
ix
,τ
iy
]
t
为无人船位置控制输入，式中为无人船在地球坐标系下x轴方向的控制输入，为无人船在地球坐标系下y轴方向的控制输入，k
qf
和k
rf
均为滤波系数，ψ
iq
＝[ψ
ix
,ψ
iy
]为位置回归向量，ψ
ix
为x轴方向的回归向量，ψ
iy
为y轴方向的回归向量，ψ
ir
为艏向角速度方向的回归向量；
[0027]
计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
具体公式为：
[0028][0029]
其中：q
if
＝[q
ixf
,q
iyf
]地球坐标系下的速度滤波信息，q
ixf
为x轴方向速度滤波信息，q
iyf
为y轴方向速度滤波信息，为受控无人船在地球坐标系下的速度信息滤波变量，为的导数，为艏向角速度信息滤波变量，为的导数，qi(0)为受控无人船在地球坐标系下的速度初始值，ri(0)为艏向角速度初始值；
[0030]
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储到数据集s
iq
和s
ir
中具体公式为：
[0031][0032]
其中：分别为tk时刻的x轴方向速度滤波信息，tk时刻的y轴方向速度滤波信息，tk时刻的x轴方向位置控制输入滤波信息，tk时刻的y轴方向位置控制输入滤波信息，tk时刻的x轴方向位置回归向量滤波信息，tk时刻的y轴方向位置回归向量滤波信息，tk时刻的艏向角速度滤波信息，tk时刻的艏向角速度控制输入滤波信息，tk时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。
[0033]
进一步的，学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息过程为：
[0034]
c1、根据式(8)更新权值
[0035][0036]
其中：γ
i1
,γ
i2
,γ
i3
,γ
i4
分别为增益系数，为位置权值w
iq
的估计值，为x轴方向位置权值估计值，为y轴方向位置权值估计值，w
ir
的估计值为艏向角权值w
ir
的估计值，为受控无人船在地球坐标系下的速度信息qi的估计值，为艏向角速度信息ri的估计值；为艏向角速度估计误差，为地球坐标系下的速度信息估计误差，ξ
iq
＝diag{ξ
ix
,ξ
iy
}，ξ
ix
,ξ
iy
,ξ
ir
根据式(9)得到
[0037][0038]
其中从上述数据集s
iq
和s
ir
中获取；
[0039]
c2、根据式(10)计算
[0040]
[0041]
其中为位置运动模型的待求解估计信息，为艏向角方向待求解估计信息。
[0042]
进一步的，计算地球坐标系下标称控制输入和参考艏向角过程为：
[0043]
d1、将位置环模型(2)改写如下：
[0044][0045]
其中：为无人船在地球坐标系下标称控制输入，为无人船在地球坐标系下x轴方向的标称控制输入，为无人船在地球坐标系下y轴方向的标称控制输入；
[0046]
d2、将无人船在地球坐标系下的位置运动模型的待求解估计信息代入模型(11)并进行离散化，具体公式如下：
[0047][0048]
其中：z
iq
(tk)＝[pi(tk),qi(tk)]
t
表示tk时刻的无人船状态向量；ts为采样间隔时间；向量向量向量向量02＝[0,0]
t
；
[0049]
d3、利用公式(12)在tk时刻进行状态预测如下：
[0050][0051]
其中：z
iq
(tk|tk)为tk时刻状态采样值；z
iq
(tk lts|tk),l＝1,...,n
p
为tk时刻对tk lts 时刻的状态预测，n
p
为预测时域；为t
k-ts时刻施加到受控无人船的地球坐标系下的标称控制输入；为tk mts时刻的标称控制增量，nc表示控制时域；
[0052]
d4、利用递归关系将公式(8)表示如下：
[0053][0054]
其中为tk时刻的预测状态输出序列；为tk时刻控制增量序列；
[0055][0056][0057]
相应的令将式(14)写成如下形式：
[0058][0059]
d5、构造如下优化问题模型：
[0060][0061]
式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界；q1、q2和q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；
[0062]
d6、将优化问题模型(15)转化成如下形式：
[0063][0064]
其中：a＝[i2,-i2]
t
，
[0065]
通过求解优化问题模型(16)，得到最优标称控制输入增量序列从而得到地球坐标系下标称控制输入序列即地球坐标系下标称控制输入信息
[0066]
进一步的，计算安全纵向速度控制输入τ
iu
和参考艏向角过程为：
[0067]
e1、构造如(17a),(17b)和(17c)所示安全约束
[0068][0069][0070][0071]
其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障
碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，p
ij
为无人船与无人船之间的相对距离，p
ic
为无人船与动态障碍物之间的相对距离，p
io
为无人船与静态障碍物之间的相对距离，为地球坐标系下安全控制输入信息，e
ij
,e
ic
,e
io
如式(18)所示
[0072][0073]
其中：q
ij
为无人船与无人船之间的相对速度，q
ic
为无人船与动态障碍物之间的相对速度，ds为避碰安全距离，ro为静态障碍物半径距离，为无人船最大加速度；
[0074]
e2、构造如(19)所示优化问题
[0075][0076]
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息
[0077]
e3、根据式(20)计算安全纵向速度控制输入τ
iu
和参考艏向角
[0078][0079]
其中和为中第一个和第二个元素。
[0080]
进一步的，得出艏向角速度控制输入值τ
ir
过程为：
[0081]
f1、将模型(3)进行离散化如下：
[0082]zir
(tk ts)＝a
irzir
(tk) b
ir
τ
ir
(tk) c
ir
ꢀꢀ
(21)
[0083]
其中：表示tk时刻的无人船艏向角状态向量；ts为采样间隔时间；向量向量向量
[0084]
f2、利用式(21)进行预测如下：
[0085][0086]
其中：z
ir
(tk|tk)为tk时刻状态采样值；z
ir
(tk lts|tk),l＝1,...,n
p
为tk时刻对tk lts 时刻的艏向角状态预测，n
rp
为艏向角预测时域；τ
ir
(t
k-ts)为上一采样时刻施加的艏向角速度控制输入；δτ
ir
(tk mts|tk),m＝1,...,nc为tk mts时刻的艏向角速度控制增量，n
rc
表示艏向角控制时域；
[0087]
f3、利用递归关系将式(22)表示如下：
[0088][0089]
其中：为tk时刻的艏向角预测状态输出序列；为tk时刻的艏向角速度控制增量序列；
[0090]090][0091]
相应的令将式(23)写成如下形式：
[0092][0093]
f4、构造如下优化问题模型：
[0094][0095]
式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；与分别为艏向角速度控制增量的上下界；与分别为艏向角速度控制输入的上下界；为参考艏向角序列；
[0096]
f5、将优化问题模型(25)转化成如下形式：
[0097][0098]
其中：
[0099]ar
＝[i
r2
,-i
r2
]
t
[0100][0101]
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列得到最优艏向角速度控制输入序列将序列中第一个元素施加到受控无人船。有益效果：本发明，解决了的问题。
[0102]
有益效果
[0103]
一、本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息，并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息；因此不需要固定精确的无人船数学模型参数信息，仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息，从而在复杂海洋环境下，可以提高无人船的抗扰动性与控制精确性。
[0104]
二、本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避，以及无人船编队中无人船与无人船之间的碰撞规避问题，分别构造碰撞规避安全约束，并考虑无人船实际执行机构约束，设计避碰避障控制优化问题并进行滚动优化求解，提高了无人船编队的安全性。
附图说明
[0105]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0106]
图1为本发明公开的多无人船编队防碰撞抗干扰控制系统结构示意图；
[0107]
图2为本发明中一种交互信息网络拓扑示意图；
[0108]
图3为无人船编队无风险编队轨迹示意图；
[0109]
图4a为第150秒无人船编队轨迹图；
[0110]
图4b为第300秒无人船编队轨迹图；
[0111]
图4c为第355秒无人船编队轨迹图；
[0112]
图4d为第600秒无人船编队轨迹图；
[0113]
图5a为5艘船的纵向速度控制输入示意图；
[0114]
图5b为艏向角速度控制输入示意图；
[0115]
图6为无人船与无人船之间距离示意图；
[0116]
图7为5艘无人船与4个动态障碍物a、b、c、d的距离示意图；
[0117]
图8为5艘无人船与3个静态障碍物1、2、3的距离示意图；
[0118]
图9为第i艘无人船位置运动模型和艏向角方向的待求解估计示意图。
具体实施方式
[0119]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0120]
本实施例提供了一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，如图1，包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；
[0121]
所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τi、受控无人船输出地球坐标系下的速度信息qi、无人船艏向角速度信息ri和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
；其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τ
iu
和艏向角速度τ
ir
；
[0122]
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储到数据集s
iq
和s
ir
中；
[0123]
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集s
iq
和s
ir
、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri，并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息
[0124]
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块；
[0125]
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块；
[0126]
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算安全
纵向速度控制输入τ
iu
和参考艏向角并将安全纵向速度控制输入τ
iu
输出至所述受控无人船，将参考艏向角输入至所述艏向角跟踪预测控制模块；
[0127]
所述艏向角跟踪预测控制模块根据所述参考艏向角无人船艏向角速度信息ri以及艏向角方向待求解估计信息得出艏向角速度控制输入值τ
ir
，并将艏向角速度控制输入值τ
ir
输入至所述受控无人船。
[0128]
本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息，并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息；因此不需要固定精确的无人船数学模型参数信息，仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息，从而在复杂海洋环境下，可以提高无人船的抗扰动性与控制精确性。
[0129]
本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避，以及无人船编队中无人船与无人船之间的碰撞规避问题，分别构造碰撞规避安全约束，并考虑无人船实际执行机构约束，设计避碰避障控制优化问题并进行滚动优化求解，提高了无人船编队的安全性。
[0130]
在具体实施例中，所述受控无人船的运动学与动力学模型具体为：
[0131][0132]
其中，为无人船的位置姿态信息，xi、yi、为无人船在地球坐标系下x轴方向的位置信息、y轴方向的位置信息以及艏向角信息；为ηi的导数信息；ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；和为ui、vi和ri的导数；f
iu
、f
iv
和f
ir
为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解； m
iu
和m
ir
分别为船体纵向和艏摇方向的惯量系数；t为时间。
[0133]
在具体实施例中，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：
[0134][0135][0136]
其中：pi＝[xi,yi]、qi＝[q
ix
,q
iy
]为受控无人船在地球坐标系下的位置信息和速度信息，为受控无人船在地球坐标系下x轴和y轴方向的速度信息；分别为pi、qi导数；f
iq
＝[f
ix
,f
iy
]为无人船在地球坐标系下的位置运动模型的待求解，f
ix
、f
iy
为无人船在地球坐标系下x轴和y轴方向的位置运动模型的待求
解，其具体转化方式如下：
[0137][0138]
在具体实施例中，计算地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
具体公式为：
[0139][0140]
其中：g
iq
＝[g
ix
,g
iy
]，g
ix
为x轴方向位置控制输入滤波信息，g
iy
为y轴方向位置控制输入滤波信息，n
iq
＝[n
ix
,n
iy
]，n
ix
为x轴方向位置回归向量滤波信息，n
iy
为y轴方向位置回归向量滤波信息，τ
iq
＝[τ
ix
,τ
iy
]
t
为无人船位置控制输入，式中为无人船在地球坐标系下x轴方向的控制输入，为无人船在地球坐标系下y轴方向的控制输入，k
qf
和k
rf
均为滤波系数，ψ
iq
＝[ψ
ix
,ψ
iy
]为位置回归向量，ψ
ix
为x轴方向的回归向量，ψ
iy
为y轴方向的回归向量，ψ
ir
为艏向角速度方向的回归向量；
[0141]
计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
具体公式为：
[0142][0143]
其中：q
if
＝[q
ixf
,q
iyf
]地球坐标系下的速度滤波信息，q
ixf
为x轴方向速度滤波信息，q
iyf
为y轴方向速度滤波信息，为受控无人船在地球坐标系下的速度信息滤波变量，为的导数，为艏向角速度信息滤波变量，为的导数，qi(0)为受控无人船在地球坐标系下的速度初始值，ri(0)为艏向角速度初始值；
[0144]
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储到数据集s
iq
和s
ir
中具体公式为：
[0145][0146]
其中：分别为tk时刻的x轴方向速度滤波信息，tk时刻的y轴方向速度滤波信息，tk时刻的x轴方向位置控制输入滤波信息，tk时刻的y轴方向位置控制输入滤波信息，tk时刻的x轴方向位置回归向量滤波信息，tk时刻的y轴方向位置回
归向量滤波信息，tk时刻的艏向角速度滤波信息，tk时刻的艏向角速度控制输入滤波信息，tk时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。
[0147]
在具体实施例中，学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息过程为：
[0148]
c1、根据式(8)更新权值
[0149][0150]
其中：γ
i1
,γ
i2
,γ
i3
,γ
i4
分别为增益系数，为位置权值w
iq
的估计值，为x轴方向位置权值估计值，为y轴方向位置权值估计值，w
ir
的估计值为艏向角权值w
ir
的估计值，为受控无人船在地球坐标系下的速度信息qi的估计值，为艏向角速度信息ri的估计值；为艏向角速度估计误差，为地球坐标系下的速度信息估计误差，ξ
iq
＝diag{ξ
ix
,ξ
iy
}，ξ
ix
,ξ
iy
,ξ
ir
根据式(9)得到
[0151][0152]
其中从上述数据集s
iq
和s
ir
中获取；
[0153]
c2、根据式(10)计算
[0154][0155]
其中为位置运动模型的待求解估计信息，为艏向角方向待求解估计信息。
[0156]
在具体实施例中，计算地球坐标系下标称控制输入和参考艏向角过程为：
[0157]
d1、将位置环模型(2)改写如下：
[0158][0159]
其中：为无人船在地球坐标系下标称控制输入，为无人船在地球坐标系下x轴方向的标称控制输入，为无人船在地球坐标系下y轴方向的标称控制输入；
[0160]
d2、将无人船在地球坐标系下的位置运动模型的待求解估计信息代入模型(11)并进行离散化，具体公式如下：
[0161][0162]
其中：z
iq
(tk)＝[pi(tk),qi(tk)]
t
表示tk时刻的无人船状态向量；ts为采样间隔时间；向量向量向量向量02＝[0,0]
t
；
[0163]
d3、利用公式(12)在tk时刻进行状态预测如下：
[0164][0165]
其中：z
iq
(tk|tk)为tk时刻状态采样值；z
iq
(tk lts|tk),l＝1,...,n
p
为tk时刻对tk lts 时刻的状态预测，n
p
为预测时域；为t
k-ts时刻施加到受控无人船的地球坐标系下的标称控制输入；为tk mts时刻的标称控制增量，nc表示控制时域；
[0166]
d4、利用递归关系将公式(8)表示如下：
[0167][0168]
其中为tk时刻的预测状态输出序列；为tk时刻控制增量序列；
[0169][0170][0171]
相应的令将式(14)写成如下形式：
[0172]
[0173]
d5、构造如下优化问题模型：
[0174][0175]
式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界；q1、q2和q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；
[0176]
d6、将优化问题模型(15)转化成如下形式：
[0177][0178]
其中：a＝[i2,-i2]
t
，
[0179]
通过求解优化问题模型(16)，得到最优标称控制输入增量序列从而得到地球坐标系下标称控制输入序列即地球坐标系下标称控制输入信息
[0180]
在具体实施例中，计算安全纵向速度控制输入τ
iu
和参考艏向角过程为：
[0181]
e1、构造如(17a),(17b)和(17c)所示安全约束
[0182][0183][0184][0185]
其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，p
ij
为无人船与无人船之间的相对距离，p
ic
为无人船与动态障碍物之间的相对距离，p
io
为无人船与静态障碍物之间的相对距离，为地球坐标系下安全控制输入信息，e
ij
,e
ic
,e
io
如式(18)所示
[0186][0187]
其中：q
ij
为无人船与无人船之间的相对速度，q
ic
为无人船与动态障碍物之间的相
对速度，ds为避碰安全距离，ro为静态障碍物半径距离，为无人船最大加速度；
[0188]
e2、构造如(19)所示优化问题
[0189][0190]
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息
[0191]
e3、根据式(20)计算安全纵向速度控制输入τ
iu
和参考艏向角
[0192][0193]
其中和为中第一个和第二个元素。
[0194]
在具体实施例中，得出艏向角速度控制输入值τ
ir
过程为：
[0195]
f1、将模型(3)进行离散化如下：
[0196]zir
(tk ts)＝a
irzir
(tk) b
ir
τ
ir
(tk) c
ir
ꢀꢀ
(21)
[0197]
其中：表示tk时刻的无人船艏向角状态向量；ts为采样间隔时间；向量向量向量
[0198]
f2、利用式(21)进行预测如下：
[0199][0200]
其中：z
ir
(tk|tk)为tk时刻状态采样值；z
ir
(tk lts|tk),l＝1,...,n
p
为tk时刻对tk lts 时刻的艏向角状态预测，n
rp
为艏向角预测时域；τ
ir
(t
k-ts)为上一采样时刻施加的艏向角速度控制输入；δτ
ir
(tk mts|tk),m＝1,...,nc为tk mts时刻的艏向角速度控制增量，n
rc
表示艏向角控制时域；
[0201]
f3、利用递归关系将式(22)表示如下：
[0202][0203]
其中：为tk时刻的艏向角
预测状态输出序列；为tk时刻的艏向角速度控制增量序a列；
[0204][0204][0205]
相应的令将式(23)写成如下形式：
[0206][0207]
f4、构造如下优化问题模型：
[0208][0209]
式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；与分别为艏向角速度控制增量的上下界；与分别为艏向角速度控制输入的上下界；为参考艏向角序列；
[0210]
f5、将优化问题模型(25)转化成如下形式：
[0211][0212]
其中：
[0213]ar
＝[i
r2
,-i
r2
]
t
[0214][0215]
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列得到最优艏向角速度控制输入序列将序列中第一个元素施加到受控无人船。
[0216]
在具体实施例中，交互信息网络拓扑示意图如图2所示，1号船可以访问虚拟领导者产生的参考轨迹信息，即d1＝1；2号船与3号船可以接受1 号船的信息，即a
21
＝1，d2＝0，a
31
＝1，d3＝0；4号船可以接受2号船的信息，即a
42
＝1，d4＝0；5号船可以接受3号船的信息，即a
52
＝1，d5＝0。跟踪一条由虚拟领导者产生的参考轨迹：
[0217][0218]
本例中无人船均为欠驱动无人船，即只有纵向速度控制输入τ
iu
和艏向角速度控制输入τ
ir
。由于实际无人船存在推力和扭转力矩限制，故纵向速度控制输入和艏向角速度控制输入存在约束，即τ
iumax
＝3，τ
iumax
＝0，τ
irmax
＝-τ
irmin
＝2；
[0219]
编队中各无人船起始状态分别为：编队中各无人船起始状态分别为：编队中各无人船起始状态分别为：编队模式：d
10
＝[0,0,0,0]
t
，d
21
＝[-9,0,0,0]
t
，d
31
＝[0,-9,0,0]
t
， d
42
＝[-9,0,0,0]
t
，d
53
＝[0,-9,0,0]
t
。
[0220]
采样间隔时间ts＝0.1s，预测时域n
p
＝4，控制时域nc＝3。艏向角预测时域n
rp
＝3，艏向角控制时域n
rc
＝2，安全距离ds＝3，圆形静态障碍物半径ro＝3。
[0221]
仿真结果如图3-9所示。图3是无人船编队无碰撞跟踪轨迹示意图，由图3可以看出编队中五艘无人船可以避开三个圆形静态障碍物以及四个动态障碍船，并且相互之间没有碰撞，逐渐进入编队模式并以固定队形跟踪参考直线轨迹。图4a-图4b是不同时间段无人船编队轨迹示意图，图4a
‑ꢀ
图4d分别为第150秒、第300秒、第355秒和第600秒时的编队无人船轨迹图和无人船的位置示意图，由图中可以看出五艘无人船从编队暂态过程到编队队形保持过程中可以实现无人船与静态障碍物、动态障碍船舶及无人船之间的碰撞规避。
[0222]
图5a和图5b是编队中五艘无人船的纵向速度控制输入和艏向角速度控制输入示意图。由图可以看出纵向速度控制输入满足设定约束上下界限。艏向角速度方向控制输入满足设定约束上下界限。
[0223]
图6是编队中无人船与无人船之间距离示意图，由图可以看出编队中无人船与无人船之间的距离一直大于设定的碰撞规避安全距离，实现了无人船之间的碰撞规避。
[0224]
图7是编队中5艘无人船与4个动态障碍船之间距离示意图，由图可以看出编队中无人船与动态障碍船之间的距离一直大于设定的碰撞规避安全距离，实现了无人船与动态障碍物的碰撞规避。
[0225]
图8是编队中5艘无人船与3个静态障碍物之间距离示意图，由图可以看出编队中无人船与静态障碍物之间的距离一直大于设定的碰撞规避安全距离，实现了无人船与静态障碍物的碰撞规避。
[0226]
图9是第i艘无人船的模型待求解估计示意图，其中虚线为模型位置函数实际值，实线为模型待求解的估计值，由图中可以看出实线与虚线吻合程度很高，表明本发明所设计的方法能够实时准确学习模型中待求解信息。
[0227]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。