基于弯道自适应强跟踪卡尔曼滤波的侧向坡度估计算法

摘要

道路的侧向坡度直接影响车辆侧向运动，侧向坡度估计已成为智能汽车稳定控制系统的关键部分之一。然而，侧向坡度与车身侧倾之间存在耦合，且侧向力估计困难，准确的侧向坡度估计难度较大。为此，提出了一种基于加速度传感器的可拓融合侧向坡度估计算法：首先，提出加速度传感器模型和车辆侧倾模型，采用弯道自适应强跟踪卡尔曼滤波算法（CASTKF）对侧向坡度进行估计；然后，提出基于侧向加速度传感器的直接估计方法，防止CASTKF算法在失去可观性后的错误估计；再后，利用可拓算法对两种模式的估计值进行数据融合；最后，采用硬件在环测试（HIL）验证所提算法的有效性。结果表明，智能汽车的侧向坡度估计中采用CASTKF融合算法具有更高的精度和鲁棒性。

关键词

智能汽车; 侧向坡度估计; 可拓融合; 强跟踪卡尔曼滤波; 自适应滤波

随着汽车电动化和智能化水平的不断提高，主动安全系统被广泛应用于量产汽车^［1］。除侧风干扰外^［2］，道路侧向坡度作为重要的外部载荷，将直接影响车辆侧向加速度、质心侧偏角等关键行驶参数的估计^［3］，进而影响电子稳定系统（ESP）等控制系统的状态^［4］。目前已有部分学者对侧向坡度估计问题展开研究，采用的估计方法可主要有：直接方法，即利用车载传感器进行估计；间接方法，即基于侧向动力学模型进行估计^［5］。

直接方法主要使用GPS、惯性测量单元（inertial measurement unit， IMU）等对侧向坡度进行估计。David等^［6］利用GPS提供的三维速度信息，可直接实现道路坡度的估计。熊璐等^［7］也提出利用卫星导航系统和惯性导航系统对路面坡度等状态进行解算。但GPS成本较高，实时性差，目前仍很难应用于量产车上^［8］。Minje等^［9］在计算车身侧倾角后，利用贝叶斯跟踪方法，通过比较IMU实测的侧向加速度和由车辆运动状态估算的侧向加速度，对侧向坡度进行估计，但估计结果对噪声敏感。Liao等^［10］基于侧向加速度传感器模型，利用卡尔曼滤波算法（Kalman filter， KF）对侧向坡度进行估计，但并未考虑车身侧倾角对估计的影响，且剧烈工况下的模型误差可能导致KF算法发散^［11］。直接方法不受车辆参数和路面附着影响，但IMU信号中存在侧向坡度与车身侧倾的耦合，估计结果受行驶状况影响。

间接方法主要基于车辆侧向动力学进行估计。Jeong等^［12］基于包括侧倾的3自由度车辆模型，并考虑模型的参数不确定性，利用有限记忆滤波器对侧向坡度进行估计。Boada等^［13］基于相同的3自由度车辆模型，利用双模扩展卡尔曼滤波算法对侧向坡度等关键参数进行估计，并利用概率密度截断方法，考虑估计值的范围限制。Kim等^［14］基于改进的单轨模型，即车辆模型中不体现驱动轮的侧偏刚度，以降低系统不确定性，并利用比例积分$\mathrm{H}\mathrm{\infty }$滤波器对侧向坡度进行估计。间接方法对传感器的敏感程度低，但车轮侧向力的获取十分困难，线性轮胎模型只有在小侧偏角时才有较高的精度；非线性轮胎模型较为复杂，估计精度将严重依赖轮胎参数的准确程度^［15］，算法的适应性较差。

本文提出一种基于侧向加速度传感器的侧向坡度估计方法。针对传感器信号中侧向坡度与车身侧倾的耦合问题，引入瞬态侧倾模型，防止侧倾角对侧坡估计产生影响。为提高算法在弯道工况下的估计精度，防止由于模型误差导致发散，利用弯道自适应强跟踪卡尔曼滤波算法（curve adaptive strong tracking Kalman filter， CASTKF）实现侧向坡度与侧倾角的同步估计。由于CASTKF方法在直线工况下将失去可观性，提出利用加速度传感器直接对侧坡进行计算，并利用可拓算法进行数据融合。最后进行硬件在环实验（hard wire in loop test， HIL），验证算法的有效性。

