CN114840003A - 一种单领航多auv协同定位及轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种单领航多AUV协同定位及轨迹跟踪控制方法，步骤一：建立跟踪器运动学模型、领航器运动学模型以及两者之间距离量测的模型；步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计；步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹；步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。本发明所提出的方法具有很强的鲁棒性和有效性。对不同的多个跟踪器均有很好的效果。

Description

一种单领航多AUV协同定位及轨迹跟踪控制方法

技术领域

本发明属于水下多航行器协同定位及轨迹跟踪控制领域，涉及一种单领航多AUV协同定位及轨迹跟踪控制方法，特别是一种基于扩展卡尔曼滤波的单领航多AUV协同定位及轨迹跟踪控制方法。

背景技术

基于距离的同步目标定位与追踪由于其在海洋科学领域的大量应用的重要性，近年来受到了广泛的关注。在执行任务中，跟踪器配备了测量目标距离的声学设备，并利用这些信息估计目标的状态(定位任务)，同时在预定目标附近保持所需的相对几何编队，以获取目标的最大距离信息用于估计(追踪任务)。由于低功率声学设备所能测量的最大量程有限制，所以追踪任务在海洋环境中具有极大的相关性。主要问题与定位任务有关。除了设计一个合适的目标状态估计器外，关键问题是如何规划跟踪器的运动，以便通过适当的机动使跟踪器到目标的范围提供足够的信息来估计目标的状态。从技术上讲，这个问题的答案必须提供跟踪器运动的条件，使获得的距离产生目标运动的可观测性。第二个问题与跟踪任务有关，即如何控制水下航行器，使它们收敛到目标附近，同时保持一个所需的相对几何编队。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种基于扩展卡尔曼滤波的单领航多AUV协同定位及轨迹跟踪控制方法，具有很强的鲁棒性和有效性。

为解决上述技术问题，本发明一种单领航多AUV协同定位及轨迹跟踪控制方法，包括：

步骤一：建立跟踪器运动学模型、领航器运动学模型以及两者之间距离量测的模型；

步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计；

步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹；

步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。

进一步的，步骤一具体为：

建立跟踪器运动学模型：

假设有N个跟踪器(N≥1)，{I}＝{x_I,y_I,z_I}为惯性坐标系，

为第i个跟踪器的自身坐标系，i∈N；

代表惯性坐标系中第i个跟踪器的坐标；

表示第i个跟踪器在自身坐标系中的速度向量，则跟踪器的运动学模型是：

其中，R(η^[i])是从自身坐标系到惯性系的转换矩阵，其中向量

是由滚转角、俯仰角、偏航角组成的向量，转换矩阵的导数满足：

其中ω^[i]＝[p^[i],q^[i],r^[i]]^T为自身固定的角速度向量，p^[i],q^[i],r^[i]分别为x、y、z方向对应的角速度，S是一个矩阵，当跟踪器运动为三维运动时，定义为：

当跟踪器运动为二维运动时：

建立领航器运动学模型：

将领航器看作被跟踪的目标，则

代表目标的轨迹，

代表惯性系下目标的速度向量，假设目标在缓慢的改变速度，其运动模型采用准稳态模型的形式描述：

其中

代表目标的状态，其中υ(t)是缓慢变化，w～N(0,Q_t)为零均值高斯噪声，协方差为Q_t，

代表着k时刻的目标状态，进行离散化得：

x_k+1＝Fx_k+w_k

其中，当跟踪器运动为三维运动时，

当跟踪器运动为二维运动时

T_s是采样时间，w～N(0,Q)，

建立距离量测模型：

表示跟踪器和目标之间在k时刻的真实距离

其中

为跟踪器i在k时刻所处位置，跟踪器与目标之间的量测模型为：

其中，

为高斯量测噪声。

进一步的，步骤二具体为：

假设对目标状态x的估计为：

其中，

为对目标真实位置q的估计，

为对目标真实速度υ的估计；

令

代表在时间k之前跟踪器到目标的距离的集合，系统估计的量测向量和量测矩阵z,Ω分别为：

根据扩展卡尔曼滤波进行估计，其基本方程为：

在k＝0时初始化

可得：

距离量测估计为：

量测向量估计为：

量测向量更新为：

状态一步预测为：

量测向量，量测矩阵一步预测为：

其中，

为k时刻对距离量测的估计，F为系统状态方程的雅可比矩阵；

