CN114877886A - 一种车载组合导航设备姿态自适应估计方法 - Google Patents

一种车载组合导航设备姿态自适应估计方法 Download PDF

Info

Publication number
CN114877886A
CN114877886A CN202210536660.6A CN202210536660A CN114877886A CN 114877886 A CN114877886 A CN 114877886A CN 202210536660 A CN202210536660 A CN 202210536660A CN 114877886 A CN114877886 A CN 114877886A
Authority
CN
China
Prior art keywords
vehicle
navigation
state
vehicle body
angle
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.)
Pending
Application number
CN202210536660.6A
Other languages
English (en)
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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN202210536660.6A priority Critical patent/CN114877886A/zh
Publication of CN114877886A publication Critical patent/CN114877886A/zh
Pending legal-status Critical Current

Links

Images

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/20—Instruments for performing navigational calculations
    • 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
    • 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/52—Determining velocity
    • 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

Landscapes

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

Abstract

本发明公开了一种车载组合导航设备姿态自适应估计方法，应用于车载组合导航系统，包括惯性传感器IMU和GNSS模块，所述方法包括：S1，传感器数据采集和校正；S2，计算导航设备姿态角的俯仰角和横滚角；S3，设置导航系统状态并初始化；S4，组合导航算法初始化；S5，对当前导航系统状态进行惯导推算；S6，车辆状态识别；S7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵；S8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正；S9，评定导航设备安装姿态的估计结果。

Description

