CN112632876B - 一种基于dmhe和dmpc的无人船协同目标跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于DMHE和DMPC的无人船协同目标跟踪控制方法，属于工业自动控制和海洋工程领域。本发明主要用于多无人船协同目标跟踪控制，首先建立无人船的运动学和动力学模型，目标运动模型以及量测模型，之后设计基于无迹卡尔曼滤波的有限时域MHE算法，解决了到达代价估计问题和单个无人船对目标的估计问题，接着根据上述到达代价的估计方法，通过邻居节点间信息交流，设计DMHE算法，解决多无人船对目标的协同估计问题，最后根据目标估计的位置信息以及邻居节点位置信息交流，设计DMHE算法，解决多无人船协同目标跟踪控制问题。

Description

一种基于DMHE和DMPC的无人船协同目标跟踪控制方法

技术领域

本发明设计属于工业自动控制和海洋工程领域，具体涉及一种基于DMHE和DMPC的无人船协同目标跟踪控制方法。

背景技术

随着传感器、计算机、通信、网络化系统等技术迅猛发展，具有先进制导、导航和控制(Guidance,Navigation and Control,GNC)功能的无人船，逐渐引起了人们的重视。无人船是一种无需人员干预，可以在各种复杂的环境中执行任务，具有高度非线性动力学特性的水面运载工具。它的优势在于结构简单、用途广泛、操作方便、自主性强、释放和回收较为快捷、能避免未知领域人为操作风险，因此在军事和民用领域都有广泛应用。

无人船进行目标跟踪任务时会面临环境复杂多变和任务复杂多样的情况，而单无人船在进行大范围目标跟踪任务时，容易受到自身航程和通信距离限制，使得活动范围有限，工作效率低，对于环境的感知能力较差。多无人船协同目标跟踪可以充分利用系统中某些无人船的高精度信息，使得装备较差的无人船获得更好的跟踪效果；同时，多无人船活动范围更大，对目标的搜索范围更广。

多无人船协同作业包括集中式和分布式两种模式。集中式模式要求中央节点与所有无人船通信，这种模式的好处在于信息获取比较丰富，整体系统协同效果比较好。但是一旦中央节点出现故障，那么整个系统也会随之陷入瘫痪，并且如果系统维度过大，带来的中心节点计算量和通信量也会很高，影响时效性和容错性。而分布式模式中无人船只需要与邻居节点交换信息，通信压力小，时效性高，增强了系统可维护性和灵活性。因此，使用分布式模式实现多无人船的协同控制是一种可靠方法。

目前在对目标的协同估计方面，虽然基于卡尔曼滤波的分布式协同估计方法取得了较好的估计效果(如Distributed Kalman filtering based on consensusstrategies,IEEE Journal on Selected Areas in Communications,2008,26(4):622-633.)，但是由于这种方法无法处理以约束形式存在的过程噪声，因此在实际估计过程中可能会出现不合理的估计结果。同时，也未能考虑到目标估计的优化性能(如DistributedKalman filter using weighted averaging,Proc.of the International Symposium onMathernatical Theory of Network,2006:1-6.)。在目标的协同跟踪控制方面，目前的大多数方法都不考虑协同编队控制的优化效果和控制过程中无人船推力和转矩的约束(如Cooperative target tracking control of Multiple Robots,IEEE Transactions onIndustrial Electronics,2012,59(8):3232–3240.)。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于DMHE和DMPC的无人船协同目标跟踪控制方法。

技术方案

一种基于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}、均值的权值

和协方差的权值

通过比例修正后，χ_i、

和

具体形式如下：

其中，参数λ＝α²(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)当时刻T>N时，求解基于无迹卡尔曼滤波的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)当时刻T>N时，接收邻居节点

的信息

求解基于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求解的优化问题如下所示：

x_i(τ)∈χ_i

其中，χ_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)通过

代替求解。

有益效果