进而获得k时刻对目标状态估计的先验和后验密度，即

和

其中，x_k为第k时刻目标状态，

为k时刻估计状态，

为根据k-1时刻的状态估计对k时刻状态进行预测得到的结果,P_k|k-1为根据k-1时刻的均方误差阵进行预测得到k时刻的均方误差矩阵。

进一步的，步骤三具体为：

当跟踪器运动为三维运动时，定义跟踪器在三维惯性系下的真实环绕轨迹为：

其中，γ为路径参数化变量，

为常量参数；

当跟踪器运动为二维运动时，定义跟踪器在二维惯性系下的真实环绕轨迹为：

r(γ)＝[r_x cos(γ+γ₀),r_y sin(γ+γ₀)]^T

根据路径跟随理念MPF，跟踪器理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成，即：

P_d(t)＝r(γ^[i](t))+q(t)，i∈N

其中，q(t)为目标位置信息，r^[i](γ)为跟踪器环绕目标时的运动信息；

跟踪器间的夹角应满足：

对于N＝2

对于N≥3，i,j为两个相邻的跟踪器。

进一步的，步骤四具体为：

令期望下γ的微分为

其中，

为一个不为0的常数，跟踪器实际轨迹与理想轨迹之间的速度误差为：

跟踪器实际与理想轨迹之间的位置误差为：

其中，ε是一个无穷小非零向量，

为完整的跟踪误差向量，对

求导可得：

其中，

分别对应跟踪器三维运动和二维运动的情况，其中ε_i表示向量ε的第i个向量；

对速度误差e_γ求导可得：

为使跟踪误差e到零，设计了以下控制器：

其中，

本发明的有益效果：本发明提出了一个完整的路径规划，控制，和估计框架来解决基于距离的多个潜航器进行目标定位和追踪的方法。本发明所提出的方法具有很强的鲁棒性和有效性，对不同的多个跟踪器均有很好的效果。本发明具体涉及到多个跟踪器进行轨迹规划、利用协同扩展卡尔曼滤波算法(DEKF)对领航器协同定位、以及协同跟踪控制器使跟踪器完成轨迹跟踪任务。主要计算了每个跟踪器经过的轨迹跟踪曲线形式的轨迹。通过适当地参数化曲线，可以保证如果跟踪器精确地跟踪期望的轨迹跟踪曲线，那么目标运动就可以被观察到。同时，跟踪器收敛到一个理想的相对编队，在一个明确定义的数学意义上，将获得的距离信息渲染为“最大”，用于目标状态估计。针对多跟踪器的情况，提出了一种高效的分布式估计与控制(DEC)策略，用于跟踪器以协同的方式定位和跟踪目标。所提出的DEC可确保所有跟踪器渐进地对目标状态的估计达成一致，并在目标附近保持所需的几何结构。该策略要求跟踪器之间交换的信息非常有限，因此在实际实施中非常有效。

附图说明

图1为二维多跟踪器理想轨迹规划示意图；

图2为三个跟踪器相互之间的通信拓扑图；

图3为单领航三个跟踪器在二维下的目标定位及轨迹跟踪控制仿真图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明首先为多个跟踪器进行轨迹规划、其次利用协同扩展卡尔曼滤波算法(DEKF)对领航器协同定位、而后利用协同跟踪控制器使跟踪器完成轨迹跟踪任务。包括：

步骤一：建立领航器、跟踪器以及两者之间距离量测的模型。

跟踪器模型：

假设有N个跟踪器(N≥1)，{I}＝{x_I,y_I,z_I}为惯性坐标系，

为第i个跟踪器的自身坐标系，i∈N。

代表惯性坐标系中第i个跟踪器的坐标。

表示第i个跟踪器在自身坐标系中的速度向量。

所以跟踪器的运动学模型是：

其中R(η^[i])是从自身坐标系到惯性系的转换矩阵。其中向量

这是一个由三个方向欧拉角组成的向量(roll，pitch，yaw)。旋转矩阵的导数满足：

其中ω^[i]＝[p^[i],q^[i],r^[i]]^T为自身固定的角速度向量，S是一个矩阵，被定义为

为了方便计算，我们一般认为跟踪器是一个欠驱动系统，所以三维情况下

故第i个跟踪器的输入为

在二维情况下，向量η^[i]＝ψ^[i],ω^[i]＝r^[i]，这种情况下的话，输入就是

