CN102095424A - 一种适合车载光纤航姿***的姿态测量方法 - Google Patents

Abstract

本发明涉及属于惯性导航技术领域，具体涉及一种适合车载光纤航姿***的姿态测量方法。本发明采用基于卡尔曼滤波器的低成本光纤航姿***与GPS组合导航***，在SINS初始对准前，对GPS故障信号进行检测与隔离，保证SINS/GPS组合***的可靠性，当GPS输出有效后，选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程。本发明能够大大加快计算速度，实现实时滤波计算，而且与速度+位置作为观测量的组合***相比，其导航精度尤其是姿态计算机的精度有很大的提高。

Description

一种适合车载光纤航姿***的姿态测量方法

技术领域

本发明涉及属于惯性导航技术领域，具体涉及一种适合车载光纤航姿***的姿态测量方法。尤其适用于高动态，对姿态精度要求较高的车载平台。

背景技术

随着车载导航***在民用领域里应用需求的只益增长，近年来车载组合导航技术得到迅速发展。成本低、精度可靠已经成为用户对民用车载导航***的基本要求。GPS定姿***具有成本低、功耗小、体积小、数据长期稳定性好等优点；但是GPS定姿***也有其自身的缺陷，例如数据更新率低、短期稳定性差，受GPS信号质量制约，在遮挡严重的区域和发生周跳时，姿态计算将会发生错误等。捷联惯导***(SINS)可以提供连续的载***置、速度、姿态信息，但是由于陀螺误差随时间不断累积，姿态确定的误差随之增大，在低成本的条件下为了满足姿态确定的精度要求，使用纯惯性导航方案显然是不适用的。

专利号为200710144846.2的《适合于光纤陀螺的载体姿态测量方法》中采用了如下方法：通过外部设备确定载体的初始位置参数与初始速度值；光纤陀螺捷联惯性导航***进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值；确定姿态更新周期；采集光纤陀螺、加速度计输出的载体相对于惯性坐标系的角速度计算旋转矢量的增量；通过旋转矢量与四元数的关系，得到姿态更新周期内姿态更新四元数；由姿态四元数更新方程更新姿态四元数；计算载体坐标系b系相对于导航坐标系n系的捷联矩阵T；求载体相对导航坐标系的姿态角等步骤。

由于原有光纤陀螺的载体姿态各种测量方法往往是以速度、位置作为观测量的，即使有些方法采用GPS速度和姿态信息作为观测量，对各参数的评估精度较高，相对于采用GPS位置和速度信息作为参考信息时，对姿态误差的评估精度有了较大提高，但是，对位置和速度误差的评估精度未有明显改善。引入GPS姿态信息后对惯导姿态误差的评估精度明显较未引入姿态信息时要好，但是对位置、速度的评估效果影响不大，没有明显的改善作用。

发明内容

本发明所要解决的技术问题是提供一种适合车载光纤航姿***的姿态测量方法，在车载应用平台上，实现高精度的姿态测量，为车载平台提供高精度的姿态信息，从而降低对光纤航姿***光纤陀螺和加速度计的精度要求。

为解决上述技术问题，本发明的技术方案为：

1、一种适合车载光纤航姿***的姿态测量方法，采用基于卡尔曼滤波器的低成本光纤航姿***与GPS组合导航***，其特征在于，

按照***解算流程，当GPS输出有效后，利用GPS输出的姿态、速度和SINS输出的姿态、速度之差作为观测量，具体包括如下步骤：

首先建立以SINS误差方程为基础的组合导航***状态方程，并在误差方程的基础上建立组合***的量程方程，用一阶线性随机微分方程来描述***的误差状态传播特性和***的量测误差状态，方程如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

选取SINS导航参数计算误差、光纤陀螺仪以及加速度计误差作为***的状态变量，共12维，即：

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

式中：φ_E，φ_N，φ_U为SINS姿态计算误差，δV_E和δV_N为SINS的速度误差；δL和δλ为SINS位置误差；ε_bx、ε_by和ε_bz为3个轴向陀螺漂移误差

为加速度计测量误差；

选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程，即：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

