[发明专利]一种基于DMHE和DMPC的无人船协同目标跟踪控制方法

权利要求书

1.一种基于DMHE和DMPC的无人船协同目标跟踪控制方法，其特征在于步骤如下：

步骤1：建立编队系统中无人船i的运动学和动力学模型：

其中，d₁₁＝-X_u，d₂₂＝-Y_v，d₃₃＝-N_r；其中x_i和y_i分别表示无人船i在惯性坐标系中相应位置，ψ_i表示无人船i航向角，u_i表示无人船i前向速度，v_i表示无人船i横向速度，r_i表示其角速度，u_i1表示无人船i前向推力，u_i2表示无人船i偏航力矩，m表示其质量，以及X_u，Y_v，N_r分别是与附加质量和阻尼力有关的流体动力参数；

建立目标运动模型，选取匀加速运动模型：

其中，x、和分别表示目标x轴方向位置、速度和加速度，y、和分别表示目标y轴方向位置、速度和加速度，w(t)表示零均值、协方差为Q的高斯白噪声；

将式(2)离散化后，运动模型为：

x_r(k+1)＝Fx_r(k)+Gw(k) (3)

其中，T是采样周期，矩阵F和G具体形式如下：

在混合坐标系下建立量测方程，具体形式如下：

z_i(k)＝h(x_r(k))+v_i(k) (5)

其中，z_i(k)＝[r_i(k),θ_i(k)]^T，r_i(k)，θ_i(k)分别表示k时刻无人船i量测的目标距离和方位角，h(x)＝[h_ir,h_iθ]^T，v_i(k)表示表示零均值、协方差为R的高斯白噪声；h_ir和h_iθ具体形式如下：

步骤2：单无人船的目标估计问题：

首先，考虑利用当前时刻T之前所有的量测信息，设计全信息MHE算法；将(3)和(5)式整理成如下形式：

满足约束条件：

其中，约束集合χ、W和均为紧闭凸集；

令建立如下优化问题：

其中，P₀表示初始状态x_r,0的协方差矩阵，是阶段代价项；

通过求解优化问题(10)，可以得到T时刻估计值在下一量测时刻令T＝T+1，同时增加量测值z_i,T+1，重复求解扩大一个维度的优化问题(10)，完成算法循环；

接着设计有限时域估计算法，将全信息估计问题转化为固定时域为N的估计问题和到达代价设计问题；建立如下优化问题：

其中，Θ_T-N(x_r,T-N)为到达代价函数，定义为：

(12)式即：利用前向动态规划原理，将全信息MHE问题转化为当采样时刻k∈[T-N,...T-1]时的固定时域优化问题，通过固定优化问题得到初始最优估计值那么当k∈[0,...,T-N-1]时，如果存在干扰序列{w₀,w₁,...,w_T-N-1}可以将系统从初始点在满足约束条件下导引到且满足代价函数V_T-N最小；由于到达代价难以直接计算，采用无迹卡尔曼滤波算法进行估计，所述的无迹卡尔曼滤波算法如下：

UT变换：考虑n维变量x_r,k，其均值为协方差为P_x,k，传递函数x_r,k+1＝f(x_r,k)；根据均值和方差得到sigma点{χ_i:i＝0,...,2n}、均值的权值W_i^m和协方差的权值W_i^c；通过比例修正后，χ_i、W_i^m和W_i^c具体形式如下：

其中，参数λ＝α²(n+κ)-n，κ是调节参数通常为0或3-n，α是一个较小正数，β表示先验分布因子，将sigma点通过函数x_r,k+1＝f(x_r,k)进行非线性传递，得到变换后集合

预测过程：

更新过程：

通过上述过程可以得到估计状态协方差矩阵P_x,k，下面给出到达代价函数的近似函数：

其中，Φ_T是到达代价的常值部分；

建立优化问题如下：

其中，代价函数为：

基于无迹卡尔曼滤波的MHE算法步骤如下：

(1)初始化：给定Q、R和P₀加权矩阵，初始值有限时域长度N，初始时刻T＝1；

(2)当时刻T≤N时，求解全信息MHE优化问题(10)，得到估计值

(3)当时刻TN时，求解基于无迹卡尔曼滤波的MHE优化问题(17)，得到估计值

(4)由(13)-(15)式计算状态协方差矩阵P_x,T-N，得出到达代价Θ_T-N(x_r,T-N)的近似函数；

(5)在下一时刻，令T＝T+1，同时增加量测值z_i,T+1，返回步骤(2)，重复上述过程；

步骤3：多无人船协同目标跟踪：

无人船i接收邻居节点无人船对目标位置的估计信息，根据单无人船目标估计中对到达代价Θ_T-N(x_r,T-N)以及阶段代价函数的构造，建立T时刻无人船i优化问题的指标函数为：

