一种基于无损卡尔曼滤波的救援清障车姿态角估计方法技术

本发明专利技术公开了一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，本方法根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。

【技术实现步骤摘要】

本专利技术涉及一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其目的在于通过无损变换使救援清障车姿态角估计的非线性模型适用于线性假设下的标准卡尔曼滤波体系，从而获得准确的救援清障车姿态角估计值，可用于救援清障车姿态监控及危险姿态预警，属于救援清障车安全预警领域。
技术介绍
随着我国经济的快速增长，道路交通基础设施条件逐渐改善，机动车保有量逐年增长，道路车辆事故救援、城市违章车辆处理以及自然灾害抢险救援等社会需求也随着快速增长。与此同时，作为汽车后市场的救援清障行业也逐渐被重视，得到了快速的发展。根据相关数据显示，救援清障车在作业过程中发生侧翻或倾翻的事故率也在逐年增加，往往存在着未能正常作业却自身发生事故的情况。究其原因，一方面存在救援清障车操作人员未按规定作业、处理事故经验不足等主观因素；另一方面车辆的姿态监控装置尚未完善，不能为操作人员提供准确的姿态信息以及安全预警也是不可回避的客观因素。目前，已有专利文献对救援清障车姿态角进行研究：专利文献CN103616013 A公开了一种基于递归最小二乘的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，但是仅适用于横向坡度和纵向坡度较小的路面(即可采用arcsinδ≈δ进行约等于处理的情况，其中δ为较小的坡度值)，应用工况受限；专利文献CN104061899 A公开了一种基于卡尔曼滤波的车辆姿态角估计方法，可应用于较为复杂的工况，但是该方法是在对测量数据计算后进行滤波，并未采用基于非线性滤波的估计方法，滤波精度和效率存在一定的不足。为了解决上述技术在实际应用中的不足，本专利技术提供了一种基于无损卡尔曼滤波的救援清障车姿态角估计方法。该方法采用较少的低成本MEMS(Micro-Electro-Mechanical System)加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对车辆姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。
技术实现思路
本专利技术的目的是提供一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，该方法使用较少的MEMS加速度传感器，基于车辆运动学模型对姿态角进行估计，可以为救援清障车提供精确可靠的姿态信息并提供有效的安全预警。本专利技术采用的技术方案如下：一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其特征在于：本专利技术采用两个低成本的MEMS加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波的精度和效率，从而提供更为可靠的救援清障车姿态角估计值。具体步骤包括：1)建立救援清障车的运动学模型忽略地球旋转速度，车辆运动学方程为： v · x = a x + w z v y - w y v z + g sin θ v · y = a y - w z v x + w x v z - g sin φ cos θ v · z = a z + w y v x - w x v y - g cos φ cos θ - - - ( 1 ) 本文档来自技高网...

