【源码+视频】串口6050模块打造的低成本arduino自平衡车，控制算法不断升级中。。。

zhaowenwin

用自带卡尔曼滤波的串口6050模块制作的低成本自平衡小车，串口6050模块+小车底盘+L298驱动+Arduino uno R3，成本一百多。小车可以非常稳定地停在中间位置，控制算法非常简单，只用了比例控制就可以实现稳定控制了，足见串口6050模块测量精度之高。后续正在开发稳定度更高的自适应PID控制算法，希望有兴趣的朋友一起来玩，抛砖引玉，希望朋友们贡献出更稳定的控制代码。

淘宝地址：http://item.taobao.com/item.htm?id=19785706431

扫盲（大神请略过）：
卡尔曼滤波：一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。能够有效提高测量精度。
比例控制：即电机的输出电压，也就是PWM的脉宽宽度正比于小车的倾角。
PID控制：在工程实际中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制。

如果无法播放，请点击此处在新窗口打开

arduino源码：

[pre lang="arduino" line="1" file="MPU6050"]/*
This code is used for connecting arduino to serial mpu6050 module, and test in arduino uno R3 board.
connect map:
arduino   mpu6050 module
VCC    5v/3.3v
TX     RX<-0
TX     TX->1
GND    GND
note:
because arduino download and mpu6050 are using the same serial port, you need to un-connect 6050 module when you want to download program to arduino.
Created 14 Nov 2013
by Zhaowen

serial mpu6050 module can be found in the link below:
http://item.taobao.com/item.htm?id=19785706431
*/

unsigned char Re_buf[11],counter=0;
unsigned char sign=0;
int M11=5;
int M12=6;
int M21=9;
int M22=10;

float a[3],w[3],Angle[3],T;
short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
void setup() {
  Serial.begin(115200);
  pinMode(M11,OUTPUT);analogWrite(M11,0);
  pinMode(M12,OUTPUT);analogWrite(M12,0);
  pinMode(M21,OUTPUT);analogWrite(M21,0);
  pinMode(M22,OUTPUT);analogWrite(M22,0);
}
void SetMotor(float v1,float v2)
{
  if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
  else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
  else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
  else  {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
  
if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
  else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
  else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
  else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}
  
  Serial.print(Angle[0]*10);Serial.print(" ");
  Serial.print(v1);Serial.print(" ");
  Serial.print(v2);Serial.print(" ");
}
void loop() {
  float k=20;
  if (sign==0) return;
  sign=0;
  SetMotor(Angle[0]*k,Angle[0]*k);
  Serial.print("angle:");
  Serial.print(Angle[0]);Serial.print(" ");
  Serial.print(Angle[1]);Serial.print(" ");
  Serial.print(Angle[2]);Serial.print(" ");
  Serial.print("T:");
  Serial.println(T);
  //delay(100);
}

void serialEvent() {
  while (Serial.available()) {
   
    //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
  
    Re_buf[counter]=(unsigned char)Serial.read();
    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
    counter++;      
    if(counter==11)             //接收到11个数据
    {   
       counter=0;               //重新赋值，准备下一帧数据的接收
       switch(Re_buf [1])
        {
        case 0x51:
                a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
                a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
                a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
                break;
        case 0x52:
                w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
                w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
                w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
                break;
        case 0x53:
                Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
                Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
                Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
                T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
                sign=1;
                break;
        }
    }      
  }
}[/code]
废话不多说，直接上图了吧。
串口6050模块：

小车拆解图，中间绿色模块为串口6050模块

小车上电工作情况展示，两轮自平衡，静若处子。弱光拍摄，曝光时间较长，得益与串口6050模块高精度的姿态解算精度，稳定效果超乎想象。

arduinoSuper

{:soso_e179:}

zhaowenwin

arduinoSuper 发表于 2013-11-19 10:42

谢谢支持！

邵林寺

马上下来，晚上安我平衡车上试试

邵林寺

视频看了，感觉这个小车还有好多问题要处理。简单的比例控制不可靠，有时还要手动的去扶，这个实际上还没有达到平衡要求。小车要调试到即使用手去推这个小车，他也能自己调整到平衡状态。
之前我也用这款电机做过，平衡效果和你这个类似，后期加入了好多参数才算稳定的。楼主加油吧。
不过，这个串口模块说实话做的很好。

zhaowenwin

邵林寺 发表于 2013-11-19 12:51
视频看了，感觉这个小车还有好多问题要处理。简单的比例控制不可靠，有时还要手动的去扶，这个实际上还没有 ...

呵呵，是呀，剩下的就是调参数的问题了。控制算法上再搞搞，就会有稳定版了

zhaowenwin

邵林寺 发表于 2013-11-19 12:51
视频看了，感觉这个小车还有好多问题要处理。简单的比例控制不可靠，有时还要手动的去扶，这个实际上还没有 ...

高手在人间呀！

邵林寺

zhaowenwin 发表于 2013-11-19 13:05
高手在人间呀！

收到我的消息了吗？希望能分析一下原因

isilcala

“亲爱的老公”……

scyishuying

{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}

isilcala

我想问的是这个模块本身的算法会升级么

zhaowenwin

isilcala 发表于 2013-11-19 13:17
我想问的是这个模块本身的算法会升级么

算法已经是稳定版本，无须升级

zhaowenwin

邵林寺 发表于 2013-11-19 13:10
收到我的消息了吗？希望能分析一下原因

上电后续保持静止1秒左右，系统将对陀螺仪进行自动校准。以后的数据才会准确

邵林寺

isilcala 发表于 2013-11-19 13:16
“亲爱的老公”……

人家是带把的，不搞断背。。。。
只是有个串口的问题请教一下而已。。。。

zhaowenwin

邵林寺 发表于 2013-11-19 13:21
人家是带把的，不搞断背。。。。
只是有个串口的问题请教一下而已。。。。

察，忘了把背景声音给去掉了。。。。。