本发明专利技术公开了一种救援清障车姿态角估计方法。本方法首先根据救援清障车的工作特点，建立了救援清障车的运动学模型。在此模型基础上，利用鲁棒滤波算法获得救援清障车俯仰角与侧倾角的精确估计值。本发明专利技术方法可以在无法确切知道干扰信号的统计特性的情况下，估计救援清障车在较大坡度率工况下的俯仰角与侧倾角，具有精度高、成本低、实时性好、适用范围广等显著优点，可用于救援清障车姿态监控及危险姿态预警。
【技术实现步骤摘要】
本专利技术涉及一种救援清障车姿态角估计方法，尤其涉及一种基于Hoc鲁棒滤波的救 援清障车姿态角估计方法，其目的在于为救援清障车提供姿态监控及危险姿态预警，属于 救援清障车安全预警领域。
技术介绍
近几年，我国的救援清障车得到了高速发展。但是，根据有关部门的统计，救援清 障车发生侧翻或倾翻的事故率也逐年增加，存在着很多未能安全救起被救车辆，而自身却 发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素 外，救援清障车的姿态监控装置尚不完善，无法为驾驶员提供准确的姿态信息和安全预警 也是不可回避的客观因素。 专利"一种基于卡尔曼滤波的救援清障车姿态角估计方法"（专利号： 201410279302)提出了一种基于卡尔曼滤波的救援清障车俯仰角、侧倾角的估算方法，该方 法解决了在复杂救援环境下，估算较大角度的侧倾角与俯仰角的问题。此方法用两个低成 本微电子机械系统(Micro Electro Mechanical systems，MEMS)加速度传感器取代完整的 六维惯性测量单元（Inertial Measurement Unit，IMU) JEMS加速度传感器相对于一般传 统惯性传感器具有集成化、低成本、低功耗、高抗过载能力和动态响应快等优点。 虽然MEMS加速度传感器具有上述优点，但是其本身存在着高度不确定的非线性噪 声，使得噪声信号很难被准确描述。在卡尔曼滤波中，MEMS加速度传感器的噪声信号往往用 随机模型来表示，例如高斯马尔可夫模型和自回归模型。这些模型只是对MEMS加速度传感 器噪声的一个近似描述。在实际应用过程中，不确定的非线性噪声会使MEMS加速度传感器 的噪声统计特性发生变化，从而使得预先设定的噪声模型不准确。但传统的卡尔曼滤波算 法，只有在确切知道外部干扰信号的统计特性前提下才具有较高的滤波精度，所以在这种 情况下卡尔曼滤波往往会出现发散、精度退化等问题，从而导致不理想的估计效果。 为了解决上述技术在应用中的不足，本专利技术专利提出一种基于Hoc鲁棒滤波的救援 清障车俯仰角、侧倾角估算方法。Hoc滤波是将鲁棒控制设计中引入的性能指标Hoc范数应用 于滤波，以解决系统中存在的各种不确定性问题，它将噪声看作是能量有限的随机信号，构 建一个滤波器使得从干扰输入到估计误差输出的闭环传递函数的Hoc范数最小或者小于给 定的正数γ。因此Hoc对噪声的不确定性具有很强的鲁棒性。本专利技术提出的方法可以用于在 无法确切知道干扰信号的统计特性的情况下，估计较大坡度率工况中救援清障车的俯仰 角、侧倾角。其精度更高，适用范围更广，为救援清障车提供更好的姿态监控及危险姿态预 警。
技术实现思路
本专利技术的目的是提出一种基于Hoc鲁棒滤波的救援清障车姿态角估计方法，该方法 使用的传感器数量少，且可以在无法确切知道干扰信号的统计特性的情况下，估算出救援 清障车在较大坡度率工况中的俯仰角、侧倾角，具有精度高、成本低、实时性好、适用范围广 的特点，可以为救援清障车提供姿态监控以及危险姿态预警。 本专利技术采用的技术方案如下：一种基于Hoc鲁棒滤波的救援清障车姿态角估计方 法，其特征在于:根据救援清障车工作特点，首先对其进行运动学建模，然后通过Hoc滤波算 法实现对救援清障车俯仰角与侧倾角等姿态角的实时、准确估计，该方法可以用于无法确 切知道干扰信号的统计特性的情况下，估计救援清障车的俯仰角、侧倾角，且仅需两个低成 本MEMS车载加速度传感器。具体步骤包括： 1)建立救援清障车的运动学模型 由于救援清障车在工作时的俯仰角速度、侧倾角速度、垂向速度均为零，且忽略地 球旋转速度，则可建立救援清障车的运动学方程：式（1)中，Vx，Vy分别表不车辆的纵向和横向速度，ax，ay分别表不车辆的纵向和横 向加速度，ωζ表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的；Θ表示 车辆的俯仰角，Φ表示车辆的侧倾角，g表示重力加速度，取g = 9.78m/s2;上标志"· "表示 微分，4表不Vx的微分，％.表不Vy的微分； 由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止 状态，即可以合理地认为vx、v y、色、和ω z均为零，则式（1)可以简化为： ax+g sin θ = 〇 (2) ay_g sin<})cos9 = 〇由式⑵可得 2)建立Hoc滤波的状态方程和观测方程离散化后的Hoc滤波状态方程的矩阵形式为： 式(4)中，k表示离散化时刻;X为系统状态向量，且Xk=T，其中W1、W2 分别表示两个系统噪声分量;状态转移矩阵，这是因为救援清障车在工 作过程中俯仰角与侧倾角是基本保持不变的，可以认为上一采样时刻的俯仰角与侧倾角等于下一采样时刻的俯仰角与侧倾角；ΓΗ为系统噪声输入矩阵，取Γ^为离散化的Hoc滤波观测方程的矩阵形式为： Zk = Hk · Xk+Vk (5) 式(5)中，Z为观测向量，Η为观测阵，V表示观测噪声向量，V=，其中ne是俯 仰角的观测噪声，ηΦ是侧倾角的观测噪声；由于观测向量与状态向量都是指俯仰角与侧倾分别为通过传感器测量值直接推算得 出的俯仰角与侧倾角值，根据式(3)、有： 式（6)中，aSLmfc:_、分别表示利用低成本MEMS加速度传感器所测得的纵向加 速度、横向加速度； 3 )H。。滤波递推：对于式(4)和式(5)所描述的系统状态方程和测量方程，建立次优Hoc滤波的递推过 程，对于一定的正数γ，次优Hoc滤波递推过程主要包括以下三个步骤：状态线性组合的估计 Yfc-:i^Yk-i的估计值，Yk-i为系统的待估计的向量，Xfc-.I是Xk-i的估计值，Lk-i为给定 的状态量线性组合矩阵，由于需要估计的量就是状态变量Xu， 时间修正估计误差方差阵：，Pk的初始值Ρο可选为 量测修正 经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的俯仰角与侧 倾角； 本专利技术的优点及显著效果： (1)本专利技术提出了一种救援清障车俯仰角、侧倾角的估计方法，具有高精度、低成 本、实时性好、适用范围广等优点，可用于救援清障车在复杂工况下的姿态监控和危险姿态 预警； (2)本专利技术提出的方法适用于MEMS加速度传感器的噪声特性无法准确建模的情 况，并且结果可以满足实际应用的精度与实时性要求； (3)本专利技术仅需两个低成本MEMS加速度传感器，具有成本低的优点，便于大规模推 广。【附图说明】 图1为本专利技术所提出方法的流程框图； 图2是俯仰角估计仿真结果对比图； 图3是侧倾角估计仿真结果对比图。【具体实施方式】 实施实例1近几年，我国的救援清障车行业得到了高速发展。但是，根据有关部门的统计，救 援清障车发生侧翻或倾翻的事故率也逐年增加，存在着很多未能安全救起被救车辆，而自 身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因 素外，救援清障车的姿态监控装置尚不完善，无法为驾驶员提供准确的姿态信息和安全预 警也是不可回避的客观因素。专利"一种基于卡尔曼滤波的救援清障车姿态角估计方法" (专利号：201410279302)提出了一种基于卡尔曼滤波的救援清障车俯仰角、侧倾角的估算 方法，该方法解决了在复杂救援环境下，较大角度的俯仰角与侧本文档来自技高网...
【技术保护点】
一种基于鲁棒滤波的救援清障车姿态角估计方法，其特征在于：根据救援清障车工作特点，对其进行运动学建模，然后通过鲁棒滤波算法实现对救援清障车俯仰角与侧倾角等姿态角的实时、准确估计，该方法可以在无法确切知道干扰信号的统计特性的情况下，估计救援清障车在较大坡度率工况下的俯仰角与侧倾角，且仅需两个低成本微电子机械系统(Micro Electro Mechanical systems，MEMS)车载加速度传感器，具有低成本、高精度的特点；具体步骤包括：1)建立救援清障车的运动学模型由于救援清障车在工作时的俯仰角速度、侧倾角速度、垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的运动学方程：v·x=ax+ωzvy+g sinθ---(1)]]>v·y=ay+ωzvx+gsinφcosθ]]>式(1)中，vx，vy分别表示车辆的纵向和横向速度，ax，ay分别表示车辆的纵向和横向加速度，ωz表示车辆的横摆角速度，上述变量的定义都是在车体坐标系中定义的；θ表示车辆的俯仰角，φ表示车辆的侧倾角；g表示重力加速度，取g＝9.78m/s2；上标志“·”表示微分，表示vx的微分，表示vy的微分；由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态，即可以合理地认为vx、vy、和ωz均为零，则式(1)可以简化为：ax+g sinθ＝0 (2)ay‑g sinφcosθ＝0由式(2)可得θ=arcsin(-axg)---(3)]]>φ=arcsin(aygcosθ)]]>2)建立H∞滤波的状态方程和观测方程离散化后的H∞滤波状态方程的矩阵形式为：式(4)中，k表示离散化时刻；X为系统状态向量，且Xk＝[x1 x2]T，x1＝θk，x2＝φk，即Xk＝[θk φk]T，上角标T表示对矩阵转置；W表示系统噪声向量，且W＝[w1 w2]T，其中w1、w2分别表示两个系统噪声分量；状态转移矩阵为这是因为救援清障车在工作过程中俯仰角与侧倾角是基本保持不变的，可以认为上一采样时刻的俯仰角与侧倾角等于下一采样时刻的俯仰角与侧倾角；Γk‑1为系统噪声输入矩阵，取Γk‑1为1001;]]>离散化后的H∞滤波观测方程的矩阵形式为：Zk＝Hk·Xk+Vk (5)式(5)中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[nθ nφ]，其中nθ是俯仰角的观测噪声，nφ是侧倾角的观测噪声；由于观测向量与状态向量都是指俯仰角与侧倾角，所以Hk=1001;Zk=θmkφmk,]]>其中和分别为通过传感器测量值直接推算得出的俯仰角与侧倾角值，根据式(3)、有：θmk=arcsin(-ax_mkg)---(6)]]>φmk=arcsin(ay_mkg cosθmk)]]>式(6)中，分别表示利用低成本MEMS加速度传感器所测得的纵向加速度、横向加速度；3)H∞滤波递推：对于式(4)和式(5)所描述的系统状态方程和测量方程，建立次优H∞滤波的递推过程，对于一定的正数γ，次优H∞滤波递推过程主要包括以下三个步骤：状态线性组合的估计Y^k-1=Lk-1X^k-1---(7)]]>是Yk‑1的估计值，Yk‑1为系统的待估计的向量，是Xk‑1的估计值，Lk‑1为给定的状态量线性组合矩阵，由于需要估计的量就是状态变量Xk‑1，所以取Lk‑1为1001;]]>时间修正状态一步预测：估计误差方差阵：其中Re,k-1=I00-γ2I+Hk-1Lk-1Pk-1[Hk-1TLk-1T],]]>Pk的初始值P0可选为(π/180)200(π/180)2;]]>量测修正滤波增益矩阵：Kk=PkHkT(I+HkPkHkT)-1---(10)]]>状态估计：X^k=X^k,k-1+Kk(Zk-HkX^k,k-1)---(11)]]>经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的俯仰角和侧倾角。...
【技术特征摘要】
【专利技术属性】
技术研发人员：李旭，蒋荣，陈伟，徐启敏，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。