两轮系统是一种高度不稳定、非线性及多干扰系统，要使两轮系统达到自平衡，其模型决定了系统的重心须在两车轮轴线上。在两轮小车自平衡控制过程中，小车姿态倾角测量的准确性与实时性决定了小车的控制精度及稳定性,仅仅依靠陀螺仪或者加速度计对两轮小车姿态倾角测量，其测量误差及振动干扰较大，往往达不到两轮小车的自平衡控制。本文提出了基于陀螺仪与加速度计数据融合的两轮自平衡小车控制方法，由陀螺仪与加速度计建立姿态倾角测量系统，通过卡尔曼滤波器进行数据融合得到小车高精度、高可靠性的姿态倾角，最后利用数字PID控制算法实现两轮小车的自平衡控制[1]。

1 两轮自平衡小车的动力学模型
    两轮自平衡小车主要由车体、核心控制板、电机驱动板和车轮等部件构成，小车两轮各参数（半径、转动惯量、质量等）相同且两轮共轴，由两直流电机独立驱动。图1为小车模型[2]，假设小车质量为m、车轮半径为r、车轮前进方向移动速度为v、车体自身水平作用力为H、车轮所受地面摩擦力为Hf、车轮对车轴的等效转动惯量为J、车体对车轮等效转动惯量为I、车轮转速为ω、电机电磁转矩为Tm、车体质心位置坐标(x,y)、车体质量ma、车体与垂直方向所成角度为φ、车轮对车身产生的扭矩为T。

    对车轮受力分析可知，车轮不仅受直流电机的输出转矩、地面支持力和摩擦力的影响，同时由于惯性作用还受到车体自身作用力的影响。将车轮的运动分解为前进方向与绕轴方向的运动，车轮运动方程可描述为[2]：
  
2 卡尔曼滤波数据融合过程
    两轮小车姿态倾角的测量由陀螺仪与加速度计完成。陀螺仪测量的是旋转运动，输出角速度信号，旋转越快，其输出值越大，对角速度数据取积分，即能得到车体倾角值[3]。陀螺仪易受震动、温度和不稳定力矩等影响，产生漂移误差，计算小车姿态倾角时由于积分作用其测量误差会越来越大。加速度计测量的是线性运动，输出加速度信号，速度变化越快，输出值越大，通过三角变换可得到加速度计与重力方向的夹角[4]。加速度计没有累计误差，但是加速度计易受震动干扰。因此，仅单独使用陀螺仪或者加速度计对小车姿态进行测量，很难保证测量结果的精确性和可靠性。卡尔曼滤波是一种较好的多传感器信息融合算法。采用卡尔曼滤波算法对加速度计与陀螺仪所测数据进行优化处理，补偿加速度计的动态误差与陀螺仪的漂移误差，可得到精度与可靠性更高的小车姿态倾角。
    卡尔曼滤波算法是以最小均方误差估计的一套递推估计算法，其算法核心思想：采用信号与偏差的状态方程，利用前一状态得到的估计值和当前状态的测量值来更新对状态变量的预测与估计，最终求得当前状态的最优估计值。
    卡尔曼滤波过程可分为状态预测和测量修正两个过程。状态预测方程及时地由前一时刻的系统状态和噪声方差估计出当前时刻系统状态；测量修正方程则将当前时刻测量值结合状态预测过程得到的当前估计状态来得到系统最优估计值。
   
    图3为陀螺仪接口电路，陀螺仪选用微型角速度传感器ENC-03。由于陀螺仪输出电压信号较小，其输出信号经TLV2211放大6倍左右。图4为加速度计接口电路，加速度计选用3轴小量程加速度传感器MMA7361，其输出电压信号是小车运动方向与重力方向加速度的混合值。陀螺仪、加速度计接口电路均采用+5 V电源，与单片机STC12C5608AD供电电源相匹配，简化了硬件设计。陀螺仪、加速度计输出电压信号均由STC12C5608AD片内集成A/D进行采集。

3.2 软件设计
    图5为系统软件实现框图，系统软件主要包括小车姿态传感器数据采集程序、卡尔曼滤波程序、PID控制程序、直流电机PWM控制程序等部分。STC12C5608AD片内集成A/D转换和自带PWM输出功能,只需读写STC12C5608AD单片机内部与A/D转换和PWM相关的寄存器，就能完成对传感器的数据采集与直流电机的PWM控制。因此，系统软件关键在于卡尔曼滤波算法与数字PID算法的实现。

   
    采用双闭环数字PID对两轮系统进行自平衡控制，其控制结构如图6所示。对光电编码器产生的脉冲转换得到两轮实时速度信号，以该信号作为反馈量实现速度闭环控制；通过对陀螺仪与加速度计采集的数据进行融合得到小车倾角信息，以小车倾角作为反馈量实现小车姿态闭环控制[7]。这样就实现了两轮自平衡系统的双闭环数字PID控制，系统速度环与小车姿态环均采用位置型PID算法。

    本文介绍了基于数据融合的两轮小车自平衡控制系统的设计方法，在建立两轮自平衡小车的动力学模型的基础上，通过卡尔曼滤波算法有效地补偿震动、温度及不稳定力矩对陀螺仪与加速度传感器的影响，得到小车最优姿态倾角，最后通过双闭环数字PID控制算法实现两轮小车的自平衡控制。给予一定系统干扰，小车仍可实现自平衡，平衡时其倾角范围在±10°内。数据融合技术应用于两轮自平衡控制系统的方法，不仅提高了两轮系统的稳定性与抗干扰性，同时也为两轮系统姿态控制领域提供了一种新的设计思路[8]。
参考文献
[1] 冯智勇, 曾瀚, 张力,等. 基于陀螺仪与加速度仪新号融合的倾角姿态测量[J].西南师范大学学报(自然科学版), 2011(4):137-141.
[2] 李凡红. 两轮自平衡小车系统[D]. 北京:北京交通大学, 2010.
[3] 李俊博,朱涛,邹艳忠.陀螺稳定系统参数测试仪设计[J].计算机测量与控制,2011,19(2):481-482.
[4] 于玮, 封维忠, 武建军. 基于MEMS加速度传感器的数字水平仪[J].仪表技术与传感器,2011(1):24-26,48.
[5] 王光林. 两轮电动车自平衡控制算法的研究[D]. 广州：华南理工大学, 2011.
[6] 代杰, 段晨东,李磊,等. 彩色液晶显示模块在智能仪表中的应用[J].工业仪表与自动化装置,2012(2):27-30.
[7] 周海旭. 两轮自平衡机器人系统设计[D]. 哈尔滨：哈尔滨工程大学, 2010.
[8] 夏玉宝,何辅云,葛飞.基于多传感器数据融合的漏磁信号采集与处理[J]. 电子技术应用, 2008(2):116-118.