|
姿態(tài)角（Euler角）pitch yaw roll 飛行器的姿態(tài)角并不是指哪個角度，是三個角度的統(tǒng)稱,。 它們是：俯仰,、滾轉(zhuǎn),、偏航,。你可以想象是飛機(jī)圍繞XYZ三個軸分別轉(zhuǎn)動形成的夾角。 地面坐標(biāo)系（earth-surface inertial reference frame）Sg--------OXgYgZg ?、僭诘孛嫔线x一點(diǎn)Og ?、谑筙g軸在水平面內(nèi)并指向某一方向 ③Zg軸垂直于地面并指向地心（重力方向） ?、躖g軸在水平面內(nèi)垂直于Xg軸,，其指向按右手定則確定 機(jī)體坐標(biāo)系（Aircraft-body coordinate frame）Sb-------OXYZ ①原點(diǎn)O取在飛機(jī)質(zhì)心處,，坐標(biāo)系與飛機(jī)固連 ?、趚軸在飛機(jī)對稱平面內(nèi)并平行于飛機(jī)的設(shè)計(jì)軸線指向機(jī)頭 ③y軸垂直于飛機(jī)對稱平面指向機(jī)身右方 ?、躾軸在飛機(jī)對稱平面內(nèi),，與x軸垂直并指向機(jī)身下方 歐拉角/姿態(tài)角（Euler Angle） 機(jī)體坐標(biāo)系與地面坐標(biāo)系的關(guān)系是三個Euler角，反應(yīng)了飛機(jī)相對地面的姿態(tài),。 俯仰角θ（pitch）：機(jī)體坐標(biāo)系X軸與水平面的夾角,。當(dāng)X軸的正半軸位于過坐標(biāo)原點(diǎn)的水平面之上（抬頭）時(shí)，俯仰角為正,，否則為負(fù),。 偏航角ψ（yaw）： 機(jī)體坐標(biāo)系xb軸在水平面上投影與地面坐標(biāo)系xg軸（在水平面上，指向目標(biāo)為正）之間的夾角,，由xg軸逆時(shí)針轉(zhuǎn)至機(jī)體xb的投影線時(shí),，偏航角為正，即機(jī)頭右偏航為正,，反之為負(fù),。 滾轉(zhuǎn)角Φ（roll）：機(jī)體坐標(biāo)系zb軸與通過機(jī)體xb軸的鉛垂面間的夾角，機(jī)體向右滾為正,，反之為負(fù),。 首先要明確，MPU6050是一款姿態(tài)傳感器,，使用它就是為了得到待測物體（如四軸,、平衡小車）x、y,、z軸的傾角（俯仰角Pitch,、滾轉(zhuǎn)角Roll,、偏航角Yaw）。我們通過I2C讀取到MPU6050的六個數(shù)據(jù)（三軸加速度AD值,、三軸角速度AD值）經(jīng)過姿態(tài)融合后就可以得到Pitch,、Roll、Yaw角,。 主要介紹三種姿態(tài)融合算法：四元數(shù)法,、一階互補(bǔ)算法和卡爾曼濾波算法。 一,、四元數(shù)法關(guān)于四元數(shù)的一些概念和計(jì)算就不寫上來了,，我也不懂。我能告訴你的是：通過下面的算法,，可以把六個數(shù)據(jù)轉(zhuǎn)化成四元數(shù)（q0,、q1、q2,、q3）,，然后四元數(shù)轉(zhuǎn)化成歐拉角（P、R,、Y角）,。 雖然MPU6050自帶的DMP庫可以直接輸出四元數(shù)，減輕STM32的運(yùn)算負(fù)擔(dān),，這里在此沒有使用,，因?yàn)槲沂怯肧TM32的硬件I2C讀取MPU6050數(shù)據(jù)的，DMP庫需要對I2C函數(shù)進(jìn)行修改,，如DMP庫中的I2C寫：i2c_write（st.hw-》addr,，st.reg-》pwr_mgmt_1，1,，&（data［0］））,；有4個輸入變量，而STM32硬件I2C的I2C寫為：voidMPU6050_I2C_ByteWrite（u8slaveAddr,，u8pBuffer,，u8writeAddr），只有3個輸入量（這之間的差異好像是由于MPU6050的DMP庫是針對MSP430單片機(jī)寫的）,，所以必須進(jìn)行修改,，但是改固件庫是一件很痛苦的事，你們應(yīng)該都懂,。當(dāng)然,，如果你用模擬I2C的話，是容易實(shí)現(xiàn)的，網(wǎng)上的DMP移植幾乎都是基于模擬I2C的,。 要注意的的是,，四元數(shù)算法輸出的是三個量Pitch、Roll和Yaw,，運(yùn)算量很大,。而像平衡小車這樣的例子只需要一個角（Pitch或Roll）就可以滿足工作要求，個人覺得做平衡小車最好不用四元數(shù)法,。 #include《math.h》 #include“stm32f10x.h” //------------------------ //變量定義 #defineKp100.0f//比例增益支配率收斂到加速度計(jì)/磁強(qiáng)計(jì) #defineKi0.002f//積分增益支配率的陀螺儀偏見的銜接 #definehalfT0.001f//采樣周期的一半 floatq0=1,，q1=0，q2=0,，q3=0;//四元數(shù)的元素,，代表估計(jì)方向 floatexInt=0，eyInt=0,，ezInt=0;//按比例縮小積分誤差 floatYaw，Pitch,，Roll;//偏航角,，俯仰角，翻滾角 voidIMUupdate（floatgx,，floatgy,，floatgz，floatax,，floatay,，floataz） { floatnorm; floatvx，vy,，vz; floatex,，ey，ez; //測量正?；?/p> norm=sqrt（ax*ax+ay*ay+az*az）; ax=ax/norm;//單位化 ay=ay/norm; az=az/norm; //估計(jì)方向的重力 vx=2*（q1*q3-q0*q2）; vy=2*（q0*q1+q2*q3）; vz=q0*q0-q1*q1-q2*q2+q3*q3; //錯誤的領(lǐng)域和方向傳感器測量參考方向之間的交叉乘積的總和 ex=（ay*vz-az*vy）; ey=（az*vx-ax*vz）; ez=（ax*vy-ay*vx）; //積分誤差比例積分增益 exInt=exInt+ex*Ki; eyInt=eyInt+ey*Ki; ezInt=ezInt+ez*Ki; //調(diào)整后的陀螺儀測量 gx=gx+Kp*ex+exInt; gy=gy+Kp*ey+eyInt; gz=gz+Kp*ez+ezInt; //整合四元數(shù)率和正?；?/p> q0=q0+（-q1*gx-q2*gy-q3*gz）*halfT; q1=q1+（q0*gx+q2*gz-q3*gy）*halfT; q2=q2+（q0*gy-q1*gz+q3*gx）*halfT; q3=q3+（q0*gz+q1*gy-q2*gx）*halfT; //正常化四元 norm=sqrt（q0*q0+q1*q1+q2*q2+q3*q3）; q0=q0/norm; q1=q1/norm; q2=q2/norm; q3=q3/norm; Pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//pitch,，轉(zhuǎn)換為度數(shù) Roll=atan2（2*q2*q3+2*q0*q1,，-2*q1*q1-2*q2*q2+1）*57.3;//rollv //Yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//此處沒有價(jià)值,，注掉 } 二,、一階互補(bǔ)算法MPU6050可以輸出三軸的加速度和角速度。通過加速度和角速度都可以得到Pitch和Roll角（加速度不能得到Y(jié)aw角）,，就是說有兩組Pitch,、Roll角，到底應(yīng)該選哪組呢,？別急,，先分析一下,。MPU6050的加速度計(jì)和陀螺儀各有優(yōu)缺點(diǎn)，三軸的加速度值沒有累積誤差,，且通過算tan（）可以得到傾角,，但是它包含的噪聲太多（因?yàn)榇郎y物運(yùn)動時(shí)會產(chǎn)生加速度，電機(jī)運(yùn)行時(shí)振動會產(chǎn)生加速度等）,，不能直接使用,；陀螺儀對外界振動影響小，精度高,，通過對角速度積分可以得到傾角,，但是會產(chǎn)生累積誤差。所以,，不能單獨(dú)使用MPU6050的加速度計(jì)或陀螺儀來得到傾角,，需要互補(bǔ)。一階互補(bǔ)算法的思想就是給加速度和陀螺儀不同的權(quán)值,，把它們結(jié)合到一起,，進(jìn)行修正。得到Pitch角的程序如下： //一階互補(bǔ)濾波 floatK1=0.1;//對加速度計(jì)取值的權(quán)重 floatdt=0.001;//注意：dt的取值為濾波器采樣時(shí)間 floatangle; angle_ax=atan（ax/az）*57.3;//加速度得到的角度 gy=（float）gyo［1］/7510.0;//陀螺儀得到的角速度 Pitch=yijiehubu（angle_ax,，gy）; floatyijiehubu（floatangle_m,，floatgyro_m）//采集后計(jì)算的角度和角加速度 { angle=K1*angle_m+（1-K1）*（angle+gyro_m*dt）; returnangle; } 互補(bǔ)算法只能得到一個傾角，這在平衡車項(xiàng)目中夠用了,，而在四軸飛行器設(shè)計(jì)中還需要Roll和Yaw,，就需要兩個互補(bǔ)算法，我是這樣寫的,，注意變量不要搞混： //一階互補(bǔ)濾波 floatK1=0.1;//對加速度計(jì)取值的權(quán)重 floatdt=0.001;//注意：dt的取值為濾波器采樣時(shí)間 floatangle_P,，angle_R; floatyijiehubu_P（floatangle_m，floatgyro_m）//采集后計(jì)算的角度和角加速度 { angle_P=K1*angle_m+（1-K1）*（angle_P+gyro_m*dt）; returnangle_P; } floatyijiehubu_R（floatangle_m,，floatgyro_m）//采集后計(jì)算的角度和角加速度 { angle_R=K1*angle_m+（1-K1）*（angle_R+gyro_m*dt）; returnangle_R; } 單靠MPU6050無法準(zhǔn)確得到Y(jié)aw角,，需要和地磁傳感器結(jié)合使用。 三,、卡爾曼濾波其實(shí)卡爾曼濾波和一階互補(bǔ)有些相似,，輸入也是一樣的?？柭硪约笆裁?個公式等等的,，我也不太懂，就不寫了,，感興趣的話可以上網(wǎng)查,。在此給出具體程序，和一階互補(bǔ)算法一樣，每次卡爾曼濾波只能得到一個方向的角度,。 #include《math.h》 #include“stm32f10x.h” #include“Kalman_Filter.h” //卡爾曼濾波參數(shù)與函數(shù) floatdt=0.001;//注意：dt的取值為kalman濾波器采樣時(shí)間 floatangle,，angle_dot;//角度和角速度 floatP［2］［2］={{1，0},， {0,，1}}; floatPdot［4］={0，0,，0,，0}; floatQ_angle=0.001，Q_gyro=0.005;//角度數(shù)據(jù)置信度,，角速度數(shù)據(jù)置信度 floatR_angle=0.5,，C_0=1; floatq_bias，angle_err,，PCt_0,，PCt_1，E,，K_0,，K_1，t_0,，t_1; //卡爾曼濾波 floatKalman_Filter（floatangle_m，floatgyro_m）//angleAx和gyroGy { angle+=（gyro_m-q_bias）*dt; angle_err=angle_m-angle; Pdot［0］=Q_angle-P［0］［1］-P［1］［0］; Pdot［1］=-P［1］［1］; Pdot［2］=-P［1］［1］; Pdot［3］=Q_gyro; P［0］［0］+=Pdot［0］*dt; P［0］［1］+=Pdot［1］*dt; P［1］［0］+=Pdot［2］*dt; P［1］［1］+=Pdot［3］*dt; PCt_0=C_0*P［0］［0］; PCt_1=C_0*P［1］［0］; E=R_angle+C_0*PCt_0; K_0=PCt_0/E; K_1=PCt_1/E; t_0=PCt_0; t_1=C_0*P［0］［1］; P［0］［0］-=K_0*t_0; P［0］［1］-=K_0*t_1; P［1］［0］-=K_1*t_0; P［1］［1］-=K_1*t_1; angle+=K_0*angle_err;//最優(yōu)角度 q_bias+=K_1*angle_err; angle_dot=gyro_m-q_bias;//最優(yōu)角速度 returnangle,； } 總結(jié)：三種融合算法都能夠輸出姿態(tài)角（Pitch和Roll）,，一次四元數(shù)法可以輸出P、R,、Y三個傾角,，計(jì)算量比較大。一階互補(bǔ)和卡爾曼濾波每次只能輸出一個軸的姿態(tài)角,。 |
|
|
