【状态估计】基于拓展卡尔曼滤波的自力车建立数学模型和状态估计器附matlab代码

某大厂资深算法工程师，从事Matlab算法仿真工作10年,擅长智能优化算法、神经网络预测、机器学习、信号处理、元胞自动机、图像处理、路径规划、无人机、无线传感器网络、车间调度、生产调度等多种领域的Matlab仿真，更多仿真源码、算法改进、Matlab项目和期刊发表可私信合作。

本文链接：https://blog.csdn.net/m0_60703264/article/details/146093042

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

自平衡车，作为一种新兴的个人交通工具，因其便携、灵活等优点而备受关注。其核心技术在于精确的状态估计和有效的控制算法，以维持车辆在动态环境下的平衡。其中，状态估计至关重要，它为控制系统提供准确的状态信息，从而实现精准的平衡控制。本文将探讨基于拓展卡尔曼滤波（Extended Kalman Filter, EKF）的自平衡车数学建模与状态估计器设计，详细阐述建模过程、滤波器构建以及性能分析，旨在为自平衡车的控制系统设计提供理论依据和实践指导。

一、自平衡车的数学建模

精确的数学模型是状态估计的基础。自平衡车可以抽象为一个倒立摆系统，其运动受到重力、电机驱动力以及各种摩擦力的影响。为了简化分析，我们做出以下假设：

车辆在二维平面内运动，忽略侧向运动和扭转。
电机响应迅速，忽略电机动力学。
轮胎与地面之间为纯滚动，忽略滑动。
重心位于车辆的中心线上。

基于以上假设，我们可以建立自平衡车的非线性动力学模型。定义以下状态变量：

θ：车身倾角，表示车身与垂直方向的夹角。
ω：车身角速度，表示车身倾角的变化率。
φ：轮子角度，表示轮子的旋转角度。
v：车速，表示车辆的水平移动速度。

根据牛顿第二定律和力矩平衡原理，可以得到以下状态方程：

角度方程:
css

J * d²θ/dt² = m * g * l * sin(θ) + τ - b * ω

其中：
- J: 车身转动惯量。
- m: 车身质量。
- g: 重力加速度。
- l: 重心到轮子的垂直距离。
- τ: 电机产生的扭矩。
- b: 阻尼系数。
轮子角度方程:
ini

r * dφ/dt = v

其中：
- r: 轮子半径。
速度方程:
scss

(M + m) * dv/dt = τ/r - f * v - m * l * ω * cos(θ) * dθ/dt - m * l * sin(θ) * d²θ/dt²

其中：
- M: 车轮和电机总质量。
- f: 摩擦力系数。

将上述微分方程组转化为状态空间表达式：

scss

x(t) = [θ, ω, φ, v]^T ẋ(t) = f(x(t), u(t))

其中：

x(t): 状态向量。
u(t): 控制输入，通常为电机电压或扭矩。
f(x(t), u(t)): 非线性状态函数，由上述微分方程组导出。

二、拓展卡尔曼滤波器(EKF)的设计

由于自平衡车的动力学模型是非线性的，直接应用标准卡尔曼滤波会产生较大的误差。因此，需要使用拓展卡尔曼滤波(EKF)来处理非线性问题。EKF通过对非线性函数进行一阶泰勒展开，将其线性化，然后应用标准卡尔曼滤波算法进行状态估计。

EKF的主要步骤如下：

状态预测： 利用上一时刻的状态估计值和控制输入，预测当前时刻的状态：
scss

x̂(k|k-1) = f(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)

其中：
- x̂(k|k-1): k时刻的状态预测值。
- 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)的雅可比矩阵，在x̂(k-1|k-1)处计算。
- Q(k-1): 过程噪声协方差矩阵，表示模型误差。
观测预测： 根据状态预测值，预测当前时刻的观测值：
scss

ẑ(k|k-1) = h(x̂(k|k-1))

其中：
- ẑ(k|k-1): k时刻的观测预测值。
- h(x): 观测函数，描述状态向量与观测值之间的关系。例如，如果使用陀螺仪测量角速度，加速度计测量倾角，则h(x) = [θ, ω]^T。
卡尔曼增益计算： 计算卡尔曼增益，用于权衡预测值和观测值的可信度：
scss

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的雅可比矩阵，在x̂(k|k-1)处计算。
- R(k): 观测噪声协方差矩阵，表示传感器噪声。
状态更新： 根据实际观测值和卡尔曼增益，更新状态估计值：
scss

x̂(k|k) = x̂(k|k-1) + K(k) * (z(k) - ẑ(k|k-1))

其中：
- x̂(k|k): k时刻的状态估计值。
- z(k): k时刻的实际观测值。
协方差更新： 更新估计误差协方差矩阵：
scss

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].