CN115451949A - 一种基于车轮安装惯性测量单元的车辆定位方法 - Google Patents

一种基于车轮安装惯性测量单元的车辆定位方法 Download PDF

Info

Publication number
CN115451949A
CN115451949A CN202211006263.4A CN202211006263A CN115451949A CN 115451949 A CN115451949 A CN 115451949A CN 202211006263 A CN202211006263 A CN 202211006263A CN 115451949 A CN115451949 A CN 115451949A
Authority
CN
China
Prior art keywords
measurement unit
wheel
vehicle
inertial measurement
state
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
CN202211006263.4A
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.)
Nanjing University of Posts and Telecommunications
Original Assignee
Nanjing University of Posts and Telecommunications
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 Posts and Telecommunications filed Critical Nanjing University of Posts and Telecommunications
Priority to CN202211006263.4A priority Critical patent/CN115451949A/zh
Publication of CN115451949A publication Critical patent/CN115451949A/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
    • 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
    • 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
    • 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
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass

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)
  • Navigation (AREA)

Abstract

本发明公开了一种基于车轮安装惯性测量单元的车辆定位方法，将IMU安装在车辆车轮上，基于已知IMU安装参数，通过车辆、车轮运动学约束构建状态方程和观测方程，利用卡尔曼滤波器，将车轮转角、车轮旋转角速度和车身姿态作为状态量进行估计，进而获得车辆位姿与速度。同时提供了车轮IMU安装参数的估计方法，首先，在车轮‑IMU对准阶段，获得IMU相对于车轮的初步安装姿态；然后，将安装参数包含在卡尔曼滤波器状态量中进行估计，获得车轮IMU安装参数、车辆位姿、速度。本发明提高了IMU相对于车轮的安装参数估计精度，并进一步提高了基于车轮安装IMU的车辆定位精度。

Description

