CN103033186B - 一种用于水下滑翔器的高精度组合导航定位方法 - Google Patents
一种用于水下滑翔器的高精度组合导航定位方法 Download PDFInfo
- Publication number
- CN103033186B CN103033186B CN201210585170.1A CN201210585170A CN103033186B CN 103033186 B CN103033186 B CN 103033186B CN 201210585170 A CN201210585170 A CN 201210585170A CN 103033186 B CN103033186 B CN 103033186B
- Authority
- CN
- China
- Prior art keywords
- moment
- speed
- filter
- square error
- ukf
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 47
- 238000005259 measurement Methods 0.000 claims abstract description 43
- 230000003044 adaptive effect Effects 0.000 claims abstract description 25
- 238000012545 processing Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 36
- 230000008569 process Effects 0.000 claims description 18
- 238000005070 sampling Methods 0.000 claims description 15
- 230000004807 localization Effects 0.000 claims description 11
- 230000003190 augmentative effect Effects 0.000 claims description 10
- 238000005562 fading Methods 0.000 claims description 9
- 230000001186 cumulative effect Effects 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 230000003313 weakening effect Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 2
- 230000004044 response Effects 0.000 claims description 2
- 230000008901 benefit Effects 0.000 abstract description 3
- 230000000694 effects Effects 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 4
- 230000007812 deficiency Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 108010076504 Protein Sorting Signals Proteins 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Landscapes
- Navigation (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公布了一种用于水下滑翔器的高精度组合导航定位方法,惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度的数据,融合基于Runge-Kutta法(RK4)的航位推算得到的数据,再经自适应卡尔曼AKF和无迹卡尔曼UKF二级滤波,滤波后反馈回IMU来校正IMU的累积误差,最终输出较准的位置速度姿态等信息。本发明用于水下滑翔器的自主导航,具有高精度、实时性、稳定性等优点,使得整体导航***能长航时、高精度、低功耗稳定运行,能迅速准确地得到水下航行器当前位姿信息,同时为其提供航迹和位置参数。
Description
技术领域
本发明属于导航技术领域,涉及水下滑翔器的导航定位,为一种用于水下滑翔器的高精度组合导航定位方法,该方法具有高可靠性、高精度、高稳定性及实时性等特点,可实现水下滑翔器的准确定位和自主导航。
背景技术
对于水下滑翔器的定位与导航,为了达到低功耗、低成本、长航时的目的,要求导航传感器的数量尽量少,基于微型机电***MEMS的惯性测量单元IMU以其体积小、重量轻、低功耗等优点,加之其不受外界干扰,能在短期内提供较高精度,被作为水下滑翔器的首选导航元件。然而基于MEMS的IMU本身存在一些固有器件误差,加上热噪声、电子干扰等,会造成非静态的随机误差漂移和MEMS器件的标度因子呈较严重的非线性。在运行期间,这些非线性误差会增长,基于MEMS的惯性导航***INS的性能也会随之下降,在没有额外的校正时位置误差会累积增长。对于大多数水面导航,全球定位***GPS已经是很成熟且可靠的技术,但在水下,GPS可能遇到失锁、信号阻断、多径效应问题。所以航位推算算法将对INS的误差校正起到至关重要的作用。
尽管卡尔曼滤波KF通过去除INS误差中线性部分来补偿***输出,但KF是适用于由高斯白噪声所产生的线性误差状态模型,对基于MEMS的INS的非线性动态模型,它的效果并不佳。扩展卡尔曼滤波EKF通过去掉高次项强制将非线性模型近似成线性化模型。但这种线性化在高维非线性模型中将会导致滤波器的发散,效果较差。无迹卡尔曼滤波UKF从原理上克服了EKF的缺陷,在非线性***中比EKF要优很多,但是UKF在计算非线性变换的随机变量统计性时会有较大计算量。
因水下滑翔器导航与定位要求低功耗、长航时、高精度、小体积等,所以制约了器件的选择,而目前已有的算法均有不足。如何结合水下滑翔器导航的背景,在数据处理、算法优化、滤波方式等方面进行算法设计以达到高精度、高实时性、长航时、高稳定性等,在国内外还没有太多报道。
发明内容
本发明要解决的问题是:水下滑翔器导航与定位要求低功耗、长航时、高精度、小体积等,所以制约了器件的选择,而目前已有的算法均有不足,需要一种用于水下滑翔器的导航定位方法,克服传统组合导航***由于误差随时间累积而不能高精度长航时实时导航与定位的欠缺,在数据处理、算法优化、滤波方式等方面达到高精度、高实时性、长航时和高稳定性。
本发明的技术方案为:一种用于水下滑翔器的高精度组合导航定位方法,以惯性测量单元IMU为导航原件,包括以下步骤:
1)将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器,去除掉粗大噪声,完成数据的粗处理;
2)对粗处理后的数据进行细处理,所述细处理为计算粗处理后得到的数据的高斯分布均值和方差;
3)采用基于龙格-库塔法Runge-Kutta的航位推算DR,推算每个更新周期内滑翔器的位置、速度和姿态;
4)设置两级滤波***,第一级滤波为自适应卡尔曼滤波AKF,第二级滤波是无迹卡尔曼滤波UKF,自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益,通过自适应强跟踪的扩展卡尔曼滤波EKF预测均方误差,所述滤波增益与预测均方误差共同调节得到估计均方误差,将所述估计均方误差代入无迹卡尔曼滤波UKF,得到两级滤波***,两级滤波***对输入数据进行UKF滤波;
5)将步骤2)得到的高斯分布均值和方差,以及步骤3)得到的推算数据融合进行UKF滤波,由UKF估出的误差反馈回惯性测量单元IMU,校正惯性测量单元IMU的累积误差,维持惯性测量单元IMU的精度,输出准确的位置、速度和姿态信息,实现稳定的导航定位。
步骤3)的基于龙格-库塔法Runge-Kutta的航位推算DR具体为:设航位推算的采样周期为T,在采样周期内进行m次速度、位置和姿态的信息更新,每次更新的周期为采样前速度为v0,在采样周期内每次更新的速度为vj,j=1,2,....,m,采样后速度为vm,则在时间t内更新的速度由式(b)递推:
推导得到速度vj,然后再代入式(c)中:
式中ve,vn,vu分别表示推算后东北天三向速度,H,P,R分别表示航向角、俯仰角和横滚角,然后根据三个方向的速度再推算位移,这样在每个更新周期得到速度、位置和姿态信息。
步骤4)中第一级滤波自适应卡尔曼滤波AKF具体为:
a)卡尔曼方程中滤波增益方程为式中Pk/k-1为k时刻的理论预测均方误差,Hk为k时刻的理论量测矩阵,Rk为k时刻的理论量测噪声的方差矩阵;自适应卡尔曼滤波AKF采用模糊自适应算法调节滤波增益Kk,记 式中P′k/k-1为k时刻的实际预测均方误差,H′k为k时刻的实际量测矩阵,R′k为k时刻的实际量测噪声的方差矩阵,Sk表示k时刻更新序列的理论协方差,Ck表示k时刻实际协方差,记deltak=Ck-Sk,deltak为模糊逻辑控制器的输入,Rk作为输出,如果deltak不变则Rk维持不变;如果deltak大于零则Rk减小;如果deltak小于零则Rk增大,通过自适应调节Rk,自适应调节滤波增益
b)自适应卡尔曼滤波AKF通过自适应强跟踪扩展卡尔曼滤波EKF来调节理论预测均方误差Pk/k-1,设自适应强跟踪EKF的状态估计为式中为状态一步预测,Φk,k-1为k-1到k时刻的***一步转移矩阵,为k-1时刻的状态预测,Kk为滤波增益,Zk为k时刻的量测向量,h(·)为观测函数,rk为量测方程中带时变均值和协方差的高斯白噪声序列的均值;预测均方误差为式中Qk-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差,λk-1为k-1时刻的自适应渐消因子,采用时变的自适应渐消因子对过去的数据进行渐消,以减弱陈旧数据对当前滤波值的影响,通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现减弱影响这一目标,自适应渐消因子由下式确定:
其中λ0,k=max{1,tr[Nk]/tr[Mk]},
ρ为遗忘因子,lk≥1为弱化因子,取lk=1-d(k),dk=(1-ρ)/(1-ρk+1);ω1、ωk为残差序列;
c)由a)和b)更新得到的Kk和Pk/k-1共同调节,代入Pk/k=(I-KkHk)Pk/k-1,得到估计均方误差Pk/k,并实时自适应调节估计均方误差Pk/k。
步骤4)中第二级滤波无迹卡尔曼滤波UKF具体为:考虑驱动噪声对***的影响,在UKF中进行状态扩维,加入量测噪声Rk、***噪声Qk及自适应卡尔曼滤波AKF得到的估计均方误差Pk/k进行状态扩维,组成新的协方差矩阵,进行扩维后的状态变量和协方差矩阵为:
状态变量:
式中Xk表示k时刻的***状态向量,Wk表示k时刻的***噪声,Vk为k时刻的量测噪声;
协方差矩阵:
将步骤2)得到的高斯分布均值和方差,以及步骤3)DR得到的推算数据融合进行UKF滤波,利用初始状态估计,设定最初的2n+1个Sigma点,用过程模型变化这些Sigma点,即对状态变量进行U变换,通过UKF滤波计算出滤波增益、预测误差和估计均方误差信息,最终估出的速度、位置、姿态等误差反馈回IMU,校正IMU的累积误差。
本发明克服现有技术的不足,提供一种用于水下滑翔器的高精度组合导航定位方法,克服了传统组合导航***由于误差随时间累积而不能高精度长航时实时导航与定位的欠缺,并采取多级数据处理、精准航位推算算法、智能自适应卡尔曼滤波、多级滤波等,完成了水下滑翔器高精度长航时、高实时性、稳定自主导航与定位。本发明与现有技术相比的优点在于:
(1)IIR低通滤波器能有效的去除原始数据中的粗大噪声,而且能根据实际情况调节滤波阶数及参数保证最佳性能,对于水下导航这种非线性环境比较适用;
(2)将IMU的输出数据求高斯分布的均值和方差,比直接求数据的均值和方差更贴近实际数据的输出,能更好得拟合实际数据输出模型,为后续的算法处理提供更准确的输入。
(3)基于RK4的航位推算法,能在每个更新阶段较准确地推算出其速度信息,从而得到位移。比传统的直接用经纬度来求位移误差大大减小;
(4)AKF用两步自适应来调节估计均方误差,而该误差是UKF中用到的关键参数,从而减少了因***的不确定性及非线性导致的参数偏差。
(5)对于水下导航的特殊环境,UKF成为适用于该环境较佳的选择,将上述处理后的数据融合做差作为状态量和观测量,再结合调整后的参数,能很好地估计误差,反馈到IMU来校正误差,得到精确的速度、位置、姿态等信息,达到高精度导航与定位的目的。
附图说明
图1为航位推算速度分解示意图。
图2为本发明的多级滤波原理框图。
图3为本发明方法框图。
具体实施方式
本发明公布了一种用于水下滑翔器的高精度组合导航定位方法,涉及的内容包括惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度数据;基于Runge-Kutta(RK4)法的航位推算DR;分级滤波算法自适应卡尔曼AKF和无迹卡尔曼UKF;经两次处理后的IMU输出数据与DR输出融合作为UKF的输入,滤波后反馈回IMU来校正IMU的累积误差。
本发明包括以下步骤:
(1)IMU输出的原始数据带有比较大的噪声,通过IIR低通滤波器可以去除高频噪声。但截止频率不能过低,不然会滤掉有用信号导致实际滤波结果有偏差。IIR低通滤波器的设计为:
式(a)中,r(K)是导航***惯性传感器的输出信号序列,s(K)是低通滤波器的输出序列,即滤波结果,K为自然数,n表示IIR滤波器的阶数,以4阶IIR为例,参数b0、ai、bi,i=1,....,4能使用不同的方法计算得到,如插值法等;根据实际情况选用不同阶数以达到理想效果。IIR低通滤波器的设计为公知技术,不再详述。
IIR滤波器能抑制高频信号,具有实时迭代结构,比模块处理的小波变换法计算量要小得多,和后续的基于卡尔曼的滤波方式组合在一起具有更简单的结构。该滤波器工作在频域范围,和小波变换不同,它更是关注于有用信号的提取,滤除比真实输出信号更高的粗大噪声。
(2)通过粗处理后,求处理后的数据的高斯分布均值和方差。粗处理后再求高斯分布均值和方差更接近实际噪声的分布,比用求平均数来求均值和方差要精确得多,提高了整个***的精度。数据的概率密度为:均值μ表示绝大多数数据所处的值,方差σ表示绝大多数数据偏离均值的幅度值,作为后续滤波的重要参数能更准确的描述原始数据的特性。
(3)航位推算DR作为唯一辅助INS的导航方式,对整个***的导航精度起着至关重要的作用。如图1,本发明用基于RK4航位推算法准确推算每个更新周期内滑翔器的位置、速度、姿态等。设采样时间为T,在该周期内进行m次速度、位置、姿态等信息更新,每次更新的周期为采样前速度为v0,在采样周期内每次更新的速度为vj,j=1,2,....,m,采样后速度为vm。则在时间t内更新的速度可按方程组(b)递推:
方程组(b)为由基于Runge-Kutta(RK4)法计算速度,其中f()为RK4函数,为本领域技术人员公知,不再详述。
推出速度vj,然后再代入方程组(c)中:
方程组(c)即为航位推算,式中ve,vn,vu分别表示推算后东北天三向速度,H,P,R分别表示航向角、俯仰角和横滚角。然后根据三个方向的速度再推算位移。这样在每个更新周期能得到较准确的速度、位置和姿态信息,为后续的误差补偿奠定了基础。
(4)采用两级滤波算法。如图2,第一级滤波是AKF(自适应卡尔曼滤波),第二级滤波是UKF(无迹卡尔曼滤波)。AKF调整参数分为两步,第一步采用模糊自适应算法调节增益Kk。记 Sk表示更新序列的理论协方差,Ck表示实际协方差,式中Pk/k-1为k时刻的理论预测均方误差,Hk为k时刻的理论量测矩阵,Rk为k时刻的理论量测噪声的方差矩阵,P′k-1为k时刻的实际预测均方误差,H′k为k时刻的实际量测矩阵,R′k为k时刻的实际量测噪声的方差矩阵。记deltak=Ck-Sk。deltak为模糊逻辑控制器的输入,Rk作为输出。如果deltak基本不变则Rk维持不变;如果deltak大于零则Rk减小;如果deltak小于零则Rk增大。这样可以通过自适应调节Rk,达到自适应调节滤波增益的目的。
(5)AKF的第二步是通过自适应强跟踪EKF(扩展卡尔曼滤波)来调节预测均方误差Pk/k-1。状态估计为预测均方误差为 式中为状态一步预测,Φk,k-1为k-1时刻到k时刻的***一步转移矩阵,为k-1时刻的状态预测,Kk为滤波增益,Zk为k时刻的量测向量,h(·)为观测函数,rk为量测方程中带时变均值和协方差的高斯白噪声序列的均值,Qk-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差。λk-1≥1为k-1时刻的自适应渐消因子,采用时变的渐消因子对过去的数据进行渐消,减弱陈旧数据对当前滤波值的影响,通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现这一目标。自适应渐消因子可由下式确定:
其中λ0,k=max{1,tr[Nk]/tr[Mk]}, tr[·]表示求矩阵的迹;
Ω0,k表示新息方差阵:
ρ为遗忘因子,lk≥1为弱化因子,取lk=1-d(k),dk=(1-ρ)/(1-ρk+1),ω1、ωk为残差序列。
(6)由(5)调节得到的预测均方误差Pk/k-1的和(4)调节得到的滤波增益Kk代入Pk/k=(I-KkHk)Pk/k-1,得到估计均方误差Pk/k。考虑驱动噪声对***的影响,在UKF中进行状态扩维,将噪声项添加到状态项中,将量测噪声Rk、***噪声Qk及步骤(5)中得到的估计均方误差Pk/k进行状态扩维,组成新的协方差矩阵,进行扩维后的状态变量和协方差矩阵为:
式中Xk和前面一样,表示k时刻的***状态向量,Wk表示k时刻的***噪声,Vk为k时刻的量测噪声。
将DR输出的数据和IMU细处理后输出的数据融合进行UKF滤波,利用初始状态估计,设定最初的2n+1个Sigma点,用过程模型变化这些Sigma点,即对状态变量进行U变换。依次进行下述的计算:
计算预测估计值
计算预测协方差P(k+1/k);
通过测量方程计算测量值Zi(k+1/k);
计算预测测量值
计算信息方差Pzz(k+1/k);
计算和Z(k+1/k)的协方差Pxz(k+1/k);
计算卡尔曼增益K(k+1);
更新误差协方差P(k+1/k+1);
更新状态
上述计算过程为公知的UKF通用算法流程,不再详述。
经过上述步骤,滤波增益、预测误差、估计均方误差等信息被计算出,最终估出的速度、位置、姿态等误差反馈回IMU,校正IMU的累积误差。
对于***的状态模型和量测模型多是非线性的水下滑翔器导航***,UKF摒弃了对非线性函数进行线性化的传统做法,采用卡尔曼线性滤波框架,对于一步预测方程,使用无迹变换来处理均值和协方差的非线性传递,采用非线性滤波的UKF方法,明显提高了导航精度。
下面结合附图对发明的技术方案进行详细说明:
如图3,IMU单元将三轴陀螺、三轴加速度计、三轴高性能磁力计集成与一体,该单元体积小,集成度高,功耗低,带有自校正和标定功能。滑翔器在水下运行时,IMU的输出带有大的噪声,先经过IIR低通滤波器粗处理过程滤掉高频噪声,截止频率不能选太低,否则有用信号将会被滤掉,可以通过实际调试来确定。滤波器的阶数根据实际情况调整,保证最好的滤波效果,随着滤波阶数的确定,构建了滤波器的整体框架,之后可以确定相关系数完成粗滤波过程。
之后,进行数据细处理,将输出的数据求其高斯分布的均值及方差,这种求法能更拟合实际的输出情况,所得的均值和方差更贴近实际值。
水下滑翔器除了IMU之外,没有GPS等辅助导航方式,惯性导航器件的漂移不断累积使得IMU的输出误差越来越大,此时用航位推算DR的方法来估计实际的速度、位置、姿态等信息。位置漂移直接决定滑翔器的定位精度,所以准确的推算位置至关重要。将一个采样周期根据具体需要划分成若干更新周期,这个划分数量要适宜,过多会造成计算量的过大,不利于实时更新,而且滑翔器滑翔速度本来比较缓慢,更新次数过多也没有必要;过少会造成累积误差比较大,精度会有所降低,可在实际实施时通过测试调整来确定。在每个更新周期内利用RK4来精确推算速度,再用姿态解算算出姿态,两者结合推出较准确的位置。
经过前述的过程后,要进入滤波阶段,水下滑翔器在较复杂的环境中,模型成非线性且多变,传统的卡尔曼滤波KF滤波在此情况下可能不能达到较好效果,本发明运用两级滤波进行,即自适应卡尔曼滤波AKF和无迹卡尔曼滤波UKF。AKF又分为两步,一步是用自适应模糊控制法调节滤波增益,一步是用自适应强跟踪EKF(扩展卡尔曼滤波)来调节预测均方误差。调节后的滤波增益和预测均方误差再共同作用更新估计均方误差。
UKF考虑了各噪声对***的影响,将噪声项添加到状态项中,即状态扩维,并将调节后的均方误差、***噪声、量测噪声合并,组成新的协方差矩阵。IMU输出和DR输出做差作为状态量和观测量进入UKF进行滤波,以卡尔曼线性滤波为框架,确定Sigma点的个数、位置和相应的权值,使用无迹变换来处理均值和协方差,对非线性函数的概率密度分布进行近似,用一系列确定样本来逼近状态的后验概率密度,推算出较精确的位置、速度、姿态等误差,反馈回IMU进行校正,最终输出高精度的位置姿态等信息。本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
Claims (3)
1.一种用于水下滑翔器的高精度组合导航定位方法,其特征是以惯性测量单元IMU为导航元件,包括以下步骤:
1)将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器,去除掉粗大噪声,完成数据的粗处理;
2)对粗处理后的数据进行细处理,所述细处理为计算粗处理后得到的数据的高斯分布均值和方差;
3)采用基于龙格-库塔法Runge-Kutta的航位推算DR,推算每个更新周期内滑翔器的位置、速度和姿态;
4)设置两级滤波***,第一级滤波为自适应卡尔曼滤波AKF,第二级滤波是无迹卡尔曼滤波UKF,自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益,通过自适应强跟踪的扩展卡尔曼滤波EKF预测均方误差,所述滤波增益与预测均方误差共同调节得到估计均方误差,将所述估计均方误差代入无迹卡尔曼滤波UKF,得到两级滤波***,两级滤波***对输入数据进行UKF滤波;
第一级滤波自适应卡尔曼滤波AKF具体为:
a)卡尔曼方程中滤波增益方程为式中Pk/k-1为k时刻的理论预测均方误差,Hk为k时刻的理论量测矩阵,Rk为k时刻的理论量测噪声的方差矩阵;自适应卡尔曼滤波AKF采用模糊自适应算法调节滤波增益Kk,记Ck=H′kP′k/k-1H′T k+R′k,式中P′k/k-1为k时刻的实际预测均方误差,H′k为k时刻的实际量测矩阵,R′k为k时刻的实际量测噪声的方差矩阵,Sk表示k时刻更新序列的理论协方差,Ck表示k时刻实际协方差,记deltak=Ck-Sk,deltak为模糊逻辑控制器的输入,Rk作为输出,如果deltak不变则Rk维持不变;如果deltak大于零则Rk减小;如果deltak小于零则Rk增大,通过自适应调节Rk,自适应调节滤波增益
b)自适应卡尔曼滤波AKF通过自适应强跟踪扩展卡尔曼滤波EKF来调节理论预测均方误差Pk/k-1,设自适应强跟踪EKF的状态估计为式中为状态一步预测,Φk,k-1为k-1到k时刻的***一步转移矩阵,为k-1时刻的状态预测,Kk为滤波增益,Zk为k时刻的量测向量,h(·)为观测函数,rk为量测方程中带时变均值和协方差的高斯白噪声序列的均值;预测均方误差为式中Qk-1为量测方程中带时变均值和协方差的高斯白噪声序列的协方差,λk-1为k-1时刻的自适应渐消因子,采用时变的自适应渐消因子对过去的数据进行渐消,以减弱陈旧数据对当前滤波值的影响,通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现减弱影响这一目标,自适应渐消因子由下式确定:
其中λ0,k=max{1,tr[Nk]/tr[Mk]},
ρ为遗忘因子,lk≥1为弱化因子,取lk=1-d(k),dk=(1-ρ)/(1-ρk+1);ω1、ωk为残差序列;
c)由a)和b)更新得到的Kk和Pk/k-1共同调节,代入Pk/k=(I-KkHk)Pk/k-1,得到估计均方误差Pk/k,并实时自适应调节估计均方误差Pk/k;
5)将步骤2)得到的高斯分布均值和方差,以及步骤3)得到的推算数据融合进行UKF滤波,由UKF估出的误差反馈回惯性测量单元IMU,校正惯性测量单元IMU的累积误差,维持惯性测量单元IMU的精度,输出准确的位置、速度和姿态信息,实现稳定的导航定位。
2.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法,其特征是步骤3)的基于龙格-库塔法Runge-Kutta的航位推算DR具体为:设航位推算的采样周期为T,在采样周期内进行m次速度、位置和姿态的信息更新,每次更新的周期为采样前速度为v0,在采样周期内每次更新的速度为vj,j=1,2,....,m,采样后速度为vm,则在时间t内更新的速度由式(b)递推:
推导得到速度vj,然后再代入式(c)中:
式中ve,vn,vu分别表示推算后东北天三向速度,H,P分别表示航向角和俯仰角,然后根据三个方向的速度再推算位移,这样在每个更新周期得到速度、位置和姿态信息。
3.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法,其特征是步骤4)中第二级滤波无迹卡尔曼滤波UKF具体为:考虑驱动噪声对***的影响,在UKF中进行状态扩维,加入量测噪声Rk、***噪声Qk及自适应卡尔曼滤波AKF得到的估计均方误差Pk/k进行状态扩维,组成新的协方差矩阵,进行扩维后的状态变量和协方差矩阵为:
状态变量:
式中Xk表示k时刻的***状态向量,Wk表示k时刻的***噪声,Vk为k时刻的量测噪声;
协方差矩阵:
将步骤2)得到的高斯分布均值和方差,以及步骤3)DR得到的推算数据融合进行UKF滤波,利用初始状态估计,设定最初的2n+1个Sigma点,用过程模型变化这些Sigma点,即对状态变量进行U变换,通过UKF滤波计算出滤波增益、预测误差和估计均方误差信息,最终估出的速度、位置、姿态等误差反馈回IMU,校正IMU的累积误差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210585170.1A CN103033186B (zh) | 2012-12-30 | 2012-12-30 | 一种用于水下滑翔器的高精度组合导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210585170.1A CN103033186B (zh) | 2012-12-30 | 2012-12-30 | 一种用于水下滑翔器的高精度组合导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103033186A CN103033186A (zh) | 2013-04-10 |
CN103033186B true CN103033186B (zh) | 2015-04-01 |
Family
ID=48020323
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201210585170.1A Expired - Fee Related CN103033186B (zh) | 2012-12-30 | 2012-12-30 | 一种用于水下滑翔器的高精度组合导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103033186B (zh) |
Families Citing this family (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103759742B (zh) * | 2014-01-22 | 2017-04-05 | 东南大学 | 基于模糊自适应控制技术的捷联惯导非线性对准方法 |
CN104880200B (zh) * | 2014-05-13 | 2017-12-22 | 北京航天计量测试技术研究所 | 复合制导***初始姿态现场校准***及方法 |
CN105468882A (zh) * | 2014-07-28 | 2016-04-06 | 航天恒星科技有限公司 | 卫星自主定轨方法及*** |
CN104112079A (zh) * | 2014-07-29 | 2014-10-22 | 洛阳理工学院 | 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法 |
CN106767778A (zh) * | 2016-11-15 | 2017-05-31 | 哈尔滨工程大学 | 一种气垫船船位推算控制方法 |
CN106767792A (zh) * | 2017-01-16 | 2017-05-31 | 东南大学 | 一种水下滑翔器导航***及高精度姿态估计方法 |
CN107289933B (zh) * | 2017-06-28 | 2019-08-20 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
CN107894234A (zh) * | 2017-11-22 | 2018-04-10 | 哈尔滨工业大学 | 一种基于双向滤波的监控导航方法 |
CN108169744B (zh) * | 2017-12-08 | 2022-06-24 | 中国船舶重工集团公司第七二四研究所 | 一种地波雷达与卫星海洋动力反演信息融合处理方法 |
CN110366095B (zh) * | 2019-05-28 | 2020-06-19 | 浙江大学 | 一种水下节点分布式定位***及方法 |
CN110602723B (zh) * | 2019-08-27 | 2022-05-03 | 华侨大学 | 一种基于水下边缘设备的两级双向预测获取数据方法 |
CN110567490B (zh) * | 2019-08-29 | 2022-02-18 | 桂林电子科技大学 | 一种大失准角下sins初始对准方法 |
CN110673148A (zh) * | 2019-10-25 | 2020-01-10 | 海鹰企业集团有限责任公司 | 一种主动声纳目标实时航迹解算方法 |
CN110865333B (zh) * | 2019-11-19 | 2021-08-24 | 浙江大学 | 洋流影响下水下滑翔机单信标无源声学定位方法 |
CN111273084B (zh) * | 2020-02-24 | 2022-03-04 | 广东电网有限责任公司广州供电局 | 一种自适应强跟踪无迹卡尔曼滤波同步相量参数估计方法 |
CN111366962A (zh) | 2020-03-12 | 2020-07-03 | 国家深海基地管理中心 | 一种深远海低成本长航时协同导航定位*** |
CN111773558B (zh) * | 2020-07-01 | 2021-05-11 | 苏州雷泰医疗科技有限公司 | 一种使用光栅伺服剂量率的方法、装置及放射治疗设备 |
CN112504275B (zh) * | 2020-11-16 | 2022-09-02 | 哈尔滨工程大学 | 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法 |
CN112595320B (zh) * | 2020-11-23 | 2023-06-30 | 北京联合大学 | 基于ros的室内智能轮椅高精定位自主导航方法及*** |
CN114683259A (zh) * | 2020-12-28 | 2022-07-01 | 财团法人工业技术研究院 | 机械手臂校正***及机械手臂校正方法 |
TW202224872A (zh) | 2020-12-28 | 2022-07-01 | 財團法人工業技術研究院 | 機械手臂校正系統及機械手臂校正系統方法 |
CN114763994B (zh) * | 2021-05-06 | 2024-01-30 | 苏州精源创智能科技有限公司 | 一种应用于扫地机器人的惯性姿态导航*** |
CN113472318B (zh) * | 2021-07-14 | 2024-02-06 | 青岛杰瑞自动化有限公司 | 一种顾及观测模型误差的分级自适应滤波方法及*** |
CN113670314B (zh) * | 2021-08-20 | 2023-05-09 | 西南科技大学 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
CN116625360B (zh) * | 2023-07-19 | 2023-09-29 | 苏州精源创智能科技有限公司 | 一种基于强跟踪模型的扫地机打滑识别和修正方法 |
CN118171050B (zh) * | 2024-05-14 | 2024-07-12 | 河海大学 | 用于组合导航***的低计算成本非线性卡尔曼滤波方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN202382747U (zh) * | 2011-12-16 | 2012-08-15 | 东南大学 | 一种用于小型水下滑翔器的组合导航装置 |
CN102692223A (zh) * | 2012-06-27 | 2012-09-26 | 东南大学 | 用于wsn/ins组合导航的多级非线性滤波器的控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7739045B2 (en) * | 2006-05-31 | 2010-06-15 | Honeywell International Inc. | Rapid self-alignment of a strapdown inertial system through real-time reprocessing |
-
2012
- 2012-12-30 CN CN201210585170.1A patent/CN103033186B/zh not_active Expired - Fee Related
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN202382747U (zh) * | 2011-12-16 | 2012-08-15 | 东南大学 | 一种用于小型水下滑翔器的组合导航装置 |
CN102692223A (zh) * | 2012-06-27 | 2012-09-26 | 东南大学 | 用于wsn/ins组合导航的多级非线性滤波器的控制方法 |
Non-Patent Citations (2)
Title |
---|
MSINS/GPS组合导航***及其数据融合技术研究;马云峰;《中国优秀博士学位论文全文数据库信息科技辑》;20070415(第4期);第843-846页 * |
Study on the Design and Algorithm of INS/MCP/DR Integrated Navigation Method for Underwater Glider;Huang Haoqian etc.;《2012 Third International Conference on Digital Manufacturing & Automation》;20120802;正文第4,42,57-58,96-97,112页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103033186A (zh) | 2013-04-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103033186B (zh) | 一种用于水下滑翔器的高精度组合导航定位方法 | |
CN103217175B (zh) | 一种自适应容积卡尔曼滤波方法 | |
CN103630137B (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
CN102519450B (zh) | 一种用于水下滑翔器的组合导航装置及方法 | |
CN105785999B (zh) | 无人艇航向运动控制方法 | |
CN101464152B (zh) | 一种sins/gps组合导航***自适应滤波方法 | |
CN101033973B (zh) | 微小型飞行器微惯性组合导航***的姿态确定方法 | |
CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
CN101846734B (zh) | 农用机械导航定位方法、***及农用机械工控机 | |
CN104020480B (zh) | 一种带自适应因子的交互式多模型ukf的卫星导航方法 | |
CN106291645A (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
CN103063212B (zh) | 一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法 | |
CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
CN107063245B (zh) | 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 | |
CN103759742A (zh) | 基于模糊自适应控制技术的捷联惯导非线性对准方法 | |
CN103399336B (zh) | 一种非高斯噪声环境下gps/sins组合导航方法 | |
CN112432644B (zh) | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 | |
CN101871782A (zh) | 基于set2fnn的gps/mems-ins组合导航***定位误差预测方法 | |
CN104121907A (zh) | 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法 | |
CN102749633A (zh) | 一种卫星导航接收机的动态定位解算方法 | |
CN103292812A (zh) | 一种微惯性sins/gps组合导航***的自适应滤波方法 | |
CN102162733A (zh) | 一种基于svm的auv舰位推算导航误差实时修正方法 | |
CN102854798A (zh) | 一种船舶动力定位参数自适应观测器的在线调整方法 | |
CN103792562A (zh) | 一种基于变换采样点的强跟踪ukf的滤波方法 | |
CN107515414A (zh) | 一种电离层闪烁下基于矢量跟踪的自适应卡尔曼滤波方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20150401 |