CN105021192B - 一种基于零速校正的组合导航系统的实现方法 - Google Patents

Abstract

本发明公开了一种基于零速校正的组合导航系统的实现方法，包括GPS系统及捷联惯性导航系统，具体步骤：S1初始化硬件设备，S2捷联惯性导航系统初始对准，S3处理器读取捷联惯性导航的数据，并进行处理，S4载体坐标系数据转换为导航坐标系，得到导航坐标系中载体运动的位置和速度，S5将两个系统的数据进行同步，并同时进行GPS有无及零速状态的检测，满足条件进入零速校正，否则进入步骤S6，S6将同步后的数据输入到集中卡尔曼滤波器，输出最优的误差估计值，进行校正。本发明的组合导航系统具有比单独导航系统更好的导航和定位效果。

Description

一种基于零速校正的组合导航系统的实现方法

技术领域

本发明涉及一种基于STM32的组合导航系统，具体涉及一种基于零速校正的组合导航系统的实现方法。

背景技术

在载体导航领域中，定位系统是一个必备的装置。当前使用最广泛的是全球定位系统(GPS)，但在一些复杂环境下，GPS的信号动态性能较差，信号输出频率较低，不能满足使用者的要求。而捷联惯导系统(SINS)利用安装在运动载体上的测量装置来跟踪载体系统的运动状态，然后输出运动载体的速度和当前位置，在短距离有较高的精度，自主性强，但是在远距离和长期运动中输出信号会随着时间的积累而发散，长期的稳定性较差。

在捷联惯性导航系统中，需要通过陀螺仪测量出地球的自转角速度来实现初始对准，然而受限于低品质陀螺仪的精度，无法测量出地球自转角速度。而电子罗盘(MC)可以有效的测量出载体所处的姿态，为初始捷联矩阵的构造提供数据。

对于GPS/SINS组合导航系统，当GPS信号失效时，只有捷联惯性导航进行工作，此时由于单独的捷联惯性导航系统因为误差累计而出现定位发散的情况，尤其是低品质的陀螺仪和加速度本身输出的信息具有较大的误差。即便在GPS信号没有失效的情况下，由低品质陀螺仪和加速度计长时间工作带来的误差量也会影响定位的精度。这使得对捷联惯性系统进行信息校正成为提高组合导航系统定位精度的关键。

发明内容

为了克服现有技术存在的缺点与不足，本发明提供一种基于零速校正的组合导航系统的实现方法。

本发明采用如下技术方案：

一种基于零速校正的组合导航系统的实现方法，所述组合导航系统为捷联惯性导航系统及GPS系统的组合，所述捷联惯性导航系统包括加速度计、陀螺仪及电子罗盘，所述捷联惯性导航系统及GPS系统设置在载体平台上，还包括ARM处理器，所述ARM处理器分别与GPS系统及捷联惯性导航系统连接，具体步骤如下：

S1初始化各个硬件设备；

S2捷联惯性导航系统初始对准，具体是通过电子罗盘、加速度计及陀螺仪确定初始时刻载体平台的捷联矩阵；

S3 ARM处理器读取加速度计及陀螺仪的当前数据，并对当前数据进行均值滤波处理，消除野值，所述当前数据为载体的加速度和转动速率；

S4将滤波处理后的捷联惯性导航系统的数据，通过捷联解算及捷联矩阵将载体平台坐标系转换为导航坐标系中，得到在导航坐标系中载体运动的位置和速度；

S5将GPS系统数据和S4处理后捷联惯性导航系统的数据同步，并同时进行GPS信号有无的检测，如果GPS信号检测正常则转入S6，如果GPS信号缺失并且载体进入零速状态则转入零速校正的过程；

S6将同步后的GPS系统数据和捷联惯性导航系统数据输入到集中卡尔曼滤波器，输出最优的误差估计值，使用该估计值去校正捷联惯性导航系统数据，实现对载体的导航。

所述零速校正的过程，包括，当出现GPS信号缺失的时候，对集中卡尔曼滤波器的量测向量以及量测方程进行更改，且当载体进入零速状态的时候，将量测向量中的速度观测值置零和位置观测值置零，然后对捷联惯性导航系统的速度和位置输出进行校正。

所述卡尔曼滤波器的误差量状态方程的误差状态向量由位置误差、速度误差、姿态误差角、加速度计零偏误差以及陀螺仪漂移误差构成；

所述卡尔曼滤波器的量测方程中的量测向量为Z_I＝H_IX_I+V_I，其中Z_I为量测向量，H_I为量测矩阵，X_I为前述误差状态，V_I为观测噪声矩阵。

所述零速状态检测具体步骤为：

S5-1计算比例幅值

