CN103968838B - 一种基于极坐标系的auv曲线运动状态下的协同定位方法 - Google Patents

Abstract

本发明提供的是一种基于极坐标系的AUV曲线运动状态下的协同定位方法。其中主AUV装备精度较高的惯性测量单元，而子AUV装备较低精度的IMU。首先，子AUV通过融合自身的惯性测量单元测量载体运动信息，并测定相对于主AUV的距离和方位信息，最后利用扩展卡尔曼滤波来实现协同定位。本发明与传统的基于笛卡尔坐标系的算法相比，在AUV做曲线运动时具有较高的定位精度。

Description

一种基于极坐标系的AUV曲线运动状态下的协同定位方法

技术领域

本发明涉及的是一种在曲线运动状态下基于极坐标系的AUV曲线运动状态下的协同定位方法。

背景技术

未来是海洋文明的世纪，海洋领域的探索将成为世界的发展趋势，因此水面船艇作业、应用的研究具有广阔的前景。自主水下载体(Autonomous Underwater Vehicle,AUV)实际上是一种水下无人平台，以其体积小、机动性强和成本低等优势成为研究热点；可以执行各种导航和水下地理勘察等复杂任务[1]。随着人类进一步开发认识海洋，AUV研究、应用的理论价值和实际意义更为凸显。AUV与常规的船艇比较，易于操作、灵活快捷，可以经受住各种复杂环境的考验。AUV在允许侦测区域进行隐蔽及公开作业，要充分利用传感器信息，这就需要多AUV进行协同定位。目前，大多数的AUV协同定位***都是基于笛卡尔坐标系建立的，且其运动状态大都为匀速直航，而极坐标系是针对曲线运动的，所以提出了一种基于极坐标系的协同定位算法，另外由于在AUV作曲线运动时，单纯的距离信息不能够很好的描述AUV的运动状态，因此提出了一种以距离加方位信息作为观测量的协同导航定位算法并进行了可观性分析。

发明内容

本发明的目的在于提供一种提高精度的基于极坐标系的AUV曲线运动状态下的新协同定位方法。

本发明的目的是这样实现的：

步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；

步骤二：采集子AUV相对于主AUV的距离和方位信息；

采集的子AUV与主AUV之间的相对距离为：

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角；

采集的子AUV与主AUV之间的相对方位角为：

当rcosθ-r₁cosθ₁≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角；

当rcosθ-r₁cosθ₁＞0时，有：

步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子AUV的导航定位；

所涉及的***状态向量为：

其中：为主AUV的航向角；

所涉及的***离散状态方程为：

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的***输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻***的噪声驱动矩阵；W(t)为噪声矩阵：

***的输入向量：

U_k-1＝[v_k-1 ω_k-1]^T

对应的输入矩阵：

***的噪声矩阵：

其中：δr_1,k-1,δθ_1,k-1,分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，δr_k-1,δθ_k-1,分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差；

***的状态转移矩阵为：

其中：T为采样周期；

所涉及的***量测方程为：

其中：Z_k为k时刻***的量测向量；H_k为k时刻***的量测矩阵；V_k为量测噪声；

***的量测向量为：

Z_k＝[R_k φ_k]^T，

当rcosθ-r₁cosθ₁≤0时，量测矩阵为：

当rcosθ-r₁cosθ₁＞0时，量测矩阵为：

本发明的有益效果在于：

(1)与传统的基于笛卡尔坐标系下的***动力学模型相比，极坐标系下的***动力学模型更适用于AUV作曲线运动的状态下。

(2)与传统的仅用相对距离信息作为观测量的算法相比，采用距离加方位信息作为观测量时能够更好的反映曲线运动下AUV的运动状态，从而使得定位结果更精确。

(3)采用李导数对***的可观测性进行了分析，为协同定位提供了先决条件，进而有助于定位精度的进一步提高。

附图说明

图1是本发明的方法流程图；

图2是AUV的估计模型；

图3是主AUV和子AUV的真实运动轨迹；

图4是基于传统算法时的子AUV定位误差曲线；

图5是基于传统算法时的子AUV的位置估计曲线；

图6是基于传统算法时的子AUV的位置估计曲线的末端放大图；

图7是基于新算法时的子AUV定位误差曲线；

图8是基于新算法时的子AUV的位置估计曲线；

图9是基于新算法时的子AUV的位置估计曲线的末端放大图。

具体实施方式

下面将结合附图对本发明作进一步的详细说明。

