当前位置：首页 > news >正文

两轮自平衡车原理详解与代码实现

news 2025/10/13 12:46:55

前言

两轮自平衡车（Self-Balancing Robot）是一种基于倒立摆原理的智能机器人系统，通过实时检测车身倾角并调整电机输出来保持动态平衡。本文将深入探讨平衡车的工作原理、控制算法以及具体的代码实现。

一、平衡车的物理原理

1.1 倒立摆模型

平衡车的核心原理基于倒立摆（Inverted Pendulum）模型。想象一根倒立的杆子，如果不施加控制，它会因重力而倒下。平衡车就像是一个可移动的倒立摆，通过不断调整底部（轮子）的位置来保持顶部（车身）的平衡。

1.2 动力学方程

平衡车的运动可以用以下简化的动力学方程描述：

θ̈ = (g/l)·sin(θ) - (1/l)·ẍ·cos(θ)

其中：

θ：车身倾角
g：重力加速度
l：重心到轮轴的距离
ẍ：轮子的加速度

当倾角较小时（sin(θ)≈θ，cos(θ)≈1），方程可以线性化为：

θ̈ = (g/l)·θ - (1/l)·ẍ

二、传感器系统

2.1 MPU6050六轴传感器

MPU6050是最常用的姿态传感器，集成了三轴陀螺仪和三轴加速度计。

陀螺仪：测量角速度（°/s）

优点：短时间内精度高，响应快
缺点：存在积分漂移

加速度计：测量加速度（m/s²）

优点：长期稳定，无积分误差
缺点：易受振动干扰，动态响应慢

2.2 互补滤波算法

由于单一传感器都有缺陷，我们需要融合两种传感器的数据。互补滤波是一种简单有效的数据融合方法：

// 互补滤波算法实现
typedef struct {float angle;        // 融合后的角度float gyro_bias;    // 陀螺仪零偏float dt;           // 采样时间float alpha;        // 互补滤波系数 (0.98 typical)
} ComplementaryFilter;float complementary_filter(ComplementaryFilter *cf, float accel_angle, float gyro_rate) {// 陀螺仪积分float gyro_angle = cf->angle + gyro_rate * cf->dt;// 互补滤波融合cf->angle = cf->alpha * gyro_angle + (1 - cf->alpha) * accel_angle;return cf->angle;
}// 从加速度计计算倾角
float calculate_accel_angle(float ax, float ay, float az) {return atan2(ax, sqrt(ay * ay + az * az)) * 180 / PI;
}

2.3 卡尔曼滤波（进阶）

卡尔曼滤波提供了更优的数据融合方案：

typedef struct {float Q_angle;      // 角度过程噪声协方差float Q_gyro;       // 陀螺仪偏差过程噪声协方差float R_measure;    // 测量噪声协方差float angle;        // 估计角度float bias;         // 陀螺仪偏差float P[2][2];      // 误差协方差矩阵
} KalmanFilter;float kalman_filter(KalmanFilter *kf, float accel_angle, float gyro_rate, float dt) {// 预测步骤float rate = gyro_rate - kf->bias;kf->angle += dt * rate;// 更新误差协方差kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);kf->P[0][1] -= dt * kf->P[1][1];kf->P[1][0] -= dt * kf->P[1][1];kf->P[1][1] += kf->Q_gyro * dt;// 计算卡尔曼增益float S = kf->P[0][0] + kf->R_measure;float K[2];K[0] = kf->P[0][0] / S;K[1] = kf->P[1][0] / S;// 更新估计float y = accel_angle - kf->angle;kf->angle += K[0] * y;kf->bias += K[1] * y;// 更新误差协方差float P00_temp = kf->P[0][0];float P01_temp = kf->P[0][1];kf->P[0][0