一种基于运动约束的车载航姿系统误差补偿方法及系统与流程

本发明涉及车载导航定位技术领域，特别是涉及一种基于运动约束的车载航姿系统误差补偿方法及系统。

背景技术：

对于具有特殊用途的车辆，如发射车、装甲车、指挥车、测绘车、勘探车等，通常需要具备高精度的航向与姿态确定能力，这就必须借助车载航姿系统来实现，而且车载航姿系统在各种复杂恶劣环境下尤其是在战场环境下，不仅需具备高精度的特点，而且还应具备良好的机动性、自主性以及便捷性等特点。

目前，国内外用于车载平台的中高精度航姿系统通常基于捷联惯组开发研制而来。众所周知，捷联惯组是一种高度自主的导航设备，不仅能够实现车辆航姿的确定，而且还能输出载车的速度和位置等信息，非常适用于各种复杂恶劣环境包括战场环境。但是，单纯靠捷联惯组用于航姿确定存在一个突出的问题：系统误差会随着工作时间的延长而不断积累，无法完成长时间高精度的定姿。目前，为了解决这一问题，通常采用卫星导航系统来辅助捷联惯组实现高精度定姿，取得了较为不错的使用效果。但是，由于卫星导航系统存在一定的缺陷，其信号非常容易被遮挡或被干扰，在特殊场所或战场环境下甚至可能被恶意屏蔽，因此卫星导航系统辅助捷联惯组实现航姿确定的方法其独立自主性和可靠性较差，一定程度上限制了这种方法在各种特殊应用领域特别是在军事领域内的广泛应用。

技术实现要素：

本发明的目的是提供一种基于运动约束的车载航姿系统误差补偿方法及系统，在各种复杂恶劣环境下尤其是在战场环境下能够实现长时间高精度定姿。

为实现上述目的，本发明提供了如下方案：

一种基于运动约束的车载航姿系统误差补偿方法，包括：

基于航姿系统的误差模型，建立误差补偿滤波的系统状态方程；

基于车辆运动约束条件，建立误差补偿滤波的量测方程；

根据所述系统状态方程和所述量测方程，确定航姿系统数学平台姿态误差的最优估计结果；

根据所述最优估计结果对车载航姿系统进行误差在线补偿。

可选的，所述建立误差补偿滤波的系统状态方程，具体包括：

根据航姿系统的误差模型，结合车辆行驶运动特点，选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差作为误差补偿滤波的系统状态，建立误差补偿滤波的系统状态方程。

可选的，所述基于车辆运动约束条件，建立误差补偿滤波的量测方程，具体包括：

利用车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为误差补偿滤波的量测，建立获得误差补偿滤波的量测方程。

可选的，所述根据所述系统状态方程和所述量测方程，确定航姿系统数学平台姿态误差的最优估计结果，具体包括：

根据所述系统状态方程和所述量测方程采用卡尔曼滤波方法进行滤波计算，确定航姿系统数学平台姿态误差的最优估计结果。

一种基于运动约束的车载航姿系统误差补偿系统，包括：

系统状态方程建立模块，用于基于航姿系统的误差模型，建立误差补偿滤波的系统状态方程；

量测方程建立模块，用于基于车辆运动约束条件，建立误差补偿滤波的量测方程；

最优估计结果确定模块，用于根据所述系统状态方程和所述量测方程，确定航姿系统数学平台姿态误差的最优估计结果；

误差在线补偿模块，用于根据所述最优估计结果对车载航姿系统进行误差在线补偿。

可选的，所述系统状态方程建立模块，具体包括：

系统状态方程建立单元，用于根据航姿系统的误差模型，结合车辆行驶运动特点，选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差作为误差补偿滤波的系统状态，建立误差补偿滤波的系统状态方程。

可选的，所述量测方程建立模块，具体包括：

量测方程建立单元，用于利用车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为误差补偿滤波的量测，建立获得误差补偿滤波的量测方程。

可选的，所述最优估计结果确定模块，具体包括：

