基STM32两轮衡车控制系统设计
Control system design of two wheel self balancing vehicle based on stm32
容摘
文研究两轮衡车控制系统设计实现两轮衡车机械结构简单直立行驶方式赋予灵活操控方式场景中投入日常生活代步工业生产特殊搬运需求等等控制系统相关算法相关理具广阔应前景
文STM32单片机控芯片提出系统设计方案讲解两轮衡车控制系统需求框架设计思路然需硬件进行选型电路设计系统进行软件开发通两轮衡车姿态分析解算运PID算法PDPIP算法两轮衡车直立环速度环转环进行控制蓝牙线通讯技术实现两轮衡车线操控功实现控制便捷性
系统进行调试测试证明该两轮衡车控制系统行性稳定性
关键词：两轮衡车 控制系统 STM32单片机 PID算法 蓝牙线通讯
Abstract
This paper mainly studies the design and implementation of a twowheel selfbalancing vehicle control system The mechanical structure of the twowheel selfbalancing car is simple and the upright driving mode gives flexible control methods which can be put into use in different scenarios such as daily life mobility special handling requirements for industrial production etc The related algorithms and related theories of its control system have broad application prospects
This article takes STM32 microcontroller as the main control chip puts forward a system design plan and explains the requirements framework and design ideas of the twowheel selfbalancing vehicle control system Then select the required hardware and circuit design and software development of the system After analyzing and solving the attitude of the twowheel balancing car the PD PI and P algorithms of the PID algorithm are used to control the upright ring speed ring and steering ring of the twowheel balancing car And the use of Bluetooth wireless communication technology to achieve twowheeled selfbalancing car wireless control function to achieve its control convenience
Finally the system was debugged and tested to prove the feasibility and stability of the twowheel balance car control system
Keywords Twowheeled selfbalancing car Control system STM32 singlechip PID algorithm Bluetooth
目录
第章 绪 1
11 文研究背景意义 1
12 国外两轮衡车研究历史现状 1
13 文研究容目标 2
14 文结构安排 2
第二章 系统设计方案 3
21 需求分析 3
22 系统框架 3
23 设计思路 4
第三章 硬件系统设计 5
31 STM32系统设计 5
311 控芯片 5
312 系统设计 6
32 电源系统 6
321 降压芯片选型 6
322 电源系统原理图 7
33 MPU6050六轴陀螺仪传感器模块 7
331 MPU6050概述 7
332 MPU6050系统原理图 8
34 TB6612FNG电机驱动模块 9
341 TB6612FNG模块概述 9
342 TB6612FNG原理图 9
35 BT04A蓝牙串口模块 10
351 BT04A蓝牙串口模块概述 10
352 BT04A蓝牙串口模块原理图 10
36 OLED显示屏模块 11
361 OLED显示屏概述 11
362 OLED显示屏接线图 11
37 电机编码器 12
38 章结 12
第四章 软件系统开发 13
41 总体方案 13
42 MPU6050读取姿态信息 13
421 读取原始数 13
422 读取DMP转换欧拉角 14
423 IIC通讯方式 14
43 霍尔编码器测速 16
431 程序框图 16
432 编码器软件四倍频 16
44 蓝牙线传输 17
441 STM32串口通讯 17
45 遥控终端开发 18
第五章 两轮衡车关键算法 20
51 MPU6050姿态解算 20
511 DMP输出四元数 20
512 欧拉角转换 20
52 两轮衡车PID控制算法 20
521 PID算法概述应 20
522 基PD控制直立环 21
523 基PI控制速度环 21
524 基PD控制转环 22
第六章 系统测试 23
61 测试目测试环境 23
62 电源系统检测 23
63 STM32系统测试 24
64 蓝牙线通讯测试 25
65 陀螺仪测试 25
66 两轮衡车PID调试 27
661 直立环PD算法调试 27
662 速度环PI算法调试 28
663 转环PD算法调试 28
67 两轮衡车遥控测试 30
68 章结 30
结 31
参考文献 32
致谢 33
第章 绪
11 文研究背景意义
类生存发展已步入21世纪高科技时代日常生产生活中离开数字技术计算机技术机械设计技术等高科技技术两轮衡电动车两轮轴机械结构通带精密电子陀螺仪(SolidState Gyroscopes)电子衡系统实现动衡直立行走功外形巧行动灵活环保方便特点市场广认发展目前种需移动运输操作传统机械设备基驱四轮式者履带式形式存传统机械移动方式已满足生产生活工智求市场急需种更方便灵活机械辅助移动设备直立式两轮衡电动车出现满足需求
两轮衡车系统运行时衡控制规划行驶操控等种功时运行关键点实现衡时环境中直立行驶实现控制操作两轮衡车概念稳定动态性系统非线性迅速成种控制理研究台具较科研意义
12 国外两轮衡车研究历史现状
年全球国两轮衡机器研究越越火爆迅速发展国纷纷研发实验机两轮衡车衡控制方案陆续研发设计出呈现出控制系统样性两轮衡车稳定行驶衡系统改造快速方便应环境里工业生产需承载运输日常生活代步等国说巨机遇外国公司市场生产研发相应商业产品投放市场
2002年台名Segway HT两轮直立式衡载设备问世该设备美国Segway公司研制该设备行驶灵活体型巧特点员密集机场中机场安保警务员站立该两轮衡车群中灵活快速移动行驶居高行驶方式满足观察机场处需求够时发现处理疑情况2006年家位德国Transport公司针室外现场摄影工作者研发两轮摄车该设备广泛应电视节目录制甚电影特殊情节拍摄中2007年日丰田汽车公司研发台丰田机动机器(Mobility Robot)台15千克机器高行驶速度高达时20千米
哈尔滨工程学研究员两块C8051单片机机交互位机组成控制系统通传感器测量计算出车体姿态信息脉宽度调制(PWM)控制两台直流电机外加机交互线传输等技术制造台两轮直立衡机器
中国科技学研发款两轮衡代步电动车采左右轮轴机械结构通建立系统数学模型建立控制算法计算输出脉宽度调制(PWM)控制控制两伺服电机转矩电动车行驶程中保持直立衡
13 文研究容目标
文研究容：
（1）两轮衡车系统设计方案
（2）两轮衡车硬件选型电路设计
（3）两轮衡车控制算法
（4）控制系统调试测试
根设计功求实现两轮衡车衡情况完成遥控操作两轮衡车前进退转弯等行驶功
14 文结构安排
文六章节构成：
第章绪研究文背景意义分析国外两轮衡车历史现状提出文研究意义目
第二章系统设计方案两轮衡车需求进行分析设计系统框架提出设计思路
第三章硬件系统设计结合两轮衡车功需求需硬件进行选型分析设计原理图
第四章软件系统开发提出软件系统总体方案分析模块软件系统开发流程
第五章两轮衡车关键算法陀螺仪输出姿态数进行解算PID算法电机进行控制两轮衡车保持直立衡
第六章系统测试两轮衡车系统进行测试PID算法调试验证两轮衡车控制算法行性稳定性
第二章 系统设计方案
21 需求分析
系统求两轮衡车衡状态通蓝牙线传输技术遥控实现两轮衡车移动行驶功系统STM32F103C8T6作衡车控芯片实现功求：
（1）获取车衡姿态进行姿态解算
（2）PID算法控制两轮衡车静止行驶状态保持衡
（3）够通蓝牙线传输技术手机APP蓝牙遥控操车行驶
（4）OLED显示屏显示电池电容量等车状态信息
22 系统框架
系统核心控制器选STM32F103C8T6单片机LM2596AMS1117芯片组成电源系统提供硬件稳定安全工作电压环境MPU6050六轴传感器测量出车车体行驶姿态电机编码器车移动速度OLED显示屏显示车体姿态信息电池电压状况TB6612电机驱动模块负责驱动电流直流电机运转BT04A蓝牙模块实现两轮衡车遥控器间线通讯功系统结构图21示
图21 系统结构图
23 设计思路
根模块传感器STM32间连接通讯方式系统设计思路图图22示
图22 硬件设计思路
第三章 硬件系统设计
31 STM32系统设计
311 控芯片
两轮衡车系统工作时需反复快速读取陀螺仪电机编码器等传感器数马算法处理输出控制量反馈控制直流电机控制系统控芯片处理速度运行存求较高
控制系统控芯片选取意法半导体公司（ST）生产STM32F103C8T6型号单片机核CortexM3ARM公司设计参数表31示
表31 STM32C8T6参数表
工作电压
2V~36V
总线宽度
32位
速度
72 MHz
FLASH容量
64KB
RAM容量
20K
72MHz速度32位总线宽度满足两轮衡车控制系统处理速度求64 KBFLASH存储器满足程序存储
图31STM32F10x系统构架图该图提供STM32F103C8T6单片机部结构关系外设资源目然STM32运开发提供帮助
图31 STM32F10x系列系统构架图
312 系统设计
单片机系统定义满足单片机正常工作系统STM32单片机系统单片机外电源复位时钟三种电路调试接口boot启动选项组图32系统系统原理图
单片机选取LQFP48封装单片机电源降压电源电源系统详细设计容面章节中分析复位电路设计键触发复位RST引脚通键S2键引脚RST接RST引脚高电拉低低电触发单片机复位功键松开系统正常工作复位电路中1uf电容10kΩ电阻提供复位时间外置8MHz晶振时钟加入20uf负载电容该电容作保证晶振正常工作时钟电路提供精准时间SWD载模式需引脚较少节约偏资源设置boot模式设计较简单需boot0boot1引脚引出跳线帽方式设置
图32 STM32F103C8T6系统原理图
32 电源系统
321 降压芯片选型
直流电机驱动电压电流较系统选取12V锂电池供电12V电压环境单片机传感器模块说电压太高直接供电会烧毁击穿模块需电压进行降压处理
选LM2596降压芯片输入电压40V驱动电流达3A热关断限流保护力负载调节力强12V锂电池电压通LM2596组成降压电路输出稳定5V电压
选AMS111733正低压降稳压器LM2596输出5V电压进行降压输出33V电压
322 电源系统原理图
根设备需电源求根LM2596AMS111733数手册设计出设备电源系统图33
图33 电源系统原理图
衡车输入电压12VLM2596降压输出电压5V负载3A5V电压AMS111733降压输出33V电压电源系统加入电流开关S3开S3系统电源开启降压模块开始工作提供稳定电压关闭S3整电源系统关闭
33 MPU6050六轴陀螺仪传感器模块
331 MPU6050概述
MPU6050六轴陀螺仪传感器含MEMS 陀螺仪MEMS加速度计三轴嵌DMP（Digital Motion Processor）数字运动处理器通讯方式IIC协议SPI协
VCC引脚接入33V5V电压GND引脚接SDASCL两组引脚IIC通讯引脚信号线MPU6050六轴陀螺仪传感器需外接传感器时XDAXCL引脚分接IIC串行数信号线时钟信号线AD0通接接电设置AD0值01AD00时MPU6050址0x68AD01时MPU6050址0x69INT引脚中断输出引脚MPU6050六轴陀螺仪传感器引脚说明表32
表32 MPU6050引脚说明
引脚名称
说明
VCC
电源输入33V5V
GND
接
SCL
IIC时钟信号线SCL
SDA
IIC数信号线SDA
AUX_DA
IIC串行数信号线外接传感器
AUX_CL
IIC串行时钟信号线外接传感器
AD0
接悬空时址0x68接VCC址0x69
INT
中断输出引脚
332 MPU6050系统原理图
翻阅MPU6050数手册设计出接线原理图图34示
图34 MPU6050原理图
MPU6050电源引脚33V电压电源入口器件旁边加入滤波电容滤交流成分输出直流更滑确保MPU6050硬件稳定性满足控制系统高精度姿态数值需
AD0引脚接AD0值0设置MPU6050设备址0x68IIC通讯引脚SDASCL分接入STM32PB9PB8引脚加入47KΩ拉电阻作保证正常高电输出起保护芯片作
34 TB6612FNG电机驱动模块
341 TB6612FNG模块概述
两轮衡车带两直流电机12V锂电池然电机转动法直接控制转速控制车体保持衡需块专门负责驱动控制电机芯片
东芝半导体公司生产TB6612FNG芯片款直流电机驱动器件该芯片带电流MOSFETH桥结构够输出双通道电路刚驱动两轮衡车两电机该芯片低热耗需加散热片100KHzPWM信号输入频率满足两轮衡车控制系统需求TB6612FNG参数表33示：
表33 TB6612FNG参数表
工作电压
5V
输入电压
15V
输出电流
32A
功模式
正反转短路刹车停机
图35 TB6612FNG芯片图
342 TB6612FNG原理图
TB6612FNG电机连接原理图图36
图36 TB6612FNG原理图
TB6612FNG芯片PWMA引脚PWMB引脚分接STM32单片机PA11引脚PA8引脚单片机输出PWM信号控制两电机TB6612FNG输出引脚O1O2O3O4引脚分两组引出方便点机接线AN1AN2BN1BN2分连接单片机应IO口控制电机转真值表表34示
表34 TB6612FNG控制电机运转方真值表
引脚
停止
正转
反转
AN1
0
1
0
AN2
0
0
1
35 BT04A蓝牙串口模块
351 BT04A蓝牙串口模块概述
设备BT04A蓝牙模块该模块采蓝牙V21+EDR技术兼容UART接口成低功耗低接收灵敏性高外围电路需少许元件实现蓝牙线传输功
图37 BT04A蓝牙串口通讯模块
BT04A蓝牙串口模块参数：
表35 BT04A参数表
工作电压
33V
通讯模式
UART
通讯效距离
15米左右
户根需通AT模式输入应AT指令设置蓝牙模块名字角色(Master)者角色(Slave)配码等信息
352 BT04A蓝牙串口模块原理图
BT04A蓝牙串口模块原理图图38示
图38 BT04A蓝牙串口模块原理图
BT04A模块串口通讯引脚STM32串口通讯引脚交叉相连STM32串口输出引脚连接BT04A模块串口输入引脚STM32串口输入引脚连接BT04A模块串口输出引脚BT04A模块P12引脚外接LED灯显示蓝牙模块状态
36 OLED显示屏模块
361 OLED显示屏概述
096寸OLED显示屏显示两轮衡车电时电压车体倾斜姿态电机编码器数值等信息参数表36示
表36 OLED显示屏参数表
工作电压
33V
分辨率
128×64
显示存
128×64位字节
响应时间
微秒十微秒
表37OLED显示屏模块引脚说明
表37 OLED显示屏模块引脚表
GND
电源线
SDA
数线
VCC
输入电压
RST
复位
SCL
时钟线
DC
命令数
362 OLED显示屏接线图
OLED显示屏接线图39示输入电压33VSCLSDARSTDC引脚分接STM32PC15PC14PC13PB4引脚
图39 OLED显示屏接线图
37 电机编码器
编码器轴角度方位移转换成数字脉种旋转式传感器编码器设置电机转动轴位置输出应波形电机转动速度信息
设备采AB相输出增量式霍尔编码器图310需编码器接5V工作电源电机电便输出AB相方波信号辨转测量速度
图310 增量式霍尔编码器
38 章结
章针硬件选型设计应外围电路衡车功出发设计硬件系统芯片模块进行介绍设计工作电路模块控芯片STM32连接完成硬件系统设计
第四章 软件系统开发
41 总体方案
软件系统开始时初始化函数读取电压值判断定时器否定时时间果定时时间未回定时开始果定时时间陀螺仪输出DMP数转换欧拉角显示屏显示车体状态信息判断衡启动键否键没循环检测键果键进行PID运算输出PWM 控制电机运转控制车衡检测蓝牙否连接果蓝牙已连接点亮指示灯接收指令输出应PWM控制车行驶状态果蓝牙没连接回衡启动键检测检软件系统流程图图41示
图41 软件系统流程图
42 MPU6050读取姿态信息
421 读取原始数
MPU6050原始数获取流程图42示
图42 MPU6050姿态读取流程图
根IIC协议通讯方式软件方式控制STM32两IO口模拟出IIC通讯协议然MPU6050陀螺仪进行初始化操作配置时钟陀螺仪量程加速度量程等初始化通软件模拟IIC协议读取MPU6050寄存器值加速度角加速度信息
422 读取DMP转换欧拉角
读取MPU6050置DMP姿态信息转换欧拉角流程图43示
图43读取DMP转换欧拉角流程图
MPU6050初始化设置通IIC读取FIFO寄存器中值q30格式值进行浮点数计算欧拉角转换计算出PitchRoll
423 IIC通讯方式
文中MPU6050STM32通讯方式IIC通信
IIC种数总线SDA时钟总线SCL两条串行总线组成种串行通信协议总线IIC连接设备时设置机机设置机模式时址总线需获取机址设置机模式时需配置匹配址机发出应答信号设备中MPU6050倍设置机
整IIC协议通信流程包含起始信号(S)应答信号(ACK)数传输（发送接收）停止信号(P)
STM32需读取MPU6050数时STM32先发起开始信号 (S)方式数总线SDA时钟总线SCL保持高电数总线SDA高电转换低电形成降周期06usIIC通讯结束时时钟总线SCL保持高电06us数总线SDA低电升高电形成升IIC通讯结束标志(P)IIC开始信号结束时序图44示
图44 IIC开始信号结束信号图
IIC数宽度8位字节IIC开始信号发出发送8位数时钟总线SCL处高电数总线SDA保持稳定低电时发送数0反时钟总线SCL处高电数总线SDA保持稳定高电电时发送数1
图45 IIC数发送方式
发送完8位二进制数需应答信号(ACK)信号判定否传输次数果第9时钟脉期间时钟线高电数总线SDA稳定低电效应答位(Acknowledge)果第9时钟脉前低电期间数总线SDA保持高电次应答信号效应答位(Notacknowledge)
图46 IIC应答信号时序图
43 霍尔编码器测速
431 程序框图
电机编码器测速流程图47
图47 电机编码器测速流程
系统开始首先STM32定时器TIM2定时器TIM4进行初始化设置成编码器接口模式然开2定时器编码器接口电跳变触发定时器计算读取STM32定时器计数寄存器值计算出速度
432 编码器软件四倍频
假时间T编码器输出AB相2波形图图48示M法测量A相(B相)升者降测速时候时间T样测速方式计数2次软件四倍频算法编码器输出波形进行四倍频A相B相输出完整波形波形时时测量A相B相编码器升降图48中1~4示样样时间T计数8次提高测速工作效率
图48 编码器AB相波形4倍频
44 蓝牙线传输
该设备蓝牙线传输技术基STM32USART串口传输技术连接蓝牙模块全双工传输模式直接串口数传输相连接普通串口线进行数线传输
441 STM32串口通讯
STM32串口通讯流程图图49示
图49 串口通讯流程图
45 遥控终端开发
衡车控制系统嵌蓝牙线传输技术遥控终端开发较简单方便首先STM32编程设置前进退左转右转等操作指令指令表表41示
表41 遥控操作指令表
操作
字符指令
十六进制(HEX)指令
前进
A
0x41
退
E
0x45
左转
B
0x42
右转
F
0x46
刹车
Z
0x5A
然手机载 蓝牙调试器APP开编程模式前进退左转右转4键进行编辑前进钮时发送前进指令A前进钮松开时发送刹车指令Z防陷入前进操作循环键编程图图410~图413示
图410 前进键设置 图411退键设置
图412 左转键设置 图413 右转键设置
设置四操作键蓝牙调试器APP遥控界面图414示
蓝牙调试器APP连接衡车板蓝牙编程键时蓝牙调试器APP发送相应指令衡车板通串口传输STM32STM32计算PWM控制车完成应行驶操作
图414 蓝牙调试器APP遥控界面
第五章 两轮衡车关键算法
51 MPU6050姿态解算
511 DMP输出四元数
四元数种超复数代表加入旋转角三维空间表达式：
q(q0q1q2q3) (51)
中q0实数q1~q3虚部实数
MPU6050带硬件运动处理器DMP(Digital Motion Processing)够原始角速度数转换四元数减轻控芯片STM32工作负担DMP较简单需载官方DMP驱动库文件移植STM32DMP轴加速度角速度需注意DMP输出数格式浮点数放230倍q30模式需输出数转化浮点数表达公式：
q0quat[0] q30 (52)
q1quat[1] q30 (53)
q2quat[2] q30 (54)
q3quat[3] q30 (55)
512 欧拉角转换
欧拉角确定某围绕定点转动刚体位置组三独立角参量理解X轴滚转角(roll)Y轴俯仰角(pitch)Z轴航偏角(yaw) 组成
系统应pitchroll两参数
pitch值转换式：
asin−2×q1×q3+2×q0×q2180π (56)
roll值转换式：
atan2(2×q2×q3+2×q0×q1 −2×q1×q1−2×q2×2+1)180π (57)
52 两轮衡车PID控制算法
521 PID算法概述应
PID算法常见控制算法例(Proportional)积分(Integral)微分(Differential)三种计算组成工程实践中般P必须衍生出许组合PID控制器PPIPID等
两轮衡车控制系统分三部分第两轮衡车直立控制系统第二两轮衡车速度控制系统第三两轮衡车转控制系统三系统分PID控制算法
图51 PID算法图
522 基PD控制直立环
直立控制系统PD控制器D（微分）控制加入够快速响应车体受干扰调整车体衡状态PD控制车体直立程序框图图52示
图52 PD控制直立流程图
523 基PI控制速度环
速度控制系统PI控制器P控制器具线性利实际输出值定值两者间偏差偏差例积分进行线性组合构成控制量控制目标象PI控制车体速度程序框图图53
图53 PI控制直立流程图
524 基PD控制转环
PD控制车体速度程序框图图54
图54 PD控制转流程图
第六章 系统测试
61 测试目测试环境
两轮衡车系统功实现需通功模块相互配合完成保证两轮衡车控制系统正常运转需两轮衡车模块PID算法进行调试确保系统稳定性系统瓷砖表面房间里进行测试
62 电源系统检测
测试方法：两轮衡车控制板接电池开电源系统总开关万表测量LM2596芯片2号输出引脚电源间电压测量AMS1117332号输出引脚电源间电压
测试结果：LM2596芯片2号输出输出电压498V图61示AMS1117332号输出引脚输出电压33V图62示
结：测试结果显示电源系统功正常
图61 LM2596输出电压测试图
图62 AMS111733输出电压测试图
63 STM32系统测试
测试方法：Keil uVision5软件编写测试程序控制LED灯1S时间间隔进行闪烁编译通STLink烧录器载两轮衡车控制板STM32芯片里观察LED情况测试结果表61示
表61 STM32系统测试
测试步骤
预期情况
实际情况
结
控制板电
LED灯1S时间间隔进行闪烁
LED灯1S时间间隔进行闪烁
载调试时钟IO功正常
长复位键
LED灯熄灭
LED灯熄灭
复位功正常
松开复位键
LED灯恢复闪烁
LED灯恢复闪烁
图63 系统点灯图
图64 系统熄灯图
64 蓝牙线通讯测试
蓝牙线通讯实现车控制终端重通讯方式两轮衡车线遥控程中起非常重作实现操控便捷性
测试方法：蓝牙模块通数线连接电脑电脑开串口调试器手机蓝牙调试助手连接蓝牙模块分空旷墙体房间里面根手机端蓝牙模块间距离分进行测试观察电脑串口调试器接受信息测试结果表62示
表62 蓝牙线通讯测试系统测试
空间状态
距离
预期情况
实际情况
结
开阔
障碍
5米
电脑端串口调试器接受收手机端发信息
电脑接收信息
通
10米
电脑接收信息
通
相隔
面墙体
5米
电脑接收信息
通
10米
电脑接收信息
通
测试结果：STM32串口功正常蓝牙模块功正常线通讯功正常
65 陀螺仪测试
MPU6050六轴陀螺仪提供控制系统精准姿态信息模块运行否正常影响两轮衡车衡功
测试方法：车控制系统板电MPU6050X轴旋转轴改变板倾斜角度观察OLED屏角度信息测试结果表63示
表63 MPU6050测试表
车板姿态
预期情况
实际情况
结
板正面水放置桌面
显示屏显示角度0°
显示屏显示角度359°
通
板正面前垂直放置桌面
显示屏显示角度270°
显示屏显示角度275°
通
板正面前垂直放置桌面
显示屏显示角度90°
显示屏显示角度90°
通
图65 MPU6050读取板正面水桌面姿态图
图66 MPU6050读取板正面前垂直桌面姿态图
图67 MPU6050读取板背面前垂直桌面姿态图
测试结果：MPU6050测试出姿态方信息正确桌面法保证水程度加手扶着板观察显示屏读取出数值存误差通期PID控制调试控制车衡该误差忽略计测试验证OLED屏幕功正常
66 两轮衡车PID调试
测试两轮衡车关键系统均正常接调试PID算法验证衡车衡功测试静止状态外力干扰否实现衡功
661 直立环PD算法调试
测试方法：改变kp值kd值观察车衡状态测试结果表64表65示
测试结果：kp值500kd值1时车趋衡抖动现象段时间便倒时kpkd值进行幅放缩kpkd值缩06倍时kp值300kd值1时基没抖动现象段时间倒接需加入速度环控制保持稳定性
表64 直立环kp值调试表
kp
车状态
分析
100
车倾斜瞬间电机倾斜方加速运转车倒
kp值极性错误导致车倒
100
车倾斜瞬间电机倾斜方反方加速运转车趋直立倒
kp值极性正确响应慢导致车倒
300
车倾斜瞬间响应加快趋衡倒
kp值偏导致车倒
500
车倾斜瞬间响应快出现回抖动状况
kp值基满足条件需调节kd抑制抖动
表65 直立环kd值调试表
kd
车状态
分析
05
车倾斜瞬间电机倾斜方运转车倒
kd值极性错误导致车倒
05
车倾斜瞬间电机倾斜方反方运转车倒衡趋势
kd值极性正确响应慢导致车倒
1
车趋衡
kd值基满足条件微调更完美
662 速度环PI算法调试
速度控制正反馈kpki均正数
假设速度偏差速度半时候电机转动速度达快通软件方式电机编码器4倍频10ms获取次脉频率车轮转动速度时左右两电机编码器输出数值约160根输出PWM值7200估算kp值：
kpmax 7200160290 (61)
设置ki值：
ki kp ki (62)
调试方法：根公式kp值约90ki值045kpki合适改变kp值观察车衡状态测试结果表66示
测试结果：kp值80ki值04时满足控制求够外界干扰做出反馈具定抗干扰力
表66 速度环kdki值调试表
kd
ki
车状态
分析
40
02
车速度稳定车回摆动
速度控制响应慢kdki值
60
03
速度控制响应加快车摆动距离缩短
速度控制响应偏慢kdki值偏
80
04
车摆动距离幅减
kdki值基满足条件
100
05
车开始摆动轻碰倒
kdki值抗干扰力差
663 转环PD算法调试
测试方法：设定kd0kp02逐步幅增kp值观察衡车转情况
测试结果：kd0kp16时车转性直线行驶性优秀满足控制条件测试结果表67示
表67 转环kp值调试表
kd值
车状态
分析
02
车转控制性差
kp值
08
车转控制性转直线行驶偏差
kp值偏
16
车转控制性直线行驶偏差
kp值满足控制条件
664 PID调试结
调试步骤两轮衡车实现衡功具定抗干扰性衡车直立图图68示
图68 两轮衡车衡图
67 两轮衡车遥控测试
测试方法：手机蓝牙调试器连接衡车蓝牙水瓷砖面进行遥控操作测试结果表68示
表68 两轮衡车遥控测试
遥控操作
预期情况
实际情况
结
前进
前进
前进
通
退
退
退
通
左转
左转
左转
通
右转
右转
右转
通
测试结果：两轮衡车遥控性稳定实现控制便捷性
68 章结
章通进行种测试调试检测两轮衡车模块功运转否正常确保两轮衡车功部分状态正常调节两轮衡车PID算法参数值测试两轮衡车遥控操作实现控制便捷性两轮衡车稳定性
结
文勾画出两轮衡车流程图通PID算法直立环速度环转环进行控制实现两轮衡车车衡功结合蓝牙线传输技术两轮衡车进行线遥控操作实现操控便捷性
通文研究验证两轮衡车控制系统行性稳定性
参考文献
[1]刘军例说STM32[M]北京航空学出版社2011
[2]BP神网络PID控制器参数整定中应[J] 陈书谦张丽虹 计算机仿真 2010(10)
[3]具时滞单级倒立摆系统稳定性分析[J] 曹青松黎林 煤矿机械 2009(04)
[4]单片机复位电路设计分析[J] 包国彬张建民刘嬴 光电技术应 2005(03)
[5]数字PID算法刷直流电机控制器中应[J] 谢世杰陈生潭楼天 现代电子技术 2004(02)
[6]单轴双轮衡代步车研究设计[D] 张吉昌中国海洋学 2009
[7]两轮衡机器适应控制算法研究[D] 黎田哈尔滨工业学 2007
[8]两轮衡机器控制技术研究[D] 王瑜哈尔滨工程学 2009
[9]直流刷电动机原理应[M] 机械工业出版社 张琛编著 1996
[10]基STM32两轮衡车设计实现[D] 潘二伟黑龙江学 2018
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
基STM32两轮衡车控制系统设计
Control system design of two wheel self balancing vehicle based on stm32
容摘
文研究两轮衡车控制系统设计实现两轮衡车机械结构简单直立行驶方式赋予灵活操控方式场景中投入日常生活代步工业生产特殊搬运需求等等控制系统相关算法相关理具广阔应前景
文STM32单片机控芯片提出系统设计方案讲解两轮衡车控制系统需求框架设计思路然需硬件进行选型电路设计系统进行软件开发通两轮衡车姿态分析解算运PID算法PDPIP算法两轮衡车直立环速度环转环进行控制蓝牙线通讯技术实现两轮衡车线操控功实现控制便捷性
系统进行调试测试证明该两轮衡车控制系统行性稳定性
关键词：两轮衡车 控制系统 STM32单片机 PID算法 蓝牙线通讯
Abstract
This paper mainly studies the design and implementation of a twowheel selfbalancing vehicle control system The mechanical structure of the twowheel selfbalancing car is simple and the upright driving mode gives flexible control methods which can be put into use in different scenarios such as daily life mobility special handling requirements for industrial production etc The related algorithms and related theories of its control system have broad application prospects
This article takes STM32 microcontroller as the main control chip puts forward a system design plan and explains the requirements framework and design ideas of the twowheel selfbalancing vehicle control system Then select the required hardware and circuit design and software development of the system After analyzing and solving the attitude of the twowheel balancing car the PD PI and P algorithms of the PID algorithm are used to control the upright ring speed ring and steering ring of the twowheel balancing car And the use of Bluetooth wireless communication technology to achieve twowheeled selfbalancing car wireless control function to achieve its control convenience
Finally the system was debugged and tested to prove the feasibility and stability of the twowheel balance car control system
Keywords Twowheeled selfbalancing car Control system STM32 singlechip PID algorithm Bluetooth
目录
第章 绪 1
11 文研究背景意义 1
12 国外两轮衡车研究历史现状 1
13 文研究容目标 2
14 文结构安排 2
第二章 系统设计方案 3
21 需求分析 3
22 系统框架 3
23 设计思路 4
第三章 硬件系统设计 5
31 STM32系统设计 5
311 控芯片 5
312 系统设计 6
32 电源系统 6
321 降压芯片选型 6
322 电源系统原理图 7
33 MPU6050六轴陀螺仪传感器模块 7
331 MPU6050概述 7
332 MPU6050系统原理图 8
34 TB6612FNG电机驱动模块 9
341 TB6612FNG模块概述 9
342 TB6612FNG原理图 9
35 BT04A蓝牙串口模块 10
351 BT04A蓝牙串口模块概述 10
352 BT04A蓝牙串口模块原理图 10
36 OLED显示屏模块 11
361 OLED显示屏概述 11
362 OLED显示屏接线图 11
37 电机编码器 12
38 章结 12
第四章 软件系统开发 13
41 总体方案 13
42 MPU6050读取姿态信息 13
421 读取原始数 13
422 读取DMP转换欧拉角 14
423 IIC通讯方式 14
43 霍尔编码器测速 16
431 程序框图 16
432 编码器软件四倍频 16
44 蓝牙线传输 17
441 STM32串口通讯 17
45 遥控终端开发 18
第五章 两轮衡车关键算法 20
51 MPU6050姿态解算 20
511 DMP输出四元数 20
512 欧拉角转换 20
52 两轮衡车PID控制算法 20
521 PID算法概述应 20
522 基PD控制直立环 21
523 基PI控制速度环 21
524 基PD控制转环 22
第六章 系统测试 23
61 测试目测试环境 23
62 电源系统检测 23
63 STM32系统测试 24
64 蓝牙线通讯测试 25
65 陀螺仪测试 25
66 两轮衡车PID调试 27
661 直立环PD算法调试 27
662 速度环PI算法调试 28
663 转环PD算法调试 28
67 两轮衡车遥控测试 30
68 章结 30
结 31
参考文献 32
致谢 33
第章 绪
11 文研究背景意义
类生存发展已步入21世纪高科技时代日常生产生活中离开数字技术计算机技术机械设计技术等高科技技术两轮衡电动车两轮轴机械结构通带精密电子陀螺仪(SolidState Gyroscopes)电子衡系统实现动衡直立行走功外形巧行动灵活环保方便特点市场广认发展目前种需移动运输操作传统机械设备基驱四轮式者履带式形式存传统机械移动方式已满足生产生活工智求市场急需种更方便灵活机械辅助移动设备直立式两轮衡电动车出现满足需求
两轮衡车系统运行时衡控制规划行驶操控等种功时运行关键点实现衡时环境中直立行驶实现控制操作两轮衡车概念稳定动态性系统非线性迅速成种控制理研究台具较科研意义
12 国外两轮衡车研究历史现状
年全球国两轮衡机器研究越越火爆迅速发展国纷纷研发实验机两轮衡车衡控制方案陆续研发设计出呈现出控制系统样性两轮衡车稳定行驶衡系统改造快速方便应环境里工业生产需承载运输日常生活代步等国说巨机遇外国公司市场生产研发相应商业产品投放市场
2002年台名Segway HT两轮直立式衡载设备问世该设备美国Segway公司研制该设备行驶灵活体型巧特点员密集机场中机场安保警务员站立该两轮衡车群中灵活快速移动行驶居高行驶方式满足观察机场处需求够时发现处理疑情况2006年家位德国Transport公司针室外现场摄影工作者研发两轮摄车该设备广泛应电视节目录制甚电影特殊情节拍摄中2007年日丰田汽车公司研发台丰田机动机器(Mobility Robot)台15千克机器高行驶速度高达时20千米
哈尔滨工程学研究员两块C8051单片机机交互位机组成控制系统通传感器测量计算出车体姿态信息脉宽度调制(PWM)控制两台直流电机外加机交互线传输等技术制造台两轮直立衡机器
中国科技学研发款两轮衡代步电动车采左右轮轴机械结构通建立系统数学模型建立控制算法计算输出脉宽度调制(PWM)控制控制两伺服电机转矩电动车行驶程中保持直立衡
13 文研究容目标
文研究容：
（1）两轮衡车系统设计方案
（2）两轮衡车硬件选型电路设计
（3）两轮衡车控制算法
（4）控制系统调试测试
根设计功求实现两轮衡车衡情况完成遥控操作两轮衡车前进退转弯等行驶功
14 文结构安排
文六章节构成：
第章绪研究文背景意义分析国外两轮衡车历史现状提出文研究意义目
第二章系统设计方案两轮衡车需求进行分析设计系统框架提出设计思路
第三章硬件系统设计结合两轮衡车功需求需硬件进行选型分析设计原理图
第四章软件系统开发提出软件系统总体方案分析模块软件系统开发流程
第五章两轮衡车关键算法陀螺仪输出姿态数进行解算PID算法电机进行控制两轮衡车保持直立衡
第六章系统测试两轮衡车系统进行测试PID算法调试验证两轮衡车控制算法行性稳定性
第二章 系统设计方案
21 需求分析
系统求两轮衡车衡状态通蓝牙线传输技术遥控实现两轮衡车移动行驶功系统STM32F103C8T6作衡车控芯片实现功求：
（1）获取车衡姿态进行姿态解算
（2）PID算法控制两轮衡车静止行驶状态保持衡
（3）够通蓝牙线传输技术手机APP蓝牙遥控操车行驶
（4）OLED显示屏显示电池电容量等车状态信息
22 系统框架
系统核心控制器选STM32F103C8T6单片机LM2596AMS1117芯片组成电源系统提供硬件稳定安全工作电压环境MPU6050六轴传感器测量出车车体行驶姿态电机编码器车移动速度OLED显示屏显示车体姿态信息电池电压状况TB6612电机驱动模块负责驱动电流直流电机运转BT04A蓝牙模块实现两轮衡车遥控器间线通讯功系统结构图21示
图21 系统结构图
23 设计思路
根模块传感器STM32间连接通讯方式系统设计思路图图22示
图22 硬件设计思路
第三章 硬件系统设计
31 STM32系统设计
311 控芯片
两轮衡车系统工作时需反复快速读取陀螺仪电机编码器等传感器数马算法处理输出控制量反馈控制直流电机控制系统控芯片处理速度运行存求较高
控制系统控芯片选取意法半导体公司（ST）生产STM32F103C8T6型号单片机核CortexM3ARM公司设计参数表31示
表31 STM32C8T6参数表
工作电压
2V~36V
总线宽度
32位
速度
72 MHz
FLASH容量
64KB
RAM容量
20K
72MHz速度32位总线宽度满足两轮衡车控制系统处理速度求64 KBFLASH存储器满足程序存储
图31STM32F10x系统构架图该图提供STM32F103C8T6单片机部结构关系外设资源目然STM32运开发提供帮助
图31 STM32F10x系列系统构架图
312 系统设计
单片机系统定义满足单片机正常工作系统STM32单片机系统单片机外电源复位时钟三种电路调试接口boot启动选项组图32系统系统原理图
单片机选取LQFP48封装单片机电源降压电源电源系统详细设计容面章节中分析复位电路设计键触发复位RST引脚通键S2键引脚RST接RST引脚高电拉低低电触发单片机复位功键松开系统正常工作复位电路中1uf电容10kΩ电阻提供复位时间外置8MHz晶振时钟加入20uf负载电容该电容作保证晶振正常工作时钟电路提供精准时间SWD载模式需引脚较少节约偏资源设置boot模式设计较简单需boot0boot1引脚引出跳线帽方式设置
图32 STM32F103C8T6系统原理图
32 电源系统
321 降压芯片选型
直流电机驱动电压电流较系统选取12V锂电池供电12V电压环境单片机传感器模块说电压太高直接供电会烧毁击穿模块需电压进行降压处理
选LM2596降压芯片输入电压40V驱动电流达3A热关断限流保护力负载调节力强12V锂电池电压通LM2596组成降压电路输出稳定5V电压
选AMS111733正低压降稳压器LM2596输出5V电压进行降压输出33V电压
322 电源系统原理图
根设备需电源求根LM2596AMS111733数手册设计出设备电源系统图33
图33 电源系统原理图
衡车输入电压12VLM2596降压输出电压5V负载3A5V电压AMS111733降压输出33V电压电源系统加入电流开关S3开S3系统电源开启降压模块开始工作提供稳定电压关闭S3整电源系统关闭
33 MPU6050六轴陀螺仪传感器模块
331 MPU6050概述
MPU6050六轴陀螺仪传感器含MEMS 陀螺仪MEMS加速度计三轴嵌DMP（Digital Motion Processor）数字运动处理器通讯方式IIC协议SPI协
VCC引脚接入33V5V电压GND引脚接SDASCL两组引脚IIC通讯引脚信号线MPU6050六轴陀螺仪传感器需外接传感器时XDAXCL引脚分接IIC串行数信号线时钟信号线AD0通接接电设置AD0值01AD00时MPU6050址0x68AD01时MPU6050址0x69INT引脚中断输出引脚MPU6050六轴陀螺仪传感器引脚说明表32
表32 MPU6050引脚说明
引脚名称
说明
VCC
电源输入33V5V
GND
接
SCL
IIC时钟信号线SCL
SDA
IIC数信号线SDA
AUX_DA
IIC串行数信号线外接传感器
AUX_CL
IIC串行时钟信号线外接传感器
AD0
接悬空时址0x68接VCC址0x69
INT
中断输出引脚
332 MPU6050系统原理图
翻阅MPU6050数手册设计出接线原理图图34示
图34 MPU6050原理图
MPU6050电源引脚33V电压电源入口器件旁边加入滤波电容滤交流成分输出直流更滑确保MPU6050硬件稳定性满足控制系统高精度姿态数值需
AD0引脚接AD0值0设置MPU6050设备址0x68IIC通讯引脚SDASCL分接入STM32PB9PB8引脚加入47KΩ拉电阻作保证正常高电输出起保护芯片作
34 TB6612FNG电机驱动模块
341 TB6612FNG模块概述
两轮衡车带两直流电机12V锂电池然电机转动法直接控制转速控制车体保持衡需块专门负责驱动控制电机芯片
东芝半导体公司生产TB6612FNG芯片款直流电机驱动器件该芯片带电流MOSFETH桥结构够输出双通道电路刚驱动两轮衡车两电机该芯片低热耗需加散热片100KHzPWM信号输入频率满足两轮衡车控制系统需求TB6612FNG参数表33示：
表33 TB6612FNG参数表
工作电压
5V
输入电压
15V
输出电流
32A
功模式
正反转短路刹车停机
图35 TB6612FNG芯片图
342 TB6612FNG原理图
TB6612FNG电机连接原理图图36
图36 TB6612FNG原理图
TB6612FNG芯片PWMA引脚PWMB引脚分接STM32单片机PA11引脚PA8引脚单片机输出PWM信号控制两电机TB6612FNG输出引脚O1O2O3O4引脚分两组引出方便点机接线AN1AN2BN1BN2分连接单片机应IO口控制电机转真值表表34示
表34 TB6612FNG控制电机运转方真值表
引脚
停止
正转
反转
AN1
0
1
0
AN2
0
0
1
35 BT04A蓝牙串口模块
351 BT04A蓝牙串口模块概述
设备BT04A蓝牙模块该模块采蓝牙V21+EDR技术兼容UART接口成低功耗低接收灵敏性高外围电路需少许元件实现蓝牙线传输功
图37 BT04A蓝牙串口通讯模块
BT04A蓝牙串口模块参数：
表35 BT04A参数表
工作电压
33V
通讯模式
UART
通讯效距离
15米左右
户根需通AT模式输入应AT指令设置蓝牙模块名字角色(Master)者角色(Slave)配码等信息
352 BT04A蓝牙串口模块原理图
BT04A蓝牙串口模块原理图图38示
图38 BT04A蓝牙串口模块原理图
BT04A模块串口通讯引脚STM32串口通讯引脚交叉相连STM32串口输出引脚连接BT04A模块串口输入引脚STM32串口输入引脚连接BT04A模块串口输出引脚BT04A模块P12引脚外接LED灯显示蓝牙模块状态
36 OLED显示屏模块
361 OLED显示屏概述
096寸OLED显示屏显示两轮衡车电时电压车体倾斜姿态电机编码器数值等信息参数表36示
表36 OLED显示屏参数表
工作电压
33V
分辨率
128×64
显示存
128×64位字节
响应时间
微秒十微秒
表37OLED显示屏模块引脚说明
表37 OLED显示屏模块引脚表
GND
电源线
SDA
数线
VCC
输入电压
RST
复位
SCL
时钟线
DC
命令数
362 OLED显示屏接线图
OLED显示屏接线图39示输入电压33VSCLSDARSTDC引脚分接STM32PC15PC14PC13PB4引脚
图39 OLED显示屏接线图
37 电机编码器
编码器轴角度方位移转换成数字脉种旋转式传感器编码器设置电机转动轴位置输出应波形电机转动速度信息
设备采AB相输出增量式霍尔编码器图310需编码器接5V工作电源电机电便输出AB相方波信号辨转测量速度
图310 增量式霍尔编码器
38 章结
章针硬件选型设计应外围电路衡车功出发设计硬件系统芯片模块进行介绍设计工作电路模块控芯片STM32连接完成硬件系统设计
第四章 软件系统开发
41 总体方案
软件系统开始时初始化函数读取电压值判断定时器否定时时间果定时时间未回定时开始果定时时间陀螺仪输出DMP数转换欧拉角显示屏显示车体状态信息判断衡启动键否键没循环检测键果键进行PID运算输出PWM 控制电机运转控制车衡检测蓝牙否连接果蓝牙已连接点亮指示灯接收指令输出应PWM控制车行驶状态果蓝牙没连接回衡启动键检测检软件系统流程图图41示
图41 软件系统流程图
42 MPU6050读取姿态信息
421 读取原始数
MPU6050原始数获取流程图42示
图42 MPU6050姿态读取流程图
根IIC协议通讯方式软件方式控制STM32两IO口模拟出IIC通讯协议然MPU6050陀螺仪进行初始化操作配置时钟陀螺仪量程加速度量程等初始化通软件模拟IIC协议读取MPU6050寄存器值加速度角加速度信息
422 读取DMP转换欧拉角
读取MPU6050置DMP姿态信息转换欧拉角流程图43示
图43读取DMP转换欧拉角流程图
MPU6050初始化设置通IIC读取FIFO寄存器中值q30格式值进行浮点数计算欧拉角转换计算出PitchRoll
423 IIC通讯方式
文中MPU6050STM32通讯方式IIC通信
IIC种数总线SDA时钟总线SCL两条串行总线组成种串行通信协议总线IIC连接设备时设置机机设置机模式时址总线需获取机址设置机模式时需配置匹配址机发出应答信号设备中MPU6050倍设置机
整IIC协议通信流程包含起始信号(S)应答信号(ACK)数传输（发送接收）停止信号(P)
STM32需读取MPU6050数时STM32先发起开始信号 (S)方式数总线SDA时钟总线SCL保持高电数总线SDA高电转换低电形成降周期06usIIC通讯结束时时钟总线SCL保持高电06us数总线SDA低电升高电形成升IIC通讯结束标志(P)IIC开始信号结束时序图44示
图44 IIC开始信号结束信号图
IIC数宽度8位字节IIC开始信号发出发送8位数时钟总线SCL处高电数总线SDA保持稳定低电时发送数0反时钟总线SCL处高电数总线SDA保持稳定高电电时发送数1
图45 IIC数发送方式
发送完8位二进制数需应答信号(ACK)信号判定否传输次数果第9时钟脉期间时钟线高电数总线SDA稳定低电效应答位(Acknowledge)果第9时钟脉前低电期间数总线SDA保持高电次应答信号效应答位(Notacknowledge)
图46 IIC应答信号时序图
43 霍尔编码器测速
431 程序框图
电机编码器测速流程图47
图47 电机编码器测速流程
系统开始首先STM32定时器TIM2定时器TIM4进行初始化设置成编码器接口模式然开2定时器编码器接口电跳变触发定时器计算读取STM32定时器计数寄存器值计算出速度
432 编码器软件四倍频
假时间T编码器输出AB相2波形图图48示M法测量A相(B相)升者降测速时候时间T样测速方式计数2次软件四倍频算法编码器输出波形进行四倍频A相B相输出完整波形波形时时测量A相B相编码器升降图48中1~4示样样时间T计数8次提高测速工作效率
图48 编码器AB相波形4倍频
44 蓝牙线传输
该设备蓝牙线传输技术基STM32USART串口传输技术连接蓝牙模块全双工传输模式直接串口数传输相连接普通串口线进行数线传输
441 STM32串口通讯
STM32串口通讯流程图图49示
图49 串口通讯流程图
45 遥控终端开发
衡车控制系统嵌蓝牙线传输技术遥控终端开发较简单方便首先STM32编程设置前进退左转右转等操作指令指令表表41示
表41 遥控操作指令表
操作
字符指令
十六进制(HEX)指令
前进
A
0x41
退
E
0x45
左转
B
0x42
右转
F
0x46
刹车
Z
0x5A
然手机载 蓝牙调试器APP开编程模式前进退左转右转4键进行编辑前进钮时发送前进指令A前进钮松开时发送刹车指令Z防陷入前进操作循环键编程图图410~图413示
图410 前进键设置 图411退键设置
图412 左转键设置 图413 右转键设置
设置四操作键蓝牙调试器APP遥控界面图414示
蓝牙调试器APP连接衡车板蓝牙编程键时蓝牙调试器APP发送相应指令衡车板通串口传输STM32STM32计算PWM控制车完成应行驶操作
图414 蓝牙调试器APP遥控界面
第五章 两轮衡车关键算法
51 MPU6050姿态解算
511 DMP输出四元数
四元数种超复数代表加入旋转角三维空间表达式：
q(q0q1q2q3) (51)
中q0实数q1~q3虚部实数
MPU6050带硬件运动处理器DMP(Digital Motion Processing)够原始角速度数转换四元数减轻控芯片STM32工作负担DMP较简单需载官方DMP驱动库文件移植STM32DMP轴加速度角速度需注意DMP输出数格式浮点数放230倍q30模式需输出数转化浮点数表达公式：
q0quat[0] q30 (52)
q1quat[1] q30 (53)
q2quat[2] q30 (54)
q3quat[3] q30 (55)
512 欧拉角转换
欧拉角确定某围绕定点转动刚体位置组三独立角参量理解X轴滚转角(roll)Y轴俯仰角(pitch)Z轴航偏角(yaw) 组成
系统应pitchroll两参数
pitch值转换式：
asin−2×q1×q3+2×q0×q2180π (56)
roll值转换式：
atan2(2×q2×q3+2×q0×q1 −2×q1×q1−2×q2×2+1)180π (57)
52 两轮衡车PID控制算法
521 PID算法概述应
PID算法常见控制算法例(Proportional)积分(Integral)微分(Differential)三种计算组成工程实践中般P必须衍生出许组合PID控制器PPIPID等
两轮衡车控制系统分三部分第两轮衡车直立控制系统第二两轮衡车速度控制系统第三两轮衡车转控制系统三系统分PID控制算法
图51 PID算法图
522 基PD控制直立环
直立控制系统PD控制器D（微分）控制加入够快速响应车体受干扰调整车体衡状态PD控制车体直立程序框图图52示
图52 PD控制直立流程图
523 基PI控制速度环
速度控制系统PI控制器P控制器具线性利实际输出值定值两者间偏差偏差例积分进行线性组合构成控制量控制目标象PI控制车体速度程序框图图53
图53 PI控制直立流程图
524 基PD控制转环
PD控制车体速度程序框图图54
图54 PD控制转流程图
第六章 系统测试
61 测试目测试环境
两轮衡车系统功实现需通功模块相互配合完成保证两轮衡车控制系统正常运转需两轮衡车模块PID算法进行调试确保系统稳定性系统瓷砖表面房间里进行测试
62 电源系统检测
测试方法：两轮衡车控制板接电池开电源系统总开关万表测量LM2596芯片2号输出引脚电源间电压测量AMS1117332号输出引脚电源间电压
测试结果：LM2596芯片2号输出输出电压498V图61示AMS1117332号输出引脚输出电压33V图62示
结：测试结果显示电源系统功正常
图61 LM2596输出电压测试图
图62 AMS111733输出电压测试图
63 STM32系统测试
测试方法：Keil uVision5软件编写测试程序控制LED灯1S时间间隔进行闪烁编译通STLink烧录器载两轮衡车控制板STM32芯片里观察LED情况测试结果表61示
表61 STM32系统测试
测试步骤
预期情况
实际情况
结
控制板电
LED灯1S时间间隔进行闪烁
LED灯1S时间间隔进行闪烁
载调试时钟IO功正常
长复位键
LED灯熄灭
LED灯熄灭
复位功正常
松开复位键
LED灯恢复闪烁
LED灯恢复闪烁
图63 系统点灯图
图64 系统熄灯图
64 蓝牙线通讯测试
蓝牙线通讯实现车控制终端重通讯方式两轮衡车线遥控程中起非常重作实现操控便捷性
测试方法：蓝牙模块通数线连接电脑电脑开串口调试器手机蓝牙调试助手连接蓝牙模块分空旷墙体房间里面根手机端蓝牙模块间距离分进行测试观察电脑串口调试器接受信息测试结果表62示
表62 蓝牙线通讯测试系统测试
空间状态
距离
预期情况
实际情况
结
开阔
障碍
5米
电脑端串口调试器接受收手机端发信息
电脑接收信息
通
10米
电脑接收信息
通
相隔
面墙体
5米
电脑接收信息
通
10米
电脑接收信息
通
测试结果：STM32串口功正常蓝牙模块功正常线通讯功正常
65 陀螺仪测试
MPU6050六轴陀螺仪提供控制系统精准姿态信息模块运行否正常影响两轮衡车衡功
测试方法：车控制系统板电MPU6050X轴旋转轴改变板倾斜角度观察OLED屏角度信息测试结果表63示
表63 MPU6050测试表
车板姿态
预期情况
实际情况
结
板正面水放置桌面
显示屏显示角度0°
显示屏显示角度359°
通
板正面前垂直放置桌面
显示屏显示角度270°
显示屏显示角度275°
通
板正面前垂直放置桌面
显示屏显示角度90°
显示屏显示角度90°
通
图65 MPU6050读取板正面水桌面姿态图
图66 MPU6050读取板正面前垂直桌面姿态图
图67 MPU6050读取板背面前垂直桌面姿态图
测试结果：MPU6050测试出姿态方信息正确桌面法保证水程度加手扶着板观察显示屏读取出数值存误差通期PID控制调试控制车衡该误差忽略计测试验证OLED屏幕功正常
66 两轮衡车PID调试
测试两轮衡车关键系统均正常接调试PID算法验证衡车衡功测试静止状态外力干扰否实现衡功
661 直立环PD算法调试
测试方法：改变kp值kd值观察车衡状态测试结果表64表65示
测试结果：kp值500kd值1时车趋衡抖动现象段时间便倒时kpkd值进行幅放缩kpkd值缩06倍时kp值300kd值1时基没抖动现象段时间倒接需加入速度环控制保持稳定性
表64 直立环kp值调试表
kp
车状态
分析
100
车倾斜瞬间电机倾斜方加速运转车倒
kp值极性错误导致车倒
100
车倾斜瞬间电机倾斜方反方加速运转车趋直立倒
kp值极性正确响应慢导致车倒
300
车倾斜瞬间响应加快趋衡倒
kp值偏导致车倒
500
车倾斜瞬间响应快出现回抖动状况
kp值基满足条件需调节kd抑制抖动
表65 直立环kd值调试表
kd
车状态
分析
05
车倾斜瞬间电机倾斜方运转车倒
kd值极性错误导致车倒
05
车倾斜瞬间电机倾斜方反方运转车倒衡趋势
kd值极性正确响应慢导致车倒
1
车趋衡
kd值基满足条件微调更完美
662 速度环PI算法调试
速度控制正反馈kpki均正数
假设速度偏差速度半时候电机转动速度达快通软件方式电机编码器4倍频10ms获取次脉频率车轮转动速度时左右两电机编码器输出数值约160根输出PWM值7200估算kp值：
kpmax 7200160290 (61)
设置ki值：
ki kp ki (62)
调试方法：根公式kp值约90ki值045kpki合适改变kp值观察车衡状态测试结果表66示
测试结果：kp值80ki值04时满足控制求够外界干扰做出反馈具定抗干扰力
表66 速度环kdki值调试表
kd
ki
车状态
分析
40
02
车速度稳定车回摆动
速度控制响应慢kdki值
60
03
速度控制响应加快车摆动距离缩短
速度控制响应偏慢kdki值偏
80
04
车摆动距离幅减
kdki值基满足条件
100
05
车开始摆动轻碰倒
kdki值抗干扰力差
663 转环PD算法调试
测试方法：设定kd0kp02逐步幅增kp值观察衡车转情况
测试结果：kd0kp16时车转性直线行驶性优秀满足控制条件测试结果表67示
表67 转环kp值调试表
kd值
车状态
分析
02
车转控制性差
kp值
08
车转控制性转直线行驶偏差
kp值偏
16
车转控制性直线行驶偏差
kp值满足控制条件
664 PID调试结
调试步骤两轮衡车实现衡功具定抗干扰性衡车直立图图68示
图68 两轮衡车衡图
67 两轮衡车遥控测试
测试方法：手机蓝牙调试器连接衡车蓝牙水瓷砖面进行遥控操作测试结果表68示
表68 两轮衡车遥控测试
遥控操作
预期情况
实际情况
结
前进
前进
前进
通
退
退
退
通
左转
左转
左转
通
右转
右转
右转
通
测试结果：两轮衡车遥控性稳定实现控制便捷性
68 章结
章通进行种测试调试检测两轮衡车模块功运转否正常确保两轮衡车功部分状态正常调节两轮衡车PID算法参数值测试两轮衡车遥控操作实现控制便捷性两轮衡车稳定性
结
文勾画出两轮衡车流程图通PID算法直立环速度环转环进行控制实现两轮衡车车衡功结合蓝牙线传输技术两轮衡车进行线遥控操作实现操控便捷性
通文研究验证两轮衡车控制系统行性稳定性
参考文献
[1]刘军例说STM32[M]北京航空学出版社2011
[2]BP神网络PID控制器参数整定中应[J] 陈书谦张丽虹 计算机仿真 2010(10)
[3]具时滞单级倒立摆系统稳定性分析[J] 曹青松黎林 煤矿机械 2009(04)
[4]单片机复位电路设计分析[J] 包国彬张建民刘嬴 光电技术应 2005(03)
[5]数字PID算法刷直流电机控制器中应[J] 谢世杰陈生潭楼天 现代电子技术 2004(02)
[6]单轴双轮衡代步车研究设计[D] 张吉昌中国海洋学 2009
[7]两轮衡机器适应控制算法研究[D] 黎田哈尔滨工业学 2007
[8]两轮衡机器控制技术研究[D] 王瑜哈尔滨工程学 2009
[9]直流刷电动机原理应[M] 机械工业出版社 张琛编著 1996
[10]基STM32两轮衡车设计实现[D] 潘二伟黑龙江学 2018
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
