自平衡小车，无论如何立不起来，可能是什么原因？修改，已经立起来了，谢谢pz_cloud

liangquan

配置：
Arduino UNO
L298N
MPU6050
直流电机12V，无反馈，150r/min

采用PID_v1库对直流电机进行速度控制，希望小车不倒，可是无论怎样调整参数，最好的效果是小车大幅震荡，无法直立。

代码

1. #include<Wire.h>
   
2. #include "gyro_accel.h"
   
3. #include "PID_v1.h"
   
4. 
   
5. // Defining constants
   
6. #define dt 1            // time difference in milli seconds
   
7. #define rad2degree 57.3  // radian to degree conversion
   
8. #define Filter_gain 0.95  // e.g. angle = angle_gyro*Filter_gain + angle_accel*(1-Filter_gain)
   
9. 
   
10. // Global Variables
    
11. unsigned long t = 0;      // Time Variables
    
12. float angle_x_gyro=0, angle_y_gyro=0, angle_z_gyro=0, angle_x_accel=0, angle_y_accel=0, angle_z_accel=0, angle_x=0, angle_y=0, angle_z=0;
    
13. 
    
14. // DC motor driver with L298N
    
15. const int motor1PWMPin = 5; // PWM Pin of Motor 1
    
16. const int motor1Polarity1 = 4; // Polarity 1 Pin of Motor 1
    
17. const int motor1Polarity2 = 7;  // Polarity 2 Pin of Motor 1
    
18. const int motor2PWMPin = 6;  // PWM Pin of Motor 2
    
19. const int motor2Polarity1 = 8; // Polarity 1 Pin of Motor 2
    
20. const int motor2Polarity2 = 12;  // Polarity 2 Pin of Motor 2
    
21. 
    
22. int ValM1 = 255;  // Initial Value for PWM Motor 1
    
23. int ValM2 = 255;  // Initial Value for PWM Motor 2
    
24. 
    
25. double Setpoint, Input, Output;
    
26. 
    
27. PID myPID(&Input, &Output, &Setpoint, 10, 0, 1, DIRECT);
    
28. 
    
29. void setup()
    
30. {
    
31.   // MPU-6050
    
32.   Serial.begin(115200);
    
33.   Wire.begin();
    
34.   MPU6050_ResetWake();
    
35.   MPU6050_SetGains(0,1);  // Setting the lows scale
    
36.   MPU6050_SetDLPF(0);    // Setting the DLPF to inf Bandwidth for calibration
    
37.   MPU6050_OffsetCal();  // very important
    
38.   MPU6050_SetDLPF(6);  // Setting the DLPF to lowest Bandwidth
    
39.   
    
40.   t = millis();
    
41.   
    
42.   // DC motor
    
43.   pinMode(motor1PWMPin, OUTPUT);
    
44.   pinMode(motor1Polarity1, OUTPUT);
    
45.   pinMode(motor1Polarity2, OUTPUT);
    
46.   
    
47.   pinMode(motor2PWMPin, OUTPUT);
    
48.   pinMode(motor2Polarity1, OUTPUT);
    
49.   pinMode(motor2Polarity2, OUTPUT);
    
50.   
    
51.   // set enablePin of motor 1 high so that motor 1 can turn on
    
52.   digitalWrite(motor1PWMPin, HIGH);
    
53.   digitalWrite(motor1Polarity1, HIGH);
    
54.   digitalWrite(motor1Polarity2, LOW);
    
55. 
    
56.   // set enablePin of motor 2 high so that motor 2 can turn on  
    
57.   digitalWrite(motor2PWMPin, HIGH);
    
58.   digitalWrite(motor2Polarity1, HIGH);
    
59.   digitalWrite(motor2Polarity2, LOW);
    
60.   
    
61.   Input = 0.0;
    
62.   Setpoint = 0.0;
    
63.   
    
64.   myPID.SetMode(AUTOMATIC);
    
65.   myPID.SetSampleTime(1);
    
66. }
    
