一种基于零速校正的组合导航系统的实现方法
【技术领域】
[0001] 本发明涉及一种基于STM32的组合导航系统，具体涉及一种基于零速校正的组合 导航系统的实现方法。
【背景技术】
[0002] 在载体导航领域中，定位系统是一个必备的装置。当前使用最广泛的是全球定位 系统（GPS)，但在一些复杂环境下，GPS的信号动态性能较差，信号输出频率较低，不能满足 使用者的要求。而捷联惯导系统（SINS)利用安装在运动载体上的测量装置来跟踪载体系 统的运动状态，然后输出运动载体的速度和当前位置，在短距离有较高的精度，自主性强， 但是在远距离和长期运动中输出信号会随着时间的积累而发散，长期的稳定性较差。
[0003] 在捷联惯性导航系统中，需要通过陀螺仪测量出地球的自转角速度来实现初始对 准，然而受限于低品质陀螺仪的精度，无法测量出地球自转角速度。而电子罗盘（MC)可以 有效的测量出载体所处的姿态，为初始捷联矩阵的构造提供数据。
[0004] 对于GPS/SINS组合导航系统，当GPS信号失效时，只有捷联惯性导航进行工作，此 时由于单独的捷联惯性导航系统因为误差累计而出现定位发散的情况，尤其是低品质的陀 螺仪和加速度本身输出的信息具有较大的误差。即便在GPS信号没有失效的情况下，由低 品质陀螺仪和加速度计长时间工作带来的误差量也会影响定位的精度。这使得对捷联惯性 系统进行信息校正成为提高组合导航系统定位精度的关键。
【发明内容】
[0005] 为了克服现有技术存在的缺点与不足，本发明提供一种基于零速校正的组合导航 系统的实现方法。
[0006] 本发明采用如下技术方案：
[0007] -种基于零速校正的组合导航系统的实现方法，所述组合导航系统为捷联惯性导 航系统及GPS系统的组合，所述捷联惯性导航系统包括加速度计、陀螺仪及电子罗盘，所述 捷联惯性导航系统及GPS系统设置在载体平台上，还包括ARM处理器，所述ARM处理器分别 与GPS系统及捷联惯性导航系统连接，具体步骤如下：
[0008] S1初始化各个硬件设备；
[0009] S2捷联惯性导航系统初始对准，具体是通过电子罗盘、加速度计及陀螺仪确定初 始时刻载体平台的捷联矩阵；
[0010] S3ARM处理器读取加速度计及陀螺仪的当前数据，并对当前数据进行均值滤波处 理，消除野值，所述当前数据为载体的加速度和转动速率；
[0011] S4将滤波处理后的捷联惯性导航系统的数据，通过捷联解算及捷联矩阵将载体平 台坐标系转换为导航坐标系中，得到在导航坐标系中载体运动的位置和速度；
[0012] S5将GPS系统数据和S4处理后捷联惯性导航系统的数据同步，并同时进行GPS信 号有无的检测，如果GPS信号检测正常则转入S6,如果GPS信号缺失并且载体进入零速状态 则转入零速校正的过程；
[0013] S6将同步后的GPS系统数据和捷联惯性导航系统数据输入到集中卡尔曼滤波器， 输出最优的误差估计值，使用该估计值去校正捷联惯性导航系统数据，实现对载体的导航。
[0014] 所述零速校正的过程，包括，当出现GPS信号缺失的时候，对集中卡尔曼滤波器的 量测向量以及量测方程进行更改，且当载体进入零速状态的时候，将量测向量中的速度观 测值置零和位置观测值置零，然后对捷联惯性导航系统的速度和位置输出进行校正。
[0015] 所述卡尔曼滤波器的误差量状态方程的误差状态向量由位置误差、速度误差、姿 态误差角、加速度计零偏误差以及陀螺仪漂移误差构成；
[0016] 所述卡尔曼滤波器的量测方程中的量测向量为ZI=HA+V:，其中Z:为量测向量， 氏为量测矩阵，X:为前述误差状态，V:为观测噪声矩阵。
[0017] 所述零速状态检测具体步骤为：
[0018] S5-1计算比例幅值
[0019] MEMS惯导系统在每个离散时刻怀。的输出，计算当前任意时刻处的加速 度计输出比力幅值，即：
[0020]
[0021]其中fi(tm)(i=X，y,z)为tm时刻的加速度计输出比力
[0022] S5-2计算判断指标
[0023] 求取计算区间lj时段内的比力幅值均值
[0026] S5-3运动状态判定
[0027] 根据加速度计输出频率，设定区间长度叫，此处取为1秒；根据加速度计输出噪声 的方差特性，设定方差阈值Gatev，当Varm〈Gatev，则判定当前时刻处于静止状态，否则判定 为运动状态。
[0028] 所述误差状态向量及量测向量均是在导航坐标系下的参数。
[0029] 本发明的有益效果
[0030] 本发明经过理论上证明基于GPS/INS/MC的组合导航具有比单独导航系统更好的 导航和定位效果，零速校正算法的应用提高了捷联惯性导航系统的导航精度，特别是在GPS信号缺失的情况下。
【附图说明】
[0031] 图1是组合导航系统的结构框图；
[0032]图2是本发明捷联惯性导航系统进行捷联解算的流程图；
[0033]图3是本发明基于零速校正的组合导航系统工作流程图；
[0034] 图4为硬件平台上实际定位测试对比图，分别为添加零速校正的组合导航定位， 单独的GPS定位与实际位置的对比；
[0035] 图5(a)与图5(b)为实际定位经炜度误差对比图，其中图5(a)为添加零速校正的 组合导航定位与单独GPS定位的经度误差对比，图5 (b)为炜度误差对比。
【具体实施方式】
[0036] 下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不 限于此。
[0037] 实施例
[0038] 本实施例中捷联惯性导航系统使用6轴运动处理组件mpu6050,GPS系统使用 WaveshareU-BLOXNE0-6QGPS模块。电子罗盘采用HMC5983L三轴地磁传感器。组合导航 系统的数据采集和任务管理部分以ARM处理器的stm32F103作为平台，其具有自身资源丰 富，外设扩展性好，价格低廉的优势。
[0039] -种基于零速校正的组合导航系统的实现方法，所述组合导航系统为捷联惯性导 航系统及GPS系统的组合，所述捷联惯性导航系统包括加速度计、陀螺仪及电子罗盘，所述 捷联惯性导航系统及GPS系统设置在载体平台上，还包括ARM处理器，所述ARM处理器分别 与GPS系统及捷联惯性导航系统连接；
[0040] S1初始化各个硬件设备
[0041] 对硬件平台上电，初始化串口和子系统硬件平台。硬件平台基于STM32H03处理 器，首先配置系统时钟为72MHz，使用外部8M晶振。分别初始化串口 1和串口 2,串口 1用 来读取GPS的信息，串口 2用来将系统的导航信息通过串口转USB输出到相连接的电脑上， 最后分别进行I2C端口和各个传感器配置的初始化。
[0042] S2捷联惯性导航初始对准，具体是通过电子罗盘、加速度计及陀螺仪确定初始时 刻载体平台的捷联矩阵。
[0043] 通过加速度计得到由重力引起的沿载体平台坐标系轴的分量和重力加速度之间 的夹角，我们可以得到载体的初始俯仰角Q和横滚角Y，其中：
[0046] 为加速度计测量到的在载体平台坐标系上沿y轴方向的加速度，为载体平台 坐标系上沿x轴方向的加速度。
[0047] 而航向角也可由电子罗盘得到（与正北方向夹角）。由此，我们可以确定初始捷 联惯性矩阵
[0048]
[0049] 上面所述矩阵Cf即为载体平台坐标系到GPS导航坐标系（选取东北天坐标系）之 间的转换矩阵。
[0050] S3ARM处理器读取加速度计及陀螺仪的当前数据，并对当前数据进行均值滤波处 理，消除野值，所述当前数据为加速度和转动速率；
[0051] 由于所选用的加速度计产生的随机误差会大大影响后续的数据精度，在一开始对 原始数据进行简单处理很有必要。通过一开始的静止平放读取出此次器件上电加速度计和 陀螺仪的偏差，在后续的数据读取中采用均值滤波，连续读取五次的加速度值取平均值，并 减去前面得到的偏差。这样可以有效的去除掉杂值和上电时的产生的常值误差。
[0052] S4将滤波处理后的捷联惯性导航系统的数据，通过捷联解算及捷联矩阵将载体平 台坐标系转换为导航坐标系中，得到在导航坐标系中载体运动的位置和速度；
[0053] 由前面对捷联惯性矩阵的计算，可以实现由载体平台坐标系到导航坐标系的坐标 转换，则沿着载体平台坐标系测量的比力fb就可以转换到导航坐标系上，得到在导航坐标 系上的加速度fn，即
[0054]
[0055] 由于载体在不断的运动之中，其姿态在发生不断的变化，因此需要根据当时的姿 态实时计算出当时对应的捷联姿态矩阵，即捷联矩阵的更新。对于捷联矩阵的更新有几种 方法，本发明中采用四元数法，其具有精度高，无锁死角的优点。在上文中的捷联矩阵同样 可以用四元数q的方式表示
[0056]
[0057] 其中四元数的导数可由下面公式得出
[0062] 其中《x，《y，是陀螺仪测得的沿着载体坐标系各个轴的旋转角速度。这样由 上一时刻的四元数值以及此刻陀螺仪测得的角速度就可以得到此刻更新后的四元数，再由 捷联矩阵与四元数之间的关系可以得到更新后的捷联矩阵。
[0063] 由于地球自转的存在，上文中得到的在导航坐标系中的加速度并不能简单积分得 到速度，在这里需要对地球自转对速度的影响进行修正，有公式
[0069] L为当前位置的炜度，《1(；为地球自转角速度。
[0070] 而最终输出的位置信息由经炜度和高度表示，可由下面公式计算
[0074] 其中R为地球半径，至此，由捷联惯性导航系统