CN113340298B - 一种惯导和双天线gnss外参标定方法 - Google Patents

一种惯导和双天线gnss外参标定方法 Download PDF

Info

Publication number
CN113340298B
CN113340298B CN202110563502.5A CN202110563502A CN113340298B CN 113340298 B CN113340298 B CN 113340298B CN 202110563502 A CN202110563502 A CN 202110563502A CN 113340298 B CN113340298 B CN 113340298B
Authority
CN
China
Prior art keywords
time
moment
matrix
dual
carrier
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
CN202110563502.5A
Other languages
English (en)
Other versions
CN113340298A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202110563502.5A priority Critical patent/CN113340298B/zh
Publication of CN113340298A publication Critical patent/CN113340298A/zh
Application granted granted Critical
Publication of CN113340298B publication Critical patent/CN113340298B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明公开了一种惯导和双天线GNSS外参标定方法，属于卫星导航技术领域。该方法包括如下步骤：用双天线GNSS输出对SINS进行初始化，采用陀螺、加速度计传感器对载体的姿态、速度、位置进行预测，再将GNSS输出的位置、速度和航向通过卡尔曼滤波器实现对惯导和双天线GNSS外参的在线估计。相较于利用专业光学设备以获得较高测量精度的传统方法，本发明无须额外的专业设备，不仅节约了标定成本，还可以提高标定效率。

Description