MEMS惯导系统在每个离散时刻t₁,t₂…t₁₀的输出，计算当前任意时刻t_m处的加速度计输出比力幅值，即：

其中f_i(t_m)(i＝x,y,z)为t_m时刻的加速度计输出比力

S5-2计算判断指标

求取计算区间时段内的比力幅值均值

S5-3运动状态判定

根据加速度计输出频率，设定区间长度m₁，此处取为1秒；根据加速度计输出噪声的方差特性，设定方差阈值Gate_v，当Var_m<Gate_v，则判定当前时刻处于静止状态，否则判定为运动状态。

所述误差状态向量及量测向量均是在导航坐标系下的参数。

本发明的有益效果

本发明经过理论上证明基于GPS/INS/MC的组合导航具有比单独导航系统更好的导航和定位效果，零速校正算法的应用提高了捷联惯性导航系统的导航精度，特别是在GPS信号缺失的情况下。

附图说明

图1是组合导航系统的结构框图；

图2是本发明捷联惯性导航系统进行捷联解算的流程图；

图3是本发明基于零速校正的组合导航系统工作流程图；

图4为硬件平台上实际定位测试对比图，分别为添加零速校正的组合导航定位，单独的GPS定位与实际位置的对比；

图5(a)与图5(b)为实际定位经纬度误差对比图，其中图5(a)为添加零速校正的组合导航定位与单独GPS定位的经度误差对比，图5(b)为纬度误差对比。

具体实施方式

下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不限于此。

实施例

本实施例中捷联惯性导航系统使用6轴运动处理组件mpu6050,GPS系统使用Waveshare U-BLOX NEO-6Q GPS模块。电子罗盘采用HMC5983L三轴地磁传感器。组合导航系统的数据采集和任务管理部分以ARM处理器的stm32F103作为平台，其具有自身资源丰富，外设扩展性好，价格低廉的优势。

一种基于零速校正的组合导航系统的实现方法，所述组合导航系统为捷联惯性导航系统及GPS系统的组合，所述捷联惯性导航系统包括加速度计、陀螺仪及电子罗盘，所述捷联惯性导航系统及GPS系统设置在载体平台上，还包括ARM处理器，所述ARM处理器分别与GPS系统及捷联惯性导航系统连接；

S1初始化各个硬件设备

对硬件平台上电，初始化串口和子系统硬件平台。硬件平台基于STM32f103处理器，首先配置系统时钟为72MHz，使用外部8M晶振。分别初始化串口1和串口2，串口1用来读取GPS的信息，串口2用来将系统的导航信息通过串口转USB输出到相连接的电脑上，最后分别进行I2C端口和各个传感器配置的初始化。

S2捷联惯性导航初始对准，具体是通过电子罗盘、加速度计及陀螺仪确定初始时刻载体平台的捷联矩阵。

通过加速度计得到由重力引起的沿载体平台坐标系轴的分量和重力加速度之间的夹角，我们可以得到载体的初始俯仰角θ和横滚角γ，其中：

为加速度计测量到的在载体平台坐标系上沿y轴方向的加速度，为载体平台坐标系上沿x轴方向的加速度。

而航向角ψ可由电子罗盘得到(与正北方向夹角)。由此，我们可以确定初始捷联惯性矩阵

上面所述矩阵即为载体平台坐标系到GPS导航坐标系(选取东北天坐标系)之间的转换矩阵。

S3 ARM处理器读取加速度计及陀螺仪的当前数据，并对当前数据进行均值滤波处理，消除野值，所述当前数据为加速度和转动速率；

由于所选用的加速度计产生的随机误差会大大影响后续的数据精度，在一开始对原始数据进行简单处理很有必要。通过一开始的静止平放读取出此次器件上电加速度计和陀螺仪的偏差，在后续的数据读取中采用均值滤波，连续读取五次的加速度值取平均值，并减去前面得到的偏差。这样可以有效的去除掉杂值和上电时的产生的常值误差。

S4将滤波处理后的捷联惯性导航系统的数据，通过捷联解算及捷联矩阵将载体平台坐标系转换为导航坐标系中，得到在导航坐标系中载体运动的位置和速度；

由前面对捷联惯性矩阵的计算，可以实现由载体平台坐标系到导航坐标系的坐标转换，则沿着载体平台坐标系测量的比力f^b就可以转换到导航坐标系上，得到在导航坐标系上的加速度fⁿ，即

由于载体在不断的运动之中，其姿态在发生不断的变化，因此需要根据当时的姿态实时计算出当时对应的捷联姿态矩阵，即捷联矩阵的更新。对于捷联矩阵的更新有几种方法，本发明中采用四元数法，其具有精度高，无锁死角的优点。在上文中的捷联矩阵同样可以用四元数q的方式表示

其中四元数的导数可由下面公式得出

