两轮自平衡车

1、两轮自平衡送餐车【摘要】：本项目为“两轮自平衡车送餐机器人”系统的研究与实现，从加速度计和陀螺仪传感器得出的角度。运用卡尔曼滤波优化，补偿陀螺仪的漂移误差和加速度计的动态误差，得到更优的倾角近似值。根据PID控制调节参数，实现两轮直立行走。通过电磁传感器对电磁线的检测和GPS模块精确定位，实现了平衡车的自动送餐功能。【关键字】：加速度 计陀螺仪 卡尔曼滤波PID控制调节 电磁传感器 GPS模块【Abstract】：This is a project of research and Realization of a two wheeled self balancing robot car roo

2、m system, from the accelerometer and gyro sensor of angle. Using the Calman filter optimization, the dynamic error of gyroscope drift error and acceleration compensation plan, to get better approximations angle. According to the PID control parameters, achieve two upright. Through the detection and

3、accurate positioning of GPS module electromagnetic sensors on the magnet wire, the balance of the car automatic room function.【Keyword】:saccelerometer gyroscope Calman filtering PID control electromagnetic sensor GPS module第一章引言2第二章基本原理32.1两轮自平衡送餐车整体框架32.2送餐车直立控制32.3速度控制42.4方向控制5第三章硬件电路设计与实现53.1主控芯片

4、53.2电机驱动方案53.3电磁信号处理电路63.4红外遥控模块6第四章系统软件控制流程图7第五章卡尔曼滤波84.1卡尔曼滤波简介84.2卡尔曼滤波实现的效果84.3卡尔曼滤波原型94.4卡尔曼滤波化简104.5卡尔曼滤波参数调试125总结13第一章引言两轮自平衡送餐车具有运动灵活、智能控制、操作简单等优点。因此它适用于在狭小空间内运行，可以穿梭于餐厅的任意的位置，也可以将自平衡电动车改装成两轮自平衡机器人。 由于两轮自平衡送餐车的两轮结构，使得它的重心在上、支点在下，故在非控制状态（或静态）下为一不稳定系统。然而，可以利用倒立摆系统的控制原理，通过微处理器的控制使它能够如倒立摆一样稳定在一个

5、平衡位置处，并能在保持平衡的状态下按照使用者的指令要求正常运行。两轮自平衡送餐车实际上是一级直线式倒立摆和旋转式倒立摆的结合体，它的控制原理与倒立摆系统的基本一致。更形象地说，自平衡送餐车的工作原理更像人行走的过程。 对于人而言，当人体的重心向前倾斜并失去平衡时，人通过自身的感觉器官能够察觉到自己身体的倾斜（角度），于是他会做出一个反应向前迈出一步来防止自己摔倒在地上。如果身体一直前倾，为了保持平衡，人就会一步又一步地往前走。因此，如果将两个由电机驱动的车轮看成人的双腿，再加上能够测量车体相对于水平面倾角大小和速度的传感器，最后通过微处理器的控制便能够实现车体自平衡的效果。因而当人站在车上时，

6、只要将身体带动车体一起往前倾（或后倾）就可以实现送餐车送餐前进（或后退）。因此两轮自平衡送餐车具有很大的开发意义，实现两轮送餐车后还可以做很多的其他的项目，比如两轮自平衡电动车等等。第二章基本原理2.1两轮自平衡送餐车整体框架MC9CXS128红外遥控模块电磁检测和GPS模块电机驱动模块角度传感器和加速度传感器模块图1系统框架图2.2送餐车直立控制世界上还没有任何一个天才杂技演员可以蒙着眼睛使得木棒在自己指尖上直立，因为没有了眼睛观察进行负反馈，当木棒在人的指尖上直立时，眼睛观察到木棒的倾斜角度和倾斜趋势（角速度），相应的通过人脑做出反应，使手掌移动以抵消木棒的倾斜角度和趋势，从而保持木棒的直

7、立。这就形成了反馈机制。其反馈机制如图2图2反馈机制车模平衡控制也是通过负反馈来实现的，与上面保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。如图2.2所示。图3 通过车轮运动保持车模平衡2.3速度控制 对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。要实现对车模速度的控制，必须测量车模的实时速度，改变车模的倾角，以及如何根据速度误差控制车模倾角。安装在电机输出轴上的光电编码器来测量得到车模

8、的车轮速度。通过给定车模直立控制的设定值，在角度控制调节下，车模将会自动维持在一个角度。在车模直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得车模往前倾角需要更大。如此循环，车模很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。如图4所示。图4车模倾角控制速度中的正反馈车模的速度控制本质上是通过调节车模的倾角实现的，由于车模是一个非最小相位系统，因此该反馈控制如果比例和速度过大，很容易形成正反馈，使得车模失控，造成系统的不稳定性。因此速度的调节过程需要非常缓慢和平滑。2.4方向控制实现送餐车方向控制是保证车

9、模沿着送餐道路的关键。直立送餐车所在的电磁组的道路中心线铺设有一根漆包线，里面通有100mA的20kHz交变电流。因此在送餐道路中心线周围产生一个交变磁场。通过道路电磁中心线偏差检测与电机差动和GPS模块实现方向控制，从而进一步保证送餐路上。（1）电磁中心线的偏差检测道路电磁中心线检测简单的方法可以通过安装在送餐车前方的两个电磁感应线圈实现。线圈一般采用10mH的工字型电感。（2）电机差动控制利用电磁线偏差检测信号分别与送餐车速度控制信号进行加和减，形成左右轮差动控制电压，使得送餐车左右轮运行角速度不一致进而控制车模方向。(3)GPS模块 根据GPS模块上读到精确的经纬度值，实现坐标的定位，从

10、而实现自动送餐的功能。第三章硬件电路设计与实现3.1主控芯片3.2电机驱动方案我们采用BTS7960芯片作为电机驱动芯片。BTS7960通路内阻为16m4，最大电流为43A，具有集成度高、外接电路少、内阻小等特点，因此，我们采用4片BTS7960芯片组成全桥驱动电路进行驱动。BTS7960电路图如图5所示图5BTS7960电路图53.3电磁信号处理电路路线的导线中通有20KHz的方波信号，通过检测方波信号的强弱来判断导线之于车模的相对位置，以此做出控制。为了检测信号强弱，使用电感电容构成谐振电路。电感使用10mH工字电感，电容使用6.4nF电容。图6电磁检波放大电路原理图3.4红外遥控模块编码

11、格式1、0和1的编码遥控器发射的信号由一串O和1的二进制代码组成不同的芯片对0和1的编码有所不同。通常有曼彻斯特编码和脉冲宽度编码。TC9012的O和1采用PWM方法编码，即脉冲宽度调制，其O码和1码如图1所示(以遥控接收输出的波形为例)。O码由O56ms低电平和056ms高电平组合而成脉冲宽度为112ms。1码由056ms低电平和169ms高电平组合而成脉冲宽度为225ms。在编写解码程序时通过判断脉冲的宽度，即可得到0或1。按键的编码 当我们按下遥控器的按键时，遥控器将发出如图2的一串二进制代码，我们称它为一帧数据。根据各部分的功能。可将它们分为5部分，分别为引导码、地址码、地址码、数据码

12、、数据反码。遥控器发射代码时均是低位在前，高位在后。分析可以得到引导码高电平为45ms，低电平为45ms。当接收到此码时表示一帧数据的开始。单片机可以准备接收下面的数据。地址码由8位二进制组成，共256种地址码重发了一次。主要是加强遥控器的可靠性如果两次地址码不相同则说明本帧数据有错应丢弃。不同的设备可以拥有不同的地址码因此。同种编码的遥控器只要设置地址码不同，也不会相互干扰。地址码为十六进制的0EH(注意低位在前)。在同一个遥控器中所有按键发出的地址码都是相同的。数据码为8位，可编码256种状态，代表实际所按下的键。数据反码是数据码的各位求反，通过比较数据码与数据反码可判断接收到的数据是否正

13、确。如果数据码与数据反码之间的关系不满足相反的关系则本次遥控接收有误数据应丢弃。在同一个遥控器上，所有按键的数据码均不相同。数据码为十六进制的0CH，数据反码为十六进制的0F3H(注意低位在前)两者之和应为0FFH。单片机遥控接收电路红外遥控接收可采用较早的红外接收二极管加专用的红外处理电路的方法。如CXA20106，电路复杂，现在一般不采用。较好的接收方法是用一体化红外接收头，它将红外接收二极管、放大、解调、整形等电路做在一起，只有三个引脚分别是 5V电源、地、信号输出。红外接收头的信号输出接单片机的INTO或INTl脚，增加了一只PNP型三极管对输出信号进行放大。第四章系统软件控制流程图如

14、图7所示，为系统软件整体结构框图，主要包括：系统参数的手动调节；电磁传感器；角度传感器和陀螺仪模块；路径控制模块；电机控制模块。图7系统软件工作流程图系统接通电源后，经初始化；通过系统参数的手动调节，使系统能够按照准确的设想方式运行；然后通过电磁传感器采集数据和角度传感器及陀螺仪对车模与地面的角度及角速度角度的感知，经过相应处理得出当前送餐路径和送餐车的信息；同时，电机测速模块测得模型车当前的运行速度，反馈给系统；最后，经路径控制系统综合当前赛道路径信息和送餐车速度值作出相应的处理，来控制电机的运行。第五章卡尔曼滤波4.1卡尔曼滤波简介近年来被应用于计算机图像处理，例如头脸识别，图像分割，图像

15、边缘检测等等。卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，本次用于两轮自平衡车很好的实现了功能。4.2卡尔曼滤波实现的效果 当决定使用加速度计（反应角度）以及陀螺仪（反应角速度）以后，我们对参考的论文中为何要同时使用

16、加速度计以及陀螺仪产生过一些疑问。因为如果需要角速度，我们可以通过 这样的方法对加速度计进行微分，计算出角速度，或者可以通过对于陀螺仪 积分，从而获得角度。既然如此为何我们需要使用两个传感器。于是，我们对两个传感器进行了数据采样，观测其输出信号的关系。由于考虑电机转动产生的震动会对传感器产生比较大的干扰，因此我们测试了不同PWM占空比下传感器输出，并用MATLAB绘制曲线，并进行相关运算。陀螺仪的数据测试:图8PWM 20%占空比下，车模直立状态下的陀螺仪输出图9PWM 60%占空比下，车模直立状态下的陀螺仪输出图10PWM 100%占空比下，车模直立状态下的陀螺仪输出 图11PWM占空比与陀

17、螺仪方差关系总结：随着PWM占空比的不断提高，陀螺仪的噪声也更加显著。4.3卡尔曼滤波原型首先我们建立系统的状态方程和测量方程，由于倾角和倾角角速度存在导数关系，系统倾斜真实角度 可以用来做一个状态向量。在该系统中，采用加速度传感器估计出陀螺仪常值偏差b，以此偏差作为状态向量得到相应的状态方程和观测方程 式中，为包含固定偏差的陀螺仪输出角速度，为加速度计经处理后得到的角度值，为陀螺仪测量噪声，为加速度传感器测量噪声，为陀螺仪漂移误差， 和相互独立，此处假设二者为满足正态分布的白色噪声。令为系统采样周期，得到离散系统的状态方程和测量方程： 同时，要估算k时刻的实际角度，就必须根据k-1时刻的角度

18、值，再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。在此,须知道系统过程噪声协方差阵Q 以及测量误差的协方差矩阵R, 对卡尔曼滤波器进行校正，Q与R矩阵的形式如下：式中。和分别是加速度传感器和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，数值越小表明信任程度越高。在该系统中陀螺仪的值更为接近准确值，因此取 的值小于 的值。当前状态: （1）4.4卡尔曼滤波化简上述五个公式便是卡尔曼滤波的五条数学公式在本项目中的使用。但是，以上五个公式，仅仅只是矩阵形式。虽然可以在MATLAB里进行仿真，但是却不能使用单

19、片机进行有效运算。然后，将卡尔曼滤波矩阵形式转化为方程形式：这里，X01即为我们需要的角度卡尔曼滤波值。可以看到的是，卡尔曼滤波方程形式共有14个公式，同时很多参数的运算涉及浮点数，这对于单片机的高效运行时极为不利的，因此我们需要通过一些方法对卡尔曼滤波的公式进行化简，从而在不失精度的情况下，尽可能减小运算量。需要注意的是，Q,R两个参数是关于传感器和系统的方差，他们随着系统的工作状况不同而会产生相应变化，对应到我们的系统，在车模运行状态不同（倾角不同，PWM不同）情况下，Q,R都是不同的。根据相关参考文献，Q,R虽然都是关于时间的变量，但是由于卡尔曼滤波有很好的收敛性，所以可以将Q，R都取比

20、较极端的参数。用常量来定义。然后再看下图注意方框中的公式，根据我们的观察，不难发现，整个方框中都是为了获得卡尔曼增益（矩阵Kg），我们设想，能否使用一个常数来等效替代卡尔曼增益那？根据我们在实验中的观察，卡尔曼增益是一个收敛的变量，并且针对到我们的这个系统，他的值非常小，直立状态下趋近于一个常数。（所有这些工作都在MATLAB下完成）所以，我们将方框中的所有公式完全省略，通过实验整定，选取一个近似Kg来替代方框中的所有运算。同时，通过NATLAB观测各变量的变化趋势，我们尝试让Kg2=0（Kg1，Kg2本身就是非常小的变量，所以可以让其等于0）。并发现Kg2对于整个卡尔曼滤波的影响非常有限。所

21、以将Kg2设定为0。以下就是我们组程序中卡尔曼滤波的简化算法实现X1=X0+gyro*dt;X0=X1+(acc-X1)*kg;化简后的卡尔曼滤波框图如下：图12卡尔曼滤波框图4.5卡尔曼滤波参数调试化简后的卡尔曼滤波主要是对dt以及Kg两个参数进行整定。需要特别指出的是，这里的dt并不只是一个采样间隔。由于陀螺仪的输出和加速度计输出的量纲并不相同，所以陀螺仪采样值dt并不直接反应一个角度，而是与实际角度相差一个系数。因此此处的dt可以等效理解为dt=采样间隔*比例系数。dt越大，积分速度越快，卡尔曼输出追随实际角度的情况越好（当然不能太大，不然可会出现超前相位）。但是dt越大，陀螺仪漂移造成的影响也就越大。Kg决定了加速度计的权重。Kg越大，实际输出的漂移就越小，但是滤波效果的噪声也就越大。 图13化简后的卡尔曼滤波dt和kg是一对矛盾，不能太过于极端。首先是静态整定。将车模保持在稳定直立状态，让车轮以恒定PWM（80%以上）转动，然后调节参数。目标是达到上图效果。红色为卡尔曼滤波输出，蓝色为加速度计直接输出。如果红色抖动非常厉害，可以适当减小Kg的大小。接着是动态整定还是保持车轮恒