本发明提出的一种基于DMHE和DMPC的无人船协同目标跟踪控制方法，首先建立无人船的运动学和动力学模型，目标运动模型以及量测模型，之后设计基于无迹卡尔曼滤波的有限时域MHE算法，解决了到达代价估计问题和单个无人船对目标的估计问题，接着根据上述到达代价的估计方法，通过邻居节点间信息交流，设计DMHE算法，解决多无人船对目标的协同估计问题，最后根据目标估计的位置信息以及邻居节点位置信息交流，设计DMHE算法，解决多无人船协同目标跟踪控制问题。

本发明利用分布式控制结构，减少系统通信压力与工作成本，增强系统的可维护性和容错率；采用模型预测控制算法解决有约束参数优化问题；采用滚动时域估计估计算法解决有约束参数估计问题，提高估计的准确率。

附图说明

图1是全信息MHE算法与UKF算法仿真对比图

图2是基于UKF的有限时域MHE算法与UKF算法仿真对比图

图3是无人船1的协同目标跟踪仿真图

图4是无人船2的协同目标跟踪仿真图

图5是无人船3的协同目标跟踪仿真图

图6是三艘无人船协同目标跟踪控制效果图

图7是有限时域MHE算法流程框图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

一种基于DMHE和DMPC的无人船协同目标跟踪控制方法包括以下步骤：

首先，建立编队系统中无人船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分别是与附加质量和阻尼力有关的流体动力参数。

建立目标运动模型，这里选取匀加速(CA)运动模型：

上述表达式中，x、

和

分别表示目标x轴方向位置、速度和加速度，y、

和

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

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

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

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

对于平面运动目标观测时，一般是在平面极坐标系中输出观测数据，即目标的距离和方位角。而一般目标运动模型是在直角坐标系下建立的，因此需要在混合坐标系下建立量测方程，具体形式如下：

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

其中，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θ具体形式如下：

在解决多无人船协同目标估计问题之前，需要根据设计目标建立优化函数，其中包括阶段代价项、目标协同项和到达代价项。下面首先按照单无人船全信息MHE算法、单无人船有限时域MHE算法和基于无迹卡尔曼滤波的单无人船有限时域MHE算法的顺序，逐步给出阶段代价项和到达代价项的具体形式，然后基于无人船的状态信息设计目标协同项。

单无人船目标估计问题：

首先，考虑利用当前时刻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}、均值的权值

和协方差的权值

通过比例修正后，χ_i、

和

具体形式如下：

其中，参数λ＝α²(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)当时刻T>N时，求解基于无迹卡尔曼滤波的MHE优化问题(17)，得到估计值

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

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

多无人船协同目标跟踪：

协同目标跟踪是指利用量测信息，设计协同滤波器，得到目标状态信息趋于一致的估计值。本部分设计一种基于状态信息的DMHE协同目标跟踪算法。

无人船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是给定加权矩阵，

是阶段代价项，R和Q分别是量测噪声和过程噪声协方差矩阵。

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

(1)初始化：给定Q、R和P₀加权矩阵，初始值

有限时域长度N，初始时刻T＝1。

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

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

(4)当时刻T>N时，接收邻居节点

的信息

求解基于DMHE的目标跟踪优化问题(19)，得到估计值

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

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

多无人船协同跟踪控制：

为了实现无人船之间的协同控制，各无人船需要与邻居节点进行信息交流。对于每个无人船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的位置跟踪误差。

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

x_i(τ)∈χ_i

其中，χ_i、

和Ω_i分别为状态约束，控制约束和终端约束。

求解优化问题(25)，将控制序列

的第一个元素作用于系统。

优化过程估计值更新：

由于在每一采样时刻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)通过

代替求解。

实施例1：

以三艘无人船为例，其中无人船2与无人船1进行信息交流，无人船3与无人船1进行信息交流，具体方案如下：

步骤1.根据无人船运动学和动力学模型，建立无人船i非线性状态空间表达式：

上式中，m＝23.8，I_zz＝1.76，

X_u＝-0.7225，N_r＝-1.9，Y_v＝-0.861，

