两轮自平衡小车控制系统设计

Design of Control System for Two-Wheeled Self-Balancing Car

摘要: 两轮自平衡小车因其占用面积小、结构简单、灵活性高等特点，已经被应用于生活代步、安保等相关领域。本文从自平衡小车平衡性的角度出发，采用STM32F103C8T6控制器，设计出PID控制器来满足对小车自身平衡的基本控制，设计了卡尔曼滤波器来优化姿态传感器MPU6050的输出数据，以确保小车获取的姿态信息的准确性，设计了超声波测距模块实现小车的避障功能。然后完成了系统设计方案，进行了器件选型，并完成硬件设计。最后进行了软件设计，编写并测试了各模块程序，成功搭建出完整平衡小车的控制模型。整机测试结果表明该系统设计能很好地完成平衡小车的自平衡和避障功能。

Abstract: The two-wheeled self-balancing car has been used in life mobility, security and other related fields due to its small footprint, simple structure and high flexibility. From the point of view of self-balancing car balance in this paper, the STM32F103C8T6 is adopted, a PID controller is designed to meet the basic control of the car’s own balance, a Kalman filter is designed to optimize the output data of the attitude sensor MPU6050 to ensure the accuracy of the attitude information obtained by the car, and the ultrasonic distance measuring module is designed to realize the obsta-cle avoidance function of the car. Then the system design scheme is designed, the device selection is performed, and the hardware system is designed. Finally, the software system is designed, each module program was written and tested, and the control model of the complete balance car was successfully built. The test results show that the system design can complete the self-balancing and obstacle avoidance function of the balancing car.

1. 引言

两轮自平衡小车是轮式移动机器人的一种，本质上属于不稳定自主移动机器人。两轮自平衡小车系统具有不稳定、多耦合、时变和参数不确定性等特点，是一个集智能环境感知、路径规划、自主避障、智能决策和自主控制等功能于一体的复杂非线性系统，其体积小、质量轻，能够实现各种功能，给人们生活带来便利 [1] - [6] 。国内外专家对此进行深入研究，日本筑波大学的Ha和Yuta联合设计了一种两轮独立驱动，利用陀螺仪检测车身姿态，利用编码器检测车轮转速的自平衡机器人“Yamabico Kurara”，是第一个具有完整控制体系的机器人 [7] 。丹麦Lego公司Steve设计两轮自平衡机器人Legway，控制灵活，模块化设计，不仅能保持自平衡和直线运动，还可以实现爬坡、拐弯和旋转等功能，与其它平衡车相比其能带给驾驶者更好的舒适感 [8] 。2015年，小米公司推出了“九号平衡车”，这是一款载人自平衡车，它的出现使得人们的出行效率得到了极大提高 [9] [10] [11] 。本文采用STM32F103C8T6控制器作为系统主控芯片，设计了一款两轮自平衡小车，该车采用MPU6050姿态传感器获得平衡车的倾角和角速度，采用增量式霍尔编码器获得平衡车的车轮速度，采用卡尔曼滤波算法优化姿态传感器的数据和采用PID算法实现平衡车自身平衡的控制，同时采用超声波测距模块实现避障功能。

2. 控制原理和系统建模

2.1. 控制原理

图1为小车平衡状态图，在三组图中左图为小车的最佳平衡状态，但是小车在无外力干扰的情况下并不会一直保持在这种状态。因此当小车处于中间图的状态时小车向右倾斜，此时小车需要向右加速维持平衡，同时当小车处于右图状态时小车向左倾斜，此时小车需向左加速维持平衡。正是因为这种机制才能控制小车的实时平衡。

2.2. 小车车轮模型分析

自平衡小车的驱动力是由直流电机输出的力矩来带动车轮提供的，在这个控制系统中其输入量便是电机的输出转矩。见图2，在理想情况下两车轮分别建立的力学模型如下。

Figure 1. Balance state diagram of car

图1. 小车平衡状态图

Figure 2. Car left and right wheel force analysis diagram

图2. 小车左右车轮受力分析图

图2中左轮，可以依据牛顿第二定律 $F=Ma$ 和转动惯量定律 $M=J\beta$ 建立左轮的平衡方程式：