领航器模型：

将领航器看作被跟踪的目标，则

代表着可以被追踪的目标轨迹即领航器的轨迹，

代表惯性系下目标的速度向量即领航器的速度，即便目标的轨迹是未知的，我们也可以假设目标在缓慢的改变速度，其运动模型可以用形式的准稳态模型来描述即：

其中

代表目标的状态，其中υ(t)是缓慢变化，w～N(0,Q_t)为零均值高斯噪声，协方差为Q_t，

代表着k时刻的领航器目标状态，为了方便对目标状态进行估计，将式(3)进行离散化得：

x_k+1＝Fx_k+w_k (4)

其中

T_s是采样时间，w～N(0,Q)，

对于二维的情况，

距离量测模型：

假设每个跟踪器都装备对目标测距的声学测量装置，

表示两者之间在k时刻的真实距离

其中

为跟踪器i在k时刻所处位置，所以跟踪器与目标之间的量测模型为：

其中

为高斯量测噪声。

步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计。

假设对目标状态x的估计为：

其中，

为对目标真实位置q的估计，

为对目标真实速度υ的估计。

令

代表在时间k之前跟踪器到目标的距离的集合。同时，系统估计的量测向量和量测矩阵z,Ω，分别为：

根据扩展卡尔曼滤波进行估计，其基本方程为：

在k＝0时初始化

则由式(7)可知：

距离量测估计:

量测向量估计：

量测向量更新：

状态一步预测：

量测向量，量测矩阵一步预测：

其中，

为k时刻对距离量测的估计，F为系统状态方程的雅可比矩阵。

经过上述估计过程就可获得k时刻对目标状态估计的先验和后验密度，即

和

其中，x_k为第k时刻目标状态，

为k时刻估计状态，

为根据k-1时刻的状态估计对k时刻状态进行预测得到的结果,P_k|k-1为根据k-1时刻的均方误差阵进行预测得到k时刻的均方误差矩阵。

步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹。

为了估计目标的距离信息使用Fisher信息矩阵(FIM)作为一种量化方法，它提出了一类最大距离信息的最优轨迹，其中包含跟踪器针对目标作环绕运动。因此，定义跟踪器在三维惯性系下的真实环绕轨迹为：

其中，γ为路径参数化变量，

为常量参数。

对于二维空间，

根据路径跟随理念(MPF)，跟踪器理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成，即

P_d(t)＝r(γ^[i](t))+q(t)，i∈N (17)

其中，q(t)为目标位置信息，r^[i](γ)即为上式所定义的跟踪器环绕目标时的运动信息。

对于多个跟踪器，为了获得更好的可观测性，跟踪器间的夹角应满足一定要求，如图1所示，当有两个跟踪器时，每个跟踪器到目标的向量之间夹角应为π/2，当跟踪器超过3个时，两个相邻跟踪器的相对向量与目标所成的角度为2π/N。即：

步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。

令期望下γ的微分为

其中，

为一个不为0的常数，所以，跟踪器实际轨迹与理想轨迹之间的速度误差为：

跟踪器实际与理想轨迹之间的位置误差为：

其中，ε是一个无穷小非零向量。所以，

为完整的跟踪误差向量，如果e^[i]收敛到0，跟踪器就可以收敛到以目标为中心的理想轨迹上，故对

求导可得：

其中，

分别对应三维和二维的情况，其中ε_i表示向量ε的第i个向量。

对速度误差e_γ求导可得

所以为了让跟踪误差e到零，设计了以下控制器：

其中，

为进一步说明本发明的效果，采用三个跟踪器对单目标在二维情况下进行定位及轨迹跟踪控制仿真。其中三个跟踪器之间的通信拓扑结构如图2所示；二维情况下目标的轨迹为q＝[20sin(0.1t),0.3t]^T(m)。其余参数均为量测距离σ＝0.5m，轨迹曲线(轨迹跟踪曲线)

轨迹跟踪控制器参数ε＝[-0.5,0]^T，K_p＝diag([0.4,0.2])，k_γ＝500，k_c＝0.2。EKF算法相关参数为V^[i]＝10，Q＝10^-3diag(1,1,0.1,0.1)，πⁱ＝π^j＝0.5,T_s＝2s。图3是目标在二维情况下沿曲线运动情况下多跟踪器仿真过程图。从结果示意图可以看出，跟踪器可以很好的完成目标定位及轨迹跟踪任务，跟踪器最终沿预定轨迹进行跟踪，而且系统具有稳定性。