最优估计结果确定单元，用于根据所述系统状态方程和所述量测方程采用卡尔曼滤波方法进行滤波计算，确定航姿系统数学平台姿态误差的最优估计结果。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明首先根据航姿系统的误差模型，选取航姿系统误差补偿滤波的系统状态，建立相应的系统状态方程；然后，利用车辆运动约束条件，推导获得对应的量测方程；采用卡尔曼滤波设计航姿系统误差补偿的滤波算法，充分利用车辆在行驶过程中所固有的运动约束条件，实现在车辆行驶过程中对车载航姿系统进行误差在线估计与补偿修正，有效克服了传统航姿系统的误差随时间不断发散以及需要不断停车进行误差修正的缺点，同时也摆脱了对外部信息或设备的依赖，极大地提高了车载航姿系统的快速机动性能和使用便捷程度。本发明工程易于实现，成本低廉，实用性强，非常适用于军事、测绘、勘探等特殊应用领域。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为基于运动约束的车载航姿系统误差补偿技术的原理框图；

图2为本发明基于运动约束的车载航姿系统误差补偿方法流程图；

图3为本发明基于运动约束的车载航姿系统误差补偿系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于运动约束的车载航姿系统误差补偿方法及系统，能够在各种复杂恶劣环境下尤其是在战场环境下能够实现长时间高精度定姿。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明涉及车载导航定位技术领域，对于具有特殊用途的车辆，如发射车、装甲车、指挥车、测绘车、勘探车等，通常需要具备高精度的航向与姿态确定能力，这就必须借助车载航姿系统来实现，而且车载航姿系统在各种复杂恶劣环境下尤其是在战场环境下，不仅需具备高精度的特点，而且还应具备良好的机动性、自主性、可靠性以及便捷性等特点。传统的基于纯惯组的航姿系统存在误差随着时间不断积累的突出问题，无法完成长时间高精度的定姿；目前采用较多的卫星导航辅助惯组定姿方法存在信号易被遮挡或干扰的问题，导致独立自主性和可靠性较差。

对于车载航姿系统所搭载的平台—车辆而言，如果正常行驶在相对平坦的道路上，一般不会发生侧向滑行或跳跃而起，此时若将车辆的行驶速度沿着载车的侧向和垂向进行投影，那么得到的侧向和垂向投影分量应该为零，这就是所谓的车辆运动约束条件。利用上述车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为观测量，借助卡尔曼滤波最优估计手段，可以辅助车载航姿系统在载车行进过程中实现其系统误差的在线估计与补偿，具有精度高、机动性强、自主性好、使用便捷等突出优点，能够有效解决传统航姿系统误差随时间缓慢发散的缺点，而且能够避免卫星导航辅助航姿确定方法中可靠性和抗干扰性较差的问题。为此，本发明提出了一种基于运动约束的车载航姿系统误差补偿方法及系统，重点研究航姿误差的在线估计与补偿算法，从而不仅实现精确的航姿测量，而且具有较强的机动性、自主性、可靠性和抗干扰性，工程上也易于实现、成本低廉，非常适用于军事、测绘、勘探等特殊应用领域。图1为基于运动约束的车载航姿系统误差补偿技术的原理框图。图2为本发明基于运动约束的车载航姿系统误差补偿方法流程图。

图2为本发明基于运动约束的车载航姿系统误差补偿方法流程图。如图2所示，一种基于运动约束的车载航姿系统误差补偿方法包括：

步骤101：基于航姿系统的误差模型，建立误差补偿滤波的系统状态方程，具体包括：

根据航姿系统的误差模型，结合车辆行驶运动特点，选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差作为误差补偿滤波的系统状态，建立误差补偿滤波的系统状态方程。

考虑到目前车载中高精度航姿系统通常基于捷联惯组开发研制而来，因此可将捷联惯组导航解算的系统误差模型视为航姿系统的误差模型，结合车辆行驶运动相对较慢的特点，忽略位置误差带来的影响，选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差等作为航姿误差补偿滤波的系统状态，包括：航姿系统数学平台沿东、北、天向的姿态误差φe,φn,φu，沿东、北、天向的速度误差δve,δvn,δvu，x、y、z轴上陀螺的常值漂移εbx,εby,εbz，x、y、z轴上加速度计的常值偏置▽bx,▽by,▽bz，即航姿误差补偿滤波的系统状态x为：

x＝[φe,φn,φu,δve,δvn,δvu,εbx,εby,εbz,▽bx,▽by,▽bz]^t