1 传感器模型及车辆模型

1.1　侧向加速度传感器模型

基于微电子机械系统加速度传感器工作原理，当车辆在侧坡上运动时，侧向加速度传感器信号将同时包含侧向坡度信息和车身的侧倾信息，即：

$$a_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}=a_y+g\mathrm{s}\mathrm{i}\mathrm{n}\left({\varphi }_r+{\varphi }_v\right)+e_{\mathrm{1}}$$

（1）

式中：$a_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}$为侧向加速度传感器信号值；$a_y$为车辆侧向加速度；${\varphi }_v$为车身相对车架坐标系$x_f$轴的侧倾角；${\varphi }_r$为相对地面坐标系$x_g$轴的侧向坡度角（见图1）；$e_{\mathrm{1}}$为纵向加速度传感器误差。由于${\varphi }_r$和${\varphi }_v$的实际值较小，且将$\mathrm{s}\mathrm{i}\mathrm{n}{\varphi }_r$视为状态变量，则式（1）可进一步简化为以下线性方程：

$$a_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}=a_y+g\left(\mathrm{s}\mathrm{i}\mathrm{n}{\varphi }_r+{\varphi }_v\right)+e_{\mathrm{1}}$$

（2）

a  车辆坐标系

b  车身侧倾模型

图1  车辆坐标系与侧倾模型

Fig.1  Relevant vehicle models

由于实际道路修建过程中坡度变化率较小^［16］，因此假设道路坡度为慢变量，即${\dot{\varphi }}_r\approx \stackrel{}{\left(\mathrm{s}\mathrm{i}\mathrm{n}{\varphi }_r\right)}=\mathrm{0}$，则各坐标系下的侧倾角速度满足^［17］：

$${\dot{\varphi }}_s={\dot{\varphi }}_v+{\dot{\varphi }}_r+e_{\mathrm{2}}$$

（3）

式中：$e_{\mathrm{2}}$为量测噪声，${\dot{\varphi }}_s$为IMU测量的侧倾角速度，可近似作为${\dot{\varphi }}_v$的量测值。

车辆运动学公式如下：

$$\begin{array}{l}{\dot{v}}_x=a_x+\dot{\psi }{v}_y\\ {\dot{v}}_y=a_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}-g\left(\mathrm{s}\mathrm{i}\mathrm{n}{\varphi }_r+{\varphi }_v\right)-\dot{\psi }{v}_x-e_{\mathrm{1}}\end{array}$$

（4）

式中：$v_x$为车身坐标系下的纵向车速；$a_x$为纵向加速度；$v_y$为车身坐标系下的侧向车速；$\dot{\psi }$为横摆角速度，$\psi$为横摆角。

2.2　侧倾动力学模型

由于传感器模型信号存在车身侧倾的耦合，因此通过引入侧倾动力学模型，并考虑车身侧倾对侧向坡度估计的影响。如图1b所示，车辆在侧向坡度角为${\varphi }_r$的坡路上的侧倾动力学模型可表示为：

$$\begin{array}{l}{I}_{eq}{\ddot{\varphi }}_v=m_{s}gh\mathrm{s}\mathrm{i}\mathrm{n}\left({\varphi }_v+{\varphi }_r\right)+\\ m_s{a}_{y}h-K_{\varphi }{\varphi }_v-C_{\varphi }{\dot{\varphi }}_v\end{array}$$

（5）

式中：$I_{eq}=I_x+m_s{h}^{\mathrm{2}}$为等效转动惯量；$I_x$为车身绕侧倾轴线的转动惯量；$m_s$为簧上质量；$h$为质心至侧倾中心的高度；$K_{\varphi }$是车身侧倾刚度；$C_{\varphi }$是车身侧倾阻尼系数。将式（1）代入式（5）则有：

$${\ddot{\varphi }}_v=\frac{m_{s}h{a}_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}-K_{\varphi }{\varphi }_v-C_{\varphi }{\dot{\varphi }}_v}{I_{eq}}$$

（6）

2 弯道自适应强跟踪卡尔曼滤波算法

2.1　卡尔曼滤波算法

由式（3）、式（4）、式（6）构成的线形系统离散状态方程可表示为：

$$\begin{array}{l}{\mathit{x}}_{k+\mathrm{1}}={\mathit{A}}_k{\mathit{x}}_k+{\mathit{B}}_k{\mathit{u}}_k+{\mathit{w}}_k\\ {\mathit{z}}_k={\mathit{H}}_k{\mathit{x}}_k+{\mathit{v}}_k\end{array}$$