基于极坐标系的AUV曲线运动状态下的新协同定位算法，包括以下几个步骤：

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、和角速度信息。

步骤二：利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的***方程。

步骤三：利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的***方程转换到极坐标下。

步骤四：采集子AUV相对于主AUV的距离和方位信息。

步骤五：基于步骤三和步骤四建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位。

利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的***方程为：

其中：v_i和ω_i分别为第i个水下自主载体(Autonomous Underwater Vehicle,AUV)的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数。由于AUV作曲线运动，则ω_i≠0。

利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的***方程转换到极坐标下为：

其中：r_i和θ_i分别为第i个AUV在极坐标系下的半径和极角。

采集的子AUV相对于主AUV的距离和方位信息为：

(1)以两个AUV的协同导航定位***为例，采集的子AUV与主AUV之间的相对距离为：

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角。

(2)采集的子AUV与主AUV之间的相对方位角为：

当rcosθ-r₁cosθ₁≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角。

当rcosθ-r₁cosθ₁＞0时，有：

基于步骤三和步骤四建立EKF的状态方程和量测方程包括：

(1)EKF算法的状态方程：

***的状态变量为：

其中：为主AUV的航向角。

***的离散状态方程为：

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的***输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻***的噪声驱动矩阵；W(t)为噪声矩阵。

***的输入向量：

U_k-1＝[v_k-1 ω_k-1]^T

对应的输入矩阵：

***的噪声矩阵：

其中：δr_1,k-1,δθ_1,k-1,分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，δr_k-1,δθ_k-1,分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差。

***的状态转移矩阵为：

其中：T为采样周期。

(2)EKF算法的离散量测方程：

其中：Z_k为k时刻***的量测向量；H_k为k时刻***的量测矩阵；V_k为量测噪声。

***的量测向量为：

Z_k＝[R_k φ_k]^T

当rcosθ-r₁cosθ₁≤0时，量测矩阵为：

当rcosθ-r₁cosθ₁＞0时，量测矩阵为：

通过EKF算法对子AUV的位置进行估计，以实现定位。

本发明提供的是一种基于极坐标系的自主水下载体(Autonomous UnderwaterVehicle,AUV)曲线运动状态下的新的协同定位算法。其中主AUV装备精度较高的惯性测量单元(Inertial Measurement Unit,IMU)，而子AUV装备较低精度的IMU。首先子AUV通过融合自身的IMU测量信息以及相对于主AUV的距离和方位信息并利用扩展卡尔曼滤波(ExtendKalman Filter,EKF)来实现协同定位。然后对从可观测性分析的角度进一步说明了该协同定位***性能的改善。最后从仿真角度验证了该协同定位算法的有效性并获得了较高的定位性能。本发明与传统的基于笛卡尔坐标系的算法相比，具有较高的定位精度，同时比航位推算算法和只融合距离测量信息的算法的精度较。

本发明的目的是提供一种针对AUV的曲线运动状态提供一种基于极坐标系的协同定位算法。首先在普通模型的基础上建立极坐标下的***动力学模型；然后建立主AUV和子AUV之间的相对距离模型和相对方位角模型；在上述基础上利用卡尔曼滤波对***子AUV进行定位；最后利用李导数对该非线性***进行了可观测性分析，分析了本发明的优势。

本发明包括以下几个步骤：

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度和速度信息；

步骤二、建立传统的笛卡尔坐标系下AUV的***动力学方程；

步骤三、利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下AUV的***动力学方程；

步骤四、采集需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位信息；

步骤五、选取状态变量建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位；

步骤六、根据极坐标系下建立的***状态方程和量测方程，利用李导数对该非线性***进行可观测性分析。

本发明的一种基于极坐标系的AUV曲线运动状态下的新协同定位算法，流程图如图1所示，包括以下几个步骤：

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、速度和角速度信息。

步骤二：建立传统的笛卡尔坐标系下第i个AUV的***动力学方程为：

其中：v_i和ω_i分别为第i个水下自主载体(Autonomous Underwater Vehicle,AUV)的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数。由于AUV作曲线运动，则ω_i≠0。

步骤三：利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下第i个AUV的***动力学方程为：

其中：r_i和θ_i分别为第i个AUV在极坐标系下的半径和极角。

步骤四：建立需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位模型包括：

(1)以两个AUV的协同导航定位***为例，建立子AUV与主AUV之间的相对距离模型为：

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角。

(2)根据图2可建立子AUV与主AUV之间的相对方位角模型为：

