单轴两轮自平衡智能小车姿态控制系统设计

MMA7260 和 ENC-03M 模块的信号输出引脚分别与微处理器的 AD 引脚连接，然后对 采集到的数据进行数据融合，才能得到准确的姿态信息。数据融合部分没有用到复杂的 Kalman 滤波[4]，而是用了另一种方法，原理框图如图 4 所示。
图 4 数据融合原理框图[5] 以下是数据融合部分的关键程序段。 void AngleCalculate(void) /*角度数据处理 输入值为陀螺仪角速度、加速度传感器加速度*/ {
总方案结构图如图 1 所示。
陀螺仪
加速度传感器
主电源
数 据 融 合
电源转换电路
复位电路
Kinetis 60 MCU
编码器
电
机
直
驱
流
动
电
电wk.baidu.com
机
路
JTAG 接口
蓝牙模块
上位机
图 1 总方案结构图
3 系统硬件设计
微处理器选用基于 Cortex-M4 的飞思卡尔 Kinetis 60 系列微处理器。Kinetis 是基于 ARM® CortexTM-M4 具有超强可扩展性的低功耗、混合信号微控制器。ARM Cortex-M4 内 核带 DSP 指令，性能可达 1.25 DMIPS/MHz，带有 8 路电机控制/通用/PWM 定时器(FTM) 通道。快速、高精度的 16 位 ADC、12 位 DAC、可编程增益放大器、高速比较器和内部电 压参考。提供强大的信号调节、转换和分析性能的同时降低了系统成本。加速度计传感器选 用飞思卡尔 MMA7260, MMA7260 是一款三轴低 g 半导体加速度计，可以同时输出三个方向 上的加速度模拟信号，通过设置可以使得 MMA7260 各轴信号最大输出灵敏度 800mV/g ， 这个信号无需要再进行放大，直接可以送到单片机进行 AD 转换[2]。陀螺仪可以用来测量物 体的旋转角速度。陀螺仪选用村田 ENC-03M，它利用了旋转坐标系中的物体会受到科里奥 利力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映 出物体旋转的角速度[3]。陀螺仪模块电路图如图 2 所示, 该电路采用典型的差分放大电路， 其中 R32,R33 为直流偏置。
5.3 电机速度采集程序
小车的速度传感器采用编码器，在一个固定时间内 T(5ms)的脉冲数目与相应的车速成 正比，由此获得当前车轮运动状况。采集回来的数据经过低通滤波后再进行单位转换，然后 通过前一刻 PWM 通道的输出判断电机正反转。采集数据处理流程如图 5 所示。 /***************电机速度控制部分******************/ void GetMotorPulse(void) //采集电机速度脉冲 {
unsigned int uiLeftMotorPulse,uiRightMotorPulse; uiLeftMotorPulse = FTM2_CNT; //读取寄存器 uiRightMotorPulse= FTM1_CNT; //读取寄存器 FTM1_CNT=0;//清零 FTM2_CNT=0;//清零 //------------低通滤波---------------// g_iLeftMotorPulseNew = uiLeftMotorPulse; g_iRightMotorPulseNew = uiRightMotorPulse; g_iLeftMotorPulse = (int) 0.1 * g_iLeftMotorPulseOld + 0.9 * g_iLeftMotorPulseNew; g_iRightMotorPulse = (int) 0.1 * g_iRightMotorPulseOld + 0.9 * g_iRightMotorPulseNew; g_iLeftMotorPulseOld = g_iLeftMotorPulse; g_iRightMotorPulseOld = g_iRightMotorPulse; //-----------------------------------// g_iLeftMotorPulse = (int)uiLeftMotorPulse; g_iRightMotorPulse = (int)uiRightMotorPulse; if(g_ucLMotorPolar==0) g_iLeftMotorPulse = (-g_iLeftMotorPulse); //如果反转 速度为负 if(g_ucRMotorPolar==0) g_iRightMotorPulse = (-g_iRightMotorPulse); g_iLeftMotorPulseSigma += g_iLeftMotorPulse; g_iRightMotorPulseSigma += g_iRightMotorPulse; }
BTS7960 作为电机的驱动芯片，实现对电机正反转的控制。 同时，为了增加小车的机械稳定性，采取了降低重心和减小小车质量的方法，比如把小
车的电池和主控板等尽量的放在靠下的地方，将整个车模的重心降到轮轴附近，解决两轮自 平衡机器人非线性、强耦合和绝对不稳定的特点。重心越低的物体越稳定，不倒翁是一个很 好的实例。小车质量越轻，重心位置越低，则小车绕电机轴的转动惯量越小，电机的机电时 间常数越小，因此越有利于小车平衡的控制。
图 2 陀螺仪模块电路图[5]
小车的电机驱动芯片采用的是英飞凌公司的 BTS7960 半桥芯片，其最大电流可以达到 43A，因而其内阻比较小。小车在保持平衡状态的过程中，电机需要正反转，因为每个驱动 至少需要 2 块芯片构成一个 H 桥电路。小车的电机驱动电路图如图 3 所示。
图 3 电机驱动电路图
则
两式相减得增量式 PID 的算式：
∆Xk 反映的是第 k 次和第 k-1 次之间的增量，而且可以看出，使用增量式 PID 只需要保存前 两次的误差即可[6][7]。
两轮自平衡小车的姿态控制算法采用分段 PD 控制[8]，解决车模前后配重不一致引起的 移动,具体的 PD 参数是通过实验整定的，对不同的机械架构需要不同的参数。电机速度控制 框图如图 5 所示。
// *(-1)/30 校正极性，去毛刺 //单位：角度 //去毛刺 //单位：角度/秒
//时间补偿常数 fTz=1.0 //控制频率 fTp=200.0
5.2 PID 算法与平衡控制算法程序
PID 控制是工业控制过程中历史最悠久，生命力最强的控制方式。位置式 PID 的算式为：
其中 x0 为偏差信号为 0 时的控制作用，是控制量的基础。 在用计算机控制的离散系统中，PID 的算式为：
Keywords: Attitude control；Multi-sensor data fusion；Since the balance control algorithm； PD algorithm
1 引言
近年来，两轮自平衡电动车以其行走灵活、便利、节能等特点得到了很大的发展。国内 外多家科研单位和企业以及自平衡车的爱好者都对自平衡理论的应用和发展做出了许多贡 献, 大大推进了自平衡技术的发展, 同时也推出了一批杰出的自平衡机器人和载人代步车作 品。在第七届“飞思卡尔”杯全国大学生智能车竞赛的电磁组比赛中，也利用了其赛用 C 型车模双后轮驱动的特点，要求实现两轮自平衡直立行走，对推动自平衡理论的应用和发展 有着重大意义。开展对两轮自平衡车的深入研究对提高我国在该领域的科研水平、扩展机器 人的应用背景等具有重要的理论及现实意义。
关键词：姿态控制；多传感器数据融合；自平衡控制算法；PD 算法
Song yi-biao Chen heng-wei Long cha
(School of Information Engineering, Wuyi University, Jiangmen 529020， China)
Abstract: Two-wheeled self-balancing vehicle is a kind of dynamic balancing robot, need to rely on motion balance but not to a powerful control makes it stable. This paper introduces a uniaxial two-wheel self-balancing dolly attitude control system design process, using Freescale Kinetis 60 as core control unit, triaxial acceleration sensor MMA7260 and gyroscope ENC - 03 m measurement models of Angle and Angle speed, speed encoder for motor motion, through PD regulating control two motor direction, movement, balance of models of vertical state. In order to increase the mechanical stability of the car, took a lower center of gravity and reduce the car quality approach, such as the car battery and the main control board to press down as far as possible, the center of the whole car model should be near to the shaft.
目前关于两轮自平衡车的研究和成果已经很多，国内外众多商家已经将两轮自平衡车作 为运输载具商业化量销售[1]。本文的目的是探索使用比较廉价的元器件搭建一个简易的自平 衡系统，以便进行各种控制算法的实验和理论研究。
2 系统总体方案
本文设计的两轮自平衡智能小车控制系统，考虑到自平衡控制需要较高且稳定的控制速 度，采用飞思卡尔 Kinetis 60 系列微处理器作为主控制器。采用飞思卡尔 MMA7260 三轴高 灵敏度加速度传感器与村田公司的 ENC-03M 陀螺仪作为自平衡小车的姿态传感器。采用
单轴两轮自平衡智能小车姿态控制系统设计 宋一标 陈恒伟 龙察
（五邑大学 信息工程学院 广东 江门 529030） 摘 要: 两轮自平衡车属于动态平衡机器人的一种, 需要靠运动保持平衡不倒，要施加强有 力的控制手段使其稳定。本文介绍一种单轴两轮自平衡小车姿态控制系统设计过程，采用 Freescale Kinetis 60 作为核心控制单元，三轴加速度传感器 MMA7260 和陀螺仪 ENC-03M 测量车模的倾角和倾角速度，测速编码器获取电机运动状态，通过 PD 调节控制两个电机正 反向运动，保持车模直立平衡状态。为了增加小车的机械稳定性，采取了降低重心和减轻小 车质量的方法，比如把小车电池和主控板等尽量压下，将整个车模的重心降到轮轴附近。
5 系统软件设计
两轮自平衡小车软件部分主要由姿态传感器数据融合处理程序、平衡控制算法程序和电 机控制程序组成。姿态传感器数据融合处理程序主要解决 MMA7260 和 ENC-03M 输出的模 拟信号进入微控制器以及数据融合。由于本系统采用的 Kinetis 60 系列微处理器具有 PWM 功能，因此极大地简化了电机控制程序的编制，主要是 PWM 功能寄存器的使用。平衡控制 算法是独立于以上两类程序的单独模块，在以上两部分程序解决了与硬件相关的程序后，平 衡控制算法程序部分只需关注控制算法本身的编程问题。 5.1 MMA7260 和 ENC-03M 的数据融合
float fAngleDelta;
iAccZORG = (iAccZADC - g_iAccZOffset) / (-1) ; g_fAccZ = iAccZORG * ACC_Z_SCALE ; iGyroHORG =(iGyroHADC - g_iGyroHOffset) / 12 ; g_fGyroH= (float) iGyroHORG * fRgyro * 12.0 ; g_fAngle = g_fGyroHIntegral ; fAngleDelta = (g_fAccZ - g_fAngle)/ fTz; g_fGyroHIntegral += (g_fGyroH + fAngleDelta)/ fTp ; }
主要程序段如下： void AngleControl(void) {
图 5 电机速度控制部分框图
float fValue; /*分(前后)正负角度 PD,解决车模前后配重不一致引起的移动,注意车模往后仰的角度为正 方向*/ if(g_fAngle>0.0) fValue=(g_fAngleSet-g_fAngle) * g_fAngle_P_Positive + (g_fGyroHSet-g_fGyroH)* g_fAngle_D_Positive;//后仰 else if(fAngle<0.0) fValue=(g_fAngleSet-g_fAngle) * g_fAngle_P_Negative + (g_fGyroHSet-g_fGyroH)* g_fAngle_D_Negative;//前倾 else if(fAngle==0.0) /*设置上下限 */ if(fValue>1000.0) fValue=1000.0; if(fValue<-1000.0) fValue=-1000.0; g_fPWMAngleCtrl=fValue; }