互补滤波技术在自平衡机器人中的应用

孙泰祎，王子兰，王 彦，朱延正

(山东科技大学 电气与自动化工程学院,山东 青岛 266590)

互补滤波技术在自平衡机器人中的应用

孙泰祎，王子兰，王 彦，朱延正

(山东科技大学 电气与自动化工程学院,山东 青岛 266590)

首先在理论上建立了自平衡机器人的动力学模型。基于PD控制算法，开发保持自平衡机器人直立的控制算法，并设计相应的控制器。利用模拟量输出的陀螺仪ENC03和加速度计MMA7361两个传感器分别测量角速度和角加速度，再采用互补滤波技术实时获得自平衡机器人的角度。在实验部分，自平衡机器人的机械结构使用普通小功率直流电机，驱动芯片使用BTS7960大电流半桥芯片，以ColdFrie MCF52255芯片作为控制器，采用蓝牙模块进行数据传输。将编写好的程序烧写到32位飞思卡尔单片机中，实现对自平衡机器人的控制，通过实时采集数据验证所开发的程序和算法的合理性。实验结果验证了所设计自平衡机器人的机械结构与硬件电路的合理性。

自平衡机器人；PD控制算法；互补滤波

在现代化生产过程中，机器人的作用越来越显著，机器人发展水平的提升甚至可以代表一个国家综合国力的提升。随着科技的进步，移动机器人技术也逐步发展起来。利用反馈的观点建立了两轮自平衡机器人的动力学模型，国内外目前已经有大量的研究成果[1-5]。文献[1]利用可控性和可线性化得出该系统的强可检测性的条件和最大自由度，并且根据推导方程结果设计了两种特殊的控制器。仿真结果证实了设计两种控制器的有效性。文献[2]利用卡尔曼可控性的判别条件，设计线性化的机器人动力学模型，利用该模型设计一个两轮移动机器人，并且能够控制该机器人沿着预定轨道行走，该机器人有成本低、重量轻、效率高的优点。文献[3]采用Skinner操作条件反射理论实现对行为决策的优化，解决两轮机器人的运动平衡控制问题，该操作条件反射学习机制具有较快的自主平衡控制技能和较好的鲁棒性能。本研究采用牛顿力学的分析方法构建两轮自平衡机器人的动力学模型，设计控制算法，利用互补滤波技术获得了小车实时姿态，以飞思卡尔公司生产的32位冷火系列芯片MCF52255作为核心控制芯片，然后利用PD控制算法控制小车实现自平衡，最后利用陀螺仪和加速度计等模块搭建自平衡机器人的机械结构，实验验证了模型和算法的正确性。

1 自平衡机器人的动力学模型

自平衡机器人的动力学模型类似于一个底部可以运动的一级倒立摆模型[1-5]。该倒立摆模型具有低成本、高效率、强灵活性、非线性、高阶次、强耦合、多变量、受环境影响大等特点。自平衡机器人在无外力作用下不能保持直立状态，若要保持直立状态，必须使用双轮差速直流电机，在机器人预运动方向施加相同方向的加速度，如图1所示。

图1 自平衡机器人保持平衡原理图Fig.1 Schematic diagram of keeping balance of the self-balancing robot

图2 自平衡机器人受力分析图Fig.2 Stress analysis of the self-balancing robot

下面对自平衡机器人进行数学建模。假设该倒立摆模型的重力为mg(其中g为重力加速度)，车模重心的高度为L,干扰加速度为u(t)，如图2所示进行受力分析。倒立摆受到外力干扰而出现倾斜，当没有产生加速运动时,即a(t)时的运动方程为

(1)

当θ很小的时候cosθ≈1，sinθ≈θ。可以得到简化的运动方程

(2)

当产生加速运动时，可以得到倒立摆倾角θ(t)、倒立摆角速度ω(t)、倒立摆底部运动加速度a(t)和干扰加速度u(t)之间的运动方程

(3)

当θ很小时，cosθ≈1，sinθ≈θ。可以得到简化的倒立摆的运动方程

(4)

利用式(2)可以得到以系统角度θ作为输出，干扰加速度u(t)作为输入的传递函数

(5)

(6)

(7)

(8)

由上述公式和条件kd>0，kp>g可以得出该系统是可控的：

(9)

2 自平衡机器人中的互补滤波技术

在实际运行过程中，倒立摆本身的摆动所产生的加速度会产生很强的干扰信号，叠加之后的信号无法反映倒立摆的倾角。相反，陀螺仪传感器输出机器人的角速度信号，并且该信号的噪声比较小，一般不会受车体运动的影响。角速度积分，可以获得机器人的角度信号，所以陀螺仪传感器可以进一步平滑角度信号。另一方面，角速度信号经过积分运算，会产生微小的偏差和位移，并且会形成积累误差，积累误差会导致电路饱和，要想消除积累误差，就必须通过加速度传感器进行校正。

因此，要想获得倒立摆模型的实时角度θ(t)的精确值，就需要利用互补滤波技术来融合陀螺仪和加速度计的输出，将加速度产生的干扰信号滤除。

图3 互补滤波算法结构图Fig.3 Structure chart of the complementary filtering algorithm

图3是互补滤波算法的算法框图。首先利用陀螺仪传感器和加速度计传感器分别检测自平衡机器人的角速度和角度，陀螺仪测得的角速度信号Ugyro与加速度计测得的角度信号Uacce和电压输出信号成比例关系。电压输出信号连接到单片机的AD(anglog digit)采集模块，经过AD采集，将自平衡机器人的角速度信号和角度信号转化成数字量的角速度信号Cargyro和角度信号Caracce，便于控制器处理和控制。

经过上述操作，得到了自平衡机器人实时的角速度信号Cargyro和角度信号Caracce。这两个信号再减去陀螺仪的零点Zgyro和加速度计的零点Zacce，即可得到角速度偏移量 ΔCargyro和角度偏移量 ΔCaracce。两个信号经过两个比例系数Pgyro和Pacce的放大,可以得到实时的角速度信号ω和角度信号θ′。

加速度计得到的角度值θ′(k)与上一时刻的θ(k-1)进行比较，得到的偏差经过比例系数1/Tz的放大，再与陀螺仪输出的角速度信号叠加之后进行积分，可以得到当前时刻的θ(k)。根据图3的互补滤波算法，利用陀螺仪传感器信号和加速度计传感器信号的融合波形，可以得到倒立摆模型实时的角度输出信号，经过比例微分控制，以及死区处理和饱和处理可以得到驱动直流电机的PWM(pulse-width modulation)信号。再经过电机驱动模块，驱动左右两个直流电机，完成自平衡机器人的姿态调整。

图4 系统结构示意图Fig.4 Schematic diagram of system structure

3 自平衡机器人的硬件设计

为了保证机器人能够稳定地保持自平衡，需要精心设计该系统的硬件部分，主要包括：控制模块、模拟陀螺仪加速度计模块、电源模块、双电机驱动模块、蓝牙数据传输模块和小功率直流电机模块等。机械结构设计方面，首先在自平衡机器人背面添加两块海绵保护，一旦发生事故可以尽最大的可能保护小车各个模块不因撞击而损坏。由于驱动板比较薄，而且小车后边有两块海绵垫，所以将驱动模块放到小车的后边。安装稳压模块时，用两个人工钻孔固定稳压模块。由于主控芯片需要不断接各个引脚，因此，小车的主控芯片暂时悬空。为了降低小车的重心，将电池尽量往下放。图4是机器人系统结构示意图。

3.1 控制模块

控制模块的选择是飞思卡尔公司生产的冷火系列芯片MCF522255。MCF52255 和MCF52259这两个单片机型号都采用 Coldfire V2 内核。MCF52259 比 MCF52255 多了一个外部总线接口，这就多了44个引脚。外部总线接口是用来扩展外部存储器(RAM或ROM)或外设的，由于 MCF5225X系列单片机片内已经集成了足够的存储器模块和丰富强大的外设模块，很少会使用总线扩展外设，对于完成平衡任务的机器人来说就更不用扩展了。因此，选择MCF522255作为本次实验设计的主控芯片。 MCF522255芯片具有ColdFrie V2 32位内核， 80 MHz主频，4通道16位脉冲调制模块(PWM)，3个异步串口模块(UART)，片内512 K Flash ROM和64 K SRAM，2个16位中断定时器(PIT)，8通道12位模拟数字转换(ADC)， 57个外设中断源和10引脚的BWM调试方式。基于以上主要特性，可以完成控制任务。

