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

张秀坚

摘 要 自平衡小车的平衡就像是一个钟摆，它的状态是不平衡的，因此它必须要有一个足够使其稳定平衡的控制力，来使其平衡。自平衡小车工作原理是将小车的加速度和当前的姿态信息进行整合，小车的空间姿态三轴数据和运动的空间加速度由MPU-6050来测量，小车车轮的转速由光电编码器所测得的脉冲信号来获得。在运动学的基础之上，由TI公司的MSP430芯片处理数据，运算得出保持平滑的姿态电机所需数据，再把这个数据来控制驱动马达的PWM波形的输出占空比， 以此来保持车体的平衡。小车的实际测试结果表明：小车在直立平衡时倾斜角度从小车的侧面看为-6°。使用自平衡车不但体积小，而且结构简单，具体特点为工作空间狭窄，但动作灵活，在安全、军事等领域应用前景非常广泛。

关键词 陀螺仪 MSP430F149 光电编码器 PWM 互补滤波 数据融合

中图分类号：TN29 文献标识码：A 文章编号：1007-0745（2020）03-0009-07

1 前言

1.1 研究意义

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

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

1.2 本文的研究内容

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

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

2.1 自平衡小车直立控制

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

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

2.2 平衡车的机械结构

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

2.3 两轮车倾倒的受力分析

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

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

2.4 自平衡小车倾角的测量

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

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

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

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

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）。

3.2 电机驱动电路

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

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

3.3 MPU6050电路

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

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所检测的加速度、角速度两个数据。卡尔曼滤波器则是将这两个数据进行滤波、融合得到小车的倾角。

子程序如下：

void Angle_Calcu（void）

{

//------加速度--------------------------

//换算关系：16384 LSB/g

//角度较小时，   x=sinx得到角度（弧度），    deg = rad*180/3.14

//因为  x>=sinx，  故乘以 1.2 适当放大

Accel_x  = GetData（ACCEL_YOUT_H）;     //读取Y轴加速度

Angle_ax  = （Accel_x - 1100） /16384;      //去除零点偏移，计算得到角度（弧度）

Angle_ax  = Angle_ax*1.2*180/3.14;     //弧度转换为度，

//-------角速度-------------------------

Gyro_y  = GetData（GYRO_YOUT_H）;//

Gyro_y  = -（Gyro_y + 30）/16.4;       //计算角速度值，   负号为反方向

//-------卡尔曼滤波融合-----------------------

Kalman_Filter（Angle_ax，Gyro_y）;    //卡尔曼滤波计算倾角

}

（程序1  卡尔曼滤波融合子程序）

4.2.1 IIC通信程序

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

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

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

4.2.2 卡尔曼滤波器

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

//******卡尔曼参数************

float Q_angle=0.002;

float Q_gyro=0.01;

float R_angle=0.003;

float dt=0.008;//dt为kalman滤波器采样时间;

char  C_0 = 1;

float Q_bias， Angle_err;

float PCt_0， PCt_1， E;

float K_0， K_1， t_0， t_1;

float Pdot[4] ={0，0，0，0};

float PP[2][2] = { { 1， 0 }，{ 0， 1 } };

//*********************************************************

// 卡尔曼滤波

//*********************************************************

void Kalman_Filter（float Accel，float Gyro）

{

Angle+=（Gyro - Q_bias） * dt; //先验估计

Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

Pdot[1]=- PP[1][1];

Pdot[2]=- PP[1][1];

Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分

PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差

PP[1][0] += Pdot[2] * dt;

PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - Angle;//zk-先验估计

PCt_0 = C_0 * PP[0][0];

PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * PP[0][1];

PP[0][0] -= K_0 * t_0; //后验估计误差协方差

PP[0][1] -= K_0 * t_1;

PP[1][0] -= K_1 * t_0;

PP[1][1] -= K_1 * t_1;

Angle+= K_0 * Angle_err; //  后验估计

Q_bias+= K_1 * Angle_err; //  后驗估计

Gyro_y  = Gyro - Q_bias; //  输出值  （后验估计）的微分=角速度

}