一种车载组合导航设备姿态自适应估计方法
技术领域
本发明属于多传感器信息融合领域，具体来说涉及一种车载组合导航设备姿态自适应估计方法。
背景技术
车载组合导航设备(车载组合导航系统)在正常工作之前，需要良好的初始状态对准。由于车载组合导航设备在车体上的安装方式不确定，导航设备载体坐标系和车身坐标系之间的相对关系需要一种自适应方法进行估计。
目前国内外主要商用车辆的导航方式为GNSS与数字地图相结合的方式。这种方式的其中一缺点是车辆为平面上的导航、没有姿态角的输出；这对于只是定位需求而言足够，但是对于车辆进行外部环境检测时不能给出其姿态参考。
国内外已经有不少组合导航方面的研究，并在车载组合导航设备中得到实际应用。其中，惯性导航系统(INS)通常以来GNSS给出的位置信息进行初始化，INS也可利用GNSS的位置和速度在行进过程中完成导航姿态对准，或者利用GNSS双天线定向技术完成导航设备姿态组对准。但上述的这些姿态估计方法存在精度低、过程复杂等缺点。另外，还有一些技术方案中提到利用惯性传感器和全球卫星导航系统输出的位置差值，采用卡尔曼滤波技术估计出导航设备相对于车体的姿态角，或者利用地磁传感器和惯性传感器估计出导航设备相对于车体的姿态角；但是这些方案面对复杂环境，还存在收敛速度较慢、估计精度不理想及环境适应性不强等问题。
发明内容
本发明针对车载组合导航设备安装方式不确定而无法初始化的问题，提供了一种车载组合导航设备姿态自适应估计方法。
为实现上述目的，本发明提供技术方案如下：
一种车载组合导航设备姿态自适应估计方法，应用于车载组合导航系统，包括惯性传感器IMU和GNSS模块，所述方法包括：
S1，传感器数据采集和校正；
S2，计算导航设备姿态角的俯仰角和横滚角；
S3，设置导航系统状态并初始化；
S4，组合导航算法初始化；
S5，对当前导航系统状态进行惯导推算；
S6，车辆状态识别；
S7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵；
S8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正；
S9，评定导航设备安装姿态的估计结果。
优选地，所述S1包括以下步骤：
S1.1，按照一定采样频率分别采集惯性传感器IMU的惯性传感器信息、CAN总线的车速度信息及GNSS模块的GNSS输出信息，并标记采样时间戳，惯性传感器IMU包括三轴加速度计和三轴角速度计，惯性传感器信息包括三轴加速度数据和三轴角速度数据；
S1.2，采用二阶巴特沃斯低通滤波器对三轴加速度数据进行滤波，采用滑动窗口对三轴角速度数据进行滤波。
优选地，所述S2包括以下步骤：
在平直路面且车体静止状态t0下，计算导航设备姿态角的俯仰角θp(t0)和横滚角θr(t0)；俯仰角计算公式为：
Figure BDA0003646631860000031
横滚角计算公式为：
Figure BDA0003646631860000032
其中，x、y、z分别为三轴加速度x轴、y轴、z轴的加速度值。
优选地，所述S3包括以下步骤：
S3.1，设置导航设备相对车体的姿态角
Figure BDA0003646631860000033
Figure BDA0003646631860000034
为导航设备相对车体的俯仰角，
Figure BDA0003646631860000035
Figure BDA0003646631860000036
为导航设备相对车体的横滚角，
Figure BDA0003646631860000037
Figure BDA0003646631860000038
为导航设备相对车体的航向角，初值为0；
S3.2，设置导航系统的姿态角
Figure BDA0003646631860000039
Figure BDA00036466318600000310
为导航系统的俯仰角，初值为步骤S2所得的俯仰角θp(t0)，
Figure BDA00036466318600000311
为导航系统的横滚角，初值为步骤S2所得的俯仰角θr(t0)，
Figure BDA00036466318600000312
为导航系统的航向角，
Figure BDA00036466318600000313
其中，θgnss为在车速大于Vh时，基于GNSS模块获取的车体航向角；
S3.3，由GNSS模块获取的全局位置和三维速度信息分别初始化导航系统的速度和位置。
优选地，所述S4包括以下步骤：
设置导航坐标系到载体坐标系的导航变换矩阵，并基于导航系统姿态角的初值初始化导航变换矩阵；
设置车体坐标系到载体坐标系的车体变换矩阵，并基于导航设备相对车体姿态角的初值初始化车体变换矩阵；
基于导航变换矩阵获取对应的四元素初值；
设定卡尔曼滤波器的状态向量，包括姿态误差、速度误差、位置误差、角速度计动态零漂、加速度计动态零漂；
设定卡尔曼滤波器的状态转移矩阵。
优选地，基于导航变换矩阵、车体变换矩阵及四元素初值，采用捷联惯导更新方程进行惯导解算，更新导航系统状态。
优选地，所述车辆状态识别包括：
车身速度小于vstop、加速度值方差小于阈值且持续时间大于tstop时，则为停止状态；
角速度计Z轴角速度值投影到车体坐标系后的绝对值小于wbstraight且车身速度大于vstraight时，车辆处于高速直行状态；
角速度计Z轴角速度值投影到车体坐标系后的绝对值大于wbturn时，车辆处于转弯状态；
否则为其他状态；
其中，vstop为预设的车辆停止时的速度阈值，tstop为预设的车辆停止时的持续时间阈值，wbstraight为预设的车辆直行时的角速度阈值，vstraight为预设的车辆直行时的速度阈值，wbturn为预设的车辆转弯时的角速度阈值。
优选地，所述S7包括：
当车辆处于静止状态，卡尔曼滤波器的观测矩阵H设置为观测三维速度误差，噪声矩阵R设置为常数，观测向量Z为车体在导航坐标系下的三维速度；
当车辆处于转弯状态或高速直行状态时，卡尔曼滤波器的观测矩阵H设置为观测三维速度和位置误差，噪声矩阵R由角速度计和卫星信噪比按预设的自适应计算公式进行自适应调整，观测向量Z为车身在导航坐标系下的三维速度误差和位置误差。
优选地，所述自适应计算公式为R＝N1×P900/CN0-0.7+N2×Qabs(wbz)*10，其中，N1、N2、Q和P为常数，CNO为GNSS信噪比，wbz为车体转向角速度值。
优选地，所述步骤S9包括以下步骤：
S9.1，基寸
Figure BDA0003646631860000051
获取导航设备相对车体的当前航向角；
S9.2，当检测到导航设备相对车体的当前航向角收敛到方差小于一定阈值时认为估计完毕，否则返回步骤5。
与现有技术相比，本发明的有益效果为：
本发明利用低成本惯性传感器和全球导航卫星系统并辅助车身速度信息估计导航设备相对于车体的姿态角，内容涉及惯性导航、卡尔曼滤波和车体模型约束等，对导航定位设备安装方式自适应性强且有更好的精度；同时解决了以往人工标定效率低且过程繁琐问题。
附图说明
图1为本发明流程图。
具体实施方式
下面将结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
本发明是基于瑞芯微RV1126平台和恩智浦32位单片机S32K144进行开发设计的。考虑单片机实时性相对较好，整个系统中传感器的数据获取和解析均在S32K144上完成，采用FreeRTOS实时操作系统进行任务调度，很好的保证了传感器之间的时间对齐。系统的算法部分涉及到大量的矩阵计算，因此在算法实现过程中移植了EIGEN库，算法部分运行在基于LINUX操作系统的RV1126上。
参照图1所示，一种车载组合导航设备姿态自适应估计方法，应用于车载组合导航系统，包括惯性传感器IMU和GNSS模块，惯性传感器IMU为惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器，该惯性传感器IMU包括三轴加速度计和三轴角速度计；该车载组合导航设备姿态自适应估计方法包括9个步骤。
步骤1，传感器数据采集和校正，具体为：
步骤1.1，以100HZ、10HZ、2HZ采样频率分别采集惯性传感器IMU的惯性传感器信息、CAN总线的车速度信息及GNSS模块的GNSS输出信息，并设置系统时间戳；惯性传感器信息包括三轴加速度数据和三轴角速度数据；
步骤1.2，采用二阶巴特沃斯低通滤波器对三轴加速度数据进行滤波，采用滑动窗口对三轴角速度数据进行滤波。
本发明步骤1.2中，考虑车体发动机震动对三轴加速度计影响较大，因此采用二阶巴特沃斯低通滤波器对加速度计测量值进行滤波处理，低通滤波器使用Matlab工具箱中的Filter Designer进行参数设计，设置低通滤波器的采样频率Fs为IMU采样频率Fs＝100HZ，将低通滤波器的截止频率Fc设置为小于车体震动频率，这里车体振动频率为30HZ，则Fc＝20HZ，然后自动生成滤波器差分方程的参数表。通过滑动窗口滤波，使角速度计输出值更平滑，滑动窗口大小根据采样频率进行选择，本实施例中窗口大小设置为50。最后将加速度计和角速度计分别减去各自的零偏，其中加速度计零偏采用六面法实验得到，角速度计零偏由静置后的均值计算得到，这里的“静止后的均值”为IMU静止时，一段时间内角速度计输出值的平均值。
步骤2，计算导航设备姿态角的俯仰角和横滚角，具体为：
在平直路面且车体静止状态t0下，通过式(1)和式(2)分别计算导航设备姿态角的俯仰角θp(t0)和横滚角θr(t0)；
Figure BDA0003646631860000071
Figure BDA0003646631860000072
其中，x、y、z分别为三轴加速度x轴、y轴、z轴的加速度值。
姿态角是指机体坐标系与地面惯性坐标系之间的夹角，可用横滚角、俯仰角、偏航角三个角表示。
步骤3，设置导航系统状态并初始化；
S3.1，设置导航设备相对车体的姿态角
Figure BDA0003646631860000073
Figure BDA0003646631860000074
为导航设备相对车体的俯仰角，
Figure BDA0003646631860000081
Figure BDA0003646631860000082
为导航设备相对车体的横滚角，
Figure BDA0003646631860000083
Figure BDA0003646631860000084
为导航设备相对车体的航向角，初值为0；
步骤3.2，将步骤2所得的导航设备姿态角的俯仰角θp(t0)和横滚角θr(t0)作为导航系统姿态角θnb的俯仰角
Figure BDA0003646631860000085
和横滚角
Figure BDA0003646631860000086
的初始值；由CAN总线信息解算车速度，在车速大于预设的速度阈值Vh时，基于GNSS模块获取车体系x轴(这里设置车辆正前方为车体系的x轴，车辆正右边为y轴，车辆正下方为z轴)在导航坐标系下的航向角，也即车体航向角θgnss，顺时针为正；通过式(4)计算得到导航系统姿态角的航向角
Figure BDA0003646631860000087
Figure BDA0003646631860000088
则载体系相对导航坐标系的姿态角θnb，也即导航系统姿态角信息可表示为：
Figure BDA0003646631860000089
步骤3.3，由GNSS模块获取的全局位置和三维速度信息分别初始化导航系统的速度和位置。
步骤4，组合导航算法初始化，具体包括：
设置导航坐标系到载体坐标系的导航变换矩阵
Figure BDA00036466318600000810
并步骤3.2所得的基于导航系统姿态角的初值[θp(t0) θr(t0) θgnss]初始化导航变换矩阵；
设置车体坐标系到载体坐标系的车体变换矩阵
Figure BDA00036466318600000811
基于步骤3所得的导航设备相对车体的姿态角θvb的初值[θp(t0) θr(t0) 0]初始化车体变换矩阵
Figure BDA00036466318600000812
基于导航变换矩阵
Figure BDA0003646631860000091
获取对应的四元素初值q0；
设定卡尔曼滤波器的状态向量Xp，表示为：
Xp＝[δθn δVn δPn δbw δba]T (6)
其中δθn为姿态误差，δVn为速度误差，δPn为位置误差，δbw为角速度计动态零漂，δba加速度计动态零漂；
设定卡尔曼滤波器的状态转移矩阵F，状态转移矩阵F由惯性导航算法的姿态误差方程、速度误差方程、位置误差方程和IMU动态零偏模型确定；
姿态误差方程表示为：
Figure BDA0003646631860000092
式中，
Figure BDA0003646631860000093
为角度数计测量误差，
Figure BDA0003646631860000094
为导航系计算误差，&表示微分。
速度误差方程表示为：
Figure BDA0003646631860000095
式中，
Figure BDA0003646631860000096
为加速度计测量误差，
Figure BDA0003646631860000097
δgn分别为地球自转角速度计算误差、导航系旋转计算误差和重力误差。
位置误差方程表示为：
Figure BDA0003646631860000098
Figure BDA0003646631860000099
δh&＝δvU (11)
其中，δL、δλ、δh分别表示纬度误差、经度误差和高度误差，RM为子午曲率半径，RN为横向曲率半径，L纬度，h为海拔高度。
IMU动态零偏模型的状态转移矩阵表示为：
Figure BDA0003646631860000101
其中，Faa、Fgg表示加速度计和角速度计动态零偏状态转移矩阵，α、β为常数，I为三维单位矩阵。本实施中，α＝150，β＝1。
这里，姿态误差方程、速度误差方程、位置误差方程和IMU动态零偏模型为本领域的公知常识，本领域技术人员可根据实际情况自行设置。
步骤5，对当前导航系统状态进行惯导推算，具体为：
由姿态更新方程、速度更新方程和位置更新方程进行惯导推算，更新系统的姿态、速度和位置。其中，姿态更新方程可获得载体在导航坐标系下更新后的航向
Figure BDA0003646631860000102
GNSS模块可获取车辆在导航系下的航向θgntss，则更新后导航设备相对车体姿态角的航向角
Figure BDA0003646631860000103
计算公式为：
Figure BDA0003646631860000104
这里，姿态更新方程、速度更新方程和位置更新方程为本领域的公知常识，在此不做赘述。
步骤6，车辆状态识别，由车体速度和IMU测量值将车辆运行过程中的状态划分为停止状态、高速直行状态、转弯状态及其他状态，各状态的判定状态具体为：
车身速度小于vstop、加速度值方差小于阈值且持续时间大于tstop时，则为停止状态；vstop为预设的车辆停止时的速度阈值，tstop为预设的车辆停止时的持续时间阈值，本实施例中，
Figure BDA0003646631860000111
tstop＝0.8s；
角速度计Z轴角速度值投影到车体坐标系后的绝对值小于wbstraight且车身速度大于vstraight时，车辆处于高速直行状态，wbstraight为预设的车辆直行时的角速度阈值，vstraight为预设的车辆直行时的速度阈值，本实施例中，
Figure BDA0003646631860000112
角速度计Z轴角速度值投影到车体坐标系后的绝对值大于wbturn时，车辆处于转弯状态，wbturn为预设的车辆转弯时的角速度阈值；本实施例中，
Figure BDA0003646631860000113
否则为其他状态。
这里需要注意的是，其他状态时不进行卡尔曼观测融合，只挑出特征明显的状态也即静止状态、转弯状态或高速直行状态来融合。
步骤7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵。
本发明中，基于不同的车辆状态先选择不同的观测矩阵H，基于观测方程V＝Z-H*Xp，从而获得不同的卡尔曼观测新息(新的信息)。
该基于车辆状态选择卡尔曼滤波器的误差观测矩阵为：
(1)当车辆处于静止状态，卡尔曼滤波器的观测矩阵H设置为三维速度误差观测，即观测矩阵H对应三维速度元素设置为1，噪声矩阵R为0.0625，由GNSS模块得到的观测向量Z为车体在导航坐标系下的三维速度，在静止时，GNSS模块观测的三维速度就是测量误差。
(2)当车辆处于转弯状态或高速直行状态时，卡尔曼滤波器的观测矩阵H设置为三维速度误差和位置误差观测，三维速度观测矩阵设置为1，位置观测矩阵Tpr由下式计算得到：
Tpr＝diag([Rm+h，(Rn+h)cosL，-1]) (14)
式中，Rm为子午圈主曲率半径，Rn为卯酉圈主曲率半径，h、L分别为GNSS模块输出的海拔高度和纬度。
噪声矩阵R由角速度计和卫星信噪比按预设的自适应计算公式进行自适应调整：
R＝N1×P900/CN0-0.7+N2×Qabs(wbz)*10 (15)
式中，N1、N2、P和Q为常数，Q、P为信噪比、车体转向角速度值分别对此时观测噪声贡献程度的放大系数，CN0为GNSS信噪比，wbz为车体转向角速度值；本实施例中，N1＝0.2，N2＝0.1，P＝3.0，Q＝5.0；
GNSS模块得到的观测向量Z为车身在导航坐标系下的三维速度误差和位置误差，其中，三维速度误差设置为：
verror＝vnav-vgnss (16)
vnav是惯导解算的三维速度，vgnss是GNSS测量的三维速度。
三维位置误差设置为：
perror＝pnav-pgnss (17)
pnav是惯导解算的三维位置，pgnss是GNSS测量的三维位置。
步骤8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正。
本发明步骤8中，基于步骤4中的卡尔曼滤波器的状态转移矩阵和步骤7中卡尔曼滤波器的误差观测矩阵，进行卡尔曼滤波计算，获得惯性导航系统误差的最优估计，将步骤5中通过惯性导航算法更新的导航系统姿态、速度和位置减去卡尔曼滤波器输出的误差，得到修正后的导航系统姿态、速度和位置。
步骤9，评定导航设备安装姿态的估计结果，具体为：
S9.1，基寸
Figure BDA0003646631860000131
获取导航设备相对车体的当前航向角
Figure BDA0003646631860000132
S9.2，当检测到导航设备相对车体的当前航向角
Figure BDA0003646631860000133
收敛到方差小于一定阈值的时认为估计完毕，否则返回步骤5。
本发明步骤9.1中，由步骤8中修正后的姿态获得
Figure BDA0003646631860000134
在基于式(13)获得当前时刻导航设备相对车体的航向角；本发明步骤9.2中，不断迭代步骤5到步骤9过程，在多种信息观测约束下，导航设备相对车体姿态角θvb的航向角
Figure BDA0003646631860000135
快速收敛到真值，最后通过方差检验
Figure BDA0003646631860000136
当
Figure BDA0003646631860000137
的方差小于0.001阈值并持续50个计算周期，认为
Figure BDA0003646631860000138
收敛完成。
此时该收敛完成的航向角
Figure BDA0003646631860000139
为自适应估计得到的车载组合导航设备姿态角的航向角，步骤2计算得到的导航设备姿态角的俯仰角θp(t0)和横滚角θr(t0)为自适应估计得到的车载组合导航设备姿态角的俯仰角和横滚角。

