一种车辆姿态安全预警方法及系统技术方案

本发明专利技术涉及一种车辆姿态安全预警方法及系统，其中的预警方法包括：利用MEMS陀螺仪测量车辆的角速率信息，利用加速度计测量车辆的第一加速度信息，利用GPS输出信息计算得到第二加速度信息；选取姿态误差角、陀螺常值偏移和加速度计常值零偏作为状态向量，利用卡尔曼滤波基本方程对状态向量进行估计，获得估计的状态向量；使用估计的状态向量中的姿态误差角修正姿态矩阵，获得修正后的姿态矩阵，根据修正后的姿态矩阵解算出俯仰角、横滚角和航向角；根据俯仰角、横滚角、航向角、角速率值及加速度值判断车辆姿态，并输出俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态。其通过稳定的姿态解算处理过程，可真实准确地反映车辆姿态。

【技术实现步骤摘要】
一种车辆姿态安全预警方法及系统
本专利技术涉及车辆姿态测量系统，尤其涉及一种车辆姿态安全预警方法及系统。
技术介绍
市场原有的车辆姿态测量装置是一种重力悬浮机械装置，仪表盘内悬浮圆球跟随车体惯性发生延后转动。这种装置存在诸多缺陷，如：显示严重延迟，不利于观察，会导致驾驶员分神，造成潜在安全隐患，市场接受程度低。
技术实现思路
为克服现有技术存在的上述技术问题，本专利技术提供了一种车辆姿态安全预警方法，其通过稳定的姿态解算处理过程，可真实准确地反映车辆姿态。本专利技术解决上述技术问题的技术方案如下：一种车辆姿态安全预警方法，其包括以下步骤：S1：利用MEMS陀螺仪测量车辆的角速率信息，利用加速度计测量车辆的第一加速度信息，利用GPS输出信息计算得到第二加速度信息；S2：选取姿态误差角、陀螺常值偏移和加速度计常值零偏作为状态向量，利用卡尔曼滤波基本方程对所述状态向量进行估计，获得估计的状态向量；S3：使用估计的状态向量中的所述姿态误差角修正姿态矩阵，获得修正后的姿态矩阵，根据所述修正后的姿态矩阵解算出俯仰角、横滚角和航向角；将所述角速度信息扣除所述估计的状态向量中的所述陀螺常值偏移得到角速率值；根据所述第一加速度信息、第二加速度信息及所述估计的状态向量中的所述加速度计常值零偏得到加速度值；S4：根据所述俯仰角、横滚角、航向角、角速率值及加速度值判断车辆姿态，并输出所述俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态。本专利技术的有益效果是：利用稳定的姿态解算处理方法，修正姿态矩阵，达到提高姿态解算精度的目的；在陀螺采样得到的角速率中扣除滤波估计得到的陀螺常值漂移可以减小姿态解算过程中使用的角速率数据的误差，提高姿态解算精度；在加速度计输出中扣除滤波估计得到的加速度计常值零偏可以减小加速度计输出数据的误差，从而提高滤波过程中使用的量测向量的精度。采用选定的状态方程和量测方程，进行相应的参数配置，在执行完滤波估计后对相应的变量进行修正可以有效提高导航过程中姿态解算的精度，从而可真实准确地反映车辆姿态，进行可进行准确的车辆姿态安全预警。在上述技术方案的基础上，本专利技术还可以做如下改进。进一步，在所述步骤S2之前还包括以下步骤：T1：对所述角速率信息和第一加速度信息进行标定处理，获得标定后的角速率信息和第一加速度信息；T2：将所述标定后的第一加速度信息与根据GPS输出信息计算得到的所述第二加速度信息进行对比，计算两者的差值，获得加速度匹配信息，并对所述标定后的角速率信息进行正交标定；T3：判断GPS是否可用，若GPS不可用，则转至步骤S2；若GPS可用，则判断车辆是否转弯，若转弯则转至步骤S2，若不转弯则将此刻所述加速计测量得到的加速度信息与根据GPS输出信息计算得到的加速度信息进行对比，计算两者的差值，获得加速度补偿信息，再转至步骤S2。进一步，在所述步骤T3和步骤S2之间还包括以下步骤：在所述步骤T3中，若判断车辆转弯，则消除转弯时X轴陀螺输出的角速率，同时消除转弯时Y轴陀螺输出的角速率；在消除转弯时X轴陀螺的输出的角速率之后，再判断车辆是否处于转弯状态，若车辆处于转弯状态，则对X轴陀螺输出的角速率信息进行积分，得到俯仰角，并输出；若车辆没有处于转弯状态，则转至步骤S2；在消除转弯时Y轴陀螺输出的角速率之后，对Y轴陀螺输出的角速率进行积分得到第一俯仰角，同时根据加速度计的输出估计出第二俯仰角，之后计算所述第一俯仰角与所述第二俯仰角的差值；并判断GPS是否可用，若GPS不可用且所述第一俯仰角与所述第二俯仰角的差值大于预设值时，则对Y轴陀螺输出的角速率信息进行积分，估计当前的俯仰角，并输出；若GPS可用，则转至所述步骤S2。进一步，所述步骤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：根据所述状态向量、状态转移方程及量测方程，并利用所述滤波基本方程对被估计的所述状态向量进行求解，得到估计的状态向量。进一步，所述步骤S2-4具体包括：将机体坐标系b系的所述加速度匹配信息或所述加速度补偿信息fb＝[fxfyfz]T投影到平台坐标系n系，得到E为东向，N为北向，U为天向，即其中，是3×3维的从机体坐标系b系到平台坐标系n系的方向余弦矩阵，即选取的量测向量Zk为fn的前两维，即描述量测向量Zk与状态向量Xk之间关系的量测方程为：其中，是量测矩阵，g为重力加速度的大小，T00,T01,T02,T10,T11,T12为姿态矩阵中前两行的元素，Vk为2×1维量测噪声序列，其均值为0。进一步，在所述步骤S2-5中，所述滤波基本方程如下：状态一步预测方程为其中，为状态一步预测估计值，Φk,k-1为状态一步转移矩阵，为前一时刻状态向量的估计值；一步预测均方误差阵其中，Pk/k-1为一步预测均方误差阵，Pk-1为前一时刻状态估计的均方误差阵，Qk-1为系统噪声方差阵，为状态一步转移矩阵Φk,k-1的转置；计算滤波增益阵其中，Kk为滤波增益阵，Hk为量测矩阵，为Hk的转置；Rk为量测噪声序列的方差阵，其中，zk0和zk1为量测向量中的两个分量，w为权重系数；状态估计其中，为当前时刻状态向量的估计值，Zk为量测向量；计算估计的均方误差其中，Pk为当前时刻被估计的状态向量的均方误差阵，I为单位矩阵；根据所述基本滤波方程a)-e)计算得到k时刻被估计的状态向量即状态向量X(t)在tk时刻的估计值。进一步，在所述步骤S4中，还包括显示所述俯仰角、横滚角、航向角、角速率值及加速度值的步骤。进一步，还包括将所述步骤S4中判断出的所述车辆姿态进行语音播报的步骤。本专利技术还提供了一种车辆姿态安全预警系统，其包括:MEMS陀螺仪，用于测量车辆的角速率信息；加速度计，用于实时测量车辆的第一加速度信息；GPS，用于获得车辆的第二加速度信息；姿态解算单元，用于根据所述角速率信息、第一加速度信息和第二加速度信息进行姿态解算，获得车辆的姿态。进一步，还包括显示单元和语音播报单元，所述显示单元用于显示所述俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态；所述语音播报单元用于控制并播报所述车辆姿态。附图说明图1为本专利技术实施例一提供的车辆姿态安全预警方法的流程图。具体实施方式以下结合附图对本专利技术的原理和特征进行描述，所举实例只用于解释本专利技术，并非用于限定本专利技术的范围。需要本文档来自技高网...

