平衡车pid算法python 平衡车 arduino

Arduino平衡小车

1.概述

此Arduino平衡小车在主控方面由Arduino UNO R3和Arduino sensor shield v5.0传感器扩展板组成。采用TB6612FNG作为电源和电机之间的中介给带编码器的直流电机供电以及传送PWM信号。采用六轴MPU6050进行角度采集并传给Arduino由Arduino计算角度偏差以及角加速度。现在，先看一下车的总体图片（此小车采用5V电源给Arduino直接供电省去了降压模块）以便有一个大致了解。

2.平衡原理（参考亚博）

2.1角度环平衡原理

在此我们用简单的受力分析图表示小车大致受力情况。

如图可以看出当给小车提供向右的加速度时，小车在垂直杆的方向存在两个力一个是重力的分力mgsinθ ，另一个是小车由于惯性而保持原来状态不变的力（在此，我们索性叫它惯性力）ma cos θ（其与加速度方向相反，大小与加速度成正比）。

这样我们可以得到小车的恢复力公式为：F = mg sin θ-ma cos θ≈mg θ-mk1θ （其中由于 θ很小，sin θ和cos θ近似线性化为 θ。k1为负反馈控制车轮加速度 a 与偏角θ成正比的比例）。

但是，当k1>g时，恢复力的方向便会与位移的方向相反，这样在角度环的控制下小车会前后来回大幅度摆动最终朝一个方向跑去直到倒下。