d₁₁＝-X_u，d₂₂＝-Y_v，d₃₃＝-N_r。

建立匀加速(CA)目标运动模型和量测模型：

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

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

上式中离散周期T＝0.1s，设置过程噪声约束w(k)≥0，量测噪声协方差矩阵R＝diag(1,0.1)，过程噪声协方差矩阵Q＝diag(0.1,0.1)。

步骤2.设计DMHE协同目标估计算法：

(1)设定初始目标估计值

初始目标位置x_r,0＝[10,50,1,2,2,-1]^T有限时域长度N＝6，初始时刻T＝1，初始协方差矩阵P_x,0＝diag(1,1,1,1,0.1,0.1)。

(2)利用无迹卡尔曼滤波算法，求解到达代价中协方差矩阵。

首先进行UT变换：考虑k均值为

协方差为P_x,k，传递函数x_r,k+1＝f(x_r,k)。根据均值和方差得到sigma点{χ_i:i＝0,...,2n}、均值的权值

和协方差的权值

通过比例修正后，χ_i、

和

具体形式如下：

其中，参数λ＝α²(n+κ)-n，κ＝0，α＝0.1β＝2，n＝6，

将sigma点通过函数x_r,k+1＝f(x_r,k)进行非线性传递，得到变换后集合

预测过程：

更新过程：

上述过程可以得到估计状态协方差矩阵P_x,k。

(3)当时刻T≤N时，无人船i建立如下全信息DMHE算法性能指标：

其中，k＝0,1,2...N-1,A＝diag(1,1)。，j是无人船i的邻居节点，优化问题如下：

w(k)≥0

通过求解优化问题

得到初始值x_r,0和过程噪声序列

将其带入目标运动模型，得到T时刻估计值

当时刻T>N时，无人船i建立如下有限时域DMHE算法性能指标：

其中，k＝T-N,...N-1,A＝diag(1,1)，j是无人船i的邻居节点，P_x,T-N是UKF计算得到的协方差矩阵。优化问题如下：

w(k)≥0

通过求解优化问题

得到初始值x_r,T-N和过程噪声序列

将其带入目标运动模型，得到T时刻估计值

步骤3.设计DMPC协同目标跟踪控制算法：

考虑雷达传感器搭载于3艘移动无人船上，且各无人船自身位置已知，观测值即为各无人船与目标的距离和方位角，设定无人船与目标组成编队形式，USV1跟踪目标所在位置，USV2与目标分别在x和y方向相差-1m和-1m，USV3与目标分别在x和y方向相差1m和1m。建立如下指标函数：

其中，代价函数的加权矩阵分别为，P_i＝diag(5,1)，Q_i＝diag(1,1)，Q_ij＝diag(10,10)，初始状态信息为x₁(0)＝[11,51,1,2,2,-1]^Tv，x₂(0)＝[12,51,1,2,2,-1]^T，x₃(0)＝[13,52,1,2,2,-1]^T，初始输入信息为u₁(0)＝[0,0]^T，u₂(0)＝[0,0]^T，u₃(0)＝[0,0]^T，预测时域T_p＝6。优化问题如下：

-10^4≤u_i(τ)≤10^4

通过求解优化问题

得到控制序列

将控制序列第一个解作用于系统，完成一次求解过程。

步骤4.优化过程估计值的更新：

DMPC算法中在t_k时刻，无人船i传输给邻居节点的估计值

和

更新如下：

将上述控制序列带入无人船系统模型得到：

DMHE算法中在t_k时刻，无人船i传输给邻居节点的目标位置估计值

更新如下：

其中，

通过目标模型的名义系统生成的。

步骤5.k+1时刻，按照上述步骤将估计值传输给其余水下航行器，各无人船得到邻居节点估计值后按照步骤2至步骤4过程循环求解优化问题，实现多无人船协同目标跟踪控制。

Claims (1)

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)当时刻T>N时，求解基于无迹卡尔曼滤波的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)当时刻T>N时，接收邻居节点

的信息

求解基于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)通过

代替求解。

Images