两轮自平衡车

两轮自平衡送餐车

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

【关键字】：加速度计陀螺仪卡尔曼滤波PID控制调节电磁传感器GPS模块

【Abstract】：This is a project of "research and Realization of a two wheeled self balancing robot car room" 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 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)

第二章基本原理 (3)

2.1两轮自平衡送餐车整体框架 (3)

2.2送餐车直立控制 (3)

2.3速度控制 (4)

2.4方向控制 (5)

第三章硬件电路设计与实现 (5)

3.1主控芯片 (5)

3.2电机驱动方案 (5)

3.3电磁信号处理电路 (6)

3.4红外遥控模块 (6)

第四章系统软件控制流程图 (7)

第五章卡尔曼滤波 (8)

4.1卡尔曼滤波简介 (8)

4.2卡尔曼滤波实现的效果 (8)

4.3卡尔曼滤波原型 (9)

4.4卡尔曼滤波化简 (10)

4.5卡尔曼滤波参数调试 (12)

5总结 (13)

第一章引言

两轮自平衡送餐车具有运动灵活、智能控制、操作简单等优点。因此它适用于在狭小空间内运行，可以穿梭于餐厅的任意的位置，也可以将自平衡电动车改装成两轮自平衡机器人。由于两轮自平衡送餐车的两轮结构，使得它的重心在上、支点在下，故在非控制状态（或静态）下为一不稳定系统。然而，可以利用倒立摆系统的控制原理，通过微处理器的控制使它能够如倒立摆一样稳定在一个平衡位置处，并能在保持平衡的状态下按照使用者的指令要求正常运行。两轮自平衡送餐车实际上是一级直线式倒立摆和旋转式倒立摆的结合体，它的控制原理与倒立摆系统的基本一致。更形象地说，自平衡送餐车的工作原理更像人行走的过程。对于人而言，当人体的重心向前倾斜并失去平衡时，人通过自身的感觉器官能够察觉到自己身体的倾斜（角度），于是他会做出一个反应——向前迈出一步来防止自己摔倒在地上。如果身体一直前倾，为了保持平衡，人就会一步又一步地往前走。因此，如果将两个由电机驱动的车轮看成人的双腿，再加上能够测量车体相对于水平面倾角大小和速度的传感器，最后通过微处理器的控制便能够实现车体自平衡的效果。因而当人站在车上时，只要将身体带动车体一起往前倾（或后倾）就可以实现送餐车送餐前进（或后退）。

因此两轮自平衡送餐车具有很大的开发意义，实现两轮送餐车后还可以做很多的其他的项目，比如两轮自平衡电动车等等。

第二章基本原理

2.1两轮自平衡送餐车整体框架

图1系统框架图

2.2送餐车直立控制

世界上还没有任何一个天才杂技演员可以蒙着眼睛使得木棒在自己指尖上直立，因为没有了眼睛观察进行负反馈，当木棒在人的指尖上直立时，眼睛观察到木棒的倾斜角度和倾斜趋势（角速度），相应的通过人脑做出反应，使手掌移动以抵消木棒的倾斜角度和趋势，从而保持木棒的直立。这就形成了反馈机制。其反馈机制如图2

MC9CXS128

红外遥控模块

电磁检测和GPS

模块 电机驱动模块

角度传感器和加速度传感器模块

图2反馈机制

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

图3 通过车轮运动保持车模平衡

2.3速度控制

对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。要实现对车模速度的控制，必须测量车模的实时速度，改变车模的倾角，以及如何根据速度误差控制车模倾角。

安装在电机输出轴上的光电编码器来测量得到车模的车轮速度。通过给定车模直立控制的设定值，在角度控制调节下，车模将会自动维持在一个角度。在车模直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得车模往前倾角需要更大。如此循环，车模很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。如图4所示。

图4车模倾角控制速度中的正反馈

车模的速度控制本质上是通过调节车模的倾角实现的，由于车模是一个非最小相位系统，因此该反馈控制如果比例和速度过大，很容易形成正反馈，使得车模失控，造成系统的不稳定性。因此速度的调节过程需要非常缓慢和平滑。

2.4方向控制

实现送餐车方向控制是保证车模沿着送餐道路的关键。直立送餐车所在的电磁组的道路中心线铺设有一根漆包线，里面通有100mA的20kHz交变电流。因此在送餐道路中心线周围产生一个交变磁场。通过道路电磁中心线偏差检测与电机差动和GPS模块实现方向控制，从而进一步保证送餐路上。

