（最终版）(1) - 图文

基于STC12C5A60S2的双轮平衡车控制系统设计

【摘 要】 双轮自平衡小车是一个高度不稳定两轮机器人，是一种多变量、非

线性、绝对不稳定的系统，且因其运动环境复杂、运动学方程中的非完整约束，所以其控制任务也具有复杂性，需要在完成平衡控制的同时又要实现直立行走，本文主要介绍了以STC12C5A60S2单片机作为核心控制器的双轮平衡车控制系统设计方案。系统采用MPU6050六轴传感器，采集角度和角速度，应用卡尔曼融合滤波方式及PID控制算法，控制直流电机实现自平衡小车的平衡和直立行走。经过系统测试，基本达到设计要求。

【关键词】 双轮平衡车 STC12C5A60S2 控制系统

Based on STC12C5A60S2 of the Two-wheeled Balancing

Vehicle Control System Design

【Abstract】: Two-wheeled self-balancing cars is a highly unstable

two-wheeled robot, is a kind of multivariable, nonlinear and absolutely unstable systems, and because of its complex movement environment, the kinematics equations of the nonholonomic constraints, also is complex, so its control tasks need to complete balance control to achieve walking upright again at the same time, this article mainly introduced the STC12C5A60S2 microcontroller as the core controller of the two-wheel balancing car control system design scheme. System adopts MPU6050 six axis sensor, acquisition Angle and angular velocity, the application of kalman filtering fusion method and PID control algorithm, realize the balance control dc motor car balance and walking upright. Through the system test, basically meet the design requirements.

【 Keywords】: The Two-wheeled balancing vehicle STC12C5A60S2 The

control system

1

目 录

第1章 引言 .............................................................................................................................. 3

1.1 双轮自平衡小车的研究意义 ................................................................................... 3 1.2 双轮自平衡小车的发展历程和现状 ....................................................................... 3 1.3 本课题的研究内容和关键问题 ............................................................................... 4 第2章 双轮平衡小车系统的总体概述 .................................................................................. 5

2.1 系统组成 ................................................................................................................... 5 2.2 系统各模块的主要功能 ........................................................................................... 5 2.3 系统的主要特点 ....................................................................................................... 5 第3章 双轮平衡小车系统的原理 .......................................................................................... 6

3.1 小车直立平衡物理分析与数学建模 ....................................................................... 6 3.2 角度、角速度及车速的数据采集与处理 ............................................................... 8 3.3 PID控制技术 .......................................................................................................... 13 3.4 车模速度控制 ......................................................................................................... 14 第4章 系统硬件电路设计 .................................................................................................... 17

4.1 单片机最小系统模块设计 ..................................................................................... 17 4.2 红外光电传感器GK102测速电路设计 ................................................................. 17 4.3 NRF24L01无线模块控制电路设计 ........................................................................ 18 4.4 六轴模块电路图 ..................................................................................................... 19 4.5 AD转换电路的设计 ................................................................................................ 20 4.6 电源电路模块设计 ................................................................................................. 20 4.7 直流电机的驱动电路 ............................................................................................. 21 第5章 系统软件设计 ............................................................................................................ 22

5.1 软件设计的总体思路 ............................................................................................. 22 5.2 系统程序设计主要流程图 ..................................................................................... 22 第6章 测试方案与测试结果 ................................................................................................ 26

6.1 调试参数 ................................................................................................................. 26 6.2 车模初步调试 ......................................................................................................... 26 6.3 静态参数调试 ......................................................................................................... 29 结束语 ...................................................................................................................................... 32 致 谢 ...................................................................................................................................... 33 参考文献 .................................................................................................................................. 34 附录 .......................................................................................................................................... 35

附录1：实物图 ................................................................................................................ 35 附录2：系统总原理图 .................................................................................................... 36 附录3：单片机最小系统PCB图 ................................................................................... 37 附录4：两轮车主程序 .................................................................................................... 37

2

第1章 引言

本章简要介绍了两轮自平衡小车的研究意义、起源与发展以及国内外的研究现状，并依此提出了本论文研究的主要内容。 1.1 双轮自平衡小车的研究意义

本课题旨在研制一种单轴双轮自平衡代步车。该系统是一种两轮左右平行布置的电动车，像传统的倒立摆一样，本身是一个自然不稳定体，必须施加强有力的控制手段才能使之稳定。由于它的行为与火箭飞行以及两足机器人行走有很大的相似性，因而对其进行研究具有重要的理论和实践意义。

双轮自平衡小车是一个高度不稳定两轮机器人，是一种多变量、非线性、绝对不稳定的系统，且因其运动环境复杂、运动学方程中的非完整约束，所以其控制任务也具有复杂性，需要在完成平衡控制的同时实现直立行走等任务，因其既有理论意义又有实用价值，双轮自平衡小车的研究在最近十年引起了大量机器人技术实验室的广泛关注[1]。双轮自平衡小车作为倒置系统的一种形式，是动力学理论和自动控制理论与技术相结合的研究项目，为科学理论的发展起到了指导作用，且由于结构简单、运动灵活以及适合在更小的空间里工作，有着一定的应用前景。 1.2 双轮自平衡小车的发展历程和现状

双轮自平衡小车的想法来自于倒立摆模型，倒立摆只能在平面内运动，如果摆倒向左边，那么摆的底部就必须也向左面移动，以保持摆的竖直平衡，右边情况与左边类似。两轮自平衡小车的情况与倒立摆相似，也需要类似的运动来保持摆车架的竖直平衡。

系统以姿态传感器(陀螺仪、加速度计)来监测车身所处的俯仰状态和状态变化率，通过高速中央处理器计算出适当数据和指令后，驱动电动机产生前进或后退的加速度来达到车体前后平衡的效果。车体的转弯通过车把的左右倾斜角度来控制，不同的倾斜角度对应电机不同的速度差。该系统灵活性强且结构简单、操作容易，可作为大众在多种场合的代步工具。对于姿态检测系统而言，单独使用陀螺仪或者加速度计，都不能提供有效而可靠的信息来保证车体的平衡。本文采用一种卡尔曼滤波方法来融合陀螺仪和加速度计的输出信号，补偿陀螺仪的漂移误差和加速度计的动态误差，得到一个更优的倾角近似值。

控制技术是运动控制的核心，在实际生产实践中应用最普遍的是各种以PID为代表的基本控制技术。按照偏差的比例、积分和微分进行控制的调节器，简称为PID调节器，是连续系统中技术成熟且应用广泛的一种调节器。本文对系统用到的PID控制技术做了相应的研究，从理论上分析了变积分的PID控制技术的优势，并在系统的实际测试中获得了良好的效果。

在两轮自平衡小车的研究上，国内外的专家和爱好者们取得了一系列的成果。由瑞士联邦技术学院工业电子实验室的研究人员研制的名为JOE的基于倒立摆的小型自平衡两轮车模型，是由DSP芯片进行控制的。它由车架上方所附的重物模拟实际车中的驾驶者。研究人员通过陀螺仪和光电编码器测量的数据，用线性状态反馈控制器来控制整个系统的平衡稳定。

我国在此方面的研究也取得了很大的成就，由中国科学技术大学研究出了自平衡两轮电动车，它是一种两轮式左右并行布置结构的具有自平衡系统的电动车。在车体内嵌

3

入式CPU的控制下，采集平衡传感器以及速度、加速度传感器的数据，通过一定的控制算法，计算输出PWM信号控制两个伺服电机的转矩，使车体保持平衡并能够根据人体重心的偏移，自动前进、后退及转弯。 1.3 本课题的研究内容和关键问题

本课题来源于福建省大学生电子竞赛题目，本人采用STC公司的8位微控制器STC12C5A60S2作为核心模块，通过分析建立数学模型、电机驱动电路以及编写相应软件，制作一个能够完成自主平衡的双轮平衡车。

