無人機加速計、氣壓計、GPS數(shù)據(jù)融合

在無人機控制當中，傳感器的參與那是必不可少的，特別是陀螺儀，最經(jīng)典的為MPU6050，目前大部分的無人機都是用的這個器件。熟悉MPU6050的都知道，這個傳感器可以輸出三軸加速計、三軸陀螺儀；加速計，顧名思義，直接測得物體的加速度。理論上  加速度積分得到速度，速度再積分得到位移：S = V0 * t + 1/2 * a * t ^ 2;V = v0 + a * t;按照此理想情況下：一個加速計直接積分算得速度跟位移就得了呀，為啥還要氣壓計、GPS呢。無人機為了定點懸停，需要計算XYZ三軸的速度跟位移，而加速度計有一個致命的缺點就是它積分會漂移，而且是漂移非常大的那種，如果直接用這個數(shù)據(jù)做控制，飛機都不知道要飄到哪里去了。所以才借助外部傳感器氣壓計、GPS修正加速計的漂移。加速計在無人機三軸位置速度計算時也有很大的優(yōu)點，就是其靈敏度相對很高，無人機稍微動一點加速計都可以感受的出來，然后把數(shù)據(jù)傳到無人機，無人機得以做相應(yīng)的調(diào)整。

數(shù)據(jù)融合

具體數(shù)據(jù)怎么融合呢？我們先來總結(jié)一下加速計、氣壓計、gps各自的優(yōu)缺點：加速計：優(yōu)點反應(yīng)速度快,缺點積分漂移嚴重。氣壓計GPS：優(yōu)點絕對位置比較平穩(wěn)，缺點反應(yīng)速度比較慢。所以就有了互補濾波這個偉大的，簡單易懂的濾波算法。

以下為互補濾波代碼：

void Gps_Acc_Delay(void){ volatile uint8 i; for (i = 0; i < XY_VEL_HISTORY - 1; i++) {  x_vel_history = x_vel_history[i+1];  y_vel_history = y_vel_history[i+1];    x_pos_history = x_pos_history[i+1];  y_pos_history = y_pos_history[i+1]; } x_vel_history[XY_VEL_HISTORY - 1] = x_est[1]; y_vel_history[XY_VEL_HISTORY - 1] = y_est[1]; x_pos_history[XY_VEL_HISTORY - 1] = x_est[0]; y_pos_history[XY_VEL_HISTORY - 1] = y_est[0];}void ObservationOPT_Update(float pos_x,float vel_x,float pos_y,float vel_y)  //have data and update{ uint8 i; corrOPT_xpos = pos_x-x_est[0]; corrOPT_xvel = vel_x-x_est[1]; corrOPT_ypos = pos_y-y_est[0]; corrOPT_yvel = vel_y-y_est[1]; return;}void ObservationUpdate(int baro_disdance,int16 baro_vel){ uint8 i; corr_baro = (float)baro_disdance - z_est[0]; if (fly_status.HOLD_STATUS == HOLD_STOP) {  z_est[0] = baro_disdance;  z_est[1] = 0; } return;}void TimeUpdate(struct vertical_information *pAltitude,realacc* acc){ static float high_pass_zvel = 0; z_est[2]=acc->zz-z_bias; accel_filter_predict(INAV_T,z_est); z_est[0] += corr_baro * w_z_baro * INAV_T; z_bias -= corr_baro * 0.001f * INAV_T; z_est[1] += corr_baro * baro_vel_gain * INAV_T; high_pass_zvel = z_est[1]; pAltitude->altitude_hat=z_est[0]; pAltitude->velocity_hat=high_pass_zvel; ObservationOPT_Update(x_est[0],optflow.velx,y_est[0],optflow.vely); x_est[2]=acc->xx-x_bias; accel_filter_predict(INAV_TXY,x_est);  x_est[0] += corrGPS_xpos * x_pos_gain * INAV_TXY; x_est[1] += (corrOPT_xvel*0 + corrGPS_xpos * 1.0f + corrGPS_xvel * 0.2f) * x_vel_gain * INAV_TXY; y_est[2]=acc->yy-y_bias; accel_filter_predict(INAV_TXY,y_est);  y_est[0] += corrGPS_ypos * y_pos_gain * INAV_TXY; y_est[1] += (corrOPT_yvel*0 + corrGPS_ypos * 1.0f + corrGPS_yvel * 0.2f) * y_vel_gain * INAV_TXY; if (GPS_is_Good) {  if (corrGPS_xvel > -10 && corrGPS_xvel < 10)    x_bias -= (corrGPS_xpos*0.3f + corrGPS_xvel) * INAV_TXY * 0.1f;  if (corrGPS_yvel > -10 && corrGPS_yvel < 10)    y_bias -= (corrGPS_ypos*0.3f + corrGPS_yvel) * INAV_TXY * 0.1f; } else {  x_est[1] = Body_dat.vel_x;  y_est[1] = Body_dat.vel_y;  x_est[0] = Body_dat.pos_x1;  y_est[0] = Body_dat.pos_y1; } pos_vel_xy.posx = Body_dat.pos_x1; pos_vel_xy.posy = Body_dat.pos_y1; pos_vel_xy.velx = x_est[1]; pos_vel_xy.vely = y_est[1];}

