CN111703432A - 一种智能履带车辆滑动参数实时估计方法 - Google Patents

Abstract

本发明公开了一种基于前向轨迹预测补偿的双重无迹卡尔曼滤波(DUKF)智能履带车辆滑动参数实时估计方法，属于智能履带车辆参数估计技术领域。该方法包括：步骤1，建立基于瞬时转向中心的履带车辆运动学模型；步骤2，根据传感器采集的车辆历史状态信息；步骤3，根据传感器采集的车辆当前时刻状态信息；步骤4，将步骤3得到的车辆瞬时转向中心转向极偏移量作为补偿，结合步骤2中初步估计的履带车辆滑动参数即可得到最终的履带车辆滑动参数。本发明不仅能够实时估计履带车辆滑动参数，而且利用DUKF对车辆未来的相对位姿进行滑动参数估计进而对初步估计的滑动参数进行补偿，提升了滑动参数估计的精度与车辆模型的准确性。

Description

一种智能履带车辆滑动参数实时估计方法

技术领域

本发明涉及智能履带车辆参数估计技术领域，特别是涉及一种智能履带车辆滑动参数实时估计方法。

背景技术

智能车辆作为智能交通系统和未来战斗系统的一个重要组成部分，在民用、军用和太空领域有着广阔的应用前景。随着车载计算设备能力的增强与无人控制技术的逐步完善，智能履带车辆在越野环境下的需求正在增长。履带车辆有良好的通过性与机动性，对于复杂的越野环境有很强的适应能力，是在越野环境下进行无人车辆研究的可靠平台。

但是对于滑动转向的履带车辆,由于履带与地面之间的相互作用力很复杂，建立精确的车辆模型很困难。无论是无人驾驶基于前向预测的路径规划还是给定一段控制序列对车辆进行横向控制都十分依赖履带车辆模型，不准确的模型将导致预测的轨迹或车辆的行驶轨迹与原路径具有较大的误差。常见的解决方案是建立与滑动参数有关的车辆模型，对滑动参数进行估计。现有的一些履带车辆滑动参数估计方法也会因为线性化处理导致模型在非线性较强时误差不能收敛，导致模型不准确。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种基于前向轨迹预测补偿的DUKF智能履带车辆滑动参数实时估计方法来克服现有技术的上述缺陷，本发明不仅能够实时估计履带车辆滑动参数，而且利用DUKF对车辆未来的相对位姿进行滑动参数估计进而对初步估计的滑动参数进行补偿，提升了滑动参数估计的精度与车辆模型的准确性。

为实现上述目的，本发明提供了如下技术方案：一种智能履带车辆滑动参数实时估计方法，包括如下步骤：

步骤1，建立基于瞬时转向中心的履带车辆运动学模型；

步骤2，根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数；

步骤3，将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数。

作为本发明的进一步改进，所述步骤1中，建立基于瞬时转向中心的履带车辆运动学模型具体为：

其中：

式中，x、y分别为车辆的横纵坐标，θ、β、γ分别为车辆的横摆角、俯仰角和侧倾角，V_x、V_y分别为车辆纵向速度与横向速度，ω为车辆横摆角速度，x_c为车辆瞬时转向中心，y^l、y^r分别为左右侧履带瞬时转向中心，v^l、v^r分别为车辆左右侧履带接地段纵向速度。

作为本发明的进一步改进，所述步骤2中，所述根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数具体步骤为：

步骤21，令滑动参数R_h＝(y^l,y^r,x_c)^T作为非线性履带车辆系统的状态向量，系统输入为u＝(v^l,v^r,θ,β,γ)^T，观测量为Z＝(Δx,Δy)，预测量为Z_h＝(Δx_h,Δy_h)，系统输入u为车辆历史状态信息，观测量Z为每个采样间隔车辆历史位姿的变化量；预测量Z_h为每个采样间隔将已选取的Sigma点、车辆历史状态信息与滑动参数R_h输入车辆运动学模型得到的相对位姿变化量。利用UKF算法处理预测量Z_h、观测量Z与非线性履带车辆系统的状态向量R_h，通过迭代运算初步估计履带车辆滑动参数y^l、y^r、x_c

步骤22，由已选取的Sigma点、车辆历史状态信息与滑动参数R_h输入车辆运动学模型预测车辆历史时刻的相对位姿预测，即计算预测量Z_h＝(Δx_h,Δy_h)：

其中：

式中，Δt为系统采样时间；

步骤23，利用UKF算法处理测量预测量Z_h、观测量Z与非线性履带车辆系统的状态向量R_h，通过迭代运算初步估计履带车辆滑动参数y^l、y^r、x_c。

作为本发明的进一步改进，所述步骤3中，将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数具体步骤为：

步骤31，令滑动参数