一种基于车轮安装惯性测量单元的车辆定位方法
技术领域
本发明属于惯性导航技术领域，具体涉及一种基于车轮安装惯性测量单元的车辆定位方法。
背景技术
针对车辆位姿的计算，常规的里程计往往要在车辆出厂前安装好，常规的旋转调制捷联惯导系统需要复杂精密的机电控制系统来有效抑制惯性器件的系统误差。而一个惯性测量单元(Initial Measurement Unit，IMU)仅仅通过安装在车轮上便可等效于里程计和单轴旋转调制捷联惯导系统的组合，大大提升了惯性导航定位精度，并减少成本。
微机电系统(Micro-Electro-Mechanical System，MEMS)IMU因其成本低、体积小、功耗低等优点而被广泛用于车辆定位，MEMS惯性导航面临的主要问题是定位误差随时间发散严重，针对该问题，文献[1]([1]Collin J.MEMS IMU Carouseling for GroundVehicles.《IEEE Transactions on Vehicular Technology》.2015年,第64卷，第6期)中，Collin通过将MEMS IMU安装在车轮进行车辆定位，通过三轴加速度计感知重力加速度获得车轮转动的圈数，实现“里程计”测量效果，利用车轮旋转调制作用抑制陀螺的零漂，并由陀螺感知车辆的姿态，最终达到定位的目的，在近1km的定位实验中最大误差不超过8m。文献[1]中的定位模型只在极为理想的情况下成立，文献[1]中，利用特制夹具，将MEMS IMU安装于车轮轴心，但IMU外壳中心不等同于加速度计敏感中心，安装时难以保证加速度计敏感中心恰好在车轮转轴上，存在偏心距，由此会因车轮旋转产生向心加速度和切向加速度并作用于加速度计，影响对车轮转速的准确估计。IMU相对于车轮的轴对准误差在安装时也是难以避免的，它将等效为IMU的系统误差影响车辆方位估计精度。尤其在车辆运动速度较快时，安装误差将使得基于车轮安装IMU的车辆定位模型误差突显，进一步降低车辆定位精度。利用特制夹具可以一定程度减少安装误差，但增加了安装难度和使用成本。
文献[2]([2]车轮安装惯性测量单元的安装参数现场标定方法，CN 114152269 A,2022.03.08)中提出车轮IMU安装参数标定方法和车辆定位方法，但主要存在如下问题：(1)所建立的运动学模型精度还可以提高，如忽略了科氏加速度项，直接默认车身俯仰角速度为0等；(2)所述方法侧重于进行惯性测量单元安装参数估计，因而所采用的卡尔曼滤波器中，状态量中仅为车轮旋转参数、惯性测量单元安装参数，不含有车身姿态数据，以及未用车身零水平角观测，所得车身姿态误差较大，从而定位误差较大。(3)所述方法主要是一种IMU相对于车轮的安装参数标定方法，对获得稳定的标定参数后的车辆定位未给出具体的方案。因此其所建立的基于车轮安装IMU的车辆定位模型精度仍有提升空间。
发明内容
本发明所要解决的技术问题是解决了IMU安装参数未知的问题，并且解决了基于车轮安装IMU的车辆定位模型由于IMU安装误差、车身和车轮运动，其定位的精度的问题，并给出已知IMU安装参数后的车辆定位方案。
为解决上述技术问题，本发明采用以下技术方案：
一种基于车轮安装惯性测量单元的车辆定位方法，基于安装在车辆车轮预设位置的惯性测量单元，针对惯性测量单元安装参数未知情况下，执行步骤A-B，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度；针对惯性测量单元安装参数已知情况下，执行步骤C，获得车辆实时位置、姿态和速度：
步骤A：基于惯性测量单元的输出数据，采用预设姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵；
步骤B：基于惯性测量单元相对于车轮的初步安装姿态矩阵，将预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度；
步骤C：基于已知的惯性测量单元安装参数，将预设各类型车轮参数、预设各类型车身姿态参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得车辆实时位置、姿态、速度。
作为本发明的一种优选技术方案，所述步骤A中，具体执行以下步骤，获得惯性测量单元相对于车轮的初步安装姿态矩阵：
步骤A1：采集车辆停止状态下预设时间段内惯性测量单元中三轴加速度计的输出数据，并对预设时间段内三轴加速度计的输出数据取平均值，记为
Figure BDA0003809174150000021
采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；
步骤A2：采集车辆行驶状态下预设时间段内三轴陀螺仪的输出数据，并将三轴陀螺仪输出数据减去陀螺零偏，进而获得预设时间段内三轴陀螺仪的输出数据平均值，记为
Figure BDA0003809174150000022
步骤A3：基于步骤A1和步骤A2得到的平均值数据，通过以下公式所示的姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵
Figure BDA0003809174150000023
Figure BDA0003809174150000031
其中，
Figure BDA0003809174150000032
Figure BDA0003809174150000033
Figure BDA0003809174150000034
其中，
Figure BDA0003809174150000035
表示b系到h系的姿态变换矩阵，即惯性测量单元相对于车轮的初步安装姿态矩阵；h系为沿其z轴根据右手定则转动β后与w系重合的坐标系，h系与w系的原点重合，系初始时刻其y轴为沿竖直方向指向上方；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定；h系和w系均相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系；
Figure BDA0003809174150000036
Figure BDA0003809174150000037
表示原坐标系沿着该系j轴根据右手定则转动
Figure BDA0003809174150000038
所得坐标系到原坐标系的姿态变换矩阵，j表示x、y或z轴；θh表示惯性测量单元在h系下的俯仰角，γh表示惯性测量单元在h系下的横滚角，ψh表示惯性测量单元在h系下的方位角。
作为本发明的一种优选技术方案，所述步骤B中，具体执行以下步骤，获得惯性测量单元安装参数，以及车辆实时位置、姿态、速度；
步骤B1：针对包含预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数的状态量，即状态量
Figure BDA0003809174150000039
构建状态方程；针对包含惯性测量单元输出数据和车身零水平角的观测量，即观测量
Figure BDA00038091741500000310
构建观测方程；设定初值；
其中，α为车轮转角，
Figure BDA00038091741500000311
为车轮角速度，
Figure BDA00038091741500000312
为车轮角加速度；r为惯性测量单元中心与车轮转轴之间的距离，即偏心距；γ、θ和ψ分别为车辆横滚角、俯仰角和方位角；
Figure BDA00038091741500000313
为实测的惯性测量单元中三轴加速度计的输出数据；
Figure BDA00038091741500000314
为实测的惯性测量单元中三轴陀螺仪输出角速度在h系下的投影中z轴的输出数据；
步骤B2：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤B2.1至步骤B2.4，直到车辆定位结束迭代结束，进而获得惯性测量单元安装参数和车辆实时位置、姿态、速度：
步骤B2.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；
步骤B2.2：进行卡尔曼滤波时间更新，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程获得采样时刻tk的状态误差协方差阵预测值；
步骤B2.3：进行卡尔曼滤波测量更新，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的状态误差协方差阵估计值；
步骤B2.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure BDA0003809174150000041
Figure BDA0003809174150000042
车辆位置
Figure BDA0003809174150000043
偏心距r与惯性测量单元相对于车轮的安装姿态矩阵
Figure BDA0003809174150000044
其中R为车轮半径；返回步骤B2.1，进入下一采样时刻迭代。
作为本发明的一种优选技术方案，所述构建的状态方程、观测方程如下所示：
状态方程：
Figure BDA0003809174150000045
观测方程：
Figure BDA0003809174150000046
其中，
Figure BDA0003809174150000047
Figure BDA0003809174150000048
Figure BDA0003809174150000051
gv＝[0 0 -g]T；
Figure BDA0003809174150000052
Figure BDA0003809174150000053
式中，R为车轮半径；g表示重力加速度大小；
Figure BDA0003809174150000054
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系，满足
Figure BDA0003809174150000055
上标‘T’表示矩阵取转置；
Figure BDA0003809174150000056
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure BDA0003809174150000057
Figure BDA0003809174150000058
为实测的惯性测量单元中三轴陀螺仪输出角速度；车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系为n系，相对于地球凝固，n系与初始时刻v系重合；η和ε为预设过程噪声；
Figure BDA0003809174150000059
∈、ζγ和ζθ均为预设观测噪声。
作为本发明的一种优选技术方案，所述步骤C中，基于已知的惯性测量单元安装参数，具体执行以下步骤，获得车辆实时位置、姿态、速度；
步骤C1：采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；
步骤C2：针对包含预设各类型车轮参数、预设各类型车身姿态参数的状态量，即状态量
Figure BDA00038091741500000510
构建状态方程；针对包含惯性测量单元输出数据和车身零水平角的观测量，即观测量
Figure BDA00038091741500000511
构建观测方程；设定初值；
其中，α为车轮转角，
Figure BDA00038091741500000512
为车轮角速度，
Figure BDA00038091741500000513
为车轮角加速度；γ、θ和ψ分别为车辆横滚角、俯仰角和方位角；
Figure BDA00038091741500000514
为实测的在惯性测量单元中三轴加速度计的输出数据；
Figure BDA00038091741500000515
为实测的惯性测量单元中三轴陀螺仪输出角速度在w系下的投影中z轴的输出数据；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定，w系相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系；
步骤C3：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤C3.1至步骤C3.4，直到车辆定位结束迭代结束，进而获得车辆实时位置、姿态、速度：
步骤C3.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；
步骤C3.2：进行卡尔曼滤波时间更新，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程获得采样时刻tk的状态误差协方差阵预测值；
步骤C3.3：进行卡尔曼滤波测量更新，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的状态误差协方差阵估计值；
步骤C3.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure BDA0003809174150000061
Figure BDA0003809174150000062
车辆位置
Figure BDA0003809174150000063
其中R为车轮半径；返回步骤C3.1，进入下一采样时刻迭代。
作为本发明的一种优选技术方案，所述构建的状态方程、观测方程如下所示：
状态方程：
Figure BDA0003809174150000064
观测方程：
Figure BDA0003809174150000065
其中，
Figure BDA0003809174150000066
Figure BDA0003809174150000067
Figure BDA0003809174150000068
Figure BDA0003809174150000069
Figure BDA00038091741500000610
式中，R为车轮半径；g表示重力加速度大小；
Figure BDA0003809174150000071
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系；
Figure BDA0003809174150000072
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure BDA0003809174150000073
Figure BDA0003809174150000074
为实测的惯性测量单元中三轴陀螺仪输出角速度；车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系为n系，相对于地球凝固，n系与初始时刻v系重合；η和ε为预设过程噪声；
Figure BDA0003809174150000075
∈、ζγ和ζθ均为预设观测噪声。
作为本发明的一种优选技术方案，所述惯性测量单元安装参数包括惯性测量单元坐标系相对于车轮坐标系的姿态变换矩阵
Figure BDA0003809174150000076
以及惯性测量单元中心与车轮转轴之间的距离r。
作为本发明的一种优选技术方案，所述卡尔曼滤波器采用扩展卡尔曼滤波器、无迹卡尔曼滤波器、或容积卡尔曼滤波器。
本发明的有益效果是：本发明提供了一种基于车轮安装惯性测量单元的车辆定位方法，本发明充分考虑了IMU安装参数、车身运动、车轮运动对车轮IMU输出的影响，建立了高精度的基于车轮安装IMU的车辆定位模型，构建基于该模型的状态方程与观测方程，并通过卡尔曼滤波算法进行信息融合，实现对车辆位置、速度、姿态和车轮IMU安装参数的高效估计，提高基于车轮安装IMU的车辆定位精度；并给出了给定车轮安装参数后的车辆定位方案，不用每次都对惯性测量单元安装参数进行估计，这样可减少车辆定位所需要的流程，降低模型复杂度，提高计算效率。
附图说明
图1为本发明一种基于车轮安装IMU的车辆定位方法的流程示意图；
图2为本发明实施例中IMU安装在车轮上的安装示意图；
图3为本发明实施例中坐标系及部分变量定义示意图；
图4为本发明实施例一中车轮IMU安装参数估计结果示意图；
图5为本发明实施例一中车辆定位结果示意图；
图6为本发明实施例一中车辆方位估计误差示意图；
图7为本发明实施例一中车辆速度估计结果示意图。
具体实施方式
下面结合附图对本发明进行进一步说明。下面的实施例可使本专业技术人员更全面地理解本发明，但不以任何方式限制本发明。
在本方案的实施例中，如图2所示，将MEMS IMU(惯性测量单元)固定在任意一个车辆非转向轮。计算机通过无线采集IMU数据，采样率为200Hz。设车辆处于水平路面，如图3所示，记IMU坐标系为b系，其原点位于IMU中心(准确地说，是加速度计敏感中心)，三个轴向分别沿着惯性测量单元的三个敏感方向；记车轮坐标系为w系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向IMU中心，z轴沿车轮转轴方向，指向车身左侧，y轴由右手坐标系规则确定；引入h系，使得h系沿其z轴根据右手定则转动β后与w系重合，且h系满足初始时刻其y轴沿竖直方向指向上方，h系和w系均相对于车轮凝固。记车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系n系相对于地球凝固，n系与初始时刻v系重合；记车轮半径为R，IMU中心距车轮转轴距离(即偏心距)为r。
定义
Figure BDA0003809174150000081
为q系相对于p系的运动量s在o系下的投影，运动量s可以为角速度(记为w)、位置(记为p)、速度(记为v)和加速度(记为a)，
Figure BDA0003809174150000082
由下式定义欧拉角
Figure BDA0003809174150000083
其中，
Figure BDA0003809174150000084
表示p系到q系的姿态变换矩阵，满足
Figure BDA0003809174150000085
上标‘T’表示矩阵取转置；roll、pitch和heading分别指横滚角、俯仰角和方位角，
Figure BDA0003809174150000086
表示原坐标系沿着该坐标系j轴根据右手定则转动
Figure BDA0003809174150000087
所得坐标系到原坐标系的姿态变换矩阵，j表示x、y或z。本实施例中，
Figure BDA0003809174150000088
为
Figure BDA0003809174150000089
的一阶导数，
Figure BDA00038091741500000810
为
Figure BDA00038091741500000811
的二阶导数，
Figure BDA00038091741500000812
为
Figure BDA00038091741500000813
的三阶导数。
实施例一
实施例一提供了一种基于车轮安装惯性测量单元的车辆定位方法，并且提供了估计出车轮IMU安装参数的方法。一种基于车轮安装惯性测量单元的车辆定位方法，基于安装在车辆车轮的惯性测量单元，针对惯性测量单元安装参数未知情况下，执行步骤A-B，如图1所示，分为车轮-IMU对准和车辆定位两个阶段，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度。
步骤A：进行车轮-IMU对准，基于惯性测量单元的输出数据，采用预设姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵；以及获得陀螺零偏。
所述步骤A中，具体执行以下步骤，获得惯性测量单元相对于车轮的初步安装姿态矩阵：
步骤A1：车辆在水平路面，保持静止状态，采集车辆停止状态下预设时间段内惯性测量单元中三轴加速度计的输出数据，并对预设时间段内三轴加速度计的输出数据取平均值，记为
Figure BDA00038091741500000814
为减少噪声影响，对10秒内采集的加速度计输出取平均；
满足：
Figure BDA0003809174150000091
这里，g为当地重力加速度值；
采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；为减少噪声影响，对10秒内采集的三轴陀螺仪输出取平均。
步骤A2：车辆在水平路面行驶一小段距离，采集三轴陀螺仪输出，采集车辆行驶状态下预设时间段内三轴陀螺仪的输出数据，并将三轴陀螺仪输出数据减去陀螺零偏，进而获得预设时间段内三轴陀螺仪的输出数据取平均值，记为
Figure BDA0003809174150000092
为减少噪声影响，对50秒内采集的三轴陀螺仪输出取平均；i为惯性坐标系，忽略地球自转等小量，满足：
Figure BDA0003809174150000093
步骤A3：基于步骤A1和步骤A2得到的平均值数据，通过以下公式所示的姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵
Figure BDA0003809174150000094
即h系相对于b系的姿态变换矩阵
Figure BDA0003809174150000095
Figure BDA0003809174150000096
将
Figure BDA0003809174150000097
用欧拉角描述，对应的横滚角、俯仰角和方位角分别记为γh、θh和ψh，将式(3)代入式(2)得：
Figure BDA0003809174150000098
从而由下式计算得γh和θh：
Figure BDA0003809174150000099
这里
Figure BDA00038091741500000910
记
Figure BDA00038091741500000911
将式(3)和式(4)依次代入式(1)得：
Figure BDA00038091741500000912
从而由下式计算得ψh：
Figure BDA00038091741500000913
进而基于
Figure BDA00038091741500000914
由γh、θh和ψh计算可得
Figure BDA00038091741500000915
其中，
Figure BDA00038091741500000916
表示b系到h系的姿态变换矩阵，即惯性测量单元相对于车轮的安装姿态矩阵；h系为沿其z轴根据右手定则转动β后与w系重合的坐标系，h系与w系的原点重合，h系初始时刻其y轴为沿竖直方向指向上方；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定；h系和w系均相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系。
步骤B：在车辆、车轮运动学的约束下，基于惯性测量单元相对于车轮的初步安装姿态矩阵，将预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度。
所述步骤B中，具体执行以下步骤，获得惯性测量单元安装参数，以及车辆实时位置、姿态、速度；
步骤B1：针对包含预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数的状态量，即状态量
Figure BDA0003809174150000101
构建状态方程；针对包含惯性测量单元输出数据以及车身零水平角的观测量，即观测量
Figure BDA0003809174150000102
构建观测方程；建立卡尔曼滤波器，离散化状态方程和观测方程，设定初值，进入步骤B2；设定初值包括设定状态量X初值，设定状态误差协方差阵初值为P(t0)，设定状态噪声的方差为Q，设定观测噪声V的方差阵为R，给定初始方位角和初始经纬度。
其中，预设各类型车轮参数包括车轮转角α，车轮角速度
Figure BDA0003809174150000103
车轮角加速度
Figure BDA0003809174150000104
预设惯性测量单元安装相关参数包括惯性测量单元中心与车轮转轴之间的距离即偏心距r，以及β；预设各类型车身姿态参数包括车辆横滚角γ、俯仰角θ和方位角ψ；
Figure BDA0003809174150000105
为实测的惯性测量单元中三轴加速度计的输出数据；
Figure BDA0003809174150000106
为实测的三轴陀螺仪输出角速度在h系下的投影中z轴的输出数据。
建立卡尔曼滤波器包括建立扩展卡尔曼滤波器的状态方程和观测方程。所述构建的状态方程、观测方程如下所示：
状态方程：
Figure BDA0003809174150000111
观测方程：
Figure BDA0003809174150000112
其中，
Figure BDA0003809174150000113
Figure BDA0003809174150000114
Figure BDA0003809174150000115
Figure BDA0003809174150000116
Figure BDA0003809174150000117
本实施例中近似下面的等号成立gv＝[0 0 -g]T，
Figure BDA0003809174150000118
式中，g表示重力加速度大小；
Figure BDA0003809174150000119
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系，满足
Figure BDA00038091741500001110
上标‘T’表示矩阵取转置；
Figure BDA00038091741500001111
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure BDA00038091741500001112
η和ε为预设过程噪声；η为由运动体运动不确定性导致，将其近似为零均值高斯白噪声；忽略地球自转等小量，有
Figure BDA00038091741500001113
考虑陀螺输出噪声，陀螺实际输出记为
Figure BDA00038091741500001114
Figure BDA00038091741500001115
ε包括陀螺输出噪声和地球自转等小量，将其近似为零均值高斯白噪声；
Figure BDA00038091741500001116
∈、ζγ和ζθ均为观测噪声，均近似为零均值高斯白噪声，
Figure BDA00038091741500001117
为加速度计产生的误差和模型误差等，∈为陀螺误差和模型误差等，ζγ和ζθ反映由于路面不水平等原因导致的车身非水平情况，将它们近似为零均值高斯白噪声；
Figure BDA00038091741500001118
为加速度计的实际输出，其包含输出噪声；
Figure BDA00038091741500001119
本实施例中构建的状态方程和观测方程思路如下所示：充分考虑了IMU安装参数、车身运动、车轮运动对车轮IMU输出的影响。根据点的加速度合成定理，动点在某瞬时的绝对加速度等于该瞬时它的牵连加速度、相对加速度与科氏加速度的矢量和。式中，观测方程中，忽略地球自转，加速度计的输出
Figure BDA0003809174150000121
主要受到如下几项加速度作用：(1)重力加速度，观测方程中的该项为
Figure BDA0003809174150000122
(2)牵连加速度，即动参考坐标系(即本例中的v系)与动点(即本例中的加速度计敏感中心)相重合的那一点(即牵连点)的加速度，观测方程中体现的项为
Figure BDA0003809174150000123
(这里忽略了较小的含
Figure BDA0003809174150000124
的项)；(3)相对加速度，由车轮相对于车身旋转所致，观测方程中的该项为
Figure BDA0003809174150000125
(4)科氏加速度，是由动参考坐标系为转动(如本例中，车辆转弯)时，牵连运动与相对运动相互作用而产生的，科氏加速度等于动参考坐标系角速度矢与点的相对速度矢的矢积的两倍，观测方程中的该项为
Figure BDA0003809174150000126
进而，本实施例中规避了车身运动、安装参数等带来的误差，提高了基于车轮安装IMU的车辆定位精度。式中，状态方程中，由于陀螺感知车轮旋转和车身角速度作用，故令陀螺输出减去车轮旋转角速度得到车身角速度，如式
Figure BDA0003809174150000127
针对卡尔曼滤波器，该滤波器是是一种高效率的递归滤波器。基于状态方程和测量方程，扩展卡尔曼滤波过程如下：X(tk)是tk时刻的系统状态，Z(tk)是tk时刻的实测值；首先要预测下一时刻系统的状态，即时间更新。假设现在的系统状态是tk时刻，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(tk|tk-1)＝f(X(tk-1|tk-1)，U(tk))
式中，X(tk|tk-1)是利用上一状态预测的结果，X(tk-1|tk-1)是上一状态最优的结果，U(tk)为现在状态的控制量，如果没有控制量，它可以为0，f(·)是非线性过程方程；系统状态更新后，还要更新X(tk|tk-1)对应的协方差阵，用P表示协方差阵：
P(tk|tk-1)＝AP(tk-1|tk-1)AT+BQBT
式中，P(tk|tk-1)是X(tk|tk-1)对应的状态误差协方差阵，P(tk-1|tk-1)是X(tk-1|tk-1)对应的状态误差协方差阵，A是状态方程对状态量求偏导所得的雅可比矩阵，AT表示A的转置矩阵，Q是过程噪声的协方差阵，B是状态方程对过程噪声变量求偏导所得的雅可比矩阵，BT表示B的转置矩阵，则完成对系统当前状态的预测，即时间更新。
然后，基于现在状态的预测结果，结合现在状态的实测值。得到现在状态tk时刻的状态最优化估算值X(tk|tk)，即测量更新：
X(tk|tk)＝X(tk|tk-1)+Kg(tk)(Z(tk)-h(X(tk|tk-1)))
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(tk)＝P(tk|tk-1)HT/(HP(tk|tk-1)HT+R)
式中，h(·)是非线性观测方程，H是观测方程的雅可比矩阵，HT为H的转置矩阵；
则得到了tk时刻状态下最优的估算值X(tk|tk)。卡尔曼滤波器要不断地运行下去直到系统过程结束，还要更新tk时刻X(tk|tk)的协方差阵：
P(tk|tk)＝(I-Kg(tk)H)P(tk|tk-1)
其中，I为单位矩阵，至此完成tk时刻的测量更新。当系统进入tk+1时刻时，P(tk|tk)就是下一状态的P(tk-1|tk-1)。算法就可以自回归的运算下去，直到车辆定位结束。
基于上述卡尔曼滤波器更新状态过程，结合创建的状态方程与观测方程，基于车辆初始位置进行以下步骤的更新迭代过程，步骤B2：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤B2.1至步骤B2.4，直到车辆定位结束迭代结束，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度：
步骤B2.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；tk＝kT，T为采样周期，k为正整数；
步骤B2.2：进行时间更新，通过状态方程预测状态量，计算状态方程雅可比矩阵以预测状态误差协方差阵，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程雅克比矩阵获得采样时刻tk对应的状态误差协方差阵预测值；
步骤B2.3：进行测量更新，计算观测方程雅可比矩阵以计算增益矩阵，估计状态量和状态误差协方差阵，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程雅克比矩阵，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、观测方程雅克比矩阵、采样时刻tk的状态误差协方差阵预测值以及观测噪声方差阵，获得采样时刻tk的状态误差协方差阵估计值；
步骤B2.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure BDA0003809174150000131
Figure BDA0003809174150000132
车辆位置
Figure BDA0003809174150000133
偏心距r与惯性测量单元相对于车轮的安装姿态矩阵
Figure BDA0003809174150000134
更新位置和IMU安装姿态参数；返回步骤B2.1，进入下一采样时刻迭代。
本实施例中，根据实际应用场景，所述卡尔曼滤波器除了可以采用扩展卡尔曼滤波器、还可以采用无迹卡尔曼滤波器、或容积卡尔曼滤波器等。当已完成惯性测量单元安装参数估计后，下次对车辆定位时可以将惯性测量单元安装参数作为已知量使用，不用每次都对惯性测量单元安装参数进行估计，这样可减少车辆定位所需要的流程，降低模型复杂度，提高计算效率。
图4显示本实施例中偏心距r和
Figure BDA0003809174150000141
对应的安装方位角随时间的估计结果，短时间内估计结果就趋于稳定。图5显示本实施例中车辆的定位结果，并和GPS/IMU组合导航系统Mti-G-710所得的真值轨迹进行对比。图6显示本实施例中车辆方位估计误差示意图，图7显示本实施例中车辆速度估计结果示意图，并和Mti-G-710所得的真值速度进行对比。本实施例技术具有较高的定位精度，并且能获得高精度的车辆姿态、速度。
实施例二
基于已知惯性测量单元安装参数，所述安装参数为车轮IMU安装姿态矩阵
Figure BDA0003809174150000142
和偏心距r，本实施例提供一种基于车轮安装惯性测量单元的车辆定位方法，针对惯性测量单元安装参数已知情况下，执行步骤C，获得车辆实时位置、姿态和速度：
步骤C：在车辆、车轮运动学的约束下，基于已知的惯性测量单元安装参数，将预设各类型车轮参数、预设各类型车身姿态参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得车辆实时位置、姿态、速度。
所述步骤C中，基于已知的惯性测量单元安装参数，具体执行以下步骤，进而获得车辆实时位置、姿态、速度；
步骤C1：采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；为减少噪声影响，对10秒内采集的三轴陀螺仪输出取平均。
步骤C2：针对包括预设各类型车轮参数、预设各类型车身姿态参数的状态量，即状态量
Figure BDA0003809174150000143
构建状态方程；针对包括惯性测量单元输出数据以及车身零水平角的观测量，即观测量
Figure BDA0003809174150000144
构建观测方程；建立卡尔曼滤波器，离散化状态方程和观测方程，设定初值；设定初值包括设定状态量X初值，设定状态误差协方差阵初值为P(t0)，设定状态噪声的方差为Q，设定观测噪声V的方差阵为R，给定初始方位角和初始经纬度。
其中，α为车轮转角，
Figure BDA0003809174150000145
为车轮角速度，
Figure BDA0003809174150000146
为车轮角加速度；γ、θ和ψ分别为车辆横滚角、俯仰角和方位角；
Figure BDA0003809174150000147
为实测的在惯性测量单元中三轴加速度计的输出数据；
Figure BDA0003809174150000148
为实测的惯性测量单元中三轴陀螺仪输出角速度在w系下的投影中z轴的输出数据；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定，w系相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系。
惯性测量单元安装参数已知情况下，建立卡尔曼滤波器包括建立扩展卡尔曼滤波器的状态方程和观测方程，所述构建的状态方程、观测方程如下所示：
状态方程：
Figure BDA0003809174150000151
观测方程：
Figure BDA0003809174150000152
其中，
Figure BDA0003809174150000153
Figure BDA0003809174150000154
Figure BDA0003809174150000155
Figure BDA0003809174150000156
Figure BDA0003809174150000157
本实施例中近似下面的等号成立
Figure BDA0003809174150000158
Figure BDA0003809174150000159
式中，R为车轮半径；g表示重力加速度大小；
Figure BDA00038091741500001510
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系，满足
Figure BDA00038091741500001511
上标‘T’表示矩阵取转置；
Figure BDA00038091741500001512
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure BDA00038091741500001513
车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系为n系，相对于地球凝固，n系与初始时刻v系重合；η和ε为预设过程噪声；η由运动体运动不确定性导致，将其近似为零均值高斯白噪声；忽略地球自转等小量，有
Figure BDA00038091741500001514
考虑陀螺输出噪声，陀螺实际输出记为
Figure BDA00038091741500001515
Figure BDA00038091741500001516
ε包括陀螺输出噪声和地球自转等小量，将其近似为零均值高斯白噪声；
Figure BDA00038091741500001517
∈、ζγ和ζθ均为观测噪声，均近似为零均值高斯白噪声，与实施例一中相同；
Figure BDA0003809174150000161
为加速度计的实际输出，其包含输出噪声；
Figure BDA0003809174150000162
基于前面所述的卡尔曼滤波器更新状态过程，结合创建的状态方程与观测方程，基于初始位置进行以下步骤的更新迭代过程；步骤C3：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤C3.1至步骤C3.4，直到车辆定位结束迭代结束，进而获得车辆实时位置、姿态、速度：
步骤C3.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；
步骤C3.2：进行时间更新，通过状态方程预测状态量，计算状态方程雅可比矩阵以预测状态误差协方差阵，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程雅克比矩阵获得采样时刻tk对应的状态误差协方差阵预测值；
步骤C3.3：进行测量更新，计算观测方程雅可比矩阵以计算增益矩阵，估计状态量和状态误差协方差阵，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程雅克比矩阵，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、观测方程雅克比矩阵、采样时刻tk的状态误差协方差阵预测值以及观测噪声方差阵，获得采样时刻tk的状态误差协方差阵估计值；
步骤C3.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure BDA0003809174150000163
Figure BDA0003809174150000164
车辆位置
Figure BDA0003809174150000165
返回步骤C3.1，进入下一采样时刻迭代。
本实施例中，根据实际应用场景，所述卡尔曼滤波器除了可以采用扩展卡尔曼滤波器、还可以采用无迹卡尔曼滤波器、或容积卡尔曼滤波器等。
本发明设计了一种基于车轮安装惯性测量单元的车辆定位方法，本发明充分考虑了IMU安装参数、车身运动、车轮运动对车轮IMU输出的影响，建立了高精度的基于车轮安装IMU的车辆定位模型，基于该模型构建状态方程与观测方程并通过卡尔曼滤波算法进行信息融合，实现对车辆位置、速度、姿态和车轮IMU安装参数的有效估计，提高基于车轮安装IMU的车辆定位精度。
以上仅为本发明的较佳实施例，但并不限制本发明的专利范围，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来而言，其依然可以对前述各具体实施方式所记载的技术方案进行修改，或者对其中部分技术特征进行等效替换。凡是利用本发明说明书及附图内容所做的等效结构，直接或间接运用在其他相关的技术领域，均同理在本发明专利保护范围之内。