一种惯导和双天线GNSS外参标定方法
技术领域
本发明涉及一种惯导和双天线GNSS(全球导航卫星系统)外参标定方法，属于卫星导航技术领域。
背景技术
在高精度SINS/GNSS(捷联惯性导航系统/全球导航卫星系统)组合导航系统中，需要利用GNSS测量信息对SINS的导航结果进行修正，同时估计惯性器件本身的漂移。而GNSS天线和IMU通常情况下不会安装在同一位置，因为GNSS为了减少信号遮挡会被安装于车辆顶部，而IMU(惯性测量单元)为了尽可能感应车体的角速度和加速度会被固连于车体内部，所以GNSS和SINS的位置、速度和航向也并不一致。为了更好的实现SINS与GNSS的组合，需要对两者之间的外参进行标定，包括IMU和GNSS天线之间的杆臂，以及SINS的航向与GNSS的航向之间的偏差角，其中SINS的航向为IMU的轴向指向，而双天线GNSS的航向为GNSS天线基线之间的航向。
当GNSS天线安装在车辆顶部而IMU安装在车辆内部时，利用尺或光学设备等传统手段直接测量两者之间的外参存在两方面的问题，一是测量很不方便，二是测量精度有限，而如果外参测量不准确，则会导致SINS/GNSS组合导航系统精度受限。
发明内容
针对传统外参测量手段测量精度有有限，且测量复杂、难度较大的问题，本发明提出了一种惯导和双天线GNSS外参在线标定方法，无须额外的专业设备，节约了标定的成本，提高了标定的效率。
本发明为解决上述技术问题采用以下技术方案：
一种惯导和双天线GNSS外参标定方法，包括以下步骤：
步骤1，周期采集k时刻车载传感器信息，包括惯性传感器信息和双天线GNSS信息；
步骤2，根据k时刻的惯性传感器数据，预测k时刻载体的姿态、速度、位置信息；
步骤3，若k时刻采集到双天线GNSS数据，则通过卡尔曼滤波器估计k时刻惯导和双天线GNSS之间的外参；否则，跳转至步骤1；
步骤4，跳转至步骤1。
所述步骤2的具体过程如下：
1)采用如下公式预测载体的姿态：
其中：
q(k)＝[q0(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置，其中q0(k)、q1(k)、q2(k)、q3(k)为表示k时刻载体姿态的4个数字；
q(k-1)＝[q0(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数，其中q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为表示k-1时刻载体姿态的4个数字；
ΔT为离散采样周期；
其中为k时刻机体系相对于导航系的角速率在机体系X、Y、Z轴上的分量；
为k时刻采集到的三轴陀螺数据；
为k-1时刻的导航系到机体系的姿态转移矩阵；
为k-1时刻地球自转角速率在导航系上的分量，ωie为地球自转角速率，L(k-1)为k-1时刻的纬度；
为k-1时刻导航系相对于地球系的角速度在导航系上的分量，为k-1时刻的速度在导航系东向、北向上的分量，L(k-1)、h(k-1)为k-1时刻的纬度和高度，RM、RN为地球的子午圈、卯酉圈半径；
2)采用如下公式预测载体的速度：
其中：
为k时刻的速度，为k时刻的速度在导航系东向、北向、天向上的分量；
为k-1时刻的速度，为k-1时刻的速度在导航系东向、北向、天向上的分量；
为k时刻的机体系到导航系的姿态转移矩阵；
fb(k)为k时刻采集到的三轴加速度计数据；
gn为地球重力加速度在导航系上的分量；
3)采用如下公式预测载体的位置：
其中：
λ(k)、L(k)、h(k)为k时刻的经度、纬度和高度；
λ(k-1)、L(k-1)、h(k-1)为k-1时刻的经度、纬度和高度。
所述步骤3的具体过程如下：
1)计算状态量的一步预测值
式中，
ΦM＝diag{0,0,0,0,0,0,0,0,0,0}
为载体的速度在导航系东向、北向、天向上的分量，L为载体的纬度，h为载体的高度，R为地球半径，/>为三轴加速度计的输出/>在导航系东北天方向上的分量，即/> 为机体系到导航系的姿态转移矩阵，0m×n为m×n的零矩阵，Φk,k-1为滤波器k-1时刻到k时刻的一步转移矩阵，/>为k-1时刻到k时刻的状态量一步预测值，/>为k-1时刻滤波器状态量估计值，φE、φN、φU为东向、北向、天向平台误差角，δvE、δvN、δvU为东向、北向、天向速度误差，δL、δλ、δh为纬度、经度、高度误差，/>为陀螺三轴零偏，/>为加速度计三轴零偏，ψ为双天线GNSS基线之间的航向与惯导轴向之间的偏差角，/>为GNSS主天线与惯导之间的三轴杆臂；
2)计算一步预测均方误差Pk|k-1
式中，
Pk|k-1为k-1时刻到k时刻的一步预测均方误差，Pk-1为k-1时刻的状态估计均方误差，上标T表示矩阵转置；
Γk-1为滤波器k-1时刻的系统噪声矩阵；
Qk-1＝diag{εgx 2,εgy 2,εgz 2,εax 2,εay 2,εaz 2}
Qk-1为k-1时刻的系统噪声，diag表示矩阵对角化，其中εgx、εgy、εgz分别为的模型噪声，εax、εay、εaz分别为/>的模型噪声；
3)计算k时刻卡尔曼滤波器的滤波增益Kk
式中，
Hp＝[03×6 diag{RM+h,(RN+h)cosL,1} 03×7 Hl]
Hv＝[03×3 diag{1,1,1} 03×13]
Hk为k时刻的量测矩阵，由位置量测矩阵Hp、速度量测矩阵Hv、航向量测矩阵组成，/>表示取矩阵/>的第m行，θ、/>分别为载体的俯仰角、航向角，可通过下式计算：
其中：q0、q1、q2、q3为表示载体姿态的四元数；
Rk为k时刻的量测噪声，diag表示矩阵对角化，其中εp为位置量测的噪声，εv为速度量测的噪声，为角度量测的噪声，上标-1表示矩阵求逆；
4)计算k时刻卡尔曼滤波器的状态估计值
其中，为k时刻滤波器状态量的估计值，/>为k-1时刻到k时刻的状态量一步预测值，Zk为k时刻的量测值，/>Zp、Zv、/>为位置、速度、航向量测值，LI、λI、hI为k时刻惯导预测的纬度、经度、高度，LG、λG、hG为k时刻双天线GNSS输出的纬度、经度、高度，VIE、VIN、VIU为k时刻惯导预测的速度在导航系东北天方向上的分量，VGE、VGN、VGU为k时刻双天线GNSS输出的速度在导航系东北天方向上的分量，/>为k时刻惯导预测的航向，/>为双天线GNSS输出的航向；
5)计算k时刻卡尔曼滤波器的估计均方误差Pk|k
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻的估计均方误差，I为单位矩阵；
6)基于卡尔曼滤波器，通过量测值Zk对惯导和双天线GNSS之间的外参进行估计。
本发明的有益效果如下：
本发明能够实现惯导和双天线GNSS之间外参的在线标定，无须额外的专业设备，节约了成本，也提高了效率。
附图说明
图1为本发明的流程图。
图2为本发明的航迹仿真图。
图3是采用本发明方法后对IMU和GNSS主天线之间的X轴杆臂的标定结果。
图4是采用本发明方法后对IMU和GNSS主天线之间的Y轴杆臂的标定结果。
图5是采用本发明方法后对IMU和GNSS主天线之间的Z轴杆臂的标定结果。
图6是采用本发明方法后对SINS航向与GNSS航向之间偏差角的标定结果。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
本发明实施例提供了一种惯导和双天线GNSS外参标定方法，流程图如图1所示，包括步骤如下：
步骤1，周期采集k时刻车载传感器信息；
步骤2，根据k时刻的惯性传感器数据，预测k时刻载体的姿态、速度、位置信息：
1)采用如下公式预测载体的姿态：
其中：
q(k)＝[q0(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置，其中q0(k)、q1(k)、q2(k)、q3(k)为表示k时刻载体姿态的4个数字；
q(k-1)＝[q0(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数，其中q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为表示k-1时刻载体姿态的4个数字；
ΔT为离散采样周期；
其中为k时刻机体系相对于导航系的角速率在机体系X、Y、Z轴上的分量；
为k时刻采集到的三轴陀螺数据；
为k-1时刻的导航系到机体系的姿态转移矩阵；
为k-1时刻地球自转角速率在导航系上的分量，ωie为地球自转角速率，L(k-1)为k-1时刻的纬度；
为k-1时刻导航系相对于地球系的角速度在导航系上的分量，为k-1时刻的速度在导航系东向、北向上的分量，L(k-1)、h(k-1)为k-1时刻的纬度和高度，RM、RN为地球的子午圈、卯酉圈半径；
2)采用如下公式预测载体的速度：
其中：
为k时刻的速度，/>为k时刻的速度在导航系东向、北向、天向上的分量；
为k-1时刻的速度，/> 为k-1时刻的速度在导航系东向、北向、天向上的分量；
为k时刻的机体系到导航系的姿态转移矩阵；
fb(k)为k时刻采集到的三轴加速度计数据；
gn为地球重力加速度在导航系上的分量；
3)采用如下公式预测载体的位置：
其中：
λ(k)、L(k)、h(k)为k时刻的经度、纬度和高度；
λ(k-1)、L(k-1)、h(k-1)为k-1时刻的经度、纬度和高度。
步骤3，若k时刻采集到双天线GNSS数据，则通过卡尔曼滤波器估计k时刻惯导和双天线GNSS之间的外参；否则，跳转至步骤1；
1)计算状态量的一步预测值
式中，
ΦM＝diag{0,0,0,0,0,0,0,0,0,0}
其中：为载体的速度在导航系东向、北向、天向上的分量，L为载体的纬度，h为载体的高度，R为地球半径，/>分别为三轴加速度计的输出在导航系东北天方向上的分量，即/> 为机体系到导航系的姿态转移矩阵，0m×n为m×n的零矩阵，Φk,k-1为滤波器k-1时刻到k时刻的一步转移矩阵，/>为k-1时刻到k时刻的状态量一步预测值，/>为k-1时刻滤波器状态量估计值，/>φE、φN、φU为东向、北向、天向平台误差角，δvE、δvN、δvU为东向、北向、天向速度误差，δL、δλ、δh为纬度、经度、高度误差，/>为陀螺三轴零偏，/>为加速度计三轴零偏，ψ为双天线GNSS基线之间的航向与惯导轴向之间的偏差角，/>为GNSS主天线与惯导之间的三轴杆臂；
2)计算一步预测均方误差Pk|k-1
式中，
Pk|k-1为k-1时刻到k时刻的一步预测均方误差，Pk-1为k-1时刻的状态估计均方误差，上标T表示矩阵转置；
为滤波器k-1时刻的系统噪声矩阵；
Qk-1＝diag{εgx 2,εgy 2,εgz 2,εax 2,εay 2,εaz 2}为k-1时刻的系统噪声，diag表示矩阵对角化，其中εgx、εgy、εgz分别为的模型噪声，εax、εay、εaz分别为/>的模型噪声；
3)计算k时刻卡尔曼滤波器的滤波增益Kk
/>
式中，
为k时刻的量测矩阵，由位置量测矩阵Hp、速度量测矩阵Hv、航向量测矩阵/>组成；
Hp＝[03×6 diag{RM+h,(RN+h)cosL,1} 03×7 Hl]
Hv＝[03×3 diag{1,1,1} 03×13]
表示取矩阵/>的第m行，θ、/>分别为载体的俯仰角、航向角，可通过下式计算：
其中：q0、q1、q2、q3为表示载体姿态的四元数；
为k时刻的量测噪声，其中εp为位置量测的噪声，εv为速度量测的噪声，/>为角度量测的噪声，上标-1表示矩阵求逆；
4)计算k时刻卡尔曼滤波器的状态估计值
其中，为k时刻滤波器状态量的估计值，/>为k-1时刻到k时刻的状态量一步预测值，Zk为k时刻的量测值，/>Zp、Zv、/>为位置、速度、航向量测值，LI、λI、hI为k时刻惯导预测的纬度、经度、高度，LG、λG、hG为k时刻双天线GNSS输出的纬度、经度、高度，VIE、VIN、VIU为k时刻惯导预测的速度在导航系东北天方向上的分量，VGE、VGN、VGU为k时刻双天线GNSS输出的速度在导航系东北天方向上的分量，/>为k时刻惯导预测的航向，/>为双天线GNSS输出的航向；
5)计算k时刻卡尔曼滤波器的估计均方误差Pk|k
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻的估计均方误差，I为单位矩阵；
6)基于卡尔曼滤波器，通过量测值Zk对惯导和双天线GNSS之间的外参进行估计。
实施例：
采用仿真的形式，对本发明方法进行实验验证。仿真条件设置如下：陀螺常值漂移为10°/h，随机漂移为10°/h；加速度计常值零偏为1mg，随机漂移为1mg；GNSS位置测量误差5cm，速度测量误差0.05m/s，航向测量误差0.1°；IMU和GNSS的杆臂为1m、0.5m、0.8m，航向偏差角为1°；陀螺与加速度计采样频率均为100Hz，GNSS采样频率为5Hz。
图2为车辆仿真航迹图，包括加减速、转弯、爬坡等。
进行了100次蒙特卡洛仿真测试，实验结果如图3、图4、图5和图6所示，可见估计出来的外参均在理论值附近震荡，精度均较高。实验精度统计如表1所示。
表1蒙特卡洛仿真实验精度统计
从以上精度统计可以看出，本发明方法估计的惯导和双天线GNSS之间的外参达到了很高的精度，多次计算的方差也较小，验证了本发明方法的有效性。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (2)

