CN107843255A - 面向运动再现的工程车辆行驶姿态测量系统及方法 - Google Patents

Abstract

一种工程车辆行驶姿态测量系统及方法，所述系统包括陀螺仪、加速度计和磁力计。所述方法内容是：根据加速度计测量值判断车辆运动状态，然后根据车辆运动状态确定权重矩阵，通过调整权重矩阵来限制车辆运动加速度的干扰；利用加速度计和磁力计信息构建关于姿态四元数的非线性方程组，融合加速度计和磁力计测量数据；再根据车辆运动状态确定滤波参数，实现互补滤波器截止频率的自适应；最终，将陀螺仪测量数据、加速度计和磁力计测量数据融合，保证静态和动态下姿态测量的精度。本发明可实现工程车辆在行驶时的姿态测量，结合振动模拟试验台进行运动再现，可准确地模拟出实际路况的振动情况，从而大幅度提高实验效率、降低试验费用和减少环境污染问题。

Description

面向运动再现的工程车辆行驶姿态测量系统及方法

技术领域

本发明属于姿态测量领域，具体涉及一种面向运动再现的工程车辆行驶姿态测量系统及方法。

背景技术

工程车辆载重量大、行驶速度高，主驾驶座椅在车辆行驶过程中易受强振动和冲击而损坏。工程车辆主驾驶座椅振动疲劳采用实车路试和专用设备测试的方法。实车路试存在成本高、污染环境、试验效率低和对驾驶人员健康伤害大等问题，很难推广使用。采用专用设备测试的方法需采集实车行驶姿态数据，在专用设备上复现该数据，完成座椅的振动疲劳试验。

单自由度振动试验台因技术成熟，结构简单，在座椅振动试验中使用最为广泛，但其振动环境复现能力有限。近年来，多轴振动试验系统在道路模拟、船舶、航天、地震研究等领域开始应用，但试验系统的激励信号多为随机信号或根据载荷功率谱由计算机生成，并不是真实环境中的六自由度振动信息。准确获取座椅受到的振动激励是对座椅振动环境模拟的前提，直接影响了振动试验系统的测试效果。

采用融合陀螺仪、加速度计和磁力计数据采集方案，应用传统四元数方法进行姿态解算，固然可以取得高精度的姿态测试结果，但该方案仅对车辆静态和低动态工况有效，在高动态工况时误差较大，参见中国专利名称为《一种农业机械车辆姿态测量方法及测量模块》(申请号为201511033913.4)。针对以上测量系统，急需解决针对各传感器特点，建立有效姿态解算算法融合测试数据，使各传感器性能互补，实现车辆姿态在静态和动态工况时的高精度测量。

发明内容

为解决现有工程车辆行驶姿态解算方法动态精度低的问题，本发明提出一种面向运动再现的工程车辆行驶姿态测量系统及方法，采用融合陀螺仪、加速度计和磁力计数据采集方案，由车辆运动状态确定权重矩阵和滤波参数，建立有效姿态解算算法融合测试数据，使各传感器性能实现互补，实现车辆姿态在静态和动态时的高精度测量。

本发明为达到上述目的，采取如下技术方案：

一种面向运动再现的工程车辆行驶姿态测量系统，所述测量系统包括陀螺仪、加速度计和磁力计；

所述陀螺仪用于测量工程车辆的角速度，根据陀螺仪测得的角速度对时间积分得到工程车辆的航向角、俯仰角和横滚角；

所述加速度计用于测量工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，从而确定权重矩阵和滤波参数；并将加速度计测量信息与磁力计测量信息融合得到工程车辆的航向角、俯仰角和横滚角；

所述磁力计用于测量地球磁场强度的大小，根据磁力计测得的磁场强度大小融合加速度计测量信息得到工程车辆的航向角、俯仰角和横滚角。

本发明的另外一个目的在于提供一种面向运动再现的工程车辆行驶姿态测量方法，其要解决当前车辆行驶姿态测量不准确的问题。

一种应用于面向运动再现的工程车辆行驶姿态测量系统的测量方法，该方法内容包括如下步骤：

Step1：陀螺仪测量得到的角速度信息，根据所述角速度信息计算得到工程车辆当前的航向角、俯仰角和横滚角；

Step2：磁力计测量到的地球磁场强度的大小，得到车辆行驶过程中车辆坐标系内地磁矢量的估计值；

Step3：加速度计测量到的工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，再由车辆运动状态来确定权重矩阵和滤波参数；

Step4：在Step1解算得到的航向角、俯仰角和横滚角记为姿态量的初始值，获取陀螺仪测量得到的角速度信息，并通过四元数姿态解算方法对姿态量的初始值进行更新，得到姿态量更新值；

Step5：在Step3中获得车辆运动状态，计算权重矩阵；由Step2和Step3测量信息构建关于姿态四元数的非线性方程组，并利用Gauss-Newton法求解，融合Step2磁力计和Step3加速度计测量的数据，解算得到工程车辆的航向角、俯仰角和横滚角；

