一种具有鲁棒特性的协同导航滤波方法技术

本发明专利技术公开了一种具有鲁棒特性的协同导航滤波方法。该方法是利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型，提高了从艇的定位精度。本发明专利技术的具有鲁棒特性的协同导航滤波方法通过结合投影统计异常点鉴别方法和改进Huber估计法提高在量测噪声异常情况下的协同导航从艇的定位精度，增强滤波算法的鲁棒性。该方法的特点是当量测噪声为近似高斯分布并且出现异常值时滤波仍可以保持较高的精度，具有鲁棒性。

A cooperative navigation filtering method with robust characteristics

The invention discloses a collaborative navigation filtering method with robust characteristics. This method is the use of projection statistics of the received from the boat from the main boat measurement information of abnormal judgment for measuring information weights, an improved robust cooperative navigation filtering model based on Huber estimation to improve the accuracy of positioning from the boat. The invention has the characteristics of robust collaborative navigation filtering method by projection statistical outlier discrimination method and improved Huber estimation of measurement noise under abnormal conditions of cooperative navigation from boat positioning accuracy in quantity and improve the method, robust filtering algorithm. The characteristic of the method is that the equivalent measurement noise is an approximate Gauss distribution, and the filtering can still maintain a high accuracy when the outliers are detected.

【技术实现步骤摘要】

