三部曲之一，单维度PID调整，上视频

camion

冷静一点后，觉得四轴飞行器一步到位不太现实，不如分三步曲去做

计划：
1. 用PID控制单维度（如角度），达到较好的自动化效果
2. 自平衡小车（实际上就是1加上轮子）
3. 四轴飞行器

第一步昨天做通了，在PID参数上有些体会，先上视频，效果是让陀螺仪保持45度水平角


再上代码

1. #include <I2Cdev.h>
   
2. #include <MPU6050.h>
   
3. #include <Wire.h>//添加必须的库文件
   
4. //====一下三个定义了陀螺仪的偏差===========
   
5. #define Gx_offset 0.2
   
6. #define Gy_offset 0.81
   
7. #define Gz_offset -0.88
   
8. 
   
9. //====================
   
10. MPU6050 accelgyro;
    
11. 
    
12. int16_t ax,ay,az;
    
13. int16_t gx,gy,gz;//存储原始数据
    
14. 
    
15. double aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
    
16. double tx,ty;
    
17. double Ax,Ay,Az;//单位 g(9.8m/s^2)
    
18. double Gx,Gy,Gz;//单位 °/s
    
19. 
    
20. static double vari_estima_X= 0.1;
    
21. static double vari_estima_Y= 0.1;
    
22. static double vari_metron=0.0001;
    
23. static double KG_X = 0.5;
    
24. static double KG_Y = 0.5;
    
25. static double metron_X =0;
    
26. static double metron_Y =0;
    
27. static double estima_X = 0.2;
    
28. static double estima_Y = 0.2;
    
29. static int time_span_micro =50;
    
30. static float time_span =0.05;
    
31. double now_error, last_error, error_diff, error_int;
    
32. double fusion_angle =0;
    
33. 
    
34. double Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
    
35. 
    
36. long LastTime,NowTime,TimeSpan;//用来对角速度积分的
    
37.    
    
38. void setup()//MPU6050的设置都采用了默认值，请参看库文件
    
39. {
    
40.   Wire.begin();
    
41. 
    
42.   Serial.begin(38400);
    
43. 
    
44.   Serial.println("Initializing I2C device.....");
    
45.   accelgyro.initialize();
    
46. 
    
47.   Serial.println("Testing device connections...");
    
48.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
    
49. 
    
50. }
    
51. 
    
52. void loop()
    
53. {
    
54.   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
    
55. //  Serial.print(ax/16384.00);
    
56. //  Serial.print(gx/131.00);
    
57. //======一下三行是对加速度进行量化，得出单位为g的加速度值
    
58.    Ax=ax/16384.00;
    
59.    Ay=ay/16384.00;
    
60.    Az=az/16384.00;
    
61.    //==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
    
62. //请参考：[url]http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876[/url]
    
63. //个人觉得原帖中case0算的不对，应该是z/sqrt（x*x+y*y），估计是江南楼主写错了
    
64. 
    
65.    Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Gx_offset; //offset
    
66.    Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Gy_offset; //offset
    
67.    Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
    
68.    //==========以下三行是对角速度做量化==========
    
69.    
    
70. //   ggx=gx/131.00;
    
71. //   ggy=gy/131.00;
    
72. //   ggz=gz/131.00;
    
73. 
    
74.   //===============以下是对角度进行积分处理================
    
75.   NowTime=millis();//获取当前程序运行的毫秒数
    
76.   TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
    
77. //下面三行就是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
    
78. 
    
79.   Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
    
80.   Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
    
81.   Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;
    
82. 
    
83.   
    
84.   automatic();
    
85.   
    
86.   LastTime=NowTime;  
    
87.   
    
88. //  analogWrite(6, 0);
    
89. //  analogWrite(5, 80); //conter clockwise
    
90.   
    
91. //  KG_X = KG_cal(vari_estima_X, vari_metron) ;
    
92. //  KG_Y = KG_cal(vari_estima_Y, vari_metron) ;
    
93.   KG_X =0.1;
    
94.   KG_Y =0.1;
    
95.   estima_X = estima_cal(estima_X, Angel_accX, KG_X);
    
96.   estima_Y = estima_cal(estima_Y, Angel_accY, KG_Y);
    
97. //  vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
    
98. //  vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
    
99. 
    
100.   
     
101.   //==============================
     
102. //    Serial.print(Angel_accX);
     
103. //    Serial.print(",");
     
104. //    Serial.println(Angel_accY);
     
105.      
     
106. //    Serial.print(estima_X);
     
107. //    Serial.print(",");
     
108. //    Serial.println(estima_Y);  
     
109.    
     
110. //    Serial.print(Ax);
     
111. //    Serial.print(",");
     
112. //    Serial.println(Ay);  
     
113.       
     
114.     //delay(time_span_micro);//这个用来控制采样速度
     
115. }
     
