[发明专利]一种基于卡尔曼滤波的救援清障车姿态角估计方法

说明书

技术领域

本发明涉及一种救援清障车姿态角估计方法，尤其涉及一种基于卡尔曼滤波的救援清障车姿态角估计方法，其目的在于为救援清障车提供姿态监控及危险姿态预警，属于救援清障车安全预警领域。

背景技术

近几年，为了解决道路交通事故发生后的清障救援问题，我国的救援清障车得到了高速发展。与此同时，根据有关部门的统计，救援清障车发生侧翻或倾翻的事故率也逐年增加，往往存在着未能安全救起被救车辆，而自身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素外，救援清障车的姿态监控装置尚不完善，无法为驾驶员提供准确的姿态信息以及安全预警也是不可回避的客观因素。

对于普通车辆而言，通常使用一个完整的六维惯性测量单元IMU(Inertial MeasurementUnit)来确定侧倾角和俯仰角等姿态角信息，该IMU包括3个加速度计和3个角速度陀螺仪，车辆的姿态角信息可以通过六维IMU的捷联导航算法推算得出。此方案用于救援清障车时，依然可以保证很高的精度和实时性。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。由于救援清障车在工作时，仅利用吊臂开展施救工作，而车身相对于地面处于静止状态，利用这一工作特点可以对所建立的动力学方程进行合理简化，从而减少所需的传感器数量，降低成本。如果使用MEMS(Micro-Electro-MechanicSystem，微机电系统)传感器，可以使成本进一步降低，利于设备的大规模推广。

专利“一种救援清障车姿态角估计方法”(公开号：CN103616013A)提出了一种基于递归最小二乘(RLS)的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，并且使用了MEMS传感器，具有成本低、实时性好等优点。但是该方法仅考虑路面横向坡度率与纵向坡度率小于20％的工况，此时俯仰角和侧倾角都较小，所以在估计过程中采用了约等于处理。但是考虑到在复杂救援环境下，路面的横向坡度率与纵向坡度率可能会达到30％。在一些特殊路段上，路面的横向坡度率与纵向坡度率甚至会超过55％。这些情况下，俯仰角和侧倾角较大，是不能进行约等于处理的，上述方法在实际应用过程中会受到限制。

发明内容

本发明的目的是提出一种基于卡尔曼滤波的救援清障车姿态角估计方法，该方法使用的传感器数量少，且可以估算出较大角度的俯仰角和侧倾角，具有精度高、成本低、实时性好、适用范围广的特点，可以为救援清障车提供姿态监控以及危险姿态预警。

本发明采用的技术方案如下：一种基于卡尔曼滤波的救援清障车姿态角估计方法，其特征在于：本发明根据救援清障车工作特点，对其进行动力学建模，进一步通过卡尔曼滤波算法实现对救援清障车侧倾角与俯仰角等姿态角的实时、准确估计，该方法可以用于在复杂救援环境下估计较大角度的俯仰角和侧倾角，且仅需两个低成本MEMS车载加速度传感器。具体步骤包括：

1)建立救援清障车的动力学模型

由于救援清障车在工作时的俯仰角速度、侧倾角速度与垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的动力学方程：