MCU-Vehicle-Project

Vehicle Project

Author: jrl777

基于STM32F103C8T6的两轮自平衡小车的学习过程记录，与诸君分享。内容参考平衡车开发指南。

1.原理分析

1.1 平衡原理

控制平衡小车的基本原理是控制中的负反馈机制。

由于小车只有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。

接下来需要解决的问题就转化成如何控制小车车轮的运动，以此控制小车的平衡了。需要建立两轮自平衡小车的运动学和动力学数学模型，设计反馈控制来保证车体的平衡。

1.2 动力学分析

保持车体平衡的控制规律可以简化为理想化的单摆物理模型。

对单摆模型进行受力分析。

我们都知道，在单摆偏离平衡位置时会受到重力、和阻尼力的作用。而回复力是重力在切向上的分力（单摆在切向上作间简谐运动），属于作用力。

当物体离开平衡位置后，会受到重力和绳子的合力的作用，使物体回到平衡位置。这个合力就称为：回复力。其大小为

​

在偏移角度很小的情况下，回复力与偏移的角度之间大小成正比，方向相反。 在此回复力作用下，单摆便进行周期运动。在空气中运动的单摆，由于受到空气的阻尼力， 单摆最终会停止在垂直平衡位置。空气的阻尼力与单摆运动速度成正比，方向相反。阻尼力越大，单摆越会尽快在垂直位置稳定下来。

在不同阻尼系数下，单摆的运动曲线也不同。

我们都知道，对于单摆而言，稳定在垂直位置（平衡位置）的条件有两个：

1. 受到与位移（角度）相反的恢复力；
2. 受到与运动速度（角速度）相反的阻尼力。

如果没有阻尼力，单摆会在垂直位置左右摆动。阻尼力会使得单摆最终停止在垂直位置。阻尼力过小（欠阻尼）会使得单摆在平衡位置附件来回震荡。阻尼力过大（过阻尼）会使得单摆到达平衡位置时间加长，比如气球单摆。因而存在一个临界阻尼系数，使得单摆稳定在平衡位置的时间最短。

倒立摆之所以不能象单摆一样可以稳定在垂直位置，就是因为在它偏离平衡位置的时候，所受到的回复力与位移方向相同，而不是相反！因此，倒立摆便会加速偏离垂直位置，直到倒下。 所以我们需要给予倒立摆额外的力来使其稳定在批平衡位置。

1.3 数学模型

下面对两轮自平衡小车进行简单数学建模，然后建立速度的比例微分负反馈控制，根据基本控制理论讨论小车通过闭环控制保持稳定的条件。

通过根轨迹示意图，可以很清晰的看出两轮自平衡小车的传递函数对应有两个零极点，有一个在 s 平面的右半平面。这说明两轮自平衡小车是不稳定的。

小车引入比例、微分（PD）反馈之后的系统如下图所示：

1.4 Simulink仿真

通过对两轮自平衡小车系统进行动力学分析和数学建模，在理论上设计出了控制方法。下面，调用 Matlab 软件的 Simulink 仿真工具包对两轮自平衡小车的控制系统数学模型进行仿真验证，观察角度在干扰信号的作用下的自恢复情况。

在 Simulink 文件中，建立两轮自平衡小车的数学模型，调用 PID 控制器构成控制系统的主要部分。设定输入信号值为 0，代表角度初始值为 0，小车初始在平衡位置。对输出节点，调用两个 Step 模块，通过设定 Step Time，使之构成宽度为 1s 的脉冲信号作为小车的外力干扰作用信号。整体系统构成负反馈形式，如下图所示。

1.5 小车的速度控制原理与串级PID

PID原理：PID控制算法原理（抛弃公式，从本质上真正理解PID控制） - 知乎 (zhihu.com)

通常在三轮小车、四轮小车中，我们使用 PID 进行速度控制，大部分都是负反馈。但是两轮自平衡小车的速度控制不大一样，不是负反馈，而是正反馈，因为小车的两个电机需要进行直立控制的同时去进行速度控制。