本专利技术涉及导航
，具体地说，涉及一种具有鲁棒特性的协同导航滤波方法。
技术介绍
自主水下航行器(AutonomousUnderwaterVehicles,AUV)在民用还是军事方面都有广泛的应用。随着对海洋的探索越来越深入，使得AUV的使命也变得越来越复杂化和多样化，如果仅仅依靠单个AUV去实现这些功能，不仅在成本上会提高很多，而且可靠性也会降低，而由多台水下AUV组成的协作系统具有空间分布、功能分布、冗余等特点。然而不论单AUV还是多AUV系统以及其执行何种任务，导航系统的优劣直接影响系统的性能好坏。协同导航定位技术作为一种新的导航定位方法具有只对有限数目的AUV增加设备费从而来提高编队的导航精度等优点。由于水下环境和基于声学定位的特殊性，协同导航滤波过程中的量测噪声容易出现异常，传统算法中是将量测噪声视为高斯分布，降低了从AUV的定位精度。
技术实现思路
本专利技术提出了一种新的协同导航方法，利用投影统计法对量测值进行预处理再结合改进的Huber估计形式提高协同导航滤波中对量测异常的鲁棒性，该方法适合于在量测噪声异常环境下时对协同编队进行定位，提高了从AUV的定位精度。根据本专利技术的一个方面，提供一种具有鲁棒特性的协同导航滤波方法，包括以下步骤：(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：1)获得k个N维量测向量的中位向量，表示为：其中，xk表示k个N维量测向量，k＝1，…K,xnk代表xk中第n个元素，m代表k个向量中的中位向量，2)计算每个量测向量关于中值偏差的单位向量，其中，偏差向量uk：uk＝xk-m,k＝1....K由此得到量测向量关于中值偏差的单位向量vk为：3)确定每个量测向量xk在各单位向量vk上的投影：第k个量测向量的投影向量表示为zk＝[z1k,z2k,...zKk]T，其中，H表示向量x转置，4)对每个zk，通过中值和完全中值偏差来计算标准投影pnk，其中，C为调节参数，MADk表示第k个值的完全中值偏差，5)对所有k值重复上述2)、3)、4)计算过程，则第K个点的PS值为得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点权值ωi；(2)获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型：由协同导航的运动方程和量测方程得到非线性模型：其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与k均服从零均值白噪声，系统噪声协方差阵为Q，量测噪声协方差阵为R，量测方程近似写作：因此协同导航非线性模型改写为：其中状态预测为表示实际值与预测值的差，矩阵H定义为：定义以下式子：则非线性模型可另写为：zk＝Gkxk+ξk定义残差表示估计值和量测值之间的差，ζ＝Gx-z，定义函数则对角矩阵其中wi为量测值的权值，由步骤5)计算得到，为了与状态预测和量测残差相一致，将对角矩阵Ψ分为两部分：将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性。2.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新的过程如下：时间更新Pk/k-1＝Φk,k-1Pk-1ΦTk,k-1+Qk量测更新其中Φk,k-1为状态过渡矩阵，Ψy与Ψx为重新构造观测模型后利用Huber估计求得的对角矩阵，为协同导航从艇的位置估计。3.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航运动方程的建立方法是：记两领航者AUV的位置坐标分别为从AUV的位置坐标设为(xS,yS,zS)，同时由于深度信息可自测，为简化模型投影至二维空间，则从AUV运动方程可表示为：其中V(k)表示k时刻前向合成速度，为k时刻航向角，ω(k)表示k时刻偏航角速度，T为采样周期，假设实际模型中传感器量测输入均受到高斯白噪声干扰，此时系统输入模型为：其中V(k)，ω(k)分别为前向合成速度以及偏航角速度的真实量测信息，分别为服从高斯分布且相互独立的前向速度噪声以及偏航角速度噪声，其方差阵分别为噪声协方差阵表示为：因此AUV运动方程简化表示为：Xk+1＝f(xk,uk,wk)＝xk+Γ(uk+wk)式中表示从AUV在k时刻的状态；Γ(uk+wk)为非线性项，wk为高斯白噪声。4.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤(2)中，协同导航量测方程的建立方法是：由于装备了深度计，所以各个AUV的深度信息可以认为是已知的，假设已经获取了从AUV与两个领航者AUV的距离，又称为斜距分别为r1和r2，则可以通过几何关系得到下式：其中P1和P2分别为领航者1和领航者2与从AUV的二维水平距离，同时P1与P2又可表示为：P1和P2做为量测值，量测方程可表示如下：上式中vk表示在时刻k时的量测噪声，假设量测噪声服从零均值相互独立的高斯白噪声，其方差阵表示为：5.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，在步骤5)中，得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度赋予该量测点权值的方法是，通过公式计算得到。6.根据权利要求5所述的具有鲁棒特性的协同导航滤波方法，其特征在于，步骤5)中的取值6.0025。附图说明通过结合下面附图对其实施例进行描述，本专利技术的上述特征和技术优点将会变得更加清楚和容易理解。图1是表示本专利技术实施例的一种具有鲁棒特性的协同导航滤波方法的示意图。具体实施方式下面将参考附图来描述本专利技术所述的具有鲁棒特性的协同导航滤波方法的实施例。本领域的普通技术人员可以认识到，在不偏离本专利技术的精神和范围的情况下，可以用各种不同的方式或其组合对所描述的实施例进行修正。因此，附图和描述在本质上是说明性的，而不是用于限制权利要求的保护范围。此外，在本说明书中，附图未按比例画出，并且相同的附图标记表示相同的部分。本专利技术的协同导航方法利用投影统计法对量测值进行预处理再结合改进的Huber估计形式，能够提高协同导航滤波中对量测异常的鲁棒性。下面结合附图1详细说明该协同导航方法。步骤S101，协同导航的量测值输入，即测量主艇的位置信息，并发送给从艇。步骤S102：在协同导航从艇定位中，对从艇接收到的来自主艇的量测信息与储存的前时刻的量测值进行投影统计比较，做异常判断，包括以下分步骤：1)从艇包含有前K-1个时刻的N维量测向量，而来自主艇的K个时刻的N维量测向量，记作：xk，k＝1，…K,其中xnk代表xk中第n个元素，m代表k个向量中的中位向量，表示为：2)计算每个量测向量关于中值偏差的单位向量，偏差向量uk：uk＝xk-m,k＝1....K关于偏差向量的单位向量vk为：3)确定每个量测向量xk在各单位向量上vk的投影：其中，H表示向量x的转置第k个量测向量的投影向量表示为zk＝[z1k,z2k,...zKk]T，4)对每个zk，通过中值和完全中值偏差来计算标准投影pnk其中，c为一调节参数，其正常取值为1.4826，MADk表示第k个值的完全中值偏差。5)对所有k值重复上述步骤2)、3)、4)的计算过程，则第K个点的PS值为得到PS值后，根据该量测点的量测值相对于量测点本文档来自技高网...

