一种惯性里程计组合高精度姿态测量方法技术

技术编号:12854117 阅读:80 留言:0更新日期:2016-02-11 18:32
本发明专利技术属于导航方法技术领域,具体涉及一种惯性/里程计组合高精度姿态测量方法,可应用于车载长航时组合导航,复杂地形测绘等领域。本发明专利技术技术方案以航位推算结果为基准,进行组合卡尔曼滤波计算,对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程取19个状态变量,建立了系统误差模型,利用航位推算结果,对水平失准角和水平陀螺漂移进行实时在线估计,并进行反馈修正,提高姿态精度,从而有效的提高了系统组合导航精度。

【技术实现步骤摘要】

本专利技术属于导航方法
,具体涉及一种惯性/里程计组合高精度姿态测量 方法,可应用于车载长航时组合导航,复杂地形测绘等领域。
技术介绍
对自主性、可靠性、精度等要求高的车辆导航应用场合,常使用多传感器组合导 航系统,尤其在军用场合,组合导航更是首选方案。惯性导航系统可以完全自主地提供全 面的导航信息,但其误差会随着时间不断积累。里程计作为车载导航系统中十分重要的组 成部分,其特点是成本较低,且误差不随时间积累,可以测得较精确的车速或里程,所以可 以将惯导系统与里程计进行组合,以取得较高精度的测量结果。 惯性/里程计组合的常用方法为航位推算,即利用惯导系统进行姿态测量,利用 里程计进行速度或路程测量,并经过刻度系数修正后,借助惯导系统的姿态信息,从载体坐 标系转换到导航坐标系,从而得到载体不随时间发散的、较精确的位置信息。该方法的精 度主要受里程计刻度系数和姿态精度的影响,因此如何提高姿态测量精度,成为影响惯性/ 里程计组合导航精度的主要因素。 综上,亟需提出一种基于惯性/里程计的高精度姿态测量方法,将航位推算结果 和惯性导航解算结果进行组合,利用卡尔曼滤波算法对惯导系统姿态误差和陀螺漂移误差 进行估计并修正,从而提高系统姿态测量精度。
技术实现思路
本专利技术要解决的技术问题是提出一种惯性/里程计组合高精度姿态测量方法,对 惯导系统姿态误差和陀螺漂移误差进行估计并修正,提高系统姿态测量精度,进而提高系 统组合导航精度。 为了实现这一目的,本专利技术采取的技术方案是: -种惯性里程计组合高精度姿态测量方法,包括以下步骤: (1)确定卡尔曼滤波状态方程状态变量 卡尔曼滤波状态方程取19个状态变量,分别为:北向速度误差δ Vn、天向速度误 差δ Vu、东向速度误差δ VE、讳度误差知、商度误差δ h、经度误差δ λ、北向失准角φ Ν、天 向失准角、东向失准角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 Vi.、Y加表零偏%、Z加表零偏1、航位推算纬度误差灸^、航位推算高度误差δ hD、航位 推算经度误差S λ D、里程计刻度系数误差SKD; (2)确定系统各误差方程 a)速度误差方程 式中: VN、Vu、VE-北向、天向、东向速度,单位:m/s ; RM-子午圈半径,单位:m ; RN-卯酉圈半径,单位:m ; h--掼性高度,单位:m; ωιβ--地球自转角速率,常数,取7.292115 XlO 5rad/s ; φ-纬度,单位:rad ; fN、fu、fE-滤波计算用n系下三轴比力分量,单位:m/s2 ; Cn、C21、C31、C12、C22、C 32、C13、C23、C33--姿态矩阵< 中的元素; b)位置误差方程 c)姿态角误差方程 d)陀螺和加速度计误差方程 e)航位推算位置误差方程 f)里程计刻度系数误差方程 综合式(1)~(6),可得系统状态方程如下: 式中: X(t)--系统状态变量,即上述的卡尔曼滤波状态方程19个状态变量; F--根据公式(1)~(6)确定的系统矩阵; ff(t)--系统噪声; (3)确定系统观测方程 系统观测量取导航解算位置与航位推算位置之差,则系统观测方程如下: 式中: Z(t)--观测变量; H--观测矩阵; V(t)--观测噪声; Ψ、\、h--掼性纬度、经度、高度,单位:rad,rad,m ; Ψο '' AD>hD-航位推算讳度、经度、高度,单位:rad,rad, m ; 将公式(7)和⑶离散化,可得: 式中: Xk--状态变量; --上一滤波周期至当前滤波周期的一步转移矩阵; Zk--观测量; Hk--观测矩阵; Wk--系统激励噪声序列; Vk--观测噪声序列; 一步转移矩阵〇k,k i可用下式计算: 式中: I--单位矩阵; Tn--导航周期; η--相邻两次滤波时刻之间的导航周期个数; (4)确定卡尔曼滤波递推方程组 卡尔曼滤波递推方程组如下: a)状态一步预测 b)状态估计 c)滤波增益 d) -步预测均方误差 e)估计均方误差 式中: Xh--当前滤波周期的实时状态估计值; --上一滤波周期对当前滤波周期的状态预测值; Kk--当前滤波周期的滤波增益阵; Pk/kl--上一滤波周期对当前滤波周期的预测误差估计的协方差阵; Pk--当前滤波周期的实时误差估计协方差阵; Qk!--系统噪声方差阵; Rk--观测噪声方差阵; (5)卡尔曼滤波递推计算和反馈修正 经过卡尔曼滤波递推计算后,估计出北向失准角ΦΝ、东向失准角ΦΕ、X陀螺漂移 ε x、Z陀螺漂移ε ζ ;然后对其进行反馈修正以提高姿态精;修正方法如下: a)姿态修正,用Cf表示修正后的姿态矩阵 失准角计算公式如下: Xk (i),i = 1~19,表示上述的卡尔曼滤波状态方程19个状态变量中的第i个状 态变量; 姿态修正公式如下: 式中:c代表cos, s代表sin ; b)陀螺漂移修正 陀螺漂移修正公式如下:[ 将陀螺漂移补偿到载体系下的角速率中,实时参与导航解算:[式中:表示导航周期内载体坐标系下陀螺角速率输出,单位: rad/s〇 本专利技术提出了一种惯性/里程计组合高精度姿态测量方法,主要测量车辆三维姿 态、三维速度、三维位置等导航信息。本专利技术技术方案以航位推算结果为基准,进行组合卡 尔曼滤波计算,对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程 取19个状态变量,建立了系统误差模型,利用航位推算结果,对水平失准角和水平陀螺漂 移进行实时在线估计,并进行反馈修正,提高姿态精度,从而有效的提高了系统组合导航精 度。【具体实施方式】 下面结合【具体实施方式】对本专利技术技术方案进行进一步详细说明。 -种惯性里程计组合高精度姿态测量方法,包括以下步骤: (1)确定卡尔曼滤波状态方程状态变量 卡尔曼滤波状态方程取19个状态变量,分别为:北向速度误差δ Vn、天向速度误 差δ Vu、东向速度误差δ VE、讳度误差:辱、商度误差δ h、经度误差δ λ、北向失准角φΝ、天 向失准角、东向失准角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 ▽ Xi、Y加表零偏\、Z加表零偏兄2、航位推算纬度误差和>〇、航位推算高度误差δ hD、航位 推算经度误差S λ D、里程计刻度系数误差SKD; (2)确定系统各误差方程 g)速度误差方程 式中: VN、Vu、VE-北向、天向、东向速度,单位:m/s ; RM-子午圈半径,单位:m ; RN-卯酉圈半径,单位:m;...

【技术保护点】
一种惯性里程计组合高精度姿态测量方法,其特征在于,包括以下步骤:(1)确定卡尔曼滤波状态方程状态变量卡尔曼滤波状态方程取19个状态变量,分别为:北向速度误差δVN、天向速度误差δVU、东向速度误差δVE、纬度误差高度误差δh、经度误差δλ、北向失准角φN、天向失准角φU、东向失准角φE、X陀螺漂移εx、Y陀螺漂移εy、Z陀螺漂移εz、X加表零偏Y加表零偏Z加表零偏航位推算纬度误差航位推算高度误差δhD、航位推算经度误差δλD、里程计刻度系数误差δKD;(2)确定系统各误差方程a)速度误差方程式中:VN、VU、VE——北向、天向、东向速度,单位:m/s;RM——子午圈半径,单位:m;RN——卯酉圈半径,单位:m;h——惯性高度,单位:m;ωie——地球自转角速率,常数,取7.292115×10‑5rad/s;——纬度,单位:rad;fN、fU、fE——滤波计算用n系下三轴比力分量,单位:m/s2;C11、C21、C31、C12、C22、C32、C13、C23、C33——姿态矩阵中的元素;b)位置误差方程c)姿态角误差方程d)陀螺和加速度计误差方程ϵ·i=0▿·i=0(i=x,y,z)···(4)]]>e)航位推算位置误差方程f)里程计刻度系数误差方程δK·D=0···(6)]]>综合式(1)~(6),可得系统状态方程如下:X·(t)=FX(t)+W(t)···(7)]]>式中:X(t)——系统状态变量,即上述的卡尔曼滤波状态方程19个状态变量;F——根据公式(1)~(6)确定的系统矩阵;W(t)——系统噪声;(3)确定系统观测方程系统观测量取导航解算位置与航位推算位置之差,则系统观测方程如下:式中:Z(t)——观测变量;H——观测矩阵;V(t)——观测噪声;λ、h——惯性纬度、经度、高度,单位:rad,rad,m;λD、hD——航位推算纬度、经度、高度,单位:rad,rad,m;将公式(7)和(8)离散化,可得:Xk=Φk,k-1Xk-1+Wk-1Zk=HkXk+Vk···(9)]]>式中:Xk——状态变量;Φk,k‑1——上一滤波周期至当前滤波周期的一步转移矩阵;Zk——观测量;Hk——观测矩阵;Wk——系统激励噪声序列;Vk——观测噪声序列;一步转移矩阵Φk,k‑1可用下式计算:Φk,k-1=I+Σi=1nFTn···(10)]]>式中:I——单位矩阵;Tn——导航周期;n——相邻两次滤波时刻之间的导航周期个数;(4)确定卡尔曼滤波递推方程组卡尔曼滤波递推方程组如下:a)状态一步预测X^k/k-1=Φk,k-1X^k-1···(11)]]>b)状态估计X^k/k-1=X^k/k-1+Kk(Zk-HkX^k/k-1)···(12)]]>c)滤波增益Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1···(13)]]>d)一步预测均方误差Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1···(14)]]>e)估计均方误差Pk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT···(15)]]>式中:——当前滤波周期的实时状态估计值;——上一滤波周期对当前滤波周期的状态预测值;Kk——当前滤波周期的滤波增益阵;Pk/k‑1——上一滤波周期对当前滤波周期的预测误差估计的协方差阵;Pk——当前滤波周期的实时误差估计协方差阵;Qk‑1——系统噪声方差阵;Rk——观测噪声方差阵;(5)卡尔曼滤波递推计算和反馈修正经过卡尔曼滤波递推计算后,估计出北向失准角φN、东向失准角φE、X陀螺漂移εx、Z陀螺漂移εz;然后对其进行反馈修正以提高姿态精度。...

【技术特征摘要】

【专利技术属性】
技术研发人员:艾瀛涛郭玉胜庄广琛马小艳宋雅兰莫明岗
申请(专利权)人:北京自动化控制设备研究所
类型:发明
国别省市:北京;11

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1