基于STM32的双轮平衡车

张志强

（武汉理工大学 湖北 武汉 430070）

双轮平衡车突破了传统意义上的车的概念，它的特点是：两个轮子处于共轴的两个平行平面上，差动式运动，零半径转向，依照倒立摆的原理达到动态平衡。最早由美国发明家狄恩·卡门 （Dean Kamen）与他的DEKA研发公司（DEKA Research and Development Corp.）发明设计，名为 Segway。 但是其售价也是令人咋舌：7万人民币。如何能使其功能更加丰富而又成本低廉，是本文着重解决的问题。

在微处理器的选择上，基于ARM内核的32位微控器STM32，CPU最高可工作在72 MHz，占据了高性价比和低功耗的优势，尤其是丰富的外设，快速的中断，强劲的运算速度完全满足设计平衡车的需求。

1 平衡车的力学模型

为了获得平衡车的平衡方程[1-2]，需要分析其力学结构，平衡车的主要构成是车身和左右两个车轮，影响平衡的参数有：重心、质量、转动惯量、半径等。为保持动态平衡，车身重心应置于车轴的垂直上方的一定范围内，可建立力学模型，如图1所示。

假设平衡机器人为刚体，左右两轮完全对称，并且忽略车轮与地面之间的滑动与侧向滑动，以左轮和车身为研究对象得到如下方程：

图1 力学模型Fig.1 Mechanical model

左轮方程为：

其中，xRM：水平位移 （m）；MRL：左车轮质量 （kg）；HL：车身施加于车轮的水平作用力 （N）；HTL：地面对车轮的水平作用力（N）；θRL：左轮相对于垂直分量的倾角（rad）；JRL：左轮相对于 Z轴转动惯量（kgm2）；CL：左轮电机产生的扭矩（kg·m）；R：车轮半径（m）。

车身的方程为：

其中，xp：车身水平位移分量 （m）；yp：车身垂直位移分量（m）； Mp：车身质量（kg）；VL，VR：左右两轮施加于车身的垂直作用力（N）；HL,HR：左右两轮施加于车身的水平作用力（N）；L：质心距离车轴的距离（m）；θp：车身对于 y 轴倾斜角（rad）；Jpθ：车身对于 z 轴的转动惯量（kg·m2）；CL，CR：连接左右两轮的电机产生的扭矩 （kg·m）.

当平衡车达到平衡状态时，sinθP≈θp，cosθp≈1； 根据式（1）（2）（3）（4）（5），得到微分方程：

2 控制系统设计

图2 系统组成示意图Fig.2 Schematic diagram of system components

当平衡车进入平衡状态时，安装于车身的陀螺仪和加速计实时采集数据，并传送至主控制器。主控制器将数据进行Kalman滤波[3]和平衡算法处理，得出姿态调整所需要的车轮加速度值，换算为电机的控制量，通过串口3发送到舵机驱动器，控制两路由舵机改装的伺服电机作姿态调整。安装于车轮的编码器得到实际速度和运行距离，反馈回主控制器，经由PID算法进行误差调整后再次将控制量发送到舵机控制器，形成一个闭环反馈。整个系统不断进行调整便可以维持平衡车的平衡。

平衡车行走和转弯是由遥控器的方向消息给出，当遥控接收器收到消息后，引起主控制器中断，通过平衡算法处理以后给出合适的控制量，控制左右两个轮子行走和转弯，同时LCD要显示方向键的按下状态。

当检测到平衡车失去平衡的时候，主控制器进入特别处理模式，将启动舵机M1或M2，在机械臂的作用下重新归位到平衡状态，在动作完成后，机械臂回到初始位置。

3 硬件设计

本文主控制器采用意法半导体生产的STM32F103ZE[4]作为主控制器，它使用来自于ARM公司具有突破性的Cortex-M3内核，该内核集高性能、低功耗、实时应用、竞争性价格于一体。STM32F103ZE的主要性能参数有：主频最高72 MHz，90 DMips，1.25 DMips/MHz。 512 k 字节闪存程序存储器，64 k 字节 SRAM。 外设有： 定时器、ADC、SPI、I2C、USART和USB等。