本课题研究内容包括：

（1） 传感器数据融台：采用一种卡尔曼滤波算法，将加速度计和陀螺仪检测到的车

体姿态信息进行数据融合，得到可靠的倾角及变化率信息。

（2） 电机控制算法研究：对PID控制方法进行详细的研究和介绍，把变积分PD控制

方法应用在自平衡车系统中，并检验控制方法的有效性。

（3） 电机驱动器和系统控制器设计：设计制作出适合单轴双轮自平衡小车的无刷电

机驱动器，设计制作系统控制器用以控制传感器检测车体姿态，通过状态反馈控制，PID控制等算法，驱动无刷电机使两轮车实现动态自平衡。

（4） 机械结构的调整与完成，对双轮平衡车车模进行了结构改造，完成了定位参数

的优化、车辆重心位置的调整、转向舵机的力臂的改造以及齿轮传动机构的调整等。

（5） 控制系统的硬件设计与完成，从电源模块设计开始，完成速度倾角测量模块、

驱动控制模块及串口模块等的设计。

设计的重点是如何控制实现小车的平衡，以及在平衡下实现平衡运动。小车本身是不稳定的，在运动中需要利用传感器检测当前的姿态，反馈到单片机控制电机转速来实现平衡。控制采用PID控制，核心内容是PID控制参数的整定。实现小车的平衡，控制是关键。由于各个参数难以确定（重心位置，回转半径，电机运转时的作用力和摩擦系数等等），因此在设计PID控制时各个系数难以确定，PID控制器的参数整定是整个调试的核心内容。这就需要不断的改变参数观察小车运行效果，再回头去调整各个参数。

4

第2章 双轮平衡小车系统的总体概述

2.1 系统组成

单轴双轮车系统的主要框图如图2-1，主要包括:控制核心(MCU)模块、电源管理模块、电机驱动模块、速度检测模块及调试辅助模块。每个模块都包括硬件和软件两部分。硬件为系统工作提供硬件实体，软件为系统提供各种算法。

测速脉冲采样电路 显示模块 MPU6050传感器AD采样 STC12C5A60S2 电机驱动模块 NRF24L01无线模块 串口通信 电源模块

图2-1 系统方框图

2.2 系统各模块的主要功能

该系统主要由三个部分组成：第一是小车的硬件设计，一个好的硬件构架可以减少不必要的调试麻烦；第二是平衡信号检测处理，要求传感器检测系统要能快速检测到倾斜信号，保证小车静止平衡和倾斜情况之后的调节能力；第三是单片机通过对检测到的信号进行分析处理，通过相应的C语言程序来实现。

系统以单片机STC12C5A60S2为控制核心，完成采集信号的处理和控制信号的输出。MPU-6050传感器主要采集小车的角度和角速度信号，通过卡尔曼滤波器融合滤波的方法，得到一个光滑平稳的角度和角速度。测速脉冲电路主要是测量小车的速度。通过PID控制算法计算出PWM值驱动电机。电机驱动模块采用L298N驱动芯片，驱动左右电机。NRF24L01无线模块通过无线方式控制小车的前进后退和转弯。显示模块显示测试角度、角速度和小车速度的实时跟踪值。 2.3 系统的主要特点

系统采用单片机STC12C5A60S2作为MCU，STC12C5A60S2/AD/PWM 系列单片机是宏晶科技生产的单时钟/机器周期(1T)的单片机，是高速/低功耗/超强抗干扰的新一代8051单片机，指令代码完全兼容传统8051,但速度快8-12 倍。内部集成MAX810专用复位电路,2路PWM,8路高速10位A/D转换,针对电机控制，强干扰场合。工作频率最高可达35MHZ. 保障了系统的实时性。系统硬件和软件都采用了模块化结构，这样可以按需求方便容易的增加和删减功能。系统采用数字PID控制器来控制驱动电机和伺服电机，PID控制器技术成熟，结构简单，参数容易调整，在工业中有着很广泛的应用。

5

第3章 双轮平衡小车系统的原理

3.1 小车直立平衡物理分析与数学建模

车模平衡控制是通过负反馈来实现的，与保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。

图3-1 小车平衡模型

3.1.1 小车直立平衡物理分析

如何让小车平衡在直立位置呢？我们来分析一下他的受力情况。

图3-2 小车受力分析

如图3-2，为了要达到平衡直立的目的，只有两个办法：一个是改变重力的方向；另一个是增加额外的受力，使得恢复力与位移方向相反才行。显然能够做到的只有第二种方法。

控制倒立摆底部车轮，使得它作加速运动[2]。此时倒立摆所受到的回复力为：

F=mgsin?-macos?≈mg?-mk1?1 (3-1)

公式（3-1）中，由于?很小，所以进行了线性化。为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力。因此式（3-1）可变为公式（3-2）。

F=mg?-mk1 ?1-mk2 ?? (3-2)

因此，可得控制车轮加速度的控制算法：

6

a=k1?1+k2?? (3-3)

公式(3-3)中，?1为车模倾角；??为角速度；k1、k2均为比例系数；两项相加后作为车轮加速度的控制量。只要保证在k1>g, k2>0条件下，k1可以使得车模像单摆一样维持在直立状态。这两个系数的作用如图3-3所示。

图3-3 比例系数图

3.1.2 小车直立平衡数学建模

假设倒立车模简化成高度为L，质量为m的简单倒立摆，它放置在可以左右移动的车轮上。假设外力干扰引起车模产生角加速度x (t)。沿着垂直于车模地盘方向进行受力分析，可以得到车模倾角与车轮运动加速度以及外力干扰加速度a (t) 与x (t)之间的运动方程[3]。如图3-4所示：

图3-4 车模简化图

对应车模静止时，系统输入输出的传递函数为：

H(s)=?(s)=X(s) (3-4) g(s2-)L7

1此时系统具有两个极点s=±g。一个极点位于s平面的右半平面，因此车模不L稳定。对函数引入比例和微分。此时，系统传递函数为：

H(s)=?(s) =X(s) (3-5)

k(k-g)s2+2 s+1LL1此时两个系统极点位于：

-k2±k22-4L(k1-g)s= (3-6) 2L系统稳定需要两个极点都位于s平面的左半平面。要满足这一点，需要k1>g, k2>0，直立车模可以稳定。这与前面物理分析所得出的结论是一致的。

在角度反馈控制中，与角度成比例的控制量称为比例控制；与角速度成比例的控制量称为微分控制（角速度是角度的微分）。因此上面系数分别称为比例和微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模震荡。通过微分抑制控制震荡的思想在后面的速度和方向控制中也同样适用[4]。

总结控制车模直立稳定的条件如下：

（1）能够精确测量车模倾角?1的大小和角速度??的大小； （2）可以控制车轮的加速度。 3.2 角度、角速度及车速的数据采集与处理

两轮车系统用到的惯性传感器有加速度计和陀螺仪，另外还要对电机进行测速，他们分别用于检测车身倾斜角、倾斜角速度和车体实时速度。这三种信号主要反应了两轮车的位姿和运动状态。本节主要讨论如何对来自惯性传感器的数据进行融合处理，以得到最优的车体姿态信息。

3.2.1 角度数据处理

加速度计可以测量动态和静态的线性加速度，静态加速度的一个典型例子就是重力加速度，用加速度计来直接测量物体静态重力加速度可以确定倾斜角度。当加速度传感器静止时，加速度传感器仅仅输出作用在灵敏轴上的重力加速度值，即重力加速度的分量值。

（1）通过两个轴来计算偏移角

图3-5 双轴加速度计夹角

8

通过图3-5可得出，加速度计输出与重力的关系可表示为：

AX=g?sinθ (3-7)