Claims (10)

1.一种车载组合导航设备姿态自适应估计方法，其特征在于，应用于车载组合导航系统，包括惯性传感器IMU和GNSS模块，所述方法包括：
S1，传感器数据采集和校正；
S2，计算导航设备姿态角的俯仰角和横滚角；
S3，设置导航系统状态并初始化；
S4，组合导航算法初始化；
S5，对当前导航系统状态进行惯导推算；
S6，车辆状态识别；
S7，基于车辆状态选择卡尔曼滤波器的误差观测矩阵；
S8，由卡尔曼滤波器估计导航系统状态误差，并进行当前导航系统状态的误差修正；
S9，评定导航设备安装姿态的估计结果。
2.如权利要求1所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述S1包括以下步骤：
S1.1，按照一定采样频率分别采集惯性传感器IMU的惯性传感器信息、CAN总线的车速度信息及GNSS模块的GNSS输出信息，并标记采样时间戳，惯性传感器IMU包括三轴加速度计和三轴角速度计，惯性传感器信息包括三轴加速度数据和三轴角速度数据；
S1.2，采用二阶巴特沃斯低通滤波器对三轴加速度数据进行滤波，采用滑动窗口对三轴角速度数据进行滤波。
3.如权利要求1所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述S2包括以下步骤：
在平直路面且车体静止状态t0下，计算导航设备姿态角的俯仰角θp(t0)和横滚角θr(t0)；俯仰角计算公式为：
Figure FDA0003646631850000021
横滚角计算公式为：
Figure FDA00036466318500000213
其中，x、y、z分别为三轴加速度x轴、y轴、z轴的加速度值。
4.如权利要求3所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述S3包括以下步骤：
S3.1，设置导航设备相对车体的姿态角
Figure FDA0003646631850000022
Figure FDA0003646631850000023
为导航设备相对车体的俯仰角，
Figure FDA0003646631850000024
Figure FDA0003646631850000025
为导航设备相对车体的横滚角，
Figure FDA0003646631850000026
Figure FDA0003646631850000027
为导航设备相对车体的航向角，初值为0；
S3.2，设置导航系统的姿态角
Figure FDA0003646631850000028
Figure FDA0003646631850000029
为导航系统的俯仰角，初值为步骤S2所得的俯仰角θp(t0)，
Figure FDA00036466318500000210
为导航系统的横滚角，初值为步骤S2所得的俯仰角θr(t0)，
Figure FDA00036466318500000211
为导航系统的航向角，
Figure FDA00036466318500000212
其中，θgnss为在车速大于Vh时，基于GNSS模块获取的车体航向角；
S3.3，由GNSS模块获取的全局位置和三维速度信息分别初始化导航系统的速度和位置。
5.如权利要求4所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述S4包括以下步骤：
设置导航坐标系到载体坐标系的导航变换矩阵，并基于导航系统姿态角的初值初始化导航变换矩阵；
设置车体坐标系到载体坐标系的车体变换矩阵，并基于导航设备相对车体姿态角的初值初始化车体变换矩阵；
基于导航变换矩阵获取对应的四元素初值；
设定卡尔曼滤波器的状态向量，包括姿态误差、速度误差、位置误差、角速度计动态零漂、加速度计动态零漂；
设定卡尔曼滤波器的状态转移矩阵。
6.如权利要求4所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，基于导航变换矩阵、车体变换矩阵及四元素初值，采用捷联惯导更新方程进行惯导解算，更新导航系统状态。
7.如权利要求4所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述车辆状态识别包括：
车身速度小于vstop、加速度值方差小于阈值且持续时间大于tstop时，则为停止状态；
角速度计Z轴角速度值投影到车体坐标系后的绝对值小于wbstraight且车身速度大于vstraight时，车辆处于高速直行状态；
角速度计Z轴角速度值投影到车体坐标系后的绝对值大于wbturn时，车辆处于转弯状态；
否则为其他状态；
其中，vstop为预设的车辆停止时的速度阈值，tstop为预设的车辆停止时的持续时间阈值，wbstraight为预设的车辆直行时的角速度阈值，vstraight为预设的车辆直行时的速度阈值，wbturn为预设的车辆转弯时的角速度阈值。
8.如权利要求7所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述S7包括：
当车辆处于静止状态，卡尔曼滤波器的观测矩阵H设置为观测三维速度误差，噪声矩阵R设置为常数，观测向量Z为车体在导航坐标系下的三维速度；
当车辆处于转弯状态或高速直行状态时，卡尔曼滤波器的观测矩阵H设置为观测三维速度和位置误差，噪声矩阵R由角速度计和卫星信噪比按预设的自适应计算公式进行自适应调整，观测向量Z为车身在导航坐标系下的三维速度误差和位置误差。
9.如权利要求8所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述自适应计算公式为R＝N1×P900/CN0-0.7+N2×Qabs(wbz)*10，其中，N1、N2、Q和P为常数，CN0为GNSS信噪比，wbz为车体转向角速度值。
10.如权利要求1-9任一项所述的一种车载组合导航设备姿态自适应估计方法，其特征在于，所述步骤S9包括以下步骤：
S9.1，基于
Figure FDA0003646631850000041
获取导航设备相对车体的当前航向角；
S9.2，当检测到导航设备相对车体的当前航向角收敛到方差小于一定阈值时认为估计完毕，否则返回步骤5。
CN202210536660.6A 2022-05-17 2022-05-17 一种车载组合导航设备姿态自适应估计方法 Pending CN114877886A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210536660.6A CN114877886A (zh) 2022-05-17 2022-05-17 一种车载组合导航设备姿态自适应估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210536660.6A CN114877886A (zh) 2022-05-17 2022-05-17 一种车载组合导航设备姿态自适应估计方法