Step6：在Step3中获得车辆运动状态，计算滤波参数，利用自适应滤波参数实现互补滤波；解算Step4和Step5得到的工程车辆的航向角、俯仰角和横滚角，最终将陀螺仪测量数据、加速度计和磁力计测量数据融合，保证静态和动态下姿态测量的精度。

本发明可实现工程车辆在行驶时的姿态测量，结合振动模拟试验台进行运动再现，可准确地模拟出实际路况的振动情况，从而大幅度提高实验效率、降低试验费用和减少环境污染问题。

附图说明

图1为本发明实施运动再现的工程车辆座椅振动试验台的示意图；

图2为本发明工程车辆行驶姿态测量原理的示意图；

图3为本发明姿态解算坐标系的示意图；

图4为本发明工程车辆行驶姿态测量方法的流程示意图。

具体实施方式

下面结合附图以及具体实施方式，对本发明做进一步描述：

如图1所示为本发明实施运动再现的工程车辆座椅振动试验台的示意图。运用本发明采集处理的数据信息，可在该试验台实现沿X轴、Y轴和Z轴的平移和绕X轴、Y轴和Z轴转动的运动再现。俯仰角为工程车辆坐标系的X轴与水平面的夹角，横滚角为工程车辆坐标系的Z轴与通过车辆坐标系的X轴的铅垂面间的夹角，航向角为车辆坐标系X轴在水平面上投影与地面坐标系X轴之间的夹角。也即图3所示，以X轴为旋转轴转动的角度为横滚角，以Y轴为旋转轴转动的角度为俯仰角，以Z轴为旋转轴转动的角度为航向角。

一种面向运动再现的工程车辆行驶姿态测量系统，该测量系统包括陀螺仪、加速度计和磁力计；

所述陀螺仪用于测量工程车辆的角速度，根据陀螺仪测得的角速度对时间积分得到工程车辆的航向角、俯仰角和横滚角；

所述加速度计用于测量工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，从而确定权重矩阵和滤波参数；并将加速度计测量信息与磁力计测量信息融合得到工程车辆的航向角、俯仰角和横滚角；

所述磁力计用于测量地球磁场强度的大小，根据磁力计测得的磁场强度大小融合加速度计测量信息得到工程车辆的航向角、俯仰角和横滚角。

为解决当前车辆行驶姿态测量不准确的问题，本发明还提供一种面向运动再现的工程车辆行驶姿态测量方法，记录工程车辆行驶过程中的姿态信息，将记录信息在振动模拟试验台进行运动再现，用于主驾驶座椅的振动疲劳试验。

根据加速度计测量值判断车辆运动状态，然后根据车辆运动状态确定权重矩阵，通过调整权重矩阵来限制车辆运动加速度的干扰；利用加速度计和磁力计信息构建关于姿态四元数的非线性方程组，融合加速度计和磁力计测量数据；再根据车辆运动状态确定滤波参数，实现互补滤波器截止频率的自适应；最终，将陀螺仪测量数据、加速度计和磁力计测量数据融合，保证静态和动态下姿态测量的精度。本发明方法的详细内容如下：

Step1：陀螺仪测量得到的角速度信息，根据所述角速度信息计算得到工程车辆当前的航向角、俯仰角和横滚角；

Step2：磁力计测量到的地球磁场强度的大小，得到车辆行驶过程中，车辆坐标系内地磁矢量的估计值；

Step3：加速度计测量到的工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，再由车辆运动状态来确定权重矩阵和滤波参数；

(1)判断车辆运动状态

1)静止状态

由于测量误差的存在，给定误差上限σ₀(σ₀为一个充分小的正实数)。当加速度测量值大小满足式(1)时，认为车辆处于静止状态。

考虑到加速度计测量、磁力计测量和规范化四元数方程的单位不一致，为了保证各方程所占权重一致，权重矩阵W的参数值按下式设定：

其中，G为当地重力加速度大小，H为地磁场强度大小。

滤波器参数β_f选取为0.61，对应截止频率5Hz。

2)低加速度状态

给定正实数σ₁，当加速度计测量值满足式(3)时，认为车辆处于低加速度状态。

当车辆存在较小运动加速度时，虽然加速度计会引入姿态测量误差，但误差比较小的情况下，加速度计测量值在姿态融合过程中依然起到有益作用，不能完全忽略。此时权重矩阵参数按下式设定：

式中，

滤波器参数β_f的值应该适当增大，此时令β＝0.88，对应截止频率1Hz。

3)高加速度状态

当加速度计测量值满足式(5)时，认为车辆处于高加速度状态。

此时，加速度计测量值对姿态测量完全没有意义。将权重矩阵中λ_a置零，λ_m和λ_Q不变。减小截止频率，令其等于0.5Hz，此时β_f＝0.94，

