CN114217603A - 一种多无人船编队安全优化控制系统 - Google Patents

一种多无人船编队安全优化控制系统 Download PDF

Info

Publication number
CN114217603A
CN114217603A CN202111307607.0A CN202111307607A CN114217603A CN 114217603 A CN114217603 A CN 114217603A CN 202111307607 A CN202111307607 A CN 202111307607A CN 114217603 A CN114217603 A CN 114217603A
Authority
CN
China
Prior art keywords
information
coordinate system
unmanned ship
heading
angular velocity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111307607.0A
Other languages
English (en)
Other versions
CN114217603B (zh
Inventor
彭周华
吕光颢
顾兆伟
王丹
尹勇
刘陆
王浩亮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian Maritime University
Original Assignee
Dalian Maritime University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dalian Maritime University filed Critical Dalian Maritime University
Priority to CN202111307607.0A priority Critical patent/CN114217603B/zh
Publication of CN114217603A publication Critical patent/CN114217603A/zh
Application granted granted Critical
Publication of CN114217603B publication Critical patent/CN114217603B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

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

Description

一种多无人船编队安全优化控制系统
技术领域
本发明涉及无人船协同控制领域，尤其涉及一种多无人船编队安全优化控制系统。
背景技术
无人船是船舶智能化发展的产物，作为一种现代工具已广泛应用于情报收集、监视侦察、搜寻救助、水文地理勘察、中继通信等任务中。为提高作业效率，形成规模化效应，多无人船编队协同作业得到广泛研究。多无人船编队协同关键技术领域中，编队安全控制技术是重要组成部分，探索多无人船编队安全控制技术具有重要意义。
目前无人船编队运动控制方法存在以下不足：
一、现有的无人船编队控制方法大多只关注编队控制的鲁棒性，未系统的考虑复杂海洋环境中的动态与静态障碍物，以及编队中无人船之间的碰撞问题。若忽略实际海洋环境航行过程中的障碍及碰撞规避问题，必然会引起编队安全问题，导致编队任务失败。
二、现有的无人船编队防碰撞控制方法大多只关注编队控制的避碰功能，未系统全面的考虑无人船实际状态约束、执行机构约束及能量优化、控制输入的平滑性。此外现有的无人船编队控制方法大多依赖固定精确的无人船数学模型进行控制器的设计，未考虑实际海洋环境下外部干扰及模型不确定性。若忽略这些因素进行控制方法的设计，必然会降低控制方法的最优性和工程适用性。
发明内容
本发明提供一种多无人船编队安全优化控制系统，以克服现有的无人船控制系统未考虑复杂海洋环境中的动态与静态障碍物、无人船实际状态约束、执行机构约束及能量优化、控制输入的平滑性等技术问题。
为了实现上述目的，本发明的技术方案是：
一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；
所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τi、受控无人船输出地球坐标系下的速度信息qi、无人船艏向角速度信息ri和无人船艏向角信息
Figure BDA0003340802330000021
进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir；其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中；
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri，并学习得到位置运动模型的待求解估计信息
Figure BDA0003340802330000022
及艏向角方向待求解估计信息
Figure BDA0003340802330000023
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息
Figure BDA0003340802330000024
并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000025
发送至编队跟踪控制模块和安全避碰避障控制模块；
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000026
计算地球坐标系下标称控制输入信息
Figure BDA0003340802330000027
所计算的地球坐标系下标称控制输入信息
Figure BDA0003340802330000028
输入至所述安全避碰避障控制模块；
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息
Figure BDA0003340802330000029
和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000031
计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA0003340802330000032
并将安全纵向速度控制输入τiu输出至所述受控无人船，将参考艏向角
Figure BDA0003340802330000033
输入至所述艏向角跟踪预测控制模块；
所述艏向角跟踪预测控制模块根据所述参考艏向角
Figure BDA0003340802330000034
无人船艏向角速度信息ri以及艏向角方向待求解估计信息
Figure BDA0003340802330000035
得出艏向角速度控制输入值τir，并将艏向角速度控制输入值τir输入至所述受控无人船。
进一步的，所述受控无人船的运动学与动力学模型具体为：
Figure BDA0003340802330000036
其中，
Figure BDA0003340802330000037
为无人船的位置姿态信息，xi、yi、
Figure BDA0003340802330000038
为无人船在地球坐标系下X轴方向的位置信息、Y轴方向的位置信息以及艏向角信息；
Figure BDA0003340802330000039
为ηi的导数信息；ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；
Figure BDA00033408023300000310
和
Figure BDA00033408023300000311
为ui、vi和ri的导数；fiu、fiv和fir为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解； miu和mir分别为船体纵向和艏摇方向的惯量系数；t为时间。
进一步的，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：
Figure BDA00033408023300000312
Figure BDA00033408023300000313
其中：pi＝[xi,yi]、qi＝[qix,qiy]为受控无人船在地球坐标系下的位置信息和速度信息，
Figure BDA00033408023300000314
为受控无人船在地球坐标系下X轴和Y轴方向的速度信息；
Figure BDA00033408023300000315
分别为pi、qi导数；fiq＝[fix,fiy]为无人船在地球坐标系下的位置运动模型的待求解，fix、fiy为无人船在地球坐标系下X轴和Y轴方向的位置运动模型的待求解，其具体转化方式如下：
Figure BDA0003340802330000041
进一步的，计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为：
Figure BDA0003340802330000042
其中：giq＝[gix,giy]，gix为X轴方向位置控制输入滤波信息，giy为Y轴方向位置控制输入滤波信息，Niq＝[Nix,Niy]，Nix为X轴方向位置回归向量滤波信息，Niy为Y轴方向位置回归向量滤波信息，τiq＝[τix,τiy]T为无人船位置控制输入，式中
Figure BDA0003340802330000043
为无人船在地球坐标系下X轴方向的控制输入，
Figure BDA0003340802330000044
为无人船在地球坐标系下Y轴方向的控制输入，kqf和krf均为滤波系数，ψiq＝[ψix,ψiy]为位置回归向量，ψix为X轴方向的回归向量，ψiy为Y轴方向的回归向量，ψir为艏向角速度方向的回归向量；
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为：
Figure BDA0003340802330000045
其中：qif＝[qixf,qiyf]地球坐标系下的速度滤波信息，qixf为X轴方向速度滤波信息，qiyf为Y轴方向速度滤波信息，
Figure BDA0003340802330000046
为受控无人船在地球坐标系下的速度信息滤波变量，
Figure BDA0003340802330000047
为
Figure BDA0003340802330000048
的导数，
Figure BDA0003340802330000049
为艏向角速度信息滤波变量，
Figure BDA00033408023300000410
为
Figure BDA00033408023300000411
的导数，qi(0)为受控无人船在地球坐标系下的速度初始值，ri(0)为艏向角速度初始值；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中具体公式为：
Figure BDA0003340802330000051
其中：
Figure BDA0003340802330000052
分别为tk时刻的X轴方向速度滤波信息，tk时刻的Y轴方向速度滤波信息，tk时刻的X轴方向位置控制输入滤波信息，tk时刻的Y轴方向位置控制输入滤波信息，tk时刻的X轴方向位置回归向量滤波信息，tk时刻的Y轴方向位置回归向量滤波信息，tk时刻的艏向角速度滤波信息，tk时刻的艏向角速度控制输入滤波信息，tk时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。
进一步的，学习得到位置运动模型的待求解估计信息
Figure BDA0003340802330000053
及艏向角方向待求解估计信息
Figure BDA0003340802330000054
过程为：
C1、根据式(8)更新权值
Figure BDA0003340802330000055
其中：Γi1,Γi2,Γi3,Γi4分别为增益系数，
Figure BDA0003340802330000056
为位置权值Wiq的估计值，
Figure BDA0003340802330000057
为X轴方向位置权值估计值，
Figure BDA0003340802330000058
为Y轴方向位置权值估计值，Wir的估计值
Figure BDA0003340802330000059
为艏向角权值Wir的估计值，
Figure BDA00033408023300000510
为受控无人船在地球坐标系下的速度信息qi的估计值，
Figure BDA00033408023300000511
为艏向角速度信息ri的估计值；
Figure BDA00033408023300000512
为艏向角速度估计误差，
Figure BDA00033408023300000513
为地球坐标系下的速度信息估计误差，Ξiq＝diag{Ξix,Ξiy}，Ξix,Ξiy,Ξir根据式(9)得到
Figure BDA00033408023300000514
其中
Figure BDA0003340802330000061
从上述数据集Siq和Sir中获取；
C2、根据式(10)计算
Figure BDA0003340802330000062
其中
Figure BDA0003340802330000063
为位置运动模型的待求解估计信息，
Figure BDA0003340802330000064
为艏向角方向待求解估计信息。
进一步的，计算地球坐标系下标称控制输入
Figure BDA0003340802330000065
和参考艏向角
Figure BDA0003340802330000066
过程为：
D1、将位置环模型(2)改写如下：
Figure BDA0003340802330000067
其中：
Figure BDA0003340802330000068
为无人船在地球坐标系下标称控制输入，
Figure BDA0003340802330000069
为无人船在地球坐标系下X轴方向的标称控制输入，
Figure BDA00033408023300000610
为无人船在地球坐标系下Y轴方向的标称控制输入；
D2、将无人船在地球坐标系下的位置运动模型的待求解估计信息
Figure BDA00033408023300000611
代入模型(11)并进行离散化，具体公式如下：
Figure BDA00033408023300000612
其中：Ziq(tk)＝[pi(tk),qi(tk)]T表示tk时刻的无人船状态向量；Ts为采样间隔时间；向量
Figure BDA00033408023300000613
向量
Figure BDA00033408023300000614
向量
Figure BDA00033408023300000615
Figure BDA00033408023300000616
02＝[0,0]T；
D3、利用公式(12)在tk时刻进行状态预测如下：
Figure BDA00033408023300000617
其中：Ziq(tk|tk)为tk时刻状态采样值；Ziq(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs 时刻的状态预测，Np为预测时域；
Figure BDA00033408023300000618
为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入；
Figure BDA0003340802330000071
为tk+mTs时刻的标称控制增量，Nc表示控制时域；
D4、利用递归关系将公式(8)表示如下：
Figure BDA0003340802330000072
其中
Figure BDA0003340802330000073
为tk时刻的预测状态输出序列；
Figure BDA0003340802330000074
为tk时刻控制增量序列；
Figure BDA0003340802330000075
Figure RE-GDA0003458575220000076
相应的令
Figure BDA0003340802330000077
将式(14)写成如下形式：
Figure 1
D5、构造如下优化问题模型：
Figure BDA0003340802330000079
式(15a)、(15b)分别为控制增量约束、控制输入约束,
Figure BDA00033408023300000710
与
Figure BDA00033408023300000711
与
Figure BDA00033408023300000712
分别为地球坐标系下标称控制增量、标称控制输入的上下界；Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；
D6、将优化问题模型(15)转化成如下形式：
Figure BDA00033408023300000713
其中：
Figure BDA00033408023300000714
a＝[I2,-I2]T，
Figure BDA00033408023300000715
通过求解优化问题模型(16)，得到最优标称控制输入增量序列
Figure BDA00033408023300000716
从而得到地球坐标系下标称控制输入序列
Figure BDA00033408023300000717
即地球坐标系下标称控制输入信息
Figure BDA00033408023300000718
进一步的，计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA0003340802330000081
过程为：
E1、构造如(17a),(17b)和(17c)所示安全约束
Figure BDA0003340802330000082
Figure BDA0003340802330000083
Figure BDA0003340802330000084
其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，pij为无人船与无人船之间的相对距离，pic为无人船与动态障碍物之间的相对距离，pio为无人船与静态障碍物之间的相对距离，
Figure BDA0003340802330000085
为地球坐标系下安全控制输入信息，eij,eic,eio如式(18)所示
Figure BDA0003340802330000086
其中：qij为无人船与无人船之间的相对速度，qic为无人船与动态障碍物之间的相对速度，Ds为避碰安全距离，Ro为静态障碍物半径距离，
Figure BDA0003340802330000087
为无人船最大加速度；
E2、构造如(19)所示优化问题
Figure BDA0003340802330000088
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息
Figure BDA0003340802330000089
E3、根据式(20)计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA00033408023300000810
Figure BDA00033408023300000811
其中
Figure BDA00033408023300000812
和
Figure BDA00033408023300000813
为
Figure BDA00033408023300000814
中第一个和第二个元素。
进一步的，得出艏向角速度控制输入值τir过程为：
F1、将模型(3)进行离散化如下：
Zir(tk+Ts)＝AirZir(tk)+Birτir(tk)+Cir (21)
其中：
Figure BDA0003340802330000091
表示tk时刻的无人船艏向角状态向量；Ts为采样间隔时间；向量
Figure BDA0003340802330000092
向量
Figure BDA0003340802330000093
向量
Figure BDA0003340802330000094
F2、利用式(21)进行预测如下：
Figure BDA0003340802330000095
其中：Zir(tk|tk)为tk时刻状态采样值；Zir(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs 时刻的艏向角状态预测，Nrp为艏向角预测时域；τir(tk-Ts)为上一采样时刻施加的艏向角速度控制输入；Δτir(tk+mTs|tk),m＝1,...,Nc为tk+mTs时刻的艏向角速度控制增量，Nrc表示艏向角控制时域；
F3、利用递归关系将式(22)表示如下：
Figure BDA0003340802330000096
其中：
Figure BDA0003340802330000097
为tk时刻的艏向角预测状态输出序列；
Figure BDA0003340802330000098
为tk时刻的艏向角速度控制增量序列；
Figure RE-GDA00034585752200000913
Figure RE-GDA0003458575220000101
相应的令
Figure BDA00033408023300000910
将式(23)写成如下形式：
Figure BDA00033408023300000911
F4、构造如下优化问题模型：
Figure BDA00033408023300000912
式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；
Figure BDA0003340802330000101
与
Figure BDA0003340802330000102
分别为艏向角速度控制增量的上下界；
Figure BDA0003340802330000103
与
Figure BDA0003340802330000104
分别为艏向角速度控制输入的上下界；
Figure BDA0003340802330000105
为参考艏向角序列；
F5、将优化问题模型(25)转化成如下形式：
Figure BDA0003340802330000106
其中：
Figure BDA0003340802330000107
Figure BDA0003340802330000108
ar＝[Ir2,-Ir2]T
Figure BDA0003340802330000109
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列
Figure BDA00033408023300001010
得到最优艏向角速度控制输入序列
Figure BDA00033408023300001011
将序列中第一个元素施加到受控无人船。有益效果：本发明，解决了的问题。
有益效果
一、本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息，并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息；因此不需要固定精确的无人船数学模型参数信息，仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息，从而在复杂海洋环境下，可以提高无人船的抗扰动性与控制精确性。
二、本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避，以及无人船编队中无人船与无人船之间的碰撞规避问题，分别构造碰撞规避安全约束，并考虑无人船实际执行机构约束，设计避碰避障控制优化问题并进行滚动优化求解，提高了无人船编队的安全性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明公开的多无人船编队防碰撞抗干扰控制系统结构示意图；
图2为本发明中一种交互信息网络拓扑示意图；
图3为无人船编队无风险编队轨迹示意图；
图4a为第150秒无人船编队轨迹图；
图4b为第300秒无人船编队轨迹图；
图4c为第355秒无人船编队轨迹图；
图4d为第600秒无人船编队轨迹图；
图5a为5艘船的纵向速度控制输入示意图；
图5b为艏向角速度控制输入示意图；
图6为无人船与无人船之间距离示意图；
图7为5艘无人船与4个动态障碍物a、b、c、d的距离示意图；
图8为5艘无人船与3个静态障碍物1、2、3的距离示意图；
图9为第i艘无人船位置运动模型和艏向角方向的待求解估计示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本实施例提供了一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，如图1，包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；
所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τi、受控无人船输出地球坐标系下的速度信息qi、无人船艏向角速度信息ri和无人船艏向角信息
Figure BDA0003340802330000111
进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir；其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中；
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri，并学习得到位置运动模型的待求解估计信息
Figure BDA0003340802330000121
及艏向角方向待求解估计信息
Figure BDA0003340802330000122
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息
Figure BDA0003340802330000123
并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000124
发送至编队跟踪控制模块和安全避碰避障控制模块；
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000125
计算地球坐标系下标称控制输入信息
Figure BDA0003340802330000126
所计算的地球坐标系下标称控制输入信息
Figure BDA0003340802330000127
输入至所述安全避碰避障控制模块；
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息
Figure BDA0003340802330000128
和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure BDA0003340802330000129
计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA00033408023300001210
并将安全纵向速度控制输入τiu输出至所述受控无人船，将参考艏向角
Figure BDA00033408023300001211
输入至所述艏向角跟踪预测控制模块；
所述艏向角跟踪预测控制模块根据所述参考艏向角
Figure BDA00033408023300001212
无人船艏向角速度信息ri以及艏向角方向待求解估计信息
Figure BDA00033408023300001213
得出艏向角速度控制输入值τir，并将艏向角速度控制输入值τir输入至所述受控无人船。
本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息，并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息；因此不需要固定精确的无人船数学模型参数信息，仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息，从而在复杂海洋环境下，可以提高无人船的抗扰动性与控制精确性。
本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避，以及无人船编队中无人船与无人船之间的碰撞规避问题，分别构造碰撞规避安全约束，并考虑无人船实际执行机构约束，设计避碰避障控制优化问题并进行滚动优化求解，提高了无人船编队的安全性。
在具体实施例中，所述受控无人船的运动学与动力学模型具体为：
Figure BDA0003340802330000131
其中，
Figure BDA0003340802330000132
为无人船的位置姿态信息，xi、yi、
Figure BDA0003340802330000133
为无人船在地球坐标系下X轴方向的位置信息、Y轴方向的位置信息以及艏向角信息；
Figure BDA0003340802330000134
为ηi的导数信息；ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；
Figure BDA0003340802330000135
和
Figure BDA0003340802330000136
为ui、vi和ri的导数；fiu、fiv和fir为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解； miu和mir分别为船体纵向和艏摇方向的惯量系数；t为时间。
在具体实施例中，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：
Figure BDA0003340802330000137
Figure BDA0003340802330000138
其中：pi＝[xi,yi]、qi＝[qix,qiy]为受控无人船在地球坐标系下的位置信息和速度信息，
Figure BDA0003340802330000139
为受控无人船在地球坐标系下X轴和Y轴方向的速度信息；
Figure BDA00033408023300001310
分别为pi、qi导数；fiq＝[fix,fiy]为无人船在地球坐标系下的位置运动模型的待求解，fix、fiy为无人船在地球坐标系下X轴和Y轴方向的位置运动模型的待求解，其具体转化方式如下：
Figure BDA0003340802330000141
在具体实施例中，计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为：
Figure BDA0003340802330000142
其中：giq＝[gix,giy]，gix为X轴方向位置控制输入滤波信息，giy为Y轴方向位置控制输入滤波信息，Niq＝[Nix,Niy]，Nix为X轴方向位置回归向量滤波信息，Niy为Y轴方向位置回归向量滤波信息，τiq＝[τix,τiy]T为无人船位置控制输入，式中
Figure BDA0003340802330000143
为无人船在地球坐标系下X轴方向的控制输入，
Figure BDA0003340802330000144
为无人船在地球坐标系下Y轴方向的控制输入，kqf和krf均为滤波系数，ψiq＝[ψix,ψiy]为位置回归向量，ψix为X轴方向的回归向量，ψiy为Y轴方向的回归向量，ψir为艏向角速度方向的回归向量；
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为：
Figure BDA0003340802330000145
其中：qif＝[qixf,qiyf]地球坐标系下的速度滤波信息，qixf为X轴方向速度滤波信息，qiyf为Y轴方向速度滤波信息，
Figure BDA0003340802330000146
为受控无人船在地球坐标系下的速度信息滤波变量，
Figure BDA0003340802330000147
为
Figure BDA0003340802330000148
的导数，
Figure BDA0003340802330000149
为艏向角速度信息滤波变量，
Figure BDA00033408023300001410
为
Figure BDA00033408023300001411
的导数，qi(0)为受控无人船在地球坐标系下的速度初始值，ri(0)为艏向角速度初始值；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中具体公式为：
Figure BDA0003340802330000151
其中：
Figure BDA0003340802330000152
分别为tk时刻的X轴方向速度滤波信息，tk时刻的Y轴方向速度滤波信息，tk时刻的X轴方向位置控制输入滤波信息，tk时刻的Y轴方向位置控制输入滤波信息，tk时刻的X轴方向位置回归向量滤波信息，tk时刻的Y轴方向位置回归向量滤波信息，tk时刻的艏向角速度滤波信息，tk时刻的艏向角速度控制输入滤波信息，tk时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。
在具体实施例中，学习得到位置运动模型的待求解估计信息
Figure BDA0003340802330000153
及艏向角方向待求解估计信息
Figure BDA0003340802330000154
过程为：
C1、根据式(8)更新权值
Figure BDA0003340802330000155
其中：Γi1,Γi2,Γi3,Γi4分别为增益系数，
Figure BDA0003340802330000156
为位置权值Wiq的估计值，
Figure BDA0003340802330000157
为X轴方向位置权值估计值，
Figure BDA0003340802330000158
为Y轴方向位置权值估计值，Wir的估计值
Figure BDA0003340802330000159
为艏向角权值Wir的估计值，
Figure BDA00033408023300001510
为受控无人船在地球坐标系下的速度信息qi的估计值，
Figure BDA00033408023300001511
为艏向角速度信息ri的估计值；
Figure BDA00033408023300001512
为艏向角速度估计误差，
Figure BDA00033408023300001513
为地球坐标系下的速度信息估计误差，Ξiq＝diag{Ξix,Ξiy}，Ξix,Ξiy,Ξir根据式(9)得到
Figure BDA0003340802330000161
其中
Figure BDA0003340802330000162
从上述数据集Siq和Sir中获取；
C2、根据式(10)计算
Figure BDA0003340802330000163
其中
Figure BDA0003340802330000164
为位置运动模型的待求解估计信息，
Figure BDA0003340802330000165
为艏向角方向待求解估计信息。
在具体实施例中，计算地球坐标系下标称控制输入
Figure BDA0003340802330000166
和参考艏向角
Figure BDA0003340802330000167
过程为：
D1、将位置环模型(2)改写如下：
Figure BDA0003340802330000168
其中：
Figure BDA0003340802330000169
为无人船在地球坐标系下标称控制输入，
Figure BDA00033408023300001610
为无人船在地球坐标系下X轴方向的标称控制输入，
Figure BDA00033408023300001611
为无人船在地球坐标系下Y轴方向的标称控制输入；
D2、将无人船在地球坐标系下的位置运动模型的待求解估计信息
Figure BDA00033408023300001612
代入模型(11)并进行离散化，具体公式如下：
Figure BDA00033408023300001613
其中：Ziq(tk)＝[pi(tk),qi(tk)]T表示tk时刻的无人船状态向量；Ts为采样间隔时间；向量
Figure BDA00033408023300001614
向量
Figure BDA00033408023300001615
向量
Figure BDA00033408023300001616
Figure BDA00033408023300001617
02＝[0,0]T；
D3、利用公式(12)在tk时刻进行状态预测如下：
Figure BDA0003340802330000171
其中：Ziq(tk|tk)为tk时刻状态采样值；Ziq(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs 时刻的状态预测，Np为预测时域；
Figure BDA0003340802330000172
为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入；
Figure BDA0003340802330000173
为tk+mTs时刻的标称控制增量，Nc表示控制时域；
D4、利用递归关系将公式(8)表示如下：
Figure BDA0003340802330000174
其中
Figure BDA0003340802330000175
为tk时刻的预测状态输出序列；
Figure BDA0003340802330000176
为tk时刻控制增量序列；
Figure BDA0003340802330000177
Figure RE-GDA0003458575220000179
相应的令
Figure BDA0003340802330000179
将式(14)写成如下形式：
Figure 2
D5、构造如下优化问题模型：
Figure BDA00033408023300001711
式(15a)、(15b)分别为控制增量约束、控制输入约束,
Figure BDA00033408023300001712
与
Figure BDA00033408023300001713
与
Figure BDA00033408023300001714
分别为地球坐标系下标称控制增量、标称控制输入的上下界；Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；
D6、将优化问题模型(15)转化成如下形式：
Figure BDA0003340802330000181
其中：
Figure BDA0003340802330000182
a＝[I2,-I2]T，
Figure BDA0003340802330000183
通过求解优化问题模型(16)，得到最优标称控制输入增量序列
Figure BDA0003340802330000184
从而得到地球坐标系下标称控制输入序列
Figure BDA0003340802330000185
即地球坐标系下标称控制输入信息
Figure BDA0003340802330000186
在具体实施例中，计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA0003340802330000187
过程为：
E1、构造如(17a),(17b)和(17c)所示安全约束
Figure BDA0003340802330000188
Figure BDA0003340802330000189
Figure BDA00033408023300001810
其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，pij为无人船与无人船之间的相对距离，pic为无人船与动态障碍物之间的相对距离，pio为无人船与静态障碍物之间的相对距离，
Figure BDA00033408023300001811
为地球坐标系下安全控制输入信息，eij,eic,eio如式(18)所示
Figure BDA00033408023300001812
其中：qij为无人船与无人船之间的相对速度，qic为无人船与动态障碍物之间的相对速度，Ds为避碰安全距离，Ro为静态障碍物半径距离，
Figure BDA00033408023300001813
为无人船最大加速度；
E2、构造如(19)所示优化问题
Figure BDA0003340802330000191
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息
Figure BDA0003340802330000192
E3、根据式(20)计算安全纵向速度控制输入τiu和参考艏向角
Figure BDA0003340802330000193
Figure BDA0003340802330000194
其中
Figure BDA0003340802330000195
和
Figure BDA0003340802330000196
为
Figure BDA0003340802330000197
中第一个和第二个元素。
在具体实施例中，得出艏向角速度控制输入值τir过程为：
F1、将模型(3)进行离散化如下：
Zir(tk+Ts)＝AirZir(tk)+Birτir(tk)+Cir (21)
其中：
Figure BDA0003340802330000198
表示tk时刻的无人船艏向角状态向量；Ts为采样间隔时间；向量
Figure BDA0003340802330000199
向量
Figure BDA00033408023300001910
向量
Figure BDA00033408023300001911
F2、利用式(21)进行预测如下：
Figure BDA00033408023300001912
其中：Zir(tk|tk)为tk时刻状态采样值；Zir(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs 时刻的艏向角状态预测，Nrp为艏向角预测时域；τir(tk-Ts)为上一采样时刻施加的艏向角速度控制输入；Δτir(tk+mTs|tk),m＝1,...,Nc为tk+mTs时刻的艏向角速度控制增量，Nrc表示艏向角控制时域；
F3、利用递归关系将式(22)表示如下：
Figure BDA00033408023300001913
其中：
Figure BDA00033408023300001914
为tk时刻的艏向角预测状态输出序列；
Figure BDA00033408023300001915
为tk时刻的艏向角速度控制增量序A列；
Figure RE-GDA0003458575220000203
Figure RE-GDA0003458575220000204
相应的令
Figure BDA0003340802330000202
将式(23)写成如下形式：
Figure BDA0003340802330000203
F4、构造如下优化问题模型：
Figure BDA0003340802330000204
式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；
Figure BDA0003340802330000205
与
Figure BDA0003340802330000206
分别为艏向角速度控制增量的上下界；
Figure BDA0003340802330000207
与
Figure BDA0003340802330000208
分别为艏向角速度控制输入的上下界；
Figure BDA0003340802330000209
为参考艏向角序列；
F5、将优化问题模型(25)转化成如下形式：
Figure BDA00033408023300002010
其中：
Figure BDA00033408023300002011
Figure BDA00033408023300002012
ar＝[Ir2,-Ir2]T
Figure BDA00033408023300002013
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列
Figure BDA00033408023300002014
得到最优艏向角速度控制输入序列
Figure BDA00033408023300002015
将序列中第一个元素施加到受控无人船。
在具体实施例中，交互信息网络拓扑示意图如图2所示，1号船可以访问虚拟领导者产生的参考轨迹信息，即d1＝1；2号船与3号船可以接受1 号船的信息，即a21＝1，d2＝0，a31＝1，d3＝0；4号船可以接受2号船的信息，即a42＝1，d4＝0；5号船可以接受3号船的信息，即a52＝1，d5＝0。跟踪一条由虚拟领导者产生的参考轨迹：
Figure BDA0003340802330000211
本例中无人船均为欠驱动无人船，即只有纵向速度控制输入τiu和艏向角速度控制输入τir。由于实际无人船存在推力和扭转力矩限制，故纵向速度控制输入和艏向角速度控制输入存在约束，即τiumax＝3，τiumax＝0，τirmax＝-τirmin＝2；
编队中各无人船起始状态分别为：
Figure BDA0003340802330000212
Figure BDA0003340802330000213
Figure BDA0003340802330000214
编队模式：D10＝[0,0,0,0]T，D21＝[-9,0,0,0]T，D31＝[0,-9,0,0]T，D42＝[-9,0,0,0]T，D53＝[0,-9,0,0]T。
采样间隔时间Ts＝0.1s，预测时域Np＝4，控制时域Nc＝3。艏向角预测时域Nrp＝3，艏向角控制时域Nrc＝2，安全距离Ds＝3，圆形静态障碍物半径Ro＝3。
仿真结果如图3-9所示。图3是无人船编队无碰撞跟踪轨迹示意图，由图3可以看出编队中五艘无人船可以避开三个圆形静态障碍物以及四个动态障碍船，并且相互之间没有碰撞，逐渐进入编队模式并以固定队形跟踪参考直线轨迹。图4a-图4b是不同时间段无人船编队轨迹示意图，图4a- 图4d分别为第150秒、第300秒、第355秒和第600秒时的编队无人船轨迹图和无人船的位置示意图，由图中可以看出五艘无人船从编队暂态过程到编队队形保持过程中可以实现无人船与静态障碍物、动态障碍船舶及无人船之间的碰撞规避。
图5a和图5b是编队中五艘无人船的纵向速度控制输入和艏向角速度控制输入示意图。由图可以看出纵向速度控制输入满足设定约束上下界限。艏向角速度方向控制输入满足设定约束上下界限。
图6是编队中无人船与无人船之间距离示意图，由图可以看出编队中无人船与无人船之间的距离一直大于设定的碰撞规避安全距离，实现了无人船之间的碰撞规避。
图7是编队中5艘无人船与4个动态障碍船之间距离示意图，由图可以看出编队中无人船与动态障碍船之间的距离一直大于设定的碰撞规避安全距离，实现了无人船与动态障碍物的碰撞规避。
图8是编队中5艘无人船与3个静态障碍物之间距离示意图，由图可以看出编队中无人船与静态障碍物之间的距离一直大于设定的碰撞规避安全距离，实现了无人船与静态障碍物的碰撞规避。
图9是第i艘无人船的模型待求解估计示意图，其中虚线为模型位置函数实际值，实线为模型待求解的估计值，由图中可以看出实线与虚线吻合程度很高，表明本发明所设计的方法能够实时准确学习模型中待求解信息。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种多无人船编队安全优化控制系统，用于控制编队中每艘无人船以编队形式跟踪给定参考轨迹，其特征在于：包括滤波模块、数据收集模块、数据驱动模型信息学习模块、信息交互通讯网络拓扑模块、编队跟踪控制模块、安全避碰避障控制模块、艏向角预测跟踪控制模块；
所述滤波模块用于获取每个受控无人船的运动学与动力学模型的受控无人船输入信息τi、受控无人船输出地球坐标系下的速度信息qi、无人船艏向角速度信息ri和无人船艏向角信息
Figure RE-FDA0003458575210000011
进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir；其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中；
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri，并学习得到位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000012
及艏向角方向待求解估计信息
Figure RE-FDA0003458575210000013
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000014
并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000015
发送至编队跟踪控制模块和安全避碰避障控制模块；
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000016
计算地球坐标系下标称控制输入信息
Figure RE-FDA0003458575210000017
所计算的地球坐标系下标称控制输入信息
Figure RE-FDA0003458575210000018
输入至所述安全避碰避障控制模块；
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息
Figure RE-FDA0003458575210000021
和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000022
计算安全纵向速度控制输入τiu和参考艏向角
Figure RE-FDA0003458575210000023
并将安全纵向速度控制输入τiu输出至所述受控无人船，将参考艏向角
Figure RE-FDA0003458575210000024
输入至所述艏向角跟踪预测控制模块；
所述艏向角跟踪预测控制模块根据所述参考艏向角
Figure RE-FDA0003458575210000025
无人船艏向角速度信息ri以及艏向角方向待求解估计信息
Figure RE-FDA0003458575210000026
得出艏向角速度控制输入值τir，并将艏向角速度控制输入值τir输入至所述受控无人船。
2.如权利要求1所述的一种多无人船编队安全优化控制系统，其特征在于，所述受控无人船的运动学与动力学模型具体为：
Figure RE-FDA0003458575210000027
其中，
Figure RE-FDA0003458575210000028
为无人船的位置姿态信息，xi、yi、
Figure RE-FDA0003458575210000029
为无人船在地球坐标系下X轴方向的位置信息、Y轴方向的位置信息以及艏向角信息；
Figure RE-FDA00034585752100000210
为ηi的导数信息，ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度；
Figure RE-FDA00034585752100000211
和
Figure RE-FDA00034585752100000212
为ui、vi和ri的导数；fiu、fiv和fir为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解；miu和mir分别为船体纵向和艏摇方向的惯量系数；t为时间。
3.如权利要求2所述的一种多无人船编队安全优化控制系统，其特征在于，所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3)，具体如下：
Figure RE-FDA00034585752100000213
Figure RE-FDA00034585752100000214
其中：pi＝[xi,yi]、qi＝[qix,qiy]为受控无人船在地球坐标系下的位置信息和速度信息，
Figure RE-FDA00034585752100000215
Figure RE-FDA00034585752100000216
为受控无人船在地球坐标系下X轴和Y轴方向的速度信息；
Figure RE-FDA00034585752100000217
分别为pi、qi导数；fiq＝[fix,fiy]为无人船在地球坐标系下的位置运动模型的待求解，fix、fiy为无人船在地球坐标系下X轴和Y轴方向的位置运动模型的待求解，其具体转化方式如下：
Figure RE-FDA0003458575210000031
4.如权利要求3所述的一种多无人船编队安全优化控制系统，其特征在于，计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为：
Figure RE-FDA0003458575210000032
其中：giq＝[gix,giy]，gix为X轴方向位置控制输入滤波信息，giy为Y轴方向位置控制输入滤波信息，Niq＝[Nix,Niy]，Nix为X轴方向位置回归向量滤波信息，Niy为Y轴方向位置回归向量滤波信息，τiq＝[τix,τiy]T为无人船位置控制输入，式中
Figure RE-FDA0003458575210000033
为无人船在地球坐标系下X轴方向的控制输入，
Figure RE-FDA0003458575210000034
为无人船在地球坐标系下Y轴方向的控制输入，kqf和krf均为滤波系数，ψiq＝[ψix,ψiy]为位置回归向量，ψix为X轴方向的回归向量，ψiy为Y轴方向的回归向量，ψir为艏向角速度方向的回归向量；
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为：
Figure RE-FDA0003458575210000035
其中：qif＝[qixf,qiyf]地球坐标系下的速度滤波信息，qixf为X轴方向速度滤波信息，qiyf为Y轴方向速度滤波信息，
Figure RE-FDA0003458575210000036
为受控无人船在地球坐标系下的速度信息滤波变量，
Figure RE-FDA0003458575210000037
为
Figure RE-FDA0003458575210000038
的导数，
Figure RE-FDA0003458575210000039
为艏向角速度信息滤波变量，
Figure RE-FDA00034585752100000310
为
Figure RE-FDA0003458575210000041
的导数，qi(0)为受控无人船在地球坐标系下的速度初始值，ri(0)为艏向角速度初始值；
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir，将存储到数据集Siq和Sir中具体公式为：
Figure RE-FDA0003458575210000042
其中：
Figure RE-FDA0003458575210000043
分别为tk时刻的X轴方向速度滤波信息，tk时刻的Y轴方向速度滤波信息，tk时刻的X轴方向位置控制输入滤波信息，tk时刻的Y轴方向位置控制输入滤波信息，tk时刻的X轴方向位置回归向量滤波信息，tk时刻的Y轴方向位置回归向量滤波信息，tk时刻的艏向角速度滤波信息，tk时刻的艏向角速度控制输入滤波信息，tk时刻的艏向角回归向量滤波信息，l为堆栈时间带宽。
5.如权利要求4所述的一种多无人船编队安全优化控制系统，其特征在于，学习得到位置运动模型的待求解估计信息
Figure RE-FDA0003458575210000044
及艏向角方向待求解估计信息
Figure RE-FDA0003458575210000045
过程为：
C1、根据式(8)更新权值
Figure RE-FDA0003458575210000046
其中：Γi1,Γi2,Γi3,Γi4分别为增益系数，
Figure RE-FDA0003458575210000047
为位置权值Wiq的估计值，
Figure RE-FDA0003458575210000048
为X轴方向位置权值估计值，
Figure RE-FDA0003458575210000049
为Y轴方向位置权值估计值，Wir的估计值
Figure RE-FDA00034585752100000410
为艏向角权值Wir的估计值，
Figure RE-FDA00034585752100000411
为受控无人船在地球坐标系下的速度信息qi的估计值，
Figure RE-FDA00034585752100000412
为艏向角速度信息ri的估计值；
Figure RE-FDA00034585752100000413
为艏向角速度估计误差，
Figure RE-FDA00034585752100000414
为地球坐标系下的速度信息估计误差，Ξiq＝diag{Ξix,Ξiy}，Ξix,Ξiy,Ξir根据式(9)得到
Figure RE-FDA0003458575210000051
其中
Figure RE-FDA0003458575210000052
从上述数据集Siq和Sir中获取；
C2、根据式(10)计算
Figure RE-FDA0003458575210000053
其中
Figure RE-FDA0003458575210000054
为位置运动模型的待求解估计信息，
Figure RE-FDA0003458575210000055
为艏向角方向待求解估计信息。
6.如权利要求5所述的一种多无人船编队安全优化控制系统，其特征在于，计算地球坐标系下标称控制输入
Figure RE-FDA0003458575210000056
和参考艏向角
Figure RE-FDA0003458575210000057
过程为：
D1、将位置环模型(2)改写如下：
Figure RE-FDA0003458575210000058
其中：
Figure RE-FDA0003458575210000059
为无人船在地球坐标系下标称控制输入，
Figure RE-FDA00034585752100000510
为无人船在地球坐标系下X轴方向的标称控制输入，
Figure RE-FDA00034585752100000511
为无人船在地球坐标系下Y轴方向的标称控制输入；
D2、将无人船在地球坐标系下的位置运动模型的待求解估计信息
Figure RE-FDA00034585752100000512
代入模型(11)并进行离散化，具体公式如下：
Figure RE-FDA00034585752100000513
其中：Ziq(tk)＝[pi(tk),qi(tk)]T表示tk时刻的无人船状态向量；Ts为采样间隔时间；向量
Figure RE-FDA00034585752100000514
向量
Figure RE-FDA00034585752100000515
向量
Figure RE-FDA00034585752100000516
Figure RE-FDA00034585752100000517
02＝[0,0]T；
D3、利用公式(12)在tk时刻进行状态预测如下：
Figure RE-FDA0003458575210000061
其中：Ziq(tk|tk)为tk时刻状态采样值；Ziq(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs时刻的状态预测，Np为预测时域；
Figure RE-FDA0003458575210000062
为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入；
Figure RE-FDA0003458575210000063
为tk+mTs时刻的标称控制增量，Nc表示控制时域；
D4、利用递归关系将公式(8)表示如下：
Figure RE-FDA0003458575210000064
其中
Figure RE-FDA0003458575210000065
为tk时刻的预测状态输出序列；
Figure RE-FDA0003458575210000066
为tk时刻控制增量序列；
Figure RE-FDA0003458575210000067
Figure RE-FDA0003458575210000068
相应的令
Figure RE-FDA0003458575210000069
将式(14)写成如下形式：
Figure RE-FDA00034585752100000610
D5、构造如下优化问题模型：
Figure RE-FDA00034585752100000611
式(15a)、(15b)分别为控制增量约束、控制输入约束,
Figure RE-FDA00034585752100000612
与
Figure RE-FDA00034585752100000613
与
Figure RE-FDA00034585752100000614
分别为地球坐标系下标称控制增量、标称控制输入的上下界；Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵；
D6、将优化问题模型(15)转化成如下形式：
Figure RE-FDA0003458575210000071
其中：
Figure RE-FDA0003458575210000072
a＝[I2,-I2]T，
Figure RE-FDA0003458575210000073
通过求解优化问题模型(16)，得到最优标称控制输入增量序列
Figure RE-FDA0003458575210000074
从而得到地球坐标系下标称控制输入序列
Figure RE-FDA0003458575210000075
即地球坐标系下标称控制输入信息
Figure RE-FDA0003458575210000076
7.如权利要求6所述的一种多无人船编队安全优化控制系统，其特征在于，计算安全纵向速度控制输入τiu和参考艏向角
Figure RE-FDA0003458575210000077
过程为：
E1、构造如(17a),(17b)和(17c)所示安全约束
Figure RE-FDA0003458575210000078
Figure RE-FDA0003458575210000079
Figure RE-FDA00034585752100000710
其中：(17a)表示编队中无人船之间的碰撞规避约束，(17b)表示无人船与动态障碍物之间的碰撞规避约束，(17c)表示无人船与静态障碍物的碰撞规避约束，pij为无人船与无人船之间的相对距离，pic为无人船与动态障碍物之间的相对距离，pio为无人船与静态障碍物之间的相对距离，
Figure RE-FDA00034585752100000711
为地球坐标系下安全控制输入信息，eij,eic,eio如式(18)所示
Figure RE-FDA00034585752100000712
其中：qij为无人船与无人船之间的相对速度，qic为无人船与动态障碍物之间的相对速度，Ds为避碰安全距离，Ro为静态障碍物半径距离，
Figure RE-FDA00034585752100000713
为无人船最大加速度；
E2、构造如(19)所示优化问题
Figure RE-FDA0003458575210000081
通过求解优化问题模型(19)，得到地球坐标系下安全控制输入信息
Figure RE-FDA0003458575210000082
E3、根据式(20)计算安全纵向速度控制输入τiu和参考艏向角
Figure RE-FDA0003458575210000083
Figure RE-FDA0003458575210000084
其中
Figure RE-FDA0003458575210000085
和
Figure RE-FDA0003458575210000086
为
Figure RE-FDA0003458575210000087
中第一个和第二个元素。
8.如权利要求7所述的一种多无人船编队安全优化控制系统，其特征在于，得出艏向角速度控制输入值τir过程为：
F1、将模型(3)进行离散化如下：
Zir(tk+Ts)＝AirZir(tk)+Birτir(tk)+Cir (21)
其中：
Figure RE-FDA0003458575210000088
表示tk时刻的无人船艏向角状态向量；Ts为采样间隔时间；向量
Figure RE-FDA0003458575210000089
向量
Figure RE-FDA00034585752100000810
向量
Figure RE-FDA00034585752100000811
F2、利用式(21)进行预测如下：
Figure RE-FDA00034585752100000812
其中：Zir(tk|tk)为tk时刻状态采样值；Zir(tk+lTs|tk),l＝1,...,Np为tk时刻对tk+lTs时刻的艏向角状态预测，Nrp为艏向角预测时域；τir(tk-Ts)为上一采样时刻施加的艏向角速度控制输入；Δτir(tk+mTs|tk),m＝1,...,Nc为tk+mTs时刻的艏向角速度控制增量，Nrc表示艏向角控制时域；
F3、利用递归关系将式(22)表示如下：
Figure RE-FDA00034585752100000813
其中：
Figure RE-FDA0003458575210000091
为tk时刻的艏向角预测状态输出序列；
Figure RE-FDA0003458575210000092
为tk时刻的艏向角速度控制增量序列；
Figure RE-FDA0003458575210000093
Figure RE-FDA0003458575210000094
相应的令
Figure RE-FDA0003458575210000095
将式(23)写成如下形式：
Figure RE-FDA0003458575210000096
F4、构造如下优化问题模型：
Figure RE-FDA0003458575210000097
式(25a)、(25b)分别为艏向角速度控制增量约束、艏向角速度控制输入约束；
Figure RE-FDA0003458575210000098
与
Figure RE-FDA0003458575210000099
分别为艏向角速度控制增量的上下界；
Figure RE-FDA00034585752100000910
与
Figure RE-FDA00034585752100000911
分别为艏向角速度控制输入的上下界；
Figure RE-FDA00034585752100000912
为参考艏向角序列；
F5、将优化问题模型(25)转化成如下形式：
Figure RE-FDA00034585752100000913
其中：
Figure RE-FDA00034585752100000914
Figure RE-FDA00034585752100000915
ar＝[Ir2,-Ir2]T
Figure RE-FDA00034585752100000916
求解优化问题模型(26)，得到最优艏向角速度控制输入增量序列
Figure RE-FDA00034585752100000917
得到最优艏向角速度控制输入序列
Figure RE-FDA00034585752100000918
将序列中第一个元素施加到受控无人船。
CN202111307607.0A 2021-11-05 2021-11-05 一种多无人船编队安全优化控制系统 Active CN114217603B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111307607.0A CN114217603B (zh) 2021-11-05 2021-11-05 一种多无人船编队安全优化控制系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111307607.0A CN114217603B (zh) 2021-11-05 2021-11-05 一种多无人船编队安全优化控制系统

Publications (2)

Publication Number Publication Date
CN114217603A true CN114217603A (zh) 2022-03-22
CN114217603B CN114217603B (zh) 2024-04-30

Family

ID=80696635

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111307607.0A Active CN114217603B (zh) 2021-11-05 2021-11-05 一种多无人船编队安全优化控制系统

Country Status (1)

Country Link
CN (1) CN114217603B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113741433A (zh) * 2021-08-18 2021-12-03 中国舰船研究设计中心 一种水面无人船的分布式编队方法
CN117055578A (zh) * 2023-09-21 2023-11-14 大连海事大学 低设计复杂需求的usv-uav协同路径跟踪控制方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112631305A (zh) * 2020-12-28 2021-04-09 大连海事大学 一种多无人船编队防碰撞抗干扰控制系统
CN113093742A (zh) * 2021-03-30 2021-07-09 大连海事大学 一种自动规避多障碍物的无人船路径跟踪系统
CN113189979A (zh) * 2021-04-02 2021-07-30 大连海事大学 一种无人船的分布式队列有限时间控制方法
CN113253721A (zh) * 2021-04-23 2021-08-13 大连海事大学 一种时变海流干扰下无人船集群协同避碰制导方法及系统
CN113282083A (zh) * 2021-05-17 2021-08-20 北京航空航天大学 一种基于机器人操作系统的无人车编队实验平台
WO2021217334A1 (zh) * 2020-04-27 2021-11-04 深圳市大疆创新科技有限公司 可移动平台及其避障方法和装置
WO2021217341A1 (zh) * 2020-04-27 2021-11-04 深圳市大疆创新科技有限公司 避障方法、可移动平台、控制设备和存储介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021217334A1 (zh) * 2020-04-27 2021-11-04 深圳市大疆创新科技有限公司 可移动平台及其避障方法和装置
WO2021217341A1 (zh) * 2020-04-27 2021-11-04 深圳市大疆创新科技有限公司 避障方法、可移动平台、控制设备和存储介质
CN112631305A (zh) * 2020-12-28 2021-04-09 大连海事大学 一种多无人船编队防碰撞抗干扰控制系统
CN113093742A (zh) * 2021-03-30 2021-07-09 大连海事大学 一种自动规避多障碍物的无人船路径跟踪系统
CN113189979A (zh) * 2021-04-02 2021-07-30 大连海事大学 一种无人船的分布式队列有限时间控制方法
CN113253721A (zh) * 2021-04-23 2021-08-13 大连海事大学 一种时变海流干扰下无人船集群协同避碰制导方法及系统
CN113282083A (zh) * 2021-05-17 2021-08-20 北京航空航天大学 一种基于机器人操作系统的无人车编队实验平台

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
齐小伟;任光;: "基于领导跟随的船舶航迹控制", 船舶, no. 01 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113741433A (zh) * 2021-08-18 2021-12-03 中国舰船研究设计中心 一种水面无人船的分布式编队方法
CN113741433B (zh) * 2021-08-18 2023-12-15 中国舰船研究设计中心 一种水面无人船的分布式编队方法
CN117055578A (zh) * 2023-09-21 2023-11-14 大连海事大学 低设计复杂需求的usv-uav协同路径跟踪控制方法

Also Published As

Publication number Publication date
CN114217603B (zh) 2024-04-30

Similar Documents

Publication Publication Date Title
Gao et al. Fixed-time leader-follower formation control of autonomous underwater vehicles with event-triggered intermittent communications
Peng et al. An overview of recent advances in coordinated control of multiple autonomous surface vehicles
Wei et al. MPC-based motion planning and control enables smarter and safer autonomous marine vehicles: Perspectives and a tutorial survey
Yan et al. Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control
CN107563044B (zh) 基于在线安全学习的四旋翼无人机路径跟踪控制方法
CN112631305B (zh) 一种多无人船编队防碰撞抗干扰控制系统
CN114217603A (zh) 一种多无人船编队安全优化控制系统
Qu et al. Finite-time sideslip observer-based synchronized path-following control of multiple unmanned underwater vehicles
Nielsen et al. Machine learning enhancement of manoeuvring prediction for ship Digital Twin using full-scale recordings
CN105974930A (zh) 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法
CN112068440B (zh) 基于模型预测控制的auv回收对接动力定位控制方法
Zhang et al. Disturbance observer-based composite neural learning path following control of underactuated ships subject to input saturation
Han et al. Tracking control of ship at sea based on MPC with virtual ship bunch under Frenet frame
Ribas et al. Delayed state information filter for USBL-aided AUV navigation
Liang et al. Swarm velocity guidance based distributed finite-time coordinated path-following for uncertain under-actuated autonomous surface vehicles
Li et al. Event-triggered robust adaptive control for path following of the URS in presence of the marine practice
CN114967714A (zh) 一种自主式水下机器人抗扰运动控制方法及系统
CN107315348B (zh) 一种基于惩罚式小波网络的无人潜航器位姿控制方法
Gao et al. Command filtered path tracking control of saturated ASVs based on time‐varying disturbance observer
Li et al. Data-driven docking control of autonomous double-ended ferries based on iterative learning model predictive control
Wang et al. Path Following Control for Unmanned Surface Vehicles: A Reinforcement Learning-Based Method With Experimental Validation
Liu et al. Velocity Domain-based Distributed Pursuit-Encirclement Control for Multi-USVs with Incomplete Information
de Cerqueira Gava et al. AUV cruising auto pilot for a long straight confined underwater tunnel
Gao et al. Disturbance observer based finite-time trajectory tracking control of unmanned surface vehicles with unknown dead-zones
CN115616907B (zh) 一种无人艇航向智能规划方法及控制器

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant