关键词：两轮自平衡车  卡尔曼滤波算法  加速度值  角速度值  陀螺仪

中图分类号：TM315

The Multi-sensor Information Fusion Research of Self-Balancing Two-Wheeled Electric Vehicle Based on EKF

Abstract To solve the random drift error problem for MEMS inertial sensor in self-balancing two-wheeled electric vehicle gesture measuring, the EKF practical filtering algorithm was used to realize the information fusion of accelerometer and gyroscope. According to the experiment result of inertial sensor error characteristics, using Levenberg – Marquardt nonlinear least-squares iteration fitting data, and establishing the mathematical error models to compensate the random error of the acceleration sensor and the temperature drift error of the gyroscope, then the posture signal of self-balancing electric vehicle can be estimated optimally. The experimental based on REKF information fusion result shows that the self-balancing electric vehicle posture signal optimal estimation is effective and feasible, and it is beneficial to the two-wheeled electric vehicle self-balancing control.

keywords：Self-balancing two-wheeled electric vehicle, Kalman filtering, algorithm for acceleration value, angular velocity, gyroscope

1  引言

近年来，两轮自平衡电动车利用双轮驱动的特点，实现两轮自平衡行走，其以行走灵活、便利、节能等特点得到了很大的发展。国内外有很多这方面的研究，并出现了相应的产品。

在国外，从上世纪八十年代初出现关于两轮自平衡小车的研究，主要代表研究如下[1-2]：1986年，日本电子信息通讯大学教授KazuoYamafuji由倒立摆模型制作了一台自平衡车，但其只能在特定轨道上行驶，无法实现回转、爬坡等复杂动作，值此开创了两轮自平衡车的先河^[3-4]。2001年，美国发明家Dean Kamen与他的DEKA研发公司联合发布Segway的两轮自平衡车，第一次实现了两轮自平衡车的量产，将两轮自平衡车推向市场，使得两轮自平衡车技术日趋成熟，被乔布斯誉为划时代的科技发明。2002年，瑞士联邦工业大学工业电子实验室的Felix Grasser等人研制出可以远距离遥控操作的双轮移动自平衡机器人JOE，其在硬件方面采用了DSP处理器^[5]。2003年，西澳大利亚大学机械工程学院的RichChi Ooi研制了Eyebo，其采用线性二次型最优控制，在数据处理方面采用卡尔曼滤波器，使得实时数据较为准确。2006年，马来西亚理工大学的Ongyin Chee研制了一款两轮自平衡机器人，主要用于检验PID算法在平衡控制系统的效果。2009年，日本丰田公司Toyota推出的Winglet，并于2010年实现量产，成为另一个将两轮自平衡小车实现量产化的机构^[6]。

随着科技的快速发展，国内研究人员也相继开展电动两轮自平衡车的研究^[11-12]。2002年，河南科技大学机电工程学院联合洛阳北方企业集团有限公司对平行双轮电动车进行了攻关和开发研究。2003年7月制作出了第一台样机，实现了对小车的行走平衡控制^[7-8]，中国科技大学机械工程学院及自动化学院的课题组研发了的两轮自平衡小车Free Mover，在行进、减速、刹车以及回转等各种不同状态下，整车能够始终保持平衡稳定的状态。2005年，台湾逢甲大学研制一款电动两轮自平衡车，其只能对静止或不剧烈运动时的倾斜角度作出测量，是一款精简的两轮自平衡小车^[9]。哈尔滨工程大学尹亮也设计了双轮直立自平衡机器人Sway，其引入了方向摇杆及按键来对机器人进行方向操控，减小了操作上的难度。在国内，实现量产的两轮自平衡小车是由清华大学宋宇宁与日江电器合作所生产的Chegway，该车于2010年5月正式推出，并面向市场公开发售。

两轮自平衡车是一个典型的欠驱动、静态不稳定系统^[10-12],通过姿态传感器检测车体的倾斜角度与角速度, 将检测到的信号传递给控制器, 从而驱动两个电机从而实现自平衡运行。两轮自平衡车采用的姿态传感器一般是MEMS加速度计和陀螺仪，但加速度计用作倾角计测量角度对于震动很敏感，动态性能不理想。用倾角计测量车体的倾斜角度, 它静态性能好, 适用于测量缓慢变化的信息, 数据也存在误差。陀螺仪测量角速度的动态性能好, 适于测量快速变化的信息,但其容易受到噪声和温度的影响而产生漂移误差，测量数据经积分后得到的车体角度数据会存在累积误差，当其长时间工作时累积误差会无限增大, 导致系统无法正常工作。

鉴于MEMS传感器信息存在误差问题,一些学者研究对误差进行补偿^[13-14]。澳大利亚西澳大学机械工程学院 Rich Chi Ooi利用卡尔曼滤波方法进行补偿，但其没有考虑传感器的误差模型, 倾角滤波后误差还比较大^[15]。哈尔滨工业大学机器人研究所王晓宇等人提出用扩展卡尔曼滤波算法设计滤波器, 但其状态方程较为复杂难以得到快速响应^[16]。北京工业大学电子信息与控制工程学院郜园园利用卡尔曼滤波对倾角仪与陀螺仪的信息融合,补偿惯性传感器在两轮机器人姿态检测中存在随机漂移误差问题。

受MEMS陀螺仪目前精度较低的限制, 在使用陀螺仪之前, 必须要建立合理的随机漂移误差模型, 对陀螺仪误差进行估算与补偿。其中Ka l ma n 滤波是一种递推线性最小方差估计过程，是工程应用中较为成熟的方法，它可以实现最小均方估计误差意义下的随机信号的最优线性滤波。在对随机误差信号建模以后, 可建立较为准确的系统模型和观察模型, 通过[LHJ1] Kalman滤波器进行最优估计, 从而提高漂移信号的估计精度。

本文根据以上介绍MEMS惯性传感器存在的误差问题建立陀螺仪随机漂移的误差模型，建立陀螺仪的随机漂移模型后，融合加速度计和陀螺仪数据的扩展卡尔曼滤波算法滤除陀螺仪随机漂移， 解决随机漂移误差补偿与姿态最优估计问题，弥补硬件设计的高要求，完成电动两轮平衡车位姿的实时测量，提高电动两轮平衡车的稳定运行性能。

2 两轮自平衡车基本原理数学模型

2.1自平衡车运行原理

两轮自平衡车运行的动力来自于两个车轮的直流电机驱动，控制电机的转动速度。运动控制任务可分解成三个基本控制任务： 控制平衡：通过控制两个电机正反向运动保持直立平衡状态；控制速度：通过调节车的倾角来实现车速度控制，不同的倾角会引起不同的加减速，从而达到对于速度的控制，最终演变成通过控制电机的转速来实现车轮速度的控制；控制方向：控制两个电机之间的转动差速实现转向控制。直立和方向控制任务都是直接通过控制直立车两个驱动电机完成的。在实际控制中，是将控制车辆直立和方向的控制信号叠加在一起加载电机上，只要电机处于线性状态就可以同时完成上面两个任务。

2.2 自平衡车平衡控制分析

自平衡车会在轮子滚动的方向上发生倾斜，通过负反馈控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡。建立自平衡车的运动学和动力学数学模型，设计反馈控制来保证平衡。通过对比单摆模型来说明保持自平衡车的控制规律，自平衡车可以看成放置在可以左右移动平台上倒立的单摆。

单摆在偏移角度很小的情况下，恢复力与偏移的角度之间大小成正比，方向相反。在此恢复力作用下进行周期运动。总结单摆能够稳定在垂直位置的条件有两个：（1）受到与位移（角度）相反的恢复力；（2）受到与运动速度（角速度）相反的阻尼力。若使倒立摆能够直立，可通过增加外力使得恢复力与位移方向相反，从而实现直立。控制倒立摆底部车轮，使得它作加速运动。这样站在小车上（非惯性系，以车轮作为坐标原点）分析倒立摆受力，它就会受到额外的惯性力，该力与车轮的加速度方向相反，大小成正比。这样倒立摆所受到的恢复力为：

         (1)

式中，由于很小，所以可进行线性化。假设负反馈控制是车轮加速度与偏角成正比，比例为。如果比例，（重力加速度）那么恢复力的方向便于位移方向相反。按照上面的控制方法，可把倒立摆模型变为单摆模型，能够稳定在垂直位置。因此，可得控制车轮加速度的控制算法为：

                       (2)

式中，为车模倾角；为角速度；、均为比例系数；两项相加后为车轮加速度控制量。只要保证在，条件下，可以使得车辆像单摆一样维持在直立状态。其中两个控制参数决定了直立车是否能够稳定到垂直平衡位置，它必须大于重力加速度；决定了车辆回到垂直位置的阻尼系数，选取合适的阻尼系数可以保证车模尽快稳定在垂直位置。

2.3平衡控制数学建模

将平衡车简化成高度为L，质量为m的简单倒立摆，放置在可左右移动的车轮上。假设外力干扰引起直立车产生角加速度。沿着垂直于底盘方向进行受力分析，可以得到倾角与车轮运动加速度以及外力干扰加速度与之间的运动方程。如图1所示为自平衡车简化模型：

图1自平衡车简化模型

Fig.1 The self-balancing vehicle simplified model

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

                       （3）

平衡车运动方程

  （4）

在角度很小时，运动方程简化为：

              （5）

小车静止时：

                  （6）

此时系统具有正负两个极点，一个极点位于平面的右半平面，因此直立车不稳定。引入比例、微分反馈之的