（程序2  卡尔曼滤波子程序）

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

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

4.3 计数程序设计

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

void jishuqi（）

{

P5DIR |= 0x05; //设输出端口

CCTL0 = CCIE; //CCR0开中断允许

CCR0 = 521-1; //向CCR0捕获/比较寄存器装入初值，用于比较

TACTL = TASSEL_2 + MC_1; //选 ACLK为定时器 A时钟源， 增计数模

}

// Timer A2中断服务程序

#pragma vector=TIMERA0_VECTOR

__interrupt void Timer_A （void）

{

ave_diff1=TACCR1;

}

（程序3  小车车轮转速检测子程序）

4.4 PID调节器程序设计

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

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

float PWM，  PWM_L，  PWM_R;

float Kp = 41， Ki = 11.3， Kd = 3.6;

float err，   speed_need = 0，turn_need = 0; //speed_need = 300，turn_need = 20

float position_dot，  position_dot_filter  ;

void PID （float setPoint）

{

jishuqi（）;

position_dot = ave_diff1*0.5;//编码器值读出

position_dot_filter*=   0.85;

position_dot_filter +=   position_dot*0.15;

if（Angle<-35 || Angle>35）  //角度过大，关闭电机

{

P6OUT = 0X00;

return;

}

err = Angle - setPoint;//得到实际偏差角度

PWM = - err*Kp

- Gyro_y*Ki

+ position_dot_filter*Kd ;

motorspeedout（PWM）;   //电机输出

}

（程序4  PID控制程序）

5 总结

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

参考文献：

[1] 张轩，徐苗，郑德聪，李志伟.基于变论域模糊PID控制的BLDCM仿真分析[J].微特电机，2020，48（03）：54-57.

[2] 黄文杰，向凤红.一种基于卡尔曼滤波的板球系统RBF-PID控制研究[J].电子测量技术，2020，43（02）：91-96.

[3] 徐猛.基于STM32自平衡小车的设计与制作[J].计算机产品与流通，2019（09）：110.

[4] 李详鹏，陈春，周子文，施娜，孙雅琪，杨阳.基于ME MS传感器的两轮平衡小车设计[J].科技创新与应用，2019 （22）：27-29.

[5] 冯智勇，曾瀚，张力.基于陀螺儀及加速度计信号融合的姿态角度测量[J].西南大学，2011（04）：137-141.

[6] 桑胜举，沈丁.单片机原理及应用[M].北京：中国铁道出版社，2010.

[7] 邓怡佳，李昆仑.基于STC单片机的自平衡小车设计[J].电子测试，2019（11）：33-34，29.

[8] 李瑞通. 基于陀螺仪技术的管线三维可视化研究[D].太原理工大学，2019.

[9] 何军虎，鞠剑平.自平衡车控制方法研究及仿真分析[J].机械设计与制造，2019（07）：144-146.

[10] 郝立果，邹玉茹.无线充电智能小车的设计[J].电子制作，2019（23）：34-36.

[11] 林浩.MSP430单片机Timer_A的应用[J].信息记录材料，2019，20（12）：84-85.

[12] 黄强，唐杰，林立，刘兴，张英洪.三相PWM整流器的双闭环前馈解耦控制研究[J].邵阳学院学报（自然科学版），2020，17（01）：40-47.

[13] 孙永忠，姜毅龙.载波移相PWM调制下的MMC电压平衡控制策略[J].电气传动，2020，50（02）：41-49.

[14] 孟春见，李开龙，张梦得.捷联惯性基组合导航滤波算法比较研究[J].电光与控制，2020，27（02）：18-21.

[15] 文常保，戚一娉，宿建斌，巨永锋.基于新型卡尔曼滤波算法的称重系统实现研究[J].自动化与仪表，2020，35（02）： 52-56.

[16] 李慧敏，于兴林.基于稳定图算法的发电机转动惯量参数辨识[J].电气自动化，2019（05）：8-11.

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