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