Ay=g?cosθ=g?sin（90°-θ） (3-8) θ=tan-1（AX/Ay） (3-9)

通过公式（3-9）即可得到倾斜角度值。在自平衡小车系统中，由于车体倾斜角度

小于30°，所以可以采用正切三角函数近似得到下式：

θ≈Ax/Ay (3-10)

（2）通过一个轴来计算偏移角

测量其中一个方向上的加速度值，就可以计算出车模倾角，比如使用X轴方向上的加速度信号。其变化的规律为：

AX=g?sinθ≈g?θ (3-11)

公式3-11中，g为重力加速度；θ为车模倾角。当倾角θ比较小的时候，输出电

压的变化可以近似与倾角成正比。

由于车模本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在上述测量信号上使得输出信号无法准确反映车模的倾角，如图3-6所示。

图3-6 车模运动引起加速度信号波动

加速度计测量角度时，车模倾角在两个角度位置过渡中，除了角度变化信号之外，还存在由于运动引起的电压波动，这个电压波动随着车模运动速度增加会变得很大。

3.2.2 角速度数据处理

陀螺仪的直接输出值是相对灵敏轴的角速率，角速率对时间积分即可得到围绕灵敏轴旋转过的角度值。由于系统采用微控制器循环采样程序获取陀螺仪角速率信息，即每

9

隔一段很短的时间采样一次，所以采用累加的方法实现积分的功能来计算角度值：

angle=anglen-1+gyron?dt (3-12)

公式（3-12）中angle为陀螺仪采样到第n次的角度值；anglen-1为陀螺仪第n-1次采样时的角度值；gyron为陀螺仪的第n次采样得到的瞬时角速率值；dt为主循环程序运行一遍所用时间。图3-7可见，用陀螺仪输出值积分计算角度，要求处理器运算速度足够快，采样程序应尽量简练，程序循环一遍所用时间dt越小，采样频率越高，最后积分得到的角度值才能越精确。

图3-7 角速度积分得到角度

由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号，如图3-8所示。

图3-8 角速度积分漂移现象

如何消除这个累积误差呢？方法就是通过上面的加速度传感器获得的角度信息对此进行校正。通过对比积分所得到的角度与重力加速度所得到的角度，使用它们之间的偏差改变陀螺仪的输出，从而积分的角度逐步跟踪到加速度传感器所得到的角度。如图3-9所示。

10

图3-9 通过加速度计矫正陀螺仪的角度偏移

综上所述，对于姿态检测系统而言，陀螺仪虽然动态性能良好，能够提供瞬间的动态角度变化，不受加速度变化的影响，但是由于其本身固有的特性、温度及积分过程的影响，存在累积漂移误差，不适合长时间单独工作；加速度计静态响应好，能够准确提供静态的角度，但受动态加速度影响较大，不适合跟踪动态角度运动。为了克服这些困难，采用一种简易卡尔曼滤波方法来融合陀螺仪和加速度计的输出信号，补偿陀螺仪的漂移误差和加速度计的动态误差，得到一个更优的倾角近似值。

3.2.3 加速度计与陀螺仪传感器的数据融合

传感器数据融合是指对来自多个传感器的数据进行多级别、多方面、多层次的处理，从而产生新的有意义的信息，而这种新信息是任何单一传感器所无法获得的。对于加速度计，需要采用滤波算法除去短时性快速变化的信号，保留长时性缓慢变化的信号，所以对加速度计应用低通滤波算法。一种常用的方法是给变化较快的角度信号乘上一个较小的权重系数，以削弱突变信号对整体产生的影响。

对于陀螺仪，情况正好相反，应用高通滤波方法处理陀螺仪数据，来抑制陀螺仪积分的漂移。根据前两节介绍车模角度控制和角度测量方法，可以得到如图3-10车模角度控制方案框图。

图3-10 角度控制框图

该方案中采用重力加速度计和陀螺仪通过角度互补融合方式获取车模倾角和角速度，通过两个比例常数加权后，控制电机驱动电压，使得车模产生相应的加速度，维持车模的直立。

为了保持车模的静止或者匀速运动需要消除这个安装误差。在实际车模制作过程中需要进行机械调整和软件参数设置。另外需要通过软件中的速度控制来实现速度的稳定

11

性。对角度和角速度的融合，有两种方案进行融合，一种是互补滤波方式滤波，一种是卡尔曼滤波方式滤波，在本设计中我采用卡尔曼方式滤波，下面对卡尔曼方式滤波做一个简单的介绍。

卡尔曼滤波器是一个“最优化自回归数据处理算法”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导_弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等[5]。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：X(k)=A X(k-1)+B U(k)+W(k)

再加上系统的测量值：Z(k)=H X(k)+V(k)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R。

对于满足上面的条件，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的协方差来估算系统的最优化输出。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)=A X(k-1|k-1)+B U(k) (3-16)

式(3-16)中，X(k|k-1)是利用上一状态角度预测的结果，X(k-1|k-1)是上一状态角度最优的结果，U(k)为现在状态的角度控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的协方差还没更新。我们用P表示协方差：

P(k|k-1)=A P(k-1|k-1) A'+Q (3-17)

式(3-17)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应

的协方差， A?表示A的转置矩阵，Q是系统过程的协方差。式子3-16和3-17就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3-18)

其中Kg为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H' / (H P(k|k-1) H' + R) (3-19)

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼

滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的协方差：

P(k|k)=（I-Kg(k) H）P(k|k-1) (3-20)

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就

是式子(3-17)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

12

卡尔曼滤波器的原理基本描述了，式子(3-16)，(3-17)，(3-18)，(3-19)和(3-20)就是他的5个基本公式。根据这5个公式，可以很容易的实现角度控制的程序。 3.3 PID控制技术

PID控制器就是将偏差的比例(P)、积分(I)和微分(D)通过线性组合构成控制量，用

[6]

这一控制量对被控对象进行控制。

1 du(t)=Kp [e(t)+ ?e(τ)dτ+Td e(t)] (3-21)

Ti0dt1公式3-21中， Kp e(t)为比例控制项，Kp称为比例系数。 ?e(τ)dτ为积分

Ti0tt控制项，Ti称为积分时间常熟。Td d e(t)为微分控制项，Td 为微分时间常熟。 dt把公式3-21也可以改成公式3-22.其中Kp称为比例系数，Ki称为积分系数，

Kd称为微分系数。

tu(t)=Kp e(t)+Ki ?e(τ)dτ+Kd0 d e(t) (3-22) dt比例环节即时成比例地反映控制系统的偏差信号e(t)，偏差一旦发生，控制器立即产生控制作用，以减少偏差。控制作用的强弱取决于比例系数瞄，晒越大，控制越强，但过大的晦会导致系统震荡，破坏系统的稳定性[7]。

积分环节的作用是把偏差的积累作为输出。在控制过程中，只要有偏差存在，积分环节的输出就会不断增大，直到偏差e(t)=0，输出的u(t)才可能维持在某一常量，使系统在给定值心)不变的条件下趋于稳态，因此积分环节能消除系统输出的静态误差。但积分环节也会降低系统的响应速度，增加系统的超调量。积分作用的强弱取决于积分时间常数TI，TI越大，积分作用越弱，反之则越强。

微分环节的作用是阻止偏差的变化。它是根据偏差的变化趋势(变化速度)进行控制。偏差变化得越快，微分控制器的输出就越大，并能在偏差值变大之前进行修正。微分作用的引入，将有助于减小超调量，克服振荡，使系统趋于稳定，特别对高阶系统非常有利，它加快了系统的跟踪速度。但微分的作用对输入信号的噪声很敏感，对那些噪声较大的系统一般不用微分，或在微分作用之前先对输入信号进行滤波。

PID控制器参数的确定：

