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

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

【技术实现步骤摘要】
基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
本申请涉及高动态车辆姿态估计
，尤其涉及一种基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统。
技术介绍
随着现代汽车工业的发展。智能驾驶系统已成为当今汽车行业的发展趋势。人们对智能驾驶系统的需求越来越高。这些电子系统大多需要基于汽车姿势和位置信息的反馈。因此，智能驾驶车辆系统的发展基于准确、稳定、实时的姿态和导航信息。由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。在一些实施方式中，获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值的步骤包括：其中，是b坐标系相对于n坐标系的四元数；是的导数；其中，ωx表示绕俯仰轴的角速度；ωy表示绕横滚轴的角速度；ωz表示绕偏航轴的角速度；四元运动方程的矩阵形式表示为：其中，T表示系统采样间隔；使用一阶Runge-Kutta方法求解四元微分方程，可以得出离散时间模型作为：qk-1表示k-1时刻的四元数。设置初始四元数q(0)，并使用该方程递推在k时刻四元数的值qk，然后更新姿态矩阵。在一些方式中，卡尔曼滤波过程包括对陀螺仪测量的修正以及加速度计测量的修正，其中，陀螺仪测量模型为：ωb＝Kω*ω+bω+Vω；其中，ωb是陀螺仪的测量值；Kω是比例系数矩阵；ω是陀螺仪的实际角速度；bω是陀螺仪随机漂移矩阵；Vω是测量噪声矩阵，为零均值的高斯噪声，协方差为其中，加速度计的测量模型为：ab表示加速度计的测量；Ka表示标度系数矩阵；gn＝(0，0，-g)T表示地理坐标系下的重力加速度矢量，g表示重力加速度矢量；af表示车辆的线性加速度矩阵；ba表示偏置误差矩阵；Va表示测量噪声矩阵，为零均值高斯白噪声，协方差为在一些实施方式中，通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息包括以下步骤：确定系统模型：其中：x(k)∈Rn是k时刻系统的状态向量，z(k)∈Rm是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rn和v(k)∈Rm是具有方差的关联过程噪声矢量Q(k)和R(k)；系统状态向量由下式表示：其中，是姿态四元数，bwxbwybwz是陀螺仪围绕三轴的漂移；系统状态方程如下：过程噪声协方差矩阵为系统观测向量由下式表示：其中，是车辆坐标系中的三轴加速度测量值；当车辆以恒定的速度静止行驶时，以下关系：系统的观测方程如下：测量噪声协方差矩阵如下：在一些实施方式中，状态更新包括：初步估计：计算初始x(0)值，设置过程噪声协方差Q(k)，测量噪声协方差R(k)和误差方差矩阵P(0)，时间更新：状态转移函数的一步预测：计算状态矢量一步预测误差方差矩阵：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；测量更新：从状态转换函数获取的状态的一步预测：在k时刻更新状态矢量估计：更新状态误差协方差矩阵：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矩阵，可得系统量测矩阵为：根据本专利技术的另一个方面，提供一种应用前述的方法的系统，其特征在于，包括加速度计、速度计、陀螺仪、处理器和滤波器，所述加速度计、速度计和陀螺仪的数据输出端与所述处理器的输入端相连，所述处理器的数据输出端与所述滤波器的输入端相连。另外，在本技术方案中，凡未作特别说明的，均可通过采用本领域中的常规手段来实现本技术方案。本申请方案与现有技术相比，具有以下有益效果：本申请技术方案先用陀螺仪输出的角速度值，通过四元数微分方程得到估计姿态四元数。对车载速度计和加速度计融合后的加速度进行运动加速度抑制处理，消除运动加速度对姿态估计的影响，得到观测姿态四元数。建立基于四元数的姿态估计滤波方程，从而完成了车辆姿态的高精度解算。并且能补足在没有GPS状态下的导航信息输出，提供车辆的自主导航系统的必备信息。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本申请一实施例提供的种基于多传感器惯性导航系统的高动态车辆姿态估计方法的流程图。图2是本申请另一实施例提供的应用基于多传感器惯性导航系统的高动态车辆姿态估计方法系统的结构示意图；图3是本申请的一实施方式的用于线性运动的车辆姿态数据比较图；图4是本申请的一实施方式的用于线性运本文档来自技高网...

【技术保护点】
1.一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，包括以下步骤：/n获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，进而得到观测姿态四元数；/n获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；/n通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。/n

【技术特征摘要】
20191218 CN 20191130927331.一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，包括以下步骤：
获取车载速度计和加速度计数据，利用车载速度计对加速度计数据进行补偿得到补偿后的加速度，通过运动加速度抑制处理得到加速度，进而得到观测姿态四元数；
获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值；
通过扩展卡尔曼进行多传感器的信息融合，输出最终的姿态角信息。


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所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，
所述获取陀螺仪数据，利用陀螺仪输出的角速度值，通过四元数微分方程得到四元数的状态估计值的步骤包括：



其中，是b坐标系相对于n坐标系的四元数；是的导数；

其中，ωx表示绕俯仰轴的角速度；ωy表示绕横滚轴的角速度；ωz表示绕偏航轴的角速度；
四元运动方程的矩阵形式表示为：




其中，T表示系统采样间隔；



使用一阶Runge-Kutta方法求解四元微分方程，可以得出离散时间模型作为：



qk-1表示k-1时刻的四元数。设置初始四元数q(0)，并使用该方程递推在k时刻四元数的值qk，然后更新姿态矩阵。


4.根据权利要求3所述的一种基于多传感器惯性导航系统的高动态车辆姿态计算方法，其特征在于，所述卡尔曼滤波过程包括对陀螺仪测量的修正以及加速度计测量的修正，
其中，陀螺仪测量模型为：
ωb＝Kω*ω+bω+Vω；其中，ωb是陀螺仪的测量值；Kω是比例系数矩阵；ω是陀螺仪的实际角度速度；bω是陀螺仪随机漂移矩阵；Vω是测量噪声矩阵，为零均值的高斯噪声，协方差为
其中，加速度计的测量模型为：

ab表示加速度计的测量；Ka表示标度系数矩阵；gn＝(0，0，-g)T表示地理坐标系下的重力加速度矢量，g表示重力加速度矢量；af表示...

【专利技术属性】
技术研发人员：赖郁友，耿晓东，时广轶，王春波，吴志刚，徐开明，金玉丰，
申请(专利权)人：无锡北微传感科技有限公司，北京大学深圳研究生院，
类型：发明
国别省市：江苏;32