基于扩展卡尔曼滤波的自平衡车状态估计与建模
基于扩展卡尔曼滤波的自平衡车状态估计与建模
自平衡车，作为一种新兴的个人交通工具，因其便携、灵活等优点而备受关注。其核心技术在于精确的状态估计和有效的控制算法，以维持车辆在动态环境下的平衡。其中，状态估计至关重要，它为控制系统提供准确的状态信息，从而实现精准的平衡控制。本文将探讨基于拓展卡尔曼滤波（Extended Kalman Filter, EKF）的自平衡车数学建模与状态估计器设计，详细阐述建模过程、滤波器构建以及性能分析，旨在为自平衡车的控制系统设计提供理论依据和实践指导。
一、自平衡车的数学建模
精确的数学模型是状态估计的基础。自平衡车可以抽象为一个倒立摆系统，其运动受到重力、电机驱动力以及各种摩擦力的影响。为了简化分析，我们做出以下假设：
- 车辆在二维平面内运动，忽略侧向运动和扭转。
- 电机响应迅速，忽略电机动力学。
- 轮胎与地面之间为纯滚动，忽略滑动。
- 重心位于车辆的中心线上。
基于以上假设，我们可以建立自平衡车的非线性动力学模型。定义以下状态变量：
- $\theta$：车身倾角，表示车身与垂直方向的夹角。
- $\omega$：车身角速度，表示车身倾角的变化率。
- $\varphi$：轮子角度，表示轮子的旋转角度。
- $v$：车速，表示车辆的水平移动速度。
根据牛顿第二定律和力矩平衡原理，可以得到以下状态方程：
角度方程:
$$
J \frac{d^2\theta}{dt^2} = m g l \sin(\theta) + \tau - b \omega
$$
其中：$J$: 车身转动惯量。
$m$: 车身质量。
$g$: 重力加速度。
$l$: 重心到轮子的垂直距离。
$\tau$: 电机产生的扭矩。
$b$: 阻尼系数。
轮子角度方程:
$$
r \frac{d\varphi}{dt} = v
$$
其中：$r$: 轮子半径。
速度方程:
$$
(M + m) \frac{dv}{dt} = \tau/r - f v - m l \omega \cos(\theta) \frac{d\theta}{dt} - m l \sin(\theta) \frac{d^2\theta}{dt^2}
$$
其中：$M$: 车轮和电机总质量。
$f$: 摩擦力系数。
将上述微分方程组转化为状态空间表达式：
$$
x(t) = [\theta, \omega, \varphi, v]^T
$$
$$
\dot{x}(t) = f(x(t), u(t))
$$
其中：
- $x(t)$: 状态向量。
- $u(t)$: 控制输入，通常为电机电压或扭矩。
- $f(x(t), u(t))$: 非线性状态函数，由上述微分方程组导出。
二、拓展卡尔曼滤波器(EKF)的设计
由于自平衡车的动力学模型是非线性的，直接应用标准卡尔曼滤波会产生较大的误差。因此，需要使用拓展卡尔曼滤波(EKF)来处理非线性问题。EKF通过对非线性函数进行一阶泰勒展开，将其线性化，然后应用标准卡尔曼滤波算法进行状态估计。
EKF的主要步骤如下：
- 状态预测： 利用上一时刻的状态估计值和控制输入，预测当前时刻的状态：
$$
\hat{x}(k|k-1) = f(\hat{x}(k-1|k-1), u(k-1))
$$
$$
P(k|k-1) = A(k-1) P(k-1|k-1) A(k-1)^T + Q(k-1)
$$
其中：
- $\hat{x}(k|k-1)$: k时刻的状态预测值。
- $\hat{x}(k-1|k-1)$: k-1时刻的状态估计值。
- $P(k|k-1)$: k时刻的预测误差协方差矩阵。
- $P(k-1|k-1)$: k-1时刻的估计误差协方差矩阵。
- $A(k-1)$: 状态转移矩阵，是$f(x(t), u(t))$对$x(t)$的雅可比矩阵，在$\hat{x}(k-1|k-1)$处计算。
- $Q(k-1)$: 过程噪声协方差矩阵，表示模型误差。
- 观测预测： 根据状态预测值，预测当前时刻的观测值：
$$
\hat{z}(k|k-1) = h(\hat{x}(k|k-1))
$$
其中：
- $\hat{z}(k|k-1)$: k时刻的观测预测值。
- $h(x)$: 观测函数，描述状态向量与观测值之间的关系。例如，如果使用陀螺仪测量角速度，加速度计测量倾角，则$h(x) = [\theta, \omega]^T$。
- 卡尔曼增益计算： 计算卡尔曼增益，用于权衡预测值和观测值的可信度：
$$
K(k) = P(k|k-1) H(k)^T (H(k) P(k|k-1) H(k)^T + R(k))^{-1}
$$
其中：
- $K(k)$: k时刻的卡尔曼增益矩阵。
- $H(k)$: 观测矩阵，是$h(x)$对$x$的雅可比矩阵，在$\hat{x}(k|k-1)$处计算。
- $R(k)$: 观测噪声协方差矩阵，表示传感器噪声。
- 状态更新： 根据实际观测值和卡尔曼增益，更新状态估计值：
$$
\hat{x}(k|k) = \hat{x}(k|k-1) + K(k) (z(k) - \hat{z}(k|k-1))
$$
其中：
- $\hat{x}(k|k)$: k时刻的状态估计值。
- $z(k)$: k时刻的实际观测值。
- 协方差更新： 更新估计误差协方差矩阵：
$$
P(k|k) = (I - K(k) H(k)) P(k|k-1)
$$
其中：
- $I$: 单位矩阵。
三、自平衡车 EKF 的具体实现
在自平衡车的 EKF 实现过程中，需要仔细考虑以下几个方面：
传感器选择与数据融合： 自平衡车通常使用陀螺仪、加速度计和编码器等传感器来获取状态信息。陀螺仪可以测量角速度，加速度计可以测量倾角，编码器可以测量轮子角度。为了提高状态估计的精度，需要对多种传感器的数据进行融合。EKF 能够有效地融合多种传感器的信息，利用卡尔曼增益来权衡不同传感器的可信度。
噪声参数的标定： 过程噪声协方差矩阵 $Q$ 和观测噪声协方差矩阵 $R$ 是 EKF 的关键参数。$Q$ 描述了模型的不确定性，$R$ 描述了传感器的不确定性。这些参数需要通过实验数据进行标定，才能获得最佳的滤波效果。通常，可以通过 Allan 方差分析等方法来估计传感器噪声。
状态转移矩阵 $A$ 和观测矩阵 $H$ 的计算： 由于状态方程和观测方程都是非线性的，需要计算状态转移矩阵 $A$ 和观测矩阵 $H$。$A$ 是状态函数 $f(x(t), u(t))$ 对状态向量 $x(t)$ 的雅可比矩阵，$H$ 是观测函数 $h(x)$ 对状态向量 $x$ 的雅可比矩阵。这些矩阵需要在线计算，因为它们依赖于当前的状态估计值。
离散化处理： 由于控制系统通常运行在离散时间内，需要将连续时间模型离散化。可以使用欧拉法、龙格-库塔法等方法将连续时间模型转换为离散时间模型。
四、性能分析与改进方向
EKF 在自平衡车状态估计中具有广泛的应用，但其性能受到多种因素的影响。实际应用中，需要对 EKF 的性能进行分析和改进。
收敛性分析： EKF 的收敛性是保证其稳定运行的关键。理论上，EKF 的收敛性难以保证，但通过合理的参数选择和模型改进，可以提高其收敛性。
计算复杂度分析： EKF 的计算复杂度较高，尤其是在状态维数较高的情况下。为了降低计算复杂度，可以使用简化的 EKF 算法，例如 UKF (Unscented Kalman Filter) 或 PF (Particle Filter)。UKF 使用无迹变换来逼近非线性函数，PF 使用粒子来表示概率分布。这些算法在一定程度上可以提高计算效率和估计精度。
鲁棒性分析： EKF 对模型误差和传感器噪声比较敏感。为了提高鲁棒性，可以使用自适应 EKF 算法，该算法可以根据实际情况动态调整过程噪声协方差矩阵 $Q$ 和观测噪声协方差矩阵 $R$。此外，还可以使用抗差 EKF 算法，该算法可以抑制异常值的影响。
与其他状态估计方法的比较： 除了 EKF，还有其他状态估计方法可以应用于自平衡车，例如 UKF、PF 和互补滤波。UKF 在非线性度较高的情况下通常比 EKF 具有更好的精度，但计算复杂度也更高。PF 适用于非线性度更高、非高斯噪声的情况下，但计算量巨大。互补滤波结构简单，计算量小，但精度相对较低。根据实际应用的需求，可以选择合适的状态估计方法。
运行结果
参考文献
[1] 魏克新,陈峭岩.基于多模型自适应卡尔曼滤波器的电动汽车电池荷电状态估计[J].中国电机工程学报, 2012, 32(31):8.DOI:CNKI:SUN:ZGDC.0.2012-31-002.
[2] 陆丹.基于卡尔曼滤波的汽车行驶姿态的研究[D].江苏大学,2005.DOI:10.7666/d.y827227.
[3] 何磊,宗长富,赵洪辉,等.基于扩展卡尔曼滤波的电动汽车电池状态估计[C]//Proceedings of 2010 The 3rd International Conference on Power Electronics and Intelligent Transportation System(Volume 5).0[2025-03-07].