一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法

摘要

本发明的目的在于提供一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，首先建立AUV运动学模型，然后线性化AUV模型，采用改进的蚁群算法对Q矩阵R矩阵进行最优估计：先采用蚁群算法进行首次遍历，产生大量解；采用粒子群算法找到全局最优值；再次利用蚁群算法，将当前解集置为蚁群初始出发点，然后根据蚁群中蚂蚁获得的解的质量的优劣，选出部分最优秀的蚂蚁按其解的优劣程度加权平均释放信息素。最终，将求解出来的Q,R运用到EKF中，从而实现对从AUV的定位。本发明巧妙地将智能算法与EKF相结合，不仅解决了噪声矩阵的不确定、难选择的问题，而且提高了EKF的滤波精度，应用于多AUV的定位系统中，大大提高了对从AUV定位的精确度。

主权项

一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，其特征是：(1)建立AUV运动学模型把AUV作为一个质点运动，(x,y,z)是AUV在k时刻的坐标，z是深度，由深度传感器直接测量，AUV运动学模型描述为：$\left\{\begin{array}{c}{x}_{k+1}=x_k+T·V_k·cos\left({\phi }_k\right)\\ y_{k+1}=y_k+T·V_k·sin\left({\phi }_k\right)\\ {\phi }_{k+1}={\phi }_k\end{array}\right.$式中：x_k、x_k+1分别为k、k+1时刻x方向的位置坐标；y_k、y_k+1分别为k、k+1时刻y方向的位置坐标；T是采样周期，V_k是由DVL测得的速度，φ_k是测量的航向角；此模型中，用u_k表示[V_k φ_k]^T,则有：$u_k=\left[\begin{array}{c}{V}_k\\ {\phi }_k\end{array}\right]=\left[\begin{array}{c}{V}_{m_k}+{\omega }_{v_k}\\ {\phi }_{m_k}+{\omega }_{{\phi }_k}\end{array}\right]$式中：和是独立的零均值的高斯白噪声，即协方差和也是独立的；分别表示k时刻速度和航向角的量测值；这个模型描述为：X_k+1＝f(x_k,u_k,w_k)＝X_k+g(u_k+w_k)＝f(X_k,k)+g(u_k+w_k)式中：X_k+1是AUV在k+1时刻的状态；X_k＝f(X_k,k)＝(x_k y_k φ)^T是AUV在K时刻的状态；g(u_k+w_k)是个非线性项；w_k是系统噪声，且是具有零均值的高斯白噪声；设Q为系统噪声协方差矩阵；在时刻t_k，从AUV收到了主AUV的距离在水平空间中描述为：$r_k=\sqrt{{(x_{s,k}-x_{m,k})}^2+{(y_{s,k}-y_{m,k})}^2}+l_k$式中：x_s,k,x_m,k分别是从AUV和主AUV k时刻x方向的位置坐标；y_s,k,y_m,k分别是从AUV和主AUV k时刻y方向的位置坐标；r_k是它们在水平空间中的距离；l_k是量测噪声，噪声为零均值的高斯白噪声；设R为系统量测噪声方差阵。故这个模型可描述为：r_k＝h(X_k,k)+l_k式中：h(X_k,k)表示主、从AUV水平面上的距离，且有(2)线性化AUV模型：对于AUV运动模型X_k+1式，首先将f(X_k,k)围绕滤波值展开成泰勒级数，是所估计出AUV在K时刻的状态，忽略二阶以上的高阶项，得到线性化方程为：$X_{k+1}=f({\hat{X}}_k,k)+\frac{\partial f}{\partial {\hat{X}}_k}[X_k-{\hat{X}}_k]+g(u_k+w_k)$式中：是关于的Jacobian矩阵，用F_k表示，即：$F_k=\frac{\partial f}{\partial {\hat{X}}_k}=\frac{\partial f({\hat{X}}_k,k)}{\partial X_k}{|}_{X_k={\hat{X}}_k},$将非线性函数h(X_k,k)围绕滤波值展成泰勒级数，是由k‑1时刻估计出的AUV在k时刻的状态，略去二阶以上项，得：$r_k=h({\hat{X}}_{k/k-1},k)+\frac{\partial h}{\partial X_k}{|}_{{\hat{X}}_{k/k-1}}[X_k-{\hat{X}}_{k/k-1}]+l_k$式中：是h(X_k/k‑1,k)关于X_k/k‑1的Jacobian矩阵，采用H_k表示，即：$H_k=\frac{\partial h}{\partial X_k}{|}_{{\hat{X}}_{k/k-1}}$(3)优化Q、R：在AUV运动学模型中有3个状态变量x_k,y_k,φ_k和2个输出变量x_k+1,y_k+1，则系统噪声和量测噪声的协方差矩阵分别取Q＝diag[Q1,Q2,Q3],R＝diag[R1,R2]，其中Q1、Q2、Q3为与x_k,y_k,φ_k对应的系统噪声矩阵参数；R1、R2为与x_k+1,y_k+1对应的量测噪声矩阵参数；(4)EKF求解：状态预测：由k时刻预测k+1时刻从AUV的状态${\hat{X}}_{k+1/k}=f(x_k,u_k,0)$由k时刻预测k+1时刻的协方差P_k+1/k：$P_{k+1/k}=F_k·P_k·F_k^T+H_k·Q_k·H_k^T$式中：P_k为路是k时刻的协方差，Q_k是k时刻的系统噪声协方差矩阵；更新等式：由预测和更新等式，提出线性估计：${\tilde{r}}_{k+1}=r_{k+1}-{\hat{r}}_{k+1/k}$式中：r_k+1是k+1时刻的主从AUV间的量测距离；是由k时刻估计出k+1时刻的主从AUV间的距离；是k+1时刻量测值与k时刻估计值之差；增益K_k+1等式为：$K_{k+1}=P_{k+1/k}{H}_{k+1}^T{S}_{k+1}^{-1}$P_k+1＝(I‑K_k+1H_k+1)P_k+1/k$S_{k+1}=H_{k+1}{P}_{k+1/k}{H}_{k+1}^T+R_{k+1}$式中：H_k+1是h(X_k+1/k,k+1)关于X_k+1/k的Jacobian矩阵；P_k+1为路是k+1时刻的协方差；S_k+1表示等式右边两者之和；评估从AUV在k+1时刻的状态：${\hat{X}}_{k+1}={\hat{X}}_{k+1/k}+K_{k+1}{\tilde{r}}_{k+1}.$