基于STM32F103与FreeRTOS的自平衡小车实现

首先移植FreeRTOS到STM32F103上，接着就是实现MPU6050的初始化，这里移植了正点原子的参考例程，基本实现是IIC初始化的，读写IIC，接着就可以配置MPU6050，就可以从MPU6050的指定寄存器读取到原始数据，这个原始数据还是不能用的，很好不会了，MPU6050有自带DMP（数字运动处理芯片）,不错，再驱动一下DMP就能得到，经过DMP处理后就得到了姿态四元数，

nice，经过一个算法的转化就可以得到俯仰角，横滚角，航向角。这里我们用到了就两个，横滚角直立环与航向角角度环。

这下就差一个速度环的速度了，我们使用的是带霍尔编码器的减速电机，因此只需要配置一下定时器作为捕获模式就行了，我们可以再拿一个定时器来计时，这样当计时到达进入定时器中断我们可以查看一下脉冲数，然后我们拿着电机自转一周就能得到转一圈有多少个脉冲数了，就可以得到每一个脉冲所占的比例，就可以得到r/s这样的速度，我们又可以测量半径计算周长2Πr = （r）转得到实际速度。这样我们就得到了速度。

接着就是PID算法了，采用了直立环+速度环+角度环的形式实现

PID也就是比例调节+积分调节+微分调节

举一个经典例子：假设想操控无人机向上飞到离地面20m处的位置，我们启动无人机，无人机慢慢上飞，从正准备脱离地面开始，我们无人机实际位置与期待位置相距100m，此时我们给无人机一个速度v让它上飞t秒，我们预期是v*t刚好能飞到100m就保持悬停，这时我们其实就是做了PID的P（比例）调节，比例系数为1，但我们的比例系数为1不就行了吗？为什么不行呢？当无人机很接近目标距离，也就是当前误差很小时，我们给无人机一个动力来试图到达目标高度，这是不可行的，因为实际无人机运行一定会受到阻力，这时这个动力只能与上升受到的阻力相互抵消而实际并没有上升，这也就是说现实情况并不会那么理想，这时我们P调节有啥意义呢，当我们减小P比例时，我们会花更长的时间到达那个很小误差的临界值，而当我们的P增大时，我们只需很少的时间就能到达那个临界误差值，当我们的P过大时就会先在振荡目标距离振荡很多次后在到达那个临界误差。对于高精度的设备当然不允许有这种误差存在，于是I算法就是来解决这一问题，I算法会进行误差值的累加，但显而前面几次误差过大，我们得先忽略，不然I算法也无效，I算法是积分算法，累加之前的误差，加上一个合适的系数，这就会补偿前面那个临界误差来带的影响，这样我们的PID算法就差D算法了，D算法是为了解决当我们的无人机好好的飞着突然受到了一个强风吹那咋办，岂不会立马系统就变得晃动很大，因为P这时对于的差值很大，为了解决这一点，D算法微分就刚好能解决这一点，D算法积分算法是通过计算此次偏差与上一次偏差两者的差值，这样的话如果上次偏差很小而这次偏差很大，就会变为-此次偏差，这样与p导致的大幅度改变就能相抵

好了，PID懂了，接着我们就差三个环了

速度环：输入目标速度，通过PID算法得到速度环输出

直立环：输入横滚角+速度环输出，通过PID算法得到PWM

转向环：输入航向角，通过PID算法得到转向环输出

左右两电机PWM分别为直立环算出的PWM减去和加上转向环输出

完成。

参考代码

#include "control.h"

float Med_Angle=0;      // 机械中值，能使得小车真正平衡住的角度 
float Target_Speed=0;	  // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float 
  Vertical_Kp=0,
  Vertical_Kd=0;     // 直立环Kp、Kd
float 
  Velocity_Kp=0,     // 速度环Kp、Ki（正反馈）
  Velocity_Ki=0;
float 
  Turn_Kp=0;

int Vertical_out,Velocity_out,Turn_out; // 直立环&速度环&转向环的输出变量

int Vertical(float Med,float Angle,float gyro_Y); // 函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z);

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
    if(PBin(5)==0)    // 二级判断
    { 
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
      // 1.采集编码器数据&MPU6050角度信息
      // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
      // 2.将数据压入闭环控制中，计算出控制输出量
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
			Turn_out=Turn(gyroz);	
      
      PWM_out=Vertical_out;//最终输出
      
      // 3.把控制输出量加载到电机上，完成最终控制
      MOTO1 = PWM_out-Turn_out; // 左电机
      MOTO2 = PWM_out+Turn_out; // 右电机
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加载到电机上
    }
  }
}

/*****************  
直立环PD控制器：Kp*Ek+Kd*Ek_D

入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
出口：直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y) 
{
  int PWM_out;
  
  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  
  return PWM_out;
} 

/*****************  
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
  // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.计算速度偏差
  //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  // 2.对速度偏差进行低通滤波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  // 3.对速度偏差积分出位移
  Encoder_S+=EnC_Err_Lowout;
  // 4.积分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度环控制输出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  
  return PWM_out;
}

/*****************  
转向环：系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
  int PWM_out;
  
  PWM_out = Turn_Kp*gyro_Z;
  
  return PWM_out;
}

再加上一个蓝牙模块就可以通过蓝牙app进行控制了

这里采用的PID算法是位置式的，其实增量式呢就是就行保留上一次位置式进行相减而已。

这里可以看到使用到了速度环和直立环进行串联，速度环作为外环，直立环作为内环的形式进行串级PID，而角度环过于简单就没必要再搞这种串级关系