显然，根据前述航姿系统的误差模型，忽略位置误差项，并结合系统状态x，不难获得航姿误差补偿滤波的系统状态方程，其形式可表示为：

其中，f为系统状态矩阵；g为系统噪声驱动阵；w为系统白噪声，其噪声方差阵为q。

步骤102：基于车辆运动约束条件，建立误差补偿滤波的量测方程，具体包括：

利用车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为误差补偿滤波的量测，建立获得误差补偿滤波的量测方程。

对于正常行驶在相对平坦道路上的车辆，如果不会发生侧向滑行或跳跃而起，此时若将车辆的行驶速度沿着载车的侧向和垂向进行投影，那么得到的侧向和垂向投影分量应该为零，即所谓的车辆运动约束条件。利用该车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为航姿误差补偿滤波的量测量，进而建立相应的量测方程。

将导航坐标系(n系)取为东北天地理坐标系，车体坐标系(b系)取为右前上坐标系。设航姿系统的速度输出为姿态输出对应的姿态矩阵为则航姿系统的速度输出在车体坐标系的投影为：

记则航姿误差补偿滤波的量测量z即为：

然而，由于系统各种误差的存在，导致实际航姿系统的速度输出沿车体坐标系横向和垂向上的投影分量并不为零，而是存在一定的误差，不妨假设实际航姿系统的速度输出沿车体坐标系横向和垂向上投影的误差分别为δvx,δvz，则

z＝[δvx,δvz]^t

不难推导获得，航姿系统速度输出的上述投影误差δvx,δvz与航姿系统的数学平台姿态误差、速度误差之间的关系为：

δvx＝(t31vn-t21vu)φe+(t11vu-t31ve)φn+(t21ve-t11vn)φu+t11δve+t21δvn+t31δvu

δvz＝(t33vn-t23vu)φe+(t13vu-t33ve)φn+(t23ve-t13vn)φu+t13δve+t23δvn+t33δvu

其中，ve,vn,vu分别为载车沿东、北、天向的速度，tij(i,j＝1,2,3)为载车姿态矩阵的第i行、第j列元素。

从而，根据上述关系式，并结合系统状态x，可将量测z写为如下形式：

z＝hx+v

其中，h为量测矩阵，v为量测白噪声，其噪声方差阵为r。该式即为航姿误差补偿滤波的量测方程。

步骤103：根据所述航姿系统状态方程和所述量测方程，确定航姿系统数学平台姿态误差的最优估计结果，具体包括：

根据所述系统状态方程和所述量测方程采用卡尔曼滤波方法进行滤波计算，确定航姿系统数学平台姿态误差的最优估计结果。

为了在航姿解算计算机上实现卡尔曼滤波计算，必须对前面所建立的系统状态方程和量测方程进行离散化处理。由于量测方程本身已经是离散的，因此只需对连续的系统状态方程进行离散化处理，并将其离散化成如下形式：

xk+1＝φk+1,kxk+wk

式中，φk+1,k为一步转移矩阵；wk满足e[wk]＝0且其中qk为等效离散系统噪声方差阵。可见，状态方程离散化的过程，就是计算φk+1,k和qk的过程，状态方程离散化的具体算法如下：

设卡尔曼滤波周期为t(t＝tk+1-tk)，并记f(tk)＝fk。当卡尔曼滤波周期t较短时，一步转移阵φk+1,k的实时计算公式为：

记g(tk)＝gk，则等效离散系统噪声方差阵qk的实时计算公式如下

mi+1＝fkmi+(fkmi)^t

此时，利用上述公式就可求出一步转移阵φk+1,k和等效离散系统噪声方差阵qk，从而实现对系统状态方程的离散化处理。这时，只须给定状态初值，就可利用下列离散型卡尔曼滤波基本方程进行滤波计算：

pk＝(i-kkhk)pk/k-1(i-kkhk)^t+kkpkkk^t

从而，根据上述离散型卡尔曼滤波基本方程，设定状态初值及其估计均方误差p0，并结合k时刻的量测zk，就可以递推计算获得k时刻的状态估计(k＝1,2,3,4,…)，即意味着经过滤波计算可获得航姿系统数学平台姿态误差φe,φn,φu的实时估计结果，利用该估计结果对车载航姿系统进行误差在线补偿。