当rcosθ-r₁cosθ₁≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角。

当rcosθ-r₁cosθ₁＞0时，有：

步骤五：选取状态变量建立基于EKF滤波算法的***状态方程和量测方程。

(1)建立EKF算法的状态方程：

***的状态变量为：

其中：为主AUV的航向角。

***的离散状态方程为：

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的***输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻***的噪声驱动矩阵；W(t)为噪声矩阵。

***的输入向量：

U_k-1＝[v_k-1 ω_k-1]^T (8)

对应的输入矩阵：

***的噪声矩阵：

其中：δr_1,k-1,δθ_1,k-1,分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，δr_k-1,δθ_k-1,分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差。

***的状态转移矩阵为：

其中：T为采样周期。

(2)建立EKF算法的离散量测方程：

其中：Z_k为k时刻***的量测向量；H_k为k时刻***的量测矩阵；V_k为量测噪声。

***的量测向量为：

Z_k＝[R_k φ_k]^T (13)

当rcosθ-r₁cosθ₁≤0时，量测矩阵为：

当rcosθ-r₁cosθ₁＞0时，量测矩阵为：

仿真时间2500秒，在相同的仿真条件下，分别对基于传统的笛卡尔坐标系和采用极坐标下的模型进行，为了更好的说明本发明的优势，在两种方法下分别对航推、基于位置观测信息的EKF算法和基于位置和方位角的EKF算法进行可比较。

实施效果：图3给出了主AUV，子AUV的真实轨迹；图4给出基于传统算法时的子AUV定位误差曲线，其中：EKF1表示观测信息为距离时在传统算法下的EKF估计轨迹，EKF2表示观测信息为距离加方位时在传统算法下的EKF估计轨迹，图4中的子图为基于EKF2算法的全局放大图；图5给出了基于传统算法时的子AUV的位置估计曲线，其中：EKF1表示观测信息为距离时在传统算法下的EKF估计误差，EKF2表示观测信息为距离加方位时在传统算法下的EKF估计误差，为了更加清楚明了的比较航推、EKF1和EKF2三种算法的定位结果，对其定位轨迹的末端进行放大，如图6所示；图7给出了基于本发明的子AUV定位误差曲线，其中：EKF1表示观测信息为距离时在本发明下的EKF估计轨迹，EKF2表示观测信息为距离加方位时在本发明下的EKF估计轨迹，图7中的子图为基于EKF2算法的全局放大图；图8给出了基于本发明的子AUV的位置估计曲线，其中：EKF1表示观测信息为距离时在本发明下的EKF估计误差，EKF2表示观测信息为距离加方位时在本发明下的EKF估计误差，为了更加清楚明了的比较航推、EKF1和EKF2三种算法的定位结果，对其定位轨迹的末端进行放大，如图9所示。通过图4和图7可以看出，无论是在传统算法还是在本发明提出的算法下，本发明提出的距离加方位观测法都能够获得较高的精度，同时比较图4和图7可知，对与曲线运动来说，本发明提能够获得更高的精度。

步骤六：利用李导数对非线性***进行可观测性分析。

建立非线性的状态方程为：

当rcosθ-r₁cosθ₁≤0时，建立非线性的量测方程为：

可观测性矩阵为：

其中：a₁＝r-r₁cos(θ-θ₁)；a₂＝rr₁sin(θ-θ₁)；b₂＝-r²+rr₁cos(θ-θ₁)＝-ra₁；

当rcosθ-r₁cosθ₁＞0时，建立非线性的量测方程为：

可观测性矩阵为：

其中：a₁＝r-r₁cos(θ-θ₁)；a₂＝rr₁sin(θ-θ₁)；b₂＝r²-rr₁cos(θ-θ₁)＝ra₁；

通过分析可知，当r＝r₁,θ＝θ₁，即主AUV和子AUV的位置重合时，***整体不可观；当主AUV和子AUV之间的相对距离不变时，距离观测子块即O₁不可观；当主AUV和子AUV之间的相对方位角不变时，距离观测子块即O₂不可观；当主AUV和子AUV之间的相对距离和相对方位角都不变时，整个***不可观，所以为了提高***的定位精度，保证***的可观测性，应该保证主AUV和子AUV之间的相对距离或相对方位角为变化的。

从以上实施例不难看出，相对于传统的在笛卡尔坐标系下的协同定位算法，本发明提供方法能够获得更高的精度。

Claims (1)

1.一种基于极坐标系的AUV曲线运动状态下的协同定位方法，其特征在于，包括以下步骤：

