ISSN1672 － 4305 CN12 － 1352 / N 实 验 室 科 学 第 LABORATORY SCIENCE 两轮自平衡小车的设计与实现 王良成，杨志民，胡聪聪，桂艺纹，荣 蒙 ( 三亚学院 理工学院，海南 三亚 572000) 期 卷 第 15 Vol. 15 No. 6 6 月 年 2012 12 Dec. 2012 单片机为核心控制器，配合陀螺仪 摘 要: 两轮自平衡小车被各种竞赛所喜爱，在很多大型比赛中都有涉及此方向的题目 尔系列 XS128 衡小车的结构 关键词: 两轮自平衡小车; 控制算法; 单片机; 中图分类号: TP39; G642． 44 文献标识码: A 控制算法，通过反复调试最终实现了小车的自平衡 角度传感器实现两轮小车的自平衡 doi: 10． 3969 / j． issn． 1672 － 4305． 2012． 06． 017 平衡原理 XS128 、 。 。 、 、 阐述了基于飞思卡 。 着重介绍了两轮自平 Two － wheel self － balancing car design and implementation WANG Liang － cheng YANG Zhi － min HU Cong － cong GUI Yi － wen Polytechnic Institute Sanya University Sanya 572000 China ， ， ) ， ， ， ， ( ， RONG Meng : Abstract The self － balanced two － wheel car is popular in various competitions ， and this event oc- cured in many large games． Based on the Freescal Semiconductor XS128 series single － chip micro- computer seted as the core controller together with the gyroscope and angle sensor we achieved two － ， wheel car self balance． This paper highlights the self － balanced two － wheel structure balance princi- ， ， ， : ple control algorithm and the car self － balanced through repeated testing． Key words two － wheel self － balancing car control algorithm single chip microcomputer ; ; ; XS128 ］ 1 “ ( ” 独立驱动 、 Dynamic Stabilization 两轮自平衡小车两轮共轴 车身中心 、 位于车轮轴上方，通过运动保持平衡，可不借助外力 直立行走［ 其自动平衡运作原理主要是建立在 。 ) 的基 一种被称为 动态稳定 本原理之上，也就是车辆本身的自动平衡能力 以 。 内置的精密固态陀螺仪( ) 、 角速度传感器来判断车身所处的姿势状态，透过精 密且高速的中央微处理器计算出适当的指令后，驱 动马达来做到自平衡的效果［ 本文介绍了两轮 自平衡小车的结构 硬件设计及软件 、 设计方法 。 动态平衡原理 、 Solid － State Gyroscopes ］ 2 。 1 自平衡小车结构 两轮自平衡小车主要由机械系统和控制系统组 车身上支 下平台和连接杆等，负责支撑硬件电路，搭建工 、 核 、 成，机械系统包括两个独立驱动的车轮 架 作平台; 控制系统包括直流电机 电机驱动电路 、 、 基金项目: 三 亚 学 院 教 学 改 革 项 目 ( 项 目 编 号: ) 。 JG03 2011 － 心控制器 各种传感器和电源等［ 、 ］ 3 。 。 自平衡车车身结构对控制系统算法设计有很大 影响，如车轮尺寸的选择 车身高度设计和重心位置 、 只有在结构上保证了车身的构造合理，才能减 等 少控制系统由于车本身结构设计缺陷而造成的控制 算法复杂化，甚至导致小车难以实现自平衡［ 结 构框图如图 所示 。 ］ 4 1 。 自平衡小车由陀螺仪 心控制器( 驱动电路下方) 电源 、 车身 驱动电路 核 、 、 、 车轮等关键部分组成 、 。 2 2 自平衡小车动态平衡原理 自平衡小车动态平衡原理如图 所示，安装于 车身的陀螺仪( 陀螺仪的输出值是相对灵敏轴的角 速率，角速率对时间积分即可得到周围灵敏轴旋转 过的角度值) 和加速度传感器实时采集数据［ ］，并 传送至主控制器 主控制器进行卡尔曼滤波和平衡 算法处理，得出姿态调整所需的车轮加速度值，换算 为电机的控制量，通过串口发送到舵机控制器，控制 安装于车 两路由舵机驱动的伺服电机做姿态调整 轮的编码器得到实际速度和运行距离，反馈回主控 。 。 5 

王良成，等: 两轮自平衡小车的设计与实现 35 74LS244 离芯片 的目的是为了将核心控制器与电机 驱动芯片进行隔离，防止电机启动时影响其正常工 作( 使单片机复位) ; 电机驱动芯片则是通过核心控 制单片机产生的 信号进行控制电机的转向和 转速; 开关电源芯片 电源电压稳压 ，为核心控制单片机以及其他芯片提供工 输出在 作电源，电机驱动电路如图 LM2596 所示 7． 2V PWM 5V 将 3 。 图 1 自平衡小车结构框图 4 自平衡小车软件设计 。 电机控制程序 、 序 和中断响应程序等程序段组成 为了各个传感器信号的采集，通过 行处理输出 持自平衡小车的姿态平衡 可靠性，系统还 增 加 了 软 件 看 门 狗［ 系列增强型 两轮自平衡小车的软件部分主要由初始化程 卡尔曼滤波程序 控制算法程序 、 、 它们的主要功能是 对信号进 信号，控制电机进行相应转动，保 为了进一步提高软件的 是 。XS128 为单片机， 等常 因此，可以极大简化程序编写难度，关键程 SPI、SCI、AD、PWM 公司推出的 XS128 PWM 16 。 ］ 7 S12 Freescle 片内资源丰富，接口模块有 见模块 。 序段如下 。 4． 1 初始化 #include ＜ math． h ＞ #define uint unsigned int #define uchar unsigned char #define Acceler ATD0DR0 / / float Angle_mid float Adjust_up1 ; ， Angle_last ， Adjust_up2 ; 角度 AD 转换数据 / / 角度调整中值 姿态传感器输出整定 / / 图 2 自平衡小车动态平衡原理 值 。 PID 制器，经由 发送到舵机控制器，形成一个闭环反馈 。 不断进行调整便可以维持小车的平衡［ ］ 6 算法进行误差调整后再次将控制量 整个系统 2a 当小车处于瞬时平衡状态时，两个电机处于相 对静止状态，保持小车直立( 图 ) ; 当出现扰动导 致小车倾倒( 以后仰为例) ，此时车轮行进方向为向 前，角加速度方向向后，小车失去平衡( 图 ) ; 陀螺 仪检测到这个变化配合角度传感器发送纠偏信号给 核心控制器，核心控制器根据纠偏信号的极性控制 电机转向，车轮向后转动来纠正后仰 2b 。 3 自平衡小车硬件设计 两轮平衡小车硬件设计主要是驱动电路的设 电机驱动 电 机 驱 动 芯 片 其中，隔 计，驱动电路质量将直接影响控制效果 电路 主 要 由 隔 离 芯 片 开关电源芯片 74LS244、 等组成 。 LM2596 。 BTN7971、 static float q_bias = 0 ， ， ; K_1 t_0 t_1 ， angle_err ， PCt_0 ， PCt_1 ， E ， K_0 ， void Kalman_Filter ( float angle _ m ， float gyro _ m ) / / gyro _ uint PWM_Duty1 uint Speed_L uchar PIT_isr_flg = 0 ， PWM_Duty2 ; ， Speed ， Speed_R ， ， j = 0 i ; ; / / PWM 占空比输出 4． 2 卡尔曼滤波 ， static float angle angle_dot ; / / static const float Q_angle = 0． 001 ， dt = 0． 005 / / ; 注意: 的取值为 dt kalman R_an- 滤波器采 外部需要引用的变量 ， Q_gyro = 0． 003 ， gle = 0． 5 样时间; = ］［ ［ ］ 2 2 static float P { { { } ; } ， } ， 0 ， 1 1 0 static float Pdot ［ ］ 4 { 0 = static const char C_0 = 1 ， 0 ， 0 } ; ， 0 ; 

45 : m gyro_measure { = Q_angle － P ］; ］; ［ ］［ 1 1 ［ ］［ 1 1 ; Pdot Pdot Pdot = － P = － P angle + = ［ ］ 0 ［ ］ 1 ［ ］ 2 ］ ［ Pdot 3 = Q_gyro ］［ ［ ］ 0 0 P ］［ ［ ］ P 0 1 ］［ ［ ］ 0 1 P ］［ ［ ］ P 1 1 + = Pdot + = Pdot + = Pdot + = Pdot ［ ］ 0 ［ ］ 1 ［ ］ 2 ［ ］ 3 * dt * dt ; ; ; ; * dt ; * dt angle_err = angle_m － angle PCt_0 = C_0 * P PCt_1 = C_0 * P ［ ］［ 0 0 ］［ ［ 1 0 ］; ］; E = R_angle + C_0 * PCt_0 ; ; K_0 = PCt_0 / E K_1 = PCt_1 / E t_0 = PCt_0 ; ］［ ［ 0 1 ］; t_1 = C_0 * P ［ ］［ ］ 0 0 P ］［ ［ ］ 1 0 P ］［ ［ ］ 0 1 P ］［ ［ ］ P 1 1 － = K_0 * t_0 － = K_0 * t_1 － = K_1 * t_0 － = K_1 * t_1 angle + = K_0 * angle_err q_bias + = K_1 * angle_err angle_dot = gyro_m － q_bias } ; ; ; ; ; ; ; 图 3 电机驱动电路 4． 3 电机控制 ( ) * dt gyro_m － q_bias ］［ ［ ］ 0 1 ; ］［ ［ 1 0 ］; － P 平衡小车车轮电机的控制采用 芯片 调制控制，控制信号通过 后直接控制电机速度和正反转［ PWM ］ 8 。 PWM 脉冲宽度 放大 BTN7971B ; { up1 － PWM_Duty1 = 0x06 + ( ) ; int Speed_last ( ) int Adjust _up2 + ( ) int Adjust _ void PWM_calculate { Adjust_up1 = angle_dot* 0． 02 ; ( ) void Adjust_up2 = angle* 20 ( if ( angle ＞ 0 / / ) ) angle_dot ＞ 0 if ; 加速度向前 { { PWM_Duty1 = 0 － ( ; ) Speed_last int 0x06 － PWM_Duty2 = 0x00 ( if angle_dot ＜ = 0 ) ( ) int Adjust_up1 + ( ) int Adjust_up2 + ; } PWM_Duty2 = 0x00 } ; } ( ) angle ＜ 0 if ( else { { if angle_dot ＜ = 0 PWM_Duty1 = 0x00 / /加速度向后 ) ; ( PWM_Duty2 = ; } Speed_last ) int ( － ) ) ( if { angle_dot ＞ 0 PWM_Duty1 = 0x00 ; Adjust_up2 + 0x06 － } int Adjust_up1 － ( ) int Adjust_up2 + 0x06 ( ) int Adjust_up1 － ( PWM_Duty2 = 0 － ; } ( ) Speed_last int ) int 

王良成，等: 两轮自平衡小车的设计与实现 55 } 4． 4 控制算法 ］ 9 。 MCU 波占空比［ 控制算法的编写是平衡控制的核心问题，也是 最难解决的问题，涉及角度融合和电机控制两个部 角度融合用于求出陀螺仪和加速度计的互补角 分 度值; 电机控制需要根据 算出的角度值控制 具体控制算法编写原理是: 首 PWM 先，读取加速度传感器和陀螺仪的值，然后将其作卡 尔曼融合，得到当前小车的姿态 算法 与设定姿态量比较，产生信号控制电机，核心控制单 次的姿态反复调整，就能够达到比较 片机每秒 完美的动态平衡［ ) 控制算法程序如下: 再使用 PID 300 。 。 。 ］ 10 ( void main { void 锁相环初始化 输出初始化 / / PWM 定时器初始化 ( PLL_Init PWM_Init ) ; 60 ( ) ; / / PIT_Init / / ( ) ; ( ) ; AD_Init / / AD ; 初始化 开总中断 EnableInterrupts / / ( ) 1 while { ( if { ; 0015 角度计算 / / ) ; ue } ANGLE_value = asin ( ANGLE_value ) ( ) ; * 57． 3 / / float ANGLE_speed_ ANGLE_speed_value = ; value_mid* 0． 36 角速度单位换算 ( Kalman_ Filter 卡尔曼滤波 / / ANGLE _ value ， ANGLE _ speed _ val- 5 结语 给出了关键程序段，所有程序都经调试 本文论述了两轮自平衡小车的设计全过程 对 结构，软件设计及硬件设计部分做了详 其平衡原理 、 试 细介绍 、 运行，经实践证明硬件设计及软件设计正确合理 。 通过程序反复修改，硬件持续改进，最终实现了自平 衡，平衡效果较好 。 。 。 参考文献( References) : ［ ］ 张伟民，段晓明，赵艳花 1 理论与应用， 4 ［ ］ 梁文宇，周惠兴，曹荣敏 2 2011 ( ， 30 ． ) : ］ ［ J ． 控制工程， 2010 ( S2 ］ 两轮自平衡小车控制研究［ J ． 控制 10 － 13． 双轮载人自平衡控制系统研究综述 ． ) : ］ 平衡车机电控制系统建模与仿真［ J ． 139 － 144 ， 190． PIT_isr_flg = = 1 ) ］ 孙军，万明伦，吕博，等 ［ ． 3 ) : 机械与电子， ( 2010 10 34 － 37． ( ) ; 计算 / / pwm ， PWM _ Duty1 PWM _ Duty2 PWM_calculate ( PWM _ value } pwm } } 4． 5 PIT 中断响应 #pragma CODE_SEG __NEAR_SEG NON_BANKED interrupt VectorNumber_Vpit0 void PIT_ISR { ( ) void PITTF_PTF0 = 1 ; while ATD0STAT2L_CCF0 Acceler_value = Acceler / / ) ; 角度提取 ; ) ; ( ! ( ! while ATD0STAT2L_CCF1 Gyro_value = Gyro / / ; 角速度提取 度纠正 / / ANGLE_value_mid = Acceler_value － 0x04e0 ; 角 / / ANGLE_speed_value_mid = Gyro_value － 0x0424 角速度纠正 ; ANGLE _ value = ( ) float ANGLE _ value _ mid * 0． ) ; / / ［ ］ 魏延辉，刘胜，高延滨，等 4 ． 基于两轮自平衡机器人组合定位方 ］ 法的研究［ J ． 机械与电子， 2010 ( ) : 10 58 － 63． ［ ］ 张吉昌，程凯，郑荣儿 5 ． 单轴双轮自平衡车姿态监测方案设计 ］ ［ J ． 中国海洋大学学报， 2009 ( ) : 9 467 － 470． ［ ］ 张志强 6 ． ) : ( 19 13 基于 STM32 ］ 的双轮平衡车［ J ． 电子设计工程， 2011 ， 103 － 106． ］ 薛伦生，舒涛，戴新生 ［ 7 电子测量技术， ］ ［ J ． ［ ］ 杨继志，郭敬 8 基于 基于单片机控制的小车平衡系统设计 ． ( ， 32 2009 ) : 85 － 87． 5 两轮自平衡小车控制系统设计 ． MMA7260 机电产品开发与创新， ］ ［ J ． ( ， 24 3 ) : 144 － 145． 2011 ］ 蒋纬洋，邓迟，肖晓萍 ［ ． 9 ， 31 电子测量技术， 2012 两轮自平衡车系统制作研究［ ］ J ． ( ) : 6 76 － 79． 国外 ［ ］ 秦剑 10 ］ 两轮自平衡车的制作［ J ． ． 电子制作， 2011 ( ) : 1 9 － 11． 收稿日期: 2012 － 09 － 03 修改日期: 2012 － 09 － 27 作者简介: 王良成( 1982 － ) ，男，吉林舒兰人，学士，实验师， 实践教学及实验室 、 主要研究方向为电子类实验 管理 。