Claims (8)

1.一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：基于安装在车辆车轮预设位置的惯性测量单元，针对惯性测量单元安装参数未知情况下，执行步骤A-B，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度；针对惯性测量单元安装参数已知情况下，执行步骤C，获得车辆实时位置、姿态和速度：
步骤A：基于惯性测量单元的输出数据，采用预设姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵；
步骤B：基于惯性测量单元相对于车轮的初步安装姿态矩阵，将预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得惯性测量单元安装参数，以及车辆实时位置、姿态和速度；
步骤C：基于已知的惯性测量单元安装参数，将预设各类型车轮参数、预设各类型车身姿态参数包含在状态量中，将惯性测量单元的输出数据以及车身零水平角包含在观测量中，结合卡尔曼滤波器，获得车辆实时位置、姿态、速度。
2.根据权利要求1所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述步骤A中，具体执行以下步骤，获得惯性测量单元相对于车轮的初步安装姿态矩阵：
步骤A1：采集车辆停止状态下预设时间段内惯性测量单元中三轴加速度计的输出数据，并对预设时间段内三轴加速度计的输出数据取平均值，记为
Figure FDA0003809174140000011
采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；
步骤A2：采集车辆行驶状态下预设时间段内三轴陀螺仪的输出数据，并将三轴陀螺仪输出数据减去陀螺零偏，进而获得预设时间段内三轴陀螺仪的输出数据平均值，记为
Figure FDA0003809174140000012
步骤A3：基于步骤A1和步骤A2得到的平均值数据，通过以下公式所示的姿态解算算法获得惯性测量单元相对于车轮的初步安装姿态矩阵
Figure FDA0003809174140000013
Figure FDA0003809174140000014
其中，
Figure FDA0003809174140000015
Figure FDA0003809174140000016
Figure FDA0003809174140000017
其中，
Figure FDA0003809174140000021
表示b系到h系的姿态变换矩阵，即惯性测量单元相对于车轮的初步安装姿态矩阵；h系为沿其z轴根据右手定则转动β后与w系重合的坐标系，h系与w系的原点重合，h系初始时刻其y轴为沿竖直方向指向上方；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定；h系和w系均相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系；
Figure FDA0003809174140000022
Figure FDA0003809174140000023
表示原坐标系沿着该系j轴根据右手定则转动
Figure FDA0003809174140000024
所得坐标系到原坐标系的姿态变换矩阵，j表示x、y或z轴；θh表示惯性测量单元在h系下的俯仰角，γh表示惯性测量单元在h系下的横滚角，ψh表示惯性测量单元在h系下的方位角。
3.根据权利要求2所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述步骤B中，具体执行以下步骤，获得惯性测量单元安装参数，以及车辆实时位置、姿态、速度；
步骤B1：针对包含预设各类型车轮参数、预设各类型车身姿态参数和预设惯性测量单元安装相关参数的状态量，即状态量
Figure FDA0003809174140000025
构建状态方程；针对包含惯性测量单元输出数据和车身零水平角的观测量，即观测量
Figure FDA0003809174140000026
构建观测方程；设定初值；
其中，α为车轮转角，
Figure FDA0003809174140000027
为车轮角速度，
Figure FDA0003809174140000028
为车轮角加速度；r为惯性测量单元中心与车轮转轴之间的距离，即偏心距；γ、θ和ψ分别为车辆横滚角、俯仰角和方位角；
Figure FDA0003809174140000029
为实测的惯性测量单元中三轴加速度计的输出数据；
Figure FDA00038091741400000210
为实测的惯性测量单元中三轴陀螺仪输出角速度在h系下的投影中z轴的输出数据；
步骤B2：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤B2.1至步骤B2.4，直到车辆定位结束迭代结束，进而获得惯性测量单元安装参数和车辆实时位置、姿态、速度：
步骤B2.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；
步骤B2.2：进行卡尔曼滤波时间更新，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程获得采样时刻tk的状态误差协方差阵预测值；
步骤B2.3：进行卡尔曼滤波测量更新，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的状态误差协方差阵估计值；
步骤B2.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure FDA0003809174140000031
Figure FDA0003809174140000032
车辆位置
Figure FDA0003809174140000033
偏心距r与惯性测量单元相对于车轮的安装姿态矩阵
Figure FDA0003809174140000034
其中R为车轮半径；返回步骤B2.1，进入下一采样时刻迭代。
4.根据权利要求3所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述构建的状态方程、观测方程如下所示：
状态方程：
Figure FDA0003809174140000035
观测方程：
Figure FDA0003809174140000036
其中，
Figure FDA0003809174140000037
Figure FDA0003809174140000038
Figure FDA0003809174140000039
gv＝[0 0 -g]T；
Figure FDA00038091741400000310
Figure FDA00038091741400000311
式中，R为车轮半径；g表示重力加速度大小；
Figure FDA0003809174140000041
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系，满足
Figure FDA0003809174140000042
上标‘T’表示矩阵取转置；
Figure FDA0003809174140000043
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure FDA0003809174140000044
Figure FDA0003809174140000045
为实测的惯性测量单元中三轴陀螺仪输出角速度；车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系为n系，相对于地球凝固，n系与初始时刻v系重合；η和ε为预设过程噪声；
Figure FDA0003809174140000046
∈、ζγ和ζθ均为预设观测噪声。
5.根据权利要求1所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述步骤C中，基于已知的惯性测量单元安装参数，具体执行以下步骤，获得车辆实时位置、姿态、速度；
步骤C1：采集车辆停止状态下预设时间段内惯性测量单元中三轴陀螺仪的输出数据，并对预设时间段内三轴陀螺仪的输出数据取平均值，记为陀螺零偏；
步骤C2：针对包含预设各类型车轮参数、预设各类型车身姿态参数的状态量，即状态量
Figure FDA0003809174140000047
构建状态方程；针对包含惯性测量单元输出数据和车身零水平角的观测量，即观测量
Figure FDA0003809174140000048
构建观测方程；设定初值；
其中，α为车轮转角，
Figure FDA0003809174140000049
为车轮角速度，
Figure FDA00038091741400000410
为车轮角加速度；γ、θ和ψ分别为车辆横滚角、俯仰角和方位角；
Figure FDA00038091741400000411
为实测的在惯性测量单元中三轴加速度计的输出数据；
Figure FDA00038091741400000412
为实测的惯性测量单元中三轴陀螺仪输出角速度在w系下的投影中z轴的输出数据；w系表示车轮坐标系，其原点位于车轮中心，其x轴垂直于车轮转轴，且由车轮中心指向惯性测量单元中心，z轴沿车轮转轴方向指向车身左侧，y轴由右手坐标系规则确定，w系相对于车轮凝固；惯性测量单元坐标系为b系，三个轴向分别沿着惯性测量单元的三个敏感方向，其原点位于惯性测量单元中心，即三轴加速度计敏感中心；i系为惯性坐标系；
步骤C3：基于构建的状态方程与观测方程，利用卡尔曼滤波器，迭代执行步骤C3.1至步骤C3.4，直到车辆定位结束迭代结束，进而获得车辆实时位置、姿态、速度：
步骤C3.1：基于预设采样周期，获得采样时刻tk惯性测量单元的输出数据，其中惯性测量单元的三轴陀螺仪输出数据减去陀螺零偏；
步骤C3.2：进行卡尔曼滤波时间更新，具体为：基于采样时刻tk-1的状态量估计值，通过状态方程，获得采样时刻tk的状态量预测值；并基于采样时刻tk-1的状态误差协方差阵估计值以及状态方程获得采样时刻tk的状态误差协方差阵预测值；
步骤C3.3：进行卡尔曼滤波测量更新，具体为：基于采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的增益矩阵；基于增益矩阵、采样时刻tk的状态量预测值、采样时刻tk观测量数据以及观测方程，获得采样时刻tk的状态量估计值；并且基于增益矩阵、采样时刻tk的状态误差协方差阵预测值、观测噪声方差阵以及观测方程，获得采样时刻tk的状态误差协方差阵估计值；
步骤C3.4：基于采样时刻tk的状态量估计值，获得采样时刻tk的车辆姿态
Figure FDA0003809174140000051
Figure FDA0003809174140000052
车辆位置
Figure FDA0003809174140000053
其中R为车轮半径；返回步骤C3.1，进入下一采样时刻迭代。
6.根据权利要求5所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述构建的状态方程、观测方程如下所示：
状态方程：
Figure FDA0003809174140000054
观测方程：
Figure FDA0003809174140000055
其中，
Figure FDA0003809174140000056
Figure FDA0003809174140000057
Figure FDA0003809174140000058
gv＝[0 0 -g]T；
Figure FDA0003809174140000059
Figure FDA00038091741400000510
式中，R为车轮半径；g表示重力加速度大小；
Figure FDA00038091741400000511
表示p系到q系的姿态变换矩阵，p系和q系均指坐标系；
Figure FDA00038091741400000512
为q系相对于p系的运动量s在o系下的投影，运动量s为角速度w、位置p、速度v或加速度a，
Figure FDA00038091741400000513
Figure FDA00038091741400000514
为实测的惯性测量单元中三轴陀螺仪输出角速度；车身坐标系为v系，其三个轴向依次指向车右、车前、车上，其原点位于安装IMU的车轮与地面接触点；导航坐标系为n系，相对于地球凝固，n系与初始时刻v系重合；η和ε为预设过程噪声；
Figure FDA0003809174140000061
∈、ζγ和ζθ均为预设观测噪声。
7.根据权利要求1所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述惯性测量单元安装参数包括惯性测量单元坐标系相对于车轮坐标系的姿态变换矩阵
Figure FDA0003809174140000062
以及惯性测量单元中心与车轮转轴之间的距离r。
8.根据权利要求1所述一种基于车轮安装惯性测量单元的车辆定位方法，其特征在于：所述卡尔曼滤波器采用扩展卡尔曼滤波器、无迹卡尔曼滤波器、或容积卡尔曼滤波器。
CN202211006263.4A 2022-08-22 2022-08-22 一种基于车轮安装惯性测量单元的车辆定位方法 Pending CN115451949A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211006263.4A CN115451949A (zh) 2022-08-22 2022-08-22 一种基于车轮安装惯性测量单元的车辆定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211006263.4A CN115451949A (zh) 2022-08-22 2022-08-22 一种基于车轮安装惯性测量单元的车辆定位方法