作为非线性履带车辆系统的状态向量，下层UKF迭代初值R_p0＝R_h，系统输入为u_p＝(v^l,v^r,θ,β,γ)^T，观测量为z＝(Δx_pv,Δy_pv)，预测量z_p＝(Δx_pR,Δy_pR)，系统输入u_p为车辆当前时刻的状态信息，观测量z为利用车辆当前时刻绝对速度与横摆角速度来预测未来一段采样间隔内车辆位姿的变化量，预测量z_p为将已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p输入车辆运动学模型得到未来一段采样间隔内相对位姿变化量，再次利用UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp；步骤32，利用车辆当前时刻的绝对速度与横摆角速度来预测未来一段采样时间内车辆位姿的变化量，即计算观测量z：

θ_k＝ω_v*k*Δt+θ₀

式中，V、ω_v分别为传感器测得的当前时刻车辆绝对速度与角速度，θ₀为车辆初始横摆角，θ_k为预测的第k个采样时间车辆横摆角；

步骤33，由已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p通过车辆运动学模型预测未来一段采样时间内相对位姿变化量，即预测量z_p：

其中：

步骤34，再次利用UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp，最终系统输出的履带车辆滑动参数R＝R_p。

本发明的有益效果，不仅能够实时估计履带车辆滑动参数，而且利用DUKF对车辆未来的相对位姿进行滑动参数估计进而对初步估计的滑动参数进行补偿，提升了滑动参数估计的精度与车辆模型的准确性，同时根据传感器采集的车辆当前时刻状态信息，实现数据的实时性，因而边能够更好的实现实时估计了。

附图说明

图1为本发明实现智能车辆横向控制的总体框架图；

图2为本发明提出的履带车辆模型。

具体实施方式

下面结合附图以及具体实施例对本发明做进一步的说明。

一种基于前向轨迹预测补偿的DUKF智能履带车辆滑动参数实时估计方法，如图1所示，所述一种基于前向轨迹预测补偿的DUKF智能履带车辆滑动参数实时估计方法包括以下步骤：

步骤1，建立基于瞬时转向中心的履带车辆运动学模型；

步骤2，根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数；

步骤3，将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数。

进一步地，所述步骤1中，所述建立基于瞬时转向中心的履带车辆运动学模型具体为：

其中：

式中，x、y分别为车辆的横纵坐标，θ、β、γ分别为车辆的横摆角、俯仰角和侧倾角，V_x、V_y分别为车辆纵向速度与横向速度，ω为车辆横摆角速度，x_c为车辆瞬时转向中心，y^l、y^r分别为左右侧履带瞬时转向中心，v^l、v^r分别为车辆左右侧履带接地段纵向速度。

进一步地，所述步骤2中，所述根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数具体为：

步骤21，令滑动参数R_h＝(y^l,y^r,x_c)^T作为非线性履带车辆系统的状态向量，系统输入为u＝(v^l,v^r,θ,β,γ)^T，观测量为Z＝(Δx,Δy)，预测量为Z_h＝(Δx_h,Δy_h)，系统输入u为车辆历史状态信息，观测量Z为每个采样间隔车辆历史位姿变化量；预测量Z_h为每个采样间隔将已选取的Sigma点、车辆历史状态信息与滑动参数R_h输入车辆运动学模型得到的相对位姿变化量，利用UKF算法处理测量预测量Z_h、观测量Z与非线性履带车辆系统的状态向量R_h，通过迭代运算初步估计履带车辆滑动参数y^l、y^r、x_c，UKF算法如下：

令x＝R_h，初始均值和方差为：

计算Sigma点并将产生的2n+1个Sigma点组成向量x_k(k＝0,1,2…2n)：

式中，λ为分布尺寸且λ＝α²(n+k)-n，α为一个缩放比例因子，可调整Sigma点与

的距离；

通过加权得到状态与方差的一步预测值：

其中：

式中，W_i ^m与W_i ^c分别为均值、方差的权，β包含x先验分布的高阶矩信息，对于高斯分布，β＝2是最优的；

由已选取的Sigma点、车辆历史状态信息与滑动参数R输入车辆运动学模型预测得到的车辆历史时刻的相对位姿变化量，即计算预测量Z_h＝(Δx_h,Δy_h)：

其中：

式中，Δt为系统采样时间；

通过加权得到系统的一步预测观测值：

量测更新。求系统输出的方差阵与协方差矩阵：

式中，R为测量噪声协方差；

更新滤波协方差阵与系统状态：

R_h＝x＝(y^l,y^r,x_c)^T；

进一步地，所述步骤3中，所述将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数。具体为：

步骤31，令滑动参数

作为非线性履带车辆系统的状态向量，下层UKF迭代初值R_p0＝R_h，系统输入为u_p＝(v^l,v^r,θ,β,γ)^T，观测量为z＝(Δx_pv,Δy_pv)，预测量z_p＝(Δx_pR,Δy_pR)，系统输入u_p为车辆当前时刻的状态信息，观测量z为利用车辆当前时刻绝对速度与横摆角速度来预测未来一段采样间隔内车辆位姿的变化量，预测量z_p为将已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p输入车辆运动学模型得到未来一段采样时间内相对位姿变化量,再次利用上文所述的UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp。；