【技术保护点】
一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其特征在于采用较少的低成本MEMS加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。具体步骤包括：1)建立救援清障车的运动学模型忽略地球旋转速度，车辆运动学方程为：v·x=ax+wzvy-wyvz+g sinθ]]>v·y=ay+wzvx-wxvz-gsinφcosθ---(1)]]>v·z=az+wyvx-wxvy-g cosφcosθ]]>式(1)中，wx、wy和wz分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，vx、vy和vz分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，ax、ay和az分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；θ、φ分别表示俯仰角、侧倾角；g表示重力加速度；上标“·”表示微分，如表示vx的微分。由于救援清障车在工作时，仅利用吊臂对事故车辆开展施救工作，车身部分相对于地面处于静止状态，即可以合理地认为vx、vy、vz、wx、wy和wz均为零，则救援清障车的运动学模型可合理地简化为：ax＝‑g sinθ   (2)ay＝g sinφcosθ2)所需车载传感器安装由式(2)可以看出，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的姿态角进行估计。因此，两个低成本MEMS加速度传感器即可满足救援清障车姿态角的测量要求。两个低成本MEMS加速度传感器应安装于车身(即除吊臂外救援清障车的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用于测量纵向加速度，记为ax‑m；另一个与车体坐标系横轴平行，用于测量成横向加速度，记为ay‑m。3)建立无损卡尔曼滤波的状态方程和观测方程无损卡尔曼滤波状态方程为：xk＝f(xk‑1)+Wk‑1   (3)式(3)中，k表示离散化时刻，xk＝[θ φ]T，其中xk为系统状态向量，θ、φ是系统的待估参数；由于救援清障车在工作过程中，侧倾角与俯仰角是连续缓慢变化的，可认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，所以f(xk‑1)＝xk‑1；Wk‑1表示对应k‑1时刻零均值的系统高斯白噪声向量且W＝[w1 w2]T，其中w1、w2分别表示两个系统高斯白噪声分量，Wk‑1对应的系统高斯白噪声协方差阵Qk-1=]]>σw1200σw22,]]>其中分别表示系统高斯白噪声w1、w2对应的方差；无损卡尔曼滤波观测方程为：yk＝h(xk)+Vk   (4)式(4)中，yk＝[ax_m ay_m]T；由式(2)可知h(xk)＝[‑g sinθ g sinφcosθ]T；Vk表示与Wk互不相关的零均值测量高斯白噪声向量且V＝[V1 V2]T，其中v1、v2分别表示两个测量高斯白噪声分量，Vk对应的测量高斯白噪声协方差阵Rk=σv1200σv22,]]>其中分别表示测量高斯白噪声v1、v2对应的方差。4)无损卡尔曼滤波算法：根据式(3)和式(4)所描述的系统状态方程和观测方程，结合无损变换，建立无损卡尔曼滤波递推过程，如下述步骤所示：步骤1：输入变量初始化x^0=E[x0]---(5)]]>P0=E[(x0-x^0)(x0-x^0)T]]]>步骤2：计算Sigma采样点采用对称采样策略，产生2n+1个均值为方差为P的Sigma点集，n为状态向量维数，本专利技术中n＝2；χk-10=x^k-1]]>χk-1i=x^k-1+((n+λ)Pk-1)i,i=1,2---(6)]]>χk-1i=x^k-1-((n+λ)Pk-1)i-n,i=3,4]]>式(6)中，为5个Sigma点；λ＝α2(n+k)‑n为调节Sigma点和的距离参数，调节它可提高逼近精度，其中α决定Sigma点围绕均值的散布，通常取一小正数(如：1e‑4≤α≤1)，本文取0.01；k为比例因数，本专利技术取0；为加权协方差矩阵平方根的第i列；步骤3：无损卡尔曼滤波的时间更新方程χk|k-1i=f(χk-1i)---(7)]]>x^k-=Σi=02nWimχk|k-1i---(8)]]>Px,k-=Σi=02nWic(χk|k-1i-x^k-)(χk|k-1i-x^k-)T+Qk---(9...

【技术特征摘要】
1.一种基于无损卡尔曼滤波的救援清障车姿态角估计方法，其特征在于采用较少的低成本MEMS加速度传感器采集救援清障车的状态信息，根据救援清障车的工作特点，基于车辆运动学模型对姿态角进行估计，并且舍弃了扩展卡尔曼滤波算法在解决非线性系统将其近似线性化的思路，而是通过无损变换使非线性系统适用于线性假设下的标准卡尔曼滤波体系，提高了滤波精度和效率，从而得到更为准确的姿态角估计值，为救援清障车提供姿态安全预警。具体步骤包括：1)建立救援清障车的运动学模型忽略地球旋转速度，车辆运动学方程为： v · x = a x + w z v y - w y v z + g sin θ ]]> v · y = a y + w z v x - w x v z - g sin φ cos θ - - - ( 1 ) ]]> v · z = a z + w y v x - w x v y - g cos φ cos θ ]]>式(1)中，wx、wy和wz分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，vx、vy和vz分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，ax、ay和az分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；θ、φ分别表示俯仰角、侧倾角；g表示重力加速度；上标“·”表示微分，如表示vx的微分。由于救援清障车在工作时，仅利用吊臂对事故车辆开展施救工作，车身部分相对于地面处于静止状态，即可以合理地认为vx、vy、vz、wx、wy和wz均为零，则救援清障车的运动学模型可合理地简化为：ax＝-g sinθ (2)ay＝g sinφcosθ2)所需车载传感器安装由式(2)可以看出，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的姿态角进行估计。因此，两个低成本MEMS加速度传感器即可满足救援清障车姿态角的测量要求。两个低成本MEMS加速度传感器应安装于车身(即除吊臂外救援清障车的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用于测量纵向加速度，记为ax-m；另一个与车体坐标系横轴平行，用于测量成横向加速度，记为ay-m。3)建立无损卡尔曼滤波的状态方程和观测方程无损卡尔曼滤波状态方程为：xk＝f(xk-1)+Wk-1 (3)式(3)中，k表示离散化时刻，xk＝[θ φ]T，其中xk为系统状态向量，θ、φ是系统的待估参数；由于救援清障车在工作过程中，侧倾角与俯仰角是连续缓慢变化的，可认为当前采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，所以f(xk-1)＝xk-1；Wk-1表示对应k-1时刻零均值的系统高斯白噪声向量且W＝[w1 w2]T，其中w1、w2分别表示两个系统高斯白噪声分量，Wk-1对应的系统高斯白噪声协方差阵 Q k - 1 = ]]> σ w 1 2 0 0 σ w 2 2 , ]]>其中分别表示系统高斯白噪声w1、w2对应的方差；无损卡尔曼滤波观测方程为：yk＝h(xk)+Vk (4)式(4)中，yk＝[ax_m ay_m]T；由式(2)可知h(xk)＝[-g sinθ g sinφcosθ]T；Vk表示与Wk互不相关的零均值测量高斯白噪声向量且V＝[V1 V2]T，其中v1、v2分别表示两个测量高斯白噪声分量，Vk对应的测量高斯白噪声协方差阵 R k = σ v 1 2 0 0 σ v 2 2 , ]]>其中分别表示测量高斯白噪声v1、v2对应的方差。4)无损卡尔曼滤波算法：根据式(3)和式(4)所描述的系统状态方程和观测方程，结合无损变换，建立无损卡尔曼滤波递推过程，如下述步骤所示：步骤1：输入变量初始化 x ^ 0 = E [ x 0 ] - - - ( 5 ) ]]> P 0 = E [ ( x 0 - x ^ 0 ) ( x 0 - x ^ 0 ) T ] ]]>步骤2：计算Sigma采样点采用对称采样策略，产生2n+1个均值为方差为P的Sigma点集，n为状态向量维数，本发明中n＝2； χ k - 1 0 = x ^ k - 1 ]]> χ k - 1 i = x ^ k - 1 + ( ( n + λ ) P k - 1 ) i , i = 1 , 2 - - - ( 6 ) ]]> χ k - 1 i = x ^ k - 1 - ( ( n + λ ) P k - 1 ) i - n , i = 3 , 4 ]]>式(6)中，为5个Sigma点；λ＝α2(n+k)-n为调节Sigma点和的距离参数，调节它可提高逼近精度，其中α决定Sigma点围绕均值的散布，通常取一小正数(如：1e-4≤α≤1)，本文取0.01；k...

【专利技术属性】
技术研发人员：李旭，李晨晓，徐启敏，匡立刚，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32