所以我们需要给小车提供一个阻尼力，以便让杆能够尽快回到垂直位置稳定下来。增加的阻尼力与偏角的速度成正比，方向相反，因此公式1可改为： F = mg θ-mk1 θ -mk2 θ`

按照上述倒立摆的模型，可得出控制小车车轮加速度的算法： a =k1θ+k2θ1 （其中θ为小车角度，θ1为小车角速度，k1 k2都是比例系数 ）。

在角度反馈控制中，与角度成比例的控制量是称为比例控制；与角速度成比例的控制量称为微分控制（角速度是角度的微分）。因此上面系数 k1,k2 分别称为比例和微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车震荡。通过微分抑制控制震荡的思想在后面的速度和方向控制中也同样适用。

2.2速度环控速原理

假设小车在上面直立角度控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度（也就是机械中值角）有偏差，因此小车实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，小车就会朝倾斜的方向加速前进控制速度只要通过控制小车的倾角就可以实现了。

具体实现需要解决三个问题：（1）如何测量小车速度？（2）如何通过小车直立控制实现小车倾角的改变？（3）如何根据速度误差控制小车倾角？

第一个问题很好解决，我们所购买的电机是带编码器（霍尔编码器和光电编码器，此车用的是霍尔编码器）的直流电机。此车所用的电机还有A、B两个相位。通过相位的高低电平变化并加之Arduino的外部中断，可以对脉冲进行计算，在此用脉冲的个数代替电机的转速。（这个我们在硬件的章节会具体讲解。）

第二个问题可以通过角度控制给定值来解决。给定小车直立控制的设定值（机械中值角），在角度控制调节下，小车将会自动维持在一个角度。通过前面小车直立控制算法可以知道，小车倾角终是跟踪重力加速度Z轴的角度。因此小车的倾角给定值与重力加速度Z轴角度相减得到角度偏差，便可以终决定小车的倾角。

第三个问题我们对一个简单例子进行分析。假设小车开始保持静止，然后增加给定速度，为此需要小车往前倾斜以便获得加速度。在小车直立控制下，为了能够有一个往前的倾斜角度（这个可以理解惯性保持），车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得小车往前倾角需要更大。如此循环，小车很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。 因此，不稳定。

在调试小车的时候会发现存在震荡。

要解决控制震荡问题，在前面的小车角度控制中已经有了经验，那就是在控制反馈中增加速度微分控制。但由于车轮的速度反馈信号中往往存在着噪声，对速度进行微分运算会进一步加大噪声的影响。

为此需要对上面控制方法进行改进。原系统中倾角调整过程时间常数往往很大，因此可以将该系统近似为一个积分环节。将原来的微分环节和这个积分环节合并，形成一个比例控制环节。这样可以保持系统控制传递函数不变，同时避免了微分计算。

但在控制反馈中，只是使用反馈信号的比例和微分，没有利误差积分，所以终这个速度控制是有残差的控制。但是直接引入误差积分控制环节，会增加系统的复杂度，为此就不再增加积分控制，而是通过与角度控制相结合后在进行改进。

要求小车在原地停止，速度为0。但是由于采用的是比例控制，如果此时陀螺仪有漂移，或者加速度传感器安装有误差，终小车倾角不会终调整到0，小车会朝着倾斜的方向恒速运行下去。注意此时车模不会像没有速度控制那样加速运行了，但是速度不会终为0。为了消除这个误差，可以将小车倾角设定量直接积分补偿在角度控制输出中，这样就会彻底消除速度控制误差。

2.3转向控制原理

通过左右电机速度差驱动小车转向消除小车距离道路中心的偏差。通过调整小车的方向，再加上车前行运动，可以逐步消除小车距离中心线的距离差别。这个过程是一个积分过程，因此小车差动控制一般只需要进行简单的比例控制就可以完成小车方向控制。但是由于小车本身安装有电池等比较重的物体，具有很大的转动惯量，在调整
过程中会出现小车转向过冲现象，如果不加以抑制，会使得小车过度转向而倒下。根据前面角度和速度控制的经验，为了消除小车方向控制中的过冲，需要增加角度微分控制。

3.需要采集的信号

（1）小车倾角速度陀螺仪信号，获得小车的倾角和角速度。

对于（1）我采用了B站上平衡小车之家的教学图片，图中β为Z与竖直方向的夹角，atan()是Arduino提供的反正切函数公式。

（2） 小车电机转速脉冲信号，获得小车运动速度，进行速度控制。

（3） 小车转动速度陀螺仪信号，获得小车转向角速度，进行方向控制。

4.硬件原理以及测试代码

4.1TB6612FNG

4.1.1价格

TB6612FNG一般的价格在7-9块之间。

4.1.2功能

接收Arduino的PWM信号以及正反转信号给电机。注意：TB6612FNG没有降压功能，不能给Arduino供电！

4.1.3引脚图及其功能表

AIN1	AIN2	BIN1	BIN2
接Arduino引脚	同前	同前	同前

上下一个输入引脚对应一个输出引脚

AO1	AO2	BO1	BO2
接电机M+或M-	同前	同前	同前

VM	GND
接12V电源正极（千万不要把12V电源正极接到VCC）	一个接Arduino的GND另一个接12V源的GND，还剩一个不用接
VCC	PWMA和PWMB
经过测试不用再接5V的电源了（不要接Arduino）	接Arduino带有~的引脚
STBY
接Arduino上随便一个引脚并给高电平（给高TB6612FNG工作，反之，非也。相当于使能开关）

4.2带编码器的电机

4.2.1价格

黄色的带编码器的TT减速电机一般价格在27左右一个。金属的价格一般在100块左右两个。

4.2.2实物引脚图

4.2.3相位测速解读

图中A相和B相差90度，如果A相和B相分别接Arduino的引脚，则通过引脚电平的变化就能对脉冲计数。如：利用Arduino的外部中断attachInterrupt（引脚编号，函数，CHANGE）和arrachPinChangeInterrupt(同前)就可以对脉冲计数了。（其中函数为脉冲自加或自减函数，脉冲可以自己定义为一个整形变量让其自加或自减）。

4.3MPU6050

4.3.1价格

最好买贵一点的比如30块左右的。

4.3.2功能

获得各轴角速度和角度值赋给你所设的变量如：ax，ay，az，gx，gy，gz。

4.3.3实物引脚图

4.3.4引脚功能表

VCC	GND	SDA	SCL
接Arduino上的5V	接Arduino上的GND	接Arduino上的SDA或者A4	接Arduino上的SCL或者A5
XDA	XCL	ADO	INT
用不到	同前	同前	同前

5.硬件测试代码

5.1TB6612FNG存活测试代码

define AIN1 2
define AIN2 3
define BIN1 8
define BIN2 9
define PWMA 5
define PWMB 6
int i=0,n=40;//改变n的值可以改变点击速度。
 void setup() {
 pinMode(2,OUTPUT);
 pinMode(3,OUTPUT);
 pinMode(8,OUTPUT);
 pinMode(9,OUTPUT);
 pinMode(5,OUTPUT);
 pinMode(6,OUTPUT);
 }void loop() {
 digitalWrite(AIN1,HIGH);
 digitalWrite(AIN2,LOW);
 digitalWrite(BIN1,HIGH);
 digitalWrite(BIN2,LOW);
 if(i0){
 i=i+n;
 analogWrite(PWMA,i);
 analogWrite(PWMB,i);
 if(i250){
 i=i-n;
 analogWrite(PWMA,i);
 analogWrite(PWMB,i);
 }
 }
 }5.2MPU6050存活测试代码（没有转化角度，全是原始值）（参考b站）
include <Wire.h>
const int MPU_adress = 0x68;//MPU6050在I²C总线上的地址为0x68
 int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
 void setup(){
 Wire.begin();//用begin函数启动I²C数据总线
 Wire.beginTransmission(MPU_adress);//向I²C启动发送地址
 Wire.write(0x6B);//写入MPU6050，0x6B是mpu6050的电源管理寄存器其中Bit6位控制着mpu6050是否在睡眠模式
 Wire.write(0);//用write写0关闭mpu6050的睡眠模式
 Wire.endTransmission(true);//结束Arduino与mpu6050的前期准备工作
 Serial.begin(9600);
 }
 void loop(){
 Wire.beginTransmission(MPU_adress);//启动与mpu6050的通讯
 Wire.write(0x3B);//访问保存加速度和陀螺仪数据的启示地址
 Wire.endTransmission(false);
 Wire.requestFrom(MPU_adress, 14, true);//访问14个寄存器，八种数据，向MPU6050索取数据AcX = Wire.read() << 8 | Wire.read();//7中数据14个字节，每种数据占两个字节分别为为数据的高位和低位，将高位和低位组合成两个字节才能得到完整的数据
 AcY = Wire.read() << 8 | Wire.read();//相当于前者×256和后者按位与最后得出十进制数
 AcZ = Wire.read() << 8 | Wire.read();
 Tmp = Wire.read() << 8 | Wire.read();
 GyX = Wire.read() << 8 | Wire.read();
 GyY = Wire.read() << 8 | Wire.read();
 GyZ = Wire.read() << 8 | Wire.read();Serial.print("AcX = “);Serial.print(AcX);
 Serial.print(” | AcY = “);Serial.print(AcY);
 Serial.print(” | AcZ = “);Serial.print(AcZ);
 Serial.print(” | Tmp = “);Serial.print(Tmp / 340.00 + 36.53);//地址：高八位0X41、低八位0X42直接通过读取寄存器中的值来得到温度数据，温度换算公式为：Temperature = 36.53 + regval/340
 Serial.print(” | GyX = “);Serial.print(GyX);
 Serial.print(” | GyY = “);Serial.print(GyY);
 Serial.print(” | GyZ = ");Serial.println(GyZ);
 delay(1000);
 }6.直立环代码
include <KalmanFilter.h>//卡尔曼滤波函数库内涵反正切角度转换公式
include <PinChangeInterrupt.h>//以下四行为Arduino外部中断函数库
include <PinChangeInterruptBoards.h>
include <PinChangeInterruptPins.h>
include <PinChangeInterruptSettings.h>
include <MsTimer2.h>//定时器中断函数库
include <MPU6050.h>//MPU6050函数库
include <Wire.h>//以下两行代码是与MPU6050有关的函数库
include <I2Cdev.h>
define STBY 8
define AIN1 7
define AIN2 6
define BIN1 13
define BIN2 12
define PWMA 9
define PWMB 10
MPU6050 mpu;//命名我的mpu的名字
KalmanFilter kalmanfilter;//命名我的卡尔曼滤波的名字
int16_t ax, ay, az, gx, gy, gz;
double kp = 26, ki = 0.0, kd = 0.50;
//角度数据
 float K1 = 0.05; // 对加速度计取值的权重
 float middleAngle = 4.1;//机械中直角，也就是预期的平衡角度
//卡尔曼库封装
 float Q_angle = 0.001, Q_gyro = 0.005; //角度数据置信度,角速度数据置信度
 float R_angle = 0.5 , C_0 = 1;
 float timeChange = 5; //滤波法采样时间间隔毫秒
 float dt = timeChange * 0.001; //注意：dt的取值为滤波器采样时间double angleoutput =0, leftPwm, rightPwm;
角度环控制函数
void angleout()
 {
 angleoutput = kp * (kalmanfilter.angle + middleAngle) + kd * kalmanfilter.Gyro_x;//PD算法
 }最终PWM融合函数
void Balance_finalPwm(double speedoutput,float rotationoutput,float angle,float angle6,int turnleftflag,int turnrightflag,int spinleftflag,int spinrightflag,int front,int back,float accelz,int Pin1,int Pin2,int Pin3,int Pin4,int PinPWMA,int PinPWMB){
 leftPwm = -angleoutput - speedoutput + rotationoutput;//PWM融合
 rightPwm = -angleoutput - speedoutput + rotationoutput;
 if (leftPwm > 255) {leftPwm = 255;}//PWM限福
 if (leftPwm < -255) {leftPwm = -255;}
 if (rightPwm > 255) {rightPwm = 255;}
 if (rightPwm < -255) {rightPwm = -255;}
 if (angle > 30 || angle < -30){//角度限福
 leftPwm = 0;
 rightPwm = 0;
 }
 if (leftPwm >= 0) {//电机控制
 digitalWrite(Pin2, 0);
 digitalWrite(Pin1, 1);
 analogWrite(PinPWMA, leftPwm);
 }
 else {
 digitalWrite(Pin2, 1);
 digitalWrite(Pin1, 0);
 analogWrite(PinPWMA, -leftPwm);
 }
 if (rightPwm >= 0) {
 digitalWrite(Pin4, 0);
 digitalWrite(Pin3, 1);
 analogWrite(PinPWMB, rightPwm);
 }
 else {
 digitalWrite(Pin4, 1);
 digitalWrite(Pin3, 0);
 analogWrite(PinPWMB, -rightPwm);
 }
 }void interrupt(){//中断函数
 sei();//打开全局中断
 mpu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//用MPU获取角度信息以及加速度信息给所设的变量
 kalmanfilter.Angletest(ax,ay,az,gx,gy,gz,dt,Q_angle,Q_gyro,R_angle,C_0,K1);//卡尔曼滤波融合角度
 angleout();//角度环控制直立
 Balance_finalPwm(0,0,kalmanfilter.angle,kalmanfilter.angle6,0,0,0,0,0,0,kalmanfilter.accelz,AIN1,AIN2,BIN1,BIN2,PWMA,PWMB);//最终PWM输出
 }void setup() {
 pinMode(AIN1, OUTPUT);
 pinMode(AIN2, OUTPUT);
 pinMode(BIN1, OUTPUT);
 pinMode(BIN2, OUTPUT);
 pinMode(PWMA, OUTPUT);
 pinMode(PWMB, OUTPUT);
 pinMode(STBY, OUTPUT);digitalWrite(AIN1, LOW);
 digitalWrite(AIN2, HIGH);
 digitalWrite(BIN1, HIGH);
 digitalWrite(BIN2, LOW);
 digitalWrite(STBY, HIGH);
 analogWrite(PWMA, 0);
 analogWrite(PWMB, 0);Wire.begin(); //打开IIC数据总线
 delay(1500);
 mpu.initialize();//MPU6050初始化
 delay(2);
 leftPwm = 0;
 rightPwm = 0;MsTimer2::set(5, interrupt);//设置5ms一次的定时器中断
 MsTimer2::start();中断开始
 }
 void loop(){}
7.速度环代码
include <KalmanFilter.h>
include <MPU6050.h>
include <I2Cdev.h>
include <Wire.h>
include <PinChangeInterrupt.h>
include <PinChangeInterruptBoards.h>
include <PinChangeInterruptPins.h>
include <PinChangeInterruptSettings.h>
include <MsTimer2.h>
MPU6050 mpu;
 KalmanFilter kalmanfilter;int16_t ax, ay, az, gx, gy, gz;
define STBY 8
define AIN1 7
define AIN2 6
define BIN1 13
define BIN2 12
define PWMA 9
define PWMB 10
define Encoder_Left 2 //左轮编码器A相
define Direction_Left 5**//左轮编码器B相**
define Encoder_Right 4**//右轮编码器A相**
define Direction_Right 11**//右轮编码器B相**
volatile long Velocity_L, Velocity_R = 0; //左右轮编码器数据
 int Velocity_Left, Velocity_Right = 0; //左右轮速度int time;//可以不要
 double Setpoint;//可以不要
 double //Setpoints, Outputs = 0; //速度环初始PWM输出
 double kp_speed =-40, ki_speed = kp_speed/200, kd_speed = 0.0;
 double setp0 = 0, dpwm = 0, dl = 0; //可以不要float Q;
 float Angle_ax;
 float Angle_ay;
 float K1 = 0.05;
 float angle0 = 4.10;
 int slong;float Q_angle = 0.001, Q_gyro = 0.005;
 float R_angle = 0.5 , C_0 = 1;
 float timeChange = 5;
 float dt = timeChange * 0.001;double angleoutput,leftPwm, rightPwm;
void Read_Encoder_Left() {//左轮脉冲计数函数
 if (digitalRead(Encoder_Left) == LOW) { //如果是下降沿触发的中断
 if (digitalRead(Direction_Left) == LOW) {Velocity_L–;} //根据另外一相电平判定方向
 else {Velocity_L++;}
 }
 else { //如果是上升沿触发的中断
 if (digitalRead(Direction_Left) == LOW) {Velocity_L++;} //根据另外一相电平判定方向
 else {Velocity_L–;}
 }
 }
 void Read_Encoder_Right() {//右轮脉冲计数函数
 if (digitalRead(Encoder_Right) == LOW) { //如果是下降沿触发的中断
 if (digitalRead(Direction_Right) == LOW) {Velocity_R–;}//根据另外一相电平判定方向
 else {Velocity_R++;}
 }
 else { //如果是上升沿触发的中断
 if (digitalRead(Direction_Right) == LOW) {Velocity_R++;} //根据另外一相电平判定方向
 else {Velocity_R–;}
 }
 }//速度环函数
double Balance_speed_PI(double kps,double kis,double kds,int encoder_left,int encoder_right){
 static float Velocity,Encoder_Least,Encoder,Movement=0;//定义速度，最近一次编码器（速度）偏差，编码器偏差，遥控参数
 static float Encoder_Integral,Target_Velocity;//定义偏差几积分，目标速度
 Encoder_Least=(encoder_left+encoder_right)-0**;//计算最新速度偏差，左右电机脉冲个数之和-0（0为目标速度）**
 Encoder =0.7;//一阶低通滤波
 Encoder +=Encoder_Least0.3;//一阶低通滤波
 Encoder_Integral+=Encoder;//对偏差积分得到位移
 Encoder_Integral=Encoder_Integral-Movement;//没有蓝牙遥控可以先不写
 if(Encoder_Integral>10000){Encoder_Integral=10000;}
 if(Encoder_Integral<-10000){Encoder_Integral=-10000;}
 Velocity=Encoderkps+Encoder_Integralkis;//PI算法
 return Velocity;
 }
 void Balance_finalPwm(double speedoutput,float rotationoutput,float angle,float angle6,int turnleftflag,int turnrightflag,int spinleftflag,int spinrightflag,int front,int back,float accelz,int Pin1,int Pin2,int Pin3,int Pin4,int PinPWMA,int PinPWMB){
 leftPwm = -angleoutput - speedoutput + rotationoutput;
 rightPwm = -angleoutput - speedoutput + rotationoutput;
 if (leftPwm > 255) {leftPwm = 255;}
 if (leftPwm < -255) {leftPwm = -255;}
 if (rightPwm > 255) {rightPwm = 255;}
 if (rightPwm < -255) {rightPwm = -255;}
 if (angle > 30 || angle < -30){
 leftPwm = 0;
 rightPwm = 0;
 }
 if (leftPwm >= 0) {
 digitalWrite(Pin2, 0);
 digitalWrite(Pin1, 1);
 analogWrite(PinPWMA, leftPwm);
 }
 else {
 digitalWrite(Pin2, 1);
 digitalWrite(Pin1, 0);
 analogWrite(PinPWMA, -leftPwm);
 }
 if (rightPwm >= 0) {
 digitalWrite(Pin4, 0);
 digitalWrite(Pin3, 1);
 analogWrite(PinPWMB, rightPwm);
 }
 else {
 digitalWrite(Pin4, 1);
 digitalWrite(Pin3, 0);
 analogWrite(PinPWMB, -rightPwm);
 }
 }
 void interrupt(){
 static int Velocity_Count;//定义进入速度环的时间计数
 sei();
 mpu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
 kalmanfilter.Angletest(ax,ay,az,gx,gy,gz,dt,Q_angle,Q_gyro,R_angle,C_0,K1);
 Velocity_Count++;
 if (Velocity_Count >= 8)//40ms进入速度环
 {

Velocity_Left = Velocity_L; Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
Velocity_Right = Velocity_R;  Velocity_R = 0; 
Outputs = Balance_speed_PI(kp_speed, ki_speed, kd_speed, Velocity_Left, Velocity_Right);
Velocity_Count = 0;

}
 Balance_finalPwm(Outputs,0,kalmanfilter.angle,kalmanfilter.angle6,0,0,0,0,0,0,kalmanfilter.accelz,AIN1,AIN2,BIN1,BIN2,PWMA,PWMB);
 }
 void setup() {
 pinMode(AIN1, OUTPUT);
 pinMode(AIN2, OUTPUT);
 pinMode(BIN1, OUTPUT);
 pinMode(BIN2, OUTPUT);
 pinMode(PWMA, OUTPUT);
 pinMode(PWMB, OUTPUT);
 pinMode(STBY, OUTPUT);digitalWrite(AIN1, 0);
 digitalWrite(AIN2, 1);
 digitalWrite(BIN1, 1);
 digitalWrite(BIN2, 0);
 digitalWrite(STBY, 1);
 analogWrite(PWMA, 0);
 analogWrite(PWMB, 0);pinMode(2, INPUT);
 pinMode(4, INPUT);
 pinMode(5, INPUT);
 pinMode(11, INPUT);Wire.begin();
 delay(1500);
 mpu.initialize();
 delay(2);
 leftPwm = 0;
 rightPwm = 0;
 MsTimer2::set(5, interrupt);
 MsTimer2::start();
 attachInterrupt(0, Read_Encoder_Left, CHANGE); //开启外部中断 编码器接口1
 attachPinChangeInterrupt(4, Read_Encoder_Right, CHANGE); //开启外部中断 编码器接口2
 }void loop() {
 }

8.转向环代码

int turn(int encoder_left, int encoder_right, int gyro)
{
static int bias;
int turn, encoder_temp;

encoder_temp = encoder_left - encoder_right;
bias += encoder_temp; //对角速度积分
此处省略限福代码；
turn = Kp * bias + Kd * gyro; //===结合Z轴陀螺仪进行PD控制
return turn;

}

9.直立环调试

9.1KP的极性：如果小车一直倒则把KP变为负值，也与最终PWM合成公式有关。

9.2机械中值角对于参数好的小车一般为0

9.3从小到大调KP直到出现低频来回抖动，然后在逐渐增加KD（此车的KP为26，KD为0.53）

10.速度环调试

10.1极性：转动其中一个轮子，另一个轮子和这个轮子一起往转动的方向加速即为正确。转动其中一个轮子，另一个轮子反转，则极性反了。

10.2KP的大小以小车是否能通过车轮来回摆动使其直立为标准，KI=KP/200。