三轴加速计选用飞思卡尔的MMA7260QT[5]，MMA7260QT的量程有 1.5 g、2 g、4 g、6 g 4个， 本文选择 1.5 g的量程范围，精度为800 mV/g，内部集成了低通滤波器。将MMA7260QT的x轴y轴接入STM32的AD采样，其中一个轴作冗余设计，以增强可靠性，12位的AD采样精度可达0.8 mV，转换时间为 1 μs。

陀螺仪采用亚德诺半导体的ADXRS300[6]，其量程为±300°/s，线性误差小于 0.1%，使用 FPGA采样积分后，以串口方式输出角度信息。经信号处理后的误差小于0.5°，温度漂移小于 0.6°。

舵机控制器是用于控制平衡车左右车轮伺服电机，它可以同时控制16路舵机，通过串口接收命令，并输出PWM波形。舵机选择了比亚迪公司生产的MG995，其速度为0.17 s/60°(4.8 V)，扭矩为 13 kg·cm，死区设定 4 μs。 用于车轮的舵机需要去除销钉，并用两个2.5kΩ电阻取代滑动变阻器。

遥控器部分是采用PT2262与PT2272,分别用于调制解调，PT2272解调后的信号引脚分别与STM32的Remote1～Remote4（PC8～PC11）连接，前后左右4个按钮对应其中Remote1～Remote4，当遥控器有按键按下时STM32产生中断并作相应的处理。

编码器由两部分组成，即：码盘和信号采样电路。码盘与车轮共轴，车轮转动带动码盘转动，采用两个直射式红外光电传感器作采样电路的设计。本文选用南旭科技的光电传感器 ST150，ST150的响应时间为 5 μs、光缝宽度 0.4 mm、遮光电流1 μA、通光电流0.25 mA，满足设计要求。同时使用两个ST150是为了得到相位差调整为90°两路波形，这样就可以检测出车轮的旋转方向。经过施密特触发器74HC14整形后输入到STM32的Encode1～Encode4(PC4～PC7)。编码器电路如图3所示。

图3 编码器Fig.3 Encoder

LCD选用奕力科技的ILI9320，使用SPI总线与STM32主控制器连接。将以上几个部分构建为一个系统，主控制器STM32的电路图如图4所示。

USART2和USTAR3分别与陀螺仪和舵机控制器通讯，USART2与USART3经MAX3232芯片将TTL电平转换为RS232电平，RS232的工作范围是-15～+15 V，即使存在电压衰减传输信号也可以被有效识别。

4 软件设计

本文采用实时操作系统uCOS-II[7]作为STM32的片上操作系统，根据不同的功能分为3个不同的任务。角度和角加速度等重要数据通过中断实现，保证能够快速得到处理。软件的基本结构如图5所示。

图5 软件总体框图Fig.5 Block diagram of software

图4 主控制器设计Fig.4 Design of master controller

在STM32复位以后，首先执行初始化操作。依次为PLL、SPI、串口、A/D 采样、I/O、DMA、定时器、操作系统，在完成以上操作以后，启动操作系统并创建任务，开始运行应用程序。

uCOS-II任务设计为3个，根据优先级由高到低得顺序依次是：

1）任务一 处理陀螺仪传输来的数据，并和加速计采样的数据通过Kalman算法滤波，滤波完成后发送信息量通知任务二处理。如果检测到状态为失去平衡，则通知任务二进行恢复平衡状态的操作。

2）任务二 在平衡状态下，如果收到信号量(Balace_sem)，PID算法将编码器得到的加速度值与平衡算法计算的加速度值进行调整，并将调整后的控制量输出到舵机控制器。如果收到信号量(Unbalace_sem)，则启动机械臂使得平衡车恢复平衡状态。