步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；利用子AUV的速度和航向信息建立传统的笛卡尔坐标系下AUV的***方程：

其中：v_i和ω_i分别为第i个AUV的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数；由于AUV作曲线运动，则ω_i≠0；

步骤二：利用极坐标系和笛卡尔坐标系之间的转换关系将AUV的***方程转换到极坐标下，采集子AUV相对于主AUV的距离和方位信息；

其中：r_i和θ_i分别为第i个AUV在极坐标系下的半径和极角；

采集的子AUV与主AUV之间的相对距离为：

$R=\sqrt{{\left(r\right)}^2+{\left(r_1\right)}^2-2{\mathrm{rr}}_{1}cos(\mathrm{\θ}-{\mathrm{\θ}}_1)}$

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角；

采集的子AUV与主AUV之间的相对方位角为：

当rcosθ-r₁cosθ₁≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角；

当rcosθ-r₁cosθ₁＞0时，有：

步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子AUV的导航定位；

所涉及的***状态向量为：

其中：为主AUV的航向角；

所涉及的***离散状态方程为：

${\hat{S}}_k^{-}={\mathrm{\Φ}}_{k,k-1}{\hat{S}}_{k-1}+T_{k-1}{U}_{k-1}+G_{k-1}{W}_{k-1}$

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的***输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻***的噪声驱动矩阵；W(t)为噪声矩阵：

***的输入向量：

U_k-1＝[v_k-1 ω_k-1]^T

对应的输入矩阵：

$T_{k-1}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 1\end{array}\right]$

***的噪声矩阵：

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差；

***的状态转移矩阵为：

其中：T为采样周期；

所涉及的***量测方程为：

$Z_k=H_k{\hat{S}}_k^{-}+V_k$

其中：Z_k为k时刻***的量测向量；H_k为k时刻***的量测矩阵；V_k为量测噪声；

***的量测向量为：

Z_k＝[R_k φ_k]^T

当rcosθ-r₁cosθ₁≤0时，量测矩阵为：

$H_k=\left[\begin{array}{ccc}\frac{r_k\left(r_k-r_{1,k}\right)\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k}& \frac{r_k{r}_{1,k}\sin \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k}& 0\\ \frac{-r_{1,k}\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k^2}& \frac{r_k\left(r_k-r_{1,k}\right)\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k^2}& 1\end{array}\right];$

当rcosθ-r₁cosθ₁＞0时，量测矩阵为：

$H_k=\left[\begin{array}{ccc}\frac{r_k\left(r_k-r_{1,k}\right)\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k}& \frac{r_k{r}_{1,k}\sin \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k}& 0\\ \frac{r_{1,k}\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k^2}& -\frac{r_k\left(r_k-r_{1,k}\right)\cos \left({\mathrm{\θ}}_k-{\mathrm{\θ}}_{1,k}\right)}{R_k^2}& -1\end{array}\right];$

步骤四：利用李导数对非线性***进行可观测性分析；

建立非线性的状态方程为：

当rcosθ-r₁cosθ₁≤0时，建立非线性的量测方程为：

可观测性矩阵为：

其中：a₁＝r-r₁cos(θ-θ₁)；a₂＝rr₁sin(θ-θ₁)；b₂＝-r²+rr₁cos(θ-θ₁)＝-ra₁；

当rcosθ-r₁cosθ₁＞0时，建立非线性的量测方程为：

可观测性矩阵为：

$Obs=d\left(G\right)=d\left(\begin{array}{cc}{L}_f^0\left(h_1\right)& L_f^0\left(h_2\right)\\ L_f^1\left(h_1\right)& L_f^1\left(h_2\right)\end{array}\right)=\left(\begin{array}{ccc}\frac{a_1}{h_1}& \frac{a_2}{h_1}& 0\\ \frac{c_1}{h_1^3}& \frac{c_2}{h_1^3}& \frac{c_3}{h_1^3}\end{array}|\begin{array}{ccc}\frac{b_1}{h_1^2}& \frac{b_2}{h_1^2}& 1\\ \frac{d_1}{h_1^3}& \frac{d_2}{h_1^3}& \frac{d_3}{h_1^3}\end{array}\right)=\left(O\right)=\left(O_1\right|O_2);$

其中：a₁＝r-r₁cos(θ-θ₁)；a₂＝rr₁sin(θ-θ₁)；b₂＝r²-rr₁cos(θ-θ₁)＝ra₁；