小型双轮自衡车项目报告

1、小型双轮自衡车项目报告2013.11综述本报告介绍了有关该小型自衡小车的完整制作过程，包括硬件使用，简易模型构建及最终的实际工程算法实施。本项目旨在通过此次实践提高参与人员的工程实践能力，勾连各科目所学知识，为可能的后续研究提供原型参考。该车所占空间体积可近似认为是11cm*11cm*11cm的正方体。由于此体积内搭载器件性能的有限性，本车的控制策略为简单的基于倾角误差的PD控制，其理论基础来源于单摆的自稳定原理，最终可实现基础的平衡维持。实现速度与转向控制并非不可能，但是需要对底盘进行改装以加装测速码盘。本车目前实现的功能为维持平衡直立状态，尚无法实现速度控制与转弯控制。然而平衡维持的性能同

2、样因为尚未对速度进行闭环控制而受到限制，本质上也受到了微型电机转矩和调速范围的极大限制。在较好的情况下，本车可维持自衡数分钟。PS：喜欢DIY的同志们欢迎参考此文档信息，但由于个人能力有限，未尽之处请查阅其他资料。同时欢迎观在优酷上搜索“小型双轮自衡车”以观看本项目演示视频。目录综述21、硬件组成介绍41.1小车框架41.2直流减速电机及车轮41.3陀螺仪和加速度计模块61.4控制器61.5驱动板及电源72、模型介绍与基本控制原理82.1 单摆及倒立摆82.2电机的加速度控制113.小车倾角测量124.程序编辑及算法实现13附录：卡尔曼滤波程序161、硬件组成介绍1.1小车框架本车的框架使用塑

3、料框架，其尺寸长宽均约为11cm，在合适的位置可打孔以通过螺丝安装需要的器件，包括检测器件，电机支撑架等等。1.2直流减速电机及车轮此处采用N20直流减速电机。此电机实际上是此底盘的配套电机，属于微型电机。其大小及性能参数可参见下图：（上图单位为mm）在本项目中选用减速比1:150的型号，工作电压由充电锂电池提供约8v。该减速比的选择的依据为：在此减速比下给电机施加从0到8V的阶跃电压可以使小车从完全倾倒的状态直接打回到直立状态。实际上该车装配完毕后的重量约270g，平衡时重心离地高度约4cm，完全倾倒时的倾角约为15°。从力矩供应来讲，要满足上述的挑选条件其减速比不应小于100 。

4、然而，当减速比过大时电机提供加速度的时间将会降低，故而实际操作时使用1:300的减速比也未能满足上述条件，使用1:150却可以。本车使用车轮也为该底盘及电机的配套车轮，其直径为42mm。最终与电机的装配图可见下：1.3陀螺仪和加速度计模块此处使用的陀螺仪及加速度计模块为InvenSense公司生产的MPU6050。该芯片内同时整合三轴加速度计和三轴陀螺仪模块，并支持I2C通信协议的快速模式，即传输速率可达400Kbit/s。陀螺仪模块使用时配置的量程为±2000/s，最小分辨大小约为0.061/s。加速剂模块使用时配置的量程为±2g，最小分辨大小约为0.000061g。以上

5、两个模块的数据更新频率均为1000Hz，陀螺仪数据输出延迟为0.98ms，加速度计数据输出无延迟。且输出值均已经过带宽为256Hz的滤波器。1.4控制器本车所使用控制器为STM32最小系统板。即该系统板使用芯片为ST公司生产的STM32f103RBT6芯片。该芯片为32位单片机芯片，其CPU主频可达到72MHz。外设丰富支持I2C通信，UART和USART通信等，程序总空间128K，RAM大小为20K。整个开发板的大小为64*52mm。1.5驱动板及电源由于本车所使用的电机功率有限，故而此处使用的驱动芯片为L293D，也无需加散热片。该芯片共有4个通道，支持的峰值输出电流为1.2A，每个通道额

6、定电流600mA，最大输出电压36V。由于单片机供电使用5V电压，故而此驱动板上还集成一78M05的降压芯片用以提供5V电压的输出。本车使用电池为两节神火14500充电电池，单节提供电压为3.7V，容量为800mAh，大小与普通5号电池相同。最终的硬件连接可由下图表示：2、模型介绍与基本控制原理2.1 单摆及倒立摆由于本车尚未实现速度及转向控制，故而此处只对平衡控制进行原理分析。小车的平衡控制的原理可从最常见的单摆模型中得到启发。对于单摆而言，即便受到外力干扰它也总能趋于竖直下垂的平衡状态。从这个现象中分析即可发现，促使这一现象产生的机理首先需要与偏移平衡位置成正比的回复力，其次还应有与速度成

7、正比的阻尼力。该回复力指向单摆竖直下垂的平衡位置，故而可以将球拉回平衡位置。但是仅有此力，单摆会处于无限制的震荡状态。实际的单摆还处在阻尼介质中，故而会逐渐削弱系统能量从而令球稳定在平衡位置。对于小车而言，可以将其看做是底部可以运动的倒立单摆。其简单的受力分析可见图2.2 。图2.1车模简化为倒立摆但是值得一提的是，运用分析直线运动的思维来进行分析是不确切的。右图的受力分析中明显缺少了杆子的支撑力。此支撑力由于由刚性的支撑杆提供，故而其方向和大小都是不确定的。不像单摆中的绳子，其提供拉力的方向只能是沿绳子方向。 而且单摆中所定义的回复力是垂直图2.2 以车轮为参考系的受力分析 指向平衡位置的力

8、。总而言之，该处使用的模型是不确切的。更加确切的模型需要运用有关刚体转动的知识，涉及力矩和转动惯量等等。此处无意将问题复杂，况且此模型分析由飞思卡尔比赛委员会（即是指全国大学生“飞思卡尔”杯智能汽车竞赛委员会。该处原理分析可参见该比赛发布的电磁组直立行车参考设计方案（版本2.0）给出，后经本项目实际验证可行，故而暂且以这种通俗易懂的方式进行分析。从上图可以看出假如以车轮为参考系，那么当车轮向前加速运动时，上半部的小球就会受到与轮子加速方向相反的惯性力，大小即由牛顿第二定律给出。将此力与重力都分解在与杆子垂直的方向上，则认为此二分力的合力为该小球最终的回复力：F=mgsin-macos将用弧度表

9、示，则在小角度（最宽泛的观点认为在0-30°内的误差都是可以接受的）内可以认为sin，同时将第二项简化则可得到下面的式子：F=mgsin-macosmg-mk1当然有关此处第二项的化简是无法用小角度内以弧度制角替换余弦值来解释的。实际上，此处只是进行了一个线性化处理，也即该处认为acosk1,很明显该处的近似使得两项的导数变化相差很大，前者对求导得到asin而后者即为k1（是个常数）。而k1实际上不管如何取值，与近似前的误差也是相当大的。总之，此处的近似存在问题，但是由于解释k1的用处涉及到此后的控制算法，故而此处先暂且认为这种近似是合理的。上式中如果k1足够大，那么回复力将会为负（

10、有关正负方向的定义可参见图中正负的标识），换言之，此力的方向即指向了平衡位置，从而满足了车子保持在平衡位置的基本条件。此外，正如在单摆中分析的那样，为了使小球尽快的平衡下来摆脱震荡的状态，还需要加入阻尼力，此力的大小应该与小球运动的速度成正比，方向与速度方向相反。因而最终的回复力应该具有以下形式：F=mgsin-macosmg-mk1-mk2于是抵抗车子重力的惯性力应该是：F抗=mk1+mk2最后，小车的加速度即可由上式推出：a车=k1+k2上式即为基本的控制算法，由得到的小车倾角和小车倾斜角度进行比例放大后输出为电机的加速度。换言之，也就是对小车倾角的PD控制。于是k1、k2也就是PD控制中

11、的比例系数和微分系数。得知k1、k2的意义之后，才回过头来看acosk1。此式的近似虽然大有问题，但是保证了将问题化归为最简单的PD控制。如果近一步的，我们认为k1可变、不为常数则可得到如下的变换：F=mgsin-macos=mgsin-mk1'tancos=mgsin-mk1'sinmg-mk1'式中k1'=a/tan加入阻尼之后即为下式：F=mgsin-macosmg-mk1'-mk2于是抵抗车子重力的惯性力应该是：F抗=mk1'+mk2最后，小车的加速度即可由上式推出：a车=k1'+k2而k1'=a/tan。同时注意，为了保

12、证此惯性力指向平衡位置，实际最后k1'的数值应该取绝对值，即k1'=atan 。于是最后得到的是比例系数随测量角变化的PD控制，待确定的参数仍然只有两个：a和k2。经本项目验证，使用比例系数的不变的PD控制可以达到平衡效果，使用比例系数可变的PD控制也可，效果相差不多，但是由于使用到了除法，可能会放大角度测量误差，实际上车子的控制作用变强了，抖动频率和幅度都有所增加。2.2电机的加速度控制前一部分提到，最终需要控制的是车子的加速度。然而一般来讲，电机的控制都集中在速度控制，如何实现电机的加速度控制是本部分要解决的问题。此处使用的电机为直流减速电机。所谓减速，其实是通过加装减速箱

13、以实现速度降低从而提高可输出的扭矩。此处所使用的同一型号电机并不是扭矩越大越好，扭矩大意味着减速比大，即调速范围被大大压缩。调速范围的压缩有时比力矩不足更加致命，因为当这个电机很容易就进入满速工作时，那么就再也无法通过提高转速来施加加速度了，于是系统就进入了失控状态。对于直流电机，其速度变化曲线可参考下图：图2.3电机在不同电压下的速度变化曲线从上图可以看出，电机的加速度可以通过控制输出给电机的阶跃电压来控制。而上图中的加速度段随电机型号不同而不同，与减速比、电机的转动惯量等因素有关。本项目中电机的加速度段时间约为60ms，一般电机在十几到几百个毫秒之间。只有电机处于加速运行状态系统才会受到惯

14、性力借以维持平衡，故而为了达到这个目的将控制周期压缩到几个毫秒的长度。因而可以认为在控制周期内系统处于加速阶段，也就是可控的。当然此时的控制器输出即为电压值即可，无需阶跃电压：电机得到的电压=k3*a车其中k3为放大倍数，a车即为前部分算得的加速度。当然，控制周期的缩短只能从一定程度上避免电机调速范围不够带来的加速度失控状态。如果小车不能及时纠正倾斜状态，那么电机的电压会长时间处于某一个较高的数值，也就是说电机会进入较高转速下的匀速状态，那么此时就无法再行控制了，因为已经无法通过提高较大幅度的转速来获得所需的加速度了。3.小车倾角测量本项目中小车的倾角测量使用加速度计和陀螺仪的卡尔曼融合滤波。

15、使用融合滤波是因为不论加速度计还是陀螺仪都有各自明显的优缺点：加速度计： 通过测量重力加速度在测量轴上的分量可直接反算获得绝对倾角。 受芯片的加速运动和其他机械振动影响严重。陀螺仪： 直接测量芯片转动角速度，积分可得倾角偏量。 对机械振动，及加速运动不敏感。 由于需要积分，故而零漂的影响极其严重。实际上，各种方法最终的思路都是以陀螺仪为核心，以加速度计为辅助清除零漂。由于卡尔曼滤波已经有了流程化的使用方法，此处仅将所需的系统方程和观测方程列出：系统方程：angle(k)q_bias(k)=1-dt01angle(k-1)q_bias(k-1)+dt000rate_gyro0+Q_angleQ_

16、gyro观测方程：Zk=1 0anglekq_bias(k)+R_angle式中q_bias(k)为陀螺仪的零点误差，angle(k)为测量角，rate_gyro为陀螺仪所测角速度，Q_angle、R_angle均为角度测量的方差，Q_gyro为陀螺仪测量的方差。此外，飞思卡尔提供的方案中给出了另一个更加简单的角度测量方法：本项目中未使用该种方法，因为由卡尔曼滤波得到的程序在收敛速度和准确度上都满足要求。卡尔曼滤波的程序由网上的开源代码获得，实际运行时做了一些修改，将会在下部分加以说明。4.程序编辑及算法实现控制器中的程序实际上十分简单：main()系统初始化（）；/时钟及外设初始化卡尔曼滤波

17、初始化（）；/先在静止状态下运行500次，以进行角度的初始收敛while(1) 卡尔曼滤波得到倾角及角速度（）； if (控制周期时间到) 对倾角和角速度进行均值滤波（）；计算倾角与设定角的误差（）；阈值判定（）；PD计算、输出电机电压（）；输出限幅（）； 在以上的程序中，卡尔曼滤波初始化包括了对陀螺仪零点误差的测量。虽然在前部分中此零点误差在系统方程中，按理说可以进行自动的追踪。但是实际操作时由于此追踪实际上是受到加速度计倾角值的直接影响，故而在程序中屏蔽了有关此值的更新以避免加速度计对其干扰。陀螺仪的零点误差对时间并不敏感，对温度比较敏感，故而实际运行时此零点误差在初始化时测量一次即可。此

18、外实际操作时，加速度计由于受到小车晃动带来的加速度过于明显，故而利用其读数校正陀螺仪读数时也做了限幅处理。即当加速度计和陀螺仪两个读出的倾角相差大于0.5度时，一律认为是0.5度 。注意此数的单位不是弧度。在程序中该限幅处理语句已经经过了加粗处理，有关该开源程序的具体原理此处不再赘述。有关PD控制。PD控制毫无疑问是需要给出设定值的。从前面的原理分析中认为直接对角度进行PD控制即可，实际上即是认为角度设定值为0.实际操作时，由于小车的对称性等影响，将此设定值设为0并不一定是真正能使小车平衡的位置，需要根据实际的效果进行调整。有关PD参数的整定，不论使用可变比例系数还是不变比例系数都需要两个参数

19、进行整定，除此之外还需要整定一个从小车加速度到最终电压输出的比例放大倍数k3。此k3直接与控制器控制PWM输出的占空比有关，也就是在此处使用的调压手段为PWM控制，输出占空比可调的方波经过滤波之后则可实现连续的变压。具体到单片机上，PWM占空比设置是改变单片机计时器的比较值，PWM频率的调节是改变计时器的装载值。一般而言，当比较值等于装载值时则输出高电平，即是满电压输出，k3的实际上与计时器装载值有很大关系。但是这种明确的数量关系无需过分分析。有关上述三个参数的整定，本项目使用以下的顺序：首先将小车完全倾倒，调节比例系数和k3,保证小车上电后可以从此倾倒状态恢复直立。其次，再加入微分系数调节系

20、统的震荡。最后，对比例和k3进行微调以达到满意的效果。最终算法输出的限幅不是必须的，因为对某些单片机而言比较值高于装载值不会报错，当做等于处理。附录：卡尔曼滤波程序/*/* kalman.c */* 1-D Kalman filter Algorithm, using an inclinometer and gyro */* Author: Rich Chi Ooi */* Version: 1.0 */* Date */ /* Adapted from Trammel Hudson(hudson) */ /* - */* Compilation procedure: */* Linux */

21、* gcc68 -c XXXXXX.c (to create object file) */* gcc68 -o XXXXXX.hex XXXXXX.o ppwa.o */*Upload data : */* ul filename.txt */*/* In this version: */*/* This is a free software; you can redistribute it and/or modify */* it under the terms of the GNU General Public License as published */* by the Free S

22、oftware Foundation; either version 2 of the License, */ /* or (at your option) any later version. */* */* this code is distributed in the hope that it will be useful, */* but WITHOUT ANY WARRANTY; without even the implied warranty of */* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *

23、/* GNU General Public License for more details. */* */ /* You should have received a copy of the GNU General Public License */* along with Autopilot; if not, write to the Free Software */* Foundation, Inc., 59 Temple Place, Suite 330, Boston, */* MA 02111-1307 USA */*/#include <math.h>#include

24、 "eyebot.h"/* * The state is updated with gyro rate measurement every 20ms * change this value if you update at a different rate. */static const float dt = 0.02;/* * The covariance matrix.This is updated at every time step to * determine how well the sensors are tracking the actual state.

25、*/static float P22 = 1, 0 , 0, 1 ;/* * Our two states, the angle and the gyro bias.As a byproduct of computing * the angle, we also have an unbiased angular rate available.These are * read-only to the user of the module. */float angle;float q_bias;float rate;/* * The R represents the measurement cov

26、ariance noise.R=EvvT * In this case,it is a 1x1 matrix that says that we expect * 0.1 rad jitter from the inclinometer. * for a 1x1 matrix in this case v = 0.1 */static const float R_angle = 0.001 ;/* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how

27、much we trust the inclinometer * relative to the gyros. */static const float Q_angle = 0.001;static const float Q_gyro = 0.0015;/* * state_update is called every dt with a biased gyro measurement * by the user of the module. It updates the current angle and * rate estimate. * * The pitch gyro measur

28、ement should be scaled into real units, but * does not need any bias removal. The filter will track the bias. * * A = 0 -1 * 0 0 */void stateUpdate(const float q_m) float q; float Pdot4; /* Unbias our gyro */ q = q_m - q_bias;/当前角速度：测量值-估计值 /* * Compute the derivative of the covariance matrix * (equ

29、ation 22-1) * Pdot = A*P + P*A' + Q * */ Pdot0 = Q_angle - P01 - P10; /* 0,0 */ Pdot1 = - P11; /* 0,1 */ Pdot2 = - P11; /* 1,0 */ Pdot3 = Q_gyro; /* 1,1 */ /* Store our unbias gyro estimate */ rate = q; /* * Update our angle estimate * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q

30、* dt */ angle += q * dt;/角速度积分累加到 估计角度 /* Update the covariance matrix */ P00 += Pdot0 * dt; P01 += Pdot1 * dt; P10 += Pdot2 * dt; P11 += Pdot3 * dt;/* * kalman_update is called by a user of the module when a new * inclinoometer measurement is available. * * This does not need to be called every tim

31、e step, but can be if * the accelerometer data are available at the same rate as the * rate gyro measurement. * * H = 1 0 * * because the angle measurement directly corresponds to the angle * estimate and the angle measurement has no relation to the gyro * bias. */void kalmanUpdate(const float incAn

32、gle) /* Compute our measured angle and the error in our estimate */ float angle_m = incAngle; float angle_err = angle_m - angle;/1.12 zk-H*xk_dot if(angle_err>0.5) angle_err=0.5; else if(angle_err<-0.5) angle_err=-0.5; /* * h_0 shows how the state measurement directly relates to * the state estimate. * * H = h_0 h_1 * * The h_1 shows that the state measurement does not relate * to the gyro bias estimate.