其中ω_x,ω_y,ω_z是陀螺仪测得的沿着载体坐标系各个轴的旋转角速度。这样由上一时刻的四元数值以及此刻陀螺仪测得的角速度就可以得到此刻更新后的四元数，再由捷联矩阵与四元数之间的关系可以得到更新后的捷联矩阵。

由于地球自转的存在，上文中得到的在导航坐标系中的加速度并不能简单积分得到速度，在这里需要对地球自转对速度的影响进行修正，有公式

其中：

L为当前位置的纬度，ω_ie为地球自转角速度。

而最终输出的位置信息由经纬度和高度表示，可由下面公式计算

其中R为地球半径，至此，由捷联惯性导航系统可以得到载体运动的速度和位置，整个捷联解算过程如图2所示。

S5将GPS系统数据和S4处理后捷联惯性导航系统的数据同步，并同时进行GPS信号有无的检测，如果GPS信号检测正常则转入S6，如果GPS信号缺失并且进入零速状态则转入零速校正的过程；

由于GPS系统和捷联惯性导航系统的采样频率不同，在本发明中GPS系统的采样频率为5Hz，即在定位的情况下每0.2秒GPS系统输出一次位置和速度信息。上文中捷联惯性导航系统设置在IMU的定时器中断函数中运行，输出频率100Hz，即每0.01秒输出一次由捷联惯性导航定位到的位置和速度信息。由于两者时间并不同步，将捷联惯性导航系统的输出量设置为全局变量，当GPS系统输出信息时候直接使用当时的捷联惯性导航信息进行主滤波器的运算。同时由GPS系统的输出信息时间间隔，将主滤波器的滤波时间设置为0.2秒。

S6将同步后的GPS系统数据和捷联惯性导航系统数据输入到集中卡尔曼滤波器，输出最优的误差估计值，将该估计值去校正捷联惯性导航系统数据，实现对载体的导航。组合导航的结构框图如图1所示。

在取得来自捷联惯性导航系统和GPS系统两个系统的位置和速度信息之后，下面进行组合滤波器的构造用于将两个系统的输出信息进行融合。在此我们选择导航参数误差作为滤波器的状态，利用估计出的误差来校正捷联惯性导航系统的输出。

集中卡尔曼滤波的误差状态方程采用捷联惯性导航系统的误差状态，由位置误差、速度误差、姿态误差角、加速度计零偏误差以及陀螺仪漂移误差组成，误差状态向量设置为

姿态误差方程的矢量表达式为

式中，为载体系到地里坐标系的转换矩阵，可由姿态角实时计算得到；ε^b为陀螺仪在载体载体系中表示的陀螺仪漂移，为地球自转角速率在地理坐标系上的分量，为误差量，为当前地理坐标系的转动角速度相对于惯性系的分量，为误差量，它们的具体表达式如下：

位置误差方程由以下公式计算

速度误差方程由下式得

其中v^t＝[v_x v_y v_z]，ν^b＝[ν_bx ν_by ν_bz]，f^t为加速度计测得的比力在地理系中的表示形式。

这样，由前面各子项的误差方程，可构造SINS的误差状态方程表达式为

式中系统噪声为

至于量测方程，此处采用SINS解算的位置和速度与GPS测量到的位置和速度之差作为量测信息，对于量测方程有

Z_I＝H_IX_I+V_I

其中其中Z_I为量测向量，H_I为量测矩阵，X_I为前述误差状态，V_I为观测噪声矩阵：

所述零速校正的过程，包括，当出现GPS信号缺失并且出现零速状态的时刻，对集中卡尔曼滤波器的量测向量以及量测方程进行更改，量测向量中的速度观测值及位置观测值置零，对捷联惯性导航系统的速度和位置输出进行校正。

首先需要进行零速状态的检测，在捷联惯性导航解算所在的中断函数中加入零速状态检测，由于捷联惯性导航解算的周期是0.01秒，而零速检测区间的设置是1秒钟，因此每次惯性导航解算循环10次后读取一次加速度计的状态，在十次读取之后即均匀获得了1秒钟内的加速度状态后进行数据分析。具体步骤如下：

(1)计算比力幅值：依据MEMS惯导系统在每个离散时刻t₁,t₂…t₁₀的输出，计算当前任意时刻t_m处的加速度计输出比力幅值，即：

其中f_i(t_m)(i＝x,y,z)为t_m时刻的加速度计输出比力

(2)计算判断指标：求取计算区间时段内的比力幅值均值

(3)运动状态判定：根据加速度计输出频率，设定区间长度m₁，此处取为1秒；根据加速度计输出噪声的方差特性，设定方差阈值Gate_v。当Var_m<Gate_v，则判定当前时刻处于静止状态，否则判定为运动状态。

当静止状态判定生效的时候，将集中卡尔曼滤波器的量测向量和量测方程进行更改，即将此时的速度观测量置为零，这将有效抑制速度误差。当零速状态生效的时候，对经过修改量测方程的卡尔曼滤波进行完整的滤波更新。而当GPS失效且零速状态没有生效的时候，只进行状态方程的更新，保持对惯性导航系统误差状态的更新。加入零速检测的组合导航系统工作流程图如图3所示。

最后重复S2到S6，实现对载体系统的连续导航。

使用零速校正算法对系统进行速度和位置在特定条件下的修正，采用集中式卡尔曼滤波器对子系统输出数据实现数据融合。零速校正的基本原理是当载体在导航过程中实际处于零速的时候，此时通过对零速状态的观测对载体通过捷联惯性导航系统的输出进行卡尔曼滤波校正。通过对加速度计输出的原始数据在一定的采样区间内进行数据方差的分析，当在此区间内加速度计原始数据的方差值小于一定阈值时，即认为载体此时处于零速的状态。然后在将卡尔曼滤波器的量测向量中的速度观测值和位置观测值置零，抑制速度测量值的误差发散。

如图4及图5(a)及图5(b)所示，在组合导航用于主滤波器滤波的状态方程数学模型方案选择上，为了使组合导航系统在提供较高精度的导航信息，选用导航参数的误差量作为系统状态，经过滤波计算后，得到的是误差的最优估计值，然后和INS解算出的导航参数组合得到最后的导航参数。在子系统的组合方式上，采用松组合方案，即选择速度和位置作为两个子系统的观测信息，采用这种组合方案实现简单，更适合工程应用，且可以使两个子系统各自独立工作，即使其中一个子系统暂时不能正常工作时，也可由另外一个系统单独提供导航信息。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (2)

1.一种基于零速校正的组合导航系统的实现方法，其特征在于，所述组合导航系统为捷联惯性导航系统及GPS系统的组合，所述捷联惯性导航系统包括加速度计、陀螺仪及电子罗盘，所述捷联惯性导航系统及GPS系统设置在载体平台上，还包括ARM处理器，所述ARM处理器分别与GPS系统及捷联惯性导航系统连接，具体步骤如下：

S1初始化各个硬件设备；

S2捷联惯性导航系统初始对准，具体是通过电子罗盘、加速度计及陀螺仪确定初始时刻载体平台的捷联矩阵；

S3ARM处理器读取加速度计及陀螺仪的当前数据，并对当前数据进行均值滤波处理，消除野值，所述当前数据为载体的加速度和转动速率；

S4将滤波处理后的捷联惯性导航系统的数据，通过捷联解算及捷联矩阵将载体平台坐标系转换为导航坐标系中，得到在导航坐标系中载体运动的位置和速度；

S5将GPS系统数据和S4处理后捷联惯性导航系统的数据同步，并同时进行GPS信号有无的检测，如果GPS信号检测正常则转入S6，如果GPS信号缺失并且载体进入零速状态则转入零速校正的过程；

S6将同步后的GPS系统数据和捷联惯性导航系统数据输入到集中卡尔曼滤波器，输出最优的误差估计值，使用该估计值去校正捷联惯性导航系统数据，实现对载体的导航；

当出现GPS信号缺失的时候，对集中卡尔曼滤波器的量测向量以及量测方程进行更改，且当载体进入零速状态的时候，将量测向量中的速度观测值置零和位置观测值置零，然后对捷联惯性导航系统的速度和位置输出进行校正；

所述卡尔曼滤波器的误差量状态方程的误差状态向量由位置误差、速度误差、姿态误差角、加速度计零偏误差以及陀螺仪漂移误差构成；

所述卡尔曼滤波器的量测方程中的量测向量为Z_I＝H_IX_I+V_I，其中Z_I为量测向量，H_I为量测矩阵，X_I为前述误差状态，V_I为观测噪声矩阵；

所述误差状态向量及量测向量均是在导航坐标系下的参数。

2.根据权利要求1所述的方法，其特征在于，所述零速状态检测具体步骤为：

S5-1计算比例幅值

MEMS惯导系统在每个离散时刻t₁,t₂…t₁₀的输出，计算当前任意时刻t_m处的加速度计输出比例 幅值，即：

其中fi(t_m)(i＝x,y,z)为t_m时刻的加速度计输出比例；

S5-2计算判断指标

求取计算区间时段内的比例 幅值均值

S5-3运动状态判定

根据加速度计输出频率，设定区间长度m₁，此处取为1秒；根据加速度计输出噪声的方差特性，设定方差阈值Gate_v，当Var_m<Gate_v，则判定当前时刻处于静止状态，否则判定为运动状态。