3.2 模拟陀螺仪加速度计模块

该模块采用日本村田公司生产的ENC-03陀螺仪作为单轴角速度传感器，利用3轴加速度传感器MMA7361来检测Z轴的加速度。使用该模块时，需注意陀螺仪的水平安装。

3.3 电源模块

电源模块包括电池、保险丝、开关和稳压模块。首先使用飞思卡尔智能车比赛的专用电池，供电上限是8.2 V。稳压芯片选择的是AMS1117和LM2941S，该稳压芯片输出3.3 V，供给主控芯片、电机驱动模块和陀螺仪加速度计模块。采用LM2941S经过可调电阻器稳压输出5 V电压，主要供给蓝牙模块。这两款芯片电路设计简单，输出纹波小。

3.4 双电机驱动模块

电机驱动芯片选用的是BTN7960芯片，是一款针对电机驱动应用的完全集成的大电流半桥芯片。该芯片能将电磁干扰减至最小，并且与微控制器的连接简单。驱动集成电路还具有电流检测诊断、转换率调整、死区时间生成以及过热、过压、欠压、过流和短路保护等功能。由于直立车的两个电机都需正反向控制，所以使用4片BTN7960芯片，两两组成一个H桥，分别驱动左右电机。此芯片开关频率可以达到25 kHz，可以很好地解决电机噪声大和发热的问题，同时驱动能力有了明显的提高，响应速度快。电机驱动模块原理如图5所示。

3.5 蓝牙模块

虽然无线串口已经流行了很多年，但仍存在许多问题，比如不能全双工通信、数据量大的时候容易丢数据，最高波特率仅115 200 bps等等，并且许多无线串口价格不菲。蓝牙串口和无线串口实现的效果是一样的，都可以实现串口数据的无线传输，而且蓝牙串口有很多优点，比如最高波特率可以到达 1 382 400 bps，是115 200 bps 的10倍多，保证了电脑发送数据时不会丢数据。其次，蓝牙串口是全双工的，MCU (microprogrammed control unit)高速发送数据到电脑时，电脑依然可以正常发送数据到 MCU。另外，蓝牙串口无需像 MX232、PL2303 之类的电平转换模块。最后，蓝牙串口的成本低。基于以上优点，使用蓝牙串口通信模块，来传输陀螺仪和加速度计的波形，以便进行波形拟合。

图5 电机驱动原理图Fig.5 Schematic diagram of motor drive

4 利用互补滤波技术调试机器人

根据图3的互补滤波算法结构图可知，需要整定的参数有Zacce、Zgyro、TZ、Tσ、Pacce、Pgyro、Kp、Kd。经过参数调节，最终确定参数各为Zacce=1 400、Zgyro=1 820、TZ=1、Tσ=0.007 5、Pacce=0.75、Pgyro=1。整定后可以得到如图6所示的曲线2，该曲线是实际自平衡机器人经过参数整定之后得到的角度跟踪曲线。由图6可知，经过互补滤波之后，角度跟踪情况比由单个加速度计获得的波形 (图5中曲线3)平滑了很多。

图6 互补滤波波形图Fig.6 Waveform graph of the complementary filtering

经过初步参数调节，确定了互补滤波的6个参数，为了实现机器人自平衡,还需要进一步调节Kp和Kd来实现角度控制，整定这两个参数需要从理论上分析这两个参数的作用。比例参数Kp相当于自平衡机器人的恢复力，满足Kp>g才能实现小车的初步直立。但是Kp不能无限制的增大，否则会使倒立摆来回摆动。微分参数Kd相当于倒立摆的阻尼力，该阻尼力会消除倒立摆的摆动，但是该参数过大会出现震颤现象。调节这两个参数应该采取先比例后微分的顺序。最终，确定出两个参数的值分别为：Kp= 0.591 25、Kd=0.27。经过参数整定，自平衡机器人能够检测实时的角度变化信息，并且进行决策，输出PWM波，改变电机的转速，使其在试验平台上保持直立。实验结果验证了所设计的自平衡机器人的机械结构与硬件电路的合理性。

