求助，两轮小车~

☆与☆

小车用的是ARDUINO UNO+MPU6050+L289，目前再平衡状态，总是不停地抖动，下图是平衡状态角度波形图，javascript:;求大神们指点~

1. 
   
2. #include <Wire.h>         
   
3. #include <i2cdev.h>
   
4. #include <MPU6050.h>
   
5. #include <PID_v1.h>
   
6. #define Left1 10          //定义左直流电机输出
   
7. #define Left2 11          //定义左直流电机输出
   
8. #define Right1 5          //定义右直流电机输出
   
9. #define Right2 6          //定义右直流电机输出
   
10. #define drift 20          //小车角度调整范围
    
11. #define Gry_offset -72    //陀螺仪X轴的静态飘移。
    
12. #define Gyr_Gain -0.00763 //由陀螺仪X轴读数转换为角速度值（1/131）
    
13.                           //向前方向与X轴为反方向，故乘以-1
    
14. #define ACC_Gain 0.000061 //由读数转换为加速度值(1/16384)
    
15. #define pi 3.1415926      //定义PI值
    
16. //#define K 0.715           //一阶互补滤波权重取值，不知由来，可更改。
    
17. #define Q_angle 0.01      //角度数据置信度
    
18. #define Q_omega 0.0003    //角速度数据置信度
    
19. #define R_angle 0.01      //方差噪声
    
20. unsigned long preTime = 0;//定义积分初始时间
    
21. float angleG = 0;
    
22. double Klm_angle;         
    
23. float bias = 0;
    
24. float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    
25. double Setpoint, Input, Output;
    
26. double temp_kp,temp_ki,temp_kd;
    
27. MPU6050 accelgyro;         //MPU6050初始化
    
28. int16_t ax, ay, az;
    
29. int16_t gx, gy, gz;
    
30. PID myPID(&Input,&Output,&Setpoint,10,0.05,0,DIRECT);
    
31. 
    
32. 
    
33. void car_stop()            //小车停止函数
    
34. {
    
35.     analogWrite(Left1,0);      
    
36.     analogWrite(Right1,0);   
    
37.     analogWrite(Left2,0);   
    
38.     analogWrite(Right2,0);
    
39. }
    
40. 
    
41. 
    
42. void setup()               //主程序初始化
    
43. {
    
44.     Wire.begin();
    
45.     Serial.begin(9600);   
    
46.     accelgyro.initialize(); //MPU6050初始化。
    
47.     pinMode(Left1,OUTPUT);
    
48.     pinMode(Left2,OUTPUT);
    
49.     pinMode(Right1,OUTPUT);
    
50.     pinMode(Right2,OUTPUT);
    
51.     Wire.begin();           //I2C初始化
    
52.     accelgyro.testConnection();
    
53.     Setpoint=-2.00;         //设置平衡角
    
54.     myPID.SetSampleTime(10);
    
55.     myPID.SetMode(AUTOMATIC);
    
56. }
    
57. void loop()
    
58. {
    
59.     unsigned long now = millis();            //当前时间(ms)
    
60.     float dt = (now - preTime) / 500.0;      //微分时间(s)
    
61.     preTime = now;                           //上一次采样时间(ms)
    
62.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
    
63.     float Y_Accelerometer = ay * ACC_Gain;   //转换为向前的加速度(g)，为负值
    
64.     float Z_Accelerometer = az * ACC_Gain;   //转换为向下的加速度(g)
    
65.     float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
    
66.                                                                    //获得角度值，乘以-1得正值                                                           
    
67.     float gx_revised = gx + Gry_offset;      //陀螺仪x轴静态时修正后的角速度读数，向前为负值
    
68.     float omega= Gyr_Gain* gx_revised;       //陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
    
69. 
    
70. 
    
71.     //卡尔曼滤波
    
72.     Klm_angle += (omega - bias) * dt;          // 先验估计
    
73.     P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
    
74.     P_01 += -P_11 * dt;
    
75.     P_10 += -P_11 * dt;
    
76.     P_11 += +Q_omega * dt;                     // 先验估计误差协方差
    
77.     float K_0 = P_00 / (P_00 + R_angle);
    
78.     float K_1 = P_10 / (P_00 + R_angle);
    
79.     bias += K_1 * (angleA - Klm_angle);
    
80.     Klm_angle += K_0 * (angleA - Klm_angle);   // 后验估计
    
81.     P_00 -= K_0 * P_00;
    
82.     P_01 -= K_0 * P_01;
    
83.     P_10 -= K_1 * P_00;
    
84.     P_11 -= K_1 * P_01;                        // 后验估计误差协方差
    
85.     Serial.print(angleA);
    
86.     Serial.print(",");
    
87.     Serial.println(Klm_angle);
    
88.     //PID算法结合PWM输出
    
89.     if((Klm_angle>(Setpoint-drift))&&(Klm_angle<(Setpoint+drift)))
    
90.     {
    
91.         if((Klm_angle>Setpoint)&&(Klm_angle<(Setpoint+drift)))
    
92.         {   
    
93.             myPID.SetControllerDirection(REVERSE);
    
94.             Input=Klm_angle;   
    
95.             myPID.Compute();
    
96.             analogWrite(Left2,min(Output+150,255));      
    
97.             analogWrite(Right2,min(Output+150,255));      
    
98.             analogWrite(Left1,0);               
    
99.             analogWrite(Right1,0);                  
    
100.         }
     
101.         if((Klm_angle<Setpoint)&&(Klm_angle>(Setpoint-drift)))
     
102.         {
     
103.             myPID.SetControllerDirection(DIRECT);
     
104.             Input=Klm_angle;
     
105.             myPID.Compute();
     
106.             analogWrite(Left1,min(Output+100,255));     
     
107.             analogWrite(Right1,min(Output+100,255));      
     
108.             analogWrite(Left2,0);      
     
109.             analogWrite(Right2,0);   
     
110.         }
     
111.     }
     
112.     else
     
113.     {
     
114.       car_stop();  
     
115.      }
     
116. }

复制代码

shihaipeng04

程序没看，说下思路好了， 看来程序应该是检测陀螺仪状态，然后调整小车姿态，保持平衡。 因为机械精度问题微小的震动也可能会被检测到，再加之车子调整幅度可能比较大，所以车子会一直抖动，往复运动于左右偏差之间。

思路，增加调整姿态的阀值，减小调整姿态的幅度。 或者根据偏离程度来决定调整幅度的大小。 也就是说，偏离1度 用 相对微弱的幅度和速度调整姿态，偏离10度，就大幅度和速度的调整姿态。

包哈达

i2cdev是干啥的啊  给个呗[email protected]谢啦

包哈达

i2cdev是干啥的啊  给个呗[email protected]谢啦，刚才打错了