以上代碼就是無人機里的互補濾波修正。如有不懂歡迎留言或扣我1836703877。無人機四軸本人做了五年，以上代碼全部自己寫，上傳的沒來的及添加注釋，但是代碼時成功在飛機上跑的并且無人機出貨的。

好東東啊,樓主能把代碼放在代碼框中嗎?

可以的，待我整理一下

無人機 未來發(fā)展前途廣闊
大家說一下哪家的無人機技術(shù)優(yōu)秀

目前來看，還是大疆的比較牛逼，各方面穩(wěn)定，控制，都沒得說

1. _POS_NOWstatus pos_vel_xy;
   
2. float z_est[3]={0,0,0},x_est[3]={0,0,0},y_est[3]={0,0,0};
   
3. typedef struct {
   
4. float R[3][3];                    /**< Rotation matrix body to world, (Tait-Bryan, NED)*/
   
5. }vehicle_attitude_s;
   
6. 
   
7. vehicle_attitude_s      vehicle_att;
   
8. vehicle_attitude_s      vehicle_optical;
   
9. realacc real_acceleration;
   
10. _vertical_information  baro_acc_alt;
    
11. 
    
12. //#define ALT_VEL_HISTORY 10
    
13. //volatile float alt_vel_history[ALT_VEL_HISTORY] = {0};
    
14. 
    
15. //#define XY_VEL_HISTORY 3
    
16. //volatile float x_vel_history[XY_VEL_HISTORY] = {0}, y_vel_history[XY_VEL_HISTORY] = {0};
    
17. 
    
18. static float z_bias = 0,x_bias = 0,y_bias = 0;
    
19. float w_z_baro=0.5f;
    
20. static float corr_baro = 0;//corr_baro_filter = 0;
    
21. static float corr_xvel = 0,corr_yvel = 0;
    
22. static float corr_xpos = 0,corr_ypos = 0;
    
23. 
    
24. static float acck = 0;
    
25. 
    
26. void accel_filter_predict(float dt,float x[3])
    
27. {
    
28.         x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
    
29.         x[1] += x[2] * dt;
    
30. }
    
31. 
    
32. void inertial_filter_correct(float e,float dt,float x[3],int i,float w,float vp)
    
33. {
    
34.   float ewdt = e * w * dt;
    
35.         x[i] += ewdt;
    
36.         if(i == 0)
    
37.         {
    
38.                 x[1] += vp*w * ewdt;
    
39.         }
    
40. }
    
41. 
    
42. void check_stop(int16 accz_velocity,int16 accz)
    
43. {
    
44.         static uint8 slow_down_count=0;
    
45.         static uint16 down_time=0;
    
46.         down_time++;
    
47.         if(fly_status.HOLD_STATUS == HOLD_DOWN)
    
48.         {
    
49.                 if(down_time>=200)
    
50.                 {
    
51.                         if((accz_velocity>=0&&accz<50&&accz>-50) || (accz_velocity>=50) || (accz>300 || accz<-300))
    
52.                         {
    
53.                                 slow_down_count++;
    
54.                                 if(slow_down_count>3)
    
55.                                 {
    
56.                                         fly_status.HOLD_STATUS=HOLD_STOP;
    
57.                                 }
    
58.                         }
    
59.                         else if(slow_down_count>0)
    
60.                         {
    
61.                                 if(accz_velocity<0)
    
62.                                         slow_down_count=0;
    
63.                                 else
    
64.                                         slow_down_count--;
    
65.                         }
    
66.                 }
    
67.         }
    
68.         else
    
69.         {
    
70.                 down_time=0;
    
71.                 slow_down_count=0;
    
72.         }
    
73. }
    
74. 
    
75. 
    
76. void accel_baro_mag_gps_calculator(int16 accel_x,int16 accel_y,int16 accel_z)
    