（7）

式中：${\mathit{x}}_k={\left[\begin{array}{ccccc}{v}_{x,k}& v_{y,k}& {\varphi }_{sr,k}& {\varphi }_{v,k}& {\dot{\varphi }}_{v,k}\end{array}\right]}^{\mathrm{T}}$；${\mathit{z}}_k={\left[\begin{array}{cc}{v}_{x,k}& {\dot{\varphi }}_{s,k}\end{array}\right]}^{\mathrm{T}}$；${\mathit{A}}_k$为$k$时刻状态矩阵；${\mathit{B}}_k$为$k$时刻输入矩阵；${\mathit{H}}_k$为$k$时刻输出矩阵。${\mathit{w}}_k$、${\mathit{v}}_k$分别为$k$时刻的系统噪声和量测噪声，其协方差矩阵分别为${\mathit{Q}}_k$和${\mathit{R}}_k$。可利用KF算法对各状态量进行估计。

3.2　强跟踪卡尔曼滤波

对于KF算法而言，迭代过程过度依赖过去数据，但在急转弯等状态突变的工况下，较大的模型误差将导致估计值不准确甚至估计发散。因此本文提出采用强跟踪卡尔曼滤波器（STKF）对侧向坡度进行估计，同时结合自适应算法，进一步提高转弯工况下的估计精度。

首先定义信息序列${\mathit{d}}_k$为：

$${\mathit{d}}_k={\mathit{z}}_k-{\mathit{H}}_k\cdot {\widehat{\mathit{x}}}_{k/k-\mathrm{1}}$$

（8）

与KF算法不同，STKF算法在计算先验误差协方差矩阵${\mathit{P}}_{k/k-\mathrm{1}}$时引入衰减因子${\lambda }_k$，防止KF算法过度依赖过去的数据^［18］：

$${\mathit{P}}_{k/k-\mathrm{1}}={\mathit{\lambda }}_k{\mathit{A}}_k{\mathit{P}}_{k-\mathrm{1}}{\mathit{A}}_k^{\mathrm{T}}+{\mathit{Q}}_{k-\mathrm{1}}$$

（9）

式中：$\mathit{P}$为误差协方差矩阵；${\mathit{\lambda }}_k$为对角矩阵，主对角线元素为${\lambda }_{i,k}$，$(i=\mathrm{1,2},\cdot \cdot \cdot ,\mathrm{5})$，计算式为

$${\lambda }_{i,k}=\left\{\begin{array}{l}{\sigma }_i{c}_k,{\sigma }_i{c}_k>\mathrm{1}\\ \mathrm{1},{\sigma }_i{c}_k\le \mathrm{1}\end{array}\right.$$

（10）

式中的${\sigma }_i$为先验确定的常数，且${\sigma }_i\ge \mathrm{1}$，$(i=\mathrm{1,2},\cdot \cdot \cdot ,\mathrm{5})$。$c_k$被定义为：

$$c_k=\frac{t_r\left[{\mathit{N}}_k\right]}{{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{5}}}{\sigma }_i{\mathit{M}}_k}$$

（11）

式中的${\mathit{M}}_k$和${\mathit{N}}_k$被定义为：

$${\mathit{N}}_k={\mathit{V}}_k-{\mathit{R}}_k-{\mathit{H}}_k{\mathit{Q}}_{k-\mathrm{1}}{\mathit{H}}_k^{\mathrm{T}}$$

（12）

$${\mathit{M}}_k={\mathit{H}}_k{\mathit{A}}_k{\mathit{P}}_{k-\mathrm{1}}{\mathit{A}}_k^{\mathrm{T}}{\mathit{H}}_k^{\mathrm{T}}$$

（13）