116. 
     
117. //void updateKalman (double my_metron_X,  double my_metron_Y){
     
118. //  KG_X = KG_cal(vari_estima_X, vari_metron);
     
119. //  KG_Y = KG_cal(vari_estima_Y, vari_metron);
     
120. //  estima_X = estima_cal(estima_X, my_metron_X, KG_X);
     
121. //  estima_Y = estima_cal(estima_Y, my_metron_Y, KG_Y);
     
122. //  vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
     
123. //  vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
     
124. //}
     
125. 
     
126. //------------------------------------------------------------------
     
127. double KG_cal(double vari_estima, double vari_metron){
     
128.   double temp = vari_estima*vari_estima / (vari_estima*vari_estima+ vari_metron*vari_metron);
     
129.   double result =sqrt(temp);
     
130.   return result;
     
131. }
     
132. //------------------------------------------------------------------
     
133. double estima_cal (double estima, double metron, double KG){
     
134.   double result1 = estima + KG*(metron - estima);
     
135.   return result1;
     
136. }
     
137. //------------------------------------------------------------------
     
138. double vari_estima_cal (double KG, double vari_estima){
     
139.   double result2 = vari_estima * sqrt(1-KG);
     
140.   return result2;
     
141. }
     
142. 
     
143. double PID_control(double error, double error_diff, double error_int ){
     
144.   double P=2;
     
145.   double I=1;
     
146.   double D=1;
     
147.   double command = P*error + I*error_int + D*error_diff;
     
148. 
     
149.   return command;
     
150. }
     
151. 
     
152. 
     
153. 
     
154. void automatic(){
     
155.   
     
156. //  Serial.print("now_error=");
     
157. //  Serial.println(now_error);
     
158. //  Serial.print("error_diff=");
     
159. //  Serial.println(error_diff);
     
160. 
     
161.   double command = PID_control(now_error, error_diff, error_int);
     
162. //  Serial.print("command=");
     
163. //  Serial.println(command);
     
164.   
     
165.   int regu_pwm = int (command);
     
166.   
     
167.   //Serial.println(regu_pwm);
     
168.   
     
169.   if(regu_pwm >= 250){
     
170.     regu_pwm=250;
     
171.     analogWrite(6, 0);
     
172.     analogWrite(5, regu_pwm); //conter clockwise
     
173.     Serial.println("clock");
     
174.    
     
175.   }else if (regu_pwm > 0 & regu_pwm<250){
     
176.     analogWrite(6, 0);
     
177.     analogWrite(5, regu_pwm); //conter clockwise
     
178.     Serial.println("clock");
     
179.   
     
180.   }else if (regu_pwm <= 0 & regu_pwm > -250){
     
181.     analogWrite(5, 0);
     
182.     analogWrite(6, regu_pwm); // clockwise
     
183.     Serial.println("CONTER CLOCK");
     
184.   }else if (regu_pwm <=-250){
     
185.     regu_pwm=250;
     
186.     analogWrite(5, 0);
     
187.     analogWrite(6, regu_pwm); // clockwise
     
188.     Serial.println("CONTER CLOCK");
     
189.   }
     
190.   delay (time_span_micro);
     
191.   //fusion_angle = update_Estimat(fusion_angle, estima_Y, Gy );
     
192.   autoCorrection();
     
193.   Serial.print("estima_Y=");
     
194.   Serial.println(estima_Y);
     
195.   
     
196. //  uodate_error(time_span, 45, Angel_accY);
     
197.   uodate_error(time_span, 45, estima_Y);
     
198. 
     
199.   //uodate_error(time_span, 45, fusion_angle);
     
200. }
     
201. void uodate_error(double time_span, double consigne, double mesure){
     
202.   now_error = consigne - mesure;
     
203.   error_diff = (now_error - last_error)/time_span;
     
204.   error_int = error_int + now_error*time_span;
     
205.   last_error =now_error ;
     
206. }
     