在电机控制中，首先要求系统是稳定的。在给定值变化时，被控量应当能够迅速、平稳的跟踪，超调量要小。PID参数的选择有两种可用的方法：理论设计法及实验确定法。理论设计法确定PID控制参数的前提是要有被控对象准确的数学模型，这在电机控制中往往很难做到。因此，实验确定法来选择PID控制参数便成为经常采用且行之有效的方法。

凑试法是通过模拟或闭环运行观察系统的响应曲线(如阶跃响应)，然后根据各调节参数对系统响应的大致影响，反复凑试，改变参数，以达到满意的响应，从而确定PID控制器参数。在凑试时，可以参考控制参数对控制过程的影响趋势，对参数按先比例、

13

后积分、再微分的次序反复调试。具体的整定步骤如下：

（1） 调节器参数积分系数S0=O，实际微分系数k=O，控制系统投入闭环运行，由小到

大改变比例系数S1，让扰动信号作阶跃变化，观察相应的系统响应，直到获得反应快、超调小的响应曲线，得到满意的控制过程为止。

（2） 如果在比例调节的基础上系统的静差达不到设计要求，则必须加入积分环节。取

比例系数S1为当前的值乘以0.83，由小到大增加积分系数S0，同样让扰动信号作阶跃变化，根据响应曲线的好坏反复改变比例系数和积分系数，使在保持系统良好动态性能的情况下，稳态误差得到消除，由此得到相应的整定系数，求得满意的控制过程。

（3） 积分系数S0保持不变，改变比例系数S1，观察控制过程有无改善，如有改善则继

续调整，直到满意为止。否则，将原比例系数S1增大一些，再调整积分系数S0，力求改善控制过程。如此反复试凑，直到找到满意的比例系数S1和积分系数S0为止。

（4） 若使用比例积分调节器消除了稳态误差，但动态过程经反复调整仍不令人满意，

则可加入微分环节，构成比例积分微分调节器。引入适当的实际微分系数k和实际微分时间TD，此时可适当增大比例系数S1和积分系数S0。和前述步骤相同，微分时间的整定也需反复调整，直到控制过程满意为止。 3.4 车模速度控制

对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。下面先分析一下引起车模速度变化的原因。

（1） 如何测量车模速度？

（2） 如何通过车模直立控制实现车模倾角的改变？ （3） 如何根据速度误差控制车模倾角？

第一个问题可以通过安装在电机输出轴上的光码盘来测量得到车模的车轮速度。利用控制单片机的计数器测量在固定时间间隔内速度脉冲信号的个数可以反映电机的转速。

第二个问题可以通过PID控制算法，角度控制给定值来解决。给定车模直立控制的设定值，在角度控制调节下，车模将会自动维持在一个角度。通过前面车模直立控制算法可以知道，车模倾角最终是跟踪重力加速度Z轴的角度。因此车模的倾角给定值与重力加速度Z轴角度相减，便可以最终决定车模的倾角。

图3-11 车模倾角给定

14

图3-11中车模角度控制规律可以看出车模倾角设定量与车模倾角变化之间大体呈现一个一阶惯性环节

1。

Tzs+1第三个问题分析起来相对比较困难，远比直观进行速度负反馈分析复杂。首先对一个简单例子进行分析。假设车模开始保持静止，然后增加给定速度，为此需要车模往前倾斜以便获得加速度。在车模直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得车模往前倾角需要更大。如此循环，车模很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈[6]。如图3-12所示；

图3-12 车模倾角控制速度中的正反馈

为什么负反馈控制在这儿失灵了呢？原来在直立控制下的车模速度与车模倾角之间传递函数具有非最小相位特性（在此省略了分析），在反馈控制下容易造成系统的不稳定性。但根据实际经验，是可以通过速度控制进行车模倾角控制的。为了得到合理的经验控制结论，需要将前面速度负反馈控制问题进行简化建模分析一下。所做简化假设如下：

（1) 车模角度在直立控制下简化成一个一阶过渡过程1。

Tzs+1（2) 车模倾角是由车轮运动产生，因此车轮速度是倾角变量求导再乘以车模长度。 （3) 忽略车模倾角引起车轮加速度。由于这个讨论仅对控制一开始短暂的过渡阶段而言，

此时由于车模倾角比较小，引起速度变化很小。 （4) 系统只进行速度比例负反馈。

分析简化系统的传递函数，它具有一个一阶极点-1。如果要保证系统稳定，需

Tz-k要满足Tz>k。这就要求车模倾角的调整时间常数Tz要大，调整速度缓慢和平滑，同时速度的反馈比例k要小。这一结论与实际经验是相符合的。以上两点在程序实现方法可以通过改变控制周期，减小控制参数，信号进行平滑滤波等方式来实现。

在实现上述两点之后，再分析车模运动速度的稳定性。为了简化分析，首先忽略了由于倾角控制引起的车模速度的变化。车模的速度最终由倾角所产生的加速度决定，因此将倾角的进行积分便可以得到车模的运动速度。此时车模速度控制框图如图3-13。

15

图3-13 车模速度控制框图

前面分析可知为了保证系统稳定，往往取的车模倾角控制时间常数Tz很大。这样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的震荡现象。这个现象在实际参数整定的时候可以观察到。那么如何消除速度控制过程中的震荡呢？[8]

要解决控制震荡问题，在前面的车模角度控制中已经有了经验，那就是在控制反馈中增加速度微分控制。此时系统传递函数的极点实部变大，加快了系统的收敛速度。

图3-14车模角度和速度控制图

在图3-14中，角度控制需要两个控制参数，分别是比例控制参数Pangle和微分控制Dangle参数。速度控制同样也有两个参数，分别是比例控制参数Pspeed和微分控制参数Dspeed。在这两个控制中都使用了微分控制，目的是增加车模的角度和速度的稳定性，防止控制超调。这个经验在后面车模方向控制中仍然适用。

16

第4章 系统硬件电路设计

4.1 单片机最小系统模块设计

采用宏晶科技有限公司的STC12C5A60S2单片机作为控制CPU，该单片机中包含中央处理器、程序存储器（Flash）、数据存储器（RAM）、EEPROM、定时/计数器、I/O接口、UART接口和中断系统、I2C接口、高速A/D转换模块、PWM（或捕捉/比较单元）以及硬件看门狗、电源监控、片内RC振荡器等模块。可以说STC12C5A60S2单片机几乎包含了数据采集和控制中所需的所有单元模块，可称得上一个片上系统（SOC），可以很容易地构成典型的测控系统。

本系统设计主要应用其PWM（或捕捉/比较单元）、四路10位高速A/D转换模块、I2C接口、定时/计数器、I/O接口等功能模块。如图4-1。

图4-1 最小系统模块电路图

4.2 红外光电传感器GK102测速电路设计

对射式光电传感器GK102采用高发射功率红外光电二极管和高灵敏度光电晶体管组成。采用非接触检测方式。GK102的外观图如图4-2所示，GK102的外接电路图如图4-3所示。

图4-2 GK102外观图

17

图4-3 GK102外接电路图

4.3 NRF24L01无线模块控制电路设计

nRF24L01+（或nRF24L01P）是一款工作在2.4-2.5GHz 世界通用ISM 频段的单片无线收发器芯片。无线收发器包括：频率发生器、增强型SchockBurst模式控制器、功率放大器、晶体振荡器调制器、解调器。输出功率频道选择和协议的设置可以通过SPI接口进行设置。

极低的电流消耗，当工作在发射模式下发射功率为0dBm时电流消耗为11.3mA，接收模式时为13.5mA，掉电模式和待机模式下电流消耗更低。

图4-4 nRF24L01+模块图 图4-5 nRF24L01+引脚图

图4-6 nRF24L01+模块的外部电路图

18

4.4 六轴模块电路图