Claims (5)

1.一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于，包括：

步骤一：建立跟踪器运动学模型、领航器运动学模型以及两者之间距离量测的模型；

步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计；

步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹；

步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。

2.根据权利要求1所述的一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于：步骤一具体为：

建立跟踪器运动学模型：

假设有N个跟踪器(N≥1)，{I}＝{x_I,y_I,z_I}为惯性坐标系，

为第i个跟踪器的自身坐标系，i∈N；

代表惯性坐标系中第i个跟踪器的坐标；

表示第i个跟踪器在自身坐标系中的速度向量，则跟踪器的运动学模型是：

其中，R(η^[i])是从自身坐标系到惯性系的转换矩阵，其中向量

是由滚转角、俯仰角、偏航角组成的向量，转换矩阵的导数满足：

其中ω^[i]＝[p^[i],q^[i],r^[i]]^T为自身固定的角速度向量，p^[i],q^[i],r^[i]分别为x、y、z方向对应的角速度，S是一个矩阵，当跟踪器运动为三维运动时，定义为：

当跟踪器运动为二维运动时：

建立领航器运动学模型：

将领航器看作被跟踪的目标，则

代表目标的轨迹，

代表惯性系下目标的速度向量，假设目标在缓慢的改变速度，其运动模型采用准稳态模型的形式描述：

其中

代表目标的状态，其中υ(t)是缓慢变化，w～N(0,Q_t)为零均值高斯噪声，协方差为Q_t，

代表着k时刻的目标状态，进行离散化得：

x_k+1＝Fx_k+w_k

其中，当跟踪器运动为三维运动时，

当跟踪器运动为二维运动时

T_s是采样时间，w～N(0,Q)，

建立距离量测模型：

表示跟踪器和目标之间在k时刻的真实距离

其中

为跟踪器i在k时刻所处位置，跟踪器与目标之间的量测模型为：

其中，

为高斯量测噪声。

3.根据权利要求2所述的一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于，步骤二具体为：

假设对目标状态x的估计为：

其中，

为对目标真实位置q的估计，

为对目标真实速度υ的估计；

令

代表在时间k之前跟踪器到目标的距离的集合，系统估计的量测向量和量测矩阵z,Ω分别为：

根据扩展卡尔曼滤波进行估计，其基本方程为：

在k＝0时初始化

可得：

距离量测估计为：

量测向量估计为：

量测向量更新为：

状态一步预测为：

量测向量，量测矩阵一步预测为：

其中，

为k时刻对距离量测的估计，F为系统状态方程的雅可比矩阵；

进而获得k时刻对目标状态估计的先验和后验密度，即

和

其中，x_k为第k时刻目标状态，

为k时刻估计状态，

为根据k-1时刻的状态估计对k时刻状态进行预测得到的结果,P_k|k-1为根据k-1时刻的均方误差阵进行预测得到k时刻的均方误差矩阵。

4.根据权利要求2所述一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于：步骤三具体为：

当跟踪器运动为三维运动时，定义跟踪器在三维惯性系下的真实环绕轨迹为：

P:

其中，γ为路径参数化变量，

为常量参数；

当跟踪器运动为二维运动时，定义跟踪器在二维惯性系下的真实环绕轨迹为：

r(γ)＝[r_xcos(γ+γ₀),r_ysin(γ+γ₀)]^T

根据路径跟随理念MPF，跟踪器理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成，即：

P_d(t)＝r(γ^[i](t))+q(t)，i∈N

其中，q(t)为目标位置信息，r^[i](γ)为跟踪器环绕目标时的运动信息；

跟踪器间的夹角应满足：

对于N＝2

对于N≥3，i,j为两个相邻的跟踪器。

5.根据权利要求2所述的一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于：步骤四具体为：

令期望下γ的微分为

其中，

为一个不为0的常数，跟踪器实际轨迹与理想轨迹之间的速度误差为：

跟踪器实际与理想轨迹之间的位置误差为：

其中，ε是一个无穷小非零向量，

为完整的跟踪误差向量，对

求导可得：

其中，

分别对应跟踪器三维运动和二维运动的情况，其中ε_i表示向量ε的第i个向量；

对速度误差e_γ求导可得：

为使跟踪误差e到零，设计了以下控制器：

其中，

Images