1.一种惯导和双天线GNSS外参在线标定方法，其特征在于，包括以下步骤：
步骤1，周期采集k时刻车载传感器信息，包括惯性传感器信息和双天线GNSS信息；
步骤2，根据k时刻的惯性传感器数据，预测k时刻载体的姿态、速度、位置信息；
步骤3，若k时刻采集到双天线GNSS数据，则通过卡尔曼滤波器估计k时刻惯导和双天线GNSS之间的外参；否则，跳转至步骤1；
步骤4，跳转至步骤1；
所述步骤3的具体过程如下：
1)计算状态量的一步预测值
式中，
ΦM＝diag{0,0,0,0,0,0,0,0,0,0}
为载体的速度在导航系东向、北向、天向上的分量，L为载体的纬度，h为载体的高度，R为地球半径，/>为三轴加速度计的输出/>在导航系东北天方向上的分量，即/>为机体系到导航系的姿态转移矩阵，0m×n为m×n的零矩阵，Φk,k-1为滤波器k-1时刻到k时刻的一步转移矩阵，为k-1时刻到k时刻的状态量一步预测值，/>为k-1时刻滤波器状态量估计值，φE、φN、φU为东向、北向、天向平台误差角，δvE、δvN、δvU为东向、北向、天向速度误差，δL、δλ、δh为纬度、经度、高度误差，/>为陀螺三轴零偏，/>为加速度计三轴零偏，ψ为双天线GNSS基线之间的航向与惯导轴向之间的偏差角，/>为GNSS主天线与惯导之间的三轴杆臂；
2)计算一步预测均方误差Pk|k-1
式中，
Pk|k-1为k-1时刻到k时刻的一步预测均方误差，Pk-1为k-1时刻的状态估计均方误差，上标T表示矩阵转置；
Γk-1为滤波器k-1时刻的系统噪声矩阵；
Qk-1为k-1时刻的系统噪声，diag表示矩阵对角化，其中εgx、εgy、εgz分别为的模型噪声，εax、εay、εaz分别为/>的模型噪声；
3)计算k时刻卡尔曼滤波器的滤波增益Kk
式中，
Hp＝[03×6 diag{RM+h,(RN+h)cosL,1} 03×7 Hl]
Hv＝[03×3 diag{1,1,1} 03×13]
Hk为k时刻的量测矩阵，由位置量测矩阵Hp、速度量测矩阵Hv、航向量测矩阵组成，表示取矩阵/>的第m行，θ、/>分别为载体的俯仰角、航向角，可通过下式计算：
其中：q0、q1、q2、q3为表示载体姿态的四元数；
Rk为k时刻的量测噪声，diag表示矩阵对角化，其中εp为位置量测的噪声，εv为速度量测的噪声，为角度量测的噪声，上标-1表示矩阵求逆；
4)计算k时刻卡尔曼滤波器的状态估计值
其中，为k时刻滤波器状态量的估计值，/>为k-1时刻到k时刻的状态量一步预测值，Zk为k时刻的量测值，/>Zp、Zv、/>为位置、速度、航向量测值，LI、λI、hI为k时刻惯导预测的纬度、经度、高度，LG、λG、hG为k时刻双天线GNSS输出的纬度、经度、高度，VIE、VIN、VIU为k时刻惯导预测的速度在导航系东北天方向上的分量，VGE、VGN、VGU为k时刻双天线GNSS输出的速度在导航系东北天方向上的分量，/>为k时刻惯导预测的航向，/>为双天线GNSS输出的航向；
5)计算k时刻卡尔曼滤波器的估计均方误差Pk|k
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻的估计均方误差，I为单位矩阵；
6)基于卡尔曼滤波器，通过量测值Zk对惯导和双天线GNSS之间的外参进行估计。
2.根据权利要求1所述的一种惯导和双天线GNSS外参在线标定方法，其特征在于，所述步骤2的具体过程如下：
1)采用如下公式预测载体的姿态：
其中：
q(k)＝[q0(k) q1(k) q2(k) q3(k)]T为k时刻的姿态四元数，上标T表示矩阵的转置，其中q0(k)、q1(k)、q2(k)、q3(k)为表示k时刻载体姿态的4个数字；
q(k-1)＝[q0(k-1) q1(k-1) q2(k-1) q3(k-1)]T为k-1时刻的姿态四元数，其中q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为表示k-1时刻载体姿态的4个数字；
ΔT为离散采样周期；
其中为k时刻机体系相对于导航系的角速率在机体系X、Y、Z轴上的分量；
为k时刻采集到的三轴陀螺数据；
为k-1时刻的导航系到机体系的姿态转移矩阵；
为k-1时刻地球自转角速率在导航系上的分量，ωie为地球自转角速率，L(k-1)为k-1时刻的纬度；
为k-1时刻导航系相对于地球系的角速度在导航系上的分量，为k-1时刻的速度在导航系东向、北向上的分量，L(k-1)、h(k-1)为k-1时刻的纬度和高度，RM、RN为地球的子午圈、卯酉圈半径；
2)采用如下公式预测载体的速度：
其中：
为k时刻的速度，为k时刻的速度在导航系东向、北向、天向上的分量；
为k-1时刻的速度，为k-1时刻的速度在导航系东向、北向、天向上的分量；
为k时刻的机体系到导航系的姿态转移矩阵；
fb(k)为k时刻采集到的三轴加速度计数据；
gn为地球重力加速度在导航系上的分量；
3)采用如下公式预测载体的位置：
其中：
λ(k)、L(k)、h(k)为k时刻的经度、纬度和高度；
λ(k-1)、L(k-1)、h(k-1)为k-1时刻的经度、纬度和高度。
CN202110563502.5A 2021-05-24 2021-05-24 一种惯导和双天线gnss外参标定方法 Active CN113340298B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110563502.5A CN113340298B (zh) 2021-05-24 2021-05-24 一种惯导和双天线gnss外参标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110563502.5A CN113340298B (zh) 2021-05-24 2021-05-24 一种惯导和双天线gnss外参标定方法

