多旋翼飛行器設(shè)計(jì)與控制實(shí)踐 第08講-濾波器設(shè)計(jì)實(shí)驗(yàn)

自動(dòng)化科學(xué)與電氣工程學(xué)院北京航空航天大學(xué)1.實(shí)驗(yàn)原理2.基礎(chǔ)實(shí)驗(yàn)3.分析實(shí)驗(yàn)4.設(shè)計(jì)實(shí)驗(yàn)5.小結(jié)三軸加速度計(jì)固連在多旋翼機(jī)體，其三軸與機(jī)體坐標(biāo)系一致。因此，低頻的俯仰角和滾轉(zhuǎn)角觀測(cè)量可以由加速度計(jì)測(cè)量值近似得到θmφm其中bmxbmaybmazbm]T表示加速度計(jì)測(cè)量值。同時(shí)還有兩點(diǎn)需要注意：（1）為了得到更加精確的角度信息，需要消除加速度計(jì)的慢時(shí)變漂移。（2）如果機(jī)體振動(dòng)很大，則axbm,aybm將被噪聲嚴(yán)重污染，這樣將進(jìn)一步影響角度θm,φm的估計(jì)。因此機(jī)體的減振很重要。另外姿態(tài)變化率,和角速度bω有如下關(guān)系tantanθsinφcosφsinφ/cosθtanθcosφ]||xbxb||=|0,,φ很小可以轉(zhuǎn)變?yōu)閨|ωωyb||ωωybybb||||cos||||由此可知，俯仰角和橫滾角可以由加速度計(jì)測(cè)量得到，漂移小，但噪聲大。另一方面，姿高通濾波器τs高通濾波器τs1 τs+1τs+1下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)下線性互補(bǔ)濾波。俯仰角θ的拉氏變換可以表示為θs=θs+θs）表示時(shí)間常數(shù)為了估計(jì)俯仰角，以上式子的θ需要用傳感器信息替代。1）加速度計(jì)測(cè)量的俯仰角無(wú)漂移但噪聲大，我們可以將θm其中nθ陀螺儀測(cè)量值ss2）陀螺儀的角速度測(cè)量會(huì)有漂移但噪聲小，我們可以陀螺儀測(cè)量值ss角速率積分的Laplace變換角速率積分的Laplace變換常值漂移的Laplace變換高通濾波器τs高通濾波器τs1 τs+1τs+1下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)下線性互補(bǔ)濾波。俯仰角θ的拉氏變換可以表示為θs=θs+θs）表示時(shí)間常數(shù)為了估計(jì)俯仰角，以上式子的θ需要用傳感器信息替代。線性互補(bǔ)濾波的標(biāo)準(zhǔn)形式以傳遞函數(shù)方式表示為加速度計(jì)測(cè)量陀螺儀的的俯仰角角速度積分高通濾波器τs1 高通濾波器τs1 τs+1τs+1下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)下線性互補(bǔ)濾波。俯仰角θ的拉氏變換可以表示為θs=θs+θs）表示時(shí)間常數(shù)為了估計(jì)俯仰角，以上式子的θ需要用傳感器信息替代。線性互補(bǔ)濾波的標(biāo)準(zhǔn)形式以傳遞函數(shù)方式表示為θs)θs)ns+c加速度計(jì)測(cè)量的俯仰角陀螺儀的角速度積分衰減為0T表示采樣周期T表示采樣周期Ts∈+進(jìn)一步表示為bm(z)τ+1τ+1TT再把上式化為差分方程可以得到低通濾波器將θ低通濾波器將θ無(wú)漂移的優(yōu)勢(shì)保留下來(lái)，而高通濾波器將ωybm(s)/s噪聲小的優(yōu)勢(shì)保留下來(lái) τ+Tsybmτ+Tmss卡爾曼濾波kkkkkkkkvkmx1,Rwvk,jnxmRwwk,jwkwkδkj=k=jRvvk,jvkvkδkj=k=j互相關(guān)系數(shù)Rwv系統(tǒng)噪聲方差陣Qk>0nxn觀測(cè)噪聲方差陣Rk>0mxmk=jk=j卡爾曼濾波kk,k?1k?1k?1kk,k?1k?1k?1k,k?1k?1kkkkz=Hkkkk0Rxw0,kx0wnxn0,kx0vnxmRuwk,jukwnxnzk++k|k+Φ延遲zk++k|k+Φ延遲2.步驟二：初始狀態(tài)xkk–1kkkk,k~mx1kkkk,k~mx1,k0x0）x0x0–Ex0TKk+Hk++3.步驟三：當(dāng)k=0，取P0|0=P0,0|0=05.步驟五：狀態(tài)估計(jì)預(yù)測(cè)xkk–16.步驟六：誤差協(xié)方差預(yù)測(cè)kk8.步驟八：狀態(tài)估計(jì)更新k|kk|k–1kzk–k|k–1）k|k–1kk|k–19.步驟九：誤差協(xié)方差更新Pk|kIn–KkHkPk|k–1（1）卡爾曼濾波器在進(jìn)行濾波器估計(jì)的同時(shí)還產(chǎn)生了誤差協(xié)方差陣Pk|k它可以用于表征估計(jì)精度，同時(shí)也能用于傳感器的健康評(píng)估。（2）一般來(lái)說(shuō)，采樣周期合理情況下，連續(xù)系統(tǒng)可觀，離散化的系統(tǒng)也會(huì)可觀。然而有時(shí)候采樣周期選擇不當(dāng)，系統(tǒng)可能失去可控性及可觀性。因此原則上應(yīng)該檢查離散化系統(tǒng)的可觀性。（4）如果(Φk,k?1,Hk)不可觀，那么卡爾曼濾波器仍然可以運(yùn)行，只不過(guò)不可觀的模態(tài)沒(méi)有進(jìn)行修正，只是遞推罷了。極端情況Hk=0m▽n，那么Kk=0n▽mk|kPk|k擴(kuò)展卡爾曼濾波 ,w),w)kk|k?1vk.擴(kuò)展卡爾曼濾波xkzkk,vk)且噪聲方差陣分別為Qk和Rk。 展開(kāi)形式為ww) 擴(kuò)展卡爾曼濾波H?h(x,v)k模型簡(jiǎn)化為 vkkvkkkk(?h(x,(?h(x,v))T擴(kuò)展卡爾曼濾波算法總結(jié)xk=f0nx1,Qk）zk=hxk,vk,vk~0mx1,Rk）2.步驟二：初始狀態(tài)0x0）P0x0–Ex0x0–Ex0T擴(kuò)展卡爾曼濾波算法總結(jié)3.步驟三：當(dāng)k=0，取P0|0=P0,0|0=0kkkkkkkkn6.步驟六：誤差協(xié)方差預(yù)測(cè)kk8.步驟八：狀態(tài)估計(jì)更新k|kk|k–1kzk–k|k–1）k|k–1kk|k–19.步驟九：誤差協(xié)方差更新Pk|kIn–KkHkPk|k–1趙峙堯,戴訓(xùn)華,任錦瑞,鄧恒譯.《多旋翼飛行器設(shè)計(jì)與控制》,電子（3）在數(shù)據(jù)方面，若沒(méi)有硬件，可以直接使用實(shí)驗(yàn)指導(dǎo)包“e4.1”中的數(shù)據(jù)。利用數(shù)據(jù)采集模型和Pixhawk自駕儀采集加速度計(jì)和陀螺儀數(shù)據(jù)，按步驟完成互補(bǔ)濾波，處理所得數(shù)據(jù)并繪制相關(guān)姿態(tài)角數(shù)據(jù)圖；與原數(shù)據(jù)解算的姿態(tài)角和Pixhawk自駕儀自帶濾波及三個(gè)姿態(tài)角的數(shù)據(jù)機(jī)和Pixhawk自駕儀連接好。黑紅白（1）步驟一：獲取加速度計(jì)、陀螺儀以及三個(gè)姿態(tài)角的數(shù)據(jù)2）打開(kāi)數(shù)據(jù)采集模型。a打開(kāi)”log_data.slxa該文件使用PSP工具箱的模塊搭建，可以讀取加速度、角速度、時(shí)間戳和飛控自帶算法解算出的姿態(tài)角數(shù)據(jù)。我們可以使用遙控器控制開(kāi)始寫(xiě)入數(shù)據(jù)以及停止寫(xiě)入數(shù)據(jù)，最終將數(shù)據(jù)存儲(chǔ)到Pixhawk自（1）步驟一：獲取加速度計(jì)、陀螺儀以及三個(gè)姿態(tài)角的數(shù)據(jù)3）編譯并下載文件“l(fā)og_data.slx”到Pixhawk中。一鍵編譯下載完成一鍵下載下載完成（1）步驟一：獲取加速度計(jì)、陀螺儀以及三個(gè)姿態(tài)角的數(shù)據(jù)4）采集數(shù)據(jù)。Pixhawk自駕儀的LED指示燈變紅意味著PX4軟件請(qǐng)重新拔插自駕儀）。準(zhǔn)備就緒后，將遙控器CH5撥到最頂部123456function[phi_cf,theta_cf]=Attitude_cf(dt,z,phi_cf_k,theta_cf_k,tao)%函數(shù)描述:%互補(bǔ)濾波姿態(tài)結(jié)算。%z:三軸角陀螺儀和三軸加速度計(jì)測(cè)量值，[gx,gy,gz,ax,ay,az]'，78920212223%tao:濾波器系數(shù)%輸出:%phi_cf,theta_cf:解算的姿態(tài)角，單位：radgx=z(1);gy=z(2);ax=z(4);ay=z(5);az=z(6);%使用加速度計(jì)測(cè)量值計(jì)算姿態(tài)角g=sqrt(ax*ax+ay*ay+az*az);theta_am=asin(ax/g);phi_am=-asin(ay/(g*cos(theta_am)));%互補(bǔ)濾波theta_cf=tao/(tao+dt)*(theta_cf_k+gy*dt)+dt/(tao+dt)*theta_am;phi_cf=tao/(tao+dt)*(phi_cf_k+gx*dt)+dt/(tao+dt)*phi_am;end2）運(yùn)行“Attitude_estimat濾波解算的姿態(tài)角對(duì)應(yīng)“cf”和PX4自帶算法解算的姿態(tài)角對(duì)應(yīng)“px4”。0-100-2000-100-200可以得到結(jié)論：1）直接對(duì)陀螺儀測(cè)量的角速度進(jìn)行積分得到的姿態(tài)角據(jù)來(lái)自加速度計(jì)的原始其是使用實(shí)際飛行中的數(shù)據(jù)時(shí)；3）使用互補(bǔ)濾波器估計(jì)的姿態(tài)角是平滑的并且沒(méi)有累積誤差。-10050-50100acccf51015202530245250255260265270275280 cf px451015202530500-50245250255260265270275280基于基礎(chǔ)實(shí)驗(yàn)，將互補(bǔ)濾波器bm(k))+θm(k)ss的參數(shù)τ值進(jìn)行改變，對(duì)所給數(shù)據(jù)進(jìn)行濾波，分析濾波器參數(shù)對(duì)濾波效 bm(k))+θm(k)ss在MATLAB中運(yùn)行“Attitude_cf_tao123456789%參數(shù)tao對(duì)濾波效果的影響clear;loadlogdatan=length(ax);%采集數(shù)據(jù)個(gè)數(shù)Ts=zeros(1,n);%時(shí)間間隔Ts(1)=0.004;fork=1:n-1Ts(k+1)=(timestamp(k+1)-timestamp(k))*0.000001;end2021222324theta_cf=zeros(1,n);%互補(bǔ)濾波得到的滾轉(zhuǎn)角（對(duì)應(yīng)theta）phi_cf=zeros(1,n);%互補(bǔ)濾波得到的俯仰角（對(duì)應(yīng)phi）tao=0.001;tao=tao*10;g=sqrt(ax(k)*ax(k)+ay(k)*ay(k)+az(k)*az(k));theta_am=asin(ax(k)/g);phi_am=-asin(ay(k)/(g*cos(theta_am)));theta_cf(i,k)=tao/(tao+Ts(k))*(theta_cf(i,k-1)+gy(k)*Ts(k))+Ts(k)/(tao+Ts(k))*theta_am;phi_cf(i,k)=tao/(tao+Ts(k))*(phi_cf(i,k-1)+gx(k)*Ts(k))+Ts(k)/(tao+Ts(k))*phi_am;endend00 245250255260265圖.參數(shù)τ對(duì)互補(bǔ)濾波的影響可以看到參數(shù)τ越大，對(duì)高頻噪聲的濾波作用越明顯。當(dāng)τ很大時(shí) τ+Tτ+Tssbm(k)bm(k)/course（3）在數(shù)據(jù)方面，若沒(méi)有硬件，可以直接使用實(shí)驗(yàn)指導(dǎo)包“e4.3”中的數(shù)據(jù)“l(fā)ogdata.mat”。理解卡爾曼濾波原理，并設(shè)計(jì)卡爾曼濾波器實(shí)現(xiàn)濾波器功能。進(jìn)一步，處理加速度和角速度數(shù)據(jù)，并繪制出相關(guān)姿態(tài)角數(shù)據(jù)圖，與原數(shù)據(jù)解算的姿態(tài)角和Pixhawk自駕儀自帶濾波器解聲，濾波器的過(guò)程模型可以表示為過(guò)程模型bb過(guò)程模型第三列提出來(lái)得到7=?ω第三列提出來(lái)得到7=?ωxbx=|sinφcosθ|bωm?bg?wg×xg=wbgam=?gx+na觀測(cè)模型為了在計(jì)算機(jī)上運(yùn)行卡爾曼濾波器，使用一階差分對(duì)前述式子進(jìn)行離散化得g,k]bbg,k-1bbg,k-1]bg,k-1Ts]|3m,kg,k-1T)xk-1m,kT)xk-1T)xk-13g,k]a,kk」對(duì)過(guò)程模型使用泰勒級(jí)數(shù)展開(kāi)，就可以進(jìn)一步得到卡爾曼濾波器所需要的信息：轉(zhuǎn)態(tài)轉(zhuǎn)移陣噪聲驅(qū)動(dòng)陣量測(cè)陣Φk-1(I3bωm,kT30]THk3]「bg,k]「bg,k-1]k計(jì)算狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)矩陣]Γk-1=|]bωm,k為當(dāng)前陀螺儀測(cè)量值，xk-1為上一時(shí)刻的狀態(tài)估計(jì)值。Kkk)?1其中Rk為觀測(cè)噪聲方差?！竍g,「bg,r]「bg,r-1](「bg,r-1])其中zr為加速度計(jì)測(cè)量值。Pr|r=(I-KrHr)Pr|r-1123456789function[x_aposteriori,P_aposteriori,roll,pitch]=Attitude_ekf(dt,z,q,r,x_aposteriori_k,P_aposteriori_k)%函數(shù)描述:%狀態(tài)估計(jì)的拓展卡爾曼濾波方法%輸入:%dt:更新周期%z:測(cè)量值%x_aposteriori_k:上一時(shí)刻的狀態(tài)估計(jì)%P_aposteriori_k:上一時(shí)刻估計(jì)協(xié)方差%輸出:%x_aposteriori：當(dāng)前時(shí)刻的狀態(tài)估計(jì)%P_aposteriori：當(dāng)前時(shí)刻的估計(jì)協(xié)方差%roll,pitch：歐拉角，單位:radw_m=z(1:3);%角速度測(cè)量值a_m=z(4:6);%加速度測(cè)量值2021222324252627282930313233343536373839g=norm(a_m,2);%重力加速度%wx=[0,-(wz-bzg,wy-byg;%wz-bzg,0,-(wx-bxg);%-(wy-byg),wx-bxg,0];wx=[0,-(w_m(3)-x_aposteriori_k(3)),w_m(2)-x_aposteriori_k(2);w_m(3)-x_aposteriori_k(3),0,-(w_m(1)-x_aposteriori_k(1));-(w_m(2)-x_aposteriori_k(2)),w_m(1)-x_aposteriori_k(1),0];bCn=eye(3,3)-wx*dt;%更新先驗(yàn)狀態(tài)矩陣x_apriori=zeros(1,6);x_apriori(1:3)=x_aposteriori_k(1:3);%角速度漂移x_apriori(4:6)=bCn*x_aposteriori_k(4:6);%加速度歸一化值%[x]xx_aposteriorikx=[0,-x_aposteriori_k(6),x_aposteriori_k(5);x_aposteriori_k(6),0,-x_aposteriori_k(4);-x_aposteriori_k(5),x_aposteriori_k(4),0];%更新?tīng)顟B(tài)轉(zhuǎn)移矩陣PHI=[eye(3,3),zeros(3,3);-x_aposteriorikx*dt,bCn];394041424344454647484950515253545556575859GAMMA=[eye(3,3)*dt,zeros(3,3);%噪聲驅(qū)動(dòng)陣zeros(3,3),-x_aposteriorikx*dt];Q=[eye(3,3)*q(1),zeros(3,3);zeros(3,3),eye(3,3)*q(2)];%預(yù)測(cè)誤差協(xié)方差矩陣P_apriori=PHI*P_aposteriori_k*PHI'+GAMMA*Q*GAMMA';%更新R=eye(3,3)*r(1);H_k=[zeros(3,3),-g*eye(3,3)];%卡爾曼增益K_k=(P_apriori*H_k')/(H_k*P_apriori*H_k'+R);%狀態(tài)估計(jì)矩陣x_aposteriori=x_apriori'+K_k*(a_m-H_k*x_apriori');%估計(jì)誤差協(xié)方差P_aposteriori=(eye(6,6)-K_k*H_k)*P_apriori;%計(jì)算滾轉(zhuǎn)角和俯仰角，分別對(duì)應(yīng)roll,pitchk=x_aposteriori(4:6)/norm(x_aposteriori(4:6),2);roll=atan2(k(2),k(3));%滾轉(zhuǎn)角pitch=-asin(k(1));%俯仰角end（1）步驟一：算法仿真及驗(yàn)證運(yùn)行“e4.3”中文件“Attitude_estimator.m”即可得到如下圖所示的濾波結(jié)果及對(duì)比圖。500-50-100 ekf0PX4自帶濾波算24525025526026527027528024525025526026527027528022232425262728293031222324252627282930313233343536373839404142P_aposteriori=zeros(6,6,n);P_aposteriori(:,:,1)=eye(6,6)*100;%P0x_aposteriori=zeros(6,n);x_aposteriori(:,1)=[0,0,0,0,0,-1];%X0%使用加速度計(jì)數(shù)據(jù)計(jì)算歐拉角g=sqrt(ax(k)*ax(k)+ay(k)*ay(k)+az(k)*az(k));theta_am(k)=asin(ax(k)/g);phi_am(k)=-asin(ay(k)/(g*cos(theta_am(k))));%使用陀螺儀數(shù)據(jù)計(jì)算歐拉角theta_gm(k)=theta_gm(k-1)+gy(k)*Ts(k);phi_gm(k)=phi_gm(k-1)+gx(k)*Ts(k);%互補(bǔ)濾波和EKFz=[gx(k),gy(k),gz(k),ax(k),ay(k),az(k)];[phi_cf(k),theta_cf(k)]=Attitude_cf(Ts(k),z',phi_cf(k-1),theta_cf(k-1),tao);[x_aposteriori(1:6,k),P_aposteriori(1:6,1:6,k),phi_ekf(k),theta_ekf(k)]=Attitude_ekf(Ts(k),z',w,v,x_aposteriori(1:6,k-1),P_aposteriori(1:6,1:6,k-1));endt=timestamp*0.000001;rad2deg=180/pi;文件“Attitude_estimator.m文件“Attitude_estimator.m”主函數(shù)如下表所示1234567892021clear;loadlogdatan=length(ax);%采集數(shù)據(jù)個(gè)數(shù)Ts=zeros(1,n);%時(shí)間間隔Ts(1)=0.004;Ts(k+1)=(timestamp(k+1)-timestamp(k))*0.000001;endtheta_am=zeros(1,n);