CN109521797A - 无人船引导水下航行器群的优化割草型编队控制方法 - Google Patents
无人船引导水下航行器群的优化割草型编队控制方法 Download PDFInfo
- Publication number
- CN109521797A CN109521797A CN201811621325.6A CN201811621325A CN109521797A CN 109521797 A CN109521797 A CN 109521797A CN 201811621325 A CN201811621325 A CN 201811621325A CN 109521797 A CN109521797 A CN 109521797A
- Authority
- CN
- China
- Prior art keywords
- indicate
- navigation device
- submarine navigation
- unmanned boat
- value
- 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
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/048—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators using a predictor
Abstract
本发明公开了一种无人船引导水下航行器群的优化割草型编队控制方法，用于解决现有多海洋航行器编队控制方法实用性差的技术问题。技术方案是在水下航行器中确定一个主航行器，让无人船与该主航行器通过光缆连接进行通信，主航行器按照水声通信距离与其他水下航行器进行水声通信。构造无人船与参考目标误差模型，利用模型预测控制方法，设计性能指标，实现无人船轨迹跟踪优化问题；将水下航行器分为主航行器与从航行器两部分，主航行器根据无人船和从航行器状态信息，从航行器根据其余水下航行器状态信息，利用分布式模型预测控制方法，实现有约束编队优化控制。由于采用分布式模型预测控制方法，解决了多海洋航行器有约束编队优化问题，实用性好。
Description
技术领域
本发明涉及一种多海洋航行器编队控制方法，特别涉及一种无人船引导水下航行器群的优化割草型编队控制方法。
背景技术
随着社会的快速发展，人们对于资源的需求日益增多，而占据地球71％面积的海洋中拥有着丰富的矿物和水产等资源，能极大缓解人类面临的困难。海洋资源的开发与利用将成为了人类解决资源匮乏问题的关键手段之一。
在海洋资源搜索和探测过程中，自主海洋航行器的使用能很好的避免危险或未知领域人为操作的风险，并且相较于载人海洋航行器结构更加简单，操作更加方便，释放和回收也更加快捷，成本也有所低。
在进行大范围海洋搜索和探测过程中，往往需要多种类型的海洋航行器协同完成作业任务。多海洋航行器协同作业相较于单个海洋航行器不仅能提高作业的效率，还能提高搜索与探测的精度。多海洋航行器协同作业包括集中式和分布式两种模式。集中式模式要求通信中心与所有水下航行器进行通信，要求通信距离长，通信带宽大，但是水声通信一般通信距离短，通信带宽有限，不满足集中式模式的要求。而分布式模式中水下航行器只需要与邻居节点交换信息，通信压力小，时效性高，实现条件相比集中式有所降低，减少了一定成本。此外，分布式控制方式比集中式方式容错能力和可靠性更强。因此，使用分布式控制方法实现多海洋航行器协同作业是一种可靠选择。
由于水下环境无法接受GPS信号，多水下航行器群在进行大范围协同作业时存在准确定位的难题，这一难题将导致多水下航行器群无法精确的完成协同作业任务。针对这一问题，可以采用无人船与水下航行器相互配合的主从式结构，无人船利用获取的GPS定位信息与主水航行器进行通信，而主水下航行器根据水声通信距离与其相邻的从水下航行器进行通信，实现信息交换，实现协同编队控制。
此外由多海洋航行器组成的割草型编队可执行海底绘图、海域无缝搜索和搜救、海洋清污等作业活动。与其他编队队形相比，割草型编队对于区域搜索能力强，对作业区域确保无遗漏，作业效率高，在海洋工程领域具有巨大的应用价值。
目前实现主从式海洋航行器群割草型编队的控制方法有限。文献“Leader–follower formation control of underactuated autonomous underwater vehicles,OceanEng.,vol.37,no.17,pp.1491–1502,2010.”提出一种主从式结构欠驱动AUV编队控制方法。该方法首先将AUV编队分成领导者和跟随者两部分，领导者根据自身位置生成跟随者参考轨迹，接着建立参考轨迹的虚拟目标以及跟随者与虚拟目标误差模型，最后通过李雅普洛夫法和反步法综合设计控制器，实现跟随者的位置跟踪并形成预先设定的编队结构。文献所述方法无需知道领导者速度信息和动力学信息，比较好地解决了在水下通信带宽有限时编队控制问题。但是该方法只适用于同构类型海洋航行器编队控制，不能满足多类型海洋航行器有约束编队优化控制。
发明内容
为了克服现有多海洋航行器编队控制方法实用性差的不足，本发明提供一种无人船引导水下航行器群的优化割草型编队控制方法。该方法在水下航行器中确定一个主航行器，让无人船与该主航行器通过光缆连接进行通信，主航行器按照水声通信的距离与其他水下航行器进行水声通信。构造无人船与参考目标误差模型，利用模型预测控制方法，设计性能指标，实现无人船轨迹跟踪优化问题；将水下航行器分为主航行器与从航行器两部分，主航行器根据无人船和从航行器状态信息，从航行器根据其余水下航行器状态信息，利用分布式模型预测控制方法，设计性能指标，实现有约束编队优化控制。由于采用模型预测控制算法，通过构造二次性能函数，解决了海洋航行器有约束控制问题；在编队控制方面采用主从分布式预测控制算法，降低系统通信成本，解决了多类型海洋航行器有约束编队优化问题，实用性好。
本发明解决其技术问题所采用的技术方案：一种无人船引导水下航行器群的优化割草型编队控制方法，其特点是包括以下步骤：
步骤一、建立编队系统中引导无人船的运动学和动力学模型。
式中，x和y分别表示无人船在惯性坐标系中的位置，ψ表示无人船航向角，u表示无人船前向速度，v表示无人船横向速度，r表示其角速度，u1表示无人船前向推力，u2表示无人船偏航力矩，m表示无人船质量，以及Xu，Yv，Nr表示无人船固有参数。
步骤二、建立水下航行器的运动学和动力学模型。
式中，i表示每个水下航行器，D22＝-Y'v，D33＝-N'r。xi和yi分别表示水下航行器在惯性坐标系中相应位置，ψi表示水下航行器航向角，ui表示水下航行器前向速度，vi表示水下航行器横向速度，ri表示其角速度，u'1i和u'2i分别表示水下航行器前向推力和侧向推力，u'3i表示水下航行器偏航力矩，M表示水下航行器质量，以及X'u，Y'v，N'r表示水下航行器固有参数。
步骤三、建立参考目标模型。
参考目标与无人船有相同动力特性。
式中，其中xd和yd分别表示参考目标在惯性坐标系中的位置，ψd表示参考目标航向角，ud表示参考目标前向速度，vd表示参考目标横向速度，rd表示参考目标角速度，u1d表示参考目标前向推力，u2d表示参考目标偏航力矩，m表示参考目标质量，以及Xu，Yv，Nr表示参考目标固有参数。
步骤四、建立误差模型。
将(1)式离散化，取采样时间为T,得到离散状态空间模型。
将实际目标与虚拟目标的轨迹跟踪误差投影到运载体坐标系。
式中，xe＝x-xd和ye＝y-yd分别表示x和y方向误差，ψe＝ψ-ψd表示航向角误差，ue＝u-ud和ve＝v-vd分别表示前向速度和侧向速度误差，re＝r-rd表示角速度误差。
由于跟踪目标中使得ψe趋于零，所以将(5)式简化。
(6)式求导得到误差跟踪模型。
式中，xe，ye，ψe，ue，ve和re为相应的状态误差值，u1-u1d和u2-u2d为对应的输入误差值。
将(7)式离散化，取采样时间为T。
上式中，xe＝[xe,ye,ψe,ue,ve,re]T和ue＝[u1(k)-u1d(k),u2(k)-u2d(k)]T分别表示离散误差状态和输入向量。
步骤五、无人船轨迹跟踪。
根据虚拟参考目标数学模型(4)产生割草型参考轨迹。考虑到割草型图形由直线与半圆分段构成，在设计过程中先给定控制输入，然后不断调整状态值让模型(4)产生由直线和半圆形轨迹组合生成的割草型轨迹。
假设控制时域与预测时域均为N，建立无人船的二次性能指标。
式中，x＝[x,y,ψ,u,v,r]T，u＝[u1,u2]T，xe(k+i|k)＝x(k+i|k)-xd(k+i|k)，ue(k+i|k)＝u(k+i|k)-ud(k+i|k)，分别表示系统(8)在k时刻对k+i时刻状态量和输入量的预测值，Q和R分别表示状态量和输入量的加权矩阵，P为终端加权矩阵。
通过以上步骤，无人船对于割草型轨迹的跟踪转化为求解以下优化问题。
式中，i＝0,…,N-1，x＝[x,y,ψ,u,v,r]T，f(x,u)表示(9)所示模型，umin表示模型(4)中控制输入允许的最小值向量，umax表示控制输入允许的最大值向量，xmin表示模型(4)中状态变量允许的最小值向量，xmax表示状态变量允许的最大值向量，这些约束向量不等式表示每个元素按不等式成立，表示控制输入最优序列，其中第一个元素作为最优解作用于系统(8)。
利用模型预测控制在线滚动优化迭代，实现无人船对虚拟目标割草型轨迹的优化跟踪。
步骤六、水下航行器性能指标设计。
主航行器接收到无人船的状态和位置信息，并将接收到的信息引入到性能指标中，使得主航行器能够跟踪无人船，同时为了保证水下航行器群之间的协同编队还需要引入协同指标。考虑到终端代价函数缺少参考目标值，在设计水下航行器性能指标时，将控制时域与预测时域设计为N-1。
主航行器的二次协同性能指标。
式中，表示在水下航行器中主航行器的邻居节点航行器，表示接收到的无人船的状态信息，表示接收到的邻居节点航行器状态信息，Q1，Qj和P1分别表示各项的加权矩阵。不是邻居节点航行器的直接状态信息，需要进行转换，转换方式由下面的式(15)-(16)实现。
建立其它水下航行器的二次性能指标。
式中，表示水下航行器的邻居节点航行器，表示接收到的邻居节点航行器并做出相应处理的状态信息值，，表示接收到的无人船并做出相应处理的状态信息值，Qij和Pij分别表示各项的加权矩阵。
通过上述过程，水下航行器群的协同跟踪问题转变为求解两个优化问题。
主水下航行器优化问题。
从水下航行器优化问题。
上述过程利用分布式模型预测控制算法优化迭代，解算出的最优序列U*(k)第一个元素作为最优解重新作用于系统。
步骤七、优化过程估计值的更新。
上述算法中，在每一时刻k，无人船跟踪虚拟参考目标割草型轨迹，并将自身状态信息预测值发送给水下航行器中的主航行器，同时所有水下航行器向自身邻居节点航行器发送自身状态信息预测值，最后在线优化解决式(10)，(13)和(14)问题，并将最优控制序列第一个元素作用于系统，在k+1时刻循环上述过程。在实现割草型轨迹编队过程中，水下航行器不能直接使用自身邻居节点航行器发送的位置信息值。需要根据具体要求，将割草型轨迹分为直线和半圆，在直线段引入参考值d，在曲线段根据圆形的圆心以及半径做出相应的转换。
1、直线段。
假设d＝[dx,dy]T，则 转化为：
式中，和分别表示惯性坐标系x和y方向估计值。
2、曲线段。
假设圆心坐标为O(xo,yo)，半径为则 转化为：
式中，i表示水下航行器，表示i的邻居节点水下航行器，，和分别表示惯性坐标系x和y方向估计值。
假设无人船控制时域与预测时域均为N，为了简化终端控制器的设计以及避免水下航行器终端代价函数中缺少k+N时刻的参考目标值，水下航行器控制时域与预测时域设计为N-1。
在k时刻，求解无人船和水下航行器优化问题式(10)，(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到主航行器预测状态值和预测输入值
在k+1时刻，将上述状态信息传输给主航行器，通过转换得到式(11)所示性能指标J1(k)中的和值。
在k时刻，每个水下航行器求解优化问题式(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到传输给邻居节点航行器预测状态值
考虑到缺少终端项参考目标值，需要引入无人船在k时刻对于N-1时刻的预测状态值作为参考值，即：
在k+1时刻，将上述状态信息传输给邻居节点航行器，通过转换得到式(12)所示性能指标Ji(k)中的估计值。
根据上述估计值，通过模型预测算法在线滚动优化求解问题式(10)，(13)和(14)，实现无人船和水下航行器割草型轨迹编队。
本发明的有益效果是：该方法在水下航行器中确定一个主航行器，让无人船与该主航行器通过光缆连接进行通信，主航行器按照水声通信的距离与其他水下航行器进行水声通信。构造无人船与参考目标误差模型，利用模型预测控制方法，设计性能指标，实现无人船轨迹跟踪优化问题；将水下航行器分为主航行器与从航行器两部分，主航行器根据无人船和从航行器状态信息，从航行器根据其余水下航行器状态信息，利用分布式模型预测控制方法，设计性能指标，实现有约束编队优化控制。由于采用模型预测控制算法，通过构造二次性能函数，解决了海洋航行器有约束控制问题；在编队控制方面采用主从分布式预测控制算法，降低系统通信成本，解决了多类型海洋航行器有约束编队优化问题，实用性好。
下面结合附图和具体实施方式对本发明作详细说明。
附图说明
图1是本发明无人船引导水下航行器群的优化割草型编队控制方法的流程图。
图2是本发明方法中编队运动示意图。
图3是本发明方法的仿真效果图。
具体实施方式
参照图1-3。本发明无人船引导水下航行器群的优化割草型编队控制方法具体步骤如下：
(a)建立编队系统中引导无人船的运动学和动力学模型。
上述表达式中，其中x和y分别表示无人船在惯性坐标系中的位置，ψ表示无人船航向角，u表示无人船前向速度，v表示无人船横向速度，r表示其角速度，u1表示无人船前向推力，u2表示无人船偏航力矩，m表示无人船质量，以及Xu，Yv，Nr表示无人船固有参数。
(b)建立水下航行器的运动学和动力学模型。
上述表达式中，i表示每个水下航行器， 其中xi和yi分别表示水下航行器在惯性坐标系中相应位置，ψi表示水下航行器航向角，ui表示水下航行器前向速度，vi表示水下航行器横向速度，ri表示其角速度，u'1i和u'2i分别表示水下航行器前向推力和侧向推力，u'3i表示水下航行器偏航力矩，M表示水下航行器质量，以及X'u，Y'v，N'r表示水下航行器固有参数。
(c)建立参考目标模型。
参考目标与无人船有相同动力特性。
上述表达式中，其中xd和yd分别表示参考目标在惯性坐标系中的位置，ψd表示参考目标航向角，ud表示参考目标前向速度，vd表示参考目标横向速度，rd表示参考目标角速度，u1d表示参考目标前向推力，u2d表示参考目标偏航力矩，m表示参考目标质量，以及Xu，Yv，Nr表示参考目标固有参数。
(d)建立误差模型。
将(1)式离散化，取采样时间为T,得到离散状态空间模型
将实际目标与虚拟目标的轨迹跟踪误差投影到运载体坐标系
上式中xe＝x-xd和ye＝y-yd分别表示x和y方向误差，ψe＝ψ-ψd表示航向角误差，ue＝u-ud和ve＝v-vd分别表示前向速度和侧向速度误差，re＝r-rd表示角速度误差。
由于跟踪目标中使得ψe趋于零，所以将(5)式简化
上式求导得到误差跟踪模型
上式中，xe，ye，ψe，ue，ve和re为相应的状态误差值，u1-u1d和u2-u2d为对应的输入误差值。
将(7)式离散化，取采样时间为T
上式中，xe＝[xe,ye,ψe,ue,ve,re]T和ue＝[u1(k)-u1d(k),u2(k)-u2d(k)]T分别表示离散误差状态和输入向量。
(e)无人船轨迹跟踪。
根据虚拟参考目标数学模型(4)产生割草型参考轨迹。考虑到割草型图形由直线与半圆分段构成，在设计过程中先给定控制输入，然后不断调整状态值让模型(4)产生由直线和半圆形轨迹组合生成的割草型轨迹。
假设控制时域与预测时域均为N，建立无人船的二次性能指标
上式中，x＝[x,y,ψ,u,v,r]T，u＝[u1,u2]T，xe(k+i|k)＝x(k+i|k)-xd(k+i|k)，ue(k+i|k)＝u(k+i|k)-ud(k+i|k)，分别表示系统(8)在k时刻对k+i时刻状态量和输入量的预测值，Q和R分别表示状态量和输入量的加权矩阵，P为终端加权矩阵。
通过建立以上的性能指标，则无人船对于割草型轨迹的跟踪转化为求解以下优化问题。
上式中，i＝0,…,N-1，x＝[x,y,ψ,u,v,r]T，f(x,u)表示(9)所示模型，umin表示模型(4)中控制输入允许的最小值向量，umax表示控制输入允许的最大值向量，xmin表示模型(4)中状态变量允许的最小值向量，xmax表示状态变量允许的最大值向量，这些约束向量不等式表示每个元素按不等式成立，表示控制输入最优序列，其中第一个元素作为最优解作用于系统(8)。
上述算法过程利用模型预测控制在线滚动优化迭代，实现无人船对虚拟目标割草型轨迹的优化跟踪。
(f)水下航行器性能指标设计。
首先主航行器接收到无人船的状态和位置信息，并将接收到的信息引入到性能指标中，使得主航行器能够跟踪无人船，同时为了保证水下航行器群之间的协同编队还需要引入协同指标。考虑到终端代价函数缺少参考目标值，在设计水下航行器性能指标时，将控制时域与预测时域设计为N-1。
主航行器的二次协同性能指标。
上式中，表示在水下航行器中主航行器的邻居节点航行器，表示接收到的无人船的状态信息，表示接收到的邻居节点航行器状态信息，Q1，Qj和P1分别表示各项的加权矩阵。注意到不是邻居节点航行器的直接状态信息，而是需要一定的转换，转换方式由下面的(15)-(16)实现。
建立其它水下航行器的二次性能指标
上式中，表示水下航行器的邻居节点航行器，表示接收到的邻居节点航行器并做出相应处理的状态信息值，，表示接收到的无人船并做出相应处理的状态信息值，Qij和Pij分别表示各项的加权矩阵。
通过上述过程，水下航行器群的协同跟踪问题转变为求解两个优化问题。
主水下航行器优化问题。
其余从水下航行器优化问题。
上述过程利用分布式模型预测控制算法优化迭代，解算出的最优序列U*(k)第一个元素作为最优解重新作用于系统。
(g)优化过程估计值的更新。
上述算法中，在每一时刻k，无人船跟踪虚拟参考目标割草型轨迹，并将自身状态信息预测值发送给水下航行器中的主航行器，同时所有水下航行器向自身邻居节点航行器发送自身状态信息预测值，最后在线优化解决(10)，(13)和(14)问题，并将最优控制序列第一个元素作用于系统，在k+1时刻循环上述过程。在实现割草型轨迹编队过程中，水下航行器不能直接使用自身邻居节点航行器发送的位置信息值。需要根据具体要求，将割草型轨迹分为直线和半圆，在直线段引入参考值d，在曲线段根据圆形的圆心以及半径做出相应的转换。
1、直线段。
假设d＝[dx,dy]T，则转化为
上式中，和分别表示惯性坐标系x和y方向估计值。
2、曲线段。
假设圆心坐标为O(xo,yo)，半径为则转化为
上式中，i表示水下航行器，表示i的邻居节点水下航行器，，和分别表示惯性坐标系x和y方向估计值。
假设无人船控制时域与预测时域均为N，为了简化终端控制器的设计以及避免水下航行器终端代价函数中缺少k+N时刻的参考目标值，水下航行器控制时域与预测时域设计为N-1。
在k时刻，求解无人船和水下航行器优化问题(10)，(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到主航行器预测状态值和预测输入值
在k+1时刻，将上述状态信息传输给主航行器，通过转换得到(11)所示性能指标J1(k)中的和值。
同理在k时刻，每个水下航行器求解优化问题(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到传输给邻居节点航行器预测状态值
考虑到缺少终端项参考目标值，需要引入无人船在k时刻对于N-1时刻的预测状态值作为参考值，即：
在k+1时刻，将上述状态信息传输给邻居节点航行器，通过转换得到(12)所示性能指标Ji(k)中的估计值。
根据上述估计值，通过模型预测算法在线滚动优化求解问题(10)，(13)和(14)，实现无人船和水下航行器割草型轨迹编队。
应用实施例：
以无人船与三个水下航行器编队(Nj＝0,1,2,3)为例说明本发明。
步骤1.根据无人船误差数学模型，建立离散的非线性状态空间表达式：
上式中，xe＝[xe,ye,ψe,ue,ve,re]T和ue＝[u1(k)-u1d(k),u2(k)-u2d(k)]T分别表示离散误差状态和输入向量，其中xd和yd分别表示参考目标在惯性坐标系中的位置，ψd表示参考目标航向角，ud表示参考目标前向速度，vd表示参考目标横向速度，rd表示参考目标角速度，u1d表示参考目标前向推力，u2d表示参考目标偏航力矩，m表示参考目标质量，以及Xu，Yv，Nr表示参考目标固有参数离散，xe＝x-xd和ye＝y-yd分别表示x和y方向误差，ψe＝ψ-ψd表示航向角误差，ue＝u-ud和ve＝v-vd分别表示前向速度和侧向速度误差，re＝r-rd表示角速度误差。周期T取
根据水下航行器数学模型，建立离散的非线性状态空间表达式：
上式中，i表示每个水下航行器， 其中xi和yi分别表示水下航行器在惯性坐标系中相应位置，ψi表示水下航行器航向角，ui表示水下航行器前向速度，vi表示水下航行器横向速度，ri表示其角速度，u'1i和u'2i分别表示水下航行器前向推力和侧向推力，u'3i表示水下航行器偏航力矩，M表示水下航行器质量，以及X'u，Y'v，N'r表示水下航行器固有参数离散周期T取0.1s，
步骤2.建立无人船性能指标：
其中，上式中，x＝[x,y,ψ,u,v,r]T，u＝[u1,u2]T，xe(k+i|k)＝x(k+i|k)-xd(k+i|k)，ue(k+i|k)＝u(k+i|k)-ud(k+i|k)，分别表示在k时刻对k+i时刻状态量和输入量的预测值，Q和R分别表示状态量和输入量的加权矩阵，P为终端加权矩阵k＝0,1,2...N,Q＝diag([100,100,100,0.1,0.1,0.1]),R＝diag([0.001,0.001]),P＝diag([100,100,100,0.1,0.1,0.1]),无人船状态初始值分别为x(0)＝[-1,0,0,2,0,0]T,u(0)＝[0,0]T,参考目标状态初值xd0＝[0,0,0,2,0,0]T,参考目标输入ud(k+i|k)＝[1.445,0]T。
控制变量满足以下约束条件：
-4<u(k+i|k)<4,-4<v(k+i|k)<4，-pi/2<r(k+i|k)<pi/2 (4)
上式中u(k+i|k)，v(k+i|k)和r(k+i|k)分别表示无人船前向速度，侧向速度和偏航角速度。
假定预测时域N＝6,通过求解优化问题得到最优控制序列将控制序列第一个解作用于系统，完成一次求解过程。
步骤3.建立主航行器性能指标：
上式中，表示主航行器的邻居节点航行器表示接收到的无人船的状态信息，表示接收到的邻居节点航行器状态信息，Q1，Qj和P1分别表示各项的加权矩阵。P1＝diag([10,10,10,0,0,0]),Q2＝diag([0.05,0.05,0.05,0,0,0]),Q3＝diag([0.05,0.05,0.05,0,0,0]),初始值x1(0)＝[-2,1,0,0,0,0]T,无人船和其它水下航行器预测状态初值为
控制变量满足以下约束条件：
-4<u1(k+i|k)<4,-4<v1(k+i|k)<4，-pi/2<r1(k+i|k)<pi/2 (6)
上式中u1(k+i|k)，v1(k+i|k)和r1(k+i|k)分别表示主航行器前向速度，侧向速度和偏航角速度。
假定预测时域N＝6,通过求解优化问题得到最优控制序列将控制序列第一个解作用于系统，完成一次求解过程。
建立其它水下航行器性能指标：
其中，i＝2,3，表示水下航行器的邻居节点航行器或 表示接收到的邻居节点航行器并做出相应处理的状态信息值，，表示接收到的无人船并做出相应处理的状态信息值，Qij和Pij分别表示各项的加权矩阵。Pij＝diag([10,10,10,0,0,0]),Q21＝diag([30,30,30,0,0,0]),Q23＝diag([0.05,0.05,0.05,0,0,0]),Q31＝diag([30,30,30,0,0,0]),Q32＝diag([0.05,0.05,0.05,0,0,0]),初始状态x2(0)＝[-3,1,0,0,0,0]T,x3(0)＝[-2.5,-1,0,0,0,0]T,邻居节点水下航行器预测状态初值为
控制变量满足以下约束条件：
-4<ui(k+i|k)<4,-4<vi(k+i|k)<4，-pi/2<ri(k+i|k)<pi/2 (8)
上式中ui(k+i|k)，vi(k+i|k)和ri(k+i|k)分别表示其它水下航行器前向速度，侧向速度和偏航角速度。
假定预测时域N＝6,通过求解优化问题得到最优控制序列将控制序列第一个解作用于系统，完成一次求解过程。
步骤4.优化过程估计值的更新：
根据仿真要求对邻居节点的位置估计值进行转换，转换过程分为直线段和曲线段。
1、直线段。
对于J1(k)中转换示例如下：
对于J2(k)中转换示例如下：
对于J3(k)中给出一个转换示例如下：
以上给出直线段的转换示例，在环形割草型轨迹不同的直线段，通过以上方法实现位置信息的转化。
2、曲线段。
假定O(xo,yo)＝(7,2)，R1＝2，R2＝1，R3＝3。
对于J1(k)中给出转换示例如下：
对于J2(k)中给出转换示例如下：
对于J3(k)中给出转换示例如下：
以上给出了曲线段的转换示例，在环形割草型轨迹不同的曲线段段，以上转换会受到不同圆心和半径影响。
以上转换中的估计值与的更新分如下两部分：
在k时刻，无人船传输给主航行器的估计值为：
在k时刻，水下航行器之间相互传输的估计值为：
步骤5.k+1时刻，令k＝k+1按照上述步骤将估计值传输给其余水下航行器，各水下航行器得到估计值按照步骤2至步骤4过程循环求解优化问题，使得环形割草型轨迹编队得以实现。
步骤6.当k等于设定循环次数，编队优化循环过程停止。
Claims (1)
1.一种无人船引导水下航行器群的优化割草型编队控制方法，其特征在于包括以下步骤：
步骤一、建立编队系统中引导无人船的运动学和动力学模型；
式中，d11＝-Xu，d22＝-Yv，d33＝-Nr；x和y分别表示无人船在惯性坐标系中的位置，ψ表示无人船航向角，u表示无人船前向速度，v表示无人船横向速度，r表示其角速度，u1表示无人船前向推力，u2表示无人船偏航力矩，m表示无人船质量，以及Xu，Yv，Nr表示无人船固有参数；
步骤二、建立水下航行器的运动学和动力学模型；
式中，i表示每个水下航行器，D11＝-X′u，D22＝-Y′v，D33＝-N′r；xi和yi分别表示水下航行器在惯性坐标系中相应位置，ψi表示水下航行器航向角，ui表示水下航行器前向速度，vi表示水下航行器横向速度，ri表示其角速度，u′1i和u′2i分别表示水下航行器前向推力和侧向推力，u′3i表示水下航行器偏航力矩，M表示水下航行器质量，以及X′u，Y′v，N′r表示水下航行器固有参数；
步骤三、建立参考目标模型；
参考目标与无人船有相同动力特性；
式中，d11＝-Xu，d22＝-Yv，d33＝-Nr；其中xd和yd分别表示参考目标在惯性坐标系中的位置，ψd表示参考目标航向角，ud表示参考目标前向速度，vd表示参考目标横向速度，rd表示参考目标角速度，u1d表示参考目标前向推力，u2d表示参考目标偏航力矩，m表示参考目标质量，以及Xu，Yv，Nr表示参考目标固有参数；
步骤四、建立误差模型；
将(1)式离散化，取采样时间为T,得到离散状态空间模型；
将实际目标与虚拟目标的轨迹跟踪误差投影到运载体坐标系；
式中，xe＝x-xd和ye＝y-yd分别表示x和y方向误差，ψe＝ψ-ψd表示航向角误差，ue＝u-ud和ve＝v-vd分别表示前向速度和侧向速度误差，re＝r-rd表示角速度误差；
由于跟踪目标中使得ψe趋于零，所以将(5)式简化；
(6)式求导得到误差跟踪模型；
式中，xe，ye，ψe，ue，ve和re为相应的状态误差值，u1-u1d和u2-u2d为对应的输入误差值；
将(7)式离散化，取采样时间为T；
上式中，xe＝[xe,ye,ψe,ue,ve,re]T和ue＝[u1(k)-u1d(k),u2(k)-u2d(k)]T分别表示离散误差状态和输入向量；
步骤五、无人船轨迹跟踪；
根据虚拟参考目标数学模型(4)产生割草型参考轨迹；考虑到割草型图形由直线与半圆分段构成，在设计过程中先给定控制输入，然后不断调整状态值让模型(4)产生由直线和半圆形轨迹组合生成的割草型轨迹；
假设控制时域与预测时域均为N，建立无人船的二次性能指标；
式中，x＝[x,y,ψ,u,v,r]T，u＝[u1,u2]T，xe(k+i|k)＝x(k+i|k)-xd(k+i|k)，ue(k+i|k)＝u(k+i|k)-ud(k+i|k)，分别表示系统(8)在k时刻对k+i时刻状态量和输入量的预测值，Q和R分别表示状态量和输入量的加权矩阵，P为终端加权矩阵；
通过以上步骤，无人船对于割草型轨迹的跟踪转化为求解以下优化问题；
式中，i＝0,…,N-1，x＝[x,y,ψ,u,v,r]T，f(x,u)表示(9)所示模型，umin表示模型(4)中控制输入允许的最小值向量，umax表示控制输入允许的最大值向量，xmin表示模型(4)中状态变量允许的最小值向量，xmax表示状态变量允许的最大值向量，这些约束向量不等式表示每个元素按不等式成立，表示控制输入最优序列，其中第一个元素作为最优解作用于系统(8)；
利用模型预测控制在线滚动优化迭代，实现无人船对虚拟目标割草型轨迹的优化跟踪；
步骤六、水下航行器性能指标设计；
主航行器接收到无人船的状态和位置信息，并将接收到的信息引入到性能指标中，使得主航行器能够跟踪无人船，同时为了保证水下航行器群之间的协同编队还需要引入协同指标；考虑到终端代价函数缺少参考目标值，在设计水下航行器性能指标时，将控制时域与预测时域设计为N-1；
主航行器的二次协同性能指标；
式中，表示在水下航行器中主航行器的邻居节点航行器，表示接收到的无人船的状态信息，表示接收到的邻居节点航行器状态信息，Q1，Qj和P1分别表示各项的加权矩阵；不是邻居节点航行器的直接状态信息，需要进行转换，转换方式由下面的式(15)-(16)实现；
建立其它水下航行器的二次性能指标；
式中，表示水下航行器的邻居节点航行器，表示接收到的邻居节点航行器并做出相应处理的状态信息值，，表示接收到的无人船并做出相应处理的状态信息值，Qij和Pij分别表示各项的加权矩阵；
通过上述过程，水下航行器群的协同跟踪问题转变为求解两个优化问题；
主水下航行器优化问题；
从水下航行器优化问题；
上述过程利用分布式模型预测控制算法优化迭代，解算出的最优序列U*(k)第一个元素作为最优解重新作用于系统；
步骤七、优化过程估计值的更新；
上述算法中，在每一时刻k，无人船跟踪虚拟参考目标割草型轨迹，并将自身状态信息预测值发送给水下航行器中的主航行器，同时所有水下航行器向自身邻居节点航行器发送自身状态信息预测值，最后在线优化解决式(10)，(13)和(14)问题，并将最优控制序列第一个元素作用于系统，在k+1时刻循环上述过程；在实现割草型轨迹编队过程中，水下航行器不能直接使用自身邻居节点航行器发送的位置信息值；需要根据具体要求，将割草型轨迹分为直线和半圆，在直线段引入参考值d，在曲线段根据圆形的圆心以及半径做出相应的转换；
1、直线段；
假设d＝[dx,dy]T，则转化为：
式中，和分别表示惯性坐标系x和y方向估计值；
2、曲线段；
假设圆心坐标为O(xo,yo)，半径为则转化为：
式中，i表示水下航行器，表示i的邻居节点水下航行器，，和分别表示惯性坐标系x和y方向估计值；
假设无人船控制时域与预测时域均为N，为了简化终端控制器的设计以及避免水下航行器终端代价函数中缺少k+N时刻的参考目标值，水下航行器控制时域与预测时域设计为N-1；
在k时刻，求解无人船和水下航行器优化问题式(10)，(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到主航行器预测状态值和预测输入值
在k+1时刻，将上述状态信息传输给主航行器，通过转换得到式(11)所示性能指标J1(k)中的和值；
在k时刻，每个水下航行器求解优化问题式(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到传输给邻居节点航行器预测状态值
考虑到缺少终端项参考目标值，需要引入无人船在k时刻对于N-1时刻的预测状态值作为参考值，即：
在k+1时刻，将上述状态信息传输给邻居节点航行器，通过转换得到式(12)所示性能指标Ji(k)中的估计值；
根据上述估计值，通过模型预测算法在线滚动优化求解问题式(10)，(13)和(14)，实现无人船和水下航行器割草型轨迹编队。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811621325.6A CN109521797B (zh) | 2018-12-28 | 2018-12-28 | 无人船引导水下航行器群的优化割草型编队控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811621325.6A CN109521797B (zh) | 2018-12-28 | 2018-12-28 | 无人船引导水下航行器群的优化割草型编队控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109521797A true CN109521797A (zh) | 2019-03-26 |
| CN109521797B CN109521797B (zh) | 2020-10-27 |
Family
ID=65797697
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811621325.6A Active CN109521797B (zh) | 2018-12-28 | 2018-12-28 | 无人船引导水下航行器群的优化割草型编队控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109521797B (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110780668A (zh) * | 2019-04-09 | 2020-02-11 | 北京航空航天大学 | 一种多无人艇的分布式编队合围跟踪控制方法及系统 |
| CN110908393A (zh) * | 2019-10-31 | 2020-03-24 | 中国矿业大学 | 基于探通一体化的水下无人航行器编队协同方法 |
| CN110928310A (zh) * | 2019-12-12 | 2020-03-27 | 大连海事大学 | 一种无人船领航跟随固定时间编队控制方法 |
| CN111506114A (zh) * | 2020-05-25 | 2020-08-07 | 北京理工大学 | 一种飞行器编队控制方法 |
| CN111665719A (zh) * | 2020-06-11 | 2020-09-15 | 大连海事大学 | 一种具有时效性与稳定性的补给船同步控制算法 |
| CN112632876A (zh) * | 2020-08-26 | 2021-04-09 | 西北工业大学 | 一种基于dmhe和dmpc的无人船协同目标跟踪控制方法 |
| CN112684809A (zh) * | 2020-12-14 | 2021-04-20 | 西北工业大学 | 一种基于数据驱动的水下航行器目标跟踪控制方法 |
| CN112947503A (zh) * | 2021-03-10 | 2021-06-11 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
| CN113050666A (zh) * | 2021-03-26 | 2021-06-29 | 湖南大学 | 水下自主航行器深度和纵向解耦控制方法及系统 |
| CN113064434A (zh) * | 2021-03-27 | 2021-07-02 | 西北工业大学 | 一种基于主从式编队的水面目标检测与跟踪控制方法 |
| CN113805587A (zh) * | 2021-09-17 | 2021-12-17 | 中国人民解放军国防科技大学 | 多无人车分布式编队控制方法、装置和设备 |
| CN113885534A (zh) * | 2021-11-19 | 2022-01-04 | 江苏科技大学 | 一种基于智能预测控制的水面无人船路径跟踪方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101484514B1 (ko) * | 2013-06-17 | 2015-01-21 | 한국과학기술원 | 로봇 편대 형성의 안정성이 보장되는 입자 군집 최적화 기반 예측 제어 방법，시스템 및 서버 |
| CN105974930A (zh) * | 2016-04-18 | 2016-09-28 | 哈尔滨工程大学 | 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法 |
| CN108388269A (zh) * | 2018-03-17 | 2018-08-10 | 青岛理工大学 | 基于四旋翼飞行器的无人机编队飞行控制方法 |
| CN108427414A (zh) * | 2018-03-31 | 2018-08-21 | 西北工业大学 | 一种自主水下航行器水平面自适应轨迹跟踪控制方法 |
| CN108803632A (zh) * | 2018-09-19 | 2018-11-13 | 哈尔滨工程大学 | 基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法 |
| CN108873894A (zh) * | 2018-06-11 | 2018-11-23 | 上海大学 | 一种基于多无人艇的目标跟踪协同控制系统和方法 |
| CN108897215A (zh) * | 2018-06-25 | 2018-11-27 | 西北工业大学 | 基于分布式模型预测控制的多海洋机器人协同环形扫描方法 |
-
2018
- 2018-12-28 CN CN201811621325.6A patent/CN109521797B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101484514B1 (ko) * | 2013-06-17 | 2015-01-21 | 한국과학기술원 | 로봇 편대 형성의 안정성이 보장되는 입자 군집 최적화 기반 예측 제어 방법，시스템 및 서버 |
| CN105974930A (zh) * | 2016-04-18 | 2016-09-28 | 哈尔滨工程大学 | 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法 |
| CN108388269A (zh) * | 2018-03-17 | 2018-08-10 | 青岛理工大学 | 基于四旋翼飞行器的无人机编队飞行控制方法 |
| CN108427414A (zh) * | 2018-03-31 | 2018-08-21 | 西北工业大学 | 一种自主水下航行器水平面自适应轨迹跟踪控制方法 |
| CN108873894A (zh) * | 2018-06-11 | 2018-11-23 | 上海大学 | 一种基于多无人艇的目标跟踪协同控制系统和方法 |
| CN108897215A (zh) * | 2018-06-25 | 2018-11-27 | 西北工业大学 | 基于分布式模型预测控制的多海洋机器人协同环形扫描方法 |
| CN108803632A (zh) * | 2018-09-19 | 2018-11-13 | 哈尔滨工程大学 | 基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| HUIPING LI等: "Adaptive Self-Triggered Model Predictive Control of Discrete-Time Linear Systems", 《2017 IEEE 56TH ANNUAL CONFERENCE ON DECISION AND CONTROL (CDC)》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110780668A (zh) * | 2019-04-09 | 2020-02-11 | 北京航空航天大学 | 一种多无人艇的分布式编队合围跟踪控制方法及系统 |
| CN110908393A (zh) * | 2019-10-31 | 2020-03-24 | 中国矿业大学 | 基于探通一体化的水下无人航行器编队协同方法 |
| CN110928310B (zh) * | 2019-12-12 | 2022-05-13 | 大连海事大学 | 一种无人船领航跟随固定时间编队控制方法 |
| CN110928310A (zh) * | 2019-12-12 | 2020-03-27 | 大连海事大学 | 一种无人船领航跟随固定时间编队控制方法 |
| CN111506114A (zh) * | 2020-05-25 | 2020-08-07 | 北京理工大学 | 一种飞行器编队控制方法 |
| CN111665719A (zh) * | 2020-06-11 | 2020-09-15 | 大连海事大学 | 一种具有时效性与稳定性的补给船同步控制算法 |
| CN111665719B (zh) * | 2020-06-11 | 2022-11-22 | 大连海事大学 | 一种具有时效性与稳定性的补给船同步控制算法 |
| CN112632876A (zh) * | 2020-08-26 | 2021-04-09 | 西北工业大学 | 一种基于dmhe和dmpc的无人船协同目标跟踪控制方法 |
| CN112684809A (zh) * | 2020-12-14 | 2021-04-20 | 西北工业大学 | 一种基于数据驱动的水下航行器目标跟踪控制方法 |
| CN112947503A (zh) * | 2021-03-10 | 2021-06-11 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
| CN112947503B (zh) * | 2021-03-10 | 2023-09-05 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
| CN113050666A (zh) * | 2021-03-26 | 2021-06-29 | 湖南大学 | 水下自主航行器深度和纵向解耦控制方法及系统 |
| CN113064434A (zh) * | 2021-03-27 | 2021-07-02 | 西北工业大学 | 一种基于主从式编队的水面目标检测与跟踪控制方法 |
| CN113064434B (zh) * | 2021-03-27 | 2022-07-12 | 西北工业大学 | 一种基于主从式编队的水面目标检测与跟踪控制方法 |
| CN113805587A (zh) * | 2021-09-17 | 2021-12-17 | 中国人民解放军国防科技大学 | 多无人车分布式编队控制方法、装置和设备 |
| CN113805587B (zh) * | 2021-09-17 | 2023-08-15 | 中国人民解放军国防科技大学 | 多无人车分布式编队控制方法、装置和设备 |
| CN113885534A (zh) * | 2021-11-19 | 2022-01-04 | 江苏科技大学 | 一种基于智能预测控制的水面无人船路径跟踪方法 |
| CN113885534B (zh) * | 2021-11-19 | 2023-11-21 | 江苏科技大学 | 一种基于智能预测控制的水面无人船路径跟踪方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109521797B (zh) | 2020-10-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109521797A (zh) | 无人船引导水下航行器群的优化割草型编队控制方法 | |
| Zhang et al. | Collision-avoidance navigation systems for Maritime Autonomous Surface Ships: A state of the art survey | |
| CN108200175B (zh) | 基于协同云控制的多无人船编队协同控制系统及方法 | |
| Mahmoudzadeh et al. | UUV’s hierarchical DE-based motion planning in a semi dynamic underwater wireless sensor network | |
| Breivik et al. | Ship formation control: A guided leader-follower approach | |
| CN109032136B (zh) | 基于主从分布式模型预测控制的欠驱动多无人船编队跟踪方法 | |
| Cao et al. | Toward optimal rendezvous of multiple underwater gliders: 3D path planning with combined sawtooth and spiral motion | |
| CN108594846A (zh) | 一种障碍环境下多auv编队队形优化控制方法 | |
| CN109799818B (zh) | 多参数化路径导引的无人船协同操纵控制器的设计方法 | |
| CN110609556A (zh) | 一种基于los导航法的多无人艇协同控制方法 | |
| Liao et al. | The intelligent control system and experiments for an unmanned wave glider | |
| Saska et al. | Navigation, localization and stabilization of formations of unmanned aerial and ground vehicles | |
| Wang et al. | A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments | |
| CN108897215B (zh) | 基于分布式模型预测控制的多海洋机器人协同环形扫描方法 | |
| Sousselier et al. | Line formation algorithm in a swarm of reactive robots constrained by underwater environment | |
| CN109141421A (zh) | 欠驱动auv水下动态目标跟踪中的期望路径构造方法 | |
| Cai et al. | Cooperative Artificial Intelligence for underwater robotic swarm | |
| CN112051845A (zh) | 一种面向无人拖航作业的分布式集群控制系统及其方法 | |
| Zeng et al. | Path planning for rendezvous of multiple AUVs operating in a variable ocean | |
| Nielsen et al. | Cooperative rendezvous and docking for underwater robots using model predictive control and dual decomposition | |
| Maevskiy et al. | Development of a hybrid method for planning the movement of a group of marine robotic complexes in a priori unknown environment with obstacles | |
| Bykova et al. | A dispatcher for a multi-agent control system of an autonomous underwater vehicle: structure, algorithms, and simulation results | |
| Kang et al. | Fuzzy logic based behavior fusion for multi-AUV formation keeping in uncertain ocean environment | |
| Wu et al. | UAV formation control based on consistency | |
| Okamoto et al. | Robust control of a platoon of underwater autonomous vehicles |
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 |