综上所述，权重矩阵及滤波器参数的调整方案如表1所示。

表1参数调整方案

(2)权重矩阵

当车辆存在运动加速度时，加速度计的测量值是重力加速度和运动加速度的矢量和，此时加速度计测量值的模不再是重力加速度大小，因此，根据加速度测量值的模||a^b||可以判断车辆运动状态。为了限制车辆运动加速度对数据融合结果的影响，引入权重矩阵W对四元数方程组进行修正。W为对角矩阵，其中包含三个参数λ_a,λ_m,λ_Q，定义如下：

式中，I₃×₃为单位矩阵，λ_a表示加速度计测量值所占权重，λ_m表示磁力计测量值所占权重，λ_Q表示规范化四元数方程所占权重。

考虑四元数方程组中各方程的权重后，关于q₀,q₁,q₂,q₃的非线性方程组为：

F_a(x)＝Wf_a＝[λ₁f_a1,λ₂f_a2,…,λ₇f_a7]^T＝0 (7)

其中，x＝[q₀,q₁,q₂,q₃]^T。

该方程组的求解转化为加权最小二乘问题：

利用Gauss-Newton迭代法对上式求解，即可得到由加速度计和磁力计测量值得到的姿态四元数Q_a。

(3)滤波器参数

滤波器参数β_f的选取决定了互补滤波的截止频率。当姿态信号频率高于截止频率时，系统姿态输出由陀螺仪主导，当信号频率低于截止频率时，系统输出由加速度计和磁力计融合结果主导。恰当地选择β_f值可以使系统具有合理的截止频率，从而抑制低频陀螺仪漂移和高频加速度计、磁力计噪声。

互补滤波器的数字化实现过程为：

式中：为互补滤波器输出的姿态最优估计，β_f为互补滤波器参数，β_f与滤波器截止频率f_T的关系为：

Step4：在Step1解算得到的航向角、俯仰角和横滚角记为姿态量的初始值，获取陀螺仪测量得到的角速度信息，并通过四元数姿态解算方法对姿态量的初始值进行更新，得到姿态量更新值；

Step5：在Step3中获得车辆运动状态，计算权重矩阵。由Step2和Step3测量信息构建关于姿态四元数的非线性方程组，并利用Gauss-Newton法求解，融合Step2磁力计和Step3加速度计测量数据，解算得到工程车辆的航向角、俯仰角和横滚角；

Step6：在Step3中获得车辆运动状态，计算滤波参数，利用自适应滤波参数实现互补滤波。解算Step4和Step5得到的工程车辆的航向角、俯仰角和横滚角，最终将陀螺仪测量数据、加速度计和磁力计测量数据融合，保证静态和动态下姿态测量的精度。

Claims (2)

1.一种面向运动再现的工程车辆行驶姿态测量系统，其特征在于：所述测量系统包括陀螺仪、加速度计和磁力计；

所述陀螺仪用于测量工程车辆的角速度，根据陀螺仪测得的角速度对时间积分得到工程车辆的航向角、俯仰角和横滚角；

所述加速度计用于测量工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，从而确定权重矩阵和滤波参数；并将加速度计测量信息与磁力计测量信息融合得到工程车辆的航向角、俯仰角和横滚角；

所述磁力计用于测量地球磁场强度的大小，根据磁力计测得的磁场强度大小融合加速度计测量信息得到工程车辆的航向角、俯仰角和横滚角。

2.一种应用于面向运动再现的工程车辆行驶姿态测量系统的测量方法，其特征在于，该方法内容包括如下步骤：

Step1：陀螺仪测量得到的角速度信息，根据所述角速度信息计算得到工程车辆当前的航向角、俯仰角和横滚角；

Step2：磁力计测量到的地球磁场强度的大小，得到车辆行驶过程中车辆坐标系内地磁矢量的估计值；

Step3：加速度计测量到的工程车辆加速度的大小，根据加速度大小来判断车辆运动状态，再由车辆运动状态来确定权重矩阵和滤波参数；

Step4：在Step1解算得到的航向角、俯仰角和横滚角记为姿态量的初始值，获取陀螺仪测量得到的角速度信息，并通过四元数姿态解算方法对姿态量的初始值进行更新，得到姿态量更新值；

Step5：在Step3中获得车辆运动状态，计算权重矩阵；由Step2和Step3测量信息构建关于姿态四元数的非线性方程组，并利用Gauss-Newton法求解，融合Step2磁力计和Step3加速度计测量的数据，解算得到工程车辆的航向角、俯仰角和横滚角；

Step6：在Step3中获得车辆运动状态，计算滤波参数，利用自适应滤波参数实现互补滤波；解算Step4和Step5得到的工程车辆的航向角、俯仰角和横滚角，最终将陀螺仪测量数据、加速度计和磁力计测量数据融合，保证静态和动态下姿态测量的精度。