$\{\begin{array}{l}m{\stackrel{\dot{}}{v}}_L=f_L-F_L\\ J_1{\stackrel{\dot{}}{\omega }}_L=T_L-f_{L}R\end{array}$ (1)

联立式1方程解得式2：

$m{\stackrel{\dot{}}{v}}_L=\frac{T_L}{R}-\frac{J_1{\stackrel{\dot{}}{\omega }}_L}{R}-F_L$ (2)

同理可建立小车右轮的平衡方程式：

$\{\begin{array}{l}m{\stackrel{\dot{}}{v}}_R=f_R-F_R\\ J_1{\stackrel{\dot{}}{\omega }}_R=T_R-f_{R}R\end{array}$ (3)

联立式(3)解得式(4)：

$m{\stackrel{\dot{}}{v}}_R=\frac{T_R}{R}-\frac{J_1{\stackrel{\dot{}}{\omega }}_R}{R}-F_R$ (4)

式中， $f_L$ 和 $f_R$ ：地面与左右车轮分别产生的摩擦力； $F_L$ 和 $F_R$ ：车体分别对两车轮的压力； $T_L$ 和 $T_R$ ：两车轮驱动电机分别产生的电磁转矩； ${\omega }_L$ 和 ${\omega }_R$ ：两车轮各自的转动角速度； $v_L$ 和 $v_R$ ：两车轮各自圆心的移动速度； $J_1$ ：车轮转动惯量；R：车轮半径； $F_g$ ：车轮受到竖直方向的作用力；m：单个车轮质量。

2.3. 小车车体模型分析

两轮自平衡小车直线运行的过程中，能被看作为车体绕车轴转动模型与其等效质心水平移动模型的结合体，见图3。

Figure 3. Car body stress analysis diagram

图3. 小车车体受力分析图

根据图3可以得到车体平衡方程：

$\{\begin{array}{l}M{\ddot{y}}_b=F_y-Mg\\ J_2\ddot{\Phi }=F_{y}L\sin \Phi -F_{x}L\cos \Phi -T\\ F_x=M{\ddot{x}}_b\end{array}$ (5)

假设当小车运动至某时刻时，车体的倾斜角度为 $\Phi$ ，车体质心坐标处于 $\left(x_b,y_b\right)$ ，因此可建立式(6)的平衡方程：

$\{\begin{array}{l}{x}_b=x_1+L\sin \Phi \\ y_b=L-L\cos \Phi \end{array}$ (6)

经式(6)的两阶求导得到式(7)：

$\{\begin{array}{l}{\ddot{x}}_b={\ddot{x}}_1+L\left(\ddot{\Phi }\cos \Phi -{\stackrel{\dot{}}{\Phi }}^2\sin \Phi \right)\\ {\ddot{y}}_b=L\ddot{\Phi }\sin \Phi +{\stackrel{\dot{}}{\Phi }}^2\cos \Phi \end{array}$ (7)

其中 $x_1$ 为车轮水平方向向移动的距离，可由式(8)表示：

$x_1=\left(x_L+x_R\right)/2$ (8)

联立式(5)和式(7)可解得系统转矩方程为式(9)：

$\begin{array}{c}{J}_2\ddot{\Phi }=M\left({\ddot{y}}_b-g\right)L\sin \Phi -M{\ddot{x}}_{b}L\cos \Phi -T\\ =M\left(L\ddot{\Phi }\sin \Phi +{\stackrel{\dot{}}{\Phi }}^2\cos \Phi -g\right)L\sin \Phi -\left(M{\ddot{x}}_1+ML\left(\ddot{\Phi }\cos \Phi -{\stackrel{\dot{}}{\Phi }}^2\sin \Phi \right)\right)L\cos \Phi -T\\ =M\left(L{\stackrel{\dot{}}{\Phi }}^2\sin \Phi \cos \Phi +L^2{\stackrel{\dot{}}{\Phi }}^2\sin \Phi \cos \Phi -L^2\ddot{\Phi }\cos 2\Phi -{\ddot{x}}_{1}L\cos \Phi -g\right)-T\end{array}$ (9)

式中， $F_x$ ：车轮作用在车身水平方向上的推力；M：车体的质量； $F_y$ ：车轮提供给车身向上的支撑力； $J_2$ ：车身转动惯量； $\Phi$ ：车身转过的角度；T：车轮作用给车身的扭矩； $x_b$ ：质心水平位移； $x_1$ ：车轮水平位移。

3. 系统主要算法

由于平衡小车自身结构限制，使得其本身便是一个高度不稳定的控制系统，因此为了保证小车能达到较好的平衡性能，需要在控制方法方面和数据滤波方面设计相关算法。

3.1. PID控制设计

在设计PID控制时，采取双闭环控制设计，其中角度环作为系统的外控制环，速度环作为系统的内控制环，在角度环与速度环共同作用下，使得驱动车轮的直流电机端电压得到持续的控制，从而达到小车维持平衡的目的，两轮自平衡小车控制框图如图4所示。

Figure 4. Balance car control block diagram

图4. 平衡小车控制框图

在角度环中我们将期望值设置为0度。将实时检测到的小车倾斜角度充当系统的角度反馈量，因此0度状态与倾斜角度共同构成了角度环中输入量。而角度环输出量则送给了速度环，它与反馈给小车的实际速度构成了速度环的输入量，其输出直接作用于小车。

3.2. 卡尔曼滤波设计

由于平衡小车的姿态信息是通过集成在MPU6050传感器中的陀螺仪和加速度计来获取的。然而陀螺仪的工作方式属于积分过程，而且其工作过程中会产生温漂效应，会使系统的累积误差不断加大，导致系统崩溃。同时加速度计是以微分的方式来获取加速度信息的，因为其动态性能较差，同样会包含大量的噪声对系统造成干扰。因此在设计卡尔曼滤波器过程中，首先假设预测出当前时刻k时的角度为Angle(k)，角速度为Gyro(k)，温漂噪声为 $Q_{Gyro}$ ，倾角噪声为 $Q_{Angle}$ 。便能得到：

$Angle\left(k\right)=Angle\left(k-1\right)+Gyro\left(k-1\right)\ast \Delta t-Q_{gyro}\ast \Delta t$ (10)

$Gyro\left(k\right)=Gyro\left(k-1\right)-Q_{gyro}$ (11)

其中状态方程式1和式2能够利用上一时刻的角度值和加速度，来预测出当时刻的角度值和加速度，然后通过传感器实际检测到的当前时刻角度值和加速度，来修正预测出的角度值和加速度，便可以得到当前时刻的角度的最佳估计值

4. 系统硬件设计

图5为系统硬件的总体框图，在辅助电源供电的情况下，控制器经电机驱动控制左右电机带动小车，然后通过测速模块和姿态检测模块将采集到的信息传递给控制器，构成闭环反馈。其次外加一些功能模块如：蓝牙模块、超声波模块、OLED显示模块，共同完成小车的基本功能。

Figure 5. System overall design block diagram

图5. 系统总体设计框图

4.1. 小车车体结构设计

一个优良的机械结构能够提高系统的稳定性，同时可以对小车的一些核心部件起到保护作用。因此本文采用多层结构设计，如图6所示，不仅协调了各部件的放置位置，而且小车重心低于其几何重心，保证了平衡小车能够平稳运行，提升了小车的抗干扰能力。

Figure 6. Car structure drawing

图6. 小车结构图

4.2. Stm32最小系统

控制器采用了由意法半导体公式生产的STM32F103C8T6微处理器。这款芯片采用了ARM公司的32位Cortex-M3内核处理器，其最高能达到72 M的工作频率，多达48个GPIO接口，flash大小为64 KB。它完全能够满足本文设计所需的任务处理功能要求。控制系统最小系统主要电路如图7所示，stm32最小系统主要包括时钟电路、复位电路、下载电路以及供电电路。

4.3. 辅助电源

图8为系统辅助电源电路，电池组输出的12 V电压，经由LM2596稳压芯片以及其外围电路构成的第一阶降压部分，实现了12 V到5 V的转换。LM2596芯片具有热关断和限流保护功能以及良好的线性负载能力。为满足3.3 V电压输出的要求，因此通过第一阶降压输出的5.0 V电压，再次经过AMS1117芯片构成的稳压部分进行了二降压，实现5.0 V到3.3 V的转化。AMS1117芯片又称低压漏失压调整器，其内部集成了过热以及限流保护器，广泛应用于电池供电的DC-DC电压转换。

Figure 7. Minimum system main circuit of STM32

图7. STM32最小系统主要电路

Figure 8. Auxiliary power supply circuit

图8. 辅助电源电路

4.4. 电机选型

自平衡小车的平衡控制说到底还是对电机的控制，电机的性能直接决定控制系统的稳定性，本文采取了直流减速电机作为平衡小车的驱动电机，其具有体积小，扭矩大，易控制等优点。

4.5. 电机驱动模块

由于控制器输出信号强度不足以直接驱动电机，因此为了实现控制器能够控制电机，必须借助外部电路对控制信号进行功率放大。图9为电机驱动电路，本文采用了A4950电机驱动芯片设计出了电机驱动模块。其中A4950是一款专为直流电机PWM控制而设计的，其工作电压最高可达40 V，峰值输出电流可达 ± 3.0 A。并且其内部集成了过流以及过热保护电路，能够延长期使用寿命。

Figure 9. Motor drive module

图9. 电机驱动模块

4.6. 传感器模块

传感器模块分别包括编码器模块和姿态传感器。图10为霍尔编码器和姿态传感器MPU6050实物图，它是由霍尔码盘、圆环磁体两部分构成。其中霍尔码盘是一个安装有霍尔元件的圆环硬板。而圆环磁体是一个磁性分布不均的圆环装磁石。在编码器安装时，需要将霍尔码盘固定在电机尾部，而圆环磁体则固定在电机转轴上。当电机转动时圆环磁体也随之转动，因此霍尔码盘上的霍尔元件会由于圆环磁体的转动引起磁场变化，使其能在磁场的作用下输出连续的信号脉冲。一般编码器能输出A、B两种相位相差90度的方波信号，控制芯片可以直接通过GPIO口读取信号信息，从而获取小车的速度信息。姿态传感器MPU6050实物图，它是一款内部集成了3轴陀螺仪与3轴加速度计的6轴姿态传感器，可以通过与控制器进行IIC通信，将检测到的姿态信息，传递给控制器。并且其自带DMP数据处理平台，能够减少对MCU处理空间的占用，减小数据处理难度。

Figure 10. Hall encoder and MPU6050 attitude sensor

图10. 霍尔编码器和MPU6050姿态传感器实物图

4.7. 其它功能模块

在功能模块设计上，本设计加入了OLED显示部分、蓝牙通信部分、超声波部分。其中OLED主要用于显示小车车身倾角和小车工作电压等参数，以便于小车功能调试和观测。蓝牙通信模块则能实现通过手机发送指令达到无线控制小车的功能，而超声波模块可以测量小车与前方障碍物的距离，从而达到提前躲避障碍物的功能。

5. 系统软件设计

5.1. 软件总体设计

图11为程序主体流程图，第一步需要初始化系统时钟、初始化各模块程序、开启系统5 ms的定时器中断。第二步等待中断是否到达，当中断没有来临时MCU仅提供OLED驱动，显示电池电压、小车倾角等参数。当中断来临时中断任务便开始执行。首先将读取的MPU6050输出数据，经卡尔曼滤波算法融合处理得到小车倾角。然后检测测速编码器输出的方波信号，换算之后得到小车速度。最后将小车的倾角和速度传入PID控制器，经PID控制器分析处理后输出相应PWM控制信号控制电机的驱动。在各程序相互的协调下，不断的循环调节使得小车最终处于一种平衡状态。

5.2. 软件算法程序实现

软件算法的实现包括PID控制算法和卡尔曼滤波算法。图12为PID控制算法和卡尔曼滤波算法的软件设计流程图。PID控制算法主要包括角度环PID控制和速度环PID控制两大部分构成。当只有角度环PID工作时，姿态传感器会不断检测小车的姿态信号，控制芯片实时读取姿态信息后，经滤波算法融合处理输出小车角度值，在角度环的作用下输出PWM信号控制电机使小车保持平衡，但是如果仅仅只有角度环工作，实际上并不能是使小车达到很好的平衡效果，一般会出现小车保持一段时间后朝向一方加速倒去。而当只有速度环PID工作时，测速编码器会不停的检测小车车轮转速，在速度环PID的作用下输出PWM信号保持对电机转速的控制，同样如果只有速度环工作时小车根本不能直立。只有在角度环PID工作的基础上加入速度环PID作用时并调整好PID参数才能使得小车保持较好的平衡效果。因为单独的角度环是为了让小车保持平衡而设计的，但是它并不会考虑小车的速度，从而当小车出现向前倾斜或向后倾斜一点，小车便会朝这个方向追，导致突破调节范围破坏系统平衡。所以当加入速度环后，由于速度环的作用，使得小车在平衡状态保持一个固定的速度值(一般这个值为0)，因此最后小车便会保持一种前后摆动的直立效果，调节PID参数来减小摆幅，实现小车完全直立。

Figure 11. System program main flow diagram

图11. 系统程序主流程图

Figure 12. Flow chart of PID control algorithm and Kalman filtering algorithm

图12. PID控制算法流程图和卡尔曼滤波算法流程图

卡尔曼滤波程序设计首先将姿态传感器检测到的小车角加速度和其车体倾角作为系统观测量。其次对当前的车身倾角和小车角加速度分别进行预测。然后利用倾角噪声和角加速度噪声预测出系统的方差矩阵，结合预测的方差值与实际测量值可以计算出当前的卡尔曼增益。依据上一时刻的角度最佳估计值、角速度最佳估计值以及卡尔曼增益更新预测值，可以得到出当前时刻的最佳角度和最佳加速度的估计值。最后实现系统误差协方差的更新。

5.3. 传感器驱动程序设计

霍尔编码器测速和MPU6050姿态传感器程序流程如图13所示。编码器测试程序是通过使用单片机定时器自带的编码器接口模式检测的。在初始化系统始时钟后将对应的GPIO引脚配置成编码器的接口模式，对编码器输出方波信号进行捕获，当捕获到一个上升沿(低电平转换为高电平的过程)时，定时器计数器加一，在一段时间内读取定时器计数寄存器中的值，通过相关换算即可得到车轮转速。

Figure 13. Flow chart of code velocity and attitude detection program

图13. 编码测速和姿态检测程序流程图

MPU6050姿态传感器程序首先对MPU6050进行初始化，将传感器当前所采集的数据更新至FIFO寄存器中，其次通过设置陀螺仪及加速度计计量范围调节其采集精度，最后通过STM32单片机与MPU6050进行IIC通信读取姿态信息。

6. 整车测试

小车测试如图14所示。当小车处于未工作状态，可以明显看到小车主要由控制板、电池、一组直流减速电机和两个车轮搭建而成。从小车的俯视图和正视图可以观察到，控制板搭载了OLED模块、蓝牙模块、超声波模块。因此小车具有显示电池电压以及自身倾角的功能，同时小车能通过蓝牙接收外界的命令执行相关操作，超声波模块的存在使得小车可以自主判断车前障碍物与车身的距离，从而实现躲避障碍物的功能。小车基本功能都已实现。

Figure 14. Balance car test drawing

图14. 平衡小车测试图

7. 结论

本文采用STM32F103C8T6为主控制器，利用姿态传感器和编码器采集小车的角速度、加速度、电机转速等信息，并利用卡尔曼滤波算法对数据进行优化处理，设计了PID控制器输出控制信号用于系统电机驱动，各模块相互作用，从而使系统处于平衡状态，同时系统采用超声波测距模块实现小车避障功能。测试结果验证了设计的正确性。

基金项目

2021年度湖南省大学生创新创业训练计划项目资助(4622)；2020年度湖南省大学生创新创业训练计划项目资助(3263)；2021年湖南文理学院大学生创新创业训练项目资助(XDC2021076)。

参考文献