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

Ｍｅａｓｕｒｅｍｅｎｔ　Ｃｏｎｔｒｏｌ　Ｔｅｃｈｎｏｌｏｇｙ　ａｎｄ　Ｉｎｓｔｒｕｍｅｎｔｓ　基于数据融合的两轮自平衡小车控制系统设计　；ｌ＝　魏文，段晨东，高精隆，李常磊　（长安大学电子与控制工程学院，陕西西安？１００６４）　摘要：为解决两轮自平衡系统中传感器存在较大震动干扰与漂移误差的问题，并提高系统姿态　倾角测量的精确性和实时性，提出了基于陀螺仪与加速度计数据融合的两轮系统自平衡控制方法。　建立两轮自平衡系统的动力学模型，采用卡尔曼滤波算法融合陀螺仪与加速度计信号，得到系统姿　态倾角与角速度最优估计值，通过双闭环数字ＰＩＤ算法实现两轮系统的自平衡控制。通过两轮小车自　平衡控制系统的软硬件设计，成功验证了该方法的可行性与有效性。利用该方法大大提高了两轮自　平衡系统的抗干扰性。　关键词：数据融合；卡尔曼滤波；两轮；自平衡　中图分类号：ＴＰ２７３　文献标识码：Ａ　文章编号：０２５８—７９９８（２０１３）０５－００７９－０３　Ｔｈｅ　ｄｅｓｉｇｎ　ｏｆ　ｓｅｌｆ－ｂａｌａｎｃｉｎｇ　ｔｗｏ－ｗｈｅｅｌ　ｖｅｈｉｃｌｅ　ｃｏｎｔｒｏｌ　ｓｙｓｔｅｍ　ｂａｓｅｄ　ｏｎ　ｄａｔａ　ｆｕｓｉｏｎ　Ｗｅｉ　Ｗｅｎ，Ｄｕａｎ　Ｃｈｅｎｄｏｎｇ，Ｇａｏ　Ｊｉｎｇｌｏｎｇ，Ｌｉ　Ｃｈａｎｇｌｅｉ　（Ｃｏｌｌｅｇｅ　ｏｆ　Ｅｌｅｃｔｒｏｎｉｃ　ａｎｄ　Ｃｏｎｔｒｏｌ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｃｈａｎｇ’ａｎ　Ｕｎｉｖｅｒｓｉｔｙ，Ｘｉ’ａｎ　７１００６４，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｉｎ　ｏｒｄｅｒ　ｔｏ　ｓｏｌｖｅ　ｔｈｅ　ｐｒｏｂｌｅｍ　ｏｆ　ｔｈｅ　ｌａｒｇｅ　ｖｉｂｒａｔｉｏｎ　ｉｎｔｅｒｆｅｒｅｎｃｅ　ａｎｄ　ｄｒｉｆｔ　ｅｒｒｏｒ　ｏｆ　ｔｈｅ　ｓｅｎｓｏｒ　ｉｎ　ｓｅｌｆ－ｂａｌａｎｃｉｎｇ　ｔｗｏ—　ｗｈｅｅｌ　ｖｅｈｉｃｌｅ　ｓｙｓｔｅｍ，ａｎｄ　ｔｏ　ｉｍｐｒｏｖｅ　ｔｈｅ　ｐｒｅｃｉｓｉｏｎ　ａｎｄ　ｒｅａｌ－ｔｉｍｅ　ｐｅｒｆｏｒｍａｎｃｅ　ｏｆ　ｔｈｅ　ｓｙｓｔｅｍ　ａｔｔｉｔｕｄｅ　ａｎｇｌｅ　ｍｅａｓｕｒｅｍｅｎｔ，ａ　ｍｅｔｈｏｄ　ｂａｓｅｄ　ｏｎ　ｇｙｒｏｓｃｏｐｅ　ａｎｄ　ａｃｃｅｌｅｒｏｍｅｔｅｒ　ｄａｔａ　ｆｕｓｉｏｎ　ｉｓ　ｐｒｅｓｅｎｔｅｄ．Ｏｎ　ｔｈｅ　ｂａｓｉｓ　ｏｆ　ｔｈｅ　ｅｓｔａｂｌｉｓｈｍｅｎｔ　ｏｆ　ｓｅｌｆ—ｂａｌａｎｃｉｎｇ　ｔｗｏ—ｗｈｅｅｌ　ｓｙｓ－　ｔｅｍ　ｄｙｎａｍｉｃｓ　ｍｏｄｅｌ，Ｋａｌｍａｎ—Ｆｉｌｔｅｒｉｎｇ　ａｌｇｏｒｉｔｈｍ　ｉｓ　ｕｓｅｄ　ｔｏ　ｆｕｓｅ　ｔｈｅ　ｇｙｒｏｓｃｏｐｅ　ａｎｄ　ａｃｃｅｌｅｒｏｍｅｔｅｒ　ｓｉｇｎａｌｓ　ｉｎ　ｏｒｄｅｒ　ｔｏ　ｇｅｔ　ｔｈｅ　ｏｐｔｉｍａｌ　ｅｓｔｉｍａｔｅ　ｏｆ　ｓｙｓｔｅｍ　ａｔｔｉｔｕｄｅ　ａｎｇｌｅ　ａｎｄ　ａｎｇｕｌａｒ　ｖｅｌｏｃｉｔｙ．Ｔｈｅ　ｓｅｌｆ－ｂａｌａｎｃｉｎｇ　ｔｗｏ－ｗｈｅｅｌ　ｖｅｈｉｃｌｅ　ｃｏｎｔｒｏｌ　ｓｙｓｔｅｍ　ｉｓ　ａｃｈｉｅｖｅｄ　ｂｙ　ｄｏｕｂｌｅ　ｃｌｏｓｅｄ　ｌｏｏｐ　ｄｉｇｉｔａｌ　ＰＩＤ　ａｌｇｏｒｉｔｈｍ．Ｔｈｒｏｕｇｈ　ｔｈｅ　ｈａｒｄｗａｒｅ　ａｎｄ　ｓｏｆｔｗａｒｅ　ｄｅｓｉｇｎ，ｉｔ　ｖｅｒｉｉｆｅｓ　ｔｈｅ　ｆｅａｓｉｂｉｌｉｔｙ　ａｎｄ　ｅｆｆｅｃｔｉｖｅｎｅｓｓ　ｏｆ　ｔｈｅ　ｍｅｔｈｏｄ　ｓｕｃｃｅｓｓｆｕｌｌｙ，ａｎｄ　ｔｈｅ　ｍｅｔｈｏｄ　ｇｒｅａｔｌｙ　ｉｍｐｒｏｖｅｓ　ｔｈｅ　ａｎｔｉ－ｉｎｔｅｒｆｅｒｅｎｃｅ　ｐｅｒｆｏｒｍａｎｃｅ　ｏｆ　ｓｅｌｆ—ｂａｌａｎｃｉｎｇ　ｔｗｏ—ｗｈｅｅｌ　ｓｙｓｔｅｍ．　Ｋｅｙ　ｗｏｒｄｓ：ｄａｔａ　ｆｕｓｉｏｎ；Ｋａｌｍａｎ－ｆｉｌｔｅｒｉｎｇ；ｔｗｏ－ｗｈｅｅｌ；ｓｅｌｆ－ｂａｌａｎｃｉｎｇ　两轮系统是一种高度不稳定、非线性及多干扰系统，　动板和车轮等部件构成，小车两轮各参数（半径、转动惯　要使两轮系统达到自平衡，其模型决定了系统的重心须　量、质量等）相同且两轮共轴，由两直流电机独立驱动。　在两车轮轴线上。在两轮小车自平衡控制过程中，小车姿　图１为小车模型『２＿，假设小车质量为ｍ、车轮半径为ｒ、　态倾角测量的准确性与实时性决定了小车的控制精度及　车轮前进方向移动速度为　　：稳定性，仅仅依靠陀螺仪或者加速度计对两轮小车姿态倾　、车体自身水平作用力为　角测量，其测量误差及振动干扰较大，往往达不到两轮小　日、车轮所受地面摩擦力为　车的自平衡控制。本文提出了基于陀螺仪与加速度计数　、车轮对车轴的等效转动　据融合的两轮自平衡小车控制方法，由陀螺仪与加速度　惯量为．，、车体对车轮等效　计建立姿态倾角测量系统，通过卡尔曼滤波器进行数据　转动惯量为，、车轮转速为　融合得到小车高精度、高可靠性的姿态倾角，最后利用数　（Ｅ，、电机电磁转矩为　、车　字ＰＩＤ控制算法实现两轮小车的自平衡控制『】１。　体质心位置坐标（ｘ，ｙ）、车体　１两轮自平衡小车的动力学模型　质量ｍ　车体与垂直方向所　两轮自平衡小车主要由车体、核心控制板、电机驱　成角度为　、车轮对车身产　生的扭矩为　。　图基金项目：国家自然科学基金（５１１７５０４９）；青年科学基金（６１２０１４０７）　１两轮自平衡系统模型　《电子技术应用》２０１３年第３９卷第５期　７９　对车轮受力分析可知，车轮不仅受直流电机的输出　转矩、地面支持力和摩擦力的影响，同时由于惯性作用　还受到车体自身作用力的影响。将车轮的运动分解为前　进方向与绕轴方向的运动，车轮运动方程可描述为＿２ｊ：　前时刻测量值结合状态预测过程得到的当前估计状态　来得到系统最优估计值。　对于离散控制过程，假设系统当前状态为　，前一状　态为　一１，　（　）表示当前状态的最优估计值，互（ｋＩｋ一１）表　示利用前一状态计算得到的预测值，　（ｋ—１）表示前一状　态的最优估计值，Ｋ（ｘ）表示系统卡尔曼增益，Ｑ表示系　统过程的协方差，Ｐ（ｋ）为　（ｋ）对应的协方差，Ｐ（ｋ一１）为　（　一１）对应的协方差，Ｐ（ｋＩｋ一１）为　（ｋＩｋ一１）对应的协方　ｌｍ　＝　Ｈ　｛　Ｉ　　．＼１∞＝Ｔｍ—Ｈ　ｒｒ　对上述方程组消去　，车轮运动方程简化为：　ｍ　：一　＿　＋—Ｌ　，ｒｍ一日　ｒ　ｒ　对车体进行数学建模，其动力学方程可描述为［２１：　ｍ。　＝Ｈ　ｍ　＿ｙ　Ｖ—ｍ　ｇ　，　＝　Ｚｓｉｎ　一日Ｚｃｏｓ　一Ｔ　对上述方程组化简可知：（　ｌｆｆＬ　Ｌ　＿＋ｆ）　＋　ｃｏｓ　一ｇｓｉｎ￣ｏ＋　１　Ｔ　Ｔ＝Ｏ，当小车处于动态平衡时，车体倾角一般较小，　ｍｎ￡　可近似取ｓｉｎ￣ｖ＝０，ＣＯＳ￣＝１代入上式可得简化小车动力学　方程为１２１：　１　ｒ　．　１　—　１　（２ｍｒ＋兰　＋ｍ　ｌ＋ｍ。ｒ）　＝一［　（，＋ｍ。１２）＋‰　＋　。２　Ｉ｜　Ｉ　上式就是两轮自平衡小车关于车体倾角与角加速　度的微分方程。　２卡尔曼滤波数据融合过程　两轮小车姿态倾角的测量由陀螺仪与加速度计完　成。陀螺仪测量的是旋转运动，输出角速度信号，旋转越　快，其输出值越大，对角速度数据取积分，即能得到车体　倾角值【　。陀螺仪易受震动、温度和不稳定力矩等影响，　产生漂移误差，计算小车姿态倾角时由于积分作用其测　量误差会越来越大。加速度计测量的是线性运动，输出　加速度信号，速度变化越快，输出值越大，通过三角变换　可得到加速度计与重力方向的夹角ｌ　４ｌ。加速度计没有累　计误差，但是加速度计易受震动干扰。因此，仪单独使用　陀螺仪或者加速度计对小车姿态进行测量，很难保证测　｛Ｉ｝结果的精确性和可靠性。卡尔曼滤波是一种较好的多　ｆ０感器信息融合算法。采用卡尔曼滤波算法对加速度计　陀螺仪所测数据进行优化处理，补偿加速度计的动态　误差与陀螺仪的漂移误差，可得到精度与可靠性更高的　小车＝姿态倾角。　卡尔曼滤波算法是以最小均方误差估计的一套递　推估计算法，其算法核心思想：采用信号与偏差的状态　方程，利用前一状态得到的估计值和当前状态的测量值　来更新对状态变量的预测与估计，最终求得当前状态的　最优估计值。　卡尔曼滤波过程可分为状态预测和测量修正两个　过程。状态预测方程及时地由前一时刻的系统状态和噪　Ｊ打方差估计出当前时刻系统状态；测量修正方程则将当　８０　欢迎网上投稿ｗｗｗ．ｃｈｉｎａａｅｔ．ｃｏｍ　差，卡尔曼滤波状态预测过程可描述为［５１：　Ｊｆ　（　　Ｉｋ一１）＝Ａｘ（ｋ一１）＋Ｂｕ（ｋ一１）　　Ｉ【Ｐｆ　Ｉｋ一１）：ＡＰ（ｋＩｋ一１）Ａ　＋ｐ　卡尔曼滤波测量修正方程为［５１：　Ｋ（　）＝Ｐ（　Ｉｋ一１）ＨＴ（ＨＰ（ｋＩｋ一１）　＋尺）一　（　）＝　（　Ｉｋ一１）＋Ｋ（ｋ）（ｚ（ｋ）－Ｈｘ（ｋＩｋ一１））　Ｐ（ｊ｝）：（１－Ｋ（ｋ）Ｈ）Ｐ（ｋＩｋ一１）　３系统设计　３．１硬件设计　系统硬件主要由主控制器、传感器、直流电机驱动模　块等组成，传感器包括加速度计、陀螺仪、光电编码器，通　过这些传感器完成对小车姿态参数的测量，其中车体倾　角、车体角速度分别由加速度计和陀螺仪直接测量。　两轮小车控制系统主控制器选用ｓＴＣ１２Ｃ５６０８ＡＤ微　控制器，该微控制器是宏晶公司最新推出的一款高性能　增强型８０５１　ＭＣＵ，指令系统与传统８０５１单片机完全兼　容，运行速度却是传统８０５１单片机的６～１２倍，其内部　集成７６８　Ｂ　ＲＡＭ，具有８路高速１２位Ａ／Ｄ转换，同时可　提供４路８位ＰＷＭ输出＿６Ｉ。ＳＴＣ１２Ｃ５６０８ＡＤ丰富的硬件　资源以及高速运算性能，使得系统硬件电路简单、可靠　性高。由主控制器ＳＴＣ１２Ｃ５６０８ＡＤ完成对陀螺仪、加速度　计与光电编码器的数据采集与处理，得到小车姿态倾　角，然后控制直流电机驱动模块实现小车的姿态调整，　最终达到小车的自平衡控制，系统硬件结构如图２所示。　图２系统硬件结构框图　图３为陀螺仪接口电路，陀螺仪选用微型角速度传　感器ＥＮＣ一０３。由于陀螺仪输出电压信号较小，其输出信　号经ＴＬＶ２２１１放大６倍左右。图４为加速度计接口电　《电子技术应用》２０１３年第３９卷第５期　ＰＷＭ控制。因此，系统软件关键在于卡尔曼滤波算法与　数字ＰＩＤ算法的实现。　本系统采用Ｃ５１程序实现卡尔曼滤波算法。由卡尔　曼滤波方程可知，实现卡尔曼滤波的关键在于确定合适　的状态向量。通过对滤波器的校正，系统过程的噪声协方　差矩阵Ｑ与测量误差的协方差矩阵曰确定形式如下１７］：　ｒｎ．　ｎ　１　Ｑ＝ｌｕ　：岫　　Ｒ＝【ＩＲ咄】　其中Ｑ咄与Ｑ　分别是加速度计与陀螺仪的测量协方　差，其不同取值代表卡尔曼滤波器对其的不同信任程　０．１ＩＪ，Ｆ　度，　础是小车倾角测量误差。通过小车实验调试，本系　统Ｑ　０．９２。　图３陀螺仪ＥＮＣ一０３接口电路　Ｐ５　ｌ　取值为０．０００　２，Ｑ　取值为Ｏ．００１，ＪＲ础取值为　Ｐｌ　ｖｉｎ　ｖ似　ＧＮＤ　ＡＭＳｌ１ｌ７．３．３　３　采用双闭环数字ＰＩＤ对两轮系统进行自平衡控制，　．　１　３．３　Ｖ－Ｍ￣４Ａ　２　３　４　５　２　其控制结构如图６所示。对光电编码器产生的脉冲转换　得到两轮实时速度信号，以该信号作为反馈量实现速度　闭环控制；通过对陀螺仪与加速度计采集的数据进行融　合得到小车倾角信息，以小车倾角作为反馈量实现小车　姿态闭环控制［７］。这样就实现了两轮自平衡系统的双闭　环数字ＰＩＤ控制，系统速度环与小车姿态环均采用位置　型ＰＩＤ算法。　本文介绍了基于数据融合的两轮小车自平衡控制　系统的设计方法，在建立两轮自平衡小车的动力学模型　的基础上，通过卡尔曼滤波算法有效地补偿震动、温度　６　ａｄｅｒ　６　：：　Ｊ三３ｎＦ一一　Ｇｍ　ＭＭＡ　Ｎ／Ｃ　３－３　ｖ．　ＩＭＡ　ｌ】ｔ　ｚ　Ｙ（眦　ｃ　１—＊　—ｌｑＣ—　ｌｍ３　｝＿Ｊ　ｚ　Ｉｌ盘爱　Ｉ　龆巨　—　ｇ＿ｓｃｌｅｃｔ　＂　．１ｌｋｌ￣　　一１５　１ｋｎ　一　Ｉ｝　图４加速度计ＭＭＡ７３６１接口电路　及不稳定力矩对陀螺仪与加速度传感器的影响，得到小　车最优姿态倾角，最后通过双闭环数字ＰＩＤ控制算法实　现两轮小车的自平衡控制。给予一定系统干扰，小车仍　可实现自平衡，平衡时其倾角范围在±１０。内。数据融合　技术应用于两轮自平衡控制系统的方法，不仅提高了两　路，加速度计选用３轴小量程加速度传感器ＭＭＡ７３６１，　其输出电压信号是小车运动方向与重力方向加速度的　混合值。陀螺仪、加速度计接口电路均采用＋５　Ｖ电源，　与单片机ＳＴＣ１２Ｃ５６０８ＡＤ供电电源相匹配，　简化了硬件设计。陀螺仪、加速度计输出电压　信号均由ＳＴＣ１２Ｃ５６０８ＡＤ片内集成Ａ／Ｄ进行　采集。　３．２软件设计　轮系统的稳定性与抗干扰性，同时也为两轮系统姿态控　制领域提供了一种新的设计思路　Ｉ。　图５为系统软件实现框图，系统软件主　姿态传感器数据采集程序、卡尔曼滤波程序、ＰＩＤ控制　程序、直流电机ＰＷＭ控制程序等部分。ＳＴＣ１２Ｃ５６０８ＡＤ　片内集成Ａ／Ｄ转换和自带ＰＷＭ输出功能，只需读写　ＳＴＣ１２Ｃ５６０８ＡＤ单片机内部与Ａ／Ｄ转换和ＰＷＭ相关的　寄存器，就能完成对传感器的数据采集与直流电机的　………－ｌ＿…＿ｌ－一一一一－ｌ＿一～…一…　图６双闭环数字ＰＩＤ控制结构　参考文献　［１］冯智勇，曾瀚，张力，等．基于陀螺仪与ＪＪ【Ｉ速度仪新号『础　合的倾角姿态测量［Ｊ】．西南师范大学学报（自然科学版），　２０１１ｆ４）：１３７—１４１．　［２】李凡红．两轮自平衡小车系统ｆＤ】．北京：　北京交通大学，２０１０．　ＳＴＣ１２Ｃ５６０８ＡＤ　！　：角速度　：加速度　一电机驱动：［３］李俊博，朱涛，邹艳忠．陀螺稳定系统参数　模块　；　一一一一一　测试仪设计［Ｊ】．计算机测量与控制，２０ｌ１，　１９（２）：４８１－４８２．　（下转第８５页）　８１　圈５系统软件实现框图　《电子技术应用》２０１３年第３９卷第５期