系统传递函数为：

              (7)

此时两个系统极点位于：

                 (8)

系统稳定需要两个极点都位于平面的左半平面。由此可以得出，当且时，自平衡车可以稳定，与前文通过分析所得结论一致。

3 主要控制元器件及模块

3.1 XS128单片机最小系统

MC9S12XS128MAA是十六位单片机，由十六位中央处理单元（CPU12X）、128KB程序Flash（P-lash）、8KB RAM、8KB数据Flash（D-lash）组成片内存储器。本文采用112引脚的MC9S12XS128MAA。其程序下载使BDM下载器，通过BDM下载器和CodeWarrior编译器能够实现程序的在线调试。

3.2 双路全桥驱动模块电路

采用四片BTS7960驱动芯片，每一块BTS7960形成一路半桥，可实现直流伺服电机向一个方向的速度控制。两块并联则形成一路全桥驱动电路，两组全桥电路分别引出两个PWM输入接口，用于接收单片机发出的PWM信号，经BTS7960防大[LHJ2] 功率实现对伺服电机直流线性控制。

3.3 MMA7361三轴加速度传感器

加速度传感器可以测量由地球引力作用或者物体运动所产生的加速度，本文所采用的MMA7361加速度传感器只需要测量一个方向的加速度值，就可以计算出车模的倾角，比如使用Z轴方向的加速度信号。自平衡车直立时，固定加速度器在Z轴水平方向，此时输出信号为零偏电压信号。当发生倾斜时，重力加速度便会在Z轴方向形成加速度分量，从而引起该轴输出电压变化。变化规律为。式中，为重力加速度；为车模倾角；为加速度传感器灵敏度系数。当倾角比较小的时候，输出电压的变化可以近似与倾角成正比。

理论上讲，只要依靠加速度传感器就可以知道自平衡车的倾角，然而在实际情况下，直立车的运动会使自身产生摆动。摆动所产生的加速度会产生很大的干扰信号，扰动信号叠加在测量信号上使得输出信号无法准确反映直立车的倾角。

当自平衡车倾角在两个角度位置过度，除角度变化信号之外，还存在由于运动引起的电压波动，如图2融合角所示，这个电压波动随着车模运动速度增加会变大。为了减少运动引起的干扰，加速度传感器安装的高度越低越好，但是无法彻底消除运动的影响。因此自平衡控制所需要的倾角信息需要通过另外一种器件获得，就是加速度传感器—陀螺仪。

图2 角度融合图

Fig.2  Angle fusion figure

3.4 光电编码器

采用欧姆龙200线光电编码器，可以输出正反双相，相位相差90度的方波脉冲信号，配合XS128单片机内置的脉冲计数寄存器，可以准确捕捉单位时间内光电编码器发出的脉冲个数，从而确定车轮转速，实现对直立车的PID闭环控制。

4 软件算法思想

系统初始化完成后，首先进入自平衡车检测程序。该程序通过读取加速度计的数值判断车体是否处于直立状态，如果一旦处于直立状态则启动车体直立控制、方向控制以及速度控制。

程序在主循环中不断发送监控数据，通过串口发送到上位机进行监控，同时判断车体倾角是否超过一定范围来检查车体是否倾倒。若车体倾倒，则停止自平衡车运行，包括车的直立控制、速度控制以及方向控制，然后重新进入车模直立判断过程。

自平衡车的直立控制、速度控制以及方向控制都在中断程序中完成。通过全局标志变量确定是否进行这些闭环控制。使用DSC的一个定时器产生1ms周期中断，中断服务程序任务被均匀分配在0-4的中断片段中。因此每个中断片段中任务执行的周期为5ms，频率200Hz。将任务分配到不同中断片段中，防止这些任务累积执行时间超过1ms，扰乱1ms中断的时序及任务之间的时间先后顺序。当然也可以将所有中断任务都合并在一起，使用一个5ms的中断来完成。这些任务包括：① 电机测速脉冲计数器读取与清除。累积电机转动角度和累积电机速度，为速度控制提供平均数。② 启动AD转换。进行20次模拟量采集，然后计算各个通道的模拟量的平均值。这个过程是对于模拟信号进行低通滤波。③ 平衡控制过程。包括平衡车角度计算、平衡控制计算、电机PWM输出等。④ 速度控制：在这个时间片段中，进行0-19计数。在其中第0片段中，进行速度PID调节，速度调节的周期为100ms，即每秒钟调节10次。⑤ 方向控制：根据ZigBee接收到遥控器的数据指令，计算偏差数值，然后计算电机差模控制电压数值。程序流程如图3。

           a)主程序流程图          b)  1ms定时器中断程序流程图    c) 串口接收中断程序流程

图3 程序流程图

Fig.3 The program flow