我们先使用常规的速度负反馈算法试一下，看在平衡小车上面是否有效果。首先我们给定一个目标速度值，由于在直立控制的作用下，此时小车要向前倾斜以获取加速度，车轮需要往后运动，这样小车的速度就会下降。因为是负反馈，速度下降之后，速度控制的偏差增大，小车往前倾斜的角度增大，如此反复，小车便会倒下。常规的速度负反馈在直立控制的影响下起到了正反馈效果。如下图所示。

根据以上的分析，在直立控制里面加入速度负反馈无法达到速度闭环的目的，而且还会破坏直立控制系统。下面我们换一种思路。

为保证直立控制的优先级，我们把速度控制放在直立控制的前面，也就是速度控制调节的结果仅仅是改变直立控制的目标值。因为根据经验可知，小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度，就需要增加小车向前倾斜的角度，倾斜角度加大之后，车轮在直立控制的作用下需要向前运动保持小车平衡，速度增大；如果要降低小车向前行驶的速度，就需要减小小车向前的倾斜角度，倾斜角度减小之后，车轮在直立控制的作用下向后运动保持小车平衡，速度减小。如下图所示。

根据上面的原理图，我们把速度和直立两个控制器串联起来工作，其中速度控制的输出作为直立控制的输入，而直立控制的输出作为系统的输出，这其实就是一个串级控制系统。直立控制在前面有介绍，使用的 PD 控制。因为编码器可能存在的噪声，为防止噪声被放大并消除系统的静差，这里我们速度控制使用 PI 控制。

至此，我们得到了让小车保持直立且速度为给定值的控制算法，由一个负反馈的直立 PD 控制器和一个正反馈的速度 PI 控制器组成。控制原理图进行的演变，如下图所示。

2. 电路开发

2.1 系统框架一览

由上面系统框架图可以看到，整个系统围绕 STM32F103C8T6 主芯片运行，电池经过降压稳压后提供稳定的 5v 和 3.3v 给 STM32 主芯片和各电子模块，STM32 主芯片通过读取 MPU-6050 传感器芯片的数据，获取小车系统的运动状态，再通过 TB6612FNG 驱动芯片控制直流电机的运动，直流电机通过编码器反馈转速给 STM32 主芯片，这就构成了两轮自平衡小车的基本硬件框架。其他的辅助功能，比如蓝牙、OLED、超声波、红外等等都是可加可不加的功能，我们为了提高两轮自平衡小车的可玩性和操纵性，一并加进去。

3.程序框架

打开自平衡小车的工程文件，展开的工程分组如下图所示：

如图所示，该工程共有6个具体的分组。

1. FWlib分组：主要是HAL库的文件代码
2. CMSIS分组：主要是Cortex-M3 核心的文件代码
3. Startup 分组：stm32f10x 的启动文件代码
4. BSP 分组：外设硬件的驱动程序代码，例如MPU6050、oled 等等
5. SYS 分组：主要是滴答定时器和调试相关的配置代码
6. USER 分组：用户代码，主程序

讲解一下 USER 分组的 main.c。main.c 包含了所有硬件初始化和参数初始化代码。初始化代码如下：

int main(void)
{        
    BspInit();       //初始化BSP
    PIDInit();       //初始化PID    
    CarUpstandInit();//初始化系统参数
    SysTick_Init();  //初始化定时器        
    if(IsInfrareOK())//检测是否悬挂红外模块
        g_iGravity_Offset = 1; //如果检测到悬挂红外模块，则更改偏移值。
    ShowHomePageInit();//初始化OLED显示屏主页
    while (1)//进入主循环
    {    
        SecTask();//秒级任务，主要是记录小车运行时间、读取电池电压等非实时任务

        if(SoftTimer[1] == 0)//系统软件定时器1，分辨率为1ms，递减计数
        {// 每隔20ms 执行一次
            SoftTimer[1] = 20;
            ResponseIMU();//上报姿态数据到APP        
            DebugService();    //上位机调试数据发送        
            Parse(Uart3Buffer);    //APP数据解析函数    
        }            

        if(SoftTimer[2] == 0)//系统软件定时器2，分辨率为1ms，递减计数
        {// 每隔20ms 执行一次
            SoftTimer[2] = 20;
            ShowHomePage();//刷新OLED页面
            Read_Distane();//读取超声波测距距离
            if(g_CarRunningMode == ULTRA_FOLLOW_MODE){
                if(IsUltraOK())UltraControl(0);    //设置超声波跟随模式
             }
            if(g_CarRunningMode == ULTRA_AVOID_MODE){
                if(IsUltraOK())UltraControl(1);    //设置超声波避障模式
             }
            else if(g_CarRunningMode == INFRARED_TRACE_MODE){
                TailingControl();//设置红外循迹模式
            }
        }            
    }
}