$${\mathit{V}}_k=\left\{\begin{array}{l}{\mathit{d}}_{\mathrm{1}}{\mathit{d}}_{\mathrm{1}}^{\mathrm{T}},k=\mathrm{1}\\ \frac{\rho {\mathit{V}}_{k-\mathrm{1}}+{\mathit{d}}_k{\mathit{d}}_k^{\mathrm{T}}}{\mathrm{1}+\rho },k>\mathrm{1}\end{array}\right.$$

（14）

式中：${\mathit{V}}_k$为新息协方差矩阵；$\rho$为遗忘因子，一般取为0.95。

3.3　弯道自适应强跟踪卡尔曼滤波（CASTKF）

根据STKF的过程，若${\mathit{R}}_k$与真实值相差过大，则将引入错误的${\mathit{\lambda }}_k$，导致估计精度反而下降^［19］。文献［20］提出的利用Sage-Husa算法对噪声进行估计，但很难保证在剧烈工况下的实时性和${\mathit{R}}_k$估计值的合理性。因此，本文提出弯道自适应调整方法，利用横摆角速度$\dot{\psi }$和方向盘转角$\delta$判断弯道情况，自适应调节系统的量测协方差矩阵${\mathit{R}}_k$。

当车辆处于较为剧烈的侧向运动时，式（6）表示的侧倾模型精度将下降，但式（3）表示的量测模型精度将提高。可利用横摆角速度$\dot{\psi }$和方向盘转角$\delta$表示侧向运动的剧烈程度，即横摆角速度$\dot{\psi }$和方向盘转角$\delta$较大时，量测噪声协方差矩阵${\mathit{R}}_k$应自适应地减小，使算法更趋近于量测的结果：

$${\mathit{R}}_k=\left[\begin{array}{cc}{c}_{r\mathrm{11}}& \mathrm{0}\\ \mathrm{0}& c_{r\mathrm{21}}+c_{r\mathrm{22}}\left|\dot{\psi }\right|+c_{r\mathrm{23}}\left|\delta \right|\end{array}\right]$$

（15）

式中：$c_{r\mathrm{11}}$、$c_{r\mathrm{21}}$、$c_{r\mathrm{22}}$、$c_{r\mathrm{23}}$分别为为通过试验或仿真先验确定的各调节参数^［21］。CASTKF的算法流程如图2所示。

图2  CASTKF算法流程图

Fig.2  Flowchart of CASTKF algorithm

3.4　直接计算法

当横摆角速度$\dot{\psi }$长时间为0时，即当车辆在直线道路上行驶时，CASTKF算法将失去可观性，无法提供准确的估计值，因此引入直接计算法对CASTKF算法进行补充。对式（4）进一步简化，当车辆行驶在带有一定侧坡的直线道路上时，不存在车辆的侧向运动，因此$v_y\approx \mathrm{0}$，$\dot{\psi }\approx \mathrm{0}$，${\varphi }_v\approx \mathrm{0}$，对加速度传感器信号进行低通滤波后，可直接对直线道路下的侧向坡度${\widehat{\varphi }}_r{\_}_{\mathrm{d}\mathrm{i}\mathrm{r}}$进行计算：

$${\widehat{\varphi }}_r{\_}_{\mathrm{d}\mathrm{i}\mathrm{r}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{s}\mathrm{i}\mathrm{n}\left(\frac{a_y{\_}_{\mathrm{s}\mathrm{e}\mathrm{n}}}{g}\right)$$

（16）

为防止模式的突然切换造成估计结果的不连续，本文采用可拓算法判断直线行驶工况，计算融合系数，实现两种估计算法的平稳切换。本文提出的侧向坡度估计算法结构如图3所示。

图3  侧向坡度估计算法整体结构

Fig.3  Structure of road bank angle estimation algorithm

4 可拓融合算法

可拓融合算法可根据车辆行驶状态判断各融合域内的融合系数。与利用门限值直接进行模式切换相比，融合算法可以保证不同模式的平稳切换，提高估计精度；与模糊融合算法相比，可拓融合算法复杂度低，计算负担更小。可拓融合算法的计算步骤主要包括：

（1） 特征量的选取。如图4所示，为保证在直线行驶工况下采用直接计算法对侧坡进行估计，本文以车辆横摆角速度$\dot{\psi }$和方向盘转角速度$\dot{\delta }$为特征量，判断直线行驶工况，并建立$\dot{\psi }-\dot{\delta }$的可拓空间。并选取适当的经典域边界$\pm {\dot{\psi }}_{\mathrm{1}}$、$\pm {\dot{\delta }}_{\mathrm{1}}$，与可拓域边界$\pm {\dot{\psi }}_{\mathrm{2}}$、$\pm {\dot{\delta }}_{\mathrm{2}}$，将估计状态分为经典域、可拓域和非域，在不同融合域内采用不同的侧坡计算方法。

图4  可拓融合域的划分

Fig.4  Division of extension fusion domain

（2） 计算关联函数$K(S)$。设点$Q_{\mathrm{0}}$为可拓集合中任意一点，与$O$点连接并延长，与可拓域和经典域的边界相交于$Q_{\mathrm{1}}$、$Q_{\mathrm{2}}$、$Q_{\mathrm{3}}$、$Q_{\mathrm{4}}$，即将二维可拓域转化为一维可拓域，并以代表一维可拓集内的经典域；代表一维可拓集内的可拓域。定义关联函数$K(S)$为：

$$K(S)=\frac{\rho \left(Q_{\mathrm{0}},X_j\right)}{D\left(Q_{\mathrm{0}},X_k,X_j\right)}$$

（17）

式中：$\rho \left(Q_{\mathrm{0}},X_j\right)$、$\rho \left(Q_{\mathrm{0}},X_k\right)$分别为为点到经典域和可拓域的可拓距^［22］，且$D\left(Q_{\mathrm{0}},X_k,X_j\right)=\rho \left(Q_{\mathrm{0}},X_j\right)-\rho \left(Q_{\mathrm{0}},X_k\right)$。

（3） 融合系数的确定。与利用横摆角速度门限直接对两种计算方法进行切换不同，利用融合系数对两种算法估计结果进行数据融合处理：

$${\widehat{\varphi }}_r=k\cdot {\widehat{\varphi }}_r{\_}_{\mathrm{e}\mathrm{s}\mathrm{t}}+(\mathrm{1}-k)\cdot {\widehat{\varphi }}_r{\_}_{\mathrm{d}\mathrm{i}\mathrm{r}}$$

（18）

式中：$k$即为融合系数；${\widehat{\varphi }}_r{\_}_{\mathrm{e}\mathrm{s}\mathrm{t}}$为CASTKF方法的估计值；${\widehat{\varphi }}_r{\_}_{\mathrm{d}\mathrm{i}\mathrm{r}}$为直接计算法的估计值。如表1所示，可利用关联函数$K(S)$划分$Q_{\mathrm{0}}$所处的融合域，确定两种估计值的融合系数。在经典域内，$\dot{\psi }$和$\dot{\delta }$均较小，可基本确定车辆处于直线行驶状态，因此以直接计算法的结果作为最终估计结果；在非域内，$\dot{\psi }$和$\dot{\delta }$均较大，可基本确定车辆处于转弯工况，因此采用CASTKF方法进行估计；在可拓域内，为防止两种模式跳变产生估计结果的不连续，因此以$K(S)$作为融合系数，对两种方法的估计结果进行数据融合。

表1  不同融合域下的关联函数与融合系数

Tab.1  Correlation function and fusion coefficient at different fusion domains

融合域	$K(S)$	k
经典域	$K(S)<0$	0
可拓域	$0\le K(S)\le 1$	$K(S)$
非域	$K(S)>1$	1

5 硬件在环仿真验证

利用硬件在环测试验证提出算法的有效性，测试台架结构如图5所示。选择某牵引车为试验测试车辆，实验车辆主要相关参数如表2所示。本文提出的侧坡估计算法将在3种工况下验证：第一种工况为水平路面下的双移线工况，即验证剧烈侧倾运动对估计算法的影响情况；第二种工况为10%侧坡的环形跑道上的加速行驶工况，即验证在转弯工况下侧向坡度的估计情况；第三种工况为10%侧坡的直线道路上的加速行驶工况，即验证在系统失去可观性下侧向坡度的估计情况。

图5  硬件在环测试结构

Fig.5  Scheme of hardware-in-loop simulation test

表2  整车参数表

Tab.2  Vehicle parameters

参数	参数值	参数	参数值
簧上质量/kg	4457	车轮半径/mm	510
前轴非簧上质量/kg	570	主减速器传动比	4.4
后轴非簧上质量/kg	735	轮距/mm	2030
轴距/mm	5013	整备质量/t	6.5

为了突出算法的有效性，以KF的可拓融合方法、未考虑侧倾运动的KF方法^［10］作为对比，并用平均绝对误差$\mathrm{M}\mathrm{A}\mathrm{E}$、均方根误差$\mathrm{R}\mathrm{M}\mathrm{S}\mathrm{E}$定量表示算法的精确程度。

5.1　水平路面双移线工况下的侧向坡度估计

试验结果如图6所示，车辆以70 km/h的速度进行双移线试验，提出的CASTKF可拓融合方法、KF可拓融合方法和不考虑侧倾KF方法的MAE、RMSE表3所示。

a  侧倾角对比

b  侧向坡度对比

图6  双移线工况下的侧向坡度估计结果

Fig.6  Road bank estimation results in double line change test

表3  双移线工况下的MAE、RMSE误差

Tab.3  MAE and RMSE errors in double line change test

误差	估计方法
	不考虑侧倾KFEKF	KF可拓融合	CASTKF 可拓融合
MAE	0.26	0.17	0.13
RMSE	0.40	0.25	0.17

图6a为真实侧倾角和估计侧倾角的对比，由于双移线工况较为剧烈，提出的车身侧倾模型存在一定的简化，因此真实值与估计值之间存在较小的误差；图6b为各种方法的侧向坡度估计值，在双移线工况下，车身存在较大的侧倾运动，因此不考虑侧倾运动的KF方法误差最大；在剧烈工况下，KF算法的估计精度将下降，CASTKF融合算法的估计精度明显高于KF融合算法。

5.2　10%侧坡的环形跑道侧向坡度估计

车辆以全挡位，50 km/h全油门加速，试验道路为带有10%侧向坡度的环形跑道。试验结果如图7所示，各算法的MAE、RMSE如表4。

a  侧倾角对比

b  侧向坡度对比

图7  10%侧坡的环形跑道侧向坡度估计结果

Fig.7  Road bank estimation results of annular runway with a 10% side slope

表4  10%侧坡的环形跑道下的MAE、RMSE误差

Tab.4  MAE and RMSE errors of annular runway with a 10% side slope

估计方法	MAE	RMSE
不考虑侧倾KF	2.32	2.86
KF可拓融合	0.69	1.99
CASTKF可拓融合	0.53	1.72

图7a为实际侧倾角与估计侧倾角的对比，随着车辆不断加速，车身侧倾角逐渐变大；图7b为各个算法的侧向坡度估计值。由于初始过程中车辆尚未处于稳定，因此三种算法都有较大的误差，但CASTKF算法抖动更小且收敛更快。对于不考虑侧倾的KF算法，侧倾角将对侧向坡度的估计产生影响，且使最终的收敛结果绝对值偏小。

5.3　10%侧坡的直线道路侧向坡度估计

车辆以全挡位，50 km/h全油门加速，试验道路为有10%侧向坡度的直线道路，验证在CASTKF算法失去可观性后的算法有效性。试验结果如图8，各算法的MAE、RMSE如表5所示。

a  实际侧倾角

b  侧向坡度对比

图8  10%侧坡的直线道路侧向坡度估计结果

Fig.8  Road bank estimation results of straight road with a 10% side slope

表5  10%侧坡的直线道路下侧坡估计MAE、RMSE误差

Tab.5  MAE and RMSE errors of straight road with a 10% side slope

估计方法	MAE	RMSE
不考虑侧倾KF	5.94	6.29
KF可拓融合	1.04	2.60
CASTKF可拓融合	1.03	2.59

如图8a所示，车辆在带有一定侧向坡度的直线道路上行驶时，仍然存在较小的侧倾运动，但车身侧倾角较小，可以忽略不计；如图8b所示，对于未考虑侧倾的KF算法，由于算法已经失去可观性，因此估计结果与真实值存在较大的差距。此时两种融合算法的输出结果基本为直接计算法的结果，在初始阶段存在模式的切换，两种算法的误差值稍有区别。

6 结语

本文提出了一种考虑车身侧倾的基于CASTKF的可拓融合侧向坡度估计方法。与KF方法相比，STKF可以增加观测器对突变状态的跟踪能力，防止模型误差导致估计结果的错误甚至发散；CASTKF可以在此基础上根据车辆状态自适应调整量测的协方差矩阵，提高算法的估计精度。由于CASTKF方法在直线行驶时失去可观性，本文提出直接计算法，即基于侧向加速度传感器对侧向坡度进行计算。之后利用可拓融合算法判断行驶工况，对两种估计结果进行数据融合。最终的硬件在环仿真结果表明该算法在各种工况下都能实现较好的侧向坡度估计，估计精度和性能明显好于KF的可拓融合算法和未考虑车身侧倾的算法。本文提出的算法对提高侧向坡度估计精度具有较强的参考意义。