77. {
    
78.         static uint8 usart1_count = 0;
    
79.        
    
80.         static int16 acc_xyz[3];
    
81.         static float no_filteraccxx,no_filteraccyy,no_filteracczz;
    
82.        
    
83.         if (accel_x != 0 && accel_y != 0 && accel_z != 0)
    
84.         {
    
85.                 acck=(1/Fylib_Qrsqrt(accel_x*accel_x + accel_y*accel_y+accel_z*accel_z)-MV_ACC_RP)/MV_ACC_RP;
    
86.                         if(acck<0) acck=-acck;
    
87.         }
    
88.        
    
89.         acc_xyz[0]=accel_x*980/MV_ACC_RP;acc_xyz[1]=accel_y*980/MV_ACC_RP;acc_xyz[2]=accel_z*980/MV_ACC_RP;
    
90.         //&Eacute;&Iuml;&Otilde;&yacute;&Iuml;&Acirc;&cedil;&ordm;&pound;&not;±±&Otilde;&yacute;&Auml;&Iuml;&cedil;&ordm;&pound;&not;&para;&laquo;&Otilde;&yacute;&Icirc;÷&cedil;&ordm;
    
91.         no_filteracczz=-acc_xyz[1]*vehicle_att.R[2][0]-acc_xyz[0]*vehicle_att.R[2][1]+acc_xyz[2]*vehicle_att.R[2][2]-980;
    
92.         no_filteraccxx=-(-acc_xyz[1]*vehicle_optical.R[0][0]-acc_xyz[0]*vehicle_optical.R[0][1]+acc_xyz[2]*vehicle_optical.R[0][2]);
    
93.         no_filteraccyy=(acc_xyz[1]*vehicle_optical.R[1][0]+acc_xyz[0]*vehicle_optical.R[1][1]-acc_xyz[2]*vehicle_optical.R[1][2]);
    
94.        
    
95. //        no_filteracczz += (float)(agx + agy);
    
96. //        no_filteraccxx += (float)(agy + agz);
    
97. //        no_filteraccyy += (float)(agx + agz);
    
98.        
    
99.        
    
100.         //high_pass
     
101.         real_acceleration.xx=(int16)no_filteraccxx;
     
102.         real_acceleration.yy=(int16)no_filteraccyy;
     
103.         real_acceleration.zz=(int16)no_filteracczz;
     
104.        
     
105.         TimeUpdate(&baro_acc_alt,(realacc*)&real_acceleration);
     
106.        
     
107.         check_stop(baro_acc_alt.velocity_hat,no_filteracczz);
     
108.        
     
109.         usart1_count++;
     
110.         if(usart1_count>=1)
     
111.         {
     
112.                 usart1_count = 0;
     
113. //          SendDatShowINT((int)z_est[0],(int)(baro_data.baro_home),(int)(baro_data.velocity*3.1f),(int)baro_to_alt);
     
114.         SendDatShow((int)y_est[1],(int)x_est[1],(int)(acck * 100),(int)baro_data.baro_home);
     
115.         }
     
116.        
     
117.        
     
118. }
     
119. 
     
120. void ahrs_update_R_bf_to_ef(float angle_pitch,float angle_roll,float angle_yaw)
     
121. {
     
122.         float sin_pitch = sin(angle_pitch * M_DEG_TO_RAD);
     
123.         float cos_pitch = cos(angle_pitch * M_DEG_TO_RAD);
     
124.         float sin_roll = sin(angle_roll * M_DEG_TO_RAD);
     
125.         float cos_roll = cos(angle_roll * M_DEG_TO_RAD);
     
126.         float sin_yaw = sin(angle_yaw * M_DEG_TO_RAD);
     
127.         float cos_yaw = cos(angle_yaw * M_DEG_TO_RAD);
     
128.        
     
129.         vehicle_att.R[0][0] = cos_pitch * cos_yaw;
     
130.         vehicle_optical.R[0][0]=cos_pitch;
     
131.         vehicle_att.R[0][1] = sin_roll * sin_pitch * cos_yaw + cos_roll * sin_yaw;
     
132.         vehicle_optical.R[0][1] = 0;//sin_roll * sin_pitch + cos_roll;
     
133.         vehicle_att.R[0][2] = cos_roll * sin_pitch * cos_yaw - sin_roll * sin_yaw;
     
134.         vehicle_optical.R[0][2] = sin_pitch;//cos_roll * sin_pitch - sin_roll;
     
135.        
     
136.         vehicle_att.R[1][0] = cos_pitch * sin_yaw;
     
137.         vehicle_optical.R[1][0] = 0;//cos_pitch;
     
138.         vehicle_att.R[1][1] = -sin_roll * sin_pitch * sin_yaw - cos_roll * cos_yaw;
     
139.         vehicle_optical.R[1][1] =cos_roll;// -sin_roll * sin_pitch - cos_roll;
     
140.         vehicle_att.R[1][2] = cos_roll * sin_pitch * sin_yaw + sin_roll * cos_yaw;
     
141.         vehicle_optical.R[1][2] =-sin_roll;// cos_roll * sin_pitch;// + sin_roll;
     
142.        
     
143.         vehicle_att.R[2][0] = -sin_pitch;
     
144.         vehicle_att.R[2][1] = sin_roll * cos_pitch;
     
145.         vehicle_att.R[2][2] = cos_roll * cos_pitch;       
     
146.        
     
147.        
     
148. }
     
