基于MSP430 两轮自平衡小车算法的研究

（彭水苗族土家族自治县职业教育中心，重庆 409600）

1 前言

1.1 研究意义

应用意义：自平衡车巧妙地运用到了其自身的重力来保持其平衡，并把这种重力用整个控制系统转换成动力，它本身的重力越大，行驶动能也会变得越大，比较环保。驾驶人员不必担心如何来使小车自身平衡，平衡小车自身就有稳定控制系统，使其平衡。使用自平衡车解决了残疾人不能骑自行车的问题。车身小巧灵活，可以作业在狭窄路段以及大转角的工作场。平衡小车的优点有很多：环保、方便、快捷、灵活，具有很广阔的应用前景。他将会成为未来的主流。

研究意义：自平衡小车车身不是完全稳定的，因此需要对电机进行控制以此来保持小车平衡的姿势，由车身整体协调机构转动电动车的平衡的控制和马达、驱动轮子、传感器、软件，微处理器是多种功能的控制，控制信号由动态方向决定，执行和行为控制是一个具有综合复杂特点的非线性系统，很难得到有效控制，控制算法比较灵活，具有挑战性，因此理论研究价值很高。

1.2 本文的研究内容

有很多种方法可以使自平衡小车平衡，而本文主要研究两种方法，其一是PID 控制算法，PID 调节器又称为PID控制器[1]，主要是获得自平衡小车的姿态信号作为电机的输出信号，同时对自平衡小车电机的运动速度进行有效控制，使得小车保持平衡状态。另一种算法是卡尔曼滤波算法，为了得到小车姿态准确稳定的信息，姿态检测算法主要通过对互补滤波融合姿态传感器(陀螺仪传感器以及加速度传感器)数据和卡尔曼滤波器和相结合计算相应平衡的参数[2]。

2 自平衡小车自平衡设计原理

2.1 自平衡小车直立控制

图1 是单摆的受力分析，能够分析出普通单摆由不平衡位置恢复到平衡位置的原理。

图1 普通单摆受力

当单摆离开平衡位置之后，就会受到重力以及拉线的作用力，使物体向平衡的位置移动，最后回到平衡位置点。

2.2 平衡车的机械结构

图2 平衡车机械结构

小车使用图2 的结构制作研究，车体分为电池、主控制和电机驱动、姿态信号检测两层，电池层用于放置给驱动电机供电的12V6A 聚合物锂电池，采用由TI 公司的MSP430F149 芯片组成的开发电路板构成，电机驱动层的电机驱动模块采用L298N，姿态检测是由MPU6050 芯片组成的GY-521 模块[3]。

2.3 两轮车倾倒的受力分析

两轮车本身是极不稳定的，显然如果没有外力作用在车身，那么小车一定会发生倾倒的现象。其受力分析如图3所示。

图3 小车受力情况分析图

在理想情况下，如果此时车身重力M 的方向与车轮支持力H 的方向之差相反时，系统合力为0，因此系统达到稳定状态，θ 角度很小，可以忽略不计。在现实情况下是由很多的干扰，θ 的角度不是一直为0，只要θ 角有一定的值，即使很小的角度值，M 的方向与H 的方向亦产生了角度，合力就不为0，根据牛顿运动定律可知，由于质量的原因θ 角度越来越大，直至车身倒在地上[4]。

2.4 自平衡小车倾角的测量

六轴陀螺仪可以对当前的角度信号、加速度信号数据进行采集，采用积分的方式对角速度数据进行分析、处理，处理器在程序中通过运算可以得到相应的角度值[5]。因为陀螺仪自身采集数据比较灵敏，根据陀螺仪的时钟每秒可以采集很多组数据，因此很容易受到周围环境噪声的干扰，所以对微型机械陀螺仪的操作要注意不能产生较大的噪声，保证微型陀螺仪可以安全地工作。陀螺仪在一定的情况下会产生误差，不能单独采用陀螺仪来当作角度的测量的器件，所以，另外再利用加速度传感器和三轴角度姿态传感器共同完成姿态信号数据的检测。

图4 MPU-6050 的具体结构

对于车模倾角和倾角加速度的测量过程中难免出现一些问题，而这些问题增加了感测器的漂移以及设定给予所带来的影响，所以就选择了MPU-6050。

MPU-6050 是一款具有着高性能的三轴加速度+三轴陀螺仪的六轴传感器模块，基本引脚图如图4 所示。加速度模拟信号，如图5 所示。

图5 输出3 轴方向

