基于STM32处理器的两轮自平衡小车设计

摘 要

当前，在自动控制领域的研究人员对于智能车的研究有着一些很成熟的见解。平衡车作为智能车的一个分支，我们对于平衡车的研究是十分必要的，它不仅在实际生活中给我们很大的帮助，而且在一些工业现场也可以帮助我们很好的完成一些复杂的任务。因此，本文将从两个主要的方面来对两轮自平衡小车进行研究，一方面是从硬件，一方面是程序设计。

本文的设计方案是利用传感器MPU-6050作为平衡小车的状态检测装置，然后通过卡尔曼滤波算法对小车的倾角以及角加速度进行数据融合，从而得到小车的最优状态估计值，然后再利用PID控制算法让小车保持直立平衡状态，本文采用了ARM公司的Cortex m3内核的10系列芯片STM32F103C8T6作为小车系统的主控制器，它的功能十分强大，能够进行一些数据处理和算法的实现。通过采用这种方式制作出来的小车，能够较快的响应，车体也比较稳定，能够完成一些基本的运动动作。

关键词： 两轮自平衡小车，MPU-6050姿态传感器，STM32F103C8T6

Design of Two-Wheel Self-Balancing Trolley

Based on STM32 Processor

Abstract

At present, researchers in the field of automatic control have some very mature insights into the research of smart cars. As a branch of the smart car, the balance car is very necessary for our research on the balance car. Not only does he give us great help in actual life, but also can help us complete some complex tasks well in some industrial scenes. . Therefore, I will study the two-wheeled self-balancing car from two main aspects, on the one hand from the hardware, on the other hand is the programming.

My design scheme is to use the sensor MPU-6050 as the state detection device of the balanced car, and then use the Kalman filter algorithm to perform data fusion on the inclination and angular acceleration of the car to obtain the optimal state estimate of the car, and then use PID control The algorithm keeps the car upright and balanced. I use the 10 series chip STM32F103C8T6 of ARM company's Cortex m3 core as the main controller of the car system. His function is very powerful and can perform some data processing and algorithm implementation. The car produced in this way can respond faster, and the car body is relatively stable, and it can complete some basic movements.

Key words: Two-wheeled self-balancing car, MPU-6050 attitude sensor, STM32F103C8T6.

目 录

第1章 绪 论 1

1.1 背景 1

1.2 选题的目的及意义 2

1.3 两轮自平衡小车的国内外研究现状 3

1.3.1 两轮自平衡小车国外的研究现状 3

1.3.2 两轮自平衡小车国内的研究现状 3

1.4 主要的研究内容 3

第2章 硬件模块 4

2.1 系统框架 4

2.2 主控STM32模块 5

2.3 电源稳压降压模块 7

2.4 电机驱动模块 8

2.5 直流电机 12

2.6 姿态传感器模块MPU-6050 13

第3章 控制系统设计 16

3.1 PID控制原理 16

3.1.1 模拟 PID 控制 16

3.1.2 位置式 PID 算法 17

3.1.3 增量式 PID 算法 17

3.1.4 采样周期的选择 18

3.2 电机速度闭环外环PID控制 18

3.3 M法测速 19

3.4加速度计、陀螺仪的工作原理和数据融合 19

3.4.1加速度传感器 19

3.4.2 陀螺仪 23

3.4.3 融合加速度传感器和陀螺仪的数据 24

3.4.4 互补滤波 26

第4章 软件设计 28

4.1程序框架 28

4.2 主程序 28

总结 31

参考文献 32

致谢 33

1.绪 论

1.1 背景

近年来各国的技术研究人员对于两轮自平衡小车有过很多的研究，人们对于它的理解也不断的深入，但是要想它适用于如今纷繁复杂的工业场合，还需要进一步优化它的配置，提高它的性能。如今的工业现场环境都是非常复杂的，存在着各种约束条件，例如温度湿度等。要想在这样复杂的环境中有比较好的控制效果并且能够让平衡小车快速的完成预定的任务，必须进一步优化它的性能。因此，如何快速并且准确的完成工业场合的任务并减少人力的使用，成为了人们关心的问题。一个完善的平衡小车系统能够在工业生产上节约很多的资源。学习控制基础，它是一个很好的平台，并且这种平台价格便宜，具有极大的实用意义。所以两轮自平衡小车的开发与设计受到越来越多研发人员的重视。其所涉及的核心技术有机器人技术、机械构造、平衡算法等，在某种情况下可以代表相关国家和地区的科学技术及工业水平，因此对于它的研究是十分必要的。

1.2 选题的目的及意义

两轮自平衡小车由两个电机独立驱动，我们还需要找到车身的机械零点，让小车在自然状态下的保持距直立。当然最主要的是通过程序算法，让它在运动中保持平衡，主要让它实现直立平衡，转速平衡和转向平衡。本文采用了比较优质的硬件模块，因此它可以适应比较复杂的环境，并且可以达到比较好的应用价值。与传统的轮式机器人相比而言，两轮自平衡机器人在多个领域具有很多独特的优势。 两轮平衡小车是一个非常复杂的系统，它是一种非线性的系统，并且数据的测量会受到外部干扰。它需要在存在外部干扰的情况下通过一些算法达到理想的控制效果。当然设计它的目的不仅限于此，它还可以通过加入各种模块完成不同的控制任务，比如加入超声波模块可以实现避障。加入蓝牙模块可以实现与上位机的通讯，完成上位机传输的控制任务。加入红外寻迹模块，可以按照预定的轨道循迹。当然GPS和惯性导航设备也可以安装在机器人上实现导航的功能。因此，两轮自平衡车既可以实现简单任务，也可以实现复杂任务。它受到了各个国家的广泛关注，并且对于它的研究也是十分重要，可以更好的理解控制理论基础，具有非常好的应用场景和市场前景。

1.3 两轮自平衡小车的国内外研究现状