3）任务三 等待遥控器的信号量，如果有信号量便通知任务一进行方向调整，同时通过SPI发送到LCD显示。

传感器的数据都以中断的方式处理，根据优先级由高到低得顺序依次是：

1）用于接收陀螺仪数据的串口2；

2）连接舵机控制器的串口3；

3）左右两个编码器的I/O；

4）遥控器4个方向按钮的I/O。

加速计的AD采样使用查询的方式。在得到陀螺仪的数据后，立即进行AD采样，使得加速计和陀螺仪得到的数据尽量保持在较短的时间内。系统软件流程如图6所示。

图6 系统软件流程图Fig.6 Flow chart of system software

5 实验结论

使用ARM开发工具RealView MDK[8]对平衡车进行调试，该开发工具自带的Logic Analyzer窗口可实时跟踪变量的变化。因此，在平衡车进入平衡状态时，用Logic Analyzer窗口观测到车身的倾角变化，软件中变量Gyro_degree代表陀螺仪得到的车身倾角θp，Logic Analyzer窗口得到的数据如图7所示。

图7 Logic Analyzer数据Fig.7 Data of Logic Analyzer

根据Gyro_degree的数据，θp的数据大部分在范围-5°≤θp≤5°内振荡，说明系统运行平稳可靠。

在室内无坡度的地面上，使用遥控器做S型曲线运动，倾角的范围为：-5°≤θp≤5°，并且LCD可正确显示出方向信息。当 θp＞25°，或者 θp＜-25°时，平衡车失去平衡。此时舵机 M1与 M2启动，将平衡车调整至-5°≤θp≤5°的范围内，重启动平衡控制。

此样车按照目前市场价格，总价约1 500人民币，相对于Segway成本大大降低。

6 结束语

本文以STM32为处理器，从硬件搭建了一个自平衡系统，以实时系统uCOS-II为片上系统，进行软件设计，并对平衡系统进行功能扩展。实验证明，本系统功能丰富，成本低廉，其中自动恢复平衡的功能，可用于机器人排爆，野外探测等领域。

[1]Crasser F， D’Arrigo A,Colombi S， et al.JOE：A mobile，inverted pendulum [J].IEEE Transactions on Industrial Elec-tronics，2002， 49（1）：107-114.

[2]陈伟，延文杰，周超英，等.两轮自平衡机器人控制系统设计[J].传感器与微控制器，2008，27（4）：117-120.CHEN Wei，YAN Wen-jie，ZHOU Chao-ying.Control system design of two wheels self-balance robot[J].Transducer and Microsystem Technologies，2008,27(4)：117-120.

[3]袁泽睿.两轮自平衡机器人控制算法的研究[D].哈尔滨：哈尔滨工业大学，2006：22-31.

[4]STMicroelectronics Integrated Products.STM32 Reference manual[EB/OL].(2011-01-01)[2011-04-01].http：//www.stmicroelectronics.com.cn/internet/com/TECHNICAL_RESOURC ES/TECHNICAL_LITERATURE/REFERENCE_MANUAL/CD00171190.pdf.

[5]Freescale Integrated Products.MMA7260QT[EB/OL].(2008-05-03)[2011-04-01].http：//cache.freescale.com/files/sensors/doc/data_sheet/MMA7260QT.pdf?fsrch=1＆sr=1.

[6]Analog Devices Integrated Products.ADXRS300[EB/OL].(2004-01-01)[2011-04-01].http：//www.datasheetarchive.com/pdf/getfile.php?dir=Datasheets-4＆file=DSA-67061.pdf＆scan=.

[7]Labrosse J J.嵌入式实时操作系统μCOS-Ⅱ[M].邵贝贝，等译.北京：北京航天航空大学出版社,2003.

[8]李宁.ARM开发工具RealView MDK使用入门[M].北京：北京航空航天大学出版社，2008.