一种闭环正向滤波结合反向平滑的POS后处理方法技术

本发明专利技术属于POS后处理方法，具体涉及一种闭环正向滤波结合反向平滑的POS后处理方法。目的是提供一种高精度的位置姿态后处理方法。包括：卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其中，将正向Kalman滤波估计出的水平速度误差和高度误差，通过反馈控制参数实时反馈到惯性导航解算的输入端，进而抑制惯性速度和位置的发散，提高误差模型的线性度，并在此基础上对滤波估计量进行反向R-T-S平滑，从而得到POS的高精度位置姿态后处理结果。优点在于：满足POS的应用需求，提供高精度的位置和姿态信息给相关图像处理系统(例如包括雷达，或者相机)等等。

【技术实现步骤摘要】

本专利技术涉及一种适用于POS (Position and Orientation System)的高精度位置姿态后处理方法。
技术介绍
POS对整个任务过程中的位置和姿态精度都有极高的要求，特别是长时间工作以后，惯性导航误差短时间内增长较快，系统的误差模型逐渐不精确，导致系统的滤波效果变差，通常采用的正向Kalman滤波与反向R-T-S(Rauch-Tung-Striebel)平滑相结合的后处理方法无法解决这一问题。
技术实现思路
本专利技术的目的是提供一种高精度的位置姿态后处理方法，通过采用闭环实时反馈方法，设计合理的控制参数，有效抑制系统的误差振荡幅值，提高误差模型的精确度，从而提高POS整个任务过程中的位置和姿态精度。本专利技术是这样实现的一种闭环正向滤波结合反向平滑的POS后处理方法，是对POS的位置姿态数据进行后处理的方法，包括卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其中，在卡尔曼滤波的过程中，建立卡尔曼滤波模型的过程如下步骤1、使用二阶系统模型对惯导系统进行描述；设计惯导系统水平通道成典型二阶系统； (I \V1 (^) = S1 +K1S + K2 +— g0(I) V式中=KpK2是水平通道的两个实时反馈控制系数；设计惯导系统垂向通道成典型二阶系统；V； (^) = 52+C1^+ C2-^(2) I Re J式中&、C2是垂向通道的两个实时反馈控制系数；典型二阶系统的特征方程为V(s) = s2 +2ξ wns+ W2n(3)根据POS系统事后处理过程对滤波调节周期和超调量的要求，确定ξ、wn ;接着按如下过程计算反馈控制系数；1、根据系统的性能要求确定滤波调节周期和超调量；2、根据调节周期和超调量，确定ξ、Wn ;3、分别将式(I)、⑵与式(3)进行同系数对比，即得到公式(4)、(5)，按照公式(4)、(5)解算得反馈控制系数1、K2、Q、C2 ;K1 = 2 ξ Wn权利要求1.一种闭环正向滤波结合反向平滑的POS后处理方法，是对POS的位置姿态数据进行后处理的方法，包括卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其特征在于在卡尔曼滤波的过程中，建立卡尔曼滤波模型的过程如下步骤1、使用二阶系统模型对惯导系统进行描述；设计惯导系统水平通道成典型二阶系统；全文摘要本专利技术属于POS后处理方法，具体涉及一种闭环正向滤波结合反向平滑的POS后处理方法。目的是提供一种高精度的位置姿态后处理方法。包括卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其中，将正向Kalman滤波估计出的水平速度误差和高度误差，通过反馈控制参数实时反馈到惯性导航解算的输入端，进而抑制惯性速度和位置的发散，提高误差模型的线性度，并在此基础上对滤波估计量进行反向R-T-S平滑，从而得到POS的高精度位置姿态后处理结果。优点在于满足POS的应用需求，提供高精度的位置和姿态信息给相关图像处理系统(例如包括雷达，或者相机)等等。文档编号G01C21/16GK102997915SQ20111027304公开日2013年3月27日 申请日期2011年9月15日 优先权日2011年9月15日专利技术者周东灵, 李文耀, 尚克军, 张勤拓, 扈光锋, 周祖洋, 刘辉, 谢仕民, 邱宏波, 刘峰 申请人:北京自动化控制设备研究所本文档来自技高网...

【技术保护点】
一种闭环正向滤波结合反向平滑的POS后处理方法，是对POS的位置姿态数据进行后处理的方法，包括：卡尔曼滤波的步骤，和反向R?T?S平滑的步骤，其特征在于：在卡尔曼滤波的过程中，建立卡尔曼滤波模型的过程如下：步骤1、使用二阶系统模型对惯导系统进行描述；设计惯导系统水平通道成典型二阶系统；▿1′(s)=s2+K1s+(K2+1Re)g0---(1)式中：K1、K2是水平通道的两个实时反馈控制系数；设计惯导系统垂向通道成典型二阶系统；▿2′(s)=s2+C1s+(C2-2g0Re)---(2)式中：C1、C2是垂向通道的两个实时反馈控制系数；典型二阶系统的特征方程为▿(s)=s2+2ξwns+wn2---(3)根据POS系统事后处理过程对滤波调节周期和超调量的要求，确定ξ、wn；接着按如下过程计算反馈控制系数；1、根据系统的性能要求确定滤波调节周期和超调量；2、根据调节周期和超调量，确定ξ、wn；3、分别将式(1)、(2)与式(3)进行同系数对比，即得到公式(4)、(5)，按照公式(4)、(5)解算得反馈控制系数K1、K2、C1、C2；K1＝2ξwn(4)K2+1Re=wn2C1＝2ξwn(5)C2-2g0Re=wn2步骤2、建立POS的误差方程：式中：vN、vU、vE分别为惯导系统的北向速度、垂向速度、东向速度，单位：米/秒，为上一时刻的量；δvN、δvU、δvE分别为惯导系统的北向速度误差、垂向速度误差、东向速度误差，单位：米/秒，为上一时刻的量；为惯导系统的纬度，单位：弧度，为上一时刻的量；为惯导系统的纬度误差，单位：弧度，为上一时刻的量；h为惯导系统的高度，单位：米，为上一时刻的量；δh为惯导系统的高度误差，单位：米，为上一时刻的量；φU、φE分别为惯导系统的垂向失调角、东向失调角，单位：弧度，为上一时刻的量；fU、fE分别为垂向加速度、东向加速度，单位：米/秒2，为当前时刻的量；分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误 差、z轴加速度计零偏误差，单位：米/秒2，为当前时刻的量；C1j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；RM、RN分别为地球子午圈、卯酉圈半径，单位：米，为上一时刻的量；ωie为地球自转角速率，单位：弧度/秒，为常量；为Kalman滤波估计的系统北速误差，单位：米/秒，为当前时刻的量；K1为水平通道的实时反馈控制系数，由上述过程计算得到；式中：fN为北向加速度，单位：米/秒2，为当前时刻的量；φN为惯导系统的北向失调角，单位：弧度，为上一时刻的量；C2j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；为Kalman滤波估计的系统高度误差，单位：米，为当前时刻的量；C2为垂向通道的实时反馈控制系数，由上述过程计算得到；式中：C3j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；为Kalman滤波估计的系统东速误差，单位：米/秒，为当前时刻的量；式中：C1为垂向通道的实时反馈控制系数，由上述过程计算得到；(6g)式中：分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒；(6h)(6i)步骤3、建立卡尔曼滤波模型；取状态变量观测量其中上标G表示GPS提供的信息；λG分别为GPS提供的纬度、经度，单位：弧度；hG为GPS提供的高度，单位：米；分别为GPS提供的北向速度、垂向速度、东向速度，单位：米/秒；λ为惯导系统的经度，单位：弧度；根据给出的误差方程，确定系统的状态方程为式中：A(t)为15×15维系统参数矩阵，根据式(6)计算；L(t)为15×3维控制系数矩阵，L(t)＝[03×2?L3×7?03×6]T，其中L3×7=-C10-C200000-K10000-K2000-K1K200;U(t)=δh^(t)δv^N(t)δv^E(t)T为系统控制向量；W(t)为15×1维系统激励噪声向量；量测方程为：Z＝HX(t)+V(t)????????????????????????????????????????????????(8)式中：H为6×15维量测矩阵；V(t)为量测噪声向量；将状态方程和量测方程离散化为：Xk＝Φk，k?1Xk?1+LkUk?1+Wk?1????????????????????(9)Zk＝HXk+Vk??????????????????????????????????????(10)&Ph...

【技术特征摘要】

【专利技术属性】
技术研发人员：周东灵，李文耀，尚克军，张勤拓，扈光锋，周祖洋，刘辉，谢仕民，邱宏波，刘峰，
申请(专利权)人：北京自动化控制设备研究所，
类型：发明
国别省市：