Balancing_Cart

平衡小车课程设计

简介

使用stm32单片机设计并制作两轮平衡小车实现直立任务

关键词

stm32f103c8t6,pid,滤波,电机驱动控制,传感器数据解析

硬件设计

graph LR
A(主控stm32f103c8t6)
B(电机驱动)
C(mpu6050)
D(4s电池)
E(降压模块)
F(左电机)
G(右电机)
A-->B
A-->C
A-->E
B-->F
B-->G
E-->D

程序运行

    graph TB
    A(开始)-->B[初始化定时器,pid,i2c,mpu6050,卡尔曼滤波器等]-->D

    D[进入while循环]-->E[读取当前的mpu6050的原始值]-->F[通过卡尔曼滤波器将角度与角加速度解算出稳定的绝对角度]
    F-->G[将当前角度与电机转速加入串级pid运算中,实现直立环与速度环双层闭环]-->H[设置pid输出pwm]-->D
    a[TIM4设置为更新中断.每隔20ms更新一次事件,读取编码器值]-->b[根据编码器的当前值计算出电机速度]-->G

功能函数介绍

1.卡尔曼滤波

卡尔曼滤波是一种最优估计算法,通过多传感器数据融合来实现对真实数据的最优估计,他会动态调整数据来源置信度,从而融合多种传感器的数据达到最佳. 比如对于mpu6050,有两个数据来源分别是绝对角度,也就是三轴陀螺仪和三轴加速度计. 这其中: 陀螺仪可以准确地反映当前角速度,可以通过积分计算出角度,但是容易出现累计误差,导致偏移,也就是说他是瞬态响应准确. 加速度计可以通过力的分解得出当前的稳态角度,响应慢,容易受到振动干扰,但是数据准确. 通过对两个数据来源进行数据融合就可以得到最优的角度数据,滤波的效果如下: 其中绿色是由加速度计直接得出的角度值,红色是经过卡尔曼之后的角度值,由此可得他对抗振动的性能提高了且角度也很准确. 关于卡尔曼的原理非常复杂,有感兴趣的朋友可以搜索b站up主 DR_CAN的视频讲解: 下面是我工程的实现函数:

#include "kalman.h"

void Kalman_Init(Kalman_t *k)
{
    k->angle = 0;
    k->bias = 0;

    k->P[0][0] = 0;
    k->P[0][1] = 0;
    k->P[1][0] = 0;
    k->P[1][1] = 0;

    // 经验值，可以根据实际调整
    k->Q_angle = 0.002f;   // 越小越信任加速度的角度
    k->Q_bias  = 0.003f;   // 越小越信任陀螺仪的偏置稳定
    k->R_measure = 0.03f;  // 加速度计测量噪声
}