式中，H_V(t)是观测量为速度时的量测矩阵，H_ψ(t)是观测量为姿态时的量测矩阵，其中速度量测矢量为：

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

式中，v_SINS.N、v_SINS.E是SINS输出的北向和东向速度信息，v_SINS.N＝v_N+δv_N，v_SINS.E＝v_E+δv_E，其中v_N、v_E是载体沿地理坐标***北向和东向的真实速度；v_GPS.N、v_GPS.E是GPS接收机输出的北向和东向的速度信息，v_GPS.N＝v_N-M_N、v_GPS.E＝v_E-M_E，其中M_N、ME是GPS接收机测速误差，设为白噪声，H_v是观测量为速度时的量测矩阵；H_v＝[diag[1 1]0_2×10]，V_v是GPS接收机输出速度的量测噪声：V_v＝[M_N M_E]^T，取量测噪声为白噪声，

Z_ψ＝ψ_SINS.yaw-ψ_GPS.yaw＝H_ψX_ψ+V_ψ……………………………(6)

式中，ψ_SINS.yaw为SINS输出的航向角，ψ_SINS.yaw＝ψ_aw+δψ_SINS.yaw，其中ψ_aw为真实航向角，δψ_SINS.yaw为SINS航向角测量误差；ψ_GPS.yaw为双GPS输出的航向角，ψ_GPS.yaw＝ψ_yaw+δψ_GPS.yaw，其中δψ_GPS.yaw是双GPS航向角测量误差。视双GPS接收机测量精度和基线长度而定；

H_ψ是观测量为姿态时的量测矩阵，H_ψ＝[0_1×2 1 0_1×9]；V_ψ为双GPS输出航向角的量测噪声，V_ψ＝δψ_GPS.yay，取量测噪声为白噪声，设GPS航向测量白噪声方差为则R_ψ＝(HDOP.δ_ψ)²

离散后的状态方程和观测方程为：

X_K＝Φ_k，k-1X_k-1+Γ_k-1W_k-1……………………………….…(8)

Z_K＝H_kK_k+V_k

卡尔曼滤波算法如下，状态下一步预测：

状态估计：

滤波增益：

下一步预测均方差：

估计均方差： $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

优选的，在SINS初始对准前，对GPS故障信号进行检测与隔离，保证SINS/GPS组合***的可靠性，具体方法如下：

a、判断GPS数据检测是否位于SINS估计误差带内；

b、判断GPS接收机是否存在丢失卫星，即数据不更新现象；

c、判断GPS数据是否存在突变；

d、残差χ²检验法。

本发明的有益效果是，本发明能够大大加快计算速度，实现实时滤波计算，而且与速度+位置作为观测量的组合***相比，其导航精度尤其是姿态计算机的精度有很大的提高。

附图说明

下面结合附图和具体实施方式对本发明的技术方案作进一步具体说明。

图1为本发明的***解算原理框图。

图2为本发明SINS导航解算原理框图。

具体实施方式

按照图1***计算工作流程，***上电后，GPS进行初始化，包括设置GPS的输出数据格式、输出频率、双GPS天线的基线长度等，GPS上电正常工作后，双天线测量得到GPS双差相位观测值，求出高精度的载体姿态信息，其精度根据双天的基线长度，其测量精度能够达到0.05°(4m基线长度)。导航计算机接收GPS输出的姿态、位置、速度信息，并对GPS的输出进行故障诊断，判断其输出信息可用。GPS虽具有全球性和高精度等优点，但也存在不足之处，如数据更新率低、信号易被遮挡和多路径效应等。这些缺点造成GPS输出的信号具有以下故障特点：(1)GPS无数据输出；(2)GPS有数据输出，但是数据不更新；(3)GPS有数据输出，数据存在较大的突变。对于上述GPS的故障特点，拟采取以下措施进行检测与隔离。

a)判断GPS数据检测是否位于SINS估计误差带内；

b)判断GPS接收机是否存在丢失卫星(数据不更新)现象；

c)判断GPS数据是否存在突变；

d)残差χ²检验法；

利用上述方法对GPS故障信号进行检测与隔离，从而保证SINS/GPS组合***的可靠性。

