用matlab估计时延差,一种GNSS/INS松组合时延误差的估计及补偿方法与流程-CSDN博客

本发明属于组合导航系统性能提升领域，具体涉及一种GNSS/INS松组合时延误差的估计及补偿方法。

背景技术：

在搭建实用的低成本INS/GNSS组合导航系统时，数据处理以及数据传输引起的时间同步误差，对于没有针对时间误差建模补偿的情况下不仅会对前向加速度计估计值引入偏差，还会导致整体的组合导航定位误差无法收敛。针对上述问题，通过对误差模型加入时延估计并补偿，能有效消除时延误差的影响。

发表在论文集《第五届中国卫星导航学术年会电子文集》中的《GNSS/INS组合导航时延影响分析》一文，对时延误差分别在松组合与紧组合中对测量值的影响的大小做了理论的对比。但是未对时延误差如何进行建模分析以及补偿做工作，无法消除时延误差的影响。

技术实现要素：

本发明的目的在于提供一种GNSS/INS松组合时延误差的估计及补偿方法，能够实时估计数据融合过程的时延量，并补偿时延误差，提升组合导航定位精度。

本发明的目的是这样实现的：

一种GNSS/INS松组合时延误差的估计及补偿方法，包括以下步骤：

步骤1：设定初始的惯性元件初始估计误差以及初始的时延估计值Td；

步骤2：实时采集捷联惯导系统的陀螺和加速度计输出数据，经惯性元件初始误差修正后进行导航解算，更新载体运动状态，得到实时的载体加速度an、速度信息Vn以及姿态、位置信息；

步骤3：定量分析时延量对测量值影响；

步骤4：把常规的15阶的惯导状态误差模型扩增为16阶，状态量加入Td，对应改写状态矩阵ψ’k，测量矩阵H’k；

步骤5：利用载体实时信息与带有时延误差的GNSS测量信息Z’采用扩增的状态误差模型进行滤波，估计出新的惯性元件误差、状态误差、时延估计值T'd，反馈补偿惯导原始数据和载体状态信息和迭代更新时延估计值Td；

步骤6：利用迭代更新后的时延估计值Td计算GNSS测量信息的补偿量δZ，用以下次更新修测量信息。

步骤3所述时延量对GNSS测量信息的影响为：

Z(kTs-Td)＝Z(kTs)-Vn(kTs)*Td+an(kTs)*Td2/2

其中Ts是惯导的采样周期，kTs表示是惯导的第k个采样周期，Z(kTs-Td)是带有时延的kTs时刻的测量值，Vn(kTs)为kTs时刻的载体速度，an(kTs)为kTs时刻的载体加速度。

步骤4所述扩增后的状态误差矩阵ψ’k和测量矩阵H’k的表达式是：

其中RMh，RNh为地球子午面NMS及平面EMF内确定的点的曲率半径；csRNh为RNh与当地纬度余弦的乘积；ψk为常规的15阶惯导状态误差模型；an是惯导解算得到的实时载体加速度；Vnx、Vny、Vnz是惯导解算得到的实时载体速度。

步骤5所述的时延估计值的迭代更新表达式为：

Td＝Td+T'd。

步骤6中测量信息的时延误差补偿量表达式为：

Z＝Z+δZ。

与现有技术相比，本发明的有益效果在于：

(1)本发明不需接入外部电路来估计时延量，通过模型改进可以实时估计时延量。

(2)本发明的时延误差补偿方法可以有效消除时延误差的影响。

(3)本发明在不引入外部设备情况下，能够实现对时延量的实时估计以及时延误差的补偿，有效的提高了时延情况下的组合导航定位精度。

附图说明

图1为本发明提出的GNSS/INS松组合时延误差的估计及补偿方法的基本流程框图；

图2为本发明采用的跑车实验运动轨迹；

图3为本发明方法滤波位置误差与传统方法的对比曲线；

图4为本发明方法滤波速度误差与传统方法的对比曲线；

图5为本发明方法滤波姿态角误差与传统方法的对比曲线；

图6为本发明方法实时估计的时延量。

具体实施方式

下面结合附图对本发明作进一步详细的说明。

本发明提出一种GNSS/INS松组合时延误差的估计及补偿方法，其流程图如附图1所示，该方法的主要步骤如下：

步骤一：设定初始的惯性元件初始估计误差以及初始的时延估计值Td；

步骤二：实时采集捷联惯导系统的陀螺和加速度计输出数据，经惯性元件初始误差修正后进行导航解算，更新载体运动状态，得到实时的载体加速度an、速度信息Vn以及姿态、位置信息；

步骤三：定量分析时延量对测量值影响；

所涉及公式为：Z(kTs-Td)＝Z(kTs)-Vn(kTs)*Td+an(kTs)*Td2/2

步骤四：把常规的15阶的惯导状态误差模型扩增为16阶，状态量加入Td，对应改写状态矩阵ψ’k，测量矩阵H’k；

所涉及的矩阵表达式为：

步骤五：利用载体实时信息与带有时延误差的GNSS测量信息Z’采用扩增的状态误差模型进行滤波，估计出新的惯性元件误差、状态误差、时延估计值T'd，反馈补偿惯导原始数据和载体状态信息和迭代更新时延估计值Td；

所涉及的时延量迭代更新表达式为：Td＝Td+T'd；

步骤六：利用迭代更新后的时延估计值Td计算GNSS测量信息的补偿量δZ，用以下次更新修测量信息；

所涉及的时延误差补偿量表达式为：

Z＝Z+δZ；

为验证本发明方法的合理性，可行性，利用matlab程序对所发明的一种GNSS/INS松组合时延误差的估计及补偿方法进行仿真验证。

仿真条件及仿真结果如下所示：

(1)载体运动及初始条件设置

设定车初始纬度为34.246°，初始经度为108.910°；初始速度0m/s，匀加速至40m/s后匀速运动做三次90°转向，运动轨迹如图2所示；初始姿态角为[0°，0°，45°]；运动时长为825s；载体初始失准角为[30″，-30″，45′]，初始速度误差为[0.5m/s，0.5m/s，0.5m/s]，初始位置误差为[1m，1m，3m]。

(2)GNSS误差设置

位置误差分别取1m、1m、3m，速度误差都为0.5m/s，测量信息时延量设为200ms。

(3)仿真结果

以上述仿真条件，对所发明的一种GNSS/INS松组合时延误差的估计及补偿方法进行仿真，与传统的不考虑时延影响的松组合滤波方法对比曲线分别如图3、图4、图5所示，本发明实时估计的时延量如图6所示。

从图3、图4、图5可以看出，本发明计算方法能够有效消除时延对于组合导航滤波的影响，相较传统方法，误差曲线能够迅速收敛达到稳定值；从图6可以看出，本发明方法可以快速估计出GNSS测量信息中的时延量用以补偿时延误差，估计曲线稳定后估计值与真实值相差不到10ms。

应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明教授的内容之后，本领域技术人员可以对本发明作各种改动和修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。