其中，p_r,k＝[x_r,k,y_r,k]^T表示k时刻需要求解的x_r,k的位置变量，也就是状态信息，它可以写成由变量x_r,T-N和w_k组成的形式，是T-1时刻的一系列估计值，N表示MHE算法估计时域，是对到达代价Θ_T-N(x_r,T-N)的简化，考虑(16)式中Φ_T是常值，在优化问题的求解过程中可以省略，是基于无人船状态信息的目标协同项，A是给定加权矩阵；Q和R分别表示过程噪声矩阵和量测噪声矩阵，P_x,T-N为通过无迹卡尔曼滤波得到二点协方差矩阵；

基于DMHE的目标跟踪算法步骤如下：

(1)初始化：给定Q、R和P₀加权矩阵，初始值有限时域长度N，初始时刻T＝1；

(2)当时刻T≤N时，求解全信息MHE优化问题(10)，得到估计值

(3)更新并保存得到的估计状态序列

(4)当时刻TN时，接收邻居节点的信息求解基于DMHE的目标跟踪优化问题(19)，得到估计值

(5)由(13)-(15)式更新协方差矩阵P_x,T-N，得出到达代价Θ_T-N(x_r,T-N)的近似函数；

(6)在下一时刻，令T＝T+1，同时增加量测值z_i,T+1，返回步骤(2)，重复上述过程；

步骤4：多无人船协同跟踪控制：

对于每个无人船i假定它与邻居节点的通信网络拓扑是无向连通图结构，定义表示所有无人船的集合，表示无人船i的邻居集；则在每一采样时刻t_k，无人船i接收邻居节点的状态信息，假设信息传输没有延迟和中断，通过求解关于目标状态和邻居节点状态的代价函数，得到优化控制输入u_i，并实现如下协同控制目标：

(1)当t→∞时，p_r(t)-p_i(t)→d_ir；

(2)当t→∞时，p_i(t)-p_j(t)→d_ij,

其中p_i＝[x_i,y_i]^T和p_r＝[x_r,y_r]^T分别表示无人船i和目标的位置变量；

根据上述协同控制目标描述，建立如下的指标函数：

其中，L_i(τ；t_k)表示无人船i目标跟踪代价函数，G_i(τ；t_k)表示无人船i协同代价函数，E_i(·)为终端代价函数，T_P表示MPC算法预测时域；

函数L_i(τ；t_k)、G_i(τ；t_k)和E_i(x_ir(t_k+T_P；t_k)具体形式如下：

其中，p_ir(t)＝p_r(t)-p_i(t)-d_ir表示无人船i与目标的位置跟踪误差，p_ij(t)＝p_i(t)-p_j(t)-d_ij表示无人船i与邻居节点j的位置跟踪误差；Q_i，Q_ij和P_i分别表示各项的加权矩阵；

无人船i每一采样时刻t_k求解的优化问题如下所示：

其中，χ_i、和Ω_i分别为状态约束，控制约束和终端约束；

求解优化问题(24)，将控制序列的第一个元素作用于系统；

步骤5：优化过程估计值更新：

由于在每一采样时刻t_k，各无人船需要同步求解优化问题，因此对于无人船i来说，并不能获得邻居节点j的真实位置预测轨迹；为了解决这一问题，邻居节点j需要在求解优化问题之前向无人船i发送假设位置预测轨迹，首先下面的说明：

表示无人船i的真实预测控制输入，它的序列第一个元素作用于系统；

表示无人船i的假设预测控制输入，用于生成假设位置预测轨迹

利用上述控制输入和系统模型(1)式得到真实位置预测轨迹和假设位置预测轨迹当τ∈[t_k,t_k+1)时，将真实预测输入作用于系统，得到真实位置预测轨迹

假设控制输入设计如下：

其中，κ_i(p_ir(τ:t_k-1))表示终端控制器，在终端控制器中的，p_ir(τ:t_k-1)的初始值满足

通过假设预测控制输入和系统模型(1)，生成的假设位置预测轨迹如下：

其中，表示终端域中的假设位置预测轨迹，满足如下的形式：

根据上述分析，可将协同代价函数G_i(τ；t_k)中的p_ij(τ；t_k)通过代替求解；

此外，在代价函数L_i(τ；t_k)中，需要知道当τ∈[t_k,t_k+T_p]时，目标的位置大小p_r(τ；t_k)；而根据步骤3中DMHE算法，仅能知道当前时刻t_k的目标位置估计值p_r(t_k；t_k)，因此需要对τ∈(t_k,t_k+T_p]时的目标位置进行假设；

假设目标位置预测轨迹设计如下：

其中，是通过目标的名义系统生成的；

根据上述假设目标位置预测轨迹的设计，可将协同代价函数L_i(τ；t_k)中的p_ir(τ；t_k)通过代替求解。