代码初始化主要是硬件底驱动和系统参数的初始化。

3.1 BspInit()函数

BspInit 主要是硬件底层驱动的初始化，包括 STM32 外设的初始化、外围模块比如 MPU-6050 的初始化。具体见下面代码：

void BspInit(void)
{
    SWDConfig();            //SWD 调试接口配置，使能 SWD，失能 JTAG
    ADCInit();                //ADC 初始化
    USART1Init();            //串口 1 初始化-底板预留下载及调试用
    USART3Init(0);            //串口 3 初始化-用于蓝牙
    TIM1_Cap_Init();        //TIM1初始化-用于超声波跟随功能
    TIM3_PWM_Init();         //PWM初始化
    TIM2_Encoder_Init();    //TIM2 正交解码初始化-用于测速
    TIM4_Encoder_Init();    //TIM4 正交解码初始化-用于测速    
    i2cInit();                 //I2C 初始化    
    InfraredIOInit();        //红外 IO 口初始化
    OLED_Init();            //OLED 初始化
    MPU6050_Init();            //MPU6050 初始化    
    LEDInit();                //指示灯初始化    
    UltraSelfCheck();        //超声模块开机自检
    InfrareSelfCheck();        //红外模块开机自检    
    delay_ms(500);            //延时 0.5s，等待蓝牙模块启动
    Uart3SendStr("\r\nAT+BAUD8\r\n"); //配置蓝牙串口波特率为 115200 ( 原波特率9600 ) 
    USART3Init(1);            //更改 UART3 波特率为 115200
    delay_ms(20);           //延时 20ms,等待波特率稳定
    SetBlueToothName();        //配置蓝牙模块名称
}

3.2 PIDInit 函数

PIDInit 函数主要是对小车系统的PID算法的参数进行初始化。

PID 是 Proportional（比例）、Integral（积分）、Differential（微分）的首字母缩写；是一种结合比例、积分和微分三种环节于一体的闭环控制算法。PID 控制的实质是对目标值和实际值误差进行比例、积分、微分运算后的结果用来作用在输出上。

自平衡小车的PID算法思路上文已经讲过，整理就不再赘述了。

void PIDInit()
{
	char flag[2];
	
	ReadFlash(original_PID_Addr, flag, 2);
	if((flag[0] == 0xa5)&&(flag[1] == 0x5a))
	{// 非初次运行
		PIDRead();
	}
	else{// 初次运行
		flag[0] = 0xa5;
		flag[1] = 0x5a;
		ProgramFlash(original_PID_Addr, flag, 2);
		ProgramFlash(original_PID_Addr+2, (char*)&g_tCarAnglePID, sizeof(PID_t));// ±£´æ½Ç¶È»·Ä¬ÈÏpid²ÎÊý
		ProgramFlash(original_PID_Addr+2+16, (char*)&g_tCarSpeedPID, sizeof(PID_t));// ±£´æËٶȻ·Ä¬ÈÏPID²ÎÊý
	}
}

3.3 CarUpstandInit函数

CarUpstandInit 函数的主要功能是对小车系统的各种参数进行初始化。

即全局变量初始化函数。

