CN114217603A - 一种多无人船编队安全优化控制系统 - Google Patents
一种多无人船编队安全优化控制系统 Download PDFInfo
- 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
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 84
- 238000005457 optimization Methods 0.000 title claims abstract description 40
- 238000001914 filtration Methods 0.000 claims abstract description 99
- 230000003068 static effect Effects 0.000 claims abstract description 21
- 230000003993 interaction Effects 0.000 claims abstract description 13
- 238000004891 communication Methods 0.000 claims abstract description 11
- 238000000034 method Methods 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 16
- 238000005070 sampling Methods 0.000 claims description 14
- 238000013480 data collection Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 24
- 238000013461 design Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000013178 mathematical model Methods 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
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和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir;其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中;
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri,并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块;
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块;
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算安全纵向速度控制输入τiu和参考艏向角并将安全纵向速度控制输入τiu输出至所述受控无人船,将参考艏向角输入至所述艏向角跟踪预测控制模块;
进一步的,所述受控无人船的运动学与动力学模型具体为:
其中,为无人船的位置姿态信息,xi、yi、为无人船在地球坐标系下X轴方向的位置信息、Y轴方向的位置信息以及艏向角信息;为ηi的导数信息;ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度;和为ui、vi和ri的导数;fiu、fiv和fir为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解; miu和mir分别为船体纵向和艏摇方向的惯量系数;t为时间。
进一步的,所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3),具体如下:
其中:pi=[xi,yi]、qi=[qix,qiy]为受控无人船在地球坐标系下的位置信息和速度信息,为受控无人船在地球坐标系下X轴和Y轴方向的速度信息;分别为pi、qi导数;fiq=[fix,fiy]为无人船在地球坐标系下的位置运动模型的待求解,fix、fiy为无人船在地球坐标系下X轴和Y轴方向的位置运动模型的待求解,其具体转化方式如下:
进一步的,计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为:
其中:giq=[gix,giy],gix为X轴方向位置控制输入滤波信息,giy为Y轴方向位置控制输入滤波信息,Niq=[Nix,Niy],Nix为X轴方向位置回归向量滤波信息,Niy为Y轴方向位置回归向量滤波信息,τiq=[τix,τiy]T为无人船位置控制输入,式中为无人船在地球坐标系下X轴方向的控制输入,为无人船在地球坐标系下Y轴方向的控制输入,kqf和krf均为滤波系数,ψiq=[ψix,ψiy]为位置回归向量,ψix为X轴方向的回归向量,ψiy为Y轴方向的回归向量,ψir为艏向角速度方向的回归向量;
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为:
其中:qif=[qixf,qiyf]地球坐标系下的速度滤波信息,qixf为X轴方向速度滤波信息,qiyf为Y轴方向速度滤波信息,为受控无人船在地球坐标系下的速度信息滤波变量,为的导数,为艏向角速度信息滤波变量,为的导数,qi(0)为受控无人船在地球坐标系下的速度初始值,ri(0)为艏向角速度初始值;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中具体公式为:
其中:分别为tk时刻的X轴方向速度滤波信息,tk时刻的Y轴方向速度滤波信息,tk时刻的X轴方向位置控制输入滤波信息,tk时刻的Y轴方向位置控制输入滤波信息,tk时刻的X轴方向位置回归向量滤波信息,tk时刻的Y轴方向位置回归向量滤波信息,tk时刻的艏向角速度滤波信息,tk时刻的艏向角速度控制输入滤波信息,tk时刻的艏向角回归向量滤波信息,l为堆栈时间带宽。
C1、根据式(8)更新权值
其中:Γi1,Γi2,Γi3,Γi4分别为增益系数,为位置权值Wiq的估计值,为X轴方向位置权值估计值,为Y轴方向位置权值估计值,Wir的估计值为艏向角权值Wir的估计值,为受控无人船在地球坐标系下的速度信息qi的估计值,为艏向角速度信息ri的估计值;为艏向角速度估计误差,为地球坐标系下的速度信息估计误差,Ξiq=diag{Ξix,Ξiy},Ξix,Ξiy,Ξir根据式(9)得到
C2、根据式(10)计算
D1、将位置环模型(2)改写如下:
D3、利用公式(12)在tk时刻进行状态预测如下:
其中:Ziq(tk|tk)为tk时刻状态采样值;Ziq(tk+lTs|tk),l=1,...,Np为tk时刻对tk+lTs 时刻的状态预测,Np为预测时域;为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入;为tk+mTs时刻的标称控制增量,Nc表示控制时域;
D4、利用递归关系将公式(8)表示如下:
D5、构造如下优化问题模型:
式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界;Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵;
D6、将优化问题模型(15)转化成如下形式:
E1、构造如(17a),(17b)和(17c)所示安全约束
其中:(17a)表示编队中无人船之间的碰撞规避约束,(17b)表示无人船与动态障碍物之间的碰撞规避约束,(17c)表示无人船与静态障碍物的碰撞规避约束,pij为无人船与无人船之间的相对距离,pic为无人船与动态障碍物之间的相对距离,pio为无人船与静态障碍物之间的相对距离,为地球坐标系下安全控制输入信息,eij,eic,eio如式(18)所示
E2、构造如(19)所示优化问题
进一步的,得出艏向角速度控制输入值τir过程为:
F1、将模型(3)进行离散化如下:
Zir(tk+Ts)=AirZir(tk)+Birτir(tk)+Cir (21)
F2、利用式(21)进行预测如下:
其中: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)表示如下:
F4、构造如下优化问题模型:
F5、将优化问题模型(25)转化成如下形式:
有益效果
一、本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息,并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息;因此不需要固定精确的无人船数学模型参数信息,仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息,从而在复杂海洋环境下,可以提高无人船的抗扰动性与控制精确性。
二、本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避,以及无人船编队中无人船与无人船之间的碰撞规避问题,分别构造碰撞规避安全约束,并考虑无人船实际执行机构约束,设计避碰避障控制优化问题并进行滚动优化求解,提高了无人船编队的安全性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图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和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir;其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中;
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri,并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块;
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块;
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算安全纵向速度控制输入τiu和参考艏向角并将安全纵向速度控制输入τiu输出至所述受控无人船,将参考艏向角输入至所述艏向角跟踪预测控制模块;
本发明通过数据收集模块实时收集并存储无人船输入输出滤波信息,并利用数据驱动模型信息学习模块学习得到位置运动模型中待求解信息及艏向角运动模型中待求解信息;因此不需要固定精确的无人船数学模型参数信息,仅通过收集无人船的控制输入信息和输出状态信息进行学习并更新模型信息,从而在复杂海洋环境下,可以提高无人船的抗扰动性与控制精确性。
本发明中安全避碰避障模块针对海洋环境中动态障碍物与静态障碍物的碰撞规避,以及无人船编队中无人船与无人船之间的碰撞规避问题,分别构造碰撞规避安全约束,并考虑无人船实际执行机构约束,设计避碰避障控制优化问题并进行滚动优化求解,提高了无人船编队的安全性。
在具体实施例中,所述受控无人船的运动学与动力学模型具体为:
其中,为无人船的位置姿态信息,xi、yi、为无人船在地球坐标系下X轴方向的位置信息、Y轴方向的位置信息以及艏向角信息;为ηi的导数信息;ui、vi和ri为无人船在船体坐标系下的纵向速度、横漂速度和艏向角速度;和为ui、vi和ri的导数;fiu、fiv和fir为内部不确定性与外部海流扰动在内的纵向待求解、横向待求解和艏向角方向待求解; miu和mir分别为船体纵向和艏摇方向的惯量系数;t为时间。
在具体实施例中,所述受控无人船的运动学与动力学模型(1)解耦成位置运动模型(2)与艏向角运动模型(3),具体如下:
其中:pi=[xi,yi]、qi=[qix,qiy]为受控无人船在地球坐标系下的位置信息和速度信息,为受控无人船在地球坐标系下X轴和Y轴方向的速度信息;分别为pi、qi导数;fiq=[fix,fiy]为无人船在地球坐标系下的位置运动模型的待求解,fix、fiy为无人船在地球坐标系下X轴和Y轴方向的位置运动模型的待求解,其具体转化方式如下:
在具体实施例中,计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为:
其中:giq=[gix,giy],gix为X轴方向位置控制输入滤波信息,giy为Y轴方向位置控制输入滤波信息,Niq=[Nix,Niy],Nix为X轴方向位置回归向量滤波信息,Niy为Y轴方向位置回归向量滤波信息,τiq=[τix,τiy]T为无人船位置控制输入,式中为无人船在地球坐标系下X轴方向的控制输入,为无人船在地球坐标系下Y轴方向的控制输入,kqf和krf均为滤波系数,ψiq=[ψix,ψiy]为位置回归向量,ψix为X轴方向的回归向量,ψiy为Y轴方向的回归向量,ψir为艏向角速度方向的回归向量;
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为:
其中:qif=[qixf,qiyf]地球坐标系下的速度滤波信息,qixf为X轴方向速度滤波信息,qiyf为Y轴方向速度滤波信息,为受控无人船在地球坐标系下的速度信息滤波变量,为的导数,为艏向角速度信息滤波变量,为的导数,qi(0)为受控无人船在地球坐标系下的速度初始值,ri(0)为艏向角速度初始值;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中具体公式为:
其中:分别为tk时刻的X轴方向速度滤波信息,tk时刻的Y轴方向速度滤波信息,tk时刻的X轴方向位置控制输入滤波信息,tk时刻的Y轴方向位置控制输入滤波信息,tk时刻的X轴方向位置回归向量滤波信息,tk时刻的Y轴方向位置回归向量滤波信息,tk时刻的艏向角速度滤波信息,tk时刻的艏向角速度控制输入滤波信息,tk时刻的艏向角回归向量滤波信息,l为堆栈时间带宽。
C1、根据式(8)更新权值
其中:Γi1,Γi2,Γi3,Γi4分别为增益系数,为位置权值Wiq的估计值,为X轴方向位置权值估计值,为Y轴方向位置权值估计值,Wir的估计值为艏向角权值Wir的估计值,为受控无人船在地球坐标系下的速度信息qi的估计值,为艏向角速度信息ri的估计值;为艏向角速度估计误差,为地球坐标系下的速度信息估计误差,Ξiq=diag{Ξix,Ξiy},Ξix,Ξiy,Ξir根据式(9)得到
C2、根据式(10)计算
D1、将位置环模型(2)改写如下:
D3、利用公式(12)在tk时刻进行状态预测如下:
其中:Ziq(tk|tk)为tk时刻状态采样值;Ziq(tk+lTs|tk),l=1,...,Np为tk时刻对tk+lTs 时刻的状态预测,Np为预测时域;为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入;为tk+mTs时刻的标称控制增量,Nc表示控制时域;
D4、利用递归关系将公式(8)表示如下:
D5、构造如下优化问题模型:
式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界;Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵;
D6、将优化问题模型(15)转化成如下形式:
E1、构造如(17a),(17b)和(17c)所示安全约束
其中:(17a)表示编队中无人船之间的碰撞规避约束,(17b)表示无人船与动态障碍物之间的碰撞规避约束,(17c)表示无人船与静态障碍物的碰撞规避约束,pij为无人船与无人船之间的相对距离,pic为无人船与动态障碍物之间的相对距离,pio为无人船与静态障碍物之间的相对距离,为地球坐标系下安全控制输入信息,eij,eic,eio如式(18)所示
E2、构造如(19)所示优化问题
在具体实施例中,得出艏向角速度控制输入值τir过程为:
F1、将模型(3)进行离散化如下:
Zir(tk+Ts)=AirZir(tk)+Birτir(tk)+Cir (21)
F2、利用式(21)进行预测如下:
其中: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)表示如下:
F4、构造如下优化问题模型:
F5、将优化问题模型(25)转化成如下形式:
在具体实施例中,交互信息网络拓扑示意图如图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。跟踪一条由虚拟领导者产生的参考轨迹:
本例中无人船均为欠驱动无人船,即只有纵向速度控制输入τiu和艏向角速度控制输入τir。由于实际无人船存在推力和扭转力矩限制,故纵向速度控制输入和艏向角速度控制输入存在约束,即τiumax=3,τiumax=0,τirmax=-τirmin=2;
编队中各无人船起始状态分别为: 编队模式: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和无人船艏向角信息进而计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir;其中所述受控无人船输入信息τi包括受控无人船输入信息包括纵向速度τiu和艏向角速度τir;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中;
所述数据驱动模型信息学习模块,用于获取数据收集模块所输出的数据集Siq和Sir、受控无人船输出地球坐标系下的速度信息qi和无人船艏向角速度信息ri,并学习得到位置运动模型的待求解估计信息及艏向角方向待求解估计信息
所述信息交互通讯网络拓扑模块用于获取无人船编队中与受控无人船信息交互的受控无人船在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj、位置运动模型的待求解估计信息并将在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息发送至编队跟踪控制模块和安全避碰避障控制模块;
所述编队跟踪控制模块获取所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算地球坐标系下标称控制输入信息所计算的地球坐标系下标称控制输入信息输入至所述安全避碰避障控制模块;
所述安全避碰避障控制模块用于获取编队跟踪控制模块输出的地球坐标系下标称控制输入信息和所述信息交互通讯网络拓扑模块输出的的所述在地球坐标系下的位置信息pj、在地球坐标系下的速度信息qj和位置运动模型的待求解估计信息计算安全纵向速度控制输入τiu和参考艏向角并将安全纵向速度控制输入τiu输出至所述受控无人船,将参考艏向角输入至所述艏向角跟踪预测控制模块;
4.如权利要求3所述的一种多无人船编队安全优化控制系统,其特征在于,计算地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir具体公式为:
其中:giq=[gix,giy],gix为X轴方向位置控制输入滤波信息,giy为Y轴方向位置控制输入滤波信息,Niq=[Nix,Niy],Nix为X轴方向位置回归向量滤波信息,Niy为Y轴方向位置回归向量滤波信息,τiq=[τix,τiy]T为无人船位置控制输入,式中为无人船在地球坐标系下X轴方向的控制输入,为无人船在地球坐标系下Y轴方向的控制输入,kqf和krf均为滤波系数,ψiq=[ψix,ψiy]为位置回归向量,ψix为X轴方向的回归向量,ψiy为Y轴方向的回归向量,ψir为艏向角速度方向的回归向量;
计算得到所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif具体公式为:
其中:qif=[qixf,qiyf]地球坐标系下的速度滤波信息,qixf为X轴方向速度滤波信息,qiyf为Y轴方向速度滤波信息,为受控无人船在地球坐标系下的速度信息滤波变量,为的导数,为艏向角速度信息滤波变量,为的导数,qi(0)为受控无人船在地球坐标系下的速度初始值,ri(0)为艏向角速度初始值;
所述数据收集模块用于获取滤波模块输出的所述地球坐标系下的滤波速度信息qif、无人船艏向角速度滤波信息rif、地球坐标系下的位置控制输入滤波信息giq、艏向角速度控制输入滤波信息gir、位置回归向量滤波信息Niq和艏向角回归向量滤波信息Nir,将存储到数据集Siq和Sir中具体公式为:
C1、根据式(8)更新权值
其中:Γi1,Γi2,Γi3,Γi4分别为增益系数,为位置权值Wiq的估计值,为X轴方向位置权值估计值,为Y轴方向位置权值估计值,Wir的估计值为艏向角权值Wir的估计值,为受控无人船在地球坐标系下的速度信息qi的估计值,为艏向角速度信息ri的估计值;为艏向角速度估计误差,为地球坐标系下的速度信息估计误差,Ξiq=diag{Ξix,Ξiy},Ξix,Ξiy,Ξir根据式(9)得到
C2、根据式(10)计算
D1、将位置环模型(2)改写如下:
D3、利用公式(12)在tk时刻进行状态预测如下:
其中:Ziq(tk|tk)为tk时刻状态采样值;Ziq(tk+lTs|tk),l=1,...,Np为tk时刻对tk+lTs时刻的状态预测,Np为预测时域;为tk-Ts时刻施加到受控无人船的地球坐标系下的标称控制输入;为tk+mTs时刻的标称控制增量,Nc表示控制时域;
D4、利用递归关系将公式(8)表示如下:
D5、构造如下优化问题模型:
式(15a)、(15b)分别为控制增量约束、控制输入约束,与与分别为地球坐标系下标称控制增量、标称控制输入的上下界;Q1、Q2和Q3分别为能量指标权重矩阵、编队队形保持权重矩阵和编队跟踪误差系数矩阵;
D6、将优化问题模型(15)转化成如下形式:
E1、构造如(17a),(17b)和(17c)所示安全约束
其中:(17a)表示编队中无人船之间的碰撞规避约束,(17b)表示无人船与动态障碍物之间的碰撞规避约束,(17c)表示无人船与静态障碍物的碰撞规避约束,pij为无人船与无人船之间的相对距离,pic为无人船与动态障碍物之间的相对距离,pio为无人船与静态障碍物之间的相对距离,为地球坐标系下安全控制输入信息,eij,eic,eio如式(18)所示
E2、构造如(19)所示优化问题
8.如权利要求7所述的一种多无人船编队安全优化控制系统,其特征在于,得出艏向角速度控制输入值τir过程为:
F1、将模型(3)进行离散化如下:
Zir(tk+Ts)=AirZir(tk)+Birτir(tk)+Cir (21)
F2、利用式(21)进行预测如下:
其中: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)表示如下:
F4、构造如下优化问题模型:
F5、将优化问题模型(25)转化成如下形式:
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)
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)
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 | 深圳市大疆创新科技有限公司 | 避障方法、可移动平台、控制设备和存储介质 |
-
2021
- 2021-11-05 CN CN202111307607.0A patent/CN114217603B/zh active Active
Patent Citations (7)
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)
Title |
---|
齐小伟;任光;: "基于领导跟随的船舶航迹控制", 船舶, no. 01 * |
Cited By (3)
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 |