[发明专利]无人船引导水下航行器群的优化割草型编队控制方法

摘要

本发明公开了一种无人船引导水下航行器群的优化割草型编队控制方法，用于解决现有多海洋航行器编队控制方法实用性差的技术问题。技术方案是在水下航行器中确定一个主航行器，让无人船与该主航行器通过光缆连接进行通信，主航行器按照水声通信距离与其他水下航行器进行水声通信。构造无人船与参考目标误差模型，利用模型预测控制方法，设计性能指标，实现无人船轨迹跟踪优化问题；将水下航行器分为主航行器与从航行器两部分，主航行器根据无人船和从航行器状态信息，从航行器根据其余水下航行器状态信息，利用分布式模型预测控制方法，实现有约束编队优化控制。由于采用分布式模型预测控制方法，解决了多海洋航行器有约束编队优化问题，实用性好。

主权项

1.一种无人船引导水下航行器群的优化割草型编队控制方法，其特征在于包括以下步骤：步骤一、建立编队系统中引导无人船的运动学和动力学模型；式中，d₁₁＝‑X_u，d₂₂＝‑Y_v，d₃₃＝‑N_r；x和y分别表示无人船在惯性坐标系中的位置，ψ表示无人船航向角，u表示无人船前向速度，v表示无人船横向速度，r表示其角速度，u₁表示无人船前向推力，u₂表示无人船偏航力矩，m表示无人船质量，以及X_u，Y_v，N_r表示无人船固有参数；步骤二、建立水下航行器的运动学和动力学模型；式中，i表示每个水下航行器，D₁₁＝‑X′_u，D₂₂＝‑Y′_v，D₃₃＝‑N′_r；x_i和y_i分别表示水下航行器在惯性坐标系中相应位置，ψ_i表示水下航行器航向角，u_i表示水下航行器前向速度，v_i表示水下航行器横向速度，r_i表示其角速度，u′_1i和u′_2i分别表示水下航行器前向推力和侧向推力，u′_3i表示水下航行器偏航力矩，M表示水下航行器质量，以及X′_u，Y′_v，N′_r表示水下航行器固有参数；步骤三、建立参考目标模型；参考目标与无人船有相同动力特性；式中，d₁₁＝‑X_u，d₂₂＝‑Y_v，d₃₃＝‑N_r；其中x_d和y_d分别表示参考目标在惯性坐标系中的位置，ψ_d表示参考目标航向角，u_d表示参考目标前向速度，v_d表示参考目标横向速度，r_d表示参考目标角速度，u_1d表示参考目标前向推力，u_2d表示参考目标偏航力矩，m表示参考目标质量，以及X_u，Y_v，N_r表示参考目标固有参数；步骤四、建立误差模型；将(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)所示模型，u_min表示模型(4)中控制输入允许的最小值向量，u_max表示控制输入允许的最大值向量，x_min表示模型(4)中状态变量允许的最小值向量，x_max表示状态变量允许的最大值向量，这些约束向量不等式表示每个元素按不等式成立，表示控制输入最优序列，其中第一个元素作为最优解作用于系统(8)；利用模型预测控制在线滚动优化迭代，实现无人船对虚拟目标割草型轨迹的优化跟踪；步骤六、水下航行器性能指标设计；主航行器接收到无人船的状态和位置信息，并将接收到的信息引入到性能指标中，使得主航行器能够跟踪无人船，同时为了保证水下航行器群之间的协同编队还需要引入协同指标；考虑到终端代价函数缺少参考目标值，在设计水下航行器性能指标时，将控制时域与预测时域设计为N‑1；主航行器的二次协同性能指标；式中，表示在水下航行器中主航行器的邻居节点航行器，表示接收到的无人船的状态信息，表示接收到的邻居节点航行器状态信息，Q₁，Q_j和P₁分别表示各项的加权矩阵；不是邻居节点航行器的直接状态信息，需要进行转换，转换方式由下面的式(15)‑(16)实现；建立其它水下航行器的二次性能指标；式中，表示水下航行器的邻居节点航行器，表示接收到的邻居节点航行器并做出相应处理的状态信息值，，表示接收到的无人船并做出相应处理的状态信息值，Q_ij和P_ij分别表示各项的加权矩阵；通过上述过程，水下航行器群的协同跟踪问题转变为求解两个优化问题；主水下航行器优化问题；从水下航行器优化问题；上述过程利用分布式模型预测控制算法优化迭代，解算出的最优序列U*(k)第一个元素作为最优解重新作用于系统；步骤七、优化过程估计值的更新；上述算法中，在每一时刻k，无人船跟踪虚拟参考目标割草型轨迹，并将自身状态信息预测值发送给水下航行器中的主航行器，同时所有水下航行器向自身邻居节点航行器发送自身状态信息预测值，最后在线优化解决式(10)，(13)和(14)问题，并将最优控制序列第一个元素作用于系统，在k+1时刻循环上述过程；在实现割草型轨迹编队过程中，水下航行器不能直接使用自身邻居节点航行器发送的位置信息值；需要根据具体要求，将割草型轨迹分为直线和半圆，在直线段引入参考值d，在曲线段根据圆形的圆心以及半径做出相应的转换；1、直线段；假设d＝[d_x,d_y]^T，则转化为：式中，和分别表示惯性坐标系x和y方向估计值；2、曲线段；假设圆心坐标为O(x_o,y_o)，半径为则转化为：式中，i表示水下航行器，表示i的邻居节点水下航行器，，和分别表示惯性坐标系x和y方向估计值；假设无人船控制时域与预测时域均为N，为了简化终端控制器的设计以及避免水下航行器终端代价函数中缺少k+N时刻的参考目标值，水下航行器控制时域与预测时域设计为N‑1；在k时刻，求解无人船和水下航行器优化问题式(10)，(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到主航行器预测状态值和预测输入值在k+1时刻，将上述状态信息传输给主航行器，通过转换得到式(11)所示性能指标J₁(k)中的和值；在k时刻，每个水下航行器求解优化问题式(13)和(14)得到最优控制序列和状态值，再通过预测值更新得到传输给邻居节点航行器预测状态值考虑到缺少终端项参考目标值，需要引入无人船在k时刻对于N‑1时刻的预测状态值作为参考值，即：在k+1时刻，将上述状态信息传输给邻居节点航行器，通过转换得到式(12)所示性能指标Ji(k)中的估计值；根据上述估计值，通过模型预测算法在线滚动优化求解问题式(10)，(13)和(14)，实现无人船和水下航行器割草型轨迹编队。