改两轮自平衡系统，可以快速轻松地将其应用于多种环境，例如携带，运输和运输。这其中蕴含自从21世纪初，两轮自平衡机器人出现在大众视野前，由于其不稳定的动态性能和系统的非线性，在机械设计和控制理论方面逐渐成为了一种不可替代的优势平台，在相关理论具有重大的指导意义。

近年来，在美国，日本，瑞士和其他国家加快了对于两轮自平衡小车的研究。而且它的力学模型及其动力学特性已得到验证。通过修着巨大的商机，相应地，一些外国公司现在已经推出了商业产品并将其投放市场。

1.3.1 两轮自平衡小车国外的研究现状

一些瑞士科学家研究了一种两轮自平衡小车。如图1.1所示，它的速度可以达到每秒1.6米，超过了人们的步行速度。主控制器通过陀螺仪传来的数据进行运算进而保持系统的平衡与稳定。实验人员还可以通过手机蓝牙等设备对它进行远程控制，他可以在恶劣的环境下保持平衡。

图1.1 瑞士开发的平衡小车 图1.2 美国开发的平衡小车

由美国一个公司开发的两轮平衡电动汽车可以随着驾驶员的身体位置的倾斜保持平衡，并且能够在雨天和雪天中使用。其外观如图1.2所示，其工作原理主要基于称为动态稳定性的基本原理，即车辆本身的自平衡能力。赛格威公司的诞生源自IBOT，这是一种由美国大型医疗设备生产公司开发的自动平衡电动轮椅。该轮子的设计允许残疾人使用它自由上下楼梯。

我们注意到它的踏板处于电机轴的下方，具有一种自动平衡的能力，他的运动原理类似于倒立摆的系统。但是我们设计的的两轮自平衡小车，他在自然状态下是无法保持平衡的。因此，要想实现两轮自平衡小车的平衡是一个非常复杂的任务。

1.3.2 两轮自平衡小车国内的研究现状

目前我国对于两轮平衡车的研究也是投入了比较大的精力，国家也在大力支持对于两轮自平衡小车的开发利用及其在实际生产中的应用。在控制工程领域，一些学校做的时比较出色的。比如清华大学，东北大学等。这些学校通过一些国内外的学术交流，不断完善控制领域的一些不足。并且他们对于平衡小车的研究也是比较深入的。此外还有一些高校像是北京工业大学，它对于两轮平衡车的研究也是达到了比较深入的地步。

1.41.4 本文主要的研究内容

本文对两轮自平衡小车的研究主要包括以下几个方面：

（1）研究两轮自平衡小车的控制原理；

（2）了解两轮自平衡小车所用到的各个模块；

（3）对两轮自平衡小车进行组装并调试；

（4）总结设计的思路与心得，完成毕业论文。

2 硬件模块

2.1 系统框架

两轮自平衡小车采用的核心是STM32F103C8T6主芯片，电池经过降压稳压模块后提供稳定的 5v，3.3v的电压供给STM32主芯片和各电子模块运作，STM32处理器通过读取姿态传感MPU-6050数据，获取小车的倾角以及小车的角加速度通过PID控制算法输出PWM波形传给电机驱动模块 TB6612FNG 由他来控制直流减速电机的运动。同时直流电速电机的尾部配带有光电编码器，它可以通过M法测速来测得单位时间内的脉冲数传递给 STM32 主芯片，控制器通过内部的运算器测得车体的速度来达到一个闭环控制。

图2.1 结构框架

2.2 主控STM32模块

主控芯片相当于人的大脑，接收并处理各传感器（包括陀螺仪和加速度传感器的信号），完成计算和规划后，向各执行机构（比如电机）发出控制命令。STM32芯片板有以下特色： 板上含有2个LED灯可以对两个LED灯编程实现对他们的控制。板上装有2个按键，用户按键和用来对芯片复位的按键，其中用户的按键可以通过编程对它进行控制。板载USB-TTL电路和Micro-USB接口，仅需一条数据线即可与主机通信，也可用于固件升级。SWD接口保留用于通过外部调试器（例如JLink-ob等）进行下载和调试。

主控芯片选择使用 STM32F103C8T6，因为它的体积小（LQFP48，7x7mm），功耗低（3.3V工作电压），运算速度快（10.25MIPS， ARM 32-bit Cortex™-M3 CPU），拥有丰富的功能强大的外设模块。它的主要外设包括但不仅限于：两个ADC模块，7个DMA通道，7个TIMS，其中有一个高级定时器（TIMER1），可以实现电机驱动器的精准控制，三个通用定时器（TIMER2/3/6），两个看门狗定时器(TIMER4/7)，一个系统滴答定时器(TIMER8)，9个数据传输I/0口，其中包括两个SPIX接口，两个IICX，三个UARTS，一组CAN通信I/0，一组USB电平通信I/O。内部存储器资源包括： 64k的flash memory， 20kb的静态随机存取存储器。

下图2.2为STM32核心板（板载STM32F103C8T6芯片）:

图2.2 STM32核心板

STM32F103C8T6芯片必须用到的的 IO 引脚有：

I2C 通讯接口：读取 MPU-6050 数据

I2C1_SDA（PB8，PIN45）；

I2C1_SCL（PB9，PIN46）；

Timer正交解码接口：读取编码器的脉冲数据

TIM2_CH1（PA0）和TIM2_CH2（PA1）：TIM2定时器增量编码器输入；

TIM4_CH1（PB6）和TIM4_CH2（PB7）：TIM4定时器增量编码器输入；

电机 PWM 驱动接口：控制电机转速

TIM3_CH1（PA6）和TIM3_CH2（PA7）：电机驱动；

电机转向驱动接口：控制电机转向

AIN1(PB0)、AIN2(PB1)、BIN1(PA3)、BIN2(PA4)

程序调试 SWD 接口

SWDIO（PIN34）；

SWCLK（PIN37）；

串口下载 UART 接口

TXD（PIN6）；

RXD（PIN7）；

蓝牙通讯 UART 接口

