CN111551174A - 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 - Google Patents

基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 Download PDF

Info

Publication number
CN111551174A
CN111551174A CN202010647018.6A CN202010647018A CN111551174A CN 111551174 A CN111551174 A CN 111551174A CN 202010647018 A CN202010647018 A CN 202010647018A CN 111551174 A CN111551174 A CN 111551174A
Authority
CN
China
Prior art keywords
vehicle
acceleration
matrix
quaternion
gyroscope
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
CN202010647018.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.)
Wuxi Bewis Sensing Technology Co ltd
Peking University Shenzhen Graduate School
Original Assignee
Wuxi Bewis Sensing Technology Co ltd
Peking University Shenzhen Graduate School
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 Wuxi Bewis Sensing Technology Co ltd, Peking University Shenzhen Graduate School filed Critical Wuxi Bewis Sensing Technology Co ltd
Publication of CN111551174A publication Critical patent/CN111551174A/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Automation & Control Theory (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种基于多传感器惯性导航系统的高动态车辆姿态估计方法，包括获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，得到观测姿态四元数；获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。由此，能够消除运动加速度对姿态估计的影响，得到观测姿态四元数。建立基于四元数的姿态估计滤波方程，从而完成了车辆姿态的高精度解算，并且能补足在没有GPS状态下的导航信息输出，提供车辆的自主导航系统的必备信息。

Description

基于多传感器惯性导航系统的高动态车辆姿态计算方法及 系统
技术领域
本申请涉及高动态车辆姿态估计技术领域，尤其涉及一种基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统。
背景技术
随着现代汽车工业的发展。智能驾驶系统已成为当今汽车行业的发展趋势。人们对智能驾驶系统的需求越来越高。这些电子系统大多需要基于汽车姿势和位置信息的反馈。因此，智能驾驶车辆系统的发展基于准确、稳定、实时的姿态和导航信息。
由MEMS传感器组成的姿态测量系统由于功耗低、体积小、成本低，在当今汽车行业得到了广泛的应用。然而，单个MEMS传感器有以下缺点：1)陀螺仪在积分中存在严重的累积漂移误差。2)线性加速度和振动效应在加速度计的姿态计算中引起较大误差。整个MEMS姿态测量系统的主要缺点是车辆在运动过程中产生非常复杂的噪音，这些噪音是随机的，无法提前预测和消除。因此，不能只使用传感器的原始数据来获得运动中汽车的高精度姿态。
发明内容
本发明的目的是要提供一种基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统，可以解决上述现有技术问题中的一个或者多个。
根据本发明的一个方面提供一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，包括以下步骤：
获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，得到观测姿态四元数；
获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；
通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。
在一些实施方式中，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度包括以下公式：
fba＝fa-Va；其中，fba是车辆补偿线性运动加速度后的加速度，fa是加速度计的测量；Va是车辆的线性运动加速度；
g＝fa-(2ωie+ωeT)×V-fba，其中，g是重力加速度，也可以看作是姿态加速度fb。2ωie是车辆相对于地球的运动和地球的自转引起。此处省略它；ωeT×V是车辆在地球表面的圆周运动引起的向心加速度，V是车辆对地线性运动加速度，从而：
fb＝fa-ωeT×V-fba。
在一些实施方式中，获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值的步骤包括：
Figure BDA0002573441260000021
其中，
Figure BDA0002573441260000022
是b坐标系相对于n坐标系的四元数；
Figure BDA0002573441260000023
是
Figure BDA0002573441260000024
的导数；
Figure BDA0002573441260000025
其中，ωx表示绕俯仰轴的角速度；ωy表示绕横滚轴的角速度；ωz表示绕偏航轴的角速度；
四元运动方程的矩阵形式表示为：
Figure BDA0002573441260000026
Figure BDA0002573441260000027
其中，T表示系统采样间隔；
Figure BDA0002573441260000028
使用一阶Runge-Kutta方法求解四元微分方程，可以得出离散时间模型作为：
Figure BDA0002573441260000031
qk-1表示k-1时刻的四元数。设置初始四元数q(0)，并使用该方程递推在k时刻四元数的值qk，然后更新姿态矩阵。
在一些方式中，卡尔曼滤波过程包括对陀螺仪测量的修正以及加速度计测量的修正，
其中，陀螺仪测量模型为：
ωb＝Kω*ω+bω+Vω；其中，ωb是陀螺仪的测量值；Kω是比例系数矩阵；ω是陀螺仪的实际角速度；bω是陀螺仪随机漂移矩阵；Vω是测量噪声矩阵，为零均值的高斯噪声，协方差为
Figure BDA0002573441260000032
其中，加速度计的测量模型为：
Figure BDA0002573441260000033
ab表示加速度计的测量；Ka表示标度系数矩阵；gn＝(0，0，-g)T表示地理坐标系下的重力加速度矢量，g表示重力加速度矢量；af表示车辆的线性加速度矩阵；ba表示偏置误差矩阵；Va表示测量噪声矩阵，为零均值高斯白噪声，协方差为
Figure BDA0002573441260000034
在一些实施方式中，通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息包括以下步骤：
确定系统模型：
Figure BDA0002573441260000035
其中：x(k)∈Rn是k时刻系统的状态向量，z(k)∈Rm是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rn和v(k)∈Rm是具有方差的关联过程噪声矢量Q(k)和R(k)；
系统状态向量由下式表示：
Figure BDA0002573441260000036
其中，
Figure BDA0002573441260000037
是姿态四元数，bwx bwy bwz是陀螺仪围绕三轴的漂移；
系统状态方程如下：
Figure BDA0002573441260000041
过程噪声协方差矩阵为
Figure BDA0002573441260000042
系统观测向量由下式表示：
Figure BDA0002573441260000043
其中，
Figure BDA0002573441260000044
是车辆坐标系中的三轴加速度测量值；
当车辆以恒定的速度静止行驶时，以下关系：
Figure BDA0002573441260000045
系统的观测方程如下：
Figure BDA0002573441260000046
测量噪声协方差矩阵如下：
Figure BDA0002573441260000047
在一些实施方式中，状态更新包括：
初步估计：
计算初始x(0)值，设置过程噪声协方差Q(k)，测量噪声协方差R(k)和误差方差矩阵P(0)，
时间更新：
状态转移函数的一步预测：
Figure BDA0002573441260000048
计算状态矢量一步预测误差方差矩阵：
P(k，k-1)＝f(k，k-10P(k-1)fT (k，k-1)+Q(k)；
计算卡尔曼增益矩阵：
K(k)＝P(k，k-1)HT (k)[H(k)P(k，k-1)HT (k)+R(k)]-1；
测量更新：
从状态转换函数获取的状态的一步预测：
Figure BDA0002573441260000051
在k时刻更新状态矢量估计：
Figure BDA0002573441260000052
更新状态误差协方差矩阵：
P(k)＝[I-K(k)H(k)]P(k，k-1)[I-K(k)H(k)]T+K(k)R(k)KT (k)。
在一些实施方式中，还包括对h(x(k)，k)求取Jacobi矩阵，可得系统量测矩阵为：
Figure BDA0002573441260000053
根据本发明的另一个方面，提供一种应用前述的方法的系统，其特征在于，包括加速度计、速度计、陀螺仪、处理器和滤波器，所述加速度计、速度计和陀螺仪的数据输出端与所述处理器的输入端相连，所述处理器的数据输出端与所述滤波器的输入端相连。
另外，在本技术方案中，凡未作特别说明的，均可通过采用本领域中的常规手段来实现本技术方案。
本申请方案与现有技术相比，具有以下有益效果：本申请技术方案先用陀螺仪输出的角速度值，通过四元数微分方程得到估计姿态四元数。对车载速度计和加速度计融合后的加速度进行运动加速度抑制处理，消除运动加速度对姿态估计的影响，得到观测姿态四元数。建立基于四元数的姿态估计滤波方程，从而完成了车辆姿态的高精度解算。并且能补足在没有GPS状态下的导航信息输出，提供车辆的自主导航系统的必备信息。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请一实施例提供的种基于多传感器惯性导航系统的高动态车辆姿态估计方法的流程图。
图2是本申请另一实施例提供的应用基于多传感器惯性导航系统的高动态车辆姿态估计方法系统的结构示意图；
图3是本申请的一实施方式的用于线性运动的车辆姿态数据比较图；
图4是本申请的一实施方式的用于线性运动的车辆姿态数据比较图；
图5是本申请的一实施方式的用于转向运动的车辆姿态数据比较图；
图6是本申请的一实施方式的用于转向运动的车辆姿态数据比较图；
图7是本申请的一实施方式的用于S型运动的车辆姿态数据比较图；
图8是本申请的一实施方式的用于S型运动的车辆姿态数据比较图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1：
如图1所示一个方面提供一种基于多传感器惯性导航系统的高动态车辆姿态估计方法，其特征在于，包括以下步骤：
获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，得到观测姿态四元数。
获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值。
通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。
其中，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度包括以下公式：
fba＝fa-Va；其中，fba是车辆补偿线性运动加速度后的加速度，fa是加速度计的测量；Va是车辆的线性运动加速度；
g＝fa-(2ωie+ωeT)×V-fba，其中，g是重力加速度，也可以看作是姿态加速度fb。2ωie是车辆相对于地球的运动和地球的自转引起。此处省略了它；ωeT×V是车辆在地球表面的圆周运动引起的向心加速度，V是车辆对地线性运动加速度，从而：
fb＝fa-ωeT×V-fba。
在一些实施方式中，获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值的步骤包括：
Figure BDA0002573441260000071
其中，
Figure BDA0002573441260000072
是b坐标系相对于n坐标系的四元数；
Figure BDA0002573441260000073
是
Figure BDA0002573441260000074
的导数；
Figure BDA0002573441260000075
其中，ωx表示绕俯仰轴的角速度；ωy表示绕横滚轴的角速度；ωz表示绕偏航轴的角速度。
四元运动方程的矩阵形式表示为：
Figure BDA0002573441260000076
Figure BDA0002573441260000077
其中，T表示系统采样间隔；
Figure BDA0002573441260000078
使用一阶Runge-Kutta方法求解四元微分方程，可以得出离散时间模型作为：
Figure BDA0002573441260000079
其中，
qk-1表示k-1时刻的四元数，I为单位矩阵，设置初始四元数q(0)，并使用该方程递推在k时刻四元数的值qk，然后更新姿态矩阵。
在一些方式中，卡尔曼滤波过程包括对陀螺仪测量的修正以及加速度计测量的修正，
其中，陀螺仪测量模型为：
ωb＝Kω*ω+bω+Vω；其中，ωb是陀螺仪的测量值；Kω是比例系数矩阵；ω是假设不存在任何误差情况下，陀螺仪的实际角速度；bω是陀螺仪随机漂移矩阵；Vω是测量噪声矩阵，为零均值的高斯噪声，协方差为
Figure BDA0002573441260000081
其中，加速度计的测量模型为：
Figure BDA0002573441260000082
ab表示加速度计的测量；Ka表示比例因子矩阵；gn＝(0，0，-g)T表示地理坐标系下的重力加速度矢量，a表示重力加速度矢量；af表示车辆的线性加速度矩阵；ba表示偏置误差矩阵；Va表示测量噪声矩阵，为零均值高斯白噪声，协方差为
Figure BDA0002573441260000083
在一些实施方式中，通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息包括以下步骤：
确定系统模型：
Figure BDA0002573441260000084
其中：x(k)∈Rn是k时刻系统的状态向量，z(k)∈Rm是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rn和v(k)∈Rm是具有方差的关联过程噪声矢量Q(k)和R(k)；
系统状态向量由下式表示：
Figure BDA0002573441260000085
其中，
Figure BDA0002573441260000086
是姿态四元数，bwx bwy bwz是陀螺仪围绕三轴的漂移；
系统状态方程如下：
Figure BDA0002573441260000091
过程噪声协方差矩阵为
Figure BDA0002573441260000092
系统观测向量由下式表示：
Figure BDA0002573441260000093
其中，
Figure BDA0002573441260000094
是车辆坐标系中的三轴加速度测量值；
当车辆以恒定的速度静止行驶时，以下关系：
Figure BDA0002573441260000095
系统的观测方程如下：
Figure BDA0002573441260000096
对h(x(k)，k)求取Jacobi矩阵，可得系统量测矩阵为：
Figure BDA0002573441260000097
测量噪声协方差矩阵如下：
Figure BDA0002573441260000098
在一些实施方式中，状态更新包括：
初步估计：
计算初始x(0)值，设置过程噪声协方差Q(k)，测量噪声协方差R(k)和误差方差矩阵P(0)，
时间更新：
状态转移函数的一步预测：
Figure BDA0002573441260000101
计算状态矢量一步预测误差方差矩阵：
P(k，k-1)＝f(k，k-1) P(k-1)fT (k，k-1)+Q(k)；
计算卡尔曼增益矩阵：
K(k)＝P(k，k-1)HT (k)[H(k) P(k，k-1)HT (k)+R(k)]-1；
测量更新：
从状态转换函数获取的状态的一步预测：
Figure BDA0002573441260000102
在k时刻更新状态矢量估计：
Figure BDA0002573441260000103
更新状态误差协方差矩阵：
P(k)＝[I-K(k)H(k)]P(k，k-1)[I-K(k) H(k)]T+K(k) R(k)KT (k)。
实施例2：
如图2所示，提供一种应用前述的方法的系统，其特征在于，包括加速度计(1)、速度计(2)、陀螺仪(3)、处理器(4)和滤波器(5)，所述加速度计(1)、速度计(2)和陀螺仪(3)的数据输出端与所述处理器(4)的输入端相连，所述处理器(4)的数据输出端与所述滤波器(5)的输入端相连。
以上，需要说明的是：b系统是载体坐标系；n系统是导航坐标系。载体坐标系与导航坐标系之间的转换关系表现为：
Figure BDA0002573441260000104
载体的姿态角度由三个角度组成，围绕X轴的旋转是俯仰角，围绕Y轴的旋转是滚动角度，Z轴是偏航角度。这三个角度是θ、φ以及ψ。假设导航坐标系按Z-X-Y的顺序旋转载波坐标系。单独旋转角度以获得ψ-θ-ψ。
由欧拉角表示的转换矩阵：
Figure BDA0002573441260000105
Figure BDA0002573441260000106
四元数定义为以下方程：
q(q0 q1 q2 q3)＝q0+q1i+q2j+q3k；其中，q0是四元数的标准部分，q1，q2，q3是四元数的向量部分，i2＝j2k2＝-1。
四元数表示的转换矩阵如下：
Figure BDA0002573441260000111
由此得出三个姿态角度为：
Figure BDA0002573441260000112
实施例3
本文采用以ARM-CortexM3为核心的STM32的32位微处理器构建数据采集系统。惯性导航传感器采用BEWIS传感技术有限公司的BW-VG500。这是一种高性能MEMS惯性测量装置，集成了三轴加速度计和三轴陀螺仪。它可以精确测量车辆的参数。线性运动速度使用STM32模块读取车辆OBD系统的实际车速。在实验中，以100赫兹的频率对传感器测量的原始数据进行采样。惯性导航传感器通过STM32与车速传感器的采样点对齐。然后，使用串行端口将原始数据发送到PC。使用本申请中设计的扩展卡尔曼滤波器来处理PC上的数据，然后使用串行端口将原始数据发送到PC。最终，通过曲线描述获得结果。为了消除特殊情况，验证所建算法的有效性和普遍适用性。本实施例设计了三套实验：1)用于线性运动的车辆，如图3-图4所示；2)用于转向运动的车辆，如图5-图6所示；3)用于S型运动的车辆，如图7和图8所示。
本实施例将本申请的方法与两种姿态求解算法进行了比较：本申请的姿态求解方法基于多传感器扩展卡尔曼滤波器(M-EKF)。扩展卡尔曼滤波器姿态求解算法使用陀螺仪和加速度计(EKF)的总体布局。四元数姿态解算法使用陀螺仪(G-I)。通过三组实验比较了三种算法在不同情况下的性能。
如图3-图4所示，通过线性运动实验，本申请的多传感器EKF方法优于标准EKF算法和陀螺仪四元数法。结果表明，本申请的算法对线性加速度抑制处理有相当大的改进。它可以抵消振动对姿态的影响。
如图5-图6所示，多传感器EKF方法优于标准EKF算法和转弯运动试验中的陀螺仪四元数法。结果表明，本申请的算法有效地消除了圆周运动过程中的向心加速度。并能降低圆周运动对角速度的影响。
如图7-图8所示，从s型运动实验中获得。由于S型运动是复合运动，因此可被视为剧烈的驾驶测试。这种状态的本申请的多传感器EKF算法也优于标准的EKF算法和陀螺四元数法。本申请的算法能有效消除运动加速度对高动态运动时姿态估计的影响。
通过对实验数据的分析，可以看出，本文的算法有效地补偿了陀螺仪的漂移，消除了车辆的运动加速度。
以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，包括以下步骤：
获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，进而得到观测姿态四元数；
获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；
通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。
2.根据权利要求1所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，
所述利用车载速度计对加速度计数据进行补偿得到补偿后的加速度包括以下公式：
fba＝fa-Va；其中，fba是车辆补偿线性运动加速度后的加速度，fa是加速度计的测量；Va是车辆的线性运动加速度；
g＝fa-(2ωie+ωeT)×V-fba，其中，g是重力加速度，也可以看作是姿态加速度fb。2ωie是车辆相对于地球的运动和地球的自转引起，此处省略它；ωeT×V是车辆在地球表面的圆周运动引起的向心加速度，V是车辆对地线性运动加速度，从而：
fb＝fa-ωeT×V-fba。
3.根据权利要求2所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，
所述获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值的步骤包括：
Figure FDA0002573441250000011
其中，
Figure FDA0002573441250000012
是b坐标系相对于n坐标系的四元数；
Figure FDA0002573441250000013
是
Figure FDA0002573441250000014
的导数；
Figure FDA0002573441250000021
其中，ωx表示绕俯仰轴的角速度；ωy表示绕横滚轴的角速度；ωz表示绕偏航轴的角速度；
四元运动方程的矩阵形式表示为：
Figure FDA0002573441250000022
Figure FDA0002573441250000023
其中，T表示系统采样间隔；
Figure FDA0002573441250000024
使用一阶Runge-Kutta方法求解四元微分方程，可以得出离散时间模型作为：
Figure FDA0002573441250000025
qk-1表示k-1时刻的四元数。设置初始四元数q(0)，并使用该方程递推在k时刻四元数的值qk，然后更新姿态矩阵。
4.根据权利要求3所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，所述卡尔曼滤波过程包括对陀螺仪测量的修正以及加速度计测量的修正，
其中，陀螺仪测量模型为：
ωb＝Kω*ω+bω+Vω；其中，ωb是陀螺仪的测量值；Kω是比例系数矩阵；ω是陀螺仪的实际角度速度；bω是陀螺仪随机漂移矩阵；Vω是测量噪声矩阵，为零均值的高斯噪声，协方差为
Figure FDA0002573441250000026
其中，加速度计的测量模型为：
Figure FDA0002573441250000027
ab表示加速度计的测量；Ka表示标度系数矩阵；gn＝(0，0，-g)T表示地理坐标系下的重力加速度矢量，g表示重力加速度矢量；af表示车辆的线性加速度矩阵；ba表示偏置误差矩阵；Va表示测量噪声矩阵，为零均值高斯白噪声，协方差为
Figure FDA0002573441250000031
5.根据权利要求4所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，
所述通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息包括以下步骤：
确定系统模型：
Figure FDA0002573441250000032
其中：x(k)∈Rn是k时刻系统的状态向量，z(k)∈Rm是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rn和v(k)∈Rm是具有方差的关联过程噪声矢量Q(k)和R(k)；
系统状态向量由下式表示：
Figure FDA0002573441250000037
其中，
Figure FDA0002573441250000038
是姿态四元数，bwx bwy bwz是陀螺仪围绕三轴的漂移；
系统状态方程如下：
Figure FDA0002573441250000033
过程噪声协方差矩阵为
Figure FDA0002573441250000034
系统观测向量由下式表示：
Figure FDA0002573441250000035
其中，
Figure FDA0002573441250000036
是车辆坐标系中的三轴加速度测量值；
当车辆以恒定的速度静止行驶时，以下关系：
Figure FDA0002573441250000041
系统的观测方程如下：
Figure FDA0002573441250000042
测量噪声协方差矩阵如下：
Figure FDA0002573441250000043
。
6.根据权利要求5所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，
状态更新包括：
初步估计：
计算初始x(0)值，设置过程噪声协方差Q(k)，测量噪声协方差R(k)和误差方差矩阵P(0)，
时间更新：
状态转移函数的一步预测：
Figure FDA0002573441250000044
计算状态矢量一步预测误差方差矩阵：
P(k，k-1)＝f(k，k-1)P(k-1)fT (k，k-1)+Q(k)；
计算卡尔曼增益矩阵：
K(k)＝P(k，k-1)HT (k)[H(k)P(k，k-1)HT (k)+R(k)]-1；
测量更新：
从状态转换函数获取的状态的一步预测：
Figure FDA0002573441250000045
在k时刻更新状态矢量估计：
Figure FDA0002573441250000051
更新状态误差协方差矩阵：
P(k)＝[I-K(k)H(k)]P(k，k-1)[I-K(k)H(k)]T+K(k)R(k)KT (k) 。
7.根据权利要求5所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，还包括对h(x(k)，k)求取Jacobi矩阵，可得系统量测矩阵为：
Figure FDA0002573441250000052
8.一种应用权利要求1-7任一项所述的方法的系统，其特征在于，包括加速度计(1)、速度计(2)、陀螺仪(3)、处理器(4)和滤波器(5)，
所述加速度计(1)、速度计(2)和陀螺仪(3)的数据输出端与所述处理器(4)的输入端相连，
所述处理器(4)的数据输出端与所述滤波器(5)的输入端相连。
CN202010647018.6A 2019-12-18 2020-07-07 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 Pending CN111551174A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911309273 2019-12-18
CN2019113092733 2019-12-18