5 结论

建立了自平衡机器人的动力学模型，将互补滤波算法应用到自平衡机器人中，设计了硬件电路并进行调试。实验结果表明，通过调节参数，能够实现机器人的自平衡。但是本算法仅仅完成了自平衡机器人的直立，如果再加一个摄像头和一个竖直方向的陀螺仪，改进一下算法，还能使机器人在保持直立的状态下躲避障碍物，完成转向运动。

[1]PATHAK K,FRANCH J,AGRAWAL S K.Velocity and position control of a wheeled inverted pendulum by partial feedback linearization[J].IEEE Transactions on Robotics,2005,21(3):505-513.

[2]SALERNO A,ANGELES J.On the nonlinear controllability of a quasiholonomic mobile robot[C]//IEEE International Conference on Robotics and Automation,2003：3379-3384.

[3]任红格,阮晓钢.基于Skinner操作条件反射的两轮机器人自平衡控制[J].控制理论与应用,2010,27(10):1423-1428. REN Hongge,RUAN Xiaogang.Self-balance control of two-wheeled robot based on Skinner’s operant conditioned reflex[J].Control Theory and Applications,2010,27(10):1423-1428.

[4]HAY S.Trajectory tracking control for navigation of the inverse pendulum type self-contained mobile robot[J].Robotics and Autonomous Systems，1996,17(1):65-80.

[5]KIM Y,KIM S H,KWAK Y K.Dynamic analysis of a nonholonomic two-wheeled inverted pendulum robot[J].Journal of Intelligent and Robotic Systems,2005,44(1):25-46.

[6]BLANKESPOOR A,ROBERT R.Experimental verification of the dynamic model for a quarter size self-balancing wheelchair[C]//American Control Conference,Proceedings of the IEEE,2004:488-492.

[7]GRASSER F,DARRIGO A,COLOMBI S.JOE:A mobile,inverted pendulum[J].IEEE Transactions on Industrial Electronics,2002,19(1):107-114.

[8]SALERNO A,OSTROVSKAYA S,ANGELES J.The development of quasiholonomic wheeled robots[C]//IEEE International Conference on Robotics and Automation,2002,4:3514-3520.

[9]AMBROSE,ROBERT O.Mobile manipulation using NASA's Robonaut[C]//IEEE International Conference on Robotics and Automation,2004,2:2104-2110.

(责任编辑：吕海亮)

Application of Complementary Filtering Technique in Self-balancing Robot

SUN Taiyi,WANG Zilan,WANG Yan,ZHU Yanzheng

(College of Electrical Engineering and Automation,Shandong University of Scienceand Technology,Qingdao,Shandong 266590,China)

A dynamic model of the self-balancing robot was first established. Based on PD(proportional differential) control algorithm, a control algorithm was developed to keep the self-balancing robot erect and the corresponding controller of the self-balancing robot was designed. The gyrostat sensor ENC03 and the accelerometer sensor MMA7261 were employed to measure the angular speed and angular acceleration respectively. Then the complementary filtering technique was used to calculate the angle of the self-balancing robot. In the experiment, miniwatt DC(direct current) motor was used in the mechanical structure of the self-balancing robot, large current half-bridge chip BTS7960 as the driver chip, ColdFire MCF52255 as the control chip, and bluetooth module for data transmission. The program was downloaded to the 32-bit control chip to control the self-balancing robot. The validness of the developed algorithm/program was verified by the collected real time data. The experiment results show that the proposed mechanical structure and hardware circuit for the self-balancing robot is reasonable and valid.

self-balancing robot;PD control algorithm;complementary filtering

2017-03-31

孙泰祎(1993—)，男，山东胶州人，硕士研究生,主要从事故障诊断与容错控制研究. 朱延正(1986—)，男，山东临清人，博士后，主要从事切换系统控制及其应用研究，本文通信作者. E-mail:yanzhengzhu@sdust.edu.cn

TP242

A

1672-3767(2017)04-0096-05

10.16452/j.cnki.sdkjzk.2017.04.014