MPU-6050六轴运动处理组件，MPU-6050整合了超低功耗3轴MEMS陀螺仪、3轴MEMS加速计，测量范围达± 16g。可通过SPI(3线或4线)或I2C数字接口访问。MPU6050非常适合移动设备应用。其高分辨率(3.9mg/LSB)，能够测量不到1.0°的倾斜角度变化。通过MPU-6050可以直接测出它的角度和角速度，非常的方便。而且价格便宜。六轴模块原理图如图4-7所示，六轴模块引脚图如图4-8所示。

MPU-6050芯片中，集成了陀螺仪、加速度计和一个AD转换。芯片正常工作时陀螺仪和加速度计分别采集x轴、y轴和z轴的电压值。然后通过AD转换，转换成数字信号，最后通过IIC总线传送到控制芯片。此时得到的值不能算是角度和角速度值，这个值还必须经过一定的比例关系进行转换，才能得到角度和角速度。表4-1为六轴模块引脚功能表。

图4-7 六轴模块原理图

图4-8 六轴模块引脚图

19

表4-1 六轴模块引脚功能表 管脚 1 2 3 4 5 6 7 8 名称 Vcc_5v GND SCL SDL XDA XCL AD0 INT 功能 电源+5V 接地端 I2C 串行时钟 I2C 串行数据 I2C 主串行数据，用于外接传感器 I2C 主串行时钟，用于外接传感器 I2C Slave 地址LSB 中断数字输出（推挽或开漏） 4.5 AD转换电路的设计

STC12C5A60AD/S2系列带A/D转换的单片机的A/D转换口在P1口(P1.7-P1.0)，有8路10位高速A/D转换器,速度可达到250KHz(25万次/秒)。8路电压输入型A/D，可做温度检测、电池、电压检测、按键扫描、频谱检测等。上电复位后P1口为弱上拉型I/O口，用户可以通过软件设置将8路中的任何一路设置为A/D转换，不需作为A/D使用的口可继续作为I/O口使用。

在电路中用AD采样的方法，可以对PID算法中的各个参数值进行微调。比较方便调试。AD采样电路图如图4-9所示。

图4-9 AD采样电路图

4.6 电源电路模块设计

本系统设计的大多数芯片如单片机、传感器、AD采样、串并转换芯片工作电压都为5V，故制作+5V电源以满足供电需要，所以本设计采用7805来制作三端正电源稳压电路，它们的封装形式是TO-220。它使用方便，用很简单的电路即可以实现一个直流稳压电源, 它们的输出电压恰好为+5V，刚好是51系列单片机运行所需的电压。但是通信模块NRF24L01使用的是3.3V电压供电，所以，还必须进行3.3V变压。3.3V稳压芯片使用的是AMS1117-3.3。电源电路图如图4-10所示。

图4-10 电源电路图

20

4.7 直流电机的驱动电路

图4-11 直流电机的驱动电路

L298可驱动两个电动机，OUT1、OUT2和OUT3、OUT4之间可分别接电动机，IN1~IN4脚为输入控制电平，控制电平正反转，从而控制小车行驶方向。ENA、ENB接控制使能端，与单片机的PWM调制输出口(P1.2和P1.3)相接，控制电机的停转、加速。1脚、15脚是输出电流反馈引脚，通常使用可直接接地。如表4-2所示，对应直流电机的运行状态。图4-11为直流电机的驱动电路。

表4-2 直流电机功能表

ENA 0 1 1 1 1

IN1 x 1 0 1 0 IN2 x 0 1 1 0 运转状态 停止 正转 反转 刹揨 停止 21

第5章 系统软件设计

5.1 软件设计的总体思路

本设计要实现小车直立平衡、小车前进、小车后退、小车左转、小车右转等功能，除硬件外，还需要软件来控制。本章主要介绍单片系统对各模块的软件编程控制，如采集九轴模块里的角度与角速度，测量电机的速度，PWM脉宽调制来控制车速等。本系统设计主要采用Keil4软件编写与调试程序，程序语言采取易读性和移植性更高的C语言编写。

系统软件需要实现以下功能： ( 1 ) 数据采集：

在系统软件中，输出六轴传感器的数据、光电传感器的打开与关断控制信号。 ( 2 ) 信号处理：

对采集的信号进行处理，进行数据融合。以PWM值的大小进行表示。 ( 3 ) 数据传输与显示：

经软件处理得到的PWM值，发送到L298N控制芯片。进行对轮子的控制。

5.2 系统程序设计主要流程图

本系统设计主要采用Keil4软件编写与调试程序，程序语言采取易读性和移植性更高的C语言编写。系统程序主要分为定时器中断，捕获中断（包含匹配中断）与主程序几部分。

主程序是单片机程序的主体，整个单片机端系统软件的功能的实现都是在其中完成的，在此过程中主程序调用了子程序及中断服务程序。程序首先完成初始化过程，然后进行数据的采集过程，然后是数据的处理过程，最后是数据的输出过程，其流程图如图5-1所示。其中Angle_Calcu（）为角度角速度处理函数，Angle_calcu()为速度位置处理函数， PWM_Calcu()为PID控制算法核心，融合角度角速度速度位置四个变量[9]。

22

开始MPU6050传感器初始化定时器、外部中断、串口初始化AD转换、PWM初始化 Angle_Calcu(); 读取MPU6050的角度与角速度，并用卡尔曼滤波器进行滤波Psn_Calcu(); 采集小车速度和位置PWM_Calcu();对各种采集到的数据进行处理，并转换成PWM，对电机进行控制结束图5-1 系统主流程图

捕获中断用于检测转速，其流程图5-2。

23

开始否判断中断标志位是RA==1是speed_ml++;否speed_ml--;结束 图5-2 外部中断流程图

Angle_Calcu（）函数是处理通过传感器MPU6050得到的角度和角速度值，并进行融合与滤波，是两轮小车的核心部分，图5-3为Angle_Calcu（）函数的流程图。

开始读取MPU6050,Y轴加速度值去除x轴零点偏移,计算得到角度（弧度），并将弧度转成度读取MPU6050,X轴加速度值去除x轴零点偏移,计算得到角速度(度/秒)角速度积分得到角度值将得到的角度和角速度进入卡尔曼滤波器进行滤波Kalman_Filter(Angle_ay,Gyro_x); 计算倾角结束 图5-3 角度角速度处理函数流程图

24

PWM计算与输出函数PWM_Calcu(void)，此函数为了计算驱动电机的PWM。图5-4是PWM_Calcu(void)函数的流程图。

开始Angle<-40||Angle>40是CCAP0H = 0;CCAP1H = 0;否PWM = Kp*Angle*K_angle_AD + Kd*Gyro_x*K_angle_dot_AD;PWM += Kpn*position*K_position_AD + Ksp*speed*K_position_dot_AD;PWM_R = PWM - turn_need;PWM_L = -PWM - turn_need;PWM_output(PWM_L,PWM_R); 结束 图5-4 PWM计算与输出流程图

25

第6章 测试方案与测试结果

车模制作与软件开发为车模运行打下了基础，是否运行的稳定和快速需要通过车模精心的调试才能够完成。调试过程同时也可以使得我们加深对于控制算法物理过程深刻理解，提高解决实际工程问题的能力，为寻找更加优化的车模设计方案打下基础。调试过程对于车模制作非常重要。系统调试分为硬件调试和软件调试两个部分。只有按照一定顺序操作才能保证系统真正达到设计要求。

车模调试分为调试准备、静态参数整定、动态参数整定、车模机械调整等各个环节。整个调试中涉及到的参数和部件非常多，而且这些参数之间还有着紧密的相互影响。如果对于其中的物理过程认识不清，有没有正确的调试步骤，那么在调试过程中出现的各种错误现象就会掩盖正确的原因，影响整个调试进程，甚至会动摇制作的信心。 6.1 调试参数