Publications (1)

Publication Number Publication Date
CN111551174A true CN111551174A (zh) 2020-08-18

Family

ID=72007107

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010647018.6A Pending CN111551174A (zh) 2019-12-18 2020-07-07 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统

Country Status (1)

Country Link
CN (1) CN111551174A (zh)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111928869A (zh) * 2020-10-10 2020-11-13 蘑菇车联信息科技有限公司 车辆运动轨迹估计方法、装置及电子设备
CN112284379A (zh) * 2020-09-17 2021-01-29 江苏大学 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法
CN112304330A (zh) * 2020-10-29 2021-02-02 腾讯科技（深圳）有限公司 展示车辆的行驶状态的方法以及电子设备
CN112363196A (zh) * 2020-10-20 2021-02-12 北京嘀嘀无限科技发展有限公司 车辆属性确定方法、装置、存储介质和电子设备
CN112519714A (zh) * 2020-11-18 2021-03-19 上海海洋大学 一种防货车转弯碾压行人的装置
CN113188505A (zh) * 2021-04-14 2021-07-30 湖南三一智能控制设备有限公司 姿态角度的测量方法、装置、车辆及智能臂架
CN113465628A (zh) * 2021-06-17 2021-10-01 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及系统
CN113936044A (zh) * 2021-12-17 2022-01-14 武汉锐科光纤激光技术股份有限公司 激光设备运动状态的检测方法、装置、计算机设备及介质
CN114459475A (zh) * 2022-02-28 2022-05-10 宁波赛福汽车制动有限公司 摩托车的车身姿态检测方法、系统、摩托车及存储介质
CN114578690A (zh) * 2022-01-26 2022-06-03 西北工业大学 一种基于多传感器的智能汽车自主组合控制方法
CN114593924A (zh) * 2022-03-08 2022-06-07 上海应用技术大学 一种地铁列车摇摆检测装置及检测方法
CN114646984A (zh) * 2022-03-18 2022-06-21 天津大学 一种用于复杂环境下的无人驾驶工程作业设备精确定位方法
CN114689027A (zh) * 2020-12-29 2022-07-01 苏州物图科技有限公司 一种车载设备倾斜角监测的可视化方法
CN117395691A (zh) * 2023-12-11 2024-01-12 泉州市三川通讯技术股份有限责任公司 一种弱信号环境下的车载终端通信优化方法
CN117755308A (zh) * 2023-12-27 2024-03-26 上海朗尚传感技术有限公司 一种基于车载传感器的车辆状态监测方法
CN114646984B (zh) * 2022-03-18 2024-10-22 天津大学 一种用于复杂环境下的无人驾驶工程作业设备精确定位方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011227017A (ja) * 2010-04-23 2011-11-10 Univ Of Tokyo 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN103148868A (zh) * 2013-02-22 2013-06-12 哈尔滨工程大学 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法
CN104635251A (zh) * 2013-11-08 2015-05-20 中国地质大学（北京） 一种ins/gps组合定位定姿新方法
CN106225784A (zh) * 2016-06-13 2016-12-14 国家海洋局第二海洋研究所 基于低成本多传感器融合行人航位推算方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108225308A (zh) * 2017-11-23 2018-06-29 东南大学 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011227017A (ja) * 2010-04-23 2011-11-10 Univ Of Tokyo 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN103148868A (zh) * 2013-02-22 2013-06-12 哈尔滨工程大学 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法
CN104635251A (zh) * 2013-11-08 2015-05-20 中国地质大学（北京） 一种ins/gps组合定位定姿新方法
CN106225784A (zh) * 2016-06-13 2016-12-14 国家海洋局第二海洋研究所 基于低成本多传感器融合行人航位推算方法
CN108225308A (zh) * 2017-11-23 2018-06-29 东南大学 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
WEIXUAN DING 等: ""Tricycle Attitude Estimation and Turn Control based on MEMS Sensing Technology"", 《2018 IEEE 1ST INTERNATIONAL CONFERENCE ON MICRO/NANO SENSORS FOR AI, HEALTHCARE, AND ROBOTICS (NSENS)》 *
YENTZE KO等: ""Moving Vehicle Attitude Tracking Algorithm based on MEMS Inertial Navigation System"", 《2018 IEEE 1ST INTERNATIONAL CONFERENCE ON MICRO/NANO SENSORS FOR AI, HEALTHCARE, AND ROBOTICS (NSENS)》 *
无: ""基于多传感器惯性导航系统的动态车辆姿态算法"", 《HTTP://WWW.BWSENSING.COM.CN/NEWS-DETAILS-377.HTML》 *
无: "基于多传感器惯性导航系统的动态车辆姿态算法", 《HTTP://WWW.BWSENSING.COM.CN/NEWS-DETAILS-377.HTML》 *
陈永冰 等: "《惯性导航原理》", 30 November 2007, 国防工业出版社 *

