本发明专利技术的目的在于提供一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，首先建立AUV运动学模型，然后线性化AUV模型，采用改进的蚁群算法对Q矩阵R矩阵进行最优估计：先采用蚁群算法进行首次遍历，产生大量解；采用粒子群算法找到全局最优值；再次利用蚁群算法，将当前解集置为蚁群初始出发点，然后根据蚁群中蚂蚁获得的解的质量的优劣，选出部分最优秀的蚂蚁按其解的优劣程度加权平均释放信息素。最终，将求解出来的Q,R运用到EKF中，从而实现对从AUV的定位。本发明专利技术巧妙地将智能算法与EKF相结合，不仅解决了噪声矩阵的不确定、难选择的问题，而且提高了EKF的滤波精度，应用于多AUV的定位系统中，大大提高了对从AUV定位的精确度。
【技术实现步骤摘要】
本专利技术涉及的是一种AUV的协同定位方法。
技术介绍
随着研究海洋区域的不断扩大化、军事需求的不断复杂化，仅仅依靠单体水下航行器(AUV)独立工作将无法完成预期的目标，因此多AUV协作系统便应运而生，并逐渐成为AUV发展的一种主流趋势和方向。多AUV协作系统对定位精度有着更高的要求，如每个个体AUV不仅要保持自己高精度的定位，而且还要时刻了解周围“伙伴”的位置信息以及自身在协作系统中的相对位置。倘若AUV群中的每一个个体都配备高精度的导航设备，这将大大增加成本。因此人们提出了多AUV的协同定位技术，即采用少数高精度的主AUV来辅助其他配备低成本、低精度导航设备的从AUV实现高精度的定位。如何通过主、从AUV之间的相互通信来提高多AUV整体的定位精度已成为国内外水下航行器导航领域研究的一个重点方向。在协同定位系统中，为了提高AUV定位的准确度和精度引入了许多估计算法，广泛采用的是卡尔曼滤波。卡尔曼滤波技术发展于20世纪60年代，在协同定位系统中有卓有成效的应用，而扩展卡尔曼滤波(EKF)是目前在应用上最广泛的一种非线性滤波算法。但使用EKF进行定位最主要的问题之一就是噪声矩阵的选取，定位的精度以及收敛性在很大程度上取决于使用者对噪声矩阵的先验了解，也就是系统噪声矩阵和量测噪声矩阵事先规划的精度，如果选择不当，EKF的性能将会大大降低，甚至不能正常工作。
技术实现思路
本专利技术的目的在于提供可以通过配有高精度导航定位设备的主AUV准确有效的定位低配从AUV位置的一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法。本专利技术的目的是这样实现的：本专利技术一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，其特征是：(1)建立AUV运动学模型把AUV作为一个质点运动，(x,y,z)是AUV在k时刻的坐标，z是深度，由深度传感器直接测量，AUV运动学模型描述为：式中：xk、xk+1分别为k、k+1时刻x方向的位置坐标；yk、yk+1分别为k、k+1时刻y方向的位置坐标；T是采样周期，Vk是由DVL测得的速度，φk是测量的航向角；此模型中，用uk表示[Vkφk]T,则有：式中：和是独立的零均值的高斯白噪声，即协方差和也是独立的；分别表示k时刻速度和航向角的量测值；这个模型描述为：Xk+1＝f(xk,uk,wk)＝Xk+g(uk+wk)＝f(Xk,k)+g(uk+wk)式中：Xk+1是AUV在k+1时刻的状态；Xk＝f(Xk,k)＝(xkykφ)T是AUV在K时刻的状态；g(uk+wk)是个非线性项；wk是系统噪声，且是具有零均值的高斯白噪声；设Q为系统噪声协方差矩阵；在时刻tk，从AUV收到了主AUV的距离在水平空间中描述为：式中：xs,k,xm,k分别是从AUV和主AUVk时刻x方向的位置坐标；ys,k,ym,k分别是从AUV和主AUVk时刻y方向的位置坐标；rk是它们在水平空间中的距离；lk是量测噪声，噪声为零均值的高斯白噪声；设R为系统量测噪声方差阵。故这个模型可描述为：rk＝h(Xk,k)+lk式中：h(Xk,k)表示主、从AUV水平面上的距离，且有(2)线性化AUV模型：对于AUV运动模型Xk+1式，首先将f(Xk,k)围绕滤波值展开成泰勒级数，是所估计出AUV在K时刻的状态，忽略二阶以上的高阶项，得到线性化方程为：式中：是关于的Jacobian矩阵，用Fk表示，即：将非线性函数h(Xk,k)围绕滤波值展成泰勒级数，是由k-1时刻估计出的AUV在k时刻的状态，略去二阶以上项，得：式中：是h(Xk/k-1,k)关于Xk/k-1的Jacobian矩阵，采用Hk表示，即：(3)优化Q、R：在AUV运动学模型中有3个状态变量xk,yk,φk和2个输出变量xk+1,yk+1，则系统噪声和量测噪声的协方差矩阵分别取Q＝diag[Q1,Q2,Q3],R＝diag[R1,R2]，其中Q1、Q2、Q3为与xk,yk,φk对应的系统噪声矩阵参数；R1、R2为与xk+1,yk+1对应的量测噪声矩阵参数；(4)EKF求解：状态预测：由k时刻预测k+1时刻从AUV的状态由k时刻预测k+1时刻的协方差Pk+1/k：式中：Pk为路是k时刻的协方差，Qk是k时刻的系统噪声协方差矩阵；更新等式：由预测和更新等式，提出线性估计：式中：rk+1是k+1时刻的主从AUV间的量测距离；是由k时刻估计出k+1时刻的主从AUV间的距离；是k+1时刻量测值与k时刻估计值之差；增益Kk+1等式为：Pk+1＝(I-Kk+1Hk+1)Pk+1/k式中：Hk+1是h(Xk+1/k,k+1)关于Xk+1/k的Jacobian矩阵；Pk+1为路是k+1时刻的协方差；Sk+1表示等式右边两者之和；评估从AUV在k+1时刻的状态：本专利技术还可以包括：1、Q、R的优化是在每个采样周期对Q1、Q2、Q3、R1、R2这5个参数进行调整，当运用蚁群算法时，一组参数对应一条路径；当运用粒子群算法时，一组参数对应一个粒子，具体操作步骤如下：首先采用蚁群算法进行首次遍历；采用粒子群算法，有效的得到全局最优值；所述的粒子群算法采用交叉策略，即进行以下两个步骤：a、当前解与个体极值交叉；b、当前解与群体极值交叉；实际采样数据z与估计值的均方误差最小为目标，适应度函数obj设置为：式中：M代表周期个数；再次利用蚁群算法，将当前解集置为蚁群初始出发点，然后根据蚁群中蚂蚁获得的解的质量的好坏，从其中选出部分优秀的蚂蚁，然后按它们解的优劣程度来加权平均释放信息素，以增强蚂蚁之后探索最优解的能力：将目前解集设置为蚁群的初始出发点，在所有蚂蚁均构建完成一组参数之后，信息素蒸发按下式操作：式中：τij表示t时刻i个参数与j个参数边上的信息素强度；ρ代表信息素的蒸发率；E为赋权边的集合；τ(λ_f)为λ-分支因子的阈值函数，其定义为：式中：τmin取值为1/Cnn，Cnn是由最近邻方法得到的均方误差值；τ(t)为迭代次数t的函数：τ(t)＝0.9*(1/(ρ*Cbs)-τij(t))/ρ；在这次迭代中排名第r的蚂蚁(r≤ω-1)根据下式更新其路径上信息素含量：式中：由下式给出：Tr为排名第r的蚂蚁在目前迭代中所建立的路径；Cr为路径的长度。此时信息素的更新规则为：式中：bs为迭代最优蚂蚁或至今最优蚂蚁；目前为止最优蚂蚁被调用的频率由下式给出:始终把t为迭代次数；如果更新后τij≥1/ρCbs，则令τij＝1/ρCbs(i,j＝1,2,3,…n)。本专利技术的优势在于：本专利技术巧妙地将智能算法与EKF相结合，不仅解决了噪声矩阵的不确定、难选择的问题，而且提高了EKF的滤波精度，应用于多AUV的定位系统中，大大提高了对从AUV定位的精确度，使得从AUV无需配备高精度导航定位设备，降低了成本。附图说明图1是AUV主从式结构示意图；图2是主从AUV位置信息示意图；图3是从AUV的实际路径与改进EKF、EKF的估计路径比较仿真图；图4是图3中的局部放大图；图5是AUV的EKF定位误差比较仿真图；具体实施方式下面结合附图举例对本专利技术做更详细地描述：结合图1-5，本专利技术包括如下步骤：步骤一：建立AUV运动学模型Step1:把AUV视为一个质点运动，(x,y,z)是AUV在k时刻的坐标。z本文档来自技高网...
【技术保护点】
一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，其特征是：(1)建立AUV运动学模型把AUV作为一个质点运动，(x,y,z)是AUV在k时刻的坐标，z是深度，由深度传感器直接测量，AUV运动学模型描述为：xk+1=xk+T·Vk·cos(φk)yk+1=yk+T·Vk·sin(φk)φk+1=φk]]>式中：xk、xk+1分别为k、k+1时刻x方向的位置坐标；yk、yk+1分别为k、k+1时刻y方向的位置坐标；T是采样周期，Vk是由DVL测得的速度，φk是测量的航向角；此模型中，用uk表示[Vk φk]T,则有：uk=Vkφk=Vmk+ωvkφmk+ωφk]]>式中：和是独立的零均值的高斯白噪声，即协方差和也是独立的；分别表示k时刻速度和航向角的量测值；这个模型描述为：Xk+1＝f(xk,uk,wk)＝Xk+g(uk+wk)＝f(Xk,k)+g(uk+wk)式中：Xk+1是AUV在k+1时刻的状态；Xk＝f(Xk,k)＝(xk yk φ)T是AUV在K时刻的状态；g(uk+wk)是个非线性项；wk是系统噪声，且是具有零均值的高斯白噪声；设Q为系统噪声协方差矩阵；在时刻tk，从AUV收到了主AUV的距离在水平空间中描述为：rk=(xs,k-xm,k)2+(ys,k-ym,k)2+lk]]>式中：xs,k,xm,k分别是从AUV和主AUV k时刻x方向的位置坐标；ys,k,ym,k分别是从AUV和主AUV k时刻y方向的位置坐标；rk是它们在水平空间中的距离；lk是量测噪声，噪声为零均值的高斯白噪声；设R为系统量测噪声方差阵。故这个模型可描述为：rk＝h(Xk,k)+lk式中：h(Xk,k)表示主、从AUV水平面上的距离，且有(2)线性化AUV模型：对于AUV运动模型Xk+1式，首先将f(Xk,k)围绕滤波值展开成泰勒级数，是所估计出AUV在K时刻的状态，忽略二阶以上的高阶项，得到线性化方程为：Xk+1=f(X^k,k)+∂f∂X^k[Xk-X^k]+g(uk+wk)]]>式中：是关于的Jacobian矩阵，用Fk表示，即：Fk=∂f∂X^k=∂f(X^k,k)∂Xk|Xk=X^k,]]>将非线性函数h(Xk,k)围绕滤波值展成泰勒级数，是由k‑1时刻估计出的AUV在k时刻的状态，略去二阶以上项，得：rk=h(X^k/k-1,k)+∂h∂Xk|X^k/k-1[Xk-X^k/k-1]+lk]]>式中：是h(Xk/k‑1,k)关于Xk/k‑1的Jacobian矩阵，采用Hk表示，即：Hk=∂h∂Xk|X^k/k-1]]>(3)优化Q、R：在AUV运动学模型中有3个状态变量xk,yk,φk和2个输出变量xk+1,yk+1，则系统噪声和量测噪声的协方差矩阵分别取Q＝diag[Q1,Q2,Q3],R＝diag[R1,R2]，其中Q1、Q2、Q3为与xk,yk,φk对应的系统噪声矩阵参数；R1、R2为与xk+1,yk+1对应的量测噪声矩阵参数；(4)EKF求解：状态预测：由k时刻预测k+1时刻从AUV的状态X^k+1/k=f(xk,uk,0)]]>由k时刻预测k+1时刻的协方差Pk+1/k：Pk+1/k=Fk·Pk·FkT+Hk·Qk·HkT]]>式中：Pk为路是k时刻的协方差，Qk是k时刻的系统噪声协方差矩阵；更新等式：由预测和更新等式，提出线性估计：r~k+1=rk+1-r^k+1/k]]>式中：rk+1是k+1时刻的主从AUV间的量测距离；是由k时刻估计出k+1时刻的主从AUV间的距离；是k+1时刻量测值与k时刻估计值之差；增益Kk+1等式为：Kk+1=Pk+1/kHk+1TSk+1-1]]>Pk+1＝(I‑Kk+1Hk+1)Pk+1/kSk+1=Hk+1Pk+1/kHk+1T+Rk+1]]>式中：Hk+1是h(Xk+1/k,k+1)关于Xk+1/k的Jacobian矩阵；Pk+1为路是k+1时刻的协方差；Sk+1表示等式右边两者之和；评估从AUV在k+1时刻的状态：X^k+1=X^k+1/k+Kk+1r~k+1.]]>...
【技术特征摘要】
1.一种基于蚁群与扩展卡尔曼滤波相结合的多AUV协同定位方法，其特征是：(1)建立AUV运动学模型把AUV作为一个质点运动，(x,y,z)是AUV在k时刻的坐标，z是深度，由深度传感器直接测量，AUV运动学模型描述为：xk+1=xk+T·Vk·cos(φk)yk+1=yk+T·Vk·sin(φk)φk+1=φk]]>式中：xk、xk+1分别为k、k+1时刻x方向的位置坐标；yk、yk+1分别为k、k+1时刻y方向的位置坐标；T是采样周期，Vk是由DVL测得的速度，φk是测量的航向角；此模型中，用uk表示[Vkφk]T,则有：uk=Vkφk=Vmk+ωvkφmk+ωφk]]>式中：和是独立的零均值的高斯白噪声，即协方差和也是独立的；分别表示k时刻速度和航向角的量测值；这个模型描述为：Xk+1＝f(xk,uk,wk)＝Xk+g(uk+wk)＝f(Xk,k)+g(uk+wk)式中：Xk+1是AUV在k+1时刻的状态；Xk＝f(Xk,k)＝(xkykφ)T是AUV在K时刻的状态；g(uk+wk)是个非线性项；wk是系统噪声，且是具有零均值的高斯白噪声；设Q为系统噪声协方差矩阵；在时刻tk，从AUV收到了主AUV的距离在水平空间中描述为：rk=(xs,k-xm,k)2+(ys,k-ym,k)2+lk]]>式中：xs,k,xm,k分别是从AUV和主AUVk时刻x方向的位置坐标；ys,k,ym,k分别是从AUV和主AUVk时刻y方向的位置坐标；rk是它们在水平空间中的距离；lk是量测噪声，噪声为零均值的高斯白噪声；设R为系统量测噪声方差阵。故这个模型可描述为：rk＝h(Xk,k)+lk式中：h(Xk,k)表示主、从AUV水平面上的距离，且有(2)线性化AUV模型：对于AUV运动模型Xk+1式，首先将f(Xk,k)围绕滤波值展开成泰勒级数，是所估计出AUV在K时刻的状态，忽略二阶以上的高阶项，得到线性化方程为：Xk+1=f(X^k,k)+∂f∂X^k[Xk-X^k]+g(uk+wk)]]>式中：是关于的Jacobian矩阵，用Fk表示，即：Fk=∂f∂X^k=∂f(X^k,k)∂Xk|Xk=X^k,]]>将非线性函数h(Xk,k)围绕滤波值展成泰勒级数，是由k-1时刻估计出的AUV在k时刻的状态，略去二阶以上项，得：rk=h(X^k/k-1,k)+∂h∂Xk|X^k/k-1[Xk-X^k/k-1]+lk]]>式中：是h(Xk/k-1,k)关于Xk/k-1的Jacobian矩阵，采用Hk表示，即：Hk=∂h∂Xk|X^k/k-1]]>(3)优化Q、R：在AUV运动学模型中有3个状态变量xk,yk,φk和2个输出变量xk+1,yk+1，则系统噪声和量测噪声的协方差矩阵分别取Q＝diag[Q1,Q2,Q3],R＝diag[R1,R2]，其中Q1、Q2、Q3为与xk,yk,φk对应的系统噪声矩阵参数；R1、R2为与xk+1,yk+1对应的量测噪声矩阵参数；(4)EKF求解：状态预测：由k时刻预测k+1时刻从AUV的状态X^k+1/k=f(xk,uk,0)]]>由k时刻预测k+1时刻的协方差Pk+1/k：Pk+1/k=Fk·Pk&C...
【专利技术属性】
技术研发人员：李娟，张娟，王宏健，张伟，马涛，邱军婷，张昆玉，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。