float Kalman_Update(Kalman_t *k, float accAngle, float gyroRate, float dt)
{
    /* ============= 1. 预测步骤 ============= */

    float rate = gyroRate - k->bias; // 角速度扣除偏置
    k->angle += rate * dt;           // 预测新的角度

    // 更新协方差矩阵 P
    k->P[0][0] += dt * (dt*k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
    k->P[0][1] -= dt * k->P[1][1];
    k->P[1][0] -= dt * k->P[1][1];
    k->P[1][1] += k->Q_bias * dt;

    /* ============= 2. 更新步骤 ============= */

    float S = k->P[0][0] + k->R_measure; // 创新协方差
    float K0 = k->P[0][0] / S;           // 卡尔曼增益 K 的第一项
    float K1 = k->P[1][0] / S;           // 卡尔曼增益 K 的第二项

    float y = accAngle - k->angle;  // 创新（测量 - 预测）

    k->angle += K0 * y;             // 更新最终角度
    k->bias  += K1 * y;             // 更新偏置

    // 更新协方差矩阵
    float P00_temp = k->P[0][0];
    float P01_temp = k->P[0][1];

    k->P[0][0] -= K0 * P00_temp;
    k->P[0][1] -= K0 * P01_temp;
    k->P[1][0] -= K1 * P00_temp;
    k->P[1][1] -= K1 * P01_temp;

    return k->angle;
}

2.电机速度获取

我使用的电机是n20电机,带AB相增量式磁性编码器,通过定时器的编码器模式来实现计数.

实现方法

我通过使用定时器的更新中断来实现每20ms触发一次编码器ccr数据读取,通过换算得到电机的当前速度.

void Read_Encoder(TIM_HandleTypeDef *htim)//TIM编码器测速函数
{

  int16_t count=__HAL_TIM_GET_COUNTER(htim);//直接读取寄存器的值
  diff=count-last_count;//前后两次差值
  if(diff>32767){//逆时针转动，超越最低限额，比如10->65530，相减等于65520，那就减去65535，得到-15，表示逆时针转动了15
    diff-=65535;
  }
  else if(diff<-32767){//同理可得，这是顺时针
    diff+=65535;
  }
  last_count=count;//更新上一次编码器值
  speed=diff*60/14;
  speed_IIR=DiffFilter_IIR(speed);
  //....;pp
  //return diff;//返回差值

}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {//定时器中断回调函数,每20ms触发一次
  if (htim==&htim4) {//判断是从tim4的中断来的
    HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
    Read_Encoder(&htim2);
  }
}

3.串级pid

现在我们得到了小车的实时角度与电机速度值,我们可以实现直立环与速度环了

pid原理

pid拆分出来就是比例,积分,微分: 比例:其实就是指当前数据与目标数据的差值,反映的是路程. 积分:就是高数里指的积分,表示数据的累积 微分:简单理解就是加速度,表示数据变化的斜率

u(t)=K_P e(t)+K_i\int_0^te(\tau)d\tau+K_d\frac{de(t)}{dt} 

而串级pid的实现就是两个环的反馈数据

graph LR
A(目标角度)
B(当前角度)
C(外环-直立环)
D(内环-速度环)
E(当前速度)
F(输出pwm)
G(作用对象)
A-->C
C-->D
D-->F
B-->C
E-->D
F-->G

实现代码如下:

float balance_control_simple(float angle, float gyro, float speed) {
    // 1. 安全检查：倾斜过大停止
    if (fabsf(angle) > MAX_ANGLE) {
        speed_integral = 0;
        return 0.0f;
    }

    // 2. 直立环计算
    // 角度误差 = 目标角度 - 当前角度
    float angle_error = TARGET_ANGLE - angle;

    // 直立环输出 = Kp*角度误差 + Kd*(-角速度)
    float upright_output = ANGLE_KP * angle_error + ANGLE_KD * (-gyro);

    // 3. 速度环计算
    // 速度误差 = 直立环输出（视为目标速度） - 当前速度
    float speed_error = upright_output - speed;

    // 积分项（防饱和）
    speed_integral += speed_error;
    if (speed_integral > 100.0f) speed_integral = 100.0f;
    if (speed_integral < -100.0f) speed_integral = -100.0f;

    // 速度环输出
    float speed_output = SPEED_KP * speed_error + SPEED_KI * speed_integral;

    // 4. 总输出 = 速度环输出
    float pwm_output = speed_output;

    // 5. 限幅
    if (pwm_output > MAX_PWM) pwm_output = MAX_PWM;
    if (pwm_output < -MAX_PWM) pwm_output = -MAX_PWM;

    // 6. 死区处理
    if (fabsf(pwm_output) < DEAD_ZONE) {
        pwm_output = 0.0f;
        speed_integral *= 0.9f;  // 积分衰减
    }

    return pwm_output;
}

实现效果

单片机的运用

本项目涉及许多单片机外设,包括定时器,串口,I2C,GPIO等等

定时器

• TIM1输出pwm
• TIM2/TIM3编码器模型读取电机编码器输出
• TIM4更新中断实现20ms读取一次速度值

I2C

• 用于与mpu6050通讯

GPIO

• 控制led灯亮灭指示状态
• 读取电机驱动的数据获取转动方向
• 读取按键值

串口

• 输出整体数据用于调试

总结与反思

从结果来看,小车的平衡任务失败了,,代码方向已经尽力优化,输出的数据也趋近稳定与完美,可是无论如何调整pid数据实际的控制效果还是不好,主要的原因是电机选型失误,力矩与响应速度不足以满足精确控制,且电机轴与轮子存在很大虚位.后期可以通过更换输出力矩与响应速度更快的电机改善,然后增加转向环与遥控器功能来实现真正的平衡小车功能.

小组成员

组别:13 组长:洪俊钦 组员:迪西克,聚迪批