Publications (2)

Publication Number Publication Date
CN113340298A CN113340298A (zh) 2021-09-03
CN113340298B true CN113340298B (zh) 2024-05-17

Family

ID=77471017

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110563502.5A Active CN113340298B (zh) 2021-05-24 2021-05-24 一种惯导和双天线gnss外参标定方法

Country Status (1)

Country Link
CN (1) CN113340298B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113805109B (zh) * 2021-09-16 2022-11-11 广州文远知行科技有限公司 车载天线检测方法、装置、设备及存储介质
CN114061623B (zh) * 2021-12-30 2022-05-17 中国航空工业集团公司西安飞行自动控制研究所 一种基于双天线测向的惯性传感器零偏误差辨识方法
CN114894222B (zh) * 2022-07-12 2022-10-28 深圳元戎启行科技有限公司 Imu-gnss天线的外参数标定方法和相关方法、设备
CN116718153B (zh) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595715A (zh) * 2016-12-30 2017-04-26 中国人民解放军信息工程大学 基于捷联惯导/卫星组合导航系统里程计标定方法及装置
CN108594283A (zh) * 2018-03-13 2018-09-28 杨勇 Gnss/mems惯性组合导航系统的自由安装方法
CN110501024A (zh) * 2019-04-11 2019-11-26 同济大学 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112505737A (zh) * 2020-11-16 2021-03-16 东南大学 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102865881B (zh) * 2012-03-06 2014-12-31 武汉大学 一种惯性测量单元的快速标定方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595715A (zh) * 2016-12-30 2017-04-26 中国人民解放军信息工程大学 基于捷联惯导/卫星组合导航系统里程计标定方法及装置
CN108594283A (zh) * 2018-03-13 2018-09-28 杨勇 Gnss/mems惯性组合导航系统的自由安装方法
CN110501024A (zh) * 2019-04-11 2019-11-26 同济大学 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112505737A (zh) * 2020-11-16 2021-03-16 东南大学 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
一种多线激光雷达与GNSS/INS系统标定方法;叶珏磊;周志峰;王立端;庞正雅;;激光与红外(第01期);全文 *
弹载SINS/CNS组合导航系统在线标定方法;杨功流;王丽芬;杨淑洁;;弹箭与制导学报(第05期);全文 *
陆地车辆GNSS/MEMS惯性组合导航机体系约束算法研究;刘华;刘彤;张继伟;;北京理工大学学报(第05期);全文 *

Also Published As

Publication number Publication date
CN113340298A (zh) 2021-09-03

Similar Documents

Publication Publication Date Title
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN103245359B (zh) 一种惯性导航系统中惯性传感器固定误差实时标定方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN104215262A (zh) 一种惯性导航系统惯性传感器误差在线动态辨识方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN106052686A (zh) 基于dsptms320f28335的全自主捷联惯性导航系统
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN105091907A (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN104764467A (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN106885587A (zh) 旋翼扰动下惯性/gps组合导航外杆臂效应误差补偿方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
Maklouf et al. low cost IMU\GPS integration using kalman filtering for land vehicle navigation application
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN105928519B (zh) 基于ins惯性导航与gps导航以及磁力计的导航算法
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
Xue et al. MEMS-based multi-sensor integrated attitude estimation technology for MAV applications
CN114705215A (zh) 一种捷联惯导系统的纬度估计方法
CN113932803A (zh) 适用于高动态飞行器的惯性/地磁/卫星组合导航系统

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
GR01 Patent grant
GR01 Patent grant