按照图1***计算工作流程，在确定GPS信息可用之后，GPS的输出信息为SINS初始对准提供信息。按照图1所示，对准完成后进入SINS导航解算模块，SINS导航解算模块见附图2。

如附图2所示，进入SINS解算模块。SINS解算模块包括捷联矩阵的计算与四元数的即时修正、四元数

的最佳归一化、比力坐标转换与速度即时修正、位置矩阵的即时修正、位置速率和地球速率的解算、姿态角速率的计算、姿态角的计算、位置的计算、重力加速度的计算、高度通道解算等。进入SINS导航解算模块中，先采集光纤陀螺和加速度计输出信息，并解算光纤陀螺、加速度计输出的载体相对于惯性坐标系的角速度、视加速度计算旋转矢量的增量，按照附图2所示的解算流程，解算出载体相对导航系下的三维速度、位置、姿态信息。

如图1所示，按照***解算流程，当GPS输出有效后，利用GPS输出的姿态、速度和SINS输出的姿态、速度之差作为观测量。首先建立以SINS误差方程为基础的组合导航***状态方程，并在误差方程的基础上建立组合***的量程方程。用一阶线性随机微分方程来描述***的误差状态传播特性和***的量测误差状态，方程如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

选取SINS导航参数计算误差、光纤陀螺仪以及加速度计误差作为***的状态变量，共12维，即：

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

式中：φ_E，φ_N，φ_U为SINS姿态计算误差，δV_E和δV_N为SINS的速度误差；δL和δλ为SINS位置误差；ε_bx、ε_by和ε_bz为3个轴向陀螺漂移误差 为加速度计测量误差。

选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程，即：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

式中，H_V(t)是观测量为速度时的量测矩阵，H_ψ(t)是观测量为姿态时的量测矩阵。其中速度量测矢量为：

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

式中，v_SINS.N、v_SINS.E是SINS输出的北向和东向速度信息，v_SINS.N＝v_N+δv_N，v_SINS.E＝v_E+δv_E，其中v_N、v_E是载体沿地理坐标***北向和东向的真实速度；v_GPS.N、v_GPS.E是GPS接收机输出的北向和东向的速度信息，v_GPS.N＝v_N-M_N、v_GPS.E＝v_E-M_E，其中M_N、M_E是GPS接收机测速误差，设为白噪声。H_v是观测量为速度时的量测矩阵；H_v＝[diag[1 1]0_2×10]，V_v是GPS接收机输出速度的量测噪声：V_v＝[M_N M_E]^T，取量测噪声为白噪声。

Z_ψ＝ψ_SINS.yaw-ψ_GPS.yaw＝H_ψX_ψ+V_ψ……………………………(6)

式中，ψ_SINS.yaw为SINS输出的航向角，ψ_SINS.yaw＝ψ_aw+δψ_SINS.yaw，其中ψ_aw为真实航向角，δψ_SINS.yaw为SINS航向角测量误差；ψ_GPS.yaw为双GPS输出的航向角，ψ_GPS.yaw＝ψ_yaw+δψ_GPS.yaw，其中δψ_GPS.yaw是双GPS航向角测量误差。视双GPS接收机测量精度和基线长度而定。

H_ψ是观测量为姿态时的量测矩阵，H_ψ＝[0_1×2 1 0_1×9]；V_ψ为双GPS输出航向角的量测噪声，V_ψ＝δψ_GPS.yaw，取量测噪声为白噪声，设GPS航向测量白噪声方差为

则R_ψ＝(HDOP.δ_ψ)²

离散后的状态方程和观测方程为：

X_K＝Φ_k，k-1X_k-1+Γ_k-1W_k-1……………………………….…(8)

Z_K＝H_kK_k+V_k

卡尔曼滤波算法如下，状态下一步预测：

状态估计：

滤波增益：下一步预测均方差：

估计均方差： $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

姿态矩阵可以表示为偏航角ψ、俯仰角θ和滚动角γ的关系，由地理坐标系至载体坐标系可以通过下列顺序的三次转换来实现：①绕y轴转ψ度，②绕z轴转θ度，③绕x轴转γ度。即得：