步骤32，利用车辆当前时刻的绝对速度与横摆角速度来预测未来一段采样时间内车辆位姿的变化量，即计算观测量z：

θ_k＝ω_v*k*Δt+θ₀

式中，V、ω_v分别为传感器测得的当前时刻车辆绝对速度与角速度，θ₀为车辆初始横摆角，θ_k为预测的第k个采样时间车辆横摆角；

由已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p输入车辆运动学模型预测未来一段采样时间内相对位姿变化量，即预测量z_p：

其中：

步骤33，再次利用UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp，最终系统输出的履带车辆滑动参数R＝R_p。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种智能履带车辆滑动参数实时估计方法，其特征在于：包括如下步骤：

步骤1，建立基于瞬时转向中心的履带车辆运动学模型；

步骤2，根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数；

步骤3，将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数。

2.根据权利要求1所述的智能履带车辆滑动参数实时估计方法，其特征在于：所述步骤1中，建立基于瞬时转向中心的履带车辆运动学模型具体为：

其中：

式中，x、y分别为车辆的横纵坐标，θ、β、γ分别为车辆的横摆角、俯仰角和侧倾角，V_x、V_y分别为车辆纵向速度与横向速度，ω为车辆横摆角速度，x_c为车辆瞬时转向中心，y^l、y^r分别为左右侧履带瞬时转向中心，v^l、v^r分别为车辆左右侧履带接地段纵向速度。

3.根据权利要求1或2所述的智能履带车辆滑动参数实时估计方法，其特征在于：所述步骤2中，所述根据传感器采集的车辆历史状态信息，利用所述基于瞬时转向中心的履带车辆运动学模型预测车辆历史时刻的相对位姿变化量，并将其与车辆历史位姿信息同时输入UKF进行迭代运算初步估计履带车辆滑动参数具体步骤为：

步骤21，令滑动参数R_h＝(y^l,y^r,x_c)^T作为非线性履带车辆系统的状态向量，系统输入为u＝(v^l,v^r,θ,β,γ)^T，观测量为Z＝(Δx,Δy)，预测量为Z_h＝(Δx_h,Δy_h)，系统输入u为车辆历史状态信息，观测量Z为每个采样间隔车辆历史位姿的变化量；预测量Z_h为每个采样间隔将已选取的Sigma点、车辆历史状态信息与滑动参数R_h输入车辆运动学模型得到的相对位姿变化量。利用UKF算法处理预测量Z_h、观测量Z与非线性履带车辆系统的状态向量R_h，通过迭代运算初步估计履带车辆滑动参数y^l、y^r、x_c

步骤22，由已选取的Sigma点、车辆历史状态信息与滑动参数R_h输入车辆运动学模型预测车辆历史时刻的相对位姿预测，即计算预测量Z_h＝(Δx_h,Δy_h)：

其中：

式中，Δt为系统采样时间；

步骤23，利用UKF算法处理测量预测量Z_h、观测量Z与非线性履带车辆系统的状态向量R_h，通过迭代运算初步估计履带车辆滑动参数y^l、y^r、x_c。

4.根据权利要求1或2所述的智能履带车辆滑动参数实时估计方法，其特征在于：所述步骤3中，将步骤2初步估计的履带车辆滑动参数作为第二层UKF的初值，并根据传感器采集的车辆当前时刻状态信息，分别利用车辆绝对速度与所述基于瞬时转向中心的履带车辆运动学模型预测车辆未来时刻的相对位姿变化量，将两者同初步估计的滑动参数一起输入第二层UKF进行迭代运算得到最终的履带车辆滑动参数具体步骤为：

步骤31，令滑动参数

作为非线性履带车辆系统的状态向量，下层UKF迭代初值R_p0＝R_h，系统输入为u_p＝(v^l,v^r,θ,β,γ)^T，观测量为z＝(Δx_pv,Δy_pv)，预测量z_p＝(Δx_pR,Δy_pR)，系统输入u_p为车辆当前时刻的状态信息，观测量z为利用车辆当前时刻绝对速度与横摆角速度来预测未来一段采样间隔内车辆位姿的变化量，预测量z_p为将已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p输入车辆运动学模型得到未来一段采样间隔内相对位姿变化量，再次利用UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp；

步骤32，利用车辆当前时刻的绝对速度与横摆角速度来预测未来一段采样时间内车辆位姿的变化量，即计算观测量z：

θ_k＝ω_v*k*Δt+θ₀

式中，V、ω_v分别为传感器测得的当前时刻车辆绝对速度与角速度，θ₀为车辆初始横摆角，θ_k为预测的第k个采样时间车辆横摆角；

步骤33，由已选取的Sigma点、车辆当前时刻的状态信息u_p与滑动参数R_p通过车辆运动学模型预测未来一段采样时间内相对位姿变化量，即预测量z_p：

其中：

步骤34，再次利用UKF算法处理测量预测量z_h、观测量z与非线性履带车辆系统的状态向量R_p，通过迭代运算估计补偿后的履带车辆滑动参数

x_cp，最终系统输出的履带车辆滑动参数R＝R_p。

Images