测量陀螺仪与加速度计，能够有效的将陀螺仪信号与加速度计的测量值相融合以此来得到较为准确的倾角值。通过卡尔曼滤波的相应的计算，就能使电机转速得到控制，从而控制自平衡小车的平衡状态。

3 自平衡小车硬件电路的设计

3.1 MSP430F149 单片机最小系统

本系统的主控制芯片采用的是16 位总线的FLASH 的MSP430 单片机,它的性价比高，集成度也高于一般类型的单片机,在市场得到中广泛使用。具有可靠性能好、运算速度快、工作稳定的优点。因此，可以用来做一些较为复杂的控制系统。自平衡小车单片机的最小系统电路如图6 所示[6]。

F149 单片机中运用到：

电机转速脉冲接口：

TA1-（PIN17）；

电机PWM 驱动接口：

PWM（PIN12,13）：电机驱动PWM 脉冲信号。

电机转动方向控制：

IN1（P60);

IN2（P61);

IN3（P62）;

IN4（P63);

程序下载接口：

P1.1（PIN13）；

TCK（PIN56）；

REST（PIN57）；

P2.2（PIN22）。

串口监控接口：

UTXT0（PIN32）；

URXT0（PIN33）。

MPU6050 模块接口

SDA（PIN51）；

SCL（PIN50）。

图6 MSP430F149 电路图

图7 电机驱动电路

3.2 电机驱动电路

上图7 是由自平衡小车电机的驱动电路。因为L298N的输入信号电压和供电电压是+5V，MSP430F149 单片机可以直接为L298N 提供输入信号。然而，+5V 的电压不适合本车模，使该车模不能有一个很好的调速范围。自平衡小车的电路供电电压，不能是单一的+5V，还需要有足够大的启动电流的电源，才能让小车有动力保持车身的平衡。聚合物锂电池是比较合适的动力电源[7]。

为了让小车保持一定的平衡，驱动电机的PWM 波形使用了定时器 A 自动产生两路PWM 波形输出。可以利用资源捕获的功能来产生所需要的波形，以便于很好地分析小车平衡的条件。

3.3 MPU6050 电路

MPU6050 芯片将系统中的加速传感器和陀螺仪传感器通过整合，可以得到一些有用的数字信号，当前自平衡小车的状态位置信息由MSP430 控制器通过一定的方式读出，如IIC 的通信方式的特点就比较符合[8]。所以，该芯片适合本系统的模块电路，设计起来相对比较简单。该模块的电路连接如图8 所示。

图8 MPU6050 模块电路

图9 系统主要程序框图

3.4 供电电源电路

因为自平衡小车的控制系统电路是需要+5V 的电源，而电机需要大电流电源+12V 电源供电才能使电机有足够的爆发力转动达到平衡，因此，需要采用两路电源。本系统的控制器电源由3 节5 号电池提供，电机驱动电压是采用的12V/6A 电流6800MA 聚合物锂电池提供，这样才能达到小车启动时需要大电流的要求。

4 自平衡小车软件设计

4.1 系统程序结构分析

如图9 所示，如果要使自平衡小车保持平衡，需要多个子程序，同时也能够更好的优化程序，子程序可以是PID调节程序、IIC 通信程序、卡尔曼滤波程序、PWM 波形产生程序和车轮转速检测程序等[9]。

主程序的运行流程图相对比较简单，如图10 是所示，程序的核心是调用卡尔曼滤波子程序和PID 控制子程序，这两个子程序包含的内容丰富，下面分别对这两个主要的子程序进行详细的理解。卡尔曼滤波子程序的任务是将MPU6050 检测到的信号滤波，但是由于外界干扰较大，直立稳定的小车不断地向车轮前后方向倾斜。此时，MSP430F149 单片机通过通讯协议，将MPU6050 中小车摆动的角速度和水平方向上的加速度读取出来，再对其加工处理。卡尔曼滤波器通过一定的测量方法可以得到很准确的倾角值。PID 子程序主要是得到的倾角使控制器输出相应的占空比波形来使电机的运转得到有效控制，由卡尔曼滤波器输出的角度来调整电机相应的运转方向，以此使小车保持平衡。

4.2 卡尔曼融合程序

卡尔曼融合的子程序的流程图如下图11 所示，图中读取Y 轴加速也就是F149 通过I2C 来控制MPU6050 并传输MPU6050 所检测的加速度、角速度两个数据。卡尔曼滤波器则是将这两个数据进行滤波、融合得到小车的倾角。

图10 主程序流程图

图11 卡尔曼算法融合数据子程序流程图

子程序如下：

4.2.1 IIC 通信程序