67. 
    
68. void loop()
    
69. {
    
70.   t = millis();
    
71.   
    
72.   MPU6050_ReadData();
    
73.   
    
74.   angle_x_gyro = (gyro_x_scalled*((float)dt/1000)+angle_x);
    
75.   angle_y_gyro = (gyro_y_scalled*((float)dt/1000)+angle_y);
    
76.   angle_z_gyro = (gyro_z_scalled*((float)dt/1000)+angle_z);
    
77. 
    
78.   angle_z_accel = atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel_x_scalled)))*(float)rad2degree;
    
79.   angle_y_accel = -atan(accel_x_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
    
80.   angle_x_accel = atan(accel_y_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
    
81. 
    
82.   angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
    
83.   angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
    
84.   angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;
    
85.   
    
86.   digitalWrite(motor1PWMPin, HIGH);
    
87.   digitalWrite(motor2PWMPin, HIGH);
    
88.   
    
89.   Serial.print(angle_x);
    
90.   Serial.print("\t");
    
91.   
    
92.   // change direction is very important
    
93.   if(angle_x>0)
    
94.   {
    
95.     myPID.SetControllerDirection(REVERSE);
    
96.    
    
97.     // set enablePin of motor 1 high so that motor 1 can turn on
    
98.     digitalWrite(motor1Polarity1, HIGH);
    
99.     digitalWrite(motor1Polarity2, LOW);
    
100. 
     
101.     // set enablePin of motor 2 high so that motor 2 can turn on  
     
102.     digitalWrite(motor2Polarity1, HIGH);
     
103.     digitalWrite(motor2Polarity2, LOW);
     
104.   }
     
105.   else
     
106.   {
     
107.     myPID.SetControllerDirection(DIRECT);
     
108.     // set enablePin of motor 1 high so that motor 1 can turn on
     
109.     digitalWrite(motor1Polarity1, LOW);
     
110.     digitalWrite(motor1Polarity2, HIGH);
     
111. 
     
112.     // set enablePin of motor 2 high so that motor 2 can turn on  
     
113.     digitalWrite(motor2Polarity1, LOW);
     
114.     digitalWrite(motor2Polarity2, HIGH);
     
115.   }
     
116.   
     
117.   Input = angle_x;
     
118.   Serial.print(Input);
     
119.   Serial.print("\t");
     
120.   
     
121.   myPID.Compute();
     
122.   Serial.print(Output);
     
123.   Serial.print("\n");
     
124.   
     
125.   analogWrite(motor1PWMPin, Output);
     
126.   analogWrite(motor2PWMPin, Output);
     
127.   
     
128.   while((millis()-t) < dt)  // Maing sure the cycle time is equal to dt
     
129.   {
     
130.     // Do nothing
     
131.   }
     
132. }
     

复制代码

视频中的试验是通过USB线为Arduino供电，USB线起到了辅助支撑的作用，如果撤掉USB线，小车将震荡的更加厉害，最终无法稳定直立。

可能是什么原因呢？

1. MPU6050放置的不平？

2. 小车的电机最高转速太慢或太快？

3. PID参数还不对？

视频

liangquan

pz_cloud 发表于 2014-9-28 14:04
跟重心一样，加速度计也是越贴近轮轴越好，放高了会发现噪声非常严重，这是我试验过的

因为车子不是 ...

一语惊醒梦中人。
我将MPU6050的高度降低，只用一个参数P，小车就立起来了。

您的一句话避免了我走许多弯路，谢谢！！！

zzp1383

虽然我没有玩过平衡小车，但是从你的硬件配置及软件来看，没有反馈，就需要用模糊理论控制，也就是必须选择一个正确的常量，决定这个常量的因素有很多，电机转速、电压、负载、车轮大小、6050的安装位置等等，这些因素中，有一个变化，相应的常量值就需要改变，所以，小车立不起来是正常的，建议你在电路中加入电机反馈控制，调整起来会相对容易一点，否则，只能靠运气了！另外，电机的电压也是很关键的因素，电压决定转速！除非你用步进电机。

liangquan

zzp1383 发表于 2014-9-27 12:30
虽然我没有玩过平衡小车，但是从你的硬件配置及软件来看，没有反馈，就需要用模糊理论控制，也就是必须选择 ...

非常感谢您中肯的回答，我尝试用您说的方法改进一下。

我想到的几点：
我打算提高小车的重心，生活的经验告诉我，用手来平衡一个短棍保持直立，比用手平衡一个长棍保持直立，要难一些，我打算提高小车的重心试试。

加反馈需要在电机后面加编码器吧？我现在买的电机后端无轴伸，恐怕加编码器要困难，看来立不起来，要买新的电机了

zzp1383

    提高重心应该是一个不错的做法，我看到你的帖子，一时手痒，也准备仿一个，正在准备材料过程中，当然是借用你的程序，谢谢！也建议你在手头没有编码器电机的情况下，是否考虑步进电机？
    传感器加高，带来的数据变化应该是角度范围扩大，PID常量也需要对应加大，成熟、完整的自平衡小车，还需要考虑负载因素。我会关注你的帖子，并向你持续学习，再次感谢！！！！！！！！！

liangquan

zzp1383 发表于 2014-9-28 12:03
提高重心应该是一个不错的做法，我看到你的帖子，一时手痒，也准备仿一个，正在准备材料过程中，当然是 ...

互相学习，如果您解决了我的问题，也请告诉我！谢谢！

pz_cloud

liangquan 发表于 2014-9-27 19:32
非常感谢您中肯的回答，我尝试用您说的方法改进一下。

我想到的几点：

1.pid采样时间太短了，可能导致角度获取不准确，一般5ms完全足够了，对arduino的速度来说，10ms比较合适

2.很明确地告诉你，重心越低越好，一个极限情况是，当低至轮轴，就不需要调节也能平衡了，我们做飞思卡尔的时候都是尽量把重心降低甚至不惜增加重量改变重心

3.影响车子的参数太多了，一步一步排查，从角度获取开始，把融合的角度串口输出来，看有没有漂移，延时怎么样，这是稳定的基础，然后看你的描述应该参数有点大了，先调p，另两个参数置0，大概可以前后震荡着平衡了就够了，再一点点加d，最后看有没必要加i，这个跟你电机有关，电机响应好的话不需要i

4.需要注意的是，参数不一定成比例的，一个参数可能比另一个大一个甚至好几个数量级，不要弄错标度，比如一开始就给d加1，可能d的合适参数是0.01

5.可以看一下我发的那个帖子里的直立车的指导教程，会很有帮助

pz_cloud

zzp1383 发表于 2014-9-28 12:03
提高重心应该是一个不错的做法，我看到你的帖子，一时手痒，也准备仿一个，正在准备材料过程中，当然是 ...

跟重心一样，加速度计也是越贴近轮轴越好，放高了会发现噪声非常严重，这是我试验过的

因为车子不是刚体，而加速度计是噪声敏感原件，陀螺仪影响不大

liangquan

pz_cloud 发表于 2014-9-28 14:04
跟重心一样，加速度计也是越贴近轮轴越好，放高了会发现噪声非常严重，这是我试验过的

因为车子不是 ...

我的MPU-6050是加速度计和陀螺仪的组合，我试试把它放的低一点

liangquan

pz_cloud 发表于 2014-9-28 13:54
1.pid采样时间太短了，可能导致角度获取不准确，一般5ms完全足够了，对arduino的速度来说，10ms比较合适 ...

谢谢您的回答这么详细，我试试！

pz_cloud

不客气，新手善用搜索可以避免很多麻烦，论坛里平衡车的资料很多了

http://geek-workshop.com/thread-5688-1-1.html

http://geek-workshop.com/thread-9683-1-1.html

这是我以前发的方案，可以参考一下