207. 
     
208. double update_Estimat(double estimate, double metron_1, double metron_2){
     
209.   double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
     
210.   return temp;
     
211. }
     
212. 
     
213. void autoCorrection (){
     
214.   if ( Angel_accX > -0.2 & Angel_accX < 0.2 ){
     
215.   Gx=0;
     
216.   }
     
217. }

复制代码

接下来是体会，有这么几点：
1。  P参数越大，回复速度越快，但同时越容易调整过头出现震荡，因此适合同时调大D参数，预计惯性累加量来及时减小电机转动输出（接近要到达位置时）。故曰：大P配大D，小P配小D

2 。 卡尔曼滤波函数 updateKalman 其实没有完全调通，主要是卡尔曼增益一下子就趋近于0了，导致角度响应变得非常慢。不过，如果一直将卡尔曼增益固定在0.1 （见estima_cal函数），反而波形变得很平滑而且响应速度很好。

3。 自己做的融合算法简直就是狗P （见fusion_angle），暂时不报以希望。网上说通过三轴角速度得到的角度容易受运动影响，我保持一个角度拿着mpu6050，突然超某方向运动（但维持角度不变），在串口输出上看到影响不是那么大，可能是因为已经滤过波的吧。

4。 PID函数仍然不那么精确， 理论上说在平衡位置（这里设的是45度）时，PID控制的输出量为零，但我这里还有个十几，当然十几的pwm是带不动电机的，所以视频上面表现的还可以。

5。 autoCorrection函数是用来修正陀螺仪误差的，每次三轴加速度得到的角度为零时，也会将陀螺仪积分的角度归零，可囧的是，如果一直保持非水平位置的话，好像都没有修正的机会


希望各位为我做指点。

MicroCao

感谢分享{:soso_e179:}

隨風大俠

我看很多人說互補濾波也不錯，樓主可以試試看~。

隨風大俠

請問command最後怎麼跟控制值融合的啊??
是哪一行?啊?

pumpitup

留爪待学···

camion

TTTTTTT33 发表于 2013-6-2 13:09
請問command最後怎麼跟控制值融合的啊??
是哪一行?啊?

double command = PID_control(now_error, error_diff, error_int);
int regu_pwm = int (command);

然后防饱和处理,如果大于pwm范围255就用极值, 这里用的250,
if.. else if ... else if ....
分正转和反转写入analog pin
analogWrite(6, 0);172.   
analogWrite(5, regu_pwm); //conter clockwise

隨風大俠

camion 发表于 2013-6-2 13:17
double command = PID_control(now_error, error_diff, error_int);
int regu_pwm = int (command);

可以用這個設定範圍constrain(X, outMin, outMax);

隨風大俠

我用互補濾波時發現，如果採樣速度太慢會衝過頭，但採樣速度變快時會對抖動的影響加大，不知道該怎麼辦......

camion

靠，搞清楚为什么融合算法失效了

原因是： 原帖代码中三轴加速度的XY 和 陀螺仪的XY 是反过来的
现在上正确的融合算法，比较了一下，两种角度测法结果差别不是特别大，最多可能差十几度吧，融合后介于两者之间

1. #include <I2Cdev.h>
   
2. #include <MPU6050.h>
   
3. #include <Wire.h>//添加必须的库文件
   
4. //====一下三个定义了陀螺仪的偏差===========
   
5. #define Gx_offset 0.2
   
6. #define Gy_offset 0.81
   
7. #define Gz_offset -0.88
   
8. 
   
9. //====================
   
10. MPU6050 accelgyro;
    
11. 
    
12. int16_t ax,ay,az;
    
13. int16_t gx,gy,gz;//存储原始数据
    
14. 
    
15. double aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
    
16. double tx,ty;
    
17. double Ax,Ay,Az;//单位 g(9.8m/s^2)
    
18. double Gx,Gy,Gz;//单位 °/s
    
19. 
    
20. static double vari_estima_X= 0.1;
    
21. static double vari_estima_Y= 0.1;
    
22. static double vari_metron=0.0001;
    
23. static double KG_X = 0.5;
    
24. static double KG_Y = 0.5;
    
25. static double metron_X =0;
    
26. static double metron_Y =0;
    
27. static double estima_X = 0.2;
    
28. static double estima_Y = 0.2;
    
29. static int time_span_micro =50;
    
30. static float time_span =0.05;
    
31. double now_error, last_error, error_diff, error_int;
    
32. double fusion_angle =0;
    
33. 
    
34. double Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
    
35. 
    
36. long LastTime,NowTime,TimeSpan;//用来对角速度积分的
    
37.    
    
38. void setup()//MPU6050的设置都采用了默认值，请参看库文件
    
39. {
    
40.   Wire.begin();
    
41. 
    
42.   Serial.begin(38400);
    
43. 
    
44.   Serial.println("Initializing I2C device.....");
    
45.   accelgyro.initialize();
    
46. 
    
47.   Serial.println("Testing device connections...");
    
48.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
    
49. 
    
50. }
    
51. 
    
52. void loop()
    
53. {
    
54.   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
    
55. //  Serial.print(ax/16384.00);
    
56. //  Serial.print(gx/131.00);
    
57. //======一下三行是对加速度进行量化，得出单位为g的加速度值
    
58.    Ax=ax/16384.00;
    
59.    Ay=ay/16384.00;
    
60.    Az=az/16384.00;
    
61.    //==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
    
62. //请参考：[url]http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876[/url]
    
63. //个人觉得原帖中case0算的不对，应该是z/sqrt（x*x+y*y），估计是江南楼主写错了
    
64. 
    
65.    Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Gx_offset; //offset
    
66.    Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Gy_offset; //offset
    
67.    Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
    
68.    //==========以下三行是对角速度做量化==========
    
69.    
    
70.    ggx=gx/131.00;
    
71.    ggy=gy/131.00;
    
72.    ggz=gz/131.00;
    
73. 
    
74.   //===============以下是对角度进行积分处理================
    
75.   NowTime=millis();//获取当前程序运行的毫秒数
    
76.   
    
77. //下面三行就是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
    
78.   TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
    
79.   
    
80.   Gx=Gx+(ggx-Gx_offset)*time_span_micro/1000;
    
81.   Gy=Gy+(ggy-Gy_offset)*time_span_micro/1000;
    
82.   Gz=Gz+(ggz-Gz_offset)*time_span_micro/1000;
    
83.   Serial.print("Gy=");
    
84.   Serial.println(Gx);  
    
85.   automatic();
    
86.   
    
87. 
    
88.   LastTime=NowTime;  
    
89.   
    
90. //  KG_X = KG_cal(vari_estima_X, vari_metron) ;
    
91. //  KG_Y = KG_cal(vari_estima_Y, vari_metron) ;
    
92.   KG_X =0.1;
    
93.   KG_Y =0.1;
    
94.   estima_X = estima_cal(estima_X, Angel_accX, KG_X);
    
95.   estima_Y = estima_cal(estima_Y, Angel_accY, KG_Y);
    
96. //  vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
    
97. //  vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
    
98. 
    
99.   //==============================
    
100. //    Serial.print(Angel_accX);
     
101. //    Serial.print(",");
     
102. //    Serial.println(Angel_accY);
     
103.      
     
104. //    Serial.print(estima_X);
     
105. //    Serial.print(",");
     
106. //    Serial.println(estima_Y);  
     
107.    
     
108. //    Serial.print(Gx);
     
109. //    Serial.print(",");
     
110. 
     
111.       
     
112.     //delay(time_span_micro);//这个用来控制采样速度
     
113. }
     
114. 
     
115. void updateKalman (double my_metron_X,  double my_metron_Y){
     
116.   KG_X = KG_cal(vari_estima_X, vari_metron);
     
117.   KG_Y = KG_cal(vari_estima_Y, vari_metron);
     
118.   estima_X = estima_cal(estima_X, my_metron_X, KG_X);
     
119.   estima_Y = estima_cal(estima_Y, my_metron_Y, KG_Y);
     
120.   vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
     
121.   vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
     
122. }
     
123. 
     
124. //------------------------------------------------------------------
     
125. double KG_cal(double vari_estima, double vari_metron){
     
126.   double temp = vari_estima*vari_estima / (vari_estima*vari_estima+ vari_metron*vari_metron);
     
127.   double result =sqrt(temp);
     
128.   return result;
     
129. }
     
130. //------------------------------------------------------------------
     
131. double estima_cal (double estima, double metron, double KG){
     
132.   double result1 = estima + KG*(metron - estima);
     
133.   return result1;
     
134. }
     
135. //------------------------------------------------------------------
     
136. double vari_estima_cal (double KG, double vari_estima){
     
137.   double result2 = vari_estima * sqrt(1-KG);
     
138.   return result2;
     
139. }
     
140. 
     
141. double PID_control(double error, double error_diff, double error_int ){
     
142.   double P=1.8;
     
143.   double I=1;
     
144.   double D=1;
     
145.   double command = P*error + I*error_int + D*error_diff;
     
146.   return command;
     
147. }
     
148. 
     
149. void automatic(){
     
150.   
     
151. //  Serial.print("now_error=");
     
152. //  Serial.println(now_error);
     
153. //  Serial.print("error_diff=");
     
154. //  Serial.println(error_diff);
     
155. 
     
156.   double command = PID_control(now_error, error_diff, error_int);
     
157. //  Serial.print("command=");
     
158. //  Serial.println(command);
     
159.   
     
160.   int regu_pwm = int (command);
     
161.   
     
162.   //Serial.println(regu_pwm);
     
163.   
     
164. //  if(regu_pwm >= 250){
     
165. //    regu_pwm=250;
     
166. //    analogWrite(6, 0);
     
167. //    analogWrite(5, regu_pwm); //conter clockwise
     
168. //    //Serial.println("clock");
     
169. //   
     
170. //  }else if (regu_pwm > 0 & regu_pwm<250){
     
171. //    analogWrite(6, 0);
     
172. //    analogWrite(5, regu_pwm); //conter clockwise
     
173. //    //Serial.println("clock");
     
174. //  
     
175. //  }else if (regu_pwm <= 0 & regu_pwm > -250){
     
176. //    analogWrite(5, 0);
     
177. //    analogWrite(6, regu_pwm); // clockwise
     
178. //    //Serial.println("CONTER CLOCK");
     
179. //  }else if (regu_pwm <=-250){
     
180. //    regu_pwm=250;
     
181. //    analogWrite(5, 0);
     
182. //    analogWrite(6, regu_pwm); // clockwise
     
183. //    //Serial.println("CONTER CLOCK");
     
184. //  }
     
185.   delay (time_span_micro);
     
186.   
     
187.   
     
188.   fusion_angle = update_Estimat(fusion_angle, estima_Y, Gx );
     
189.   autoCorrection();
     
190. //  Serial.print("estima_Y=");
     
191. //  Serial.println(estima_Y);
     
192. //  Serial.print("fusion_angle=");
     
193. //  Serial.println(fusion_angle);
     
194.   
     
195. //  uodate_error(time_span, 45, Angel_accY);
     
196. //  uodate_error(time_span, 0, estima_Y);
     
197. 
     
198.   uodate_error(time_span, 45, fusion_angle);
     
199. }
     
200. void uodate_error(double time_span, double consigne, double mesure){
     
201.   now_error = consigne - mesure;
     
202.   error_diff = (now_error - last_error)/time_span;
     
203.   error_int = error_int + now_error*time_span;
     
204.   last_error =now_error ;
     
205. }
     
206. 
     
207. double update_Estimat(double estimate, double metron_1, double metron_2){
     
208.   double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
     
209.   return temp;
     
210. }
     
211. 
     
212. void autoCorrection (){
     
213.   
     
214.     Serial.print("Angel_accY=");
     
215.     Serial.println(Angel_accY);
     
216.   if ( Angel_accY > -0.5 & Angel_accY < 0.5 ){
     
217.     Serial.println("00000000000000");
     
218.     Gx=0;
     
219.   }
     
220.   
     
221.   if ( Angel_accX > -0.5 & Angel_accX < 0.5 ){
     
222.     Serial.println("00000000000000");
     
223.     Gy=0;
     
224.   }
     
225. }

复制代码

玄冰之神

楼主分享的真不错 感谢分享 希望继续分享 看了好多天的MPU6050这东西的算法 还是算不出X和Y轴方向不受重力影响的加速度  现在想研究角度 通过时时的陀螺仪角度 利用空间几何算算法把 X和Y轴运动时的加速度重力分量去掉 希望得到楼主指导。有没有更简捷的方法得到我想要的数据

advesl

mark！！！！！！！！！！！！！！！！！！！！！！