专利名称：一种用于水下滑翔器的高精度组合导航定位方法
一种用于水下滑翔器的高精度组合导航定位方法技术领域
本发明属于导航技术领域，涉及水下滑翔器的导航定位，为一种用于水下滑翔器 的高精度组合导航定位方法，该方法具有高可靠性、高精度、高稳定性及实时性等特点，可 实现水下滑翔器的准确定位和自主导航。
背景技术：
对于水下滑翔器的定位与导航，为了达到低功耗、低成本、长航时的目的，要求导 航传感器的数量尽量少，基于微型机电系统MEMS的惯性测量单元IMU以其体积小、重量轻、 低功耗等优点，加之其不受外界干扰，能在短期内提供较高精度，被作为水下滑翔器的首选 导航元件。然而基于MEMS的MU本身存在一些固有器件误差，加上热噪声、电子干扰等，会 造成非静态的随机误差漂移和MEMS器件的标度因子呈较严重的非线性。在运行期间，这 些非线性误差会增长，基于MEMS的惯性导航系统INS的性能也会随之下降，在没有额外的 校正时位置误差会累积增长。对于大多数水面导航，全球定位系统GPS已经是很成熟且可 靠的技术，但在水下，GPS可能遇到失锁、信号阻断、多径效应问题。所以航位推算算法将对 INS的误差校正起到至关重要的作用。
尽管卡尔曼滤波KF通过去除INS误差中线性部分来补偿系统输出，但KF是适用 于由高斯白噪声所产生的线性误差状态模型，对基于MEMS的INS的非线性动态模型，它的 效果并不佳。扩展卡尔曼滤波EKF通过去掉高次项强制将非线性模型近似成线性化模型。 但这种线性化在高维非线性模型中将会导致滤波器的发散，效果较差。无迹卡尔曼滤波UKF 从原理上克服了 EKF的缺陷，在非线性系统中比EKF要优很多，但是UKF在计算非线性变换 的随机变量统计性时会有较大计算量。
因水下滑翔器导航与定位要求低功耗、长航时、高精度、小体积等，所以制约了器 件的选择，而目前已有的算法均有不足。如何结合水下滑翔器导航的背景，在数据处理、算 法优化、滤波方式等方面进行算法设计以达到高精度、高实时性、长航时、高稳定性等，在国 内外还没有太多报道。发明内容
本发明要解决的问题是水下滑翔器导航与定位要求低功耗、长航时、高精度、小 体积等，所以制约了器件的选择，而目前已有的算法均有不足，需要一种用于水下滑翔器的 导航定位方法，克服传统组合导航系统由于误差随时间累积而不能高精度长航时实时导航 与定位的欠缺，在数据处理、算法优化、滤波方式等方面达到高精度、高实时性、长航时和高 稳定性。
本发明的技术方案为一种用于水下滑翔器的高精度组合导航定位方法，以惯性 测量单元IMU为导航原件，包括以下步骤
I)将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器，去除掉粗 大噪声，完成数据的粗处理；
2)对粗处理后的数据进行细处理，所述细处理为计算粗处理后得到的数据的高斯 分布均值和方差；3)采用基于龙格一库塔法Runge-Kutta的航位推算DR,推算每个更新周期内滑翔 器的位置、速度和姿态；4)设置两级滤波系统，第一级滤波为自适应卡尔曼滤波AKF，第二级滤波是无迹卡 尔曼滤波UKF，自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益，通过自适应强跟 踪的扩展卡尔曼滤波EKF预测均方误差，所述滤波增益与预测均方误差共同调节得到估计 均方误差，将所述估计均方误差代入无迹卡尔曼滤波UKF，得到两级滤波系统，两级滤波系 统对输入数据进行UKF滤波；5)将步骤2)得到的高斯分布均值和方差，以及步骤3)得到的推算数据融合进行 UKF滤波，由UKF估出的误差反馈回惯性测量单元IMU，校正惯性测量单元IMU的累积误差， 维持惯性测量单元MU的精度，输出准确的位置、速度和姿态信息，实现稳定的导航定位。步骤3)的基于龙格一库塔法Runge-Kutta的航位推算DR具体为设航位推算 的采样周期为T，在采样周期内进行m次速度、位置和姿态的信息更新，每次更新的周期为
权利要求
1.一种用于水下滑翔器的高精度组合导航定位方法，其特征是以惯性测量单元IMU为导航原件，包括以下步骤 1)将惯性测量单元IMU的输出数据输入无限脉冲响应IIR低通滤波器，去除掉粗大噪声，完成数据的粗处理； 2)对粗处理后的数据进行细处理，所述细处理为计算粗处理后得到的数据的高斯分布均值和方差； 3)采用基于龙格一库塔法Runge-Kutta的航位推算DR，推算每个更新周期内滑翔器的位置、速度和姿态； 4)设置两级滤波系统，第一级滤波为自适应卡尔曼滤波AKF，第二级滤波是无迹卡尔曼滤波UKF，自适应卡尔曼滤波AKF通过自适应模糊控制调节滤波增益，通过自适应强跟踪的扩展卡尔曼滤波EKF预测均方误差，所述滤波增益与预测均方误差共同调节得到估计均方误差，将所述估计均方误差代入无迹卡尔曼滤波UKF，得到两级滤波系统，两级滤波系统对输入数据进行UKF滤波； 5)将步骤2)得到的高斯分布均值和方差，以及步骤3)得到的推算数据融合进行UKF滤波，由UKF估出的误差反馈回惯性测量单元IMU，校正惯性测量单元IMU的累积误差，维持惯性测量单元IMU的精度，输出准确的位置、速度和姿态信息，实现稳定的导航定位。
2.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法，其特征是步骤3)的基于龙格一库塔法Runge-Kutta的航位推算DR具体为设航位推算的采样周期为T，在采样周期内进行m次速度、位置和姿态的信息更新，每次更新的周期为λ =工，采样 m前速度为％，在采样周期内每次更新的速度为J=I, 2，....，m，采样后速度为Vm，则在时间t内更新的速度由式(b)递推
3.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法，其特征是步骤4)中第一级滤波自适应卡尔曼滤波AKF具体为 a)卡尔曼方程中滤波增益方程为&(札4,+A)-S式中Pk/H为k时刻的理论预测均方误差，Hk为k时刻的理论量测矩阵，Rk为k时刻的理论量测噪声的方差矩阵；自适应卡尔曼滤波AKF采用模糊自适应算法调节滤波增益Kk，记.V,: = HkPk^Hl + Rk，C1c=HHt +R[，式中V' k/H为k时刻的实际预测均方误差，H' k*k时刻的实际量测矩阵，Ri kSk时刻的实际量测噪声的方差矩阵，Sk表示k时刻更新序列的理论协方差，Ck表不k时刻实际协方差，Sk = HlcPkfk^Hl +Rk ;记deItak=Ck-Sk, deltak为模糊逻辑控制器的输入，Rk作为输出，如果delta,不变则Rk维持不变；如果delta,大于零则Rk减小；如果delta,小于零则Rk增大，通过自适应调节Rk，自适应调节滤波增益 b)自适应卡尔曼滤波AKF通过自适应强跟踪扩展卡尔曼滤波EKF来调节理论预测均方误差Ρρη，设自适应强跟踪EKF的状态估计为JT4 s :hl+Kk(Zk -H(Xk^l)-Fk),式中足Α—, =Φ .—为状态一步预测，Φ,,η为k-1到k时刻的系统一步转移矩阵，U k-Ι时刻的状态预测，Kk为滤波增益，Zk为k时刻的量测向量，h( ·)为观测函数，rk为量测方程中带时变均值和协方差的高斯白噪声序列的均值；预测均方误差为 i =4—Au,+β—,，式中Qlri为量测方程中带时变均值和协方差的高斯白噪声序列的协方差，为k-Ι时刻的自适应渐消因子，采用时变的自适应渐消因子对过去的数据进行渐消，以减弱陈旧数据对当前滤波值的影响，通过实时调整状态预测误差的协方差矩阵以及相应的增益矩阵来实现减弱影响这一目标，自适应渐消因子由下式确定[i Λ),a- < i其中 λ Q，k=max{l，tr[Nk]/tr[Mk]},
4.根据权利要求1所述的一种用于水下滑翔器的高精度组合导航定位方法，其特征是步骤4)中第二级滤波无迹卡尔曼滤波UKF具体为考虑驱动噪声对系统的影响，在UKF中进行状态扩维，加入量测噪声Rk、系统噪声Qk及自适应卡尔曼滤波AKF得到的估计均方误差Pk/k进行状态扩维，组成新的协方差矩阵，进行扩维后的状态变量和协方差矩阵为 状态变量K =[4 wkT Vl ]Γ式中Xk表示k时刻的系统状态向量，Wk表示k时刻的系统噪声，Vk为k时刻的量测噪声; 协方差矩阵
全文摘要
本发明公布了一种用于水下滑翔器的高精度组合导航定位方法，惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度的数据，融合基于Runge-Kutta法(RK4)的航位推算得到的数据，再经自适应卡尔曼AKF和无迹卡尔曼UKF二级滤波，滤波后反馈回IMU来校正IMU的累积误差，最终输出较准的位置速度姿态等信息。本发明用于水下滑翔器的自主导航，具有高精度、实时性、稳定性等优点，使得整体导航系统能长航时、高精度、低功耗稳定运行，能迅速准确地得到水下航行器当前位姿信息，同时为其提供航迹和位置参数。
文档编号G01C21/16GK103033186SQ201210585170
公开日2013年4月10日 申请日期2012年12月30日 优先权日2012年12月30日
发明者陈熙源, 黄浩乾, 刘虎, 周智恺, 唐建, 方琳 申请人:东南大学