两轮自平衡送餐车

【摘要】：本项目为“两轮自平衡车送餐机器人”系统的研究与实现，从加速度计和陀螺仪传感器得出的角度。运用卡尔曼滤波优化，补偿陀螺仪的漂移误差和加速度计的动态误差，得到更优的倾角近似值。根据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

图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%占空比下，车模直立状态下的陀螺仪输出

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

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

图11PWM占空比与陀螺仪方差关系

总结：随着PWM占空比的不断提高，陀螺仪的噪声也更加显著。

4.3卡尔曼滤波原型

首先我们建立系统的状态方程和测量方程，由于倾角和倾角角速度存在导数关系，系统倾斜真实角度 可以用来做一个状态向量。在该系统中，采用加速度传感器估计出陀螺仪常值偏差b，以此偏差作为状态向量得到相应的状态方程和观测方程

式中，为包含固定偏差的陀螺仪输出角速度，为加速度计经处理后得到的角度值，为陀螺仪测量噪声，为加速度传感器测量噪声，为陀螺仪漂移误差， 和相互独立，此处假设二者为满足正态分布的白色噪声。令为系统采样周期，得到离散系统的状态方程和测量方程：

同时，要估算k时刻的实际角度，就必须根据k-1时刻的角度值，再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。在此,须知道系统过程噪声协方差阵Q 以及测量误差的协方差矩阵R, 对卡尔曼滤波器进行校正，Q与R矩阵的形式如下：

式中。和分别是加速度传感器和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，数值越小表明信任程度越高。在该系统中陀螺仪的值更为接近准确值，因此取的值小于的值。当前状态:

（1）

4.4卡尔曼滤波化简

上述五个公式便是卡尔曼滤波的五条数学公式在本项目中的使用。但是，以上五个公式，仅仅只是矩阵形式。虽然可以在MATLAB里进行仿真，但是却不能使用单片机进行有效运算。

然后，将卡尔曼滤波矩阵形式转化为方程形式：

这里，X01即为我们需要的角度卡尔曼滤波值。

可以看到的是，卡尔曼滤波方程形式共有14个公式，同时很多参数的运算涉及浮点数，这对于单片机的高效运行时极为不利的，因此我们需要通过一些方法对卡尔曼滤波的公式进行化简，从而在不失精度的情况下，尽可能减小运算量。

需要注意的是，Q,R两个参数是关于传感器和系统的方差，他们随着系统的工作状况不同而会产生相应变化，对应到我们的系统，在车模运行状态不同（倾角不同，PWM不同）情况下，Q,R都是不同的。

根据相关参考文献，Q,R虽然都是关于时间的变量，但是由于卡尔曼滤波有很好的收敛性，所以可以将Q，R都取比较极端的参数。用常量来定义。

然后再看下图

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

接着是动态整定还是保持车轮恒定PWM旋转，同时，摇摆车身。

图14动态整定保持车轮恒定PWM旋转

大致调节到上图所示的样子。目前存在一个相位差，这个相位差在后期直立算法的调试中是致命的，必须克服掉。解决方法，逐渐增加dt即可。

图15上图效果即可认为卡尔曼滤波参数整定完成。

5总结

本项目实现了自平衡送餐功能，直立方面降低重心很重要，重心位置太高会容易引起侧翻，所以必须得降低重心，加速度传感器与陀螺仪放下来比较好，才能减少车模对它的震动，电磁传感器电容要贴近电感，才能减少分布电容对其影响。

参考文献

[1]. 李仕伯,马旭,卓晴. 基于磁场检测的寻线小车传感器布局研究. 电子产品世界, 2009(12),41-44

[2] 张昊飏,马旭,卓晴. 基于电磁场检测的寻线智能车设计. 电子产品世界

[3] 清华大学2队技术报告. //卓晴, 黄开胜, 邵贝贝. 学做智能车. 北京: 北航出版社, 2007.3

[4]. 卓晴 黄开胜 邵贝贝，学做智能车[M]，北京：北京航空航天大学出版社，2007

[5]. 邵贝贝，单片机嵌入式应用的在线开发方法，北京：清华大学出版社，2004

[6]. Freescale Semiconductor, Inc. MC9S12DG128 Device User Guide.

[7]. 童诗白，华成英．模拟电子技术基础[M]．北京. 高等教育出版社．2000

[8]冯智勇,曾瀚,张力,赵亦欣,黄伟.基于陀螺仪及加速度计信号融合的姿态角度测量[J].西南师范大学学报，2011,36（4）:137-141

[9]陆芳,刘俊.卡尔曼滤波在陀螺仪随机漂移中的应用[J]. 微计算机信息,2007,23(8-2):222-224

[10]陈静.两轮自平衡机器人模型及控制方法研究[D].北京工业大学硕士学位论文,2008.

本文来源：https://www.2haoxitong.net/k/doc/c3d1e3ea6aec0975f46527d3240c844768eaa042.html