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

技术特征：
1.一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，其特征在于：包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τ
i
、受控无人船输出地球坐标系下的速度信息q
i
、无人船艏向角速度信息r
i
和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
；其中所述受控无人船输入信息τ
i
包括受控无人船输入信息包括纵向速度τ
iu
和艏向角速度τ
ir
；所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储到数据集s
iq
和s
ir
中；所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集s
iq
和s
ir
、受控无人船输出地球坐标系下的速度信息q
i
和无人船艏向角速度信息r
i
，并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息p
j
、在地球坐标系下的速度信息q
j
、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息p
j
、在地球坐标系下的速度信息q
j
和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块；所述编队跟踪控制模块获取所述在地球坐标系下的位置信息p
j
、在地球坐标系下的速度信息q
j
和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块；所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息p
j
、在地球坐标系下的速度信息q
j
和位置运动模型的待求解估计信息计算安全纵向速度控制输入τ
iu
和参考艏向角并将安全纵向速度控制输入τ
iu
输出至所述受控无人船，将参考艏向角输入至所述艏向角跟踪预测控制模块；所述艏向角跟踪预测控制模块根据所述参考艏向角无人船艏向角速度信息r
i
以及艏向角方向待求解估计信息得出艏向角速度控制输入值τ
ir
，并将艏向角速度控制输入值τ
ir
输入至所述受控无人船。2.如权利要求1所述的一种多无人船编队安全优化控制系统，其特征在于，所述受控无人船的运动学与动力学模型具体为：
其中，为无人船的位置姿态信息，x
i
、y
i
、为无人船在地球坐标系下x轴方向的位置信息、y轴方向的位置信息以及艏向角信息；为η
i
的导数信息，u
i
、v
i
和r
i
为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；和为u
i
、v
i
和r
i
的导数；f
iu
、f
iv
和f
ir
为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解；m
iu
和m
ir
分别为船体纵向和艏摇方向的惯量系数；t为时间。3.如权利要求2所述的一种多无人船编队安全优化控制系统，其特征在于，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：其中：p
i
＝[x
i
,y
i
]、q
i
＝[q
ix
,q
iy
]为受控无人船在地球坐标系下的位置信息和速度信息，息，为受控无人船在地球坐标系下x轴和y轴方向的速度信息；分别为p
i
、q
i
导数；f
iq
＝[f
ix
,f
iy
]为无人船在地球坐标系下的位置运动模型的待求解，f
ix
、f
iy
为无人船在地球坐标系下x轴和y轴方向的位置运动模型的待求解，其具体转化方式如下：4.如权利要求3所述的一种多无人船编队安全优化控制系统，其特征在于，计算地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
具体公式为：其中：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
为艏向角速度方向的回归向量；计算得到所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
具体公式为：其中：q
if
＝[q
ixf
,q
iyf
]地球坐标系下的速度滤波信息，q
ixf
为x轴方向速度滤波信息，q
iyf
为y轴方向速度滤波信息，为受控无人船在地球坐标系下的速度信息滤波变量，为的导数，为艏向角速度信息滤波变量，为的导数，q
i
(0)为受控无人船在地球坐标系下的速度初始值，r
i
(0)为艏向角速度初始值；所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息q
if
、无人船艏向角速度滤波信息r
if
、地球坐标系下的位置控制输入滤波信息g
iq
、艏向角速度控制输入滤波信息g
ir
、位置回归向量滤波信息n
iq
和艏向角回归向量滤波信息n
ir
，将存储到数据集s
iq
和s
ir
中具体公式为：其中：分别为t
k
时刻的x轴方向速度滤波信息，t
k
时刻的y轴方向速度滤波信息，t
k
时刻的x轴方向位置控制输入滤波信息，t
k
时刻的y轴方向位置控制输入滤波信息，t
k
时刻的x轴方向位置回归向量滤波信息，t
k
时刻的y轴方向位置回归向量滤波信息，t
k
时刻的艏向角速度滤波信息，t
k
时刻的艏向角速度控制输入滤波信息，t
k
时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。5.如权利要求4所述的一种多无人船编队安全优化控制系统，其特征在于，学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息过程为：c1、根据式(8)更新权值其中：γ
i1
,γ
i2
,γ
i3
,γ
i4
分别为增益系数，为位置权值w
iq
的估计值，为x轴方向位置权值估计值，为y轴方向位置权值估计值，w
ir
的估计值为艏向角权值w
ir
的估计值，为受控无人船在地球坐标系下的速度信息q
i
的估计值，为艏向角速度
信息r
i
的估计值；为艏向角速度估计误差，为地球坐标系下的速度信息估计误差，ξ
iq
＝diag{ξ
ix
,ξ
iy
}，ξ
ix
,ξ
iy
,ξ
ir
根据式(9)得到其中从上述数据集s
iq
和s
ir
中获取；c2、根据式(10)计算其中为位置运动模型的待求解估计信息，为艏向角方向待求解估计信息。6.如权利要求5所述的一种多无人船编队安全优化控制系统，其特征在于，计算地球坐标系下标称控制输入和参考艏向角过程为：d1、将位置环模型(2)改写如下：其中：为无人船在地球坐标系下标称控制输入，为无人船在地球坐标系下x轴方向的标称控制输入，为无人船在地球坐标系下y轴方向的标称控制输入；d2、将无人船在地球坐标系下的位置运动模型的待求解估计信息代入模型(11)并进行离散化，具体公式如下：其中：z
iq
(t
k
)＝[p
i
(t
k
),q
i
(t
k
)]
t
表示t
k
时刻的无人船状态向量；ts为采样间隔时间；向量向量向量向量02＝[0,0]
t
；d3、利用公式(12)在t
k
时刻进行状态预测如下：
其中：z
iq
(t
k
|t
k
)为t
k
时刻状态采样值；z
iq
(t
k
lts|t
k
),l＝1,...,n
p
为t
k
时刻对t
k
lts时刻的状态预测，n
p
为预测时域；为t
k-ts时刻施加到受控无人船的地球坐标系下的标称控制输入；为t
k
mts时刻的标称控制增量，n
c
表示控制时域；d4、利用递归关系将公式(8)表示如下：其中为t
k
时刻的预测状态输出序列；为t
k
时刻控制增量序列；时刻控制增量序列；相应的令将式(14)写成如下形式：d5、构造如下优化问题模型：式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界；q1、q2和q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；d6、将优化问题模型(15)转化成如下形式：
其中：a＝[i2,-i2]
t
，通过求解优化问题模型(16)，得到最优标称控制输入增量序列从而得到地球坐标系下标称控制输入序列即地球坐标系下标称控制输入信息7.如权利要求6所述的一种多无人船编队安全优化控制系统，其特征在于，计算安全纵向速度控制输入τ
iu
和参考艏向角过程为：e1、构造如(17a),(17b)和(17c)所示安全约束e1、构造如(17a),(17b)和(17c)所示安全约束e1、构造如(17a),(17b)和(17c)所示安全约束其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，p
ij
为无人船与无人船之间的相对距离，p
ic
为无人船与动态障碍物之间的相对距离，p
io
为无人船与静态障碍物之间的相对距离，为地球坐标系下安全控制输入信息，e
ij
,e
ic
,e
io
如式(18)所示其中：q
ij
为无人船与无人船之间的相对速度，q
ic
为无人船与动态障碍物之间的相对速度，d
s
为避碰安全距离，r
o
为静态障碍物半径距离，为无人船最大加速度；e2、构造如(19)所示优化问题
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息e3、根据式(20)计算安全纵向速度控制输入τ
iu
和参考艏向角和参考艏向角其中和为中第一个和第二个元素。8.如权利要求7所述的一种多无人船编队安全优化控制系统，其特征在于，得出艏向角速度控制输入值τ
ir
过程为：f1、将模型(3)进行离散化如下：z
ir
(t
k
ts)＝a
ir
z
ir
(t
k
) b
ir
τ
ir
(t
k
) c
ir
ꢀꢀꢀꢀꢀ
(21)其中：表示t
k
时刻的无人船艏向角状态向量；ts为采样间隔时间；向量向量向量f2、利用式(21)进行预测如下：其中：z
ir
(t
k
|t
k
)为t
k
时刻状态采样值；z
ir
(t
k
lts|t
k
),l＝1,...,n
p
为t
k
时刻对t
k
lts时刻的艏向角状态预测，n
rp
为艏向角预测时域；τ
ir
(t
k-ts)为上一采样时刻施加的艏向角速度控制输入；δτ
ir
(t
k
mts|t
k
),m＝1,...,nc为t
k
mts时刻的艏向角速度控制增量，n
rc
表示艏向角控制时域；f3、利用递归关系将式(22)表示如下：其中：为t
k
时刻的艏向角预测状态输出序列；为t
k
时刻的艏向角速度控制增量序列；
相应的令将式(23)写成如下形式：f4、构造如下优化问题模型：式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；与分别为艏向角速度控制增量的上下界；与分别为艏向角速度控制输入的上下界；为参考艏向角序列；f5、将优化问题模型(25)转化成如下形式：其中：其中：a
r
＝[i
r2
,-i
r2
]
t
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列得到最优艏向角速度控制输入序列将序列中第一个元素施加到受控无人船。

技术总结
本发明公开了一种多无人船编队安全优化控制系统，包括获取无人船输入输出状态信息并滤波的滤波模块，获取并存储无人船输入输出状态滤波信息的数据收集模块，获取数据集并学习无人船模型的模型信息学习模块，获取存在信息交互关系的无人船信息并发送至编队跟踪控制模块和安全避碰避障控制模块中的信息交互通讯网络拓扑模块，获取无人船的航行信息和交互信息计算标称控制输入的编队跟踪控制模块，获取标称控制输入信息与交互信息计算安全优化的纵向速度控制输入和参考艏向角的安全避碰避障控制模块，计算艏向角控制输入的艏向角跟踪预测控制模块。该系统可以在复杂海洋环境下规避动静态障碍物和船与船的碰撞。实现多无人船的无风险编队跟踪控制。船的无风险编队跟踪控制。船的无风险编队跟踪控制。


技术研发人员：彭周华 吕光颢 顾兆伟 王丹 尹勇 刘陆 王浩亮
受保护的技术使用者：大连海事大学
技术研发日：2021.11.05
技术公布日：2022/3/21