149. 
     
150. #define HIGH_PASS_PARAM 0.999f
     
151. float baro_vel_gain = 0.2f, x_vel_gain = 1.8f, y_vel_gain = 1.8f;
     
152. 
     
153. float baro_gain=0.05f;
     
154. 
     
155. void TimeUpdate(struct vertical_information *pAltitude,realacc* acc)
     
156. {
     
157.         static float high_pass_zvel = 0;//, z_est1f = 0;
     
158.         z_est[2]=acc->zz-z_bias;
     
159.         accel_filter_predict(INAV_T,z_est);
     
160.         z_est[0] += corr_baro * w_z_baro * INAV_T;
     
161.         z_bias -= corr_baro * 0.001f * INAV_T;
     
162.        
     
163.        
     
164.         z_est[1] += corr_baro * baro_vel_gain * INAV_T;
     
165.        
     
166. //        if (z_est[1] >-20 && z_est[1] <20)
     
167. //          high_pass_zvel = (high_pass_zvel + z_est[1] - z_est1f) * HIGH_PASS_PARAM;
     
168. //        else
     
169.                 high_pass_zvel = z_est[1];
     
170. //        z_est1f = z_est[1];
     
171.        
     
172.         pAltitude->altitude_hat=z_est[0];
     
173.         pAltitude->velocity_hat=high_pass_zvel;
     
174.        
     
175.         ObservationOPT_Update(x_est[0],optflow.velx,y_est[0],optflow.vely);
     
176.        
     
177.         x_est[2]=acc->xx-x_bias;
     
178.         accel_filter_predict(INAV_T,x_est);
     
179.         x_est[1] += corr_xvel * x_vel_gain * INAV_T;
     
180.        
     
181.         y_est[2]=acc->yy-y_bias;
     
182.         accel_filter_predict(INAV_T,y_est);
     
183.         y_est[1] += corr_yvel * y_vel_gain * INAV_T;
     
184.        
     
185.         pos_vel_xy.posx = 0;
     
186.         pos_vel_xy.posy = 0;
     
187.         pos_vel_xy.velx = optflow.velx;//x_est[1];
     
188.         pos_vel_xy.vely = optflow.vely;//y_est[1];
     
189. }
     
190. 
     
191. __AVERAGE_DATAF altitude_vcal={0,0};
     
192. 
     
193. float ABS_f(float values)
     
194. {
     
195.         if (values < 0 )
     
196.                 return -values;
     
197.         else
     
198.                 return values;
     
199. }
     
200. 
     
201. void ObservationUpdate(int baro_disdance,int16 baro_vel)
     
202. {
     
203.         uint8 i;
     
204.         corr_baro = (float)baro_disdance - z_est[0];
     
205.        
     
206.         if (corr_baro >20 || corr_baro <-20)
     
207.                 baro_vel_gain = 0.4f;
     
208.         else
     
209.                 baro_vel_gain = 0.2f;
     
210.        
     
211.         if (fly_status.HOLD_STATUS == HOLD_STOP)
     
212.         {
     
213.                 z_est[0] = baro_disdance;
     
214.                 z_est[1] = 0;
     
215.         }
     
216.        
     
217.         return;
     
218. }
     
219. 
     
220. void ObservationOPT_Update(float pos_x,float vel_x,float pos_y,float vel_y)
     
221. {
     
222.         uint8 i;
     
223.         corr_xpos = pos_x-x_est[0];
     
224.         corr_xvel = vel_x-x_est[1];//x_vel_history[0]);
     
225.         corr_ypos = pos_y-y_est[0];
     
226.         corr_yvel = vel_y-y_est[1];//y_vel_history[0]);
     
227.        
     
228. //        for (i = 0; i < XY_VEL_HISTORY - 1; i++)
     
229. //        {
     
230. //                x_vel_history[i] = x_vel_history[i+1];
     
231. //                y_vel_history[i] = y_vel_history[i+1];
     
232. //        }
     
233. //        x_vel_history[XY_VEL_HISTORY - 1] = x_est[1];
     
234. //        y_vel_history[XY_VEL_HISTORY - 1] = y_est[1];
     
235.         return;
     
236. }

復(fù)制代碼

以上代碼是我的加速計跟光流，氣壓計的融合算法

現(xiàn)在正需要這個研究下，謝謝樓主

謝謝分享，學(xué)習(xí)