[发明专利]一种车辆姿态安全预警方法及系统

权利要求书

1.一种车辆姿态安全预警方法，其特征在于，包括以下步骤：

S1：利用MEMS陀螺仪测量车辆的角速率信息，利用加速度计测量车辆的第一加速度信息，利用GPS输出信息计算得到第二加速度信息；

S2：选取姿态误差角、陀螺常值偏移和加速度计常值零偏作为状态向量，利用卡尔曼滤波基本方程对所述状态向量进行估计，获得估计的状态向量；

S3：使用估计的状态向量中的姿态误差角修正姿态矩阵，获得修正后的姿态矩阵，根据修正后的姿态矩阵解算出俯仰角、横滚角和航向角；将所述角速率信息扣除所述估计的状态向量中的所述陀螺常值偏移得到角速率值；根据所述第一加速度信息、第二加速度信息及所述估计的状态向量中的所述加速度计常值零偏得到加速度值；

S4：根据所述俯仰角、横滚角、航向角、角速率值及加速度值判断车辆姿态，并输出所述俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态；

在所述步骤S2之前还包括以下步骤：

T1：对所述角速率信息和第一加速度信息进行标定处理，获得标定后的角速率信息和第一加速度信息；

T2：将所述标定后的第一加速度信息与根据GPS输出信息计算得到的所述第二加速度信息进行对比，计算两者的差值，获得加速度匹配信息，并对所述标定后的角速率信息进行正交标定；

T3：判断GPS是否可用，若GPS不可用，则转至步骤S2；若GPS可用，则判断车辆是否转弯，若转弯则转至步骤S2，若不转弯则将此刻所述加速度计测量得到的加速度信息与根据GPS输出信息计算得到的加速度信息进行对比，计算两者的差值，获得加速度补偿信息，再转至步骤S2。

2.根据权利要求1所述的车辆姿态安全预警方法，其特征在于，所述步骤T3，还包括以下步骤：

若判断车辆转弯，则消除转弯时X轴陀螺输出的角速率，同时消除转弯时Y轴陀螺输出的角速率；

在消除转弯时X轴陀螺的输出的角速率之后，再判断车辆是否处于转弯状态，若车辆处于转弯状态，则对X轴陀螺输出的角速率信息进行积分，得到俯仰角，并输出；若车辆没有处于转弯状态，则转至步骤S2；

在消除转弯时Y轴陀螺输出的角速率之后，对Y轴陀螺输出的角速率进行积分得到第一横滚角，同时根据加速度计的输出估计出第二横滚角，之后计算所述第一横滚角与所述第二横滚角的差值；并判断GPS是否可用，若GPS不可用且所述第一横滚角与所述第二横滚角的差值大于预设阈值时，则对Y轴陀螺输出的角速率信息进行积分，估计当前的横滚角，并输出；若GPS可用，则转至所述步骤S2。

3.根据权利要求1所述的车辆姿态安全预警方法，其特征在于，所述步骤S2具体包括：

S2-1：选定滤波状态向量X(t)：姿态误差角φ＝[φ_E φ_N φ_U]^T、陀螺常值漂移ε^b＝[ε_xε_y ε_z]^T、加速度计常值零偏即

S2-2：确立系统状态转移方程：

其中，X'(t)为状态向量X(t)的导数，φ'、ε'^b和▽'^b依次为姿态误差角、陀螺常值漂移、加速度计常值零偏的导数；

是9×9维的连续系统的状态转移矩阵；

0代表3×3维的零阵是3×3维的从机体坐标系b系到平台坐标系n系的方向余弦矩阵；w(t)为系统的激励白噪声过程，是3维向量，G(t)是9×3维的噪声转移矩阵；

S2-3：对所述系统状态转移方程进行离散化处理得到离散化后的9×9维的状态一步转移矩阵Φ_k,k-1，Φ_k,k-1＝I+TF(t)，其中，T为滤波周期，I为9×9维的对角线元素全部为1，其余元素全部为0的单位矩阵；

S2-4：根据姿态矩阵和所述加速度匹配信息，或者根据姿态矩阵和所述加速度补偿信息确定量测向量，并根据所述量测向量与状态向量确定量测方程；

S2-5：根据所述状态向量、状态转移方程及量测方程，并利用所述滤波基本方程对被估计的所述状态向量进行求解，得到估计的状态向量。