前面给出的算法程序存在很多参数，虽然从理论上可以对这些参数进行优化计算。但是由于受到车模模型精度的影响，计算所得到的参数也只能够作为参考值——调试的起始范围。实际优化参数需要通过一定的工程步骤最终确定，这个过程称为参数整定。

首先确定需要整定的车模控制参数。这些参数包括两类： （1) 传感器参数：

? 陀螺仪比例参数。将陀螺仪采集数值转换成角速度信号，单位是°/秒。

? 陀螺仪零点偏移量。陀螺仪静之时采集量。

? 加速度计Y轴零点偏移量。重力加速度传感器在车模垂直静止时的采集量。 ? 加速度计比例参数。将加速度计采集数值转换成角度信号，单位是度。 ? 加速度计Y轴的最大值与最小值。

（2) 运动控制参数：

? 角度比例控制参数。 ? 角度微分控制参数。 ? 速度比例控制参数。 ? 速度微分控制参数。 ? 电机输出死区参数。

6.2 车模初步调试

6.2.1 检查电源

用6节5号电池串起来，用于单片机，传感器的供电，其输出电压为9V，必须要经过变压，降压到5V。我们要检测5V电压输出是否稳定。并接到主控扳上，单片机能否正常工作。在一些传感器，用的是3.3V电压供电，所以，我们还要检测经过AMS1117-3.3

26

芯片后的稳压效果。

对于电机的供电，因为电机正常工作的电压为12V。所以，我们必须要提供一个12V的稳压直流电源。在本设计中，我采取串联4节18650电池。串联后的输出电压大概为15.6V左右，高于12V电压。很容易烧毁电机。所以，必须对15.6V电压进行降压处理。我用7812芯片把15.6V电压降到12V电压。对于稳压后的电压进行检测，看起稳压效果。

6.2.2 角度与角速度的采集

在原理部分3.2.1中，对于角度测量有了一定的了解，现在我们就针对MPU6050传感器做一个具体的数据分析。

根据传感器摆放的位置，我选择了y轴加速度计数据进行角度计算，x轴陀螺仪数据进行角速度计算。单片机通过IIC总线读取MPU-6050传送过来的数据，此时得到的数据为加速度计得到的值为Accel_y，陀螺仪得到的值为Gyro_x，在静止状态下把车放置与平衡位置，待传感器稳定后，采集其中50个数据，此时的值为它的的零点偏移量。角度角速度的零点偏移如表6-1所示，Accel_y的单位为（LSB），Gyro_x的单位为（LSB）

表6-1 角度角速度零点偏移

第一组 n Accel_y Gyro_x n 第二组 Accel_y Gyro_x n 第三组 Accel_y Gyro_x n 第四组 Accel_y Gyro_x n 第五组 Accel_y Gyro_x 1 2 3 4 5 6 7 8 9 10 1100 1099 1101 1102 1100 1100 1099 1100 1100 1102 8 8 8 8 8 8 8 7 8 8 1 2 3 4 5 6 7 8 9 10 1101 1100 1098 1100 1100 1103 1097 1100 1100 1100 9 8 8 8 8 8 9 7 8 8 1 2 3 4 5 6 7 8 9 10 1100 1099 1100 1100 1099 1011 1100 1100 1012 1100 8 8 8 8 8 9 7 8 8 8 1 2 3 4 5 6 7 8 9 10 1097 1100 1100 1098 1100 1100 1099 1104 1100 1102 8 8 7 8 8 9 7 8 8 8 1 2 3 4 5 6 7 8 9 10 1101 1103 1100 1100 1097 1100 1098 1100 1100 1099 8 9 8 8 8 9 7 8 8 8 数据处理：

?Accel_y角度零点的平均值： θ=0nnnn ≈1100 (6-1)

角速度的零点平均值：d?=?Gyro_x0nn ≈8 (6-2)

此时得到了角度和角速度的零点偏移值，在章节3.2.1中我是以X轴的变化作为角度的检测，而Y轴则作为角速度的检测，此时角度θ?AX/g，由于传感器的摆放位置

27

的不同，在本设计中，以Y轴作为测量角度轴，而X轴则作为角速度采集轴，此时的公式为θ?Ay/g。通过公式（6-3）和（6-4）转换成角度和角速度。

180??Angle_ay=Ay/g??(Accel_y- 1100)/16384*1.2*/g; (6-3) ?3.14??Gyro_x =(Gyro_x-8) /16.4 (6-4)

公式（6-3）中Accel_y为MPU6050传感器加速度计传过来的数据，1100为加速度

计的零点偏移值，加速度计我选2g的范围，通过查MPU-6050传感器数据手册可知，它的换算关系是16384 LSB/g，180/3.14为了把弧度转换成度，此时得到的值不是它真正的角度值，需适当的放大1.2倍，近似的得到它的角度值。公式（6-3）中Gyro_x为MPU6050传感器陀螺仪传过来的数据，8为陀螺仪的零点偏移值，陀螺仪我选得范围为2000deg/s，通过查MPU-6050传感器数据手册可知，它的换算关系为16.4 LSB/(o/s)。此时的

Angle_ay的范围是(-90,90),平衡位置为0度。Gyro_x的值，在静止时为0，摆动时不为0，方向与角度的微分方向相一致，符合要求。

6.2.3 单片机PWM的输出测试

在单片机软件可以进行开发下载基础之上，编写单片机PWM输出控制程序。通过控制电机转速，验证驱动电路的正确性。确定两个电极输出电压的极性。保证输出PWM正电压时，电机带动车模前行；PWM输出负电压时，车模后行。

6.2.4 光码盘脉冲的测量与速度的计算

一般转速的稳态测量可以通过测频法（即“M”法）和测周法（即“T”法）来实现[7]

。测频法是指将转速传感器取来的信号送入频率计数器, 在给定的标准时间间隔内读出脉冲数, 然后计算出转速值。由于其误差来源主要是多记一个脉冲或少记一个脉冲 (不考虑晶振误差 ), 在低转速情况下这种计数带来的误差对转速测量的结果影响很大, 所以测频法更适用于高转速测量。而用测周法测量情况正好相反。测周法就是将转速传感器取来的脉冲信号作为闸门信号, 测量脉冲信号的周期, 由于晶体振荡器提供的时钟信号相当准确, 所以计时带来的误差很小,其公式如下:

n =1/T*60 (6-5)

式(6-6)中: n为转速,r/min；T为周期,s。可以看出转速越低周期越长,通过测量脉冲间隔时间就能得出转速值。又由于使用的晶振稳定性小于10, 保证了计时的准确度和稳定性, 以及转速结果的可靠性。这部分功能在测得时间值后运用测周法公式即可实现。

测量瞬态转速就是对旋转体转速变化过程的描述, 也就是反映旋转体不同时刻的转速值[8]。采样时间越短转速测量仪描述转速变化的过程越清晰, 显示转速变化过程的曲线也越光滑。为了更好地测量瞬态红外光电传感器利用高灵敏度的光电探测器作为检测元件, 包括光学成像系统及前置放大电路两部分。

从测速设备的工作性质考虑，本设计使用测周法，即“T”法。

通过单位时间内进行脉冲计数观察电机转动时运行情况，确定电机的幅度和频率。

28

?6编写脉冲采集程序并进行显示。电机在不同电压下的转速各不相同。所以，我们要测定不同PWM下的计数脉冲。并把他换算成速度。达到我们测速的要求。

数据转换原理：

轮胎的直径d为65mm，码盘格数n为12格，单片机中断时间T为10ms。 设10ms周期内单片机采集到的脉冲数x。 轮胎周长：l=πd=65*3.14=204.1(mm); 在10ms内，轮胎的圈数为：x/12;

在10ms内，走过的距离为：πd*x/12=204.1*x/12=17x(mm);

