CN107985318A - 轮式装载机重心纵向位置和重心高度动态估计方法 - Google Patents

轮式装载机重心纵向位置和重心高度动态估计方法 Download PDF

Info

Publication number
CN107985318A
CN107985318A CN201711224909.5A CN201711224909A CN107985318A CN 107985318 A CN107985318 A CN 107985318A CN 201711224909 A CN201711224909 A CN 201711224909A CN 107985318 A CN107985318 A CN 107985318A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
mtr
msubsup
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201711224909.5A
Other languages
English (en)
Other versions
CN107985318B (zh
Inventor
王继新
杨智宇
韩云武
王伟
赵永玲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jilin University
Original Assignee
Jilin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jilin University filed Critical Jilin University
Priority to CN201711224909.5A priority Critical patent/CN107985318B/zh
Publication of CN107985318A publication Critical patent/CN107985318A/zh
Application granted granted Critical
Publication of CN107985318B publication Critical patent/CN107985318B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/12—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to parameters of the vehicle itself, e.g. tyre models
    • B60W40/13—Load or weight
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/105—Speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/12—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to parameters of the vehicle itself, e.g. tyre models
    • B60W40/13—Load or weight
    • B60W2040/1315—Location of the centre of gravity
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2300/00—Indexing codes relating to the type of vehicle
    • B60W2300/17—Construction vehicles, e.g. graders, excavators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • B60W2520/105—Longitudinal acceleration
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/28—Wheel speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2552/00—Input parameters relating to infrastructure
    • B60W2552/15—Road slope, i.e. the inclination of a road segment in the longitudinal direction

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Feedback Control In General (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)

Abstract

本发明公开了一种轮式装载机重心纵向位置和重心高度动态估计方法，首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩以及测量得到的轮胎滚动半径，通过非线性滤波器估计得到纵向速度；然后依据通过传感器测量得到的纵向加速度和路面坡度、估计得到的纵向速度、通过计算得到的前后轮胎滑移率以及通过查表或计算得到的整车质量和铲掘阻力，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到动态的重心纵向位置和重心高度。本发明所述方法为前后桥独立电驱动轮式装载机智能动力学控制提供准确的重心动态位置参数，提升整车控制精度和性能。

Description

轮式装载机重心纵向位置和重心高度动态估计方法
技术领域
本发明属于轮式装载机状态参数估计技术领域，具体涉及一种轮式装载机重心纵向位置和重心高度动态估计方法，适用于前后桥独立电驱动轮式装载机(Front/Rear AxleElectric Wheel Loader,简称FREWL)。
背景技术
近年来，新能源轮式装载机日益得到国内外政府和工程机械厂商的重视。其中，动力学智能控制领域的研究是新能源轮式装载机的重点研究方向之一。为实现整车动力学智能控制，需要向控制系统在线提供整车各种关键的状态参数。然而，这些关键的状态参数有一部分无法通过传感器在线测量得到。其中，整车静态重心位置虽然可以通过静态力学方法测量得到，但是动态重心位置在作业工况中随作业时工作姿态数值变化和铲装载荷数值变化而变化，且变化幅度和频率较大。因此，通过先验估计方法在线估计重心纵向位置和重心高度是实现整车动力学智能精确控制的前提。
现有的估计方法主要通过递推最小二乘法或扩展卡尔曼滤波方法估计车辆重心位置，但该方法存在以下两个主要缺点：
1、该重心估计的方法主要应用于公路车辆领域，由于工作装置和铰接结构的存在，轮式装载机的动力学模型与公路车辆的动力学模型有较大的区别；
2、递推最小二乘法和扩展卡尔曼滤波方法通常适用于非线性不强的数学问题。由于轮式装载机在作业时重心纵向位置和重心高度波动剧烈，变化频率和波动幅度较大，其动力学模型非线性较强。
以上原因导致现有的估计方法在轮式装载机重心纵向位置和高度的动态估计上难以得到良好的估计精度。由此可见，在本技术领域，重心纵向位置和重心高度的动态估计方法需进行改进。
因此，为实现作业过程中装载机重心位置的准确动态估计，提高整车动力学智能控制精度和效果，急需一种适用于轮式装载机的重心纵向位置和高度动态估计方法。
发明内容
针对上述现有技术中存在的问题，本发明提出一种适用于前后桥独立电驱动的轮式装载机重心纵向位置和重心高度动态估计方法，为前后桥独立电驱动轮式装载机智能动力学控制提供准确的重心动态位置参数，提升整车控制精度和性能。本发明所述方法适用于各种作业工况中纵向运动的前后桥独立电驱动轮式装载机，且使用的传感器数量少，价格低廉。结合说明书附图，本发明的技术方案如下：
轮式装载机重心纵向位置和重心高度动态估计方法，所述动态估计方法首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩Tf,r以及测量得到的轮胎滚动半径reff，通过非线性滤波器估计得到纵向速度vx；然后依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度vx、通过计算得到的前后轮胎滑移率σf,r以及通过查表或计算得到的整车质量m和铲掘阻力纵向分力Fx和铲掘阻力垂向分力Fz，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h。
所述动态估计方法具体步骤如下：
一、传感器布置：
在装载机的静态重心位置安装加速度传感器和坡度传感器，用于测量装载机的纵向加速度和装载机行驶时路面坡度的变化；
在装载机桥壳上安装轮速传感器，用于测量轮胎转速；
二、纵向速度vx的估计：
基于前后桥独立电驱动轮式装载机的前后电机输出转矩实时可知的特点，假设车辆只是纵向行驶，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：
y(t＝0)＝y0………………………………(2)
公式(1)中，vx(t)是车辆的纵向速度，x(t)是前后车轮角速度ωf,r与轮胎滚动半径reff的乘积，前后车轮角速度ωf,r通过轮速传感器测量得到，轮胎滚动半径reff通过测量得到；Tf,r是前后桥电机输出转矩，L系统参数，d是自定义值；
三、推导动力学模型：
基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力Fx、前轮纵向力Fxf、后轮纵向力Fxr、前轮滚动阻力Rxf、后轮滚动阻力Rxr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σf和后轮纵向滑移率σr的计算关系如下：
上述关系式(3)中，铲掘阻力纵向分力Fx通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到；
基于力矩平衡原理，前轮垂向力Fzf、后轮垂向力Fzr与重心到前轮中心的纵向距离lf、重心到后轮中心的纵向距离lr、前轮中心到铲斗铲尖的纵向距离lc、重心高度h、铲掘阻力垂向分力Fz的计算关系如下：
前轮角加速度后轮角加速度和轮胎转动惯量Ie、前轮驱动转矩Tf、后轮驱动转矩Tr、轮胎滚动半径reff、前轮纵向力Fxf和后轮纵向力Fxr的计算关系如下：
基于公式(3)、(4)、(5)、(6)和(7)，令l＝lf+lr，l是前后轮轴距，ll＝lf+lr+lc，ll是铲斗铲尖到后轮中心纵向距离，λ＝lr/l，λ是重心纵向位置。重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系如下：
上述公式(8)、(9)和(10)即为所需动力学模型；
四、改进型平方根无迹卡尔曼滤波：
(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：
离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：
xk+1＝Fkxk+wk……………………………………(11)
zk+1＝Hk+1xk+1+vk+1………………………………(12)
上述方程(11)和(12)中，Fk是状态转移矩阵，Hk+1是量测矩阵；wk和vk+1是相互独立的过程噪声和量测噪声，Qk是wk的协方差矩阵，Rk+1是vk+1的协方差矩阵，xk是状态变量，zk是量测量；
扩充状态变量xk，得到xk＝[vx ωf ωr K h λ]T；
量测矩阵Hk+1定义为：
k+1时刻的状态量xk+1与k时刻的状态量xk、采样时间Δt的关系如下所示：
关系式(14)中，是xk的导数；
综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，得到状态方程(15)和量测方程(16)如下所示：
(2)、改进型平方根无迹卡尔曼滤波递推：
在递推运算过程中，首先定义xk的初值以及协方差矩阵Pk的初值P0，具体计算公式如下：
式中，表示估计值；
再计算2n+1个Sigma点xk|k：
上述式(18)中，n是状态的维数，表示协方差矩阵的第i列，λ＝δ2(n+κ)-n，λ＝δ2(n+κ)-n是一个缩放比例参数，用来降低预测误差；δ是一个常数，其取值控制了采样点的分布；κ是待选参数的，其取值保证(n+λ)Pk是一个半正定矩阵，计算2n+1个Sigma点xk|k所对应的权值：
上述式(19)中，ωc是协方差对应的权重；ωm是均值对应的权重；β是一个非负的权重系数；
根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：
根据上述公式(19)和(20)，计算状态变量的下一步预测和协方差矩阵Pk+1|k：
上述公式(22)中，Qk是过程噪声；qr表示对矩阵进行QR分解；
基于公式(13)和(20)，计算得到Sigma点集预测的量测值
由上述公式(19)和(23)，通过加权求和得到系统量测的均值
基于公式(19)、(20)、(21)、(23)和(24)，进一步计算系统预测的协方差：
根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵Kk：
根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：
根据前述公式(13)和(22)，系统的协方差更新为：
经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，即动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。
与现有技术相比，本发明的有益效果在于：
1、本发明提出了一种低成本、高精度、实时性好的适用于各种作业工况下前后桥独立电驱动轮式装载机纵向运动时的重心纵向位置和重心高度动态估计方法；
2、本发明利用改进型平方根无迹卡尔曼滤波估计重心纵向位置和重心高度，适用于前后桥独立电驱动轮式装载机的动力学模型和作业工况，尤其适用于由于铲掘和工作装置举升时重心纵向位置和重心高度波动剧烈、变化频率和波动幅度较大时的作业工况，克服递推最小二乘法或扩展卡尔曼滤波方法在非线性较强的问题上估计精度较差的问题，同时克服了传统无迹卡尔曼滤波算法在线估计时出现的协方差非正定问题和Cholesky因子非正定问题；
3、本发明仅需一个加速度传感器、一个轮速传感器和一个坡度传感器即可实现重心纵向位置和重心高度动态估计，具有成本低的优点，便于大规模推广。
附图说明
图1为本发明所述轮式装载机重心纵向位置和重心高度动态估计方法的流程框图；
图2为适用本发明所述方法的前后桥独立电驱动轮式装载机结构原理简图；
图3为适用本发明所述方法的前后桥独立电驱动轮式装载机受力分析图。
具体实施方式
为进一步阐述本发明所述的技术方案，结合说明书附图，本发明的具体实施方式如下：
本发明所述的轮式装载机重心纵向位置和重心高度动态估计方法简述如下：
基于前后电机实时可知的电机输出转矩Tf,r以及测量得到的轮胎滚动半径reff，通过非线性滤波器估计得到纵向速度vx；依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度vx、通过计算得到的前后轮胎滑移率σf,r以及通过查表或计算得到的整车质量m和铲掘阻力F，搭建状态方程和量测方程，并通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h。
所述动态估计方法具体步骤如下：
一、传感器布置：
根据本发明所述的动态估计方法所搭建的硬件系统中，包括一个加速度传感器、至少一个轮速传感器以及一个坡度传感器。所述加速度传感器安装在前后桥独立电驱动轮式装载机的静态重心位置附近固定位置，其用于测量前后桥独立电驱动轮式装载机的纵向加速度所述轮速传感器安装在桥壳上，其用于测量轮胎转速；所述坡度传感器安装在静态重心位置附近固定位置，其用于测量车辆行驶时路面坡度α的变化。
二、纵向速度vx的估计：
如图1所示，前电机输出转矩Tf、后电机输出转矩Tr、纵向速度vx、前轮纵向滑移率σf、后轮纵向滑移率σr、纵向加速度整车质量m、路面坡度α、铲掘阻力纵向分力Fx和铲掘阻力垂向分力Fz是进行重心纵向位置和重心高度动态估计所需的重要状态参数。
上述状态参数除了纵向速度vx外，其他参数都可以通过计算或传感器直接测量得到。在量产车辆上想要通过传感器直接测量纵向速度vx却很难实现。这是因为普通车载GPS由于存在采样频率低和延时现象，很难提供精确的纵向车速数值。车载惯性导航系统(INS)虽然可以测量精确的纵向速度，但价格昂贵，很难在量产车上安装。因此，需要一种可以低成本且可在线估计纵向速度的方法。
本发明基于前后电机输出转矩实时可知的特点，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：
y(t＝0)＝y0………………………(2)
公式(1)中，假设车辆只是纵向行驶，则vx(t)是车辆的纵向速度，x(t)是前后车轮角速度ωf,r与轮胎滚动半径reff的乘积，前后车轮角速度ωf,r通过轮速传感器测量得到，轮胎滚动半径reff通过测量得到。Tf,r是前后桥电机输出转矩，L是一个系统参数，本实施例中取值20。d是一个很小的自定义值，本实施例中取值0.01。
三、推导车辆重心纵向位置和重心高度动态估计所需动力学模型：
因为轮式装载机主要工作在低速区域，其空气阻力数值较小，因此本发明忽略空气阻力。如图2和图3所示，依据FREWL前后电机分别驱动前后桥的驱动特点，基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力Fx、前轮纵向力Fxf、后轮纵向力Fxr、前轮滚动阻力Rxf、后轮滚动阻力Rxr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σf和后轮纵向滑移率σr的计算关系如下：
关系式(3)中，铲掘阻力纵向分力Fx通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到。
基于力矩平衡原理，前轮垂向力Fzf、后轮垂向力Fzr与重心到前轮中心的纵向距离lf、重心到后轮中心的纵向距离lr、前轮中心到铲斗铲尖的纵向距离lc、重心高度h、铲掘阻力垂向分力Fz的计算关系如下：
因为前后桥电机都为独立驱动，前轮角加速度后轮角加速度和轮胎转动惯量Ie、前轮驱动转矩Tf、后轮驱动转矩Tr、轮胎滚动半径reff、前轮纵向力Fxf和后轮纵向力Fxr的计算关系如下：
基于公式(3)、(4)、(5)、(6)和(7)，令l＝lf+lr，l是前后轮轴距，ll＝lf+lr+lc，ll是铲斗铲尖到后轮中心纵向距离距离，λ＝lr/l，λ是重心纵向位置。重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系，即上述公式(3)、(6)和(7)可重新定义为以下形式：
上述公式(8)、(9)和(10)即为本发明推导车辆重心纵向位置和重心高度动态估计所需动力学模型，也就是在后续改进型平方根无迹卡尔曼滤波递推运算中所需要的动力学模型。
四、改进型平方根无迹卡尔曼滤波：
在列出改进型平方根无迹卡尔曼滤波递推运算中所需的动力学模型后，还需要进行两步去在线估计轮式装载机重心纵向位置和重心高度；第一步：基于前述公式(8)、(9)和(10)，建立改进型平方根无迹卡尔曼滤波所需的状态方程和量测方程；第二步：将状态方程和量测方程带入改进型平方根无迹卡尔曼滤波的递推算法中，在线估计重心纵向位置和重心高度。具体过程如下：
(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：
离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：
xk+1＝Fkxk+wk……………………………………(11)
zk+1＝Hk+1xk+1+vk+1………………………………(12)
上述方程(11)和(12)中，Fk是状态转移矩阵，Hk+1是量测矩阵；wk和vk+1是相互独立的过程噪声和量测噪声，Qk是wk的协方差矩阵，Rk+1是vk+1的协方差矩阵，本实施例中所选用的Qk矩阵数值为diag(0.0001,0.0001,0.000001,0.0001,0.001,)，所选用的Rk+1矩阵数值为diag(0.01,0.0001,0.0001,0,0,0)，xk是状态变量，zk是量测量。
由公式(8)、(9)和(10)可知公式(11)中的一部分状态变量可以是纵向加速度前轮角加速度和后轮角加速度然而，为了估计重心纵向位置λ、重心高度h和轮胎滑移系数K，需要扩充状态变量xk，得到xk＝[vx ωf ωr K h λ]T。因为重心纵向位置λ、重心高度h和轮胎滑移系数K是待估计量，只有纵向加速度前轮角加速度和后轮角加速度可以测量得到或根据前述步骤估计得到，因此本发明量测矩阵Hk+1为：
k+1时刻的状态量xk+1与k时刻的状态量xk、采样时间Δt的关系如下所示：
关系式(14)中，是xk的导数。
综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，可以得到用于本发明改进型平方根无迹卡尔曼滤波的状态方程(15)和量测方程(16)如下所示：
(2)、改进型平方根无迹卡尔曼滤波递推：
在得到改进型平方根无迹卡尔曼滤波所需的状态方程(15)和量测方程(16)后，通过改进型平方根无迹卡尔曼滤波递推运算在线估计整车重心纵向位置和重心高度。
在递推运算过程中，首先定义xk的初值以及协方差矩阵Pk的初值P0，具体计算公式如下：
式中，表示估计值；
再计算2n+1个Sigma点xk|k：
式中，n是状态的维数，表示协方差矩阵的第i列，λ＝δ2(n+κ)-n，λ＝δ2(n+κ)-n是一个缩放比例参数，被用来降低预测误差。δ是一个常数，其取值控制了采样点的分布，本发明取0.01。κ是待选参数的，其取值通常应该保证(n+λ)Pk是一个半正定矩阵，本实施例中κ取值为0。计算2n+1个Sigma点xk|k所对应的权值：
式中，ωc是协方差对应的权重；ωm是均值对应的权重；β是一个非负的权重系数，本实施例中β取值为2。
根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：
根据上述公式(19)和(20)，计算状态变量的下一步预测和协方差矩阵Pk+1|k：
上述公式(22)中，Qk是过程噪声；qr表示对矩阵进行QR分解。
基于公式(13)和(20)，计算得到Sigma点集预测的量测值
由上述公式(19)和(23)，通过加权求和得到系统量测的均值
基于公式(19)、(20)、(21)、(23)和(24)，为了保持对系统状态变量估计的更新，需进一步计算系统预测的协方差：
根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵Kk：
根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：
根据前述公式(13)和(22)，系统的协方差更新为：
经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，可实时、精确地动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。

Claims (2)

1.轮式装载机重心纵向位置和重心高度动态估计方法，其特征在于：
所述动态估计方法首先基于前后桥独立电驱动轮式装载机的前后电机实时可知的电机输出转矩Tf,r以及测量得到的轮胎滚动半径reff，通过非线性滤波器估计得到纵向速度vx；然后依据通过传感器测量得到的纵向加速度和路面坡度α、估计得到的纵向速度vx、通过计算得到的前后轮胎滑移率σf,r以及通过查表或计算得到的整车质量m和铲掘阻力纵向分力Fx和铲掘阻力垂向分力Fz，搭建状态方程和量测方程；最后通过改进型平方根无迹卡尔曼滤波估计得到所述轮式装载机动态的重心纵向位置λ和重心高度h。
2.如权利要求1所述轮式装载机重心纵向位置和重心高度动态估计方法，其特征在于：
所述动态估计方法具体步骤如下：
一、传感器布置：
在装载机的静态重心位置安装加速度传感器和坡度传感器，用于测量装载机的纵向加速度和装载机行驶时路面坡度的变化；
在装载机桥壳上安装轮速传感器，用于测量轮胎转速；
二、纵向速度vx的估计：
基于前后桥独立电驱动轮式装载机的前后电机输出转矩实时可知的特点，假设车辆只是纵向行驶，通过一个非线性滤波器对纵向速度进行估计，具体计算公式如下：
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>T</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>r</mi> </mrow> </msub> <mo>/</mo> <mi>L</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&gt;</mo> <mi>d</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>r</mi> </mrow> </msub> <mo>/</mo> <mi>L</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&lt;</mo> <mo>-</mo> <mi>d</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mo>-</mo> <msub> <mi>T</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>r</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>x</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>)</mo> <mo>/</mo> <mi>d</mi> <mi>L</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>e</mi> <mi>l</mi> <mi>s</mi> <mi>e</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>...</mo> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow>
y(t＝0)＝y0………………………………(2)
公式(1)中，vx(t)是车辆的纵向速度，x(t)是前后车轮角速度ωf,r与轮胎滚动半径reff的乘积，前后车轮角速度ωf,r通过轮速传感器测量得到，轮胎滚动半径reff通过测量得到；Tf,r是前后桥电机输出转矩，L系统参数，d是自定义值；
三、推导动力学模型：
基于力平衡原理，纵向加速度与整车质量m、路面坡度α、铲掘阻力纵向分力Fx、前轮纵向力Fxf、后轮纵向力Fxr、前轮滚动阻力Rxf、后轮滚动阻力Rxr、滚动阻力系数f、轮胎滑移系数K、前轮纵向滑移率σf和后轮纵向滑移率σr的计算关系如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>m</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>F</mi> <mrow> <mi>x</mi> <mi>f</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>F</mi> <mrow> <mi>x</mi> <mi>r</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>f</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>r</mi> </mrow> </msub> <mo>-</mo> <mi>m</mi> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <msub> <mi>F</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>K&amp;sigma;</mi> <mi>f</mi> </msub> <mo>-</mo> <mi>f</mi> </mrow> <mo>)</mo> </mrow> <msub> <mi>F</mi> <mrow> <mi>z</mi> <mi>f</mi> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>K&amp;sigma;</mi> <mi>r</mi> </msub> <mo>-</mo> <mi>f</mi> </mrow> <mo>)</mo> </mrow> <msub> <mi>F</mi> <mrow> <mi>z</mi> <mi>r</mi> </mrow> </msub> <mo>-</mo> <mi>m</mi> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <msub> <mi>F</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> </mtable> <mo>...</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
上述关系式(3)中，铲掘阻力纵向分力Fx通过经验公式获得，整车质量m通过车辆出厂手册获得，路面坡度α通过坡度传感器测量得到；
基于力矩平衡原理，前轮垂向力Fzf、后轮垂向力Fzr与重心到前轮中心的纵向距离lf、重心到后轮中心的纵向距离lr、前轮中心到铲斗铲尖的纵向距离lc、重心高度h、铲掘阻力垂向分力Fz的计算关系如下：
<mrow> <msub> <mi>F</mi> <mrow> <mi>z</mi> <mi>f</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>m</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mi>h</mi> <mo>-</mo> <mi>m</mi> <mi>g</mi> <mi>h</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <msub> <mi>mgl</mi> <mi>r</mi> </msub> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <msub> <mi>F</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <mrow> <msub> <mi>l</mi> <mi>c</mi> </msub> <mo>+</mo> <msub> <mi>l</mi> <mi>f</mi> </msub> <mo>+</mo> <msub> <mi>l</mi> <mi>r</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>l</mi> <mi>f</mi> </msub> <mo>+</mo> <msub> <mi>l</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mn>...</mn> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>F</mi> <mrow> <mi>z</mi> <mi>r</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mo>-</mo> <mi>m</mi> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mi>h</mi> <mo>+</mo> <mi>m</mi> <mi>g</mi> <mi>h</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <msub> <mi>mgl</mi> <mi>f</mi> </msub> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <msub> <mi>F</mi> <mi>z</mi> </msub> <msub> <mi>l</mi> <mi>c</mi> </msub> </mrow> <mrow> <msub> <mi>l</mi> <mi>f</mi> </msub> <mo>+</mo> <msub> <mi>l</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>...</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
前轮角加速度后轮角加速度和轮胎转动惯量Ie、前轮驱动转矩Tf、后轮驱动转矩Tr、轮胎滚动半径reff、前轮纵向力Fxf和后轮纵向力Fxr的计算关系如下：
<mrow> <msub> <mi>I</mi> <mi>e</mi> </msub> <msub> <mover> <mi>&amp;omega;</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>w</mi> <mi>f</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>T</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>F</mi> <mrow> <mi>x</mi> <mi>f</mi> </mrow> </msub> <mo>...</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>I</mi> <mi>e</mi> </msub> <msub> <mover> <mi>&amp;omega;</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>w</mi> <mi>r</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>T</mi> <mi>r</mi> </msub> <mo>-</mo> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>F</mi> <mrow> <mi>x</mi> <mi>r</mi> </mrow> </msub> <mo>...</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
基于公式(3)、(4)、(5)、(6)和(7)，令l＝lf+lr，l是前后轮轴距，ll＝lf+lr+lc，ll是铲斗铲尖到后轮中心纵向距离，λ＝lr/l，λ是重心纵向位置。重新推导出通过加速度传感器和轮速传感器测量得到的前后桥独立电驱动轮式装载机的纵向加速度前轮角加速度和后轮角加速度与待估计的重心纵向位置λ和重心高度h的关系如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <mo>(</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;lambda;K&amp;sigma;</mi> <mi>f</mi> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mi>&amp;lambda;</mi> </mrow> <mo>)</mo> </mrow> <msub> <mi>K&amp;sigma;</mi> <mi>r</mi> </msub> <mo>-</mo> <mi>f</mi> </mrow> <mo>)</mo> </mrow> <mi>g</mi> <mi> </mi> <mi>cos</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>+</mo> <mfrac> <mrow> <mi>h</mi> <mi>K</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mfrac> <msub> <mi>F</mi> <mi>z</mi> </msub> <mrow> <mi>m</mi> <mi>l</mi> </mrow> </mfrac> <mo>(</mo> <mi>K</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <msub> <mi>l</mi> <mi>l</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <msub> <mi>l</mi> <mi>c</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mi>f</mi> <mo>(</mo> <mrow> <msub> <mi>l</mi> <mi>c</mi> </msub> <mo>-</mo> <msub> <mi>l</mi> <mi>l</mi> </msub> </mrow> <mo>)</mo> <mo>)</mo> <mo>-</mo> <mfrac> <msub> <mi>F</mi> <mi>x</mi> </msub> <mi>m</mi> </mfrac> <mo>)</mo> <mo>/</mo> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <mi>h</mi> <mi>K</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> <mn>...</mn> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>&amp;omega;</mi> <mi>f</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>I</mi> <mi>e</mi> </msub> </mfrac> <mrow> <mo>(</mo> <msub> <mi>T</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>Kr</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>(</mo> <mrow> <mfrac> <mrow> <msub> <mi>F</mi> <mi>z</mi> </msub> <msub> <mi>l</mi> <mi>l</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mo>+</mo> <mi>g</mi> <mi>&amp;lambda;</mi> <mi>m</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mfrac> <mrow> <mi>h</mi> <mi>m</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>-</mo> <mi>g</mi> <mi> </mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;alpha;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>...</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>&amp;omega;</mi> <mi>r</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>I</mi> <mi>e</mi> </msub> </mfrac> <mrow> <mo>(</mo> <msub> <mi>T</mi> <mi>r</mi> </msub> <mo>+</mo> <msub> <mi>Kr</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <mo>(</mo> <mrow> <mfrac> <mrow> <msub> <mi>F</mi> <mi>z</mi> </msub> <msub> <mi>l</mi> <mi>c</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mo>+</mo> <mi>g</mi> <mrow> <mo>(</mo> <mrow> <mi>&amp;lambda;</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> </mrow> <mi>m</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mfrac> <mrow> <mi>h</mi> <mi>m</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>-</mo> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>...</mo> <mo>...</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
上述公式(8)、(9)和(10)即为所需动力学模型；
四、改进型平方根无迹卡尔曼滤波：
(1)、建立改进型平方根无迹卡尔曼滤波的状态方程和量测方程：
离散时间的动态系统由如下所示描述状态向量的状态方程(11)和描述量测向量的量测方程(12)组成：
xk+1＝Fkxk+wk……………………………………(11)
zk+1＝Hk+1xk+1+vk+1………………………………(12)
上述方程(11)和(12)中，Fk是状态转移矩阵，Hk+1是量测矩阵；wk和vk+1是相互独立的过程噪声和量测噪声，Qk是wk的协方差矩阵，Rk+1是vk+1的协方差矩阵，xk是状态变量，zk是量测量；
扩充状态变量xk，得到xk＝[vx ωf ωr K h λ]T；
量测矩阵Hk+1定义为：
<mrow> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>...</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
k+1时刻的状态量xk+1与k时刻的状态量xk、采样时间Δt的关系如下所示：
<mrow> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>k</mi> </msub> <mo>...</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
关系式(14)中，是xk的导数；
综合公式(8)、(9)、(10)、(11)、(12)、(13)和(14)，得到状态方程(15)和量测方程(16)如下所示：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>x</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>h</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>x</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>(</mo> <mo>(</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mi>&amp;lambda;</mi> </mrow> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <mo>-</mo> <mi>f</mi> </mrow> <mo>)</mo> </mrow> <mi>g</mi> <mi> </mi> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>+</mo> <mfrac> <mrow> <msub> <mi>h</mi> <mi>k</mi> </msub> <msub> <mi>K</mi> <mi>k</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mfrac> <msub> <mi>F</mi> <mi>z</mi> </msub> <mrow> <mi>m</mi> <mi>l</mi> </mrow> </mfrac> <mo>(</mo> <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <msub> <mi>l</mi> <mi>l</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <msub> <mi>l</mi> <mi>c</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <mi>f</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>l</mi> <mi>c</mi> </msub> <mo>-</mo> <msub> <mi>l</mi> <mi>l</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>-</mo> <mfrac> <msub> <mi>F</mi> <mi>x</mi> </msub> <mi>m</mi> </mfrac> <mo>)</mo> <mo>/</mo> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <msub> <mi>h</mi> <mi>k</mi> </msub> <msub> <mi>K</mi> <mi>k</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mrow> <mo>(</mo> <mrow> <mfrac> <mn>1</mn> <msub> <mi>I</mi> <mi>e</mi> </msub> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>T</mi> <mi>f</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;sigma;</mi> <mi>f</mi> </msub> <mrow> <mo>(</mo> <mrow> <mfrac> <mrow> <msub> <mi>F</mi> <mi>z</mi> </msub> <msub> <mi>l</mi> <mi>l</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mo>+</mo> <msub> <mi>g&amp;lambda;</mi> <mi>k</mi> </msub> <mi>m</mi> <mi> </mi> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mfrac> <mrow> <msub> <mi>h</mi> <mi>k</mi> </msub> <mi>m</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>-</mo> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mrow> <mo>(</mo> <mrow> <mfrac> <mn>1</mn> <msub> <mi>I</mi> <mi>e</mi> </msub> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mi>T</mi> <mi>r</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <mrow> <mo>(</mo> <mrow> <mfrac> <mrow> <msub> <mi>F</mi> <mi>z</mi> </msub> <msub> <mi>l</mi> <mi>c</mi> </msub> </mrow> <mi>l</mi> </mfrac> <mo>+</mo> <mi>g</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> </mrow> <mi>m</mi> <mi> </mi> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mfrac> <mrow> <msub> <mi>h</mi> <mi>k</mi> </msub> <mi>m</mi> </mrow> <mi>l</mi> </mfrac> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>-</mo> <mi>g</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>h</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;lambda;</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>v</mi> <mi>x</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>&amp;omega;</mi> <mi>f</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>&amp;omega;</mi> <mi>r</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>K</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>h</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>&amp;lambda;</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mo>...</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>x</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>f</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>h</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>v</mi> <mi>x</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>&amp;omega;</mi> <mi>f</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <msub> <mi>&amp;omega;</mi> <mi>r</mi> </msub> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>K</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>h</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>&amp;lambda;</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mo>...</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
(2)、改进型平方根无迹卡尔曼滤波递推：
在递推运算过程中，首先定义xk的初值以及协方差矩阵Pk的初值P0，具体计算公式如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mrow> <mo>&amp;lsqb;</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>&amp;rsqb;</mo> </mrow> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mrow> <mo>(</mo> <mrow> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>c</mi> <mi>h</mi> <mi>o</mi> <mi>l</mi> <mo>{</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <mrow> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mo>}</mo> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>...</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
式中，表示估计值；
再计算2n+1个Sigma点xk|k：
<mrow> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>+</mo> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> </mrow> </msqrt> <mo>)</mo> </mrow> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>~</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>-</mo> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> </mrow> </msqrt> <mo>)</mo> </mrow> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>~</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>...</mn> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
上述式(18)中，n是状态的维数，表示协方差矩阵的第i列；λ＝δ2(n+κ)-n，λ＝δ2(n+κ)-n是一个缩放比例参数，被用来降低预测误差；δ是一个常数，其取值控制了采样点的分布；κ是待选参数的，其取值保证(n+λ)Pk是一个半正定矩阵，计算2n+1个Sigma点xk|k所对应的权值：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>m</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfrac> <mi>&amp;lambda;</mi> <mrow> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> </mrow> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>c</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfrac> <mi>&amp;lambda;</mi> <mrow> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> </mrow> </mfrac> <mo>+</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <msup> <mi>&amp;alpha;</mi> <mn>2</mn> </msup> <mo>+</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>m</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>&amp;omega;</mi> <mi>c</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfrac> <mi>&amp;lambda;</mi> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>~</mo> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>...</mn> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
上述式(19)中，ωc是协方差对应的权重；ωm是均值对应的权重；β是一个非负的权重系数；
根据前述公式(15)和(18)，计算2n+1个Sigma点集的下一步预测：
<mrow> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mi>f</mi> <mo>&amp;lsqb;</mo> <mi>k</mi> <mo>,</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;rsqb;</mo> <mo>...</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
根据上述公式(19)(20)，计算状态变量的下一步预测和协方差矩阵Pk+1|k：
<mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>&amp;omega;</mi> <mi>i</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>...</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>q</mi> <mi>r</mi> <mo>{</mo> <mtable> <mtr> <mtd> <mrow> <mo>&amp;lsqb;</mo> <msqrt> <msubsup> <mi>&amp;omega;</mi> <mn>1</mn> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> </msqrt> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>0</mn> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msqrt> <msubsup> <mi>&amp;omega;</mi> <mrow> <mn>2</mn> <mi>n</mi> </mrow> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> </msqrt> <msup> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>0</mn> </msubsup> </mrow> </mtd> <mtd> <msqrt> <msub> <mi>Q</mi> <mi>k</mi> </msub> </msqrt> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> <mo>}</mo> <mo>...</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>
上述公式(22)中，Qk是过程噪声；qr表示对矩阵进行QR分解；
基于公式(13)和(20)，计算得到Sigma点集预测的量测值
<mrow> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>Hx</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>...</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>
由上述公式(19)和(23)，通过加权求和得到系统量测的均值
<mrow> <msub> <mover> <mi>y</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>&amp;omega;</mi> <mi>i</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>...</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>
基于公式(19)、(20)、(21)、(23)和(24)，进一步计算系统预测的协方差：
<mrow> <msub> <mi>P</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>q</mi> <mi>r</mi> <mo>{</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msqrt> <msubsup> <mi>&amp;omega;</mi> <mn>1</mn> <mi>c</mi> </msubsup> </msqrt> <mrow> <mo>(</mo> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>0</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msqrt> <msubsup> <mi>&amp;omega;</mi> <mrow> <mn>2</mn> <mi>n</mi> </mrow> <mi>c</mi> </msubsup> </msqrt> <mrow> <mo>(</mo> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mn>0</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <msqrt> <mi>R</mi> </msqrt> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>}</mo> <mo>...</mo> <mo>...</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>x</mi> <mi>k</mi> <mo>,</mo> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>&amp;omega;</mi> <mi>i</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>...</mo> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>
根据上述公式(25)和(26)计算卡尔曼滤波增益矩阵Kk：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>x</mi> <mi>k</mi> <mo>,</mo> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>/</mo> <msubsup> <mi>P</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> <mo>/</mo> <msub> <mi>P</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>...</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>
根据前述公式(16)、(21)、(24)和(27)则系统的状态更新为：
<mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>...</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow>
根据前述公式(13)和(22)，系统的协方差更新为：
<mrow> <msub> <mi>F</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>...</mo> <mrow> <mo>(</mo> <mn>29</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>U</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>F</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> </mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> </msup> <mo>...</mo> <mrow> <mo>(</mo> <mn>30</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;lsqb;</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>U</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <mrow> <mo>(</mo> <msub> <mi>U</mi> <mi>k</mi> </msub> <mo>+</mo> <msqrt> <msub> <mi>R</mi> <mi>k</mi> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>...</mo> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>
经过前述公式(15)和(16)以及上述改进型平方根无迹卡尔曼滤波递推公式计算后，即动态估计前后桥独立电驱动轮式装载机的重心纵向位置和重心高度。
CN201711224909.5A 2017-11-29 2017-11-29 轮式装载机重心纵向位置和重心高度动态估计方法 Expired - Fee Related CN107985318B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711224909.5A CN107985318B (zh) 2017-11-29 2017-11-29 轮式装载机重心纵向位置和重心高度动态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711224909.5A CN107985318B (zh) 2017-11-29 2017-11-29 轮式装载机重心纵向位置和重心高度动态估计方法

Publications (2)

Publication Number Publication Date
CN107985318A true CN107985318A (zh) 2018-05-04
CN107985318B CN107985318B (zh) 2019-10-25

Family

ID=62034361

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711224909.5A Expired - Fee Related CN107985318B (zh) 2017-11-29 2017-11-29 轮式装载机重心纵向位置和重心高度动态估计方法

Country Status (1)

Country Link
CN (1) CN107985318B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110901629A (zh) * 2019-11-23 2020-03-24 中国人民解放军陆军装甲兵学院士官学校 一种重型车辆侧翻预警方法及装置
CN111942399A (zh) * 2020-07-17 2020-11-17 东风汽车集团有限公司 一种基于无迹卡尔曼滤波的车速估算方法及系统
CN112124323A (zh) * 2020-09-29 2020-12-25 北京主线科技有限公司 基于轮胎模型的车辆质量辨识方法及轮胎模型生成方法
CN115407791A (zh) * 2022-08-19 2022-11-29 沈阳工业大学 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法
US11724596B1 (en) * 2023-01-24 2023-08-15 Ford Global Technologies, Llc Systems and methods for road disturbance detection and torque vectoring control

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000516703A (ja) * 1996-06-13 2000-12-12 アイティーティー・マニュファクチャリング・エンタープライジズ・インコーポレーテッド 自動車の運転状態を制御するための方法
CN104354700A (zh) * 2014-11-03 2015-02-18 武汉理工大学 一种基于无迹卡尔曼滤波的车辆参数在线估计方法
DE102014200987A1 (de) * 2014-01-21 2015-07-23 Continental Teves Ag & Co. Ohg Verfahren zur Ermittlung der Lage des Schwerpunkts eines Fahrzeugs
DE102014018717A1 (de) * 2014-12-16 2016-06-16 Wabco Gmbh Verfahren zum Ermitteln eines Schwerpunktes eines Fahrzeuges und Fahrzeug-Regelsystem
CN106394561A (zh) * 2015-11-10 2017-02-15 北京中科易电信息科技股份有限公司 一种车辆的纵向车速的估计方法和装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000516703A (ja) * 1996-06-13 2000-12-12 アイティーティー・マニュファクチャリング・エンタープライジズ・インコーポレーテッド 自動車の運転状態を制御するための方法
DE102014200987A1 (de) * 2014-01-21 2015-07-23 Continental Teves Ag & Co. Ohg Verfahren zur Ermittlung der Lage des Schwerpunkts eines Fahrzeugs
CN104354700A (zh) * 2014-11-03 2015-02-18 武汉理工大学 一种基于无迹卡尔曼滤波的车辆参数在线估计方法
DE102014018717A1 (de) * 2014-12-16 2016-06-16 Wabco Gmbh Verfahren zum Ermitteln eines Schwerpunktes eines Fahrzeuges und Fahrzeug-Regelsystem
CN106394561A (zh) * 2015-11-10 2017-02-15 北京中科易电信息科技股份有限公司 一种车辆的纵向车速的估计方法和装置

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110901629A (zh) * 2019-11-23 2020-03-24 中国人民解放军陆军装甲兵学院士官学校 一种重型车辆侧翻预警方法及装置
CN110901629B (zh) * 2019-11-23 2021-07-06 中国人民解放军陆军装甲兵学院士官学校 一种重型车辆侧翻预警方法及装置
CN111942399A (zh) * 2020-07-17 2020-11-17 东风汽车集团有限公司 一种基于无迹卡尔曼滤波的车速估算方法及系统
CN112124323A (zh) * 2020-09-29 2020-12-25 北京主线科技有限公司 基于轮胎模型的车辆质量辨识方法及轮胎模型生成方法
CN115407791A (zh) * 2022-08-19 2022-11-29 沈阳工业大学 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法
CN115407791B (zh) * 2022-08-19 2023-12-12 沈阳工业大学 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法
US11724596B1 (en) * 2023-01-24 2023-08-15 Ford Global Technologies, Llc Systems and methods for road disturbance detection and torque vectoring control

Also Published As

Publication number Publication date
CN107985318B (zh) 2019-10-25

Similar Documents

Publication Publication Date Title
CN107985318A (zh) 轮式装载机重心纵向位置和重心高度动态估计方法
CN107985315A (zh) 轮式装载机轮胎纵向力动态估计方法
CN112613253B (zh) 考虑环境因素的车辆质量和道路坡度联合自适应估计方法
CN104354700B (zh) 一种基于无迹卡尔曼滤波的车辆参数在线估计方法
CN109606378B (zh) 面向非高斯噪声环境的车辆行驶状态估计方法
CN104639003B (zh) 一种交流伺服系统的转动惯量辨识方法
CN101655504B (zh) 一种机动车辆自适应巡航系统的车速估计方法
CN109597308A (zh) 基于动力学模型的无人驾驶汽车模型预测控制器设计方法
CN110884499B (zh) 一种确定车辆质心侧偏角的方法和系统
CN107016157B (zh) 分布式驱动电动汽车路面自适应纵向车速估计系统及方法
CN106985703A (zh) 一种分布式驱动电动汽车路面自适应防滑控制系统及方法
CN105416276A (zh) 基于高阶滑模的电动汽车稳定性直接横摆力矩控制方法
CN108597058A (zh) 基于伪量测信息的分布式驱动电动汽车状态级联估计方法
CN106125548A (zh) 工业机器人动力学模型参数辨识方法
CN110497915B (zh) 一种基于加权融合算法的汽车行驶状态估计方法
CN108162976A (zh) 一种基于稀疏网格求积卡尔曼滤波的车辆行驶状态估计方法
CN116923428B (zh) 一种电动汽车质心侧偏角与轮胎侧向力联合估计方法
CN116992697B (zh) 一种智能电动汽车行驶状态信息估计方法
CN116552548B (zh) 一种四轮分布式电驱动汽车状态估计方法
CN117272525A (zh) 一种智能电动汽车路面附着系数估计方法
Hu et al. Vehicle mass and road grade estimation based on adaptive forgetting factor RLS and EKF algorithm
CN115110993A (zh) 一种井下无人驾驶单轨吊荷载质量和轨道坡度联合识别方法
Yuhao Estimation of Vehicle Status and Parameters Based on Nonlinear Kalman Filtering
CN111332278B (zh) 一种分布式驱动电动车辆横向稳定控制方法及系统
CN114154231A (zh) 一种基于深度强化学习的车辆行驶状态估计系统及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20191025

Termination date: 20211129