基于数据融合的两轮自平衡小车控制系统设计*

2013-12-07段晨东高精隆李常磊

电子技术应用 2013年5期

魏文，段晨东，高精隆，李常磊

(长安大学电子与控制工程学院，陕西西安 710064)

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

1 两轮自平衡小车的动力学模型

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

对车轮受力分析可知，车轮不仅受直流电机的输出转矩、地面支持力和摩擦力的影响，同时由于惯性作用还受到车体自身作用力的影响。将车轮的运动分解为前进方向与绕轴方向的运动，车轮运动方程可描述为[2]：

对上述方程组消去Hf，车轮运动方程简化为：

对车体进行数学建模，其动力学方程可描述为[2]：

上式就是两轮自平衡小车关于车体倾角与角加速度的微分方程。

2 卡尔曼滤波数据融合过程

两轮小车姿态倾角的测量由陀螺仪与加速度计完成。陀螺仪测量的是旋转运动，输出角速度信号，旋转越快，其输出值越大，对角速度数据取积分，即能得到车体倾角值[3]。陀螺仪易受震动、温度和不稳定力矩等影响，产生漂移误差，计算小车姿态倾角时由于积分作用其测量误差会越来越大。加速度计测量的是线性运动，输出加速度信号，速度变化越快，输出值越大，通过三角变换可得到加速度计与重力方向的夹角[4]。加速度计没有累计误差，但是加速度计易受震动干扰。因此，仅单独使用陀螺仪或者加速度计对小车姿态进行测量，很难保证测量结果的精确性和可靠性。卡尔曼滤波是一种较好的多传感器信息融合算法。采用卡尔曼滤波算法对加速度计与陀螺仪所测数据进行优化处理，补偿加速度计的动态误差与陀螺仪的漂移误差，可得到精度与可靠性更高的小车姿态倾角。

卡尔曼滤波算法是以最小均方误差估计的一套递推估计算法，其算法核心思想：采用信号与偏差的状态方程，利用前一状态得到的估计值和当前状态的测量值来更新对状态变量的预测与估计，最终求得当前状态的最优估计值。

卡尔曼滤波过程可分为状态预测和测量修正两个过程。状态预测方程及时地由前一时刻的系统状态和噪声方差估计出当前时刻系统状态；测量修正方程则将当前时刻测量值结合状态预测过程得到的当前估计状态来得到系统最优估计值。

对于离散控制过程，假设系统当前状态为k，前一状态为 k-1，(k)表示当前状态的最优估计值，(k|k-1)表示利用前一状态计算得到的预测值，(k-1)表示前一状态的最优估计值，K(x)表示系统卡尔曼增益，Q表示系统过程的协方差，P(k)为(k)对应的协方差，P(k-1)为(k-1)对应的协方差，P(k|k-1)为(k|k-1)对应的协方差，卡尔曼滤波状态预测过程可描述为[5]：

卡尔曼滤波测量修正方程为[5]：

3 系统设计

3.1 硬件设计

系统硬件主要由主控制器、传感器、直流电机驱动模块等组成，传感器包括加速度计、陀螺仪、光电编码器，通过这些传感器完成对小车姿态参数的测量，其中车体倾角、车体角速度分别由加速度计和陀螺仪直接测量。

两轮小车控制系统主控制器选用STC12C5608AD微控制器，该微控制器是宏晶公司最新推出的一款高性能增强型8051 MCU，指令系统与传统8051单片机完全兼容，运行速度却是传统8051单片机的 6～12倍，其内部集成768 B RAM，具有8路高速12位A/D转换，同时可提供4路8位PWM输出[6]。STC12C5608AD丰富的硬件资源以及高速运算性能，使得系统硬件电路简单、可靠性高。由主控制器STC12C5608AD完成对陀螺仪、加速度计与光电编码器的数据采集与处理，得到小车姿态倾角，然后控制直流电机驱动模块实现小车的姿态调整，最终达到小车的自平衡控制,系统硬件结构如图2所示。

图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算法的实现。

本系统采用C51程序实现卡尔曼滤波算法。由卡尔曼滤波方程可知，实现卡尔曼滤波的关键在于确定合适的状态向量。通过对滤波器的校正，系统过程的噪声协方差矩阵Q与测量误差的协方差矩阵R确定形式如下[7]：

其中Qangle与Qgyro分别是加速度计与陀螺仪的测量协方差,其不同取值代表卡尔曼滤波器对其的不同信任程度，Rangle是小车倾角测量误差。通过小车实验调试，本系统 Qangle取值为 0.000 2，Qgyro取值为 0.001，Rangle取值为0.92。

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

本文介绍了基于数据融合的两轮小车自平衡控制系统的设计方法，在建立两轮自平衡小车的动力学模型的基础上，通过卡尔曼滤波算法有效地补偿震动、温度及不稳定力矩对陀螺仪与加速度传感器的影响，得到小车最优姿态倾角，最后通过双闭环数字PID控制算法实现两轮小车的自平衡控制。给予一定系统干扰，小车仍可实现自平衡，平衡时其倾角范围在±10°内。数据融合技术应用于两轮自平衡控制系统的方法，不仅提高了两轮系统的稳定性与抗干扰性，同时也为两轮系统姿态控制领域提供了一种新的设计思路[8]。