TXD（PIN33）；

RXD（PIN34）；

图2.3 STM32内部结构

2.3 电源稳压降压模块

动力来源为两节 18650 动力锂电池，每节电池额定电压 3.7v（充满电压 4.1v），两节电池 串联得到的 7.4v（充满电压 8.2v）。但是，在一般的常用电路中常用的电压为 5v 和 3.3v，所以，我们并不能直接地把电池电压供电路模块，而是需要对 7.4v 进一步降压稳压，分别降压至 5v 和 3.3v，以供整个电路系统的使用。我使用一块降压稳压模块，板载 LM2940-5.0、LM1117-3.3，较好的输出电流能够满足小车的正常工作，并且还能给后续的扩展预留了电流余量。为了监测输入电压的变化情况，需要在电源电路之中增加电压检测电路，该电路通过在采样电阻的两端形成电压差，利用STM32103C8T6上的ACD检测引脚读取这个电压差并将其数字化，再乘以相应的比例系数即可得出当前电池的剩余容量。通过在控制程序中将其纳入参考，既可以实现更好的控制。电源稳压降压模块如图2.4所示，电路接线图如图2.5所示。

图2.4 电源稳压降压模块

图2.5 电路稳压模块原理图

2.4 电机驱动模块

单片机的IO引脚输出的电流一般都是很小的，可以驱动一些功率比较小的器件，像是LED灯，但是要驱动一些大功率的器件比如电机是远远不够的。因此，我们无法直接使用IO引脚输出的PWM来控制电机。为了解决这个问题，本文加入了TB6612FNG电机驱动模块，它内部是由MOSFET管搭建的双H桥电路，并且它有双通道的电流输出，因此使用一块TB6612FNG就通过同步I/0信号同时实现两个电机的配合控制。TB6612FNG驱动芯片是意法半导体生产的一款直流电动机专用驱动芯片。它的内部搭载了基于MOS管的双H的电桥，并且他能够有双通道的电流输出，因此可同时驱动两个电机。我设计的两轮自平衡小车是需要2个直流减速电机来驱动，因此使用一个TB6612FNG就足够了。通过电路设计与搭建，电机驱动模块可以设计得非常紧凑，电机驱动模块如图2.6所示，原理图如图2.7所示。

TB6612FNG 电机驱动芯片的主要特性： 它可以提供最大电压15伏的驱动，能够输出连续1.2安的输出电流，可以控制电机的正转反转刹车以及停止。

图2.6 电机驱动模块

图2.7 电机驱动模块原理图

TB6612FNG内部集成两组H桥驱动电路，所以能直接地驱动两个直流电机。TB6612FNG内部框图如下图2.8，标准接线图如图2.9，实物接线图如图2.10：

图2.8 TB6612FNG内部框图

图2.9 标准接线图

图2.10 实物接线图

其中，VM 接入电源，VCC 是内部模块的供电，接5v的电源。可以看到模块内部有三个GND引脚，只需要将一个GND接地就可，因为它内部的三个GND引脚都是导通的。同时，STBY引脚需要接入5v，这样模块才会具备运作的基本条件。根据电路图完成实物硬件的连接且检测无误之后，需要通过程序对所连接的电机进行控制。控制方法为：5个I/0口控制其中一个MOTOR，另外5个I/0口同步操控另外1一个电机。TB6612的4引脚AO1 和5引脚AO2分别接到电机的两个驱动极；然后将第16引脚PWMA、14引脚AIN1和15引脚AIN2与STM32F103C8T6相应的GPIO连接。在控制时PWMA 接到单片机的定时器引脚，利用定时器改变输出波形的高低电平占空比就可以实现电机速度的调节；第14引脚AIN1、15引脚AIN2 分别接单片机的使能控制引脚，由此来实现电机不同方向的旋转，其控制真值表2.1如图所示：

表2.1 电机正反转真值表

测试方法：VM 接电源的正极；AIN1接5v、AIN2接 GND、PWMA接5v，电机满占空比正转；AIN1接GND、AIN2接5v、PWMA接5v，电机满占空比反转。

2.5 直流电机

直流电动机主要包括电枢、场磁铁、集电环和电刷。

(1)电枢：利用多圈线圈的缠绕在磁铁上。

(2)场磁铁：能够产生永久的电磁场的磁铁或电磁铁。

(3)滑环：线圈的两端连接到两个半圆形滑环，它们随线圈一起旋转，以提供一个用于改变电流方向的换向器。

(4) 电刷：通常是由碳制成的，集电环在固定位置接触电刷以连接电源。

直流电动机的原理图如图2.11：

图2.11 电机内部结构图

减速器，一般被称为齿轮箱，通常通过与电机转轴相连接，电动机通过减速器输入轴上带有少量齿的齿轮与输出轴啮合，使其通过机械的方式附着在电机上，从而实现核心功能：减速。其广泛应用于需要精准控制以及较大扭力的情况下，下图2.12是该平衡小车使用的TTM1直流减速电机—搭配齿轮减速器的130直流电机。

图2.12 TTM1直流减速电机

2.6 姿态传感器模块MPU-6050

MPU-6050是InvenSense推出的9轴运动器件检测装置。它内部组成包含：xyz轴陀螺仪，才外其加速度检测器还附属一个额外的3向轴从接口。可以通过程序控制增加一个额外的3轴磁力控制器，这样就可以与之前的XYZ陀螺仪和加速度检测器结合实现9轴姿态检测。因此它可以非常方便地实现系统的状态计算，最大限度的减少主处理器的负担，同时减少项目开发的难度。如图2.13所示：

图2.13 MPU-6050

MPU-6050 的主要特点：

（1）通过SPI通信的方式输出配置好的3轴或9轴的旋转矩阵、四元数、欧拉角格式等综合数字信号。

（2）转角的敏感度：131 LSBs/°/sec ；全格轴角速度感测范围：±250、±500、±1000与±2000°