Publications (1)

Publication Number Publication Date
CN114877886A true CN114877886A (zh) 2022-08-09

Family

ID=82675002

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210536660.6A Pending CN114877886A (zh) 2022-05-17 2022-05-17 一种车载组合导航设备姿态自适应估计方法

Country Status (1)

Country Link
CN (1) CN114877886A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115790615A (zh) * 2023-01-30 2023-03-14 安徽蔚来智驾科技有限公司 基于自动驾驶的多传感器融合方法、装置、介质及车辆

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115790615A (zh) * 2023-01-30 2023-03-14 安徽蔚来智驾科技有限公司 基于自动驾驶的多传感器融合方法、装置、介质及车辆
CN115790615B (zh) * 2023-01-30 2023-05-05 安徽蔚来智驾科技有限公司 基于自动驾驶的多传感器融合方法、装置、介质及车辆

Similar Documents

Publication Publication Date Title
CN108731667B (zh) 用于确定无人驾驶车辆的速度和位姿的方法和装置
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
JP7073052B2 (ja) ビークルの角度位置を測定するシステムおよび方法
CN107289930B (zh) 基于mems惯性测量单元的纯惯性车辆导航方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN101290229A (zh) 硅微航姿系统惯性/地磁组合方法
JP7344895B2 (ja) 車両に装備されたジャイロメータの較正方法
CN101122637A (zh) 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111912427B (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
CN114877886A (zh) 一种车载组合导航设备姿态自适应估计方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统

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