则1ms走过的距离为：

πd*x1x1*=204.1**=1.7x(mm)12101210

因为1m/s=1mm/ms；

所以此小车的速度为：v=1.7x(m/s); 6.2.5 单片机AD转换的数据的采集

单片机STC12C5A60S2具有8个通道的AD转换。参考设计方案应用了其中四个通道。编写AD转换程序，将采集的数值发送给上微机进行显示。通过观察各通道的采集值，确认各传感器工作状态，信号动态范围以及各通道噪声的影响。最好能够将各通道采集的数值通过曲线显示，可以比较直观的观察信号的变化和噪声的幅度。

每个模拟量都会具有不同比例的噪声，需要对每个采集量进行低通滤波。简单的方法就是通过多次采集进行平均来实现。 6.3 静态参数调试

6.3.1 传感器参数

车模传感器参数包括了测量车模运动状态各传感器在车模静止时的零点偏移量以及单位换算的比例值[10]。

(1) 测量传感器零点偏移量；

在调试桌面，保持车模垂直静止状态。通过单片机软件采集传感器MPU6050的值，传递给上位计算机进行显示。确定各传感器的零点偏移量。通过多次采集平均可以获得更加稳定准确的。在8.2.2中的角度和角速度的采集过程中，可以得出它的零点偏移量。

(2) 测量加速度传感器Y轴的最大值、最小值；

保持车模正面朝上和反面朝上静止可以测到Y轴最大值和最小值。通过实验测得，加速度传感器的最大值和最小值分别是180度和-180度。 (3) 计算陀螺仪比例值；

陀螺仪比例值决定了陀螺仪速度积分得到的角度单位是度，和重力加速度传感器是一致的。这个比例可以通过查看MPU6050数据手册可以得到他的比例值，最后换算成度/秒。通过查看手册，陀螺仪范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)。

(4) 计算加速度计比例值；

同陀螺仪比例值一样，加速度计也要通过一定的比例值来把测的值转换成角度关系。单位为度。它的比例值也是通过查看数据手册获得的。通过查看手册，

29

范围为2g时，换算关系：16384 LSB/g，又因为角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14，因为x>=sinx,故乘以1.2适当放大。最后得到实际的角度。

6.3.2 控制参数静态调试

测定的控制参数有四个，分别是角度控制参数Kp，角速度控制参数Kd，位置控制参数Kpn和速度控制参数Ksp，如公式(6-7)所示，其中Angle代表角度，Gyro_x代表x轴角速度，position代表速度的积分位置，speed代表测得的速度。

PWM = Kp*Angle + Kd*Gyro_x+ Kpn*position+ Ksp*speed; (6-6)

(1) 角度控制参数整定

角度控制参数包括比例和微分两个参数。比例参数Kp相当于倒立摆的回复力。这个参数需要大于重力加速度所产生的效果才能够使得车模保持直立。随着这个参数逐步增大，车模开始能够保持直立。该参数进一步加大，车模开始出现来回的摆动现象。微分参数Kd相当于倒立摆中的阻尼力，它可以有效的抑制车模的摆动。当该参数过大时，会引起车模本身的震动。这是由于车模本身不是一个刚体，车体具有一定的共振频率。微分参数过大时会使得车模在电机的驱动下产生车体的共振。

调节这两个参数可以遵循先比例后微分的过程。首先调整比例参数，使得车模能够保持直立并且开始来回摆动了。说明此事比例参数已经可以克服重力的影像。然后逐步增加微分参数，车模逐步保持直立稳定。增大微分参数直到车模开始共振，这样就可以确定微分参数的最大值了。然后适当减小微分参数，然后逐步增加比例参数，直到车模又开始震荡，这样便可以确定比例参数的最大值。然后适当减小比例参数。在这些参数附近进行试验，直到寻找到一组车模直立的控制最优参数[11]。

(2) 速度参数整定

在角度参数初步确定之后，便可以进行速度控制参数的整定。速度控制参数包括比例Kpn和微分Ksp两个控制参数，这里的比例和微分定义是指反馈速度而言。如果相对于车模的反馈位置（速度的积分量），这两个参数也可以分别对应称作积分I和比例P参数。

首先请注意速度的调制本质上是通过调整车模的角度来实现的。图2-38中显示了速度控制环节。比例控制参数决定抑制速度误差的能力，但是只有比例控制，车模速度控制就会出现双积分负反馈控制。第一个积分是误差的积分，第二个积分是车模倾角所产生的加速度积分。因此车模速度就会出现震荡现象。通过微分控制可以消除这个震荡现象。

在调整时，由于是进行静态调整，故此先将速度控制闭环的速度设定值置为0。然后开始后面的比例和微分参数的调整。具体的调试过程可以首先逐步比例参数，车模能够在一定平衡点附近来回运动。然后在逐步增加微分参数，车模就会很快停留在平衡点出。使用外力冲击车模，车模能够很快趋于静止。然后再逐步增加比例和微分控制参数，使得车模抵抗外部干扰冲击的能力逐步增强。

当比例和微分参数达到一定的数值后，车模开始出现摆动现象。至此可以知道比例和微分的大体范围。在此范围内通过几次调整测试，可以初步确定速度控制参数的最佳组合。

30

6.3.3 小车测试结果

小车在光滑的平地上完成了平衡，它的平稳性能很好，前进后退都可以基本完成，但是也存在严重的问题，在行驶的过程中，前进后退只能缓慢进行，当行驶起来后，速度会越来越快，最后会倒下去。初步设想，小车的动态参数（速度和位置参数）并没有达到最优，所以在行驶过程中会加速直到倒下去。在以后的工作中，会优化这个参数，直到运行平稳。小车实物图如图6-1。

图6-1 小车实物图

31

结束语

本次设计，以STC12C5A60S2为控制芯片，通过软件的各种算法，设计了能实现自平衡的双轮智能车控制系统。实现了车模能够直立平衡并做相应的运动。根据设计要求，分别对硬件和软件做了各种设计。硬件中主要有单片机最小系统，直流驱动电机控制模块，电源管理模块，测速编码模块，人机交互模块组成。软件中主要有A/D模块，PWM模块，MPU6050模块，PID控制算法，人机交互控制等。控制算法主要是车速控制，车速控制采用模糊控制进行速度的设定，采用PID控制调节速度。通过PID控制的调节，小车动态性能良好，整体控制效果良好。采用分段比例控制之后，能够更好的实现平衡，移动平衡也获得了一定提高。

本设计此小车数学模型是只考虑了在光滑平面上直线运动的情况，未把路面的不平整及小车的转动情况考虑进去，我们在以后的工作中可以基于此模型的基础上，把小车各种情况下的数学模型建立起来，这样小车实现的功能可以更强。由于时间的限制，仍有许多地方需要完善。如考虑对舵机转向加上反馈信号进行闭环控制，使得舵机的转向更为准确；采用MOS管驱动电机，获得更好的加速性能。

32

致 谢

接近半年的毕业设计结束了，在这段时间的设计和学习过程中，我得到了很多人的帮助，学到了很多东西。

首先，感谢我的指导老师赖义汉和其他指导过我的老师们。有了老师们的监督和指导，让我能够很好地学习专业知识，培养自己的动手能力，这在毕业设计中发挥了非常重要的作用。在设计过程中，老师以自己精专的知识，不断地启发我，使我的设计得以顺利进行，也使得我自己的专业水平有很大程度的提高。

再次，我要感谢同学对我帮助和指导，在设计中不懂的请教同学时，总是很热心的帮我发现问题，寻找更好的解决办法。

最后，祝你们身体健康, 工作顺利。

33

参考文献

[1] 陈静,阮晓钢.一种复杂环境下两轮自平衡机器人稳定控制研究[OL].[2009-05-25].中国科技论