（3）内部集成了可控制程序模块，通过编程实现系统上存在的误差，例如，陀螺仪的零漂，陀螺仪和加速度计之间的摩擦等。

（4）具有数字运动处理引擎，能够有效减少主控制器复杂的数据融合、感测器同步化、姿势感应等的负荷内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求；

（5）可以通过内部的温度传感器计算得出传感器所处环境温度：Temperature = 36.53 + regval/340；

（6）带以数位控制的Sync_pin，同时支持视频电子影相稳定技术与定位可程序控制的interrupt，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降 中断、high-G 中断、零动作感应、触击感应、摇动感应功能

（7）.VDD 供电电压为 2.5V± 5%、3.0V± 5%、3.3V± 5%； VLOGIC 可低至 1.8V± 5%

MPU6050的预设最大电流： 50mA，陀螺仪待机检测电流约为：50uA；加速器的工作电流：0.5mA，在低耗模式下电流：40uA,自带 2048BIT的FIF0。

（8）执行I2C通信时最高速率可达500KHZ

（9）较小的芯片外形尺寸： 4x5x0.9mm

MPU-6050 的内部框图2.14如下：

图2.14 内部框图

其中，SDIO和SDOI连接到控制器的I2C通信接口上面。核心控制器通过I2C通信控制MPU6050。此外该模块还带一个I2C备用接口：SCL和SDA。通过该方式可以增加霍尔传感器、3轴传感器等方式，进而形成6轴或者9轴传感器。VLOGC是I/0端口的电压，该引脚可以最低电压为17.45V，一般情况下该引脚需要连接至稳定的VCC电源。

第3章 控制系统设计

3.1 PID控制原理

控制需要将比例（P），积分（I）和微分（D）。其原理为：将比例（P），积分（I）和微分（D）与偏差进行组合形成控制变量，将其施加到控制对象上，从而实现控制。

3.1.1 模拟PID控制

在工业生产中，模拟PID控制起到了极大的作用，如下图3.1是一个直流电机的调速结构图：

图3.1 结构框图

在上图中，将给定速度n0（t）与实际速度n（t）比较，偏差e（t）= n0（t）-n（t），将u（t）送往差分放大器放大功率之后，其所控制的电机速度就可以得到相应的改变。

PID的主要控制流程图如见下图3.2，通过反馈调节及比例（P）、积分（I）、微分（D），算法实现对被控制对象的闭环控制。

图3.2 PID结构框图

图中，r（t）是输入值，y（t） 是通过计算，给定值与实际输出值之差构成控制偏差：e（t）=r（t）-y（t），偏差信号e(t) 作为 PID控制器的输入。所以模拟 PID 控制器的控制规律为u（t）=Kp[e(t)+Tde(t)+Tfe(t)dt]。其中，Kp为系统的放大系数，Td为控制器的积分时间，也称积分系数，Tf为控制器的微分时间，也称微分系数。

这个过程所引用到的数字PID算法，在计算机程序算法中具有很主要的里程碑意义的地位。人们将模拟PID控制法作为一种算法通过计算机实现。可以通过改变PID 的参数实现不同的控制任务已达到理想的控制效果，这种算法与传统的模拟PID算法不同，在使用了单片机等微处理器之后的数字式PID更加的灵活和易于控制,在原理上数字式PID算法又可以分为位置式PID算法和增量式PID算法。

3.1.2 位置式 PID 算法

由于数字领域的时间是不连续的，所以只能通过定时采样的方式来实现数据收集。因此越小时间间隔的采样往往能实现更精确的控制，所以系统中的积分（I）和微分(D)不可以直接带入，在带入系统前需要进过离散处理，具体方法为：采样周期为T，采样数为k，通过矩形数值积分可以将离散的时间KT以一阶向后差分的算法逼近广义上的连续时间t，已期望使它实现：t=KT(k=0,1,2,3…)，在此我们令e(kt)=e。将以上的数据全部带入系统之中哪个就可以得出u=Kp[ek+ek-1+TD],其中各个参数：

Kp为 采样序号，k = 0,1,2,...；

u为第 k 次采样时刻的计算机输出值；

ek为第 k 次采样时刻输入的偏差值；

ek-1为第 k-1 次采样时刻输入的偏差值；

TD为微分系数；

由此可见当采样间隔T越小则可以获得越精确的近似值，这样离散的时间约接近连续时间的结果，由此依照PID的运行算法，就可以得出对应的结果，能实现对所有控制量的精确取值。所以称之为称为位置式PID控制算法。这种算法在偏差值上也存在一定缺陷，例如，每个输出都与上一个周期的输出状态有关，由此当迭代次数过多时，会导致ek的误差累加，甚至出现积分饱和现象，要想消除积分饱和的现象，本文可以采用积分分离法，这是工程上常见的一种处理积分饱和现象的方法。

3.1.3 增量式 PID 算法

增量式PID是数字PID的另一种分类，它是通过控制

的增量，改变输出，当系统所需控制的变量为控制增量时，可以采用增量PID。