（1）电磁中心线的偏差检测

道路电磁中心线检测简单的方法可以通过安装在送餐车前方的两个电磁感应线圈实现。线圈一般采用10mH的工字型电感。

（2）电机差动控制

利用电磁线偏差检测信号分别与送餐车速度控制信号进行加和减，形成左右轮差动控制电压，使得送餐车左右轮运行角速度不一致进而控制车模方向。(3)GPS模块

根据GPS模块上读到精确的经纬度值，实现坐标的定位，从而实现自动送餐的功能。

第三章硬件电路设计与实现

3.1主控芯片

3.2电机驱动方案

我们采用BTS7960芯片作为电机驱动芯片。BTS7960通路内阻为16m4，最大电流为43A，具有集成度高、外接电路少、内阻小等特点，因此，我们采用4片BTS7960芯片组成全桥驱动电路进行驱动。BTS7960电路图如图5所示

图5BTS7960电路图5

3.3电磁信号处理电路

路线的导线中通有20KHz的方波信号，通过检测方波信号的强弱来判断导线之于车模的相对位置，以此做出控制。为了检测信号强弱，使用电感电容构成谐振电路。电感使用10mH工字电感，电容使用6.4nF电容。

图6电磁检波放大电路原理图

3.4红外遥控模块

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

码由8位二进制组成，共256种．地址码重发了一次。主要是加强遥控器的可靠性．如果两次地址码不相同．则说明本帧数据有错．应丢弃。不同的设备可以拥有不同的地址码．因此。同种编码的遥控器只要设置地址码不同，也不会相互干扰。地址码为十六进制的0EH(注意低位在前)。在同一个遥控器中．所有按键发出的地址码都是相同的。数据码为8位，可编码256种状态，代表实际所按下的键。数据反码是数据码的各位求反，通过比较数据码与数据反码．可判断接收到的数据是否正确。如果数据码与数据反码之间的关系不满足相反的关系．则本次遥控接收有误．数据应丢弃。在同一个遥控器上，所有按键的数据码均不相同。数据码为十六进制的0CH，数据反码为十六进制的0F3H(注意低位在前)．两者之和应为0FFH。

单片机遥控接收电路红外遥控接收可采用较早的红外接收二极管加专用的红外处理电路的方法。如CXA20106，电路复杂，现在一般不采用。较好的接收方法是用一体化红外接收头，它将红外接收二极管、放大、解调、整形等电路做在一起，只有三个引脚．分别是5V电源、地、信号输出。红外接收头的信号输出接单片机的INTO或INTl脚，增加了一只PNP型三极管对输出信号进行放大。

第四章系统软件控制流程图

如图7所示，为系统软件整体结构框图，主要包括：系统参数的手动调节；电磁传感器；角度传感器和陀螺仪模块；路径控制模块；电机控制模块。

图7系统软件工作流程图

系统接通电源后，经初始化；通过系统参数的手动调节，使系统能够按照准确的设想方式运行；然后通过电磁传感器采集数据和角度传感器及陀螺仪对车模与地面的角度及角速度角度的感知，经过相应处理得出当前送餐路径和送餐车的信息；同时，电机测速模块测得模型车当前的运行速度，反馈给系统；最后，经

路径控制系统综合当前赛道路径信息和送餐车速度值作出相应的处理，来控制电机的运行。

第五章卡尔曼滤波

4.1卡尔曼滤波简介

近年来被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，本次用于两轮自平衡车很好的实现了功能。

4.2卡尔曼滤波实现的效果

当决定使用加速度计（反应角度）以及陀螺仪（反应角速度）以后，我们对参考的论文中为何要同时使用加速度计以及陀螺仪产生过一些疑问。因为如果需要角速度，我们可以通过这样的方法对加速度计进行微分，计算出角速度，或者可以通过对于陀螺仪积分，从而获得角度。既然如此为何我们需要使用两个传感器。

于是，我们对两个传感器进行了数据采样，观测其输出信号的关系。由于考虑电机转动产生的震动会对传感器产生比较大的干扰，因此我们测试了不同PWM占空比下传感器输出，并用MATLAB绘制曲线，并进行相关运算。

陀螺仪的数据测试:

图8PWM 20%占空比下，车模直立状态下的陀螺仪输出