IIC 串行通信总线。IIC 总线通过串行数据线SDA 和串行时钟线SCL 两根线将多个具有IIC 总线接口的器件接到总线上，可以发送和接收数据。ICC 总线分为三种类型的信号传输，分别是开始信号、结束信号和应答信号[10]。

本系统的IIC 通信是单向通讯，该系统的MSP430 为主控制器件，同时也是单向接收端，而MPU6050 为发送端的形式进行通讯。

图12 IIC 通信程序流程图

因此，由单片机P5.0（SCL）、P5.1（SDA）模拟实现IIC 通信程序的流程图如上图12 所示[11]。

4.2.2 卡尔曼滤波器

斯坦利·施密特(Stanley Schmidt)开创卡尔曼滤波这一先河，首次实现了这一滤波方式，阿波罗飞船导航器为飞船提供了有效可靠的导航数据。卡尔曼滤波器的实质就是利用递推估算法总结之前数据的干扰，它同时利用小均方误差的结合，对前一时刻的估算值和此时刻的测量值来估算下一时刻的值，以此类推，相互影响。这种方法具有效率高的特点，可以解决现实中的很多问题，有效减少误差，使采集回的信号更加平稳、有效。在工业控制、通信系统、电力系统、环境污染控制、生活家电、航空航天、军事设施设备星号处理等很多地方都得到了广泛的应用[12]。在图像处理方面，也可以利用卡尔曼滤波处理对模糊的图像进行还原，是图像颜色分界处变得更细腻、更缓线性的变色。卡尔曼算法如下图13 所示：

图13 卡尔曼算法推导

程序2 是卡尔曼滤波的子程序，它是依据图13 的推导原理来编写的。它的算法是属于一种递推算法。卡尔曼滤波采用一定的变量可以反映出系统的状态特征。它的模型有两种，一种是状态空间模型，该模型反映的是系统运动的规律；而另一种模型，观测模型则是反映系统观测值与状态变量之间的关系[13]。这样程序就可以自动预判下一时刻的状态，就可以更快的让系统稳定。

当加速度和角速度信号经过卡尔曼滤波器，就可以得到一个相对精确、稳定的角度信号。

4.3 计数程序设计

本系统是采用选择的F149ACLK 为定时器 A 时钟源,增计数模式来测得自平衡小车当前速度的。自平衡小车的速度需要计算，此过程需要借助于转速中断程序。对于自平衡小车速度的测量需要用到两个中断子程序。当其中一个中断子程序获得“允许测量”信号的时候，开始执行检测中断的任务，当检测到旋转编码器脉冲时，进入另外一个中断子程序，开始计数，为了保证计数的同时不被外界信号所干扰，需要控制系统暂停检测中断的任务，同时打开时间计数器[14]。当时间计数器达到计数值时，就会发出不再计数的信号，接到此信号的中断子程序又开始检测中断，以此循环，直至认为控制关闭。具体程序如下所示：

4.4 PID 调节器程序设计

采用PID 调节器对系统响应进行调节，PID 调节又叫PID 控制，闭环自动控制可以增加调节的精度。在基于自动控制正负反馈的原理之上，测量系统的实际值与测量值相两者之差，实现调节控制系统的相应误差[15]。本设计将采用比例、积分，微分来控制小车电机转动方向的速度。

为了保证系统的安全，需要对控制器的输出值进行适当的限制，然而对于数控来说，对输出值进行适当的限制，即是要对u(k)进行限制，设置一个限制值u(m)，根据需要设定u(k)与u(m)的关系，对于这种情况还需要对u(k)的积分设置相应的限制值，以此来保护系统响应不受外来信息的干扰。具体的控制程序流程图如下:

图14 PID 控制调节器程序的流程图

5 总结

一般将车模分为平衡、行走和方向三个部分进行控制，三项任务相互耦合，互相作用最终都会达到平衡，本次研究主要是在小车平衡的算法，小车平衡主要是控制好小车轮子的正转、反转和速度来保持小车的平衡，在控制平衡过程中，由于不稳定的车身，需要稳定，所以要求控制信号必须平滑且高效，所以这里主要运用了PID 算法和卡尔曼滤波等算法能够实现这一要求[16]，利用滤波后的平滑数据控制PWM 稳定、有效的控制自平衡小车自身车轮，使其保持相对平衡，这是一个实时动态平衡的过程。系统不断的检测整合三轴角速度、三轴加速度、上一时刻姿态数据、预估影响值、车轮的转速等数据进行整理、融合不断控制PWM 输出信号形成闭环控制系统，不断地使三轴陀螺仪检测的数据达到预设的值，也就是相对的平衡状态。