增量PID的具体实现过程为，在k-1采样时控制器的输出值为：u(k − 1）= Kp [e(k − 1）+ e(k) + Td]）进行减法和分类，可以获得增量PID控制算法公式 ：△u = u2 − u1 = A[e(k)-e(k-1) ]+ Be(k)+C[e(k)-2e(k-1)+e(k-2)]

其中，

A=Kp；

B=KI；

C=KD；

由此过程可以清晰明了地看出，在确定Kp、KI、KD的值之后，只要使用前后三次测量的偏差值，就可以求出控制量。增量PID 控制算法与位置PID相比，占用更小的CPU内存空间。所以在性价比方面更具优势，所以被更广泛采用。

3.1.4 采样周期的选择

在PID算法之中，通常采用香农法则，其主要内容为：把时域连续信号通过周期采样，转换为数字信号，控制采样周期的环节称为采样器或者采样开关。处理信号完成之后需要把之前获得的离散数字信号恢复为原信号，这个过程称为信号的复现。

香浓采样定理所需要遵守的原则是：若想采样后的信号具有原信号完整的数据信息，则其采样频率f需要至少为原信号频率的两倍。若把离散信号送到理想滤波器的输入端，那么它的输出就是原有的连续信号，如果采样角频率小于二倍的上限频率，就会出现关键信息丢失、频谱信号出现重叠等现象，在这种情况下采样就是失败的，而且使用其他手段无法修复该失误。

3.2 电机速度闭环外环PID控制

使用增量PID算法，对带有编码器的直流减速电机进行速度闭环外环控制。闭环控制，即通过反馈得到电机的转速将其与数期望转速相比得到偏差，利用偏差形成控制信号来改变电机的输出。对于要求不高的应用，通常使用简单的开环控制。例如，如果直流有刷电动机的两根导线通电，电动机将旋转；施加的电压越高，电动机速度越高，功率也越大。但是，在许多需要精确控制的场合，这种方法是远远地不够的。它还需要依靠某个反馈设备将电动机的速度或位置的信息反馈到微控制器或其他机械设备。如果未为低速车轮的驱动电机提供相应的电压，则移动机器人将偏离已建立的路径。由于安装过程，负载平衡不完全等原因。

由以上分析可以得出，在做电机控制时，必须采用最少一环闭环控制，具体的实现方式见下图所示：在反馈系统中添加基于某一种变量的反馈回路（在电机控制时一般使用数字式编码器）。在这个闭环控制中，I/0口提供PWM波给功率放大电路，功率放大电路据此驱动直流130电机旋转，而编码器则通过光栅检测电机实际转速，并通过与主控通信实现实时数据回传。根据实际速度与给定速度之间的差异（即“偏差”），在此过程中，微控制器会根据回传的速度值采用对应的算法（如PID算法）等，调节其控制的功率，使电机能依照设定的速度进行工作。

3.3 M法测速

M法测速的原理其实是很简单的，就是数固定时间段内的脉冲数，再用脉冲数除于时间便可以得到速度。脉冲数是速度的一个表征量，用固定时间段内累计的脉冲数除于该固定时间，就可以求得该固定时间段的平均速度。固定时间段的时间越短，所求得的速度就越接近瞬时速度。

我们的电机上装有霍尔编码器，编码器的码盘跟电机转子同轴安装，电机转动时，码盘也随轴同转，并且转一圈会输出一定数量的脉冲。M 法是数固定时间内产生的脉冲数，在实际工程中比较好实现。而 T 法是数两个脉冲间隔的时间，T 法测速在实际工程中比较少用。

3.4加速度计、陀螺仪的工作原理和数据融合

3.4.1加速度传感器

MPU-6050检测器的内部原理如下图3.3所示，其包括陀螺仪和加速度检测器，二者具有独立的运行及采集计算方式。

图3.3 盒子模型

加速度传感器类似于一个盒子模型，如图3.4——一个小球在一个方盒子中。想象一下，如果把盒子模型放在真空环境中会怎么样？在真空中，一切东西都处于无重力状态（失重），这时，小球也不例外，将悬浮在盒子的中间。在上面的图中,给每个轴分配了一对墙，假如每面墙都能感测到压力。如果我们突然把盒子向左移动，那么球会撞上 X− 墙。然后我们检测球撞击墙面产生的压力。

图3.4 加速度模型

可以很清楚的得到加速度检测器得到的力的方向与盒子的实际运动方向是不同的，即惯性力与运动方向相反，这样我们可以通过检测盒子右面受力的大小来间接地判断盒子加速的的大小。在实际应用中，这个模型可以用来测量所有由加速度引起的力的变化，进而判断加速度大小，当然也不仅仅可以用来判断加速度，在一切引力引起的变化都可以被检测到。例如我们把这个盒子置于地面上，这时圆球不再是悬浮的， 而是会落在Z-墙面上并对其施加一个力，见下图3.5：

图3.5 重力作用下加速度模型

在这种情景之下，虽然外壳没有移动，但其仍然可以在Z轴检测到力的作用。在理论上，是球受到了重力的作用，但他只是一种检测器只会判断力的效果。例如，当球在左侧到电磁力的作用时，它就会同时地撞向两面墙，这时圆球受到的是多种力的合力。以此为基础，当我们把球当做是静止的，将盒子旋转 40 度，盒子模型依然是在同一环境下，此时球会与X-面和Y-面同时接触，见下图3.6：

图3.6 旋转作用下加速度模型

其中，0.71g 这个值是不是随意写上去的，它们实际上是 √(1/2) 的近似值。这是根据勾股定理计算出来的。通过以上的分析，本文可以更好的理解加速度传感器是怎么和外部力相互作用的。坐标系如图3.7所示：

图3.7 加速度计坐标系

其中，

Rx、Ry、Rz 是向量 R 分别在 X、Y、Z 轴上的垂直投影，

Rxz 是向量 R 在 XZ平面上的投影；

Ryz 是向量 R 在 YZ平面上的投影；

Axz 是 Rz 和 Rxz 之间的夹角；

Ayz 是 Ry 和 Ryz 之间的夹角；

根据勾股定理，求得：

tan(Axz) = Rx / Rz

tan(Ayz) = Ry / Rz

那么，通过反正切函数 atan() 可得：

Axz = atan(Rx/Rz)

Ayz = atan(Ry/Rz)

我们感兴趣的角度是 Axz、Ayz。

转到新的模型中。在新模型之中XYZ三维轴互相垂直且分别都有与其垂直的墙面。方向量R 是力变化器所感知到的矢量 （它可能是重力或之前提到的电磁力）。而Rx，Ry，Rz 是矢量 R 在三轴对应墙面上的轴平行投影。

根据三维空间勾股定理，可以得到下列关系：

R=RX+RY+RZ （3.1）

回到 MPU-6050 传感器中，当已经分别读取到 3 个轴的加速度数据，但是读到的加速度值并非公认的重力加速度G，因此需要再做一次数值转换，在此我们需要加入灵敏度这一理念，其单位为LSB/g，例如加速度传感器的灵敏度= 12459LSB/g，这个数值往往在传感器的数据手册之中可以找到。

依照上面提到的理论，我们使用下列公式计算： Rx = Rx/Sensitivity 比如：当设置 MPU-6050 加速度传感器的灵敏度为 12459LSB/g，读取 MPU-6050 的 X、Z、Y轴加速度数据为

1122、16674 时，则可以计算出：

Rx = 1122LSB/16384LSB/g = 0.068g

Rz = 16674LSB/16384LSB/g = 1.018g

Axz = atan(0.068g/1.018g) = 0.589rad

这时求出来的角度单位为弧度（rad），和我们日常习惯用的单位角度（°）之间有条转换公式：

Angle = Radian/π ∗ 180 那么,Axz = 0.589rad/π ∗ 180 = 33.7°

3.4.2 陀螺仪

陀螺仪的主要检测其旋转的角度值，具体性能依赖于所搭载的轴数。例如，一个 3 轴陀螺仪检测绕 X 、Y和Z 轴的旋转。解释其具体原理之前，我们需要增加一些变量。

首先，本文定义：

Rxz – 惯性力矢量 R 在 XZ平面上的投影

Ryz – 惯性力矢量 R 在 YZ平面的上投影

在由 Rxz 和 Rz 组成的直角三角形中，运用勾股定理可得：

Rxz2 = Rx2 + Rz2 （3.2）

同样：

Ryz2 = Ry2 + Rz2 （3.3）

同时注意：

R2 = Rxz2 + Ry2 （3.4）

这条公式可以公式3.1和上面的公式推导出来，也可由 R 和 Ryz 所组成的直角三角形推导出来

同样：

R2 = Ryz2 + Rx2 （3.5）

在 MPU-6050 传感器中，就像读取加速度数据一样，你会得到一个经过内置 ADC 转换后得到的数字量，单位为 LSB。参考对加速度数据的处理，我们同样得到：

RateAxz = (GyroX − ZeroRate)/Sensitivity （3.6）

RateAyz = (GyroY − ZeroRate)/Sensitivity （3.7）

其中，

GyroX，GyroY - 这两个值是陀螺仪数据，它们分别代表矢量 R 的投影在 XZ 和 YZ平面内里的转角，也可等价的说，旋转可分解为单独绕 Y 和 X 轴的运动。

ZeroRate – 陀螺仪零偏值，换句话说，它是陀螺仪静止不动时的输出值。理论上，陀螺仪静止不动时，应该输出为 0 的值，但是受制造工艺、外部因素的影响，陀螺仪静止不动时输出一个比较小的随机的不为 0 的值，我们称之为零偏值。

Sensitivity – 陀螺仪的灵敏度，单位 LSB/(deg/s)，它的意思就是如果旋转速度增加 1°/s，陀螺仪的输出就会增加多少 LSB。灵敏度在规格书中可以查到的。

举个例子，假设我们读取到 MPU-6050 传感器的 X 轴数据：

GyroX = 12345

若此时设置陀螺仪的灵敏度为 16.4LSB/deg/s，并认为陀螺仪的零偏值为 0，代入公式 3 ，得到： RateAxz = (12345 − 0)/16.4LSB/deg/s = 751deg/s 换句话说，传感器绕 X 轴（或者说 YZ平面内）以 751°/s 的速度旋转。请注意，因为陀螺仪输出值有正负之分，负号表示该传感器朝着反方向去旋转。一份好的传感器规格书会让我们知道那个方向是正的，否则就要自己测试出哪个旋转方向会使得输出值增加。

3.4.3 融合加速度传感器和陀螺仪的数据

惯性测量元件它包含有各种各样的传感器，例如角度传感器、加速度计传感器、陀螺仪、罗盘等。每种惯性测量元件的性能都不尽相同，对比分析表格如下表3.1所示:

表3.1 惯性测量元件性能对比分析表

测斜仪测量竖直方向上的姿态角频率变化，实际应用是其输出频率低于100KHZ，不具有实时性，且非数字信号极易受到干扰，对相位频率特性的分析表明，测斜仪的相位滞后；加速度传感器主要功能是测量的与加速度或者合外力的变化，例如由变速转动、引力和线性速度变化，在程序中对所测得的值进行一次计算就可以的到速度，多次积分就可以测得相应方向位置变化，但是依照前文介绍，多次积分后会使得系统漂移误差无限累计最终严重影响结果，导致所获得的位置和速度信息与实际产生较大差距。加速度传感器可以通过三角操作获得倾斜值，但是输出信号容易被噪声污染。当加速度传感器垂直于俯仰角安装时，可以用作倾角仪，通过反正切功能的操作进行360°全范围测量，而不会出现线性化误差。陀螺仪测量角速度时，会由于天气状况，像是温度的影响，造成陀螺仪发生零漂。并且随着时间的积累，陀螺仪的误差会变得越来越大。罗盘则通过检测恒定的地磁来计算姿态角，虽然会受到内部电磁及机械误差影响，但由于地磁的稳定所的到的值在某种精度下往往是绝对准确地。由于室内存在电源线和钢结构，会导致磁场失真，因此指南针通常不适合室内工作。在上面的加速度传感器部分，似乎只要读取加速度数据，就可以通过反正切函数arctan找到所需的角度，然后对信号进行微分以获得角速度。然而，在实际应用环境中，通过传感器获得的测量值往往无法准确地反映机器人的角度变化，陀螺仪可以用来检测搭载机器人的角速度变化，想要获得其具体倾斜角，我们可以对测量出来的角速度进行积分，从而得到我们需要的倾角数据，因此，陀螺仪获得的数据量可用于汽车控制所需的角速度和角速度。但是实际会存在着这样的一种问题，如果我们得到的角速度数数据中还有一些干扰信号，通过积分以后，这些干扰信号的作用会被放大，因此误差也随着时间变大，这样可能导致测量的数据不够准确。要想能够消除这种累积误差，一种简单并且易行的方法是使用数据融合算法。数据融合算法很多，目前有两种流行的算法：互补滤波和卡尔曼滤波。

3.4.43.4.4 互补滤波

2007年，MIT 大神 Shane Colton 发表了经典论文《The Balance Filter》，里面提出了一种对类似MPU6050这种综合传感器进行数据融合的有效方法——互补滤波。互补滤波包括低通滤波（滤除或衰减短期加速度传感器波动），以及高通滤波（消除漂移对陀螺仪的影响）。

在加速度传感器部分，我们通过反正切函数求出了角度，这里用 Acc 表示；在陀螺仪部分，我们得到了角速度，这里用 Gyro表示。 互补滤波的核心公式： Angle = 0.98 ∗ (Angle + Gyro ∗ dt) + 0.02 ∗ Acc 其中，Angle 为经过互补滤波后得到的角度 Gyro 为陀螺仪部分得到的角速度 Acc 为加速度传感器部分通过反正切函数 atan() 再转换单位后的角度

dt 为滤波器的运行周期 0.98 和 0.02 为加权系数 α 和 (1 − α)

使用 c 语言把互补滤波封装成函数：

// a = tau / (tau + dt)

// acc = 加速度传感器数据

// gyro = 陀螺仪数据

// dt = 运行周期

float angle;

float a;

float ComplementaryFliter(float acc, float gyro, float dt)

{

a = 0.98;

angle = a * (angle + gyro * dt) + (1 - a) * (acc);

return angle;

}

在互补滤波公式中，像陀螺仪角速度、加速度角度这些数据可以直接从陀螺仪、加速度传感器中读取得到。可是，加权系数是怎么得来的呢？ 在《The Balance Filter》中提到关于 加权系数的求解公式。在这里，先设滤波器的加权系数为 α，时间常数为τ，运行周期为 dt，那么公式为： α = τ/(τ + dt) 运行周期 dt 通常都是我们设定的。比如在程序里，我们打算让互补滤波器在 1s(= 1000ms) 中运行 200 次，那么 dt = 1000ms/200hz = 5ms。

时间常数 τ 的取值是由我们根据系统的实际滤波需求调整的，每个不同的系统的 τ 值都不一定相同。时间常数 τ 是“信任”陀螺仪和“信任”加速度传感器的边界值。若时间常数 τ 取值越大，则更加“信任”陀螺仪积分，但跟随加速度 传感器的速度会变慢。若时间常数 τ 取值越小，则更加“信任”加速度传感器，但同时引入加速度传感器中更多的噪声。通常，互补滤波器是为了得到更“纯”的融合角度，必须要“信任”陀螺仪积分多些，以削弱加速度传感器中噪声的影响。比如，互补滤波器放在 10ms(= 0.01s) 周期中循环运行，时间常数 τ = 0.49，那么，此时加权系数为： α = τ/(τ + dt) = 0.49/(0.49 + 0.01) = 0.98 函数的入口参数是加速度传感器数据、陀螺仪数据和运行周期。经过计算后，函数返回融合角度。为了更进一步了解加权系数的作用，我们留意两个极值情况： 当 a = 1 时，那么滤波器变成 angle = 1 ∗ (Angle + Gyro ∗ dt) + (1 − 1) ∗ Acc = Angle + Gyro ∗ dt，失去了加速度传感器的修正效果，变成了一个纯粹的陀螺仪积分器，此时互补滤波器完全信任陀螺仪。当 a = 0 时，那么滤波器变成 angle = 0 ∗ (Angle + Gyro ∗ dt) + (1 − 0) ∗ Acc = Acc，此时陀螺仪完全失去了它的作用，融合角度直接等于加速度传感器的数值，此时互补滤波器完全信任加速度传感器。由此可见，加权系数 α 值的大小决定了互补滤波器是信任陀螺仪多一些还是信任加速度传感器多一些，也直接决定了互补滤波器的效果。当然，互补滤波器也存在缺陷： 初始化时不能及时跟随实际角度。这种情况，跟融合角度变量没有正确初始化和互补滤波器的时间常数取值有关。时间常数约大，陀螺仪积分比重越大，跟随加速度传感器的速度约慢，这意味着初始化时融合角度可能是非常的不精准的。陀螺仪的零漂问题。理论上，陀螺仪在静止的时候，读出来的数值应该为 0，但实际上往往不为 0，这个值被称为零偏值。陀螺仪每次上电后的零偏值是随机的。互补滤波器并不能有效地过滤掉陀螺仪的零漂。

第4章 软件设计

4.1 程序框架

编译器采用的时KEIL编译器。程序方面主要包括一些初始化函数，功能检测函数，直立环函数，速度环函数，转向环函数，PWM输出函数等。初始化函数是放在主函数中运行的，因为它只需要执行一次即可，而一些功能函数和平衡控制函数都是放在一个定时中断程序中的，因为他们需要不停地去执行。

图4.1 程序框架

4.2 主程序

主程序主要包括的是一些初始化工作以及一些处理中断的功能。因为涉及到了很多的固件库函数。因此需要在预编译时将这些文件全部包含进来，包括STM32的库函数，串口的头文件，定时器的头文件，控制函数头文件，系统调试头文件，通信头文件，电机头文件，ADC头文件等等

#include "stm32f10x.h"

#include "usart.h"

#include "SysTick.h"

#include "control.h"

#include "debug.h"

#include "communicate.h"

#include "dataflash.h"

#include "common.h"

#include "motor.h"

#include "display.h"

#include "bsp.h"

#include "ADC.h"

#include "ultrasonic.h"

#include "infrare.h"

#include "manage.h"

int main(void)

{

BspInit(); //初始化BSP

PIDInit(); //初始化PID

CarUpstandInit(); //初始化系统参数

SysTick_Init(); //初始化定时器

if(IsInfrareOK())

g_iGravity_Offset = 1; //若果检测到悬挂红外模块，则更改偏移值。

ShowHomePageInit();

while (1)

{

SecTask();

if(SoftTimer[1] == 0)

{// 每隔50ms 执行一次

SoftTimer[1] = 20;

ResponseIMU();

DebugService();

Parse(Uart3Buffer);

}

if(SoftTimer[2] == 0)

{

SoftTimer[2] = 20;

ShowHomePage();

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();

}

}

}

}

5.总结

本文介绍了一种由ARM公司的系列产品STM32F103C8T6作为主控芯片，通过利用互补波算法和PID算法的两轮自平衡小车系统。通过后续的实验检测结果表明，本论文设计的平衡小车系统能够在普通的地面上实现自平衡，而且当外部加入一定干扰的时候，小车仍然能够保持平衡状态。通过更改工作模式，小车可以实现转速平衡与转向平衡，控制效果比较理想。测试的结果表明，使用互补滤波算法，能够比较有效地消除陀螺仪漂移和车体摆动等对加速度计的影响，使之精确的计算出小车的姿态倾角，结合数字PID控制算法可效地实现小车的自平衡；采用高性能微处理器，使得互补滤波算法及PID控制算法能够在较短的时间内快速地完成响应，平衡小车的状态信息采集实时性好，提高了自平衡小车稳定性和抗干扰能力。

6.参考文献

［1］ 肖曦,唐艺伟,张猛. 两轮自平衡车CMG永磁电机控制系统[J]. 日用电器,2020,2(1):14-18.

［2］ 黄健,吕林涛. 两轮自平衡车控制系统设计[J].传感器与微系统,2020,1(2):27-30.

［3］ 王文,邓三鹏,张香玲,祁宇明. 基于姿态检测的两轮自平衡车控制方法研究[J]. 装备制造技术,2019,2(7):43-46.

［4］ 林华,徐宇宝,唐家成,崔昆峰,张晓东,曹昌勇. 两轮自平衡车姿态传感器信息融合研究[J]. 佳木斯大学学报(自然科学版),2019,3(7):41-44.

［5］ 杨皓明,赵唯. 基于串级PID控制的两轮自平衡车控制系统设计[J]. 电脑知识与技术,2019,8(2):22-24.

［6］ 吴振磊,叶金汉.一种两轮自平衡车的控制模型分析与实现[J]. 轻工科技,2018,1(2):33-35.

［7］ 陆应华. 电子系统设计教程( 第二版)[M]. 北京：国防工业出版社,2015.

［8］ 陈雯柏. 智能机器人原理与实践[M]. 北京：清华大学出版社,2016.

［9］ 李群芳. 单片机微型计算机与接口技术（第三版）[M]. 北京：电子工业出版社,2010.

［10］ 周阳. 机电一体化技术在机器人领域中的应用探讨[J]. 山东：山东工业技术, 2019,3(1):11-15.

［11］ Xiao Xi, Tang Yiwei, Zhang Meng. Two-wheel self-balancing car CMG permanent magnet motor control system [J]. Household appliances, 2020,1(2):66-72.

［12］ Huang Jian, Lü Lintao. Design of two-wheel self-balancing vehicle control system [J]. Sensors and Microsystems, 2020,1(2):46-50.

［13］ Wang Wen, Deng Sanpeng, Zhang Xiangling, Qi Yuming. Research on control method of two-wheel self-balancing vehicle based on attitude detection [J]. Equipment Manufacturing Technology, 2019,2(2):88-90.

［14］ Lin Hua, Xu Yubao, Tang Jiacheng, Cui Kunfeng, Zhang Xiaodong, Cao Changyong. Research on information fusion of attitude sensor of two-wheel self-balancing car [J]. Journal of Jiamusi University (Natural Science Edition), 2019,8(2):120-128.

［15］ Yang Haoming, Zhao Wei. Design of two-wheel self-balancing vehicle control system based on cascade PID control [J]. Computer Knowledge and Technology, 2019,3(2):22-30.

7.致谢

快乐的时光总是很短暂的，从阳春三月开始，不知不觉已经到了毕业季六月份，大学生涯转眼就要结束了，在这半年中有研究毕业设计的快乐，也有遇到解不开的难题时候的忧愁，有完成一项任务而得到的充实感，也有面对即将毕业而产生的迷茫。在大学四年里，可以说是我学习到做人知识最多的四年，相信这些平凡且不耀眼的经历会为本人将来步入社会提前打下坚实的基础。

在毕业设计即将要完成的时候，本人首先要感谢毕业设计的导师穆煜老师。在刚认识穆老师的时候，老师总是脸上挂着笑容，给人一种平易近人的感觉，在后来熟悉了之后，发现果然是这样，但是在大家不认真做毕业设计的时候，穆老师也会很严肃的对大家进行批评。在这里要感谢穆老师一个学期对大家的教诲与帮助，从开题到中期答辩，再到最后的末期答辩，因为本人之前选的课题做起来有些问题，老师于是便耐心的帮助重新选择了一个课题，之后在中期答辩之前，老师帮助大家一遍又一遍的修改总体方案，在末期之前，老师又很耐心的帮大家找出论文中存在的问题，老师的工作态度深深的影响了我，在今后的工作中一定向老师学习，做一个有责任心的人。

接下来要感谢所有为本设计提供过帮助的同学，在本人迷茫的时候是你们鼓励我不要放弃，在本人遇到难题的时候，是你们帮助本人共同查阅资料，一遍遍的检查出错的地方，在这里本人要向你们表示深深的感谢。还要感谢太原工业学院，为大家提供了这么好的学习环境。

最后，感谢各位老师可以在百忙之中抽空来审阅本论文，感谢各位老师，老师们辛苦了!

《发展速度和平衡能力》自评稿

沉淀溶解平衡教学设计

《二力平衡》教学设计

二力平衡教学设计

创意小车我制作 教学活动设计