文在线，http://www.paper.edu.cn/releasepaper/content/200905-608

[2] 杨兴明，丁学明，张培仁，赵鹏．两轮移动式倒立摆的运动控制[J]. 合肥工业大学学报，

2005.11,28（11）：1486-1488

[3] 张跃宝．两轮不稳定小车的建模与变结构控制研究[D].陕西：西安电子科技大学，2007

[4] 阮晓钢，任红格. 两轮自平衡机器人动力学建模及其平衡控制[OL]. [2008-04-10].中国科技

论文在线，http://www.paper.edu.cn/releasepaper/content/200804-341

[5] 杜云峰. 卡尔曼滤波器在过程估计中的应用[J]. 机电产品开发与创新. 2007(04)

[6] 张伟民,段晓明,赵艳花.两轮自平衡小车控制研究[J].《自动化技术与应用》2011年第30卷第

4 期：10-13

[7] 何跃,林春梅. PID控制系统的参数选择研究及应用[J].《计算机工程与设计》,2006, (08)：

1496-1498

[8] 李明爱,焦利芳,乔俊飞.自平衡两轮机器人的分层模糊控制[J].《控制工程》2009年1月第16

卷第1期：80-82+94

[9] 郭天祥，新概念51单片机C语言教程[M].电子工业出版社,2009 [10] FUKAO T, NAKAGA WA H, ADACHI N. Adaptive tracking control of a nonholonomic mobile robot

[J] . IEEE Transactions on Robotics and Automation , 2000 , 16(5) :609 -615 [11] LEE Ti-chung , SONG Kai-tai , LEE Ching-hung , et al. Tracking control of unicycle -

modeled mobile robots using a saturation feedback controller[J]. IEEE Transactions on Control Systems Technology , 2001 , 9 ( 2 ) : 305 -318.

34

附录

附录1：实物图

35

附录2：系统总原理图

36

附录3：单片机最小系统PCB图

附录4：两轮车主程序

#include #include #include #include

typedef unsigned char uchar; typedef unsigned short ushort; typedef unsigned int uint;

//******功能模块头文件*******

#include \.H\ #include \#include \#include \#include \

#include \

//延时头文件

//MPU6050头文件 //PWM头文件

//串口头文件 //卡尔曼滤波头文件

//AD转换头文件

37

//******角度参数************

float Angle_gx; //由角速度计算的倾斜角度 float Accel_y; uchar value;

//X轴加速度值暂存

//角度正负极性标记

float Angle_ay; //由加速度计算的倾斜角度 float Angle; //小车最终倾斜角度 float Gyro_x; //Y轴陀螺仪数据暂存

//******PWM参数*************

int PWM_R; //右轮PWM值计算 int PWM_L; //左轮PWM值计算 float PWM; //综合PWM计算 float PWMI;

//PWM积分值

int speed_mr; int speed_ml; char turn_need=0; char speed_need;

//******电机参数*************

float speed_r_l; //电机转速 float speed; //电机转速滤波 float position;

//******定时器，外部中断初始化************* void InitTimer0(void) {

TMOD = TMOD|0x01; TH0 = 0x0B1; TL0 = 0x0E0; EA = 1; ET0 = 1; TR0 = 1; }

void Init_exint(void) {

IT1=1;

IT0=1; EX1=1; EX0=1;

//位移

float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;

//右电机转速 //左电机转速

38

}

//******角度，角速度处理函数********** void Angle_Calcu(void) {

//-------角速度------------------------- }

//********************************************************* //电机转速和位移值计算

//*********************************************************

void Psn_Calcu(void) {

speed_r_l =(speed_mr + speed_ml)*0.5; speed *= 0.7;

//车轮速度滤波

//积分得到位移

speed += speed_r_l*0.3; position += speed; position += speed_need;

if(position<-6000) position = -6000; if(position> 6000) position = 6000;

Kalman_Filter(Angle_ay,Gyro_x); //卡尔曼滤波计算倾角

Gyro_x = GetData(GYRO_XOUT_H); //静止时角速度Y轴输出为-30左右 Gyro_x =(Gyro_x-8) /16.4; //去除零点偏移，计算角速度值,负号为方向处理 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.

//-------卡尔曼滤波融合-----------------------

//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s) Accel_y = GetData(ACCEL_YOUT_H);

//读取X轴加速度

Angle_ay = (Accel_y - 1100) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ay = Angle_ay*1.2*180/3.14; //弧度转换为度, //范围为2g时，换算关系：16384 LSB/g

//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大 //------加速度--------------------------

39

}

//********************************************************* //电机PWM值计算

//*********************************************************

static float code Kp = 9.0; //PID参数 static float code Kd = 2.6; //PID参数 static float code Kpn = 0.01; //PID参数 static float code Ksp = 2.0; //PID参数

void PWM_Calcu(void) {

if(Angle<-40||Angle>40) //角度过大，关闭电机 {

CCAP0H = 0; return; }

CCAP1H = 0;

PWM = Kp*Angle*K_angle_AD + Kd*Gyro_x*K_angle_dot_AD; //PID：角速度和角度 PWM += Kpn*position*K_position_AD + Ksp*speed*K_position_dot_AD; //PID：速度和位置 PWM_R = PWM - turn_need; PWM_L = -PWM - turn_need; PWM_output(PWM_L,PWM_R); }

//******主函数************* void main() {

InitMPU6050(); //初始化MPU6050 InitTimer0(); Init_exint(); serial_init(); AD_init(); PWM_init(); while(1) {

Angle_Calcu();

Psn_Calcu(); //电机位移计算 PWM_Calcu();

delaynms(500);

40

}

}

Sendstring(\ \Sendstring(\ \Sendstring(\Put_Ser_Dis(Gyro_x); Sendstring(\Sendstring(\Put_Ser_Dis(PWM); Sendstring(\Senddata(0x0d); Senddata(0x0a);

Put_Ser_Dis(Angle_ay);

//******定时器中断************* void Timer0Interrupt(void) interrupt 1 {

TH0 = 0x0B1; TL0 = 0x0E0;

speed_mr = speed_ml = 0; }

//********左电机中断*********************** void INT_L(void) interrupt 0 {

if(RA == 1) { speed_ml++; } else }

//********右电机中断***********************

void INT_R(void) interrupt 2 {

if(RB == 1) { speed_mr++; } else }

//右电机前进

//右电机后退

{ speed_mr--; }

//左电机前进 //左电机后退

{ speed_ml--; }

K_angle_AD=AD_work(0)/6.0; K_angle_dot_AD=AD_work(1)/6.0; K_position_AD=AD_work(6)/6.0; K_position_dot_AD=AD_work(7)/6.0;

//add your code here!

41

}

}

Sendstring(\ \Sendstring(\ \Sendstring(\Put_Ser_Dis(Gyro_x); Sendstring(\Sendstring(\Put_Ser_Dis(PWM); Sendstring(\Senddata(0x0d); Senddata(0x0a);

Put_Ser_Dis(Angle_ay);

//******定时器中断************* void Timer0Interrupt(void) interrupt 1 {

TH0 = 0x0B1; TL0 = 0x0E0;

speed_mr = speed_ml = 0; }

//********左电机中断*********************** void INT_L(void) interrupt 0 {

if(RA == 1) { speed_ml++; } else }

//********右电机中断***********************

void INT_R(void) interrupt 2 {

if(RB == 1) { speed_mr++; } else }

//右电机前进

//右电机后退

{ speed_mr--; }

//左电机前进 //左电机后退

{ speed_ml--; }

K_angle_AD=AD_work(0)/6.0; K_angle_dot_AD=AD_work(1)/6.0; K_position_AD=AD_work(6)/6.0; K_position_dot_AD=AD_work(7)/6.0;

//add your code here!

41

本文来源：https://www.bwwdw.com/article/4mjp.html