【技术保护点】
一种具有鲁棒特性的协同导航滤波方法，其特征在于，包括以下步骤：(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：1)获得k个N维量测向量的中位向量，表示为：m=med(x1k)k=1,...K...med(xnk)k=1,...K]]>其中，xk表示k个N维量测向量，k＝1，…K,xnk代表xk中第n个元素，m代表k个向量中的中位向量，2)计算每个量测向量关于中值偏差的单位向量，其中，偏差向量uk：uk＝xk‑m,k＝1....K由此得到量测向量关于中值偏差的单位向量vk为：vk=uk||uk||2=uku1k2+...uNk2,k=1....K]]>3)确定每个量测向量xk在各单位向量vk上的投影：z1k=x1Hvkz2k=x2Hvk...zik=xiHvk,k,i=1....Kx]]>第k个量测向量的投影向量表示为zk＝[z1k,z2k,...zKk]T，其中，H表示向量x转置，4)对每个zk，通过中值和完全中值偏差来计算标准投影pnk，zmedk=medn=1,...Kznk]]>MADk=c·medn=1,...K|znk-zmedk|]]>pnk=|znk-zmedk|MADk,n=1....K]]>其中，C为调节参数，MADk表示第k个值的完全中值偏差，5)对所有k值重复上述2)、3)、4)计算过程，则第K个点的PS值为PSk=maxk=1,...,K{pnk},n=1....K]]>得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点权值ωi；(2)获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型：由协同导航的运动方程和量测方程得到非线性模型：其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与k均服从零均值白噪声，系统噪声协方差阵为Q，量测噪声协方差阵为R，量测方程近似写作：因此协同导航非线性模型改写为：yk-h(x‾k)+Hkx‾kx‾k=HkIxk+wk-δk]]>其中状态预测为表示实际值与预测值的差，矩阵H定义为：Hk=∂h∂x|x=x‾k,w=w‾]]>定义以下式子：Tk=Rk00Pk/k-1]]>zk=Tk-1/2yk-h(x‾k)+Hxx‾kx‾k]]>Gk=Tk-1/2HkI]]>ξk=Tk-1/2wk-δk]]>则非线性模型可另写为：zk＝Gkxk+ξk定义残差表示估计值和量测值之间的差，ζ＝Gx‑z，定义函数则对角矩阵其中wi为量测值的权值，由步骤5)计算得到，为了与状态预测和量测残差相一致，将对角矩阵Ψ分为两部分：Ψ=Ψy00Ψx]]>将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性。...

【技术特征摘要】
1.一种具有鲁棒特性的协同导航滤波方法，其特征在于，包括以下步骤：(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：1)获得k个N维量测向量的中位向量，表示为：m=med(x1k)k=1,...K...med(xnk)k=1,...K]]>其中，xk表示k个N维量测向量，k＝1，…K,xnk代表xk中第n个元素，m代表k个向量中的中位向量，2)计算每个量测向量关于中值偏差的单位向量，其中，偏差向量uk：uk＝xk-m,k＝1....K由此得到量测向量关于中值偏差的单位向量vk为：vk=uk||uk||2=uku1k2+...uNk2,k=1....K]]>3)确定每个量测向...

【专利技术属性】
技术研发人员：卢少然，范爱中，李晟，
申请(专利权)人：中冶华天南京电气工程技术有限公司，
类型：发明
国别省市：江苏;32