void CarUpstandInit(void)
{
	//g_iAccelInputVoltage_X_Axis = g_iGyroInputVoltage_Y_Axis = 0;
	g_s16LeftMotorPulse = g_s16RightMotorPulse = 0;
	g_s32LeftMotorPulseOld = g_s32RightMotorPulseOld = 0;
	g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0;

	g_fCarSpeed = g_fCarSpeedOld = 0;
	g_fCarPosition = 0;
	g_fCarAngle    = 0;
	g_fGyroAngleSpeed = 0;
	g_fGravityAngle   = 0;

	g_fAngleControlOut = g_fSpeedControlOut = g_fBluetoothDirectionOut = 0;
	g_fLeftMotorOut    = g_fRightMotorOut   = 0;
	g_fBluetoothSpeed  = g_fBluetoothDirection = 0;
	g_fBluetoothDirectionNew = g_fBluetoothDirectionOld = 0;

  g_u8MainEventCount=0;
	g_u8SpeedControlCount=0;
 	g_u8SpeedControlPeriod=0;
}

3.4 SysTick_Init函数

SysTick_Init 是系统滴答定时器 SysTick 的初始化，在这里设置为 1ms 中断一次。

void SysTick_Init(void)
{
	/* SystemFrequency / 100     10ms中断一次
	   SystemFrequency / 1000    1ms中断一次
	 * SystemFrequency / 100000	 10us中断一次
	 * SystemFrequency / 1000000 1us中断一次
	 */
	SystemCoreClockUpdate();

	if (SysTick_Config(SystemCoreClock / 1000))	// ST3.5.0库版本
	{ 
		/* Capture error */ 
		while (1);
	}
	// 开启滴答定时器 
	SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk;
}

3.5 其他函数

if(IsInfrareOK()) g_iGravity_Offset = 1; 检测是否悬挂红外循迹模块，如果有则需要修改重心偏移值。因为在一侧悬挂红外后，小车重心会偏移。

ShowHomePageInit 主要是在 OLED 显示 logo。

while(1) 主循环中主要是执行一些非实时任务（早些迟些执行都无所谓的任务），人为定义一个秒级任务，轮询执行这些任务。这些非实时任务有上报数据、调试数据、解析协议、刷新 OLED 数据、读取距离等。

执行完初始化，代码会由于滴答定时器 SysTick 进入中断而转跳到 SysTick 定时中断服务函数中执行。在 stm32f10x_it.c 中可以找到 SysTick_Handler 定时中断服务函数。代码具体内容如下：

void SysTick_Handler(void)
{  
    SoftTimerCountDown();             //软定时器
    g_u8MainEventCount++;            //主事件计数变量
    g_u8SpeedControlPeriod++;        //速度环控制周期计数变量
    SpeedControlOutput();            //速度环控制输出函数，每1ms执行一次
    if(g_u8MainEventCount>=5)        //5ms进入一次
    {
        g_u8MainEventCount=0;
        GetMotorPulse();             //捕获电机脉冲（速度）函数，每5ms执行一次
    }
    else if(g_u8MainEventCount==1)
    {
        MPU6050_Pose();                 //读取MPU6050数据函数，每5ms执行一次
        AngleCalculate();             //角度环计算函数，每5ms执行一次
    }
    else if(g_u8MainEventCount==2)
    {
        AngleControl();                 //角度环控制函数，每5ms执行一次
    }
    else if(g_u8MainEventCount==3)
    {
        g_u8SpeedControlCount++;
        if(g_u8SpeedControlCount >= 5)//25ms
        {        
              SpeedControl();          //车模速度控制函数，每25ms调用一次
              g_u8SpeedControlCount=0;
                g_u8SpeedControlPeriod=0;
        }
    }
    else if(g_u8MainEventCount==4)
    {
        MotorManage();            //电机使能/失能控制函数，每5ms执行一次
        MotorOutput();             //电机输出函数，每5ms执行一次
    }
}

4. 软件开发

我负责的是两轮自平衡小车的软件部分（下位机）的编写与验证。

主要编程软件：

MDK-ARM；STM32CubeMx；stlink烧入

4.1 STM32CubeMx生成工程

首先，先选择对应的主控芯片的型号：stm32f103c8tc

接着，配置好工程的基本设置

4.2Usart与Printf函数重定向

MiaowLabs-STM32F1-Tiny 核心板板载 CH340 USB to TTL 芯片，该芯片连接 STM32F103C8T6 芯片的 Usart1 引脚 PA9/PA10。