因此，利用姿态矩阵

中的相关元素，就可解出载体姿态角的主值，即

ψ_主＝tg^-1(-T₃₁/T₁₁)

γ_主＝tg^-1(-T₂₃/T₂₂)

由于俯仰角、滚动角和偏航角的变化范围分别在[-90°90°]、[-180°180°]和[-180°180°]，利用ψ_主、θ_主和γ_主判断ψ、θ和γ的真值。

最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种适合车载光纤航姿***的姿态测量方法，采用基于卡尔曼滤波器的低成本光纤航姿***与GPS组合导航***，其特征在于，

按照***解算流程，当GPS输出有效后，利用GPS输出的姿态、速度和SINS输出的姿态、速度之差作为观测量，具体包括如下步骤：

首先建立以SINS误差方程为基础的组合导航***状态方程，并在误差方程的基础上建立组合***的量程方程，用一阶线性随机微分方程来描述***的误差状态传播特性和***的量测误差状态，方程如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

选取SINS导航参数计算误差、光纤陀螺仪以及加速度计误差作为***的状态变量，共12维，即：

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

式中：φ_E，φ_N，φ_U为SINS姿态计算误差，δV_E和δV_N为SINS的速度误差；δL和δλ为S INS位置误差；ε_bx、ε_by和ε_bz为3个轴向陀螺漂移误差

为加速度计测量误差；

选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程，即：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

式中，H_V(t)是观测量为速度时的量测矩阵，H_ψ(t)是观测量为姿态时的量测矩阵，其中速度量测矢量为：

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

式中，v_SINS.N、v_SINS.E是SINS输出的北向和东向速度信息，v_SINS.N＝v_N+δv_N，v_SINS.E＝v_E+δv_E，其中v_N、v_E是载体沿地理坐标***北向和东向的真实速度；v_GPS.N、v_GPS.E是GPS接收机输出的北向和东向的速度信息，v_GPS.N＝v_N-M_N、v_GPS.E＝v_E-M_E，其中M_N、M_E是GPS接收机测速误差，设为白噪声，H_v是观测量为速度时的量测矩阵；H_v＝[diag[1 1]0_2×10]，V_v是GPS接收机输出速度的量测噪声：V_v＝[M_N M_E]^T，取量测噪声为白噪声，

Z_ψ＝ψ_SINS.yaw-ψ_GPS.yaw＝H_ψX_ψ+V_ψ……………………………(6)

式中，ψ_SINS.yaw为SINS输出的航向角，ψ_SINS.yaw＝ψ_aw+δψ_SINS.yaw，其中ψ_aw为真实航向角，δψ_SINS.yaw为SINS航向角测量误差；ψ_GPS.yaw为双GPS输出的航向角，ψ_GPS.yaw＝ψ_yaw+δψ_GPS.yaw，其中δψ_GPS.yaw是双GPS航向角测量误差。视双GPS接收机测量精度和基线长度而定；

H_ψ是观测量为姿态时的量测矩阵，H_ψ＝[0_1×2 1 0_1×9]；V_ψ为双GPS输出航向角的量测噪声，V_ψ＝δψ_GPS.yaw，取量测噪声为白噪声，设GPS航向测量白噪声方差为

则R_ψ＝(HDOP.δ_ψ)²离散后的状态方程和观测方程为：

X_K＝Φ_k，k-1X_k-1+Γ_k-1W_k-1……………………………….…(8)

Z_K＝H_kK_k+V_k

卡尔曼滤波算法如下，状态下一步预测：

状态估计：

滤波增益：下一步预测均方差：

估计均方差： $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

2.根据权利要求1所述一种适合车载光纤航姿***的姿态测量方法，其特征在于，在SINS初始对准前，对GPS故障信号进行检测与隔离，保证SINS/GPS组合***的可靠性，具体方法如下：

a、判断GPS数据检测是否位于SINS估计误差带内；

b、判断GPS接收机是否存在丢失卫星，即数据不更新现象；

c、判断GPS数据是否存在突变；

d、残差χ²检验法。

Images