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

摘要

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

主权项

一种基于极坐标系的AUV曲线运动状态下的协同定位方法，其特征在于，包括以下步骤：步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；步骤二：采集子AUV相对于主AUV的距离和方位信息；采集的子AUV与主AUV之间的相对距离为：$R=\sqrt{{\left(r\right)}^2+{\left(r_1\right)}^2-2{\mathrm{rr}}_1\cos (\theta -{\theta }_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^{-}={\Phi }_{k,k-1}{\hat{S}}_{k-1}+T_{k-1}{U}_{k-1}+G_{k-1}{W}_{\text{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(r_k-r_{1,k})\cos ({\theta }_k-{\theta }_{1,k})}{R_k}& \frac{r_k{r}_{1,k}\sin ({\theta }_k-{\theta }_{1,k})}{R_k}& 0\\ \frac{-r_{1,k}\cos ({\theta }_k-{\theta }_{1,k})}{R_k^2}& \frac{r_k(r_k-r_{1,k})\cos ({\theta }_k-{\theta }_{1,k})}{R_k^2}& 1\end{array}\right];$当rcosθ‑r₁cosθ₁＞0时，量测矩阵为：$H_k=\left[\begin{array}{ccc}\frac{r_k(r_k-r_{1,k})\cos ({\theta }_k-{\theta }_{1,k})}{R_k}& \frac{r_k{r}_{1,k}\sin ({\theta }_k-{\theta }_{1,k})}{R_k}& 0\\ \frac{-r_{1,k}\cos ({\theta }_k-{\theta }_{1,k})}{R_k^2}& -\frac{r_k(r_k-r_{1,k})\cos ({\theta }_k-{\theta }_{1,k})}{R_k^2}& -1\end{array}\right].$