CN111156994B - 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 - Google Patents

一种基于mems惯性组件的ins/dr&gnss松组合导航方法 Download PDF

Info

Publication number
CN111156994B
CN111156994B CN201911408315.9A CN201911408315A CN111156994B CN 111156994 B CN111156994 B CN 111156994B CN 201911408315 A CN201911408315 A CN 201911408315A CN 111156994 B CN111156994 B CN 111156994B
Authority
CN
China
Prior art keywords
ins
gnss
zero offset
error
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911408315.9A
Other languages
English (en)
Other versions
CN111156994A (zh
Inventor
薛文通
白阳
吕明
赵蓓
周良荣
王莎
王玲玲
赵彦彦
马盼
刘芳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xingsi Semiconductor Co ltd
Original Assignee
Shanghai Xingsi Semiconductor Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Xingsi Semiconductor Co ltd filed Critical Shanghai Xingsi Semiconductor Co ltd
Priority to CN201911408315.9A priority Critical patent/CN111156994B/zh
Publication of CN111156994A publication Critical patent/CN111156994A/zh
Application granted granted Critical
Publication of CN111156994B publication Critical patent/CN111156994B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，由以下步骤组成：以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；以所述状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；利用所述状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出校正后的位置、速度和姿态信息；本发明采用消费级的IMU达到了导航级器件的性能，陀螺热致零偏估计方法可以在系统运行的过程中自主学习、自适应补偿，不需要再做任何离线标定；本发明的安装误差估计算法可以自主估计，不再需要对装配严格要求。

Description

一种基于MEMS惯性组件的INS/DR＆GNSS松组合导航方法
【技术领域】
本发明属于导航技术领域，尤其涉及一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法。
【背景技术】
现有的INS/DR系统多针对导航级IMU(惯性组件)传感器为基础的设计，和消费级MEMS IMU相比，导航级IMU的精度高几个数量级，价格昂贵，无法满足大众消费的普及，而且导航级IMU的INS/DR系统可以通过传感器本身的精度就可以完成捷联惯性系统的精对准，而且对于陀螺仪和加速度计的各类误差例如：零偏、零偏稳定性、随机误差、尺度因子等采用离线标定的方法，这给生产的技术要求和标定设备的精度要求很高，使得生产成本很高，而且不便于批量生产，同时要求IMU传感器的安装必须控制到和载体坐标系完全重合，给后期设备的装配带来诸多不便。
现有的INS/DR&GNSS组合导航系统中将陀螺仪和加速度计等各类误差(包含零偏、零偏稳定性、随机误差、尺度因子)引入到kalman滤波器的设计中，使得滤波器的状态维数达到21维之多，导致系统的复杂度和计算量庞大。
随着MEMS技术的发展，消费级MEMS IMU的成本已经降低到十几元人民币的级别，其精度也得到很大的提升，但是和传统的导航级别相比仍然相差1到2个数量级，现有车载导航或监控系统中GNSS接收机在信号遮挡或消失的情况下精度恶化甚至无法定位。
【发明内容】
本发明的目的是提供一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，以解决现有车载导航或监控系统中GNSS接收机在信号遮挡或消失的情况下精度恶化甚至无法定位的问题。
本发明采用以下技术方案：一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，由以下步骤组成：
以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；
以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；
以状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；
利用状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出校正后的位置、速度和姿态信息。
进一步地，状态微分方程为：
其中，F(t)是系统矩阵；G(t)是系统的噪声驱动矩阵；是系统的状态向量，表示为： 是系统的激励噪声，表示为： 是陀螺3个轴的零偏未标定误差；φE,φN,φU是3个失准角，δL,δλ,δh是INS/DR系统的位置误差，是东向陀螺零偏误差，是北向陀螺零偏误差，是天向陀螺零偏误差，δK是里程计的尺度因子误差。
进一步地，所述F(t)和G(t)的计算步骤如下：
其中，L是大地纬度，h是椭球体高度，ωie是地球自转角速率，和分别是东向和北向速度，RM是当前位置的地球子午圈曲率半径，RN是地球卯酉圈曲率半径，Re是地球半径，是捷联矩阵，03×3是3阶零矩阵，I3×3是3阶单位矩阵。
进一步地，INS/DR系统采用实时补偿算法对陀螺零偏进行初始标定，标定模型的辨识参数计算方法如下：
从[-40℃,85℃]的温度区间，以0.5℃为间隔作为温度序列，对每一列温度的标识零偏的可用性、求零偏的数据长度和对应温度点的零偏估计值，最终得到温度零偏映射表；
当载体处于角运动静止时，收集在同一个温度点上的连续陀螺数据样本，并用式(3)计算其均值作为零偏估计值；
其中，是待标定的陀螺零偏向量，是第k历元的陀螺采样值，N是样本容量；
任意时刻，首先读取到传感器的温度，搜索该温度在温度零偏映射表中对应的零偏是否有效，如果有效，则直接读出零偏估计值补偿到原始采样值上，否则以该温度点为中点，分别向两边搜索±10℃范围的有效温度点，然后用加权最小二乘算法对式(4)模型进行拟合，估计出模型常量参数c0x,c0y,C0z,c1x,c1y,c1z后，即可代入下面的模型估计出所需温度点的零偏值bωx,bωy,bωz：
其中，tk是k时刻的时间，Tk是k历元采样点的温度值。
进一步地，kalman滤波器迭代过程为：
设置滤波器的初值：
其中，上标T表示矩阵的转置，diag表示对角矩阵，下标0表示初始历元，是状态向量的初始值，P0是卡尔曼滤波器协方差矩阵的初始值，是初始的失准角误差的方差，是初始化位置误差的方差，是初始化陀螺零偏标定残差的方差，是初始化里程计尺度因子误差的方差，q0是系统激励噪声的方差，R是系统观测噪声方差矩阵。
求解状态转移矩阵：
Φk,k-1＝I10×10+F(tk-1)·Δt,Δt＝tk-tk-1 (7)
其中，Φk,k-1是卡尔曼滤波器的状态转移矩阵，下标(k,k-1)表示从k-1时刻向k时刻的状态转移，Δt是时间更新周期。
求解离散化的系统激励噪声矩阵Qk：
M1＝G(tk)*q0*G(tk)T (8)
M2＝F(tk)*M1+(F(tk)*M1)T (9)
M3＝F(tk)*M2+(F(tk)*M2)T (10)
kalman滤波器一步预测过程，先计算状态向量的先验估计再计算先验估计的协方差矩阵Pk,k-1，计算方法如下：
kalman滤波器量测更新过程，计算过程如下：
z＝[LGNSS-LD λGNSS-λD hGNSS-hD]T (15)
Pk＝(I10×10-Kk·Hk)·Pk,k-1 (17)
以上诸式中，Kk是卡尔曼滤波器增益矩阵，Hk是k历元的观测矩阵，z是测量向量，它是GNSS的位置与INS&DR系统的位置之差，LGNSS、λGNSS、hGNSS分别是GNSS输出的经度、纬度和高度，LD、λD、hD分别是INS&DR系统输出的经度、纬度和高度，是状态向量在k历元的后验估计，Pk是后验估计的协方差矩阵，
最后，用对INS/DR系统的姿态、里程计尺度误差和位置误差进行反馈校正。
进一步地，所述kalman滤波器反馈校正的方法为：
先用式(18)计算失准角φ的等效旋转矢量四元数qφ,q，然后用式(19)校正INS/DR系统的姿态四元数其中，下标'k'表示历元，上标‘+'表示经过校正后的量，
位置校正用(20)计算，里程计刻度因子的校正直接将状态估计量δk代入到式(21)补偿INS/DR系统的速度v，得到补偿后的速度
进一步地，在完成kalman滤波器解算过程后对INS/DR系统的安装误差和加速度计零偏进行在线估计，以消除捷联矩阵中包含的安装误差，估计方法为：
当陀螺零偏求解准确后，在开放天空，载体在水平路面上做8字形运动，且速度大于30Km/h，利用卫星导航的速度信息、加速度信息和INS系统的姿态角在数秒时间间隔的变化量解析出安装误差和加速计的零偏误差。
进一步地，所述观测方程由以下步骤组成：
以GNSS的位置和INS/DR系统的位置之差作为滤波器的观测量，则得到观测方程为：
其中，υp是GNSS位置误差。
进一步地，所述陀螺的数据采集频率为100Hz，加速度计的数据采集频率为10Hz，里程计信息有2种方式接入到INS/DR系统，一种方式是车速脉冲计数，采集频率为10Hz；另一种方式是汽车CAN总线，采集频率为1Hz。
本发明的有益效果是：采用消费级的IMU达到了导航级器件的性能，陀螺热致零偏估计方法可以在系统运行的过程中自主学习、自适应补偿，不需要再做任何离线标定；本发明的安装误差估计算法可以自主估计，不再需要对装配严格要求；简化的惯性系统和组合导航kalman滤波器及其合理的调度安排，不但可以保证有效的精度，而且对处理器选型要求极低；整个系统的设计考虑了从器件、处理器、生产、装配等各个环节的成本控制和易用性，具有极强工程实用价值。
【附图说明】
图1为本发明中速度更新的过程示意图；
图2为INS/DR&GNSS组合导航的结构示意图；
【具体实施方式】
下面结合附图和具体实施方式对本发明进行详细说明。
本发明可针对传感器的特性作适当补偿处理，并合理的规划和设计INS/DR系统，并结合应用场景和惯系统的特性简化组合导航kalman滤波器的系统模型和噪声模型，可以得到一个性能优良的组合导航系统，这对解决传统的GNSS车载导航监控系统在一些高楼林立、隧道、地下车库等信号遮挡严重甚至消失的环境下定位精度急剧下降的问题有重大的意义。
本发明公开了一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，如图2所示，由以下步骤组成：
对INS/DR系统的数据进行采集、静态检测、陀螺热致零偏模型估计与补偿、零速修正、初始对准、姿态更新、速度和位置更新。数据采集：陀螺的数据采集频率为100Hz，加速度计的数据采集频率为10Hz，里程计信息有2种方式接入到INS/DR系统，一种方式是车速脉冲计数，采集频率为10Hz；另一种方式是汽车CAN总线，采集频率为1Hz。
步骤1：以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；
状态微分方程为：
其中，F(t)是系统矩阵；G(t)是系统的噪声驱动矩阵；是系统的状态向量，表示为： 是系统的激励噪声，表示为： 是陀螺3个轴的零偏未标定误差；φE,φN,φU是3个失准角，δL,δλ,δh是INS/DR系统的位置误差，是东向陀螺零偏误差，是北向陀螺零偏误差，是天向陀螺零偏误差，δK是里程计的尺度因子误差。
根据车载的运动特性和消费级MEMS传感器的特性，惯性导航的误差方程可以被简化，简化后的系统矩阵和过程噪声激励矩阵可表示为式(2)，F(t)和G(t)的计算步骤如下:
其中，L是大地纬度，h是椭球体高度，ωie是地球自转角速率，和分别是东向和北向速度，RM是当前位置的地球子午圈曲率半径，RN是地球卯酉圈曲率半径，Re是地球半径，是捷联矩阵，03×3是3阶零矩阵，I3×3是3阶单位矩阵。
INS/DR系统采用实时补偿算法对陀螺零偏进行初始标定，标定模型的辨识参数计算方法如下：
从[-40℃,85℃]的温度区间，以0.5℃为间隔作为温度序列，对每一列温度的标识零偏的可用性、求零偏的数据长度和对应温度点的零偏估计值，最终得到温度零偏映射表；
当载体处于角运动静止时，收集在同一个温度点上的连续陀螺数据样本，并用式(3)计算其均值作为零偏估计值；
其中，是待标定的陀螺零偏向量，是第k历元的陀螺采样值，N是样本容量；
任意时刻，首先读取到传感器的温度，搜索该温度在温度零偏映射表中对应的零偏是否有效，如果有效，则直接读出零偏估计值补偿到原始采样值上，否则以该温度点为中点，分别向两边搜索±10℃范围的有效温度点，然后用加权最小二乘算法对式(4)模型进行拟合，估计出模型常量参数c0x,c0y,C0z,c1x,c1y,c1z后，即可代入下面的模型估计出所需温度点的零偏值bωx,bωy,bωz：
其中，tk是k时刻的时间，Tk是k历元采样点的温度值。
步骤2：以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；
观测方程由以下步骤组成：
以GNSS的位置和INS/DR系统的位置之差作为滤波器的观测量，则得到观测方程为：
其中，υp是GNSS位置误差。
步骤3：以状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；
kalman滤波器迭代过程为：
设置滤波器的初值：
其中，上标T表示矩阵的转置，diag表示对角矩阵，下标0表示初始历元，是状态向量的初始值，P0是卡尔曼滤波器协方差矩阵的初始值，是初始的失准角误差的方差，是初始化位置误差的方差，是初始化陀螺零偏标定残差的方差，是初始化里程计尺度因子误差的方差，q0是系统激励噪声的方差，R是系统观测噪声方差矩阵。
求解状态转移矩阵：
Φk,k-1＝I10×10+F(tk-1)·Δt,Δt＝tk-tk-1 (7)
其中，Φk,k-1是卡尔曼滤波器的状态转移矩阵，下标(k,k-1)表示从k-1时刻向k时刻的状态转移，Δt是时间更新周期。
求解离散化的系统激励噪声矩阵Qk：
M1＝G(tk)*q0*G(tk)T (8)
M2＝F(tk)*M1+(F(tk)*M1)T (9)
M3＝F(tk)*M2+(F(tk)*M2)T (10)
kalman滤波器一步预测过程，先计算状态向量的先验估计再计算先验估计的方差矩阵Pk,k-1，计算方法如下：
kalman滤波器量测更新过程，计算过程如下：
z＝[LGNSS-LDλGNSS-λDhGNSS-hD]T (15)
Pk＝(I10×10-Kk·Hk)·Pk,k-1 (17)
以上诸式中，Kk是卡尔曼滤波器增益矩阵，Hk是k历元的观测矩阵，z是测量向量，它是GNSS的位置与INS&DR系统的位置之差，LGNSS、λGNSS、hGNSS分别是GNSS输出的经度、纬度和高度，LD、λD、hD分别是INS&DR系统输出的经度、纬度和高度，是状态向量在k历元的后验估计，Pk是后验估计的协方差矩阵，I10×10表示10阶单位矩阵，
最后，用对INS/DR系统的姿态、里程计尺度误差和位置误差进行反馈校正。
在本发明设计的INS/DR系统中，陀螺的零偏已经经过专门的算法补偿到可以接受的精度了，所以反馈校正时，只对INS/DR系统的姿态、里程计尺度误差和位置误差进行校正。所以，完成校正后，除陀螺的3个轴的零偏随机漂移误差外其他的状态变量需要清零。
kalman滤波器反馈校正的方法为：
先用式(18)计算失准角φ的等效旋转矢量四元数qφ,q，然后用式(19)校正INS/DR系统的姿态四元数其中，下标'k'表示历元，上标‘+'表示经过校正后的量，
位置校正用(20)计算，里程计刻度因子的校正直接将状态估计量δK代入到式(21)补偿INS/DR系统的速度v，得到补偿后的速度
在完成kalman滤波器解算过程后对INS/DR系统的安装误差和加速度计零偏进行在线估计，以消除捷联矩阵中包含的安装误差，安装误差是指IMU传感器坐标系，即s系和机体坐标系，即b系不重合引入的误差，用表示，估计方法为：
当陀螺零偏求解准确后，在开放天空，载体在水平路面上做8字形运动，且速度大于30Km/h，利用卫星导航的速度信息、加速度信息和INS系统的姿态角在数秒时间间隔的变化量解析出安装误差和加速计的零偏误差。
步骤4：利用估计出来的状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出载体的校正后位置、速度和姿态信息。
零速修正：
当载体处于静止状态时，加速度计的输出仅仅是重力加速度值，所以可以利用加速度计的测量值对载体的水平姿态进行修正，用式(22)分别计算俯仰角θ和横滚角γ，航向角保持不变，然后用式(23)重新修正姿态四元数。
其中，是加速度计输出的比力，g是重力加速度，qk,q是姿态四元数。
初始对准：本发明提供的是消费级INS/DR系统，初始对准过程和高精度的捷联惯导系统不同，不需要做精对准，只需要做粗对准即可，另外要求载体在一定的机动的条件下，求解安装误差和加速度计的零偏。
粗对准过程：首先在静止状态下，利用式(22)求解惯性系统的2个水平姿态角(俯仰角和横滚角，然后待载体做直线运动时，利用卫星导航的速度向量求解出航向角，同时可以修正俯仰角。
静态检测包含角运动静止检测和线运动静止检测，因为本发明将利用这种静止状态启动一系列特殊的算法对系统的某些参数进行修正，所以要求检测静止的置信度较高。
角运动静止检测：陀螺传感器对载体的运动极为敏感，当载体处于静止状态时，陀螺传感器3个轴输出的数据的方差是恒定的，以3秒为时间长度对实时的数据样本进行统计，即当数据累积的时间长度为3秒时，利用式(23)计算其方差σ2，并和已经设置好的方差门限进行比较，如果3个轴的实时方差σ2连续2次均小于方差门限，则认为载体静止，否则判断载体运动，从而保证静止状态检测的置信度。方差门限通过对实际离线数据进行分析得到，按0.97的置信度取值；
线运动静止检测：主要利用里程计检测，因为里程计不存在累计误差，所以里程计对线运动的检测时100％可信的。但首先需要用加速度计或者卫星导航的速度检测里程计是否有效。
陀螺热致零偏模型估计与补偿：
对于消费级MEMS陀螺，其零偏误差可达到每小时几百度的级别，而且随温度变化剧烈，本发明给出的实时补偿算法可将零偏误差补偿到小于10°/h级别。实时估计陀螺的热致零偏需要在角运动静止的状态下进行，采用分段函数建立零偏与温度之间的关系，而且分段区间根据实时得到的样本统计值自动调整。并具有自动学习辨识功能，利用FLASH将历史的学习信息暂存，再次开电工作后与历史信息融合处理，随着系统运行时间的增大，该模型将越来越准确。
姿态更新：
姿态更新就是要求解下面式(24)的微分方程：
其中，是姿态四元数，是机体坐标系相对于导航坐标系的陀螺角速率，由构成的四元数矩阵。
式(24)四元数微分方程可以用四阶Runge-Kutta法求解，这里采用等效旋转矢量双子样法求解，求解步骤如下：
步骤z1：求角增量Δθ1、Δθ2和Δθk
Δθk＝Δθ1+Δθ2 (28)
步骤z2：求解它是从tk-1时刻到tk时刻b系相对于i系的等效旋转矢量。
步骤z3：求解
步骤z4：求解是从tk-1时刻到tk时刻b系相对于n系的等效旋转矢量.
步骤z5：求解它是以n系为参考系，b系从tk-1时刻到tk时刻的变换四元数。
步骤z6：计算
速度更新：
速度更新的过程描述为如图1所示，系统中对速度的观测信息是冗余的，可以通过读取里程计脉冲解算得到，也可以对加速度计的观测信息进行积分得到，当卫星导航信号质量较好时，也可以从卫导得到，这种冗余的设计可以保证系统的容错和可靠性，因为对加速度计积分得到的速度信息精度有限，所以仅用于容错。
里程计速度更新算法：
通过里程计脉冲和CAN总线可以测量得到载体的线速度大小v，并假设v存在尺度因子误差δK，则速度更新方程用(34)计算。
卫导速度更新算法：
当历程计失效但GNSS信号正常时，可以通过式(35)的GNSS给出的速度矢量的模代替里程计的观测速度，然后用式(36)将其分解到导航坐标系下，从而完成速度更新。
加速度计进行速度更新算法：
当里程计和GNSS都无效时，只能采用加速度计的积分来更新速度，其更新方法就是要求解式(37)的微分方程。
由于车载的动态性很低，用二阶Runge-kutta法求解上式微分方程，并对一些过程进行简化，具体步骤如下：
步骤j1：计算步长：
τ＝tk-tk-1 (38)
步骤j2：计算tk-1时刻的斜率K1
步骤j3：计算tk时刻速度的预报值：
步骤j4：计算tk时刻的斜率K2
步骤j5：计算tk时刻速度：
位置更新：

Claims (8)

1.一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，由以下步骤组成：
以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；
以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；
以所述状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；
利用所述状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出校正后的位置、速度和姿态信息；
其中，kalman滤波器迭代过程为：
设置滤波器的初值：
R＝diag{(3/6378138)2,(3/6378138)2,102} (6)
其中，上标T表示矩阵的转置，diag表示对角矩阵，下标0表示初始历元，是状态向量的初始值，P0是卡尔曼滤波器协方差矩阵的初始值，是初始的失准角误差的方差，是初始化位置误差的方差，是初始化陀螺零偏标定残差的方差，是初始化里程计尺度因子误差的方差，q0是系统激励噪声的方差，R是系统观测噪声方差矩阵，m为单位米，
求解状态转移矩阵：
Φk,k-1＝I10×10+F(tk-1)·Δt,Δt＝tk-tk-1 (7)
其中，Φk,k-1是卡尔曼滤波器的状态转移矩阵，下标(k,k-1)表示从k-1时刻向k时刻的状态转移，Δt是时间更新周期，
求解离散化的系统激励噪声矩阵Qk：
M1＝G(tk)*q0*G(tk)T (8)
M2＝F(tk)*M1+(F(tk)*M1)T (9)
M3＝F(tk)*M2+(F(tk)*M2)T (10)
kalman滤波器一步预测过程，先计算状态向量的先验估计再计算先验估计的方差矩阵Pk,k-1，计算方法如下：
kalman滤波器量测更新过程，计算过程如下：
z＝[LGNSS-LD λGNSS-λD hGNSS-hD]T (15)
Pk＝(I10×10-Kk·Hk)·Pk,k-1 (17)
以上诸式中，Kk是卡尔曼滤波器增益矩阵，Hk是k历元的观测矩阵，z是测量向量，它是GNSS的位置与INS&DR系统的位置之差，LGNSS、λGNSS、hGNSS分别是GNSS输出的经度、纬度和高度，LD、λD、hD分别是INS&DR系统输出的经度、纬度和高度，是状态向量在k历元的后验估计，Pk是后验估计的协方差矩阵，I10×10表示10阶单位矩阵，
最后，用对INS/DR系统的姿态、里程计尺度误差和位置误差进行反馈校正。
2.根据权利要求1所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于：
状态微分方程为：
其中，F(t)是系统矩阵；G(t)是系统的噪声驱动矩阵；是系统的状态向量，表示为： 是系统的激励噪声，表示为： 是陀螺3个轴的零偏未标定误差；φE,φN,φU是3个失准角，δL,δλ,δh是INS/DR系统的位置误差，是东向陀螺零偏误差，是北向陀螺零偏误差，是天向陀螺零偏误差，δK是里程计的尺度因子误差。
3.根据权利要求2所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，所述F(t)和G(t)的计算步骤如下：
其中，L是大地经度，h是椭球体高度，ωie是地球自转角速率，和分别是东向和北向速度，RM是当前位置的地球子午圈曲率半径，RN是地球卯酉圈曲率半径，Re是地球半径，是捷联矩阵，03×3是3阶零矩阵，I3×3是3阶单位矩阵。
4.根据权利要求3所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，INS/DR系统采用实时补偿算法对陀螺零偏进行初始标定，标定模型的辨识参数计算方法如下：
从[-40℃，85℃]的温度区间，以0.5℃为间隔作为温度序列，对每一列温度的标识零偏的可用性、求零偏的数据长度和对应温度点的零偏估计值，最终得到温度零偏映射表；
当载体处于角运动静止时，收集在同一个温度点上的连续陀螺数据样本，并用式(3)计算其均值作为零偏估计值；
其中，是待标定的陀螺零偏向量，是第k历元的陀螺采样值，N是样本容量；
任意时刻，首先读取到传感器的温度，搜索该温度在温度零偏映射表中对应的零偏是否有效，如果有效，则直接读出零偏估计值补偿到原始采样值上；否则以该温度点为中点，分别向两边搜索±10℃范围的有效温度点，然后用加权最小二乘算法对式(4)模型进行拟合，估计出模型常量参数c0x,c0y,c0z,c1x,c1y,c1z后，即可代入下面的模型估计出所需温度点的零偏值bωx,bωy,bωz：
其中，tk是k时刻的时间，Tk是k历元采样点的温度值。
5.根据权利要求1所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，kalman滤波器反馈校正的方法为：
先用式(18)计算失准角φ的等效旋转矢量四元数qφ,q，然后用式(19)校正INS/DR系统的姿态四元数其中，下标'k'表示历元，上标‘+'表示经过校正后的量，表示n系下东向失准角，表示n系下北向失准角，表示n系下天向失准角，n系为导航坐标系，·表示点乘；
位置校正用(20)计算，里程计刻度因子的校正直接将状态估计量δK代入到式(21)补偿INS/DR系统的速度v，得到补偿后的速度
其中，式(20)中，表示经校正的INS/DR系统在第k历元输出的经度，LD,k表示未经校正的INS/DR系统在第k历元输出的经度，δLk表示INS/DR系统在第k历元输出的经度的位置误差，表示经校正的INS/DR系统在第k历元输出的纬度，λD,k表示未经校正的INS/DR系统在第k历元输出的纬度，δλk表示INS/DR系统在第k历元输出的纬度的位置误差，表示经校正的INS/DR系统在第k历元输出的高度，hD,k表示未经校正的INS/DR系统在第k历元输出的高度，δhk表示INS/DR系统在第k历元输出的高度的位置误差。
6.根据权利要求5所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，在完成kalman滤波器解算过程后对INS/DR系统的安装误差和加速度计零偏进行在线估计，以消除捷联矩阵中包含的安装误差，估计方法为：
当陀螺零偏求解准确后，在开放天空，载体在水平路面上做8字形运动，且速度大于30Km/h，利用卫星导航的速度信息、加速度信息和INS系统的姿态角在数秒时间间隔的变化量解析出安装误差和加速计的零偏误差。
7.根据权利要求6所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，所述观测方程由以下步骤组成：
以GNSS的位置和INS/DR系统的位置之差作为滤波器的观测量，则得到观测方程为：
其中，υp是GNSS位置误差，z表示GNSS的位置和INS/DR系统的位置之差。
8.根据权利要求7所述的一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法，其特征在于，所述陀螺的数据采集频率为100Hz，加速度计的数据采集频率为10Hz，里程计信息有2种方式接入到INS/DR系统，一种方式是车速脉冲计数，采集频率为10Hz；另一种方式是汽车CAN总线，采集频率为1Hz。
CN201911408315.9A 2019-12-31 2019-12-31 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 Active CN111156994B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911408315.9A CN111156994B (zh) 2019-12-31 2019-12-31 一种基于mems惯性组件的ins/dr&gnss松组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911408315.9A CN111156994B (zh) 2019-12-31 2019-12-31 一种基于mems惯性组件的ins/dr&gnss松组合导航方法

Publications (2)

Publication Number Publication Date
CN111156994A CN111156994A (zh) 2020-05-15
CN111156994B true CN111156994B (zh) 2023-10-27

Family

ID=70559696

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911408315.9A Active CN111156994B (zh) 2019-12-31 2019-12-31 一种基于mems惯性组件的ins/dr&gnss松组合导航方法

Country Status (1)

Country Link
CN (1) CN111156994B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112051598B (zh) * 2020-06-24 2023-09-29 中铁第四勘察设计院集团有限公司 一种基于双重校正的车载gnss/ins组合导航方法
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN111780785A (zh) * 2020-07-20 2020-10-16 武汉中海庭数据技术有限公司 车载memsimu零偏自标定方法及系统
CN112197767B (zh) * 2020-10-10 2022-12-23 江西洪都航空工业集团有限责任公司 一种在线改进滤波误差的滤波器设计方法
CN112114339B (zh) * 2020-11-20 2021-02-19 四川中科川信科技有限公司 一种gnss数据微分迭代滤波解算方法
CN112710315A (zh) * 2020-12-15 2021-04-27 广州导远电子科技有限公司 一种基于智能车辆的车辆定位方法及装置
CN113188539B (zh) * 2021-04-27 2024-07-16 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113465628B (zh) * 2021-06-17 2024-07-09 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及系统
CN113865586B (zh) * 2021-09-29 2024-02-06 苏州挚途科技有限公司 安装角度的估计方法、装置和自动驾驶系统
CN113607176B (zh) * 2021-10-11 2021-12-10 智道网联科技(北京)有限公司 组合导航系统轨迹输出方法及装置
CN114002726A (zh) * 2021-11-15 2022-02-01 北京北斗华大科技有限公司 一种gnss/ins组合导航定位系统
CN114526731A (zh) * 2022-01-25 2022-05-24 杭州金通科技集团股份有限公司 一种基于助力车的惯性组合导航方向定位方法
CN114593749A (zh) * 2022-02-23 2022-06-07 贵州航天控制技术有限公司 一种导航测量组合标定补偿方法及测试台
CN117146810B (zh) * 2023-08-29 2024-02-13 广东精天科技有限公司 基于卫星导航与mems惯导的组合定位系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103487822A (zh) * 2013-09-27 2014-01-01 南京理工大学 北斗/多普勒雷达/惯性自主式组合导航系统及其方法
CN107389093A (zh) * 2017-07-13 2017-11-24 中国人民解放军63820部队吸气式高超声速技术研究中心 一种静基座下惯性平台初始自对准方法
CN108692727A (zh) * 2017-12-25 2018-10-23 北京航空航天大学 一种带有非线性补偿滤波器的捷联惯导系统
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109945860A (zh) * 2019-05-07 2019-06-28 深圳市联和安业科技有限公司 一种基于卫星紧组合的ins和dr惯性导航方法与系统
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7415354B2 (en) * 2006-04-28 2008-08-19 L-3 Communications Corporation System and method for GPS acquisition using advanced tight coupling
US8860609B2 (en) * 2008-10-23 2014-10-14 Texas Instruments Incorporated Loosely-coupled integration of global navigation satellite system and inertial navigation system
US10317421B2 (en) * 2014-03-31 2019-06-11 Stmicroelectronics S.R.L Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method
US20160146616A1 (en) * 2014-11-21 2016-05-26 Alpine Electronics, Inc. Vehicle positioning by map matching as feedback for ins/gps navigation system during gps signal loss

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103487822A (zh) * 2013-09-27 2014-01-01 南京理工大学 北斗/多普勒雷达/惯性自主式组合导航系统及其方法
CN107389093A (zh) * 2017-07-13 2017-11-24 中国人民解放军63820部队吸气式高超声速技术研究中心 一种静基座下惯性平台初始自对准方法
CN108692727A (zh) * 2017-12-25 2018-10-23 北京航空航天大学 一种带有非线性补偿滤波器的捷联惯导系统
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法
CN109945860A (zh) * 2019-05-07 2019-06-28 深圳市联和安业科技有限公司 一种基于卫星紧组合的ins和dr惯性导航方法与系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Improving GNSS road navigation integrity using MEMS INS and odometry;H. Beckmann; H. Niedermeier; B. Eissfeller;《2012 6th ESA Workshop on Satellite Navigation Technologies (Navitec 2012) & European Workshop on GNSS Signals and Signal Processing》;全文 *
基于GPS/DR的车辆导航中在线计算陀螺仪参数的一种方法;尹爱昌等;《电子器件》;第30卷(第01期);全文 *
弹载SINS/CNS组合导航系统在线标定方法;杨功流;王丽芬;杨淑洁;;弹箭与制导学报;34(05);全文 *
航位推算组合导航系统在线标定技术;刘永红，刘明雍，谢波;《中国惯性技术学报》;第23卷(第4期);全文 *

Also Published As

Publication number Publication date
CN111156994A (zh) 2020-05-15

Similar Documents

Publication Publication Date Title
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN109974697A (zh) 一种基于惯性系统的高精度测绘方法
CN107270893A (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN113029139B (zh) 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN101187567A (zh) 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN111102993A (zh) 一种旋转调制型捷联惯导系统晃动基座初始对准方法
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN109059909A (zh) 基于神经网络辅助的卫星/惯导列车定位方法与系统
CN103245359A (zh) 一种惯性导航系统中惯性传感器固定误差实时标定方法
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN113175933A (zh) 一种基于高精度惯性预积分的因子图组合导航方法
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN114415224A (zh) 一种复杂受限环境中车辆融合定位系统及方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
Nebot et al. Initial calibration and alignment of an inertial navigation
JPH0926328A (ja) 位置標定装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20230316

Address after: Room 401 and Room 402, Building 5, No. 690, Bibo Road, Pilot Free Trade Zone, Pudong New Area, Shanghai, March 2012

Applicant after: Shanghai Xingsi Semiconductor Co.,Ltd.

Address before: 710000 Floor 10, Building C, Xi'an National Digital Publishing Base, No. 996, Tiangu 7th Road, Yuhua Street Office, High-tech Zone, Xi'an City, Shaanxi Province

Applicant before: XI'AN AEROSPACE HUAXUN TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant