技术领域
[0001] 本
发明涉及一种基于MEMS传感器的SINS/GPS速度匹配对准方法，属于传感器和导航技术领域。
背景技术
[0002] 近几年，随着MEMS科学技术的飞快发展，MEMS器件因为它价格低、体积小等优点逐渐被人们用到IMU领域，如在无人机、AUV、航天航空等领域有较广泛应用，因此探讨基于MEMS传感器的SINS/GPS速度匹配对准是有重大应用前景的。
[0003] 速度匹配对准是SINS的核心技术之一。SINS的对准时间和对准
精度将会干扰其后续的工作性能，速度匹配对准可以化分为粗对准和精对准阶段。粗对准就是使用MEMS传感器的输出数据，利用
解析法求出滚动
角、
俯仰角、航向角，进而粗略的计算出初始
姿态变换矩阵。但是MEMS存在各种误差，如
陀螺仪存在常值漂移，
加速度具有零偏，而磁
力计容易受到周围环境的干扰，在环境恶劣的条件下工作时这些器件会产生很大的误差，造成姿态角精度下降。为快速获得准确的姿态变换矩阵，必须滤除MEMS传感器的各种误差。
[0004] 针对MEMS传感器的各种误差问题，因为磁力计工作时特别容易受到周围
磁场的干扰，这里使用GPS模
块来代替磁力计，利用GPS速度测量值来计算航向角角度，对于其它的各种误差，建立失准角误差微分方程、速度误差方程和惯性器件误差方程，使用滤波
算法进行估计。可以使用自适应无迹卡尔曼滤波(AUKF)算法计算，该方法的状态估计值以及协方差是可达到泰勒级数展开的三阶精度以上，AUKF和EKF计算量相当，但是EKF在工作时要计算雅可比矩阵，故AUKF较为简单且精度高。
发明内容
[0005] 针对以上初始对准中存在的问题和不足之处，本发明
专利目的在于提供一种基于MEMS传感器的SINS/GPS速度匹配对准方法。
[0006] 本发明采用以下的技术方案来实现上述的目的，具体包含以下步骤：
[0007] 基于MEMS传感器的SINS/GPS速度匹配对准方法，分为两阶段：第一阶段，使用GPS辅助SINS粗对准，使用MEMS
加速度计和GPS的输出数据来计算载体的三个姿态角，可以粗略得到初始姿态变换矩阵；第二阶段，建立失准角、惯性器件和速度误差微分方程，采用自适应UKF算法进行状态估计，为得到
滤波器准确的速度测量值，在滤波器之前对SINS和GPS速度数据分别进行时间校正和杆臂补偿。
[0008] 优选的，具体按以下步骤进行：
[0009] 步骤1：采集SINS的加速度
信号、角速率、积分速度值、三个姿态角信息和GPS速度数据、经度、纬度值；
[0010] 步骤2：使用解析法进行速度匹配的粗对准，得到姿态变换矩阵
[0011]
[0012] 可以使用MEMS加速度计测得信号来确定初始滚动角φ和俯仰角θ：
[0013]
[0014]
[0015] 使用GPS测得速度信号来计算初始航向角：
[0016] ψ＝arctan(vE/vN),[-π,π]
[0017] 式中，vE，vN为GPS测得的东向和北向速度值；
[0018] 步骤3：对SINS的速度数据进行时间校正，若SINS的
输出信号是在tk-1和tk上进行的，而GPS信息是在tGPS进行的，可以使用线性插值方程来获得SINS的速度：
[0019]
[0020] 步骤4：GPS的速度测量值杆臂校正，SINS和GPS的
位置是固定的，且表面为刚体，没有动态绕曲
变形，杆臂速度 可以写为：
[0021]
[0022] 式子中， 是
角速度，rb是GPS的天线在载体系内的杆臂长度，用计算补偿法对杆臂效应进行补偿，得到杆臂速度后，直接对GPS输出的速度信号进行补偿之后在作为滤波器的测量量；
[0023] 步骤5：搭建SINS/GPS的对准误差模型，使用AUKF进行状态估计。
[0024] 优选的，步骤5中建立SINS/GPS的速度匹配对准误差模型其步骤如下：
[0025] 可以将MEMS惯性传感器的误差看为常值和具有零均值白噪声干扰，陀螺仪的
温度漂移模型和加速度计的偏差模型可以分别简化为 和 而白噪声分量是wg和wa，则对准的滤波模型的
状态方程为：
[0026]
[0027] 选状态变量为 系统高斯白噪声是把SINS解算得到的速度与GPS测量的速度信号之间做差得到
δVx,δVy当作是模型的观测量，并且定义该观测噪声为 则该SINS的观测方程为：
[0028]
[0029] 其中，G＝I5×5，H＝[I2×2 02×8]。
[0030] 本发明有益效果在于，粗对准时，求航向角没有使用易受环境干扰的磁力计，而用GPS输出速度数据计算航向角；在精对准的过程中，在使用AUKF滤波之前，为了得到准确的姿态角和速度误差，对SINS的积分速度输出值进行时间校正，对GPS速度信息进行杆臂补偿；同时使用AUKF滤波增加了自适应能力，所以本发明可以一定程度上缩短对准时间，提高对准精度。
附图说明
[0031] 图1是本发明一个
实施例的速度匹配对准算法
流程图；
[0032] 图2是本发明一个实施例的GPS和SINS的测量时间图；
[0033] 图3是本发明一个实施例的SINS和GPS天线安装位置示意图；
[0034] 图4是本发明一个实施例的采用AUKF算法得到的东向失准角误差的曲线图；
[0035] 图5是本发明一个实施例的采用AUKF算法得到的北向失准角误差的曲线图；
[0036] 图6是本发明一个实施例的采用AUKF算法得到的天向失准角误差的曲线图。
具体实施方式
[0037] 下面将参照
说明书附图对本发明的一种基于MEMS传感器的SINS/GPS速度匹配对准的方法进行以下详细的说明：
[0038] 1、GPS辅助捷联惯导系统粗对准，采集SINS的加速度、角速度、积分速度、姿态角和GPS速度数据、经纬度值。导航
坐标系选用东北天坐标系，而加速度计信号是在载体系中的测得。使用解析法进行速度匹配的粗对准，得到姿态变换矩阵
[0039]
[0040] 使用加速度计算得到初始滚动角φ和俯仰角θ：
[0041]
[0042]
[0043] 使用GPS测得速度信号来计算初始航向角：
[0044] ψ＝arctan(vE/vN),[-π,π]
[0045] 式中，vE，vN为GPS测得的东向和北向速度值，通过计算可以得到三个初始姿态角，再根据上式就可以得到 的初值，即完成了粗对准。
[0046] 2、速度匹配对准过程，在完成了粗对准后，接着就可以进行AUKF的速度匹配对准，具体过程如图1所示，方案的原理是利用SINS和GPS速度差值来估算失准角，从而达到精对准目的，使用AUKF计算失准角是对准过程的核心部分。所以为提高对准的精度，需要有准确的SINS和GPS的速度测量值。在使用AUKF滤波算法之前，因GPS天线和SINS的位置不同，需要对GPS测得数据进行杆臂效应补偿；同时为了同步SINS和GPS的测量时间，本文也对SINS进行时间校正。
[0047] 3、时间校正，如图2所示，GPS和SINS测量值一般是在不同的时间进行的。所以，SINS的速度可以用GPS测量值的之前和之后的数据进行插值来计算得到。假设SINS的测量值是在tk-1和tk上进行的，而GPS测量是在tGPS进行的。故在GPS测量时间内，可以使用线性插值方程来获得SINS的速度：
[0048]
[0049] 4、杆臂校正，如图3，定义载体坐标系(ObXbYbZb)，惯性坐标系(OiXiYiZi)，Ob是SINS的中心，GPS天线安装于P点，两者的中心不可能安装在同一点上，这中间的距离r通常称之为杆臂，当工作时SINS的速度和GPS测得速度并不相同，为了提升对准精度，需要对GPS的速度测量值信息进行补偿。
[0050] 定义Rs，Rb分别是GPS天线和SINS到惯性坐标系原点的位置矢量，r是GPS天线到载体系原点的位置矢量，若地球自转角速度非常小，假设SINS和GPS的位置是固定，且表面是刚体，没有动态绕曲变形，杆臂速度 为：
[0051]
[0052] 是角速度，rb是GPS天线在载体坐标系内的杆臂长度，本文采用计算补偿法对杆臂效应进行补偿，求出杆臂速度后，直接对GPS输出的速度进行补偿之后再作为自适应UKF滤波器的测量量。
[0053] 5、建立基于MEMS传感器的SINS/GPS速度匹配对准的非线性
状态空间模型：
[0054]
[0055] 式子中 是e相对于i系的旋转率向量在n系下的投影，因为精对准时间不是太长，所以可以把MEMS传感器的误差看为常值和具有零均值白噪声干扰。陀螺仪的温度漂移模型和加速度计的偏差模型一般可以分别简化 和 而白噪声分量是wg和wa。
[0056] 状态变量为 系统高斯白噪声是把SINS计算得到的速度信息与GPS测量速度值之间做差
得到δVx,δVy当作是系统观测量，并且定义该观测噪声为 则该SINS的速度匹配对准系统方程为：
[0057]
[0058] 其中，G＝I5×5，H＝[I2×2 02×8]。
[0059] 6、自适应无迹卡尔曼滤波(AUKF)，根据步骤5所描述的速度匹配对准误差模型中的状态方程是非线性的，则系统的AUKF滤波器设计如下：
[0060] 1)初始化：将过程激励噪声和观测噪声增广为状态变量，则有：
[0061]
[0063]
[0064]
[0065]
[0066] 式中，n表示状态向量的维数，wi表示权值，λ＝a2(n+l)-n为一个比例参数，常选取l＝0，a是控制采样点的分布，决定采样点与均值的离散程度。常取一个很小的正数，例如1e-3，避免状态方程非线性严重时，采样点的非局域性受到影响，适当调节a和l可以提高估计均值的精度。
[0067] 3)时间更新
[0068] χi,k|k-1＝f(χi,k-1)
[0069]
[0070]
[0071] Zi,kk-1＝H·χi,k|k-1
[0072]
[0073] 4)测量更新：
[0074]
[0075]
[0076]
[0077]
[0078]
[0079] 式子中，ak为自适应因子，初值为1，它的取值范围是0≤ak≤1。如果ak取值合理，则它能够平衡系统模型预测信息与量测信息之间的全比。ak按下式构造：
[0080]
[0081] 取残差 协方差阵P:
[0082]
[0083] 根据上面公式可知，当状态值存在误差干扰时，ak小于1，即预测值在计算最优估计值中占权值尽量小；当预测值变化明显异常时，ak将接近于0，即此时预测值在计算最优估计值中占权值为0，故ak可以使用Vk和 自适应调节
[0084] 7、对本发明中的方法进行实验和仿真：
[0085] 实验采用了STM32系列的
单片机，使用基于MEMS传感器的捷联惯导系统，同时在串口处GPS模块，电脑和SINS之间的通信使用RS232串口线，在室外远离干扰无遮挡的地方进行实验。采集SINS的加速度信号、角速率、积分速度值、三个姿态角信息和GPS速度数据、经度、纬度值,之后把所采集到的实验数据放到MATLAB中进行速度匹配对准的仿真。
[0086] 假设wie＝7.292116e-5，初始失准角为[0.2°,0.2°,1.5°]，陀螺仪的漂移为5°/h，加速度计偏差0.5mg，
水平速度误差0.2m/s。根据设置参数对本发明进行MATLAB仿真，得到结果如图4、图5和图6所示。
[0087] 表1：速度匹配对准统计结果
[0088] 东向失准角 北向失准角 天向失准角
均值/(°) -0.0536 -0.0035 0.0654
RMS/(°) 0.1077 0.0213 0.1341
[0089] 根据图4、图5、图6以及表1知在粗对准结束仍有较大的失准角误差时，使用本发明的算法可以提升对准精度和缩短对准时间，从图中可知在60s后，失准角可以收敛到0.1°(RMS)左右，有效的解决了基于MEMS传感器的SINS/GPS的速度匹配对准相关问题。
