CN114994732A - 基于gnss载波相位的车载航向快速初始化装置及方法 - Google Patents

基于gnss载波相位的车载航向快速初始化装置及方法 Download PDF

Info

Publication number
CN114994732A
CN114994732A CN202210931252.0A CN202210931252A CN114994732A CN 114994732 A CN114994732 A CN 114994732A CN 202210931252 A CN202210931252 A CN 202210931252A CN 114994732 A CN114994732 A CN 114994732A
Authority
CN
China
Prior art keywords
track
module
course
tdcp
initial
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.)
Granted
Application number
CN202210931252.0A
Other languages
English (en)
Other versions
CN114994732B (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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202210931252.0A priority Critical patent/CN114994732B/zh
Publication of CN114994732A publication Critical patent/CN114994732A/zh
Application granted granted Critical
Publication of CN114994732B publication Critical patent/CN114994732B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/53—Determining attitude
    • G01S19/54—Determining attitude using carrier phase measurements; using long or short baseline interferometry
    • 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

Landscapes

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

Abstract

本发明公开了一种基于GNSS载波相位的车载航向快速初始化装置，包括航位推算模块、单星载波航向计算通道和航向最优估计模块。本发明还提供一种基于GNSS载波相位的车载航向快速初始化方法，首先DR模块根据惯性传感器陀螺输出和车辆前进距离推导出以任意设置的初始航向为初值的DR轨迹，然后单星载波航向计算通道根据单颗卫星的载波观测值获取真实运动轨迹方向，结合DR轨迹方向得到初始航向，最后通过航向最优估计模块对所有卫星通道输出的航向进行筛选和平均，得到精确且可靠的初始航向。该技术方案能够在95%的置信条件下，5秒对准时长时航向初始化精度达到0.65度。本发明采用GNSS载波相位观测作为初始化条件，在城市环境中具有较高的实用性。

Description

基于GNSS载波相位的车载航向快速初始化装置及方法
技术领域
本发明属于INS/GNSS组合导航系统技术领域，具体涉及一种基于GNSS载波相位的车载航向快速初始化装置及方法。
背景技术
惯性导航系统（Inertial Navigation System, INS）可以在不依赖外部信息的情况下自主导航，但该系统的状态（位置、速度和姿态）需要在导航开始前进行初始化。位置和速度的初始化较为容易，全球导航卫星系统（Global Navigation Satellite System,GNSS）可以准确地提供初始位置和初始速度。相比之下，姿态初始化过程比较复杂，包括航向、俯仰和横滚的初始化。
一般来说，由于高精度惯性测量单元（Inertial Measurement Unit, IMU）和低成本IMU具有不同的噪声特性，因此二者的初始对准方法不同。高精度IMU可以使用传统的静态对准进行姿态初始化，通过感知局部重力和地球自转速率的方式获得初始姿态。而低成本的微机电（Micro Electro Mechanical System, MEMS）IMU，则是利用加速度计感应地球重力得到初始俯仰和初始横滚。然而，受限于技术和工艺因素，MEMS IMU的陀螺偏置不稳定性通常超过地球自转速率（15度每小时），无法通过感知地球自转的方式进行航向初始化。低性能MEMS陀螺仪给MEMS IMU的航向初始化带来了挑战，航向快速准确地初始化是MEMSIMU亟需攻克的难题。
对于车载动态场景，MEMS IMU需要外部辅助信息进行航向初始化。常见的辅助信息有GNSS、里程计等。近年来，实现车载场景航向初始化较为常用的解决方案大致可分为两种：基于卡尔曼滤波的方法和基于优化的迭代方法。基于卡尔曼滤波的方法能够准确地对状态误差进行建模，因此该方法能获得精度很高的航向结果，但该方法需要较长的收敛时间，例如，一种使用自适应的无迹卡尔曼滤波方式可在50秒收敛至0.4度（均方根值），一种两级卡尔曼滤波的方式可在150秒收敛到0.3度。基于优化的方法将姿态对准问题转化为使用无限矢量观测的连续姿态确定问题，这种方法最初应用于导航级IMU，近年来，许多基于优化方式的变体将其扩展到低成本IMU上，但该方式同样需要较长的收敛时间，例如一种基于速度的优化对准方式60秒可获得4度的航向对准精度，一种基于多普勒的优化对准方式40秒收敛至1.57度。
虽然上述方法的对准精度可以满足需求，但均存在对准时间长的问题。然而，多数应用场景里都需要MEMS IMU具有快速的初始化能力，初始化的准确性和时间均非常重要。发明人前期提出的一种适用于倾斜RTK应用场景的快速准确航向初始化方法，能够在短时间内达到很高的精度，测试结果表明该方法在98.5%的置信条件下2s内可达1.15度的航向初始化精度。但是该方法需要实时动态（Real Time Kinematic, RTK）定位结果作为辅助信息，RTK定位需要基站信息和充足的卫星观测信息，对应用环境要求苛刻，无法应用于城市环境车载导航。因此，对于城市环境的车载航向初始化，需要一种兼顾效率、准确性和实用性的航向初始化方法。
发明内容
针对目前的航向初始化方案难以兼顾效率、准确性和实用性的问题，本发明提供一种使用GNSS载波相位观测值进行车载航向快速初始化的装置及方法。仅使用载波相位观测值和基于伪距的单点定位结果作为输入信息，不需要基于载波相位的RTK定位结果，在95%的置信条件下，5秒内航向初始化精度可以达到0.65度。
一种基于GNSS载波相位的车载航向快速初始化装置，包括DR模块、单星载波航向计算通道和航向最优估计模块。
DR模块包含姿态更新模块和位置更新模块，姿态更新模块利用陀螺输出递推下一时刻姿态，提供给位置更新模块计算下一时刻的位置。DR模块的输入为初始姿态、初始位置、陀螺输出和前进距离，输出为DR轨迹起点和终点坐标。初始姿态里的俯仰和横滚通过加速度计标定的方式得到，初始航向为任意设置值；初始位置由GNSS单点定位提供；陀螺输出来源于IMU测量值；前进距离由GNSS或里程计提供。DR模块的作用是利用陀螺输出和车辆前进距离推出DR轨迹，由于DR的初始航向为任意值，获得的DR轨迹与真实轨迹形状相似但存在旋转，该旋转角度即为DR设置的初始航向与真实初始航向的偏差。
单星载波航向计算通道包括TDCP增量计算模块和轨迹匹配模块，TDCP增量计算模块输出观测TDCP与DR轨迹TDCP之间的增量，并提供给轨迹匹配模块获得真实运动轨迹方向和初始航向。每个通道的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，输出为初始航向和真实轨迹方位。单星载波航向计算通道的作用是根据单颗卫星的载波观测值计算初始航向，与DR轨迹方向相减得到轨迹间旋转角度，也即DR的设置初始航向偏差，每个通道分别计算基于一颗卫星载波相位的初始航向，通道数与有效载波相位观测的数量一致。
TDCP增量计算模块用于计算观测TDCP与DR轨迹TDCP之间的增量，该模块的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，输出为TDCP增量，所得结果用于轨迹匹配模块。TDCP增量计算模块由DR轨迹的TDCP计算模块、观测TDCP计算模块构成，两个模块分别计算对应的TDCP并输出，输出量相减即为TDCP增量。
轨迹匹配模块利用DR轨迹起点和终点坐标计算DR轨迹方位，再使用TDCP增量推算真实运动轨迹方位，从而获得DR轨迹相对于真实轨迹的旋转角度，也即DR设置的初始航向偏差。轨迹匹配模块的输入为DR轨迹起点和终点坐标、卫星星历和TDCP增量，输出为初始航向和真实轨迹方位角。轨迹匹配模块包括DR轨迹方位角计算模块和真实轨迹方位角计算模块，DR轨迹方位角计算模块的输出是真实轨迹方位角计算模块的输入，并与真实轨迹方位角模块的输出相减以获得初始航向。
航向最优估计模块的作用为对所有单星载波航向计算通道输出的航向进行筛选，去掉可能存在粗差的航向计算值，并将筛选后的航向进行平均，以提高计算的初始航向精度。该模块的输入为所有单星载波航向计算通道的输出和卫星星历，输入的初始航向和真实轨迹方位角的数量与单星载波航向计算通道数量一致，输出为初始航向结果。
一种基于GNSS载波相位的车载航向快速初始化方法，包括以下步骤：
步骤1，利用DR模块，根据惯性传感器陀螺输出和车辆前进距离推导出以任意设置的初始航向为初值的DR轨迹。
步骤1.1，利用DR模块中姿态更新模块得到车辆下一时刻的姿态。
步骤1.2，利用步骤1.1得到的车辆姿态，使用DR模块中位置更新模块得到车辆下一时刻的位置。
步骤1.3，重复执行步骤1.1和步骤1.2得到车辆的DR轨迹。
步骤2，利用单星载波航向计算通道模块，根据单颗卫星的载波观测值获取真实运动轨迹方向，结合DR轨迹方向得到初始航向。
步骤2.1，利用DR轨迹的TDCP计算模块得到DR轨迹的TDCP。
步骤2.2，利用观测TDCP计算模块得到观测TDCP。
步骤2.3，根据步骤2.1得到的DR轨迹的TDCP和2.2得到的观测TDCP，利用TDCP增量计算模块得到TDCP增量。
步骤2.4，根据步骤1得到的DR轨迹，利用DR轨迹方位角计算模块得到车辆的DR轨迹方位角。
步骤2.5，根据步骤1得到的DR轨迹、步骤2.3得到的TDCP增量和步骤2.4得到的DR轨迹方位角，利用真实轨迹的方位角计算模块得到车辆的DR真实轨迹方位角。
步骤2.6，由步骤2.4得到的DR轨迹方位角和步骤2.5得到的DR真实轨迹方位角相减得到初始航向。
步骤3，通过航向最优估计模块对步骤2得到的所有卫星通道输出的航向进行筛选，并求平均值得到精确且可靠的初始航向。
而且，所述步骤1.1中姿态更新模块的输入为惯性传感器的陀螺输出角增量
Figure 386891DEST_PATH_IMAGE001
和
Figure 728880DEST_PATH_IMAGE002
时刻的姿态
Figure 670291DEST_PATH_IMAGE003
，输出为
Figure 252582DEST_PATH_IMAGE004
时刻的姿态
Figure 646654DEST_PATH_IMAGE005
，
该模块的公式描述如下：
Figure 592001DEST_PATH_IMAGE006
(1)
Figure 591181DEST_PATH_IMAGE007
(2)
Figure 344373DEST_PATH_IMAGE008
(3)
式中：
Figure 740588DEST_PATH_IMAGE009
表示b系到n系的方向余弦矩阵，每一时刻的方向余弦矩阵基于上一时刻 的方向余弦矩阵递推得到，初始方向余弦矩阵由该模块的输入初始姿态转换而来，初始姿 态中俯仰和横滚易由加速度计确定，航向可为任意设置值，后续步骤通过计算任意设置的 初始航向偏差计算初始航向；b系为IMU坐标系，其轴向与IMU轴向一致，坐标原点为陀螺仪 和加速度计的坐标原点；n系为当地水平坐标系，坐标原点为车辆运动起点处所在的地球表 面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下；
Figure 175112DEST_PATH_IMAGE010
是b系的等效旋转向量；
Figure 825536DEST_PATH_IMAGE011
表示向量
Figure 264477DEST_PATH_IMAGE010
的长度；
Figure 836403DEST_PATH_IMAGE012
是沿着
Figure 871356DEST_PATH_IMAGE010
方向的单位旋转矢量；
Figure 891133DEST_PATH_IMAGE013
和
Figure 986128DEST_PATH_IMAGE014
表示陀螺输出角 增量；
Figure 45351DEST_PATH_IMAGE015
是3×3的单位矩阵，
Figure 130332DEST_PATH_IMAGE016
表示反对称矩阵。
而且，所述步骤1.2中位置更新模块的输入为
Figure 958610DEST_PATH_IMAGE017
时刻的位置
Figure 224507DEST_PATH_IMAGE018
、 前进距离
Figure 817031DEST_PATH_IMAGE019
，输出为下一时刻的位置
Figure 662627DEST_PATH_IMAGE020
，具体实现过程表示为：
Figure 142150DEST_PATH_IMAGE021
(4)
Figure 828215DEST_PATH_IMAGE022
(5)
Figure 393189DEST_PATH_IMAGE023
(6)
式中：向量
Figure 573634DEST_PATH_IMAGE024
为
Figure 625773DEST_PATH_IMAGE025
时刻的纬度、经度和高度，在第一次更新 时该值为输入DR模块的初始位置；
Figure 499051DEST_PATH_IMAGE026
为下一时刻的纬度、经度和高度；
Figure 551321DEST_PATH_IMAGE027
和
Figure 725338DEST_PATH_IMAGE028
是沿经线和纬线的曲率半径；
Figure 445032DEST_PATH_IMAGE029
，
Figure 223632DEST_PATH_IMAGE030
和
Figure 215728DEST_PATH_IMAGE031
为前进距离向量
Figure 737976DEST_PATH_IMAGE032
的 分量，表示n系下
Figure 312177DEST_PATH_IMAGE033
到
Figure 261678DEST_PATH_IMAGE034
时刻的位置增量；n系为当地水平坐标系，坐标原点为车辆运动起 点处所在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下。
Figure 475491DEST_PATH_IMAGE035
时刻n系下的位置增量可由同一时刻v系下的位置增量
Figure 801430DEST_PATH_IMAGE036
通过下式转换得 到：
Figure 230137DEST_PATH_IMAGE037
(7)
式中：
Figure 599807DEST_PATH_IMAGE009
表示b系到n系的方向余弦矩阵；
Figure 317228DEST_PATH_IMAGE038
表示v系到b系的方向余弦矩阵，由IMU 的安装角决定；
Figure 181278DEST_PATH_IMAGE036
为v系下的位置增量；b系为IMU坐标系，其轴向与IMU轴向一致，坐标 原点为陀螺仪和加速度计的坐标原点；v系为车体坐标系，其坐标原点为车后轴中心点，x轴 与车辆前进方向一致，z轴垂直向下，y轴朝外垂直于xz所在平面。
Figure 710830DEST_PATH_IMAGE036
可以由v系下速度的积分计算：
Figure 470976DEST_PATH_IMAGE039
(8)
式中：
Figure 472430DEST_PATH_IMAGE040
是车辆在v系下的速度，根据v系的性质，该向量除了第一个元素以外其 余元素均为零，即
Figure 123860DEST_PATH_IMAGE041
，该值由GNSS定位信息或里程计提供。
而且，所述步骤1.3中重复执行步骤1.1和步骤1.2得到经过时间
Figure 464842DEST_PATH_IMAGE042
后的车辆位置
Figure 458206DEST_PATH_IMAGE043
，将
Figure 665066DEST_PATH_IMAGE044
作为轨迹终点，初始位置作为轨迹起点，将这两个坐标转换到地心地固 坐标系下，得到起点坐标
Figure 136498DEST_PATH_IMAGE045
和终点坐标
Figure 331987DEST_PATH_IMAGE046
，即为车辆的DR轨迹；地心地固坐标系的原点为地球原点，x 轴指向格林威治子午面的交线，z轴指向北极，y轴在赤道平面内与x轴和z轴满足右手系法 则。
而且，所述步骤2.1中DR轨迹的TDCP计算模块的输入为卫星星历和DR轨迹起点和终点坐标，输出为DR轨迹的TDCP，该模块的实现方式如下：
Figure 214362DEST_PATH_IMAGE047
(9)
式中：
Figure 924829DEST_PATH_IMAGE048
为DR轨迹的TDCP，
Figure 934373DEST_PATH_IMAGE049
为GNSS载波波长，t为DR轨迹的时间跨度，
Figure 46685DEST_PATH_IMAGE050
和
Figure 571732DEST_PATH_IMAGE051
分别表示航位推算轨迹起点和终点到对应时刻卫星i的视线距离，视线距离的计算公 式如下：
Figure 503916DEST_PATH_IMAGE052
(10)
Figure 582731DEST_PATH_IMAGE053
(11)
式中：t为DR轨迹的时间跨度，
Figure 798817DEST_PATH_IMAGE054
为DR轨迹的起点坐标， 来源于DR模块输出；
Figure 242568DEST_PATH_IMAGE055
为DR轨迹的终点坐标，来源于DR模块 输出；
Figure 927627DEST_PATH_IMAGE056
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历计算 该坐标值；
Figure 731504DEST_PATH_IMAGE057
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历计 算该坐标值；上述坐标的坐标系均为地心地固坐标系。
而且，所述步骤2.2中观测TDCP计算模块的输入为GNSS接收机在0时刻和t时刻的 载波相位观测值
Figure 552829DEST_PATH_IMAGE058
和
Figure 964219DEST_PATH_IMAGE059
，输出为观测TDCP，该模块的实现方式为：
Figure 120263DEST_PATH_IMAGE060
(12)
式中：
Figure 478563DEST_PATH_IMAGE061
为观测TDCP，
Figure 419974DEST_PATH_IMAGE062
和
Figure 475303DEST_PATH_IMAGE063
为GNSS接收机在0时刻和t时刻的载波相位观 测值。
而且，所述步骤2.3中TDCP增量计算模块的输出为：
Figure 869375DEST_PATH_IMAGE064
(13)
式中：
Figure 828104DEST_PATH_IMAGE065
为TDCP增量，即TDCP增量计算模块的输出；
Figure 873289DEST_PATH_IMAGE066
为观测TDCP，
Figure 829744DEST_PATH_IMAGE067
为DR轨迹的TDCP。
而且，所述步骤2.4中DR轨迹方位角计算模块的输入为DR轨迹起点和终点坐标，输 出为DR轨迹方位角
Figure 711112DEST_PATH_IMAGE068
，该模块的实现过程为：
Figure 457220DEST_PATH_IMAGE069
(14)
式中：
Figure 310907DEST_PATH_IMAGE070
和
Figure 500580DEST_PATH_IMAGE071
分别为DR轨迹的东向和北向的位置增量，将DR轨迹起点与 终点间的位置增量转换到n系可得该分量；n系为当地水平坐标系，坐标原点为车辆运动起 点处所在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下。
而且，所述步骤2.5中真实轨迹的方位角计算模块的输入为卫星星历、DR轨迹起点 和终点坐标、TDCP增量与DR轨迹方位角
Figure 118512DEST_PATH_IMAGE072
，输出为真实轨迹方位角
Figure 356726DEST_PATH_IMAGE073
，实现方式如下：
Figure 861657DEST_PATH_IMAGE074
(15)
式中：
Figure 474428DEST_PATH_IMAGE075
为卫星i的方位角，根据卫星星历计算；
Figure 64810DEST_PATH_IMAGE076
为卫星i的仰角，根据卫星星历 计算；
Figure 106715DEST_PATH_IMAGE077
为GNSS载波波长；
Figure 980999DEST_PATH_IMAGE078
为航位推算轨迹增量长度，根据DR轨迹起点和终点坐标计 算；
Figure 715737DEST_PATH_IMAGE072
为DR轨迹方位角，来源于DR轨迹方位角计算模块。
而且，所述步骤2.6中初始航向的计算方式为：
Figure 58993DEST_PATH_IMAGE079
(16)
Figure 685016DEST_PATH_IMAGE080
(17)
式中，
Figure 367801DEST_PATH_IMAGE081
为DR轨迹与真实运动轨迹之间的夹角，
Figure 804598DEST_PATH_IMAGE082
为DR轨迹方位角，
Figure 884419DEST_PATH_IMAGE073
为真实轨 迹方位角，
Figure 268127DEST_PATH_IMAGE083
为任意设置的初始航向，
Figure 133315DEST_PATH_IMAGE084
为初始航向。
而且，所述步骤3中计算水平面上的卫星与真实运动轨迹之间的夹角，即卫星方位角与真实轨迹方位角之差，选择角度在N1度到N2度之间的卫星，将筛选出的卫星对应的初始航向进行平均，得到最终的初始航向。
与现有技术相比，本发明具有如下优点：
1）本发明取得的航向初始化精度高。
95%的置信条件下5秒内航向初始化精度可以达到0.65度，现有算法例如使用卡尔曼滤波的方式获得初始航向，约150秒的收敛时间误差可收敛到0.3度以内，基于优化的迭代方式约60秒的收敛时间达到4度的精度水平。
2）本发明仅需GNSS载波相位观测值作为辅助信息，简单易得，实用性强。
目前常见的车载航向初始化算法的需要的辅助信息有：磁力计、双天线、GNSS定位信息等。磁力计辅助会受到磁场干扰，不适用于复杂场景。双天线法硬件成本较高。GNSS定位信息需要至少四颗卫星的观测值，有时甚至需要RTK等高精度定位结果，而获得RTK结果需要基准站信息，且需要多颗稳定观测的卫星以确保连续的厘米级定位，在城市环境下不一定满足这一条件。本发明所使用的载波相位观测值，是用于RTK定位的底层观测量，也具有相对精度高的特性，因此使用载波相位作为辅助信息可以使获得的初始航向具有较高精度。并且目前大多数接收机都可以提供载波相位观测值，使得本发明所需的初始化条件简单易得，具有很高的实用价值。
附图说明
图1为本发明航向快速初始化装置整体结构框图。
图2为本发明中DR模块结构框图。
图3为本发明中单星载波航向计算通道。
图4为本发明中TDCP增量计算模块结构框图。
图5为本发明中轨迹匹配模块结构框图。
图6为本发明原理图俯视图。
图7为本发明原理图主视图。
图8为本发明实施例中使用STIM300的数据进行的1506次对准实验的航向误差结果。
图9为本发明实施例中使用STIM300的数据进行的1506次对准实验的航向误差的累积分布函数图。
图10为本发明实施例中不同卫星获得的初始航向误差的累积分布函数图。
图11为本发明实施例中使用ICM20602的数据进行的1506次对准实验的航向误差结果。
具体实施方式
本发明提供一种基于GNSS载波相位的车载航向快速初始化装置及方法，下面结合附图和实施例对本发明的技术方案作进一步说明。该实施例测试场景为车载动态开阔场景，航向初始化模块的前进距离可以由GNSS或里程计提供，本实施例中使用GNSS提供前进距离，对准时长根据经验值设置为5秒。
如图1所示，本发明提供一种基于GNSS载波相位的车载航向快速初始化装置，包括航位推算（Dead Reckoning，DR）模块、单星载波航向计算通道和航向最优估计模块。
DR模块的结构如图2所示，包含姿态更新模块和位置更新模块，姿态更新模块利用陀螺输出递推下一时刻姿态，提供给位置更新模块计算下一时刻的位置。DR模块的输入为初始姿态、初始位置、陀螺输出和前进距离，输出为DR轨迹起点和终点坐标。初始姿态里的俯仰和横滚通过加速度计标定的方式得到，初始航向为任意设置值，本实施例中设置初始航向为0度；初始位置由GNSS单点定位提供；陀螺输出来源于IMU测量值；前进距离可由GNSS或里程计提供，本实施例中使用GNSS单点定位信息获得前进距离。DR模块的作用是利用陀螺输出和车辆前进距离推出DR轨迹，由于DR的初始航向为任意值，获得的DR轨迹与真实轨迹形状相似但存在旋转，该旋转角度即是DR设置的初始航向与真实初始航向的偏差。
单星载波航向计算通道的结构和内部连接如图3所示，单星载波航向计算通道包括载波相位时间差分（Time Differenced Carrier Phase, TDCP）增量计算模块和轨迹匹配模块，TDCP增量计算模块输出观测TDCP与DR轨迹TDCP之间的增量，并提供给轨迹匹配模块获得真实运动轨迹方向和初始航向。单星载波航向计算通道利用一颗卫星的载波相位观测值获得初始航向，不同卫星的载波相位观测值输入对应的航向计算通道，这些通道并行运行，通道数量与有效载波相位观测值数量一致。每个通道的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，输出为初始航向和真实轨迹方位。
TDCP增量计算模块的组成与连接方式如图4所示，用于计算观测TDCP与DR轨迹TDCP之间的增量。该模块的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，其中DR轨迹起点和终点坐标来源于DR模块输出，该模块的输出为TDCP增量，所得结果用于轨迹匹配模块。TDCP增量计算模块由DR轨迹的TDCP计算模块、观测TDCP计算模块构成，两个模块分别计算对应的TDCP并输出，输出量相减即为TDCP增量。
轨迹匹配模块的组成与连接方式如图5所示，该模块利用DR轨迹起点和终点坐标计算DR轨迹方位，再使用TDCP增量推算真实运动轨迹方位，从而获得DR轨迹相对于真实轨迹的旋转角度，也即DR设置的初始航向偏差。模块的输入为DR轨迹起点和终点坐标、卫星星历和TDCP增量，输出为初始航向和真实轨迹方位角。该模块包括DR轨迹方位角计算模块和真实轨迹方位角计算模块，DR轨迹方位角计算模块的输出是真实轨迹方位角计算模块的输入，并与真实轨迹方位角模块的输出相减以获得初始航向。
单星载波航向计算通道的作用是根据单颗卫星的载波观测值计算初始航向，由于每颗卫星的载波观测值增量都包含真实运动信息，从中可获取真实运动轨迹方向，与DR轨迹方向相减可得轨迹间旋转角度，也即DR的设置初始航向偏差，每个通道分别计算基于一颗卫星载波相位的初始航向，通道数与有效载波相位观测的数量一致。
航向最优估计模块的作用为对所有单星载波航向计算通道输出的航向进行筛选，去掉可能存在粗差的航向计算值，并将筛选后的航向进行平均，以提高计算的初始航向精度。该模块的输入为所有单星载波航向计算通道的输出和卫星星历，输入的初始航向和真实轨迹方位角的数量与单星载波航向计算通道数量一致，输出为初始航向结果。
本发明实施例还提供一种基于GNSS载波相位的车载航向快速初始方法，包括以下步骤：
步骤1，利用DR模块，根据惯性传感器陀螺输出和车辆前进距离推导出以任意设置的初始航向为初值的DR轨迹。
步骤1.1，利用DR模块中姿态更新模块得到车辆下一时刻的姿态。
姿态更新模块的输入为惯性传感器的陀螺输出角增量
Figure 987351DEST_PATH_IMAGE085
和
Figure 977304DEST_PATH_IMAGE086
时刻的姿态
Figure 961441DEST_PATH_IMAGE087
，输出为
Figure 930402DEST_PATH_IMAGE088
时刻的姿态
Figure 912265DEST_PATH_IMAGE089
，该模块的公式描述如下：
Figure 451831DEST_PATH_IMAGE090
(1)
Figure 223346DEST_PATH_IMAGE091
(2)
Figure 531968DEST_PATH_IMAGE092
(3)
式中：
Figure 481470DEST_PATH_IMAGE093
表示b系（IMU坐标系，其轴向与IMU轴向一致，坐标原点为陀螺仪和加速 度计的坐标原点）到n系（N-E-D坐标系，即当地水平坐标系，坐标原点为车辆运动起点处所 在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下）的方向余弦矩阵， 每一时刻的方向余弦矩阵基于上一时刻的方向余弦矩阵递推得到，初始方向余弦矩阵由该 模块的输入初始姿态转换而来，初始姿态中俯仰和横滚易由加速度计确定，航向可为任意 设置值，后续步骤通过计算任意设置的初始航向偏差计算初始航向；
Figure 960861DEST_PATH_IMAGE094
是b系的等效旋转 向量；
Figure 286800DEST_PATH_IMAGE095
表示向量
Figure 715508DEST_PATH_IMAGE094
的长度；
Figure 291370DEST_PATH_IMAGE096
是沿着
Figure 805528DEST_PATH_IMAGE094
方向的单位旋转矢量；
Figure 872841DEST_PATH_IMAGE097
和
Figure 405322DEST_PATH_IMAGE098
表示陀螺 输出角增量；
Figure 962206DEST_PATH_IMAGE099
是3×3的单位矩阵，
Figure 901343DEST_PATH_IMAGE100
表示反对称矩阵。
步骤1.2，利用步骤1.1得到的车辆姿态，使用DR模块中位置更新模块得到车辆下一时刻的位置。
位置更新模块的输入为
Figure 83931DEST_PATH_IMAGE101
时刻的位置
Figure 424914DEST_PATH_IMAGE102
、前进距离
Figure 152699DEST_PATH_IMAGE103
，输出为 下一时刻的位置
Figure 625137DEST_PATH_IMAGE104
，具体实现过程表示为：
Figure 299832DEST_PATH_IMAGE105
(4)
Figure 292059DEST_PATH_IMAGE106
(5)
Figure 640345DEST_PATH_IMAGE107
(6)
式中：向量
Figure 616391DEST_PATH_IMAGE108
为
Figure 625936DEST_PATH_IMAGE109
时刻的纬度、经度和高度，在第一次更新 时该值为输入DR模块的初始位置；
Figure 925199DEST_PATH_IMAGE110
为下一时刻的纬度、经度和高度；
Figure 260365DEST_PATH_IMAGE111
和
Figure 645079DEST_PATH_IMAGE112
是沿经线和纬线的曲率半径；
Figure 458314DEST_PATH_IMAGE113
，
Figure 159554DEST_PATH_IMAGE114
和
Figure 665622DEST_PATH_IMAGE115
为前进距离向量
Figure 334369DEST_PATH_IMAGE116
的 分量，表示n系下
Figure 888978DEST_PATH_IMAGE117
到
Figure 710304DEST_PATH_IMAGE118
时刻的位置增量；n系为当地水平坐标系，坐标原点为车辆运动起 点处所在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下量。
Figure 373891DEST_PATH_IMAGE119
时刻n系下的位置增量可由同一时刻v系下的位置增量
Figure 546246DEST_PATH_IMAGE120
通过下式转换得 到：
Figure 701284DEST_PATH_IMAGE121
(7)
式中：
Figure 829646DEST_PATH_IMAGE093
表示b系（IMU坐标系，其轴向与IMU轴向一致，坐标原点为陀螺仪和加速 度计的坐标原点）到n系（N-E-D坐标系，即当地水平坐标系，坐标原点为车辆运动起点处所 在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下）的方向余弦矩阵；
Figure 411937DEST_PATH_IMAGE122
表示v系（车体坐标系，其坐标原点为车后轴中心点，x轴与车辆前进方向一致，z轴垂直 向下，y轴朝外垂直于xz所在平面）到b系的方向余弦矩阵，由IMU的安装角决定；
Figure 71589DEST_PATH_IMAGE123
为v 系下的位置增量。
Figure 764738DEST_PATH_IMAGE123
可以由v系下速度的积分计算：
Figure 809923DEST_PATH_IMAGE124
(8)
式中：
Figure 297536DEST_PATH_IMAGE125
是车辆在v系（车体坐标系，其其坐标原点为车后轴中心点，x轴与车辆前 进方向一致，z轴垂直向下，y轴朝外垂直于xz所在平面）下的速度，根据v系的性质，该向量 除了第一个元素以外其余元素均为零，即
Figure 382167DEST_PATH_IMAGE126
，该值可由GNSS定位信息或者里程计 提供，本实施例采用GNSS定位信息。
步骤1.3，重复执行步骤1.1和步骤1.2得到车辆的DR轨迹。
重复执行步骤1.1和步骤1.2得到经过时间
Figure 128275DEST_PATH_IMAGE042
后的车辆位置
Figure 778699DEST_PATH_IMAGE043
，将
Figure 968372DEST_PATH_IMAGE127
作 为轨迹终点，初始位置作为轨迹起点，将这两个坐标转换到地心地固（Earth-Centered, Earth- Fixed，ECEF）坐标系（原点为地球原点，x轴指向格林威治子午面的交线，z轴指向北极，y轴在 赤道平面内与x轴和z轴满足右手系法则）下，得到起点坐标
Figure 71457DEST_PATH_IMAGE128
和 终点坐标
Figure 352747DEST_PATH_IMAGE129
，即为车辆的DR轨迹。
步骤2，利用单星载波航向计算通道模块，根据单颗卫星的载波观测值获取真实运动轨迹方向，结合DR轨迹方向得到初始航向。
步骤2.1，利用DR轨迹的TDCP计算模块得到DR轨迹的TDCP。
DR轨迹的TDCP计算模块的输入为卫星星历和DR轨迹起点和终点坐标，输出为DR轨迹的TDCP，该模块的实现方式如下：
Figure 326520DEST_PATH_IMAGE130
(9)
式中：
Figure 421515DEST_PATH_IMAGE131
为DR轨迹的TDCP，
Figure 261164DEST_PATH_IMAGE049
为GNSS载波波长，t为DR轨迹的时间跨度，
Figure 99807DEST_PATH_IMAGE132
和
Figure 990402DEST_PATH_IMAGE051
分别表示航位推算轨迹起点和终点到对应时刻卫星i的视线距离，视线距离的计算公 式如下：
Figure 708828DEST_PATH_IMAGE052
(10)
Figure 786506DEST_PATH_IMAGE053
(11)
式中：t为DR轨迹的时间跨度，
Figure 163260DEST_PATH_IMAGE133
为DR轨迹的起点坐 标，来源于DR模块输出；
Figure 908363DEST_PATH_IMAGE134
为DR轨迹的终点坐标，来源于DR模 块输出；
Figure 594428DEST_PATH_IMAGE135
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历计 算该坐标值；
Figure 159401DEST_PATH_IMAGE057
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历 计算该坐标值；上述坐标的坐标系均为地心地固坐标系。
步骤2.2，利用观测TDCP计算模块得到观测TDCP。
观测TDCP计算模块的输入为GNSS接收机在0时刻和t时刻的载波相位观测值
Figure 339847DEST_PATH_IMAGE136
和
Figure 129336DEST_PATH_IMAGE137
，输出为观测TDCP，该模块的实现方式为：
Figure 737035DEST_PATH_IMAGE138
(12)
式中：
Figure 54884DEST_PATH_IMAGE061
为观测TDCP，
Figure 39020DEST_PATH_IMAGE136
和
Figure 476824DEST_PATH_IMAGE137
为GNSS接收机在0时刻和t时刻的载波相位观 测值。
步骤2.3，根据步骤2.1得到的DR轨迹的TDCP和2.2得到的观测TDCP，利用TDCP增量计算模块得到TDCP增量。
TDCP增量计算模块的输出为：
Figure 458686DEST_PATH_IMAGE139
(13)
式中：
Figure 998252DEST_PATH_IMAGE140
为TDCP增量，即TDCP增量计算模块的输出；
Figure 769768DEST_PATH_IMAGE141
为观测TDCP，
Figure 343968DEST_PATH_IMAGE142
为DR轨迹的TDCP。
步骤2.4，根据步骤1得到的DR轨迹，利用DR轨迹方位角计算模块得到车辆的DR轨迹方位角。
DR轨迹方位角计算模块的输入为DR轨迹起点和终点坐标，输出为DR轨迹方位角
Figure 293470DEST_PATH_IMAGE072
，该模块的实现过程为：
Figure 320332DEST_PATH_IMAGE143
(14)
式中：
Figure 833221DEST_PATH_IMAGE144
和
Figure 261929DEST_PATH_IMAGE071
分别为DR轨迹的东向和北向的位置增量，将DR轨迹起点与 终点间的位置增量转换到n系（N-E-D坐标系，即当地水平坐标系，坐标原点为车辆运动起点 处所在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下）可得该分量。
步骤2.5，根据步骤1得到的DR轨迹、步骤2.3得到的TDCP增量和步骤2.4得到的DR轨迹方位角，利用真实轨迹的方位角计算模块得到车辆的DR真实轨迹方位角。
真实轨迹的方位角计算模块实现原理如图7所示，其输入为卫星星历、DR轨迹起点 和终点坐标、TDCP增量与DR轨迹方位角
Figure 647911DEST_PATH_IMAGE082
，输出为真实轨迹方位角
Figure 166265DEST_PATH_IMAGE073
，实现方式如下：
Figure 233578DEST_PATH_IMAGE145
(15)
式中：
Figure 516792DEST_PATH_IMAGE075
为卫星i的方位角，根据卫星星历计算；
Figure 322943DEST_PATH_IMAGE146
为卫星i的仰角，根据卫星星历 计算；
Figure 58817DEST_PATH_IMAGE147
为GNSS载波波长；
Figure 726559DEST_PATH_IMAGE148
为航位推算轨迹增量长度，根据DR轨迹起点和终点坐标计 算；
Figure 316809DEST_PATH_IMAGE082
为DR轨迹方位角，来源于DR轨迹方位角计算模块。
步骤2.6，由步骤2.4得到的DR轨迹方位角和步骤2.5得到的DR真实轨迹方位角相减得到初始航向。
轨迹匹配模块的输出为：
Figure 779015DEST_PATH_IMAGE079
(16)
Figure 2186DEST_PATH_IMAGE149
(17)
式中，
Figure 473618DEST_PATH_IMAGE150
为DR轨迹与真实运动轨迹之间的夹角；
Figure 715113DEST_PATH_IMAGE082
为DR轨迹方位角；
Figure 613798DEST_PATH_IMAGE073
为真实轨 迹方位角；
Figure 527528DEST_PATH_IMAGE083
为任意设置的初始航向，也是DR模块的输入，本实施例中为0度；
Figure 523690DEST_PATH_IMAGE084
为初始航 向。
步骤3，通过航向最优估计模块对步骤2得到的所有卫星通道输出的航向进行筛选，并求平均值得到精确且可靠的初始航向。
计算水平面上的卫星与真实运动轨迹之间的夹角，即卫星方位角
Figure 636003DEST_PATH_IMAGE151
与真实轨迹方 位角
Figure 705590DEST_PATH_IMAGE152
之差，选择角度在30度到150度之间的卫星，将筛选出的卫星对应的初始航向进行平 均，得到最终的初始航向。
实验测试验证
在开阔天空环境下进行车载实验对本发明提出的技术方案进行测试，使用的设备包括：NovAtel的测量型GNSS接收机OEM6、两种不同等级的IMU：STIM300和ICM20602、独立的参考系统POS-A15。数据采集时长约为50分钟。考虑到初始对准的效率和准确性，根据经验将初始对准时长设置为5秒，选取5秒的运动轨迹长度大于5米的数据用于对准实验。采集的数据能够支持1506次独立的航向对准实验。需要说明的是，尽管算法验证方式为对采集数据进行后处理，但本发明完全具备实时运行的条件，可以实时获得航向结果。
图8是使用STIM300的陀螺仪输出进行的1506次对准实验的航向误差结果。每次航向对准实验里最少使用了9颗卫星，最多使用了23颗卫星，即单星载波航向计算通道的数量最少为9个，最多为23个。该结果图表明本发明所提出的技术方案在5秒初始对准时长的条件下，获得的初始航向的误差均低于1.7度。图9绘制了该结果的航向误差的累积分布函数图，该图说明在5秒初始对准时长的条件下，本发明所提出的技术方案可以在95%置信水平下获得0.65度的航向初始化精度。对比现有的方案，本发明所提出的技术方案在准确性和效率方面都具有明显的优势。
为了验证算法的一致性，图10画出了根据不同卫星的载波相位观测值计算得到的初始航向误差的累积分布函数图，即不同的单星载波航向计算通道输出的累积分布图。图10中展示了22颗卫星的计算结果，这些结果均为运动轨迹与卫星方位角之间的角度在30度到150度时的航向对准样本计算结果。由图10可以看出，本发明所提出的技术方案在不同卫星间具有良好的一致性，并且单颗卫星获得的初始航向精度范围为0.7度到1.47度，置信度为95%。其中，18颗卫星在置信水平95%的条件下航向精度低于1.0度。该结果说明了本发明提供的技术方案具有较好的一致性，并且即使使用单颗卫星的观测值，也可以获得较高的初始化精度。
为了验证该技术方案在使用不同IMU时的效果，图11画出了使用 ICM20602的输出获得的初始航向误差。ICM20602是一个低成本MEMS IMU，该图展示了所有可观测卫星的平均值的误差。由图11可知，本发明所提出的技术方案在对准时长为5秒时，航向误差均在2度以内，大多数初始航向对齐误差都在1度以内。该结果表明，本发明所提出的技术方案同样适用于低成本MEMS IMU。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (10)

1.一种基于GNSS载波相位的车载航向快速初始化装置，其特征在于，包括DR模块、单星载波航向计算通道和航向最优估计模块；
DR模块包含姿态更新模块和位置更新模块，姿态更新模块利用陀螺输出递推下一时刻姿态，提供给位置更新模块计算下一时刻的位置；DR模块的输入为初始姿态、初始位置、陀螺输出和前进距离，输出为DR轨迹起点和终点坐标；初始姿态里的俯仰和横滚通过加速度计标定的方式得到，初始航向为任意设置值；初始位置由GNSS单点定位提供；陀螺输出来源于IMU测量值；前进距离由GNSS或里程计提供；DR模块的作用是利用陀螺输出和车辆前进距离推出DR轨迹，由于DR的初始航向为任意值，获得的DR轨迹与真实轨迹形状相似但存在旋转，该旋转角度即为DR设置的初始航向与真实初始航向的偏差；
单星载波航向计算通道包括TDCP增量计算模块和轨迹匹配模块，TDCP增量计算模块输出观测TDCP与DR轨迹TDCP之间的增量，并提供给轨迹匹配模块获得真实运动轨迹方向和初始航向；每个通道的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，输出为初始航向和真实轨迹方位；单星载波航向计算通道的作用是根据单颗卫星的载波观测值计算初始航向，与DR轨迹方向相减得到轨迹间旋转角度，也即DR的设置初始航向偏差，每个通道分别计算基于一颗卫星载波相位的初始航向，通道数与有效载波相位观测的数量一致；
TDCP增量计算模块用于计算观测TDCP与DR轨迹TDCP之间的增量，该模块的输入为卫星星历、DR轨迹起点和终点坐标、单颗卫星的载波相位观测值，输出为TDCP增量，所得结果用于轨迹匹配模块；TDCP增量计算模块由DR轨迹的TDCP计算模块、观测TDCP计算模块构成，两个模块分别计算对应的TDCP并输出，输出量相减即为TDCP增量；
轨迹匹配模块利用DR轨迹起点和终点坐标计算DR轨迹方位，再使用TDCP增量推算真实运动轨迹方位，从而获得DR轨迹相对于真实轨迹的旋转角度，也即DR设置的初始航向偏差；轨迹匹配模块的输入为DR轨迹起点和终点坐标、卫星星历和TDCP增量，输出为初始航向和真实轨迹方位角；该模块包括DR轨迹方位角计算模块和真实轨迹方位角计算模块，DR轨迹方位角计算模块的输出是真实轨迹方位角计算模块的输入，并与真实轨迹方位角模块的输出相减以获得初始航向；
航向最优估计模块的作用为对所有单星载波航向计算通道输出的航向进行筛选，去掉可能存在粗差的航向计算值，并将筛选后的航向进行平均，以提高计算的初始航向精度；该模块的输入为所有单星载波航向计算通道的输出和卫星星历，输入的初始航向和真实轨迹方位角的数量与单星载波航向计算通道数量一致，输出为初始航向结果。
2.一种利用权利要求1所述基于GNSS载波相位的车载航向快速初始化装置实现的基于GNSS载波相位的车载航向快速初始化方法，其特征在于，包括以下几个步骤：
步骤1，利用DR模块，根据惯性传感器陀螺输出和车辆前进距离推导出以任意设置的初始航向为初值的DR轨迹；
步骤1.1，利用DR模块中姿态更新模块得到车辆下一时刻的姿态；
步骤1.2，利用步骤1.1得到的车辆姿态，使用DR模块中位置更新模块得到车辆下一时刻的位置；
步骤1.3，重复执行步骤1.1和步骤1.2得到车辆的DR轨迹；
步骤2，利用单星载波航向计算通道模块，根据单颗卫星的载波观测值获取真实运动轨迹方向，结合DR轨迹方向得到初始航向；
步骤2.1，利用DR轨迹的TDCP计算模块得到DR轨迹的TDCP；
步骤2.2，利用观测TDCP计算模块得到观测TDCP；
步骤2.3，根据步骤2.1得到的DR轨迹的TDCP和2.2得到的观测TDCP，利用TDCP增量计算模块得到TDCP增量；
步骤2.4，根据步骤1得到的DR轨迹，利用DR轨迹方位角计算模块得到车辆的DR轨迹方位角；
步骤2.5，根据步骤1得到的DR轨迹、步骤2.3得到的TDCP增量和步骤2.4得到的DR轨迹方位角，利用真实轨迹的方位角计算模块得到车辆的DR真实轨迹方位角；
步骤2.6，由步骤2.4得到的DR轨迹方位角和步骤2.5得到的DR真实轨迹方位角相减得到初始航向；
初始航向的计算方式为：
Figure 325146DEST_PATH_IMAGE001
(16)
Figure 976707DEST_PATH_IMAGE002
(17)
式中，
Figure 428548DEST_PATH_IMAGE003
为DR轨迹与真实运动轨迹之间的夹角，
Figure 541867DEST_PATH_IMAGE004
为DR轨迹方位角，
Figure 825080DEST_PATH_IMAGE005
为真实轨迹方 位角，
Figure 381964DEST_PATH_IMAGE006
为任意设置的初始航向，
Figure 117838DEST_PATH_IMAGE007
为初始航向；
步骤3，通过航向最优估计模块对步骤2得到的所有卫星通道输出的航向进行筛选，并求平均值得到精确且可靠的初始航向；
计算水平面上的卫星与真实运动轨迹之间的夹角，即卫星方位角与真实轨迹方位角之差，选择角度在N1度到N2度之间的卫星，将筛选出的卫星对应的初始航向进行平均，得到最终的初始航向。
3.如权利要求2所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤1.1中姿态更新模块的输入为惯性传感器的陀螺输出角增量
Figure 506619DEST_PATH_IMAGE008
和
Figure 644339DEST_PATH_IMAGE009
时刻的姿 态
Figure 559075DEST_PATH_IMAGE010
，输出为
Figure 47825DEST_PATH_IMAGE011
时刻的姿态
Figure 253678DEST_PATH_IMAGE012
，该模块的公式描述如下：
Figure 698435DEST_PATH_IMAGE013
(1)
Figure 597121DEST_PATH_IMAGE014
(2)
Figure 573167DEST_PATH_IMAGE015
(3)
式中：
Figure 317132DEST_PATH_IMAGE016
表示b系到n系的方向余弦矩阵，每一时刻的方向余弦矩阵基于上一时刻的方 向余弦矩阵递推得到，初始方向余弦矩阵由该模块的输入初始姿态转换而来，初始姿态中 俯仰和横滚易由加速度计确定，航向可为任意设置值，后续步骤通过计算任意设置的初始 航向偏差计算初始航向；b系为IMU坐标系，其轴向与IMU轴向一致，坐标原点为陀螺仪和加 速度计的坐标原点；n系为当地水平坐标系，坐标原点为车辆运动起点处所在的地球表面，x 轴和y轴在当地水平面内指向北向和东向，z轴垂直向下；
Figure 881974DEST_PATH_IMAGE017
是b系的等效旋转向量；
Figure 685982DEST_PATH_IMAGE018
表 示向量
Figure 883745DEST_PATH_IMAGE017
的长度；
Figure 677739DEST_PATH_IMAGE019
是沿着
Figure 644558DEST_PATH_IMAGE017
方向的单位旋转矢量；
Figure 150626DEST_PATH_IMAGE020
和
Figure 570106DEST_PATH_IMAGE021
表示陀螺输出角增量；
Figure 170721DEST_PATH_IMAGE022
是3×3的单位矩阵，
Figure 195308DEST_PATH_IMAGE023
表示反对称矩阵。
4.如权利要求3所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤1.2中位置更新模块的输入为
Figure 606698DEST_PATH_IMAGE024
时刻的位置
Figure 28321DEST_PATH_IMAGE025
、前进距离
Figure 917780DEST_PATH_IMAGE026
，输出为下一时刻的位置
Figure 62453DEST_PATH_IMAGE027
，具体实现过程表示为：
Figure 894012DEST_PATH_IMAGE028
(4)
Figure 288084DEST_PATH_IMAGE029
(5)
Figure 981234DEST_PATH_IMAGE030
(6)
式中：向量
Figure 777151DEST_PATH_IMAGE031
为
Figure 782541DEST_PATH_IMAGE032
时刻的纬度、经度和高度，在第一次更新时该 值为输入DR模块的初始位置；
Figure 867172DEST_PATH_IMAGE033
为下一时刻的纬度、经度和高度；
Figure 364012DEST_PATH_IMAGE034
和
Figure 263704DEST_PATH_IMAGE035
是 沿经线和纬线的曲率半径；
Figure 187797DEST_PATH_IMAGE036
，
Figure 556462DEST_PATH_IMAGE037
和
Figure 43944DEST_PATH_IMAGE038
为前进距离向量
Figure 814454DEST_PATH_IMAGE039
的分 量，表示n系下
Figure 909449DEST_PATH_IMAGE040
到
Figure 765409DEST_PATH_IMAGE041
时刻的位置增量；n系为当地水平坐标系，坐标原点为车辆运动起点 处所在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下；
Figure 56582DEST_PATH_IMAGE042
时刻n系下的位置增量可由同一时刻v系下的位置增量
Figure 416019DEST_PATH_IMAGE043
通过下式转换得到：
Figure 947495DEST_PATH_IMAGE044
(7)
式中：
Figure 271510DEST_PATH_IMAGE045
表示b系到n系的方向余弦矩阵；
Figure 117106DEST_PATH_IMAGE046
表示v系到b系的方向余弦矩阵，由IMU的安 装角决定；
Figure 862209DEST_PATH_IMAGE043
为v系下的位置增量；b系为IMU坐标系，其轴向与IMU轴向一致，坐标原点 为陀螺仪和加速度计的坐标原点；v系为车体坐标系，其坐标原点为车后轴中心点，x轴与车 辆前进方向一致，z轴垂直向下，y轴朝外垂直于xz所在平面；
Figure 548274DEST_PATH_IMAGE043
可以由v系下速度的积分计算：
Figure 113247DEST_PATH_IMAGE047
(8)
式中：
Figure 496955DEST_PATH_IMAGE048
是车辆在v系下的速度，根据v系的性质，该向量除了第一个元素以外其余元素 均为零，即
Figure 80252DEST_PATH_IMAGE049
，该值由GNSS定位信息或里程计提供。
5.如权利要求2所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤1.3中重复执行步骤1.1和步骤1.2得到经过时间t后的车辆位置
Figure 687951DEST_PATH_IMAGE050
，将
Figure 740221DEST_PATH_IMAGE051
作为轨迹终点，初始位置作为轨迹起点，将这两个坐标转换到地心地固坐标系下，得到起点 坐标
Figure 724357DEST_PATH_IMAGE052
和终点坐标
Figure 631002DEST_PATH_IMAGE053
，即为车 辆的DR轨迹；地心地固坐标系的原点为地球原点，x轴指向格林威治子午面的交线，z轴指向 北极，y轴在赤道平面内与x轴和z轴满足右手系法则。
6.如权利要求5所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在于：步骤2.1中DR轨迹的TDCP计算模块的输入为卫星星历和DR轨迹起点和终点坐标，输出为DR轨迹的TDCP，该模块的实现方式如下：
Figure 409602DEST_PATH_IMAGE054
(9)
式中：
Figure 214747DEST_PATH_IMAGE055
为DR轨迹的TDCP，
Figure 989193DEST_PATH_IMAGE056
为GNSS载波波长，t为DR轨迹的时间跨度，
Figure 297814DEST_PATH_IMAGE057
和
Figure 247316DEST_PATH_IMAGE058
分别表示航位推算轨迹起点和终点到对应时刻卫星i的视线距离，视线距离的计算公式如 下：
Figure 461128DEST_PATH_IMAGE059
(10)
Figure 787067DEST_PATH_IMAGE060
(11)
式中：t为DR轨迹的时间跨度，
Figure 950195DEST_PATH_IMAGE061
为DR轨迹的起点坐标， 来源于DR模块输出；
Figure 336177DEST_PATH_IMAGE062
为DR轨迹的终点坐标，来源于DR模块 输出；
Figure 99603DEST_PATH_IMAGE063
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历计算 该坐标值；
Figure 963654DEST_PATH_IMAGE064
为DR轨迹起点对应时刻的卫星坐标，根据卫星星历计 算该坐标值；上述坐标的坐标系均为地心地固坐标系。
7.如权利要求6所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤2.2中观测TDCP计算模块的输入为GNSS接收机在0时刻和t时刻的载波相位观测值
Figure 184551DEST_PATH_IMAGE065
和
Figure 256281DEST_PATH_IMAGE066
，输出为观测TDCP，该模块的实现方式为：
Figure 992156DEST_PATH_IMAGE067
(12)
式中：
Figure 659897DEST_PATH_IMAGE068
为观测TDCP，
Figure 532038DEST_PATH_IMAGE065
和
Figure 732861DEST_PATH_IMAGE066
为GNSS接收机在0时刻和t时刻的载波相位观测值。
8.如权利要求7所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在于：步骤2.3中TDCP增量计算模块的输出为：
Figure 956032DEST_PATH_IMAGE069
(13)
式中：
Figure 161885DEST_PATH_IMAGE070
为TDCP增量，即TDCP增量计算模块的输出；
Figure 668959DEST_PATH_IMAGE071
为观测TDCP，
Figure 770907DEST_PATH_IMAGE072
为DR 轨迹的TDCP。
9.如权利要求2所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤2.4中DR轨迹方位角计算模块的输入为DR轨迹起点和终点坐标，输出为DR轨迹方位 角
Figure 215795DEST_PATH_IMAGE004
，该模块的实现过程为：
Figure 474607DEST_PATH_IMAGE073
(14)
式中：
Figure 852498DEST_PATH_IMAGE074
和
Figure 656506DEST_PATH_IMAGE075
分别为DR轨迹的东向和北向的位置增量，将DR轨迹起点与终点间 的位置增量转换到n系可得该分量；n系为当地水平坐标系，坐标原点为车辆运动起点处所 在的地球表面，x轴和y轴在当地水平面内指向北向和东向，z轴垂直向下。
10.如权利要求9所述的一种基于GNSS载波相位的车载航向快速初始化方法，其特征在 于：步骤2.5中真实轨迹的方位角计算模块的输入为卫星星历、DR轨迹起点和终点坐标、 TDCP增量与DR轨迹方位角
Figure 306799DEST_PATH_IMAGE076
，输出为真实轨迹方位角
Figure 120034DEST_PATH_IMAGE005
，实现方式如下：
Figure 86853DEST_PATH_IMAGE077
(15)
式中：
Figure 61763DEST_PATH_IMAGE078
为卫星i的方位角，根据卫星星历计算；
Figure 202282DEST_PATH_IMAGE079
为卫星i的仰角，根据卫星星历计 算；
Figure 553628DEST_PATH_IMAGE080
为GNSS载波波长；
Figure 374954DEST_PATH_IMAGE081
为航位推算轨迹增量长度，根据DR轨迹起点和终点坐标计 算；
Figure 35611DEST_PATH_IMAGE082
为DR轨迹方位角，来源于DR轨迹方位角计算模块。
CN202210931252.0A 2022-08-04 2022-08-04 基于gnss载波相位的车载航向快速初始化装置及方法 Active CN114994732B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210931252.0A CN114994732B (zh) 2022-08-04 2022-08-04 基于gnss载波相位的车载航向快速初始化装置及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210931252.0A CN114994732B (zh) 2022-08-04 2022-08-04 基于gnss载波相位的车载航向快速初始化装置及方法

Publications (2)

Publication Number Publication Date
CN114994732A true CN114994732A (zh) 2022-09-02
CN114994732B CN114994732B (zh) 2022-11-01

Family

ID=83022874

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210931252.0A Active CN114994732B (zh) 2022-08-04 2022-08-04 基于gnss载波相位的车载航向快速初始化装置及方法

Country Status (1)

Country Link
CN (1) CN114994732B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115856946A (zh) * 2023-02-21 2023-03-28 和普威视光电股份有限公司 一种飞行器对准航道检测方法、装置、终端及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080269988A1 (en) * 2003-03-20 2008-10-30 Feller Walter J Combined gnss gyroscope control system and method
EP2624013A1 (en) * 2012-01-31 2013-08-07 Amconav GmbH Method for Determining the Heading of a Moving Object
CN105241456A (zh) * 2015-11-06 2016-01-13 金陵科技学院 巡飞弹高精度组合导航方法
CN111089587A (zh) * 2019-12-27 2020-05-01 武汉大学 一种倾斜rtk航向初始化方法
CN112902950A (zh) * 2021-01-21 2021-06-04 武汉大学 一种面向低速运动载体中mems级imu的新型初始对准方法
CN113551669A (zh) * 2021-07-23 2021-10-26 山东泉清通信有限责任公司 基于短基线的组合导航定位方法及装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080269988A1 (en) * 2003-03-20 2008-10-30 Feller Walter J Combined gnss gyroscope control system and method
EP2624013A1 (en) * 2012-01-31 2013-08-07 Amconav GmbH Method for Determining the Heading of a Moving Object
CN105241456A (zh) * 2015-11-06 2016-01-13 金陵科技学院 巡飞弹高精度组合导航方法
CN111089587A (zh) * 2019-12-27 2020-05-01 武汉大学 一种倾斜rtk航向初始化方法
CN112902950A (zh) * 2021-01-21 2021-06-04 武汉大学 一种面向低速运动载体中mems级imu的新型初始对准方法
CN113551669A (zh) * 2021-07-23 2021-10-26 山东泉清通信有限责任公司 基于短基线的组合导航定位方法及装置

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
QIJIN CHEN 等: "Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS/INS Integrated System", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》 *
XIANLU TAO 等: "Precise Displacement Estimation From Time-Differenced Carrier Phase to Improve PDR Performance", 《IEEE SENSORS JOURNAL》 *
张旭 等: "高精度GNSS辅助下MEMS惯导的初始对准方法", 《淮阴工学院学报》 *
汤勇刚: "载波相位时间差分/捷联惯导组合导航方法研究", 《中国博士学位论文全文数据库》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115856946A (zh) * 2023-02-21 2023-03-28 和普威视光电股份有限公司 一种飞行器对准航道检测方法、装置、终端及存储介质

Also Published As

Publication number Publication date
CN114994732B (zh) 2022-11-01

Similar Documents

Publication Publication Date Title
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
US8374783B2 (en) Systems and methods for improved position determination of vehicles
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN102169184B (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
Georgy et al. Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN113253325B (zh) 惯性卫星序贯紧组合李群滤波方法
CN108931791A (zh) 卫惯紧组合钟差修正系统和方法
Park et al. MEMS 3D DR/GPS integrated system for land vehicle application robust to GPS outages
CN112562077A (zh) 一种融合pdr和先验地图的行人室内定位方法
Yi et al. Tightly-coupled GPS/INS integration using unscented Kalman filter and particle filter
CN105910623B (zh) 利用磁强计辅助gnss/mins紧组合系统进行航向校正的方法
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN114994732B (zh) 基于gnss载波相位的车载航向快速初始化装置及方法
Konrad et al. State estimation for a multirotor using tight-coupling of gnss and inertial navigation
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN108151765A (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Huang et al. Research on UAV flight performance test method based on dual antenna GPS/INS integrated system
CN117053802A (zh) 一种基于旋转mems imu的车载导航系统定位误差减小的方法
CN114111792B (zh) 一种车载gnss/ins/里程计组合导航方法
Erfianti et al. GNSS/IMU Sensor Fusion Performance Comparison of a Car Localization in Urban Environment Using Extended Kalman Filter
Aboutaleb et al. Examining the Benefits of LiDAR Odometry Integrated with GNSS and INS in Urban Areas
Moussa et al. Investigation of different low-cost land vehicle navigation systems based on CPD sensors and vehicle information

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