【技术保护点】
一种车辆姿态安全预警方法，其特征在于，包括以下步骤：S1：利用MEMS陀螺仪测量车辆的角速率信息，利用加速度计测量车辆的第一加速度信息，利用GPS输出信息计算得到第二加速度信息；S2：选取姿态误差角、陀螺常值偏移和加速度计常值零偏作为状态向量，利用卡尔曼滤波基本方程对所述状态向量进行估计，获得估计的状态向量；S3：使用估计的状态向量中的姿态误差角修正姿态矩阵，获得修正后的姿态矩阵，根据修正后的姿态矩阵解算出俯仰角、横滚角和航向角；将所述角速率信息扣除所述估计的状态向量中的所述陀螺常值偏移得到角速率值；根据所述第一加速度信息、第二加速度信息及所述估计的状态向量中的所述加速度计常值零偏得到加速度值；S4：根据所述俯仰角、横滚角、航向角、角速率值及加速度值判断车辆姿态，并输出所述俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态。

【技术特征摘要】
1.一种车辆姿态安全预警方法，其特征在于，包括以下步骤：S1：利用MEMS陀螺仪测量车辆的角速率信息，利用加速度计测量车辆的第一加速度信息，利用GPS输出信息计算得到第二加速度信息；S2：选取姿态误差角、陀螺常值偏移和加速度计常值零偏作为状态向量，利用卡尔曼滤波基本方程对所述状态向量进行估计，获得估计的状态向量；S3：使用估计的状态向量中的姿态误差角修正姿态矩阵，获得修正后的姿态矩阵，根据修正后的姿态矩阵解算出俯仰角、横滚角和航向角；将所述角速率信息扣除所述估计的状态向量中的所述陀螺常值偏移得到角速率值；根据所述第一加速度信息、第二加速度信息及所述估计的状态向量中的所述加速度计常值零偏得到加速度值；S4：根据所述俯仰角、横滚角、航向角、角速率值及加速度值判断车辆姿态，并输出所述俯仰角、横滚角、航向角、角速率值、加速度值及车辆姿态。2.根据权利要求1所述的车辆姿态安全预警方法，其特征在于，在所述步骤S2之前还包括以下步骤：T1：对所述角速率信息和第一加速度信息进行标定处理，获得标定后的角速率信息和第一加速度信息；T2：将所述标定后的第一加速度信息与根据GPS输出信息计算得到的所述第二加速度信息进行对比，计算两者的差值，获得加速度匹配信息，并对所述标定后的角速率信息进行正交标定；T3：判断GPS是否可用，若GPS不可用，则转至步骤S2；若GPS可用，则判断车辆是否转弯，若转弯则转至步骤S2，若不转弯则将此刻所述加速计测量得到的加速度信息与根据GPS输出信息计算得到的加速度信息进行对比，计算两者的差值，获得加速度补偿信息，再转至步骤S2。3.根据权利要求2所述的车辆姿态安全预警方法，其特征在于，在所述步骤T3和步骤S2之间还包括以下步骤：在所述步骤T3中，若判断车辆转弯，则消除转弯时X轴陀螺输出的角速率，同时消除转弯时Y轴陀螺输出的角速率；在消除转弯时X轴陀螺的输出的角速率之后，再判断车辆是否处于转弯状态，若车辆处于转弯状态，则对X轴陀螺输出的角速率信息进行积分，得到俯仰角，并输出；若车辆没有处于转弯状态，则转至步骤S2；在消除转弯时Y轴陀螺输出的角速率之后，对Y轴陀螺输出的角速率进行积分得到第一俯仰角，同时根据加速度计的输出估计出第二俯仰角，之后计算所述第一俯仰角与所述第二俯仰角的差值；并判断GPS是否可用，若GPS不可用且所述第一俯仰角与所述第二俯仰角的差值大于预设阈值时，则对Y轴陀螺输出的角速率信息进行积分，估计当前的俯仰角，并输出；若GPS可用，则转至所述步骤S2。4.根据权利要求2所述的车辆姿态安全预警方法，其特征在于，所述步骤S2具体包括：S2-1：选定滤波状态向量X(t)：姿态误差角φ＝[φEφNφU]T、陀螺常值漂移εb＝[εxεyεz]T、加速度计常值零偏即S2-2：确立系统状态转移方程：其中，X&(t)为状态向量X(t)的导数，φ&、ε&b和▽&b依次为姿态误差角φ、陀螺常值漂移εb、加速度计常值零偏▽b的导数；是9×9维的连续系统的状态转移矩阵；0代表3×3维的零阵是3×3维的从机体坐标系b系到平台坐标系n系的方向余弦矩阵；w(...

【专利技术属性】
技术研发人员：张宪龙，刘海涛，张少军，张龙，
申请(专利权)人：北京三驰科技发展有限公司，
类型：发明
国别省市：北京,11