Publications (1)

Publication Number Publication Date
CN115451949A true CN115451949A (zh) 2022-12-09

Family

ID=84298654

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211006263.4A Pending CN115451949A (zh) 2022-08-22 2022-08-22 一种基于车轮安装惯性测量单元的车辆定位方法

Country Status (1)

Country Link
CN (1) CN115451949A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116429094A (zh) * 2023-06-15 2023-07-14 小米汽车科技有限公司 定位方法、装置、电子设备及存储介质
CN116718196A (zh) * 2023-08-09 2023-09-08 腾讯科技（深圳）有限公司 一种导航方法、装置、设备及计算机可读存储介质

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116429094A (zh) * 2023-06-15 2023-07-14 小米汽车科技有限公司 定位方法、装置、电子设备及存储介质
CN116429094B (zh) * 2023-06-15 2023-09-26 小米汽车科技有限公司 定位方法、装置、电子设备及存储介质
CN116718196A (zh) * 2023-08-09 2023-09-08 腾讯科技（深圳）有限公司 一种导航方法、装置、设备及计算机可读存储介质
CN116718196B (zh) * 2023-08-09 2023-10-20 腾讯科技（深圳）有限公司 一种导航方法、装置、设备及计算机可读存储介质

Similar Documents

Publication Publication Date Title
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN115451949A (zh) 一种基于车轮安装惯性测量单元的车辆定位方法
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN103743414B (zh) 一种里程计辅助车载捷联惯导系统行进间初始对准方法
CN105300379B (zh) 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN110307836B (zh) 一种用于无人清扫车辆贴边清扫的精确定位方法
CN111678514B (zh) 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN105318876A (zh) 一种惯性里程计组合高精度姿态测量方法
CN101782391A (zh) 机动加速度辅助的扩展卡尔曼滤波航姿系统姿态估计方法
CN110285804B (zh) 基于相对运动模型约束的车辆协同导航方法
CN110940344B (zh) 一种用于自动驾驶的低成本传感器组合定位方法
CN112697138A (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN111024074A (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN110926499A (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110926483B (zh) 一种用于自动驾驶的低成本传感器组合定位系统及方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN110567456B (zh) 基于抗差卡尔曼滤波的bds/ins组合列车定位方法
CN104101345A (zh) 基于互补重构技术的多传感器姿态融合方法
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法
Mu et al. Improved decentralized GNSS/SINS/odometer fusion system for land vehicle navigation applications

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