设滤波计算获得航姿系统数学平台姿态误差的估计结果分别为则航姿系统的姿态修正矩阵为：

其中，n′系表示航姿系统在系统误差的影响下实际建立的导航坐标系。利用上述姿态修正矩阵可以对航姿系统实际得到的姿态矩阵进行误差补偿，即：

这里，即为经过误差补偿后的载车姿态矩阵。

从而，利用误差补偿后的载车姿态矩阵可以根据下式与表实时计算出载车的航向姿态角，不妨记航向角为ψ，俯仰角为θ，横滚角为γ，则：

这里，ψ主、θ主、γ主分别为航向角、俯仰角、横滚角的主值。

表1航向姿态角真值表

从而，利用上述方法与公式，就能够实时对车载航姿系统的姿态误差进行有效的在线补偿。最后，将经过误差补偿修正的航姿系统输出作为系统的输出，主要包括载车的航向角和姿态角信息，即实现长时间高精度定姿的目的。

步骤104：根据所述最优估计结果对车载航姿系统进行误差在线补偿。

图3为本发明基于运动约束的车载航姿系统误差补偿系统结构图。如图3所示，一种基于运动约束的车载航姿系统误差补偿系统包括：

系统状态方程建立模块201，用于基于航姿系统的误差模型，建立误差补偿滤波的系统状态方程。

量测方程建立模块202，用于基于车辆运动约束条件，建立误差补偿滤波的量测方程。

最优估计结果确定模块203，用于根据所述系统状态方程和所述量测方程，确定航姿系统数学平台姿态误差的最优估计结果。

误差在线补偿模块204，用于根据所述最优估计结果对车载航姿系统进行误差在线补偿。

所述系统状态方程建立模块201，具体包括：

系统状态方程建立单元，用于根据航姿系统的误差模型，结合车辆行驶运动特点，选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差作为误差补偿滤波的系统状态，建立误差补偿滤波的系统状态方程。

所述量测方程建立模块202，具体包括：

量测方程建立单元，用于利用车辆运动约束条件，将车辆行驶过程中航姿系统的速度输出在车体坐标系横向和垂向上的投影分量作为误差补偿滤波的量测，建立获得误差补偿滤波的量测方程。

所述最优估计结果确定模块203，具体包括：

最优估计结果确定单元，用于根据所述系统状态方程和所述量测方程采用卡尔曼滤波方法进行滤波计算，确定航姿系统数学平台姿态误差的最优估计结果。

本发明与现有技术相比，具有下列优势：

1、本发明提供的基于运动约束的车载航姿系统误差补偿方法，为车辆航姿确定技术领域增添了一种工程可行、效果显著的新方法，该方法具有高精度、强机动性、高自主性、成本低廉以及工程易实现等突出优点。

2、本发明不仅有效克服了传统航姿系统的误差随时间不断发散以及需要不断停车进行误差修正的缺点，同时也摆脱了对外部信息或设备的依赖，极大地提高了车载航姿系统的快速机动性能和使用便捷程度。

3、本发明通过结合车辆行驶运动特点，合理选取航姿系统的数学平台姿态误差、速度误差以及惯性器件误差等作为误差补偿滤波的系统状态，建立误差补偿滤波的系统状态方程，这不仅揭示了航姿误差的传播特性，而且为航姿误差补偿滤波的状态空间模型建立奠定了基础。

4、本发明通过充分利用车辆固有的运动约束条件，为航姿误差补偿滤波提供了必要的量测信息，并在此基础上建立了相对应的量测方程，从而使航姿误差补偿滤波最终得以可能实现。

5、本发明基于建立的系统状态方程和量测方程，借助卡尔曼滤波这一最优估计手段，经过滤波计算能够实时获得系统状态即航姿系统数学平台姿态误差等的最优估计结果，利用该估计结果才得以对航姿误差进行在线补偿。

6、本发明不需要改变车载航姿系统本身的原有结构与既定输出，只需额外配备一套误差补偿计算模块以及相应的数据传输线缆，通过将相应算法软件嵌入到计算模块中，就可利用该发明中所提出的方法进行长时间高精度的航姿确定，工程易于实现，成本低廉，实用性好。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。