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

摘要

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

主权项

1.一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，其特征是：(1)建立AUV运动学模型把AUV作为一个质点运动，(x,y,z)是AUV在k时刻的坐标，z是深度，由深度传感器直接测量，AUV运动学模型描述为：式中：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,则有：式中：和是独立的零均值的高斯白噪声，即协方差和也是独立的；分别表示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的距离在水平空间中描述为：式中：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时刻的状态，忽略二阶以上的高阶项，得到线性化方程为：式中：是关于的Jacobian矩阵，用F_k表示，即：将非线性函数h(X_k,k)围绕滤波值展成泰勒级数，是由k‑1时刻估计出的AUV在k时刻的状态，略去二阶以上项，得：式中：是h(X_k/k‑1,k)关于X_k/k‑1的Jacobian矩阵，采用H_k表示，即：(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的状态由k时刻预测k+1时刻的协方差P_k+1/k：式中：P_k是k时刻的协方差，Q_k是k时刻的系统噪声协方差矩阵；更新等式：由预测和更新等式，提出线性估计：式中：r_k+1是k+1时刻的主从AUV间的量测距离；是由k时刻估计出k+1时刻的主从AUV间的距离；是k+1时刻量测值与k时刻估计值之差；增益K_k+1等式为：P_k+1＝(I‑K_k+1H_k+1)P_k+1/k式中：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时刻的状态：Q、R的优化是在每个采样周期对Q1、Q2、Q3、R1、R2这5个参数进行调整，当运用蚁群算法时，一组参数对应一条路径；当运用粒子群算法时，一组参数对应一个粒子，具体操作步骤如下：首先采用蚁群算法进行首次遍历；采用粒子群算法，有效的得到全局最优值；所述的粒子群算法采用交叉策略，即进行以下两个步骤：a、当前解与个体极值交叉；b、当前解与群体极值交叉；实际采样数据z与估计值的均方误差最小为目标，适应度函数obj设置为：式中：M代表周期个数；再次利用蚁群算法，将当前解集置为蚁群初始出发点，然后根据蚁群中蚂蚁获得的解的质量的好坏，从其中选出部分优秀的蚂蚁，然后按它们解的优劣程度来加权平均释放信息素，以增强蚂蚁之后探索最优解的能力：将目前解集设置为蚁群的初始出发点，在所有蚂蚁均构建完成一组参数之后，信息素蒸发按下式操作：式中：τ_ij表示t时刻i个参数与j个参数边上的信息素强度；ρ代表信息素的蒸发率；E为赋权边的集合；τ(λ_f)为λ‑分支因子的阈值函数，其定义为：式中：τ_min取值为1/Cⁿⁿ，Cⁿⁿ是由最近邻方法得到的均方误差值；τ(t)为迭代次数t的函数：τ(t)＝0.9*(1/(ρ*C^bs)‑τ_ij(t))/ρ；在这次迭代中排名第r的蚂蚁(r≤ω‑1)根据下式更新其路径上信息素含量：式中：由下式给出：T^r为排名第r的蚂蚁在目前迭代中所建立的路径；C^r为路径的长度；此时信息素的更新规则为：式中：bs为迭代最优蚂蚁或至今最优蚂蚁；目前为止最优蚂蚁被调用的频率由下式给出:始终把t为迭代次数；如果更新后τ_ij≥1/ρC^bs，则令τ_ij＝1/ρC^bs(i,j＝1,2,3,…n)。