Cited By (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112284379A (zh) * 2020-09-17 2021-01-29 江苏大学 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法
CN112284379B (zh) * 2020-09-17 2023-09-22 江苏大学 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法
CN111928869A (zh) * 2020-10-10 2020-11-13 蘑菇车联信息科技有限公司 车辆运动轨迹估计方法、装置及电子设备
CN112363196A (zh) * 2020-10-20 2021-02-12 北京嘀嘀无限科技发展有限公司 车辆属性确定方法、装置、存储介质和电子设备
CN112363196B (zh) * 2020-10-20 2023-10-31 北京嘀嘀无限科技发展有限公司 车辆属性确定方法、装置、存储介质和电子设备
CN112304330A (zh) * 2020-10-29 2021-02-02 腾讯科技（深圳）有限公司 展示车辆的行驶状态的方法以及电子设备
CN112304330B (zh) * 2020-10-29 2024-05-24 腾讯科技（深圳）有限公司 展示车辆的行驶状态的方法以及电子设备
CN112519714A (zh) * 2020-11-18 2021-03-19 上海海洋大学 一种防货车转弯碾压行人的装置
CN114689027A (zh) * 2020-12-29 2022-07-01 苏州物图科技有限公司 一种车载设备倾斜角监测的可视化方法
CN113188505A (zh) * 2021-04-14 2021-07-30 湖南三一智能控制设备有限公司 姿态角度的测量方法、装置、车辆及智能臂架
CN113465628A (zh) * 2021-06-17 2021-10-01 杭州鸿泉物联网技术股份有限公司 惯性测量单元数据补偿方法及系统
CN113936044B (zh) * 2021-12-17 2022-03-18 武汉锐科光纤激光技术股份有限公司 激光设备运动状态的检测方法、装置、计算机设备及介质
CN113936044A (zh) * 2021-12-17 2022-01-14 武汉锐科光纤激光技术股份有限公司 激光设备运动状态的检测方法、装置、计算机设备及介质
CN114578690A (zh) * 2022-01-26 2022-06-03 西北工业大学 一种基于多传感器的智能汽车自主组合控制方法
CN114578690B (zh) * 2022-01-26 2023-07-21 西北工业大学 一种基于多传感器的智能汽车自主组合控制方法
CN114459475A (zh) * 2022-02-28 2022-05-10 宁波赛福汽车制动有限公司 摩托车的车身姿态检测方法、系统、摩托车及存储介质
CN114593924A (zh) * 2022-03-08 2022-06-07 上海应用技术大学 一种地铁列车摇摆检测装置及检测方法
CN114646984A (zh) * 2022-03-18 2022-06-21 天津大学 一种用于复杂环境下的无人驾驶工程作业设备精确定位方法
CN114646984B (zh) * 2022-03-18 2024-10-22 天津大学 一种用于复杂环境下的无人驾驶工程作业设备精确定位方法
CN117395691A (zh) * 2023-12-11 2024-01-12 泉州市三川通讯技术股份有限责任公司 一种弱信号环境下的车载终端通信优化方法
CN117395691B (zh) * 2023-12-11 2024-03-01 泉州市三川通讯技术股份有限责任公司 一种弱信号环境下的车载终端通信优化方法
CN117755308A (zh) * 2023-12-27 2024-03-26 上海朗尚传感技术有限公司 一种基于车载传感器的车辆状态监测方法
CN117755308B (zh) * 2023-12-27 2024-06-18 上海朗尚传感技术有限公司 一种基于车载传感器的车辆状态监测方法

Similar Documents

Publication Publication Date Title
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN116067370B (zh) 一种imu姿态解算方法及设备、存储介质
CN112683269B (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN109813308A (zh) 姿态估计方法、装置及计算机可读存储介质
CN108981694A (zh) 基于小波神经网络与ekf的姿态解算方法及系统
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN110440797A (zh) 车辆姿态估计方法及系统
CN113218391A (zh) 一种基于ewt算法的姿态解算方法
CN112066985A (zh) 一种组合导航系统初始化方法、装置、介质及电子设备
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN114608517B (zh) 一种应用于农用无人机植保作业的姿态解算方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN113325865A (zh) 一种无人机控制方法、控制装置及控制系统
CN116182871B (zh) 一种基于二阶混合滤波的海缆探测机器人姿态估计方法
CN113432612A (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
CB03 Change of inventor or designer information

Inventor after: Lai Yuyou

Inventor after: Geng Xiaodong

Inventor after: Shi Guangdie

Inventor after: Wang Chunbo

Inventor after: Jin Yufeng

Inventor before: Lai Yuyou

Inventor before: Geng Xiaodong

Inventor before: Shi Guangdie

Inventor before: Wang Chunbo

Inventor before: Wu Zhigang

Inventor before: Xu Kaiming

Inventor before: Jin Yufeng

CB03 Change of inventor or designer information
RJ01 Rejection of invention patent application after publication

Application publication date: 20200818

RJ01 Rejection of invention patent application after publication