|
本帖最后由 zkcaptain 于 2014-6-19 18:11 編輯 —————————————————————————————————————————————————————————————— 請各位不要私信密我了，沒空照顧那么多地方,，就在帖子里回吧,，我盡量回復(fù)，說的對其他人看了也受益,，說的不對,，其他人 也會指正，對大家都有好處,。 —————————————————————————————————————————————————————————————— 2014年6月16日更新： PID+亂七八糟心得部分,，內(nèi)有完整代碼和試飛視頻，見該帖：http://bbs.21ic.com/forum.php?mo ... 1&extra=#pid4167670 --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 自己摸索著做的一個小應(yīng)用,，希望有朝一日能夠飛起來,，歡迎在電機(jī),、電調(diào)、算法,、動力學(xué)等方面有研究的朋友一起討論。 做四軸飛行器也是為了好玩,，目前我只完成了第一步：基于加速度計,、陀螺儀和磁力計的姿態(tài)融合，能夠?qū)崟r更新四元數(shù),，并由四元數(shù)解算出姿態(tài)角,。接下來才是更重要的，比如上各種什么機(jī)架,、電調(diào),、電機(jī)、螺旋槳之類的,，然后寫各種PID控制代碼,，系統(tǒng)整合以后還要調(diào)試各種參數(shù)，抗干擾,，使系統(tǒng)穩(wěn)定,，最后還可能要加各種應(yīng)用器件，目前對這方面了解的還不多,。 算法參考自Madgwick寫的AHRSUpdate和IMUUpdate,，簡單有效，其中AHRSUpdate是融合了陀螺儀,、加速度計和磁力計,，而IMUUpdate只融合了陀螺儀和加速度計，但對于做四軸飛行器來說,，IMUUpdate也夠用了,，不過我沒發(fā)現(xiàn)有用AHRSUpdate的，并且我的一大半的時間都是花在AHRSUpdate這個算法上,，直到現(xiàn)在也沒用這個算法解算出來正確的四元數(shù)和歐拉角,，什么方法都試過了，導(dǎo)致我現(xiàn)在都開始懷疑這個算法的正確性,，希望用過的高手來指點(diǎn)迷徑,。 我現(xiàn)在用的算法：先用IMUUpdate算出Pitch和Roll，然后再結(jié)合互補(bǔ)濾波求出Yaw,，其中求Yaw的公式參考自john800422,。 第一部分：硬件 1.傳感器：MPU9150（INVENSENSE公司的，單芯片內(nèi)集成了加速度計,、陀螺儀和磁力計,，并且內(nèi)置DMP用于姿態(tài)融合,，不過只融合了加速度計和陀螺儀，具有自校準(zhǔn)功能,，價格比MPU6050貴很多,，但是省PCB面積，省事,，軸向重合度高,。實(shí)際上就是把MPU6050和磁力計AK8975放在同一個芯片里，程序還是使用MPU6050的驅(qū)動）,； 2.MCU：GD32F103CB（Gigadevice公司的,，支持國產(chǎn)，與ST同型號的MCU 直接兼容,，性價比更高,，外接8M晶振，晶振遠(yuǎn)離傳感器,，避免干擾磁力計） 3.電源芯片：TLV70233DBVR（TI的LDO,，輸入2-6V，輸出3.3V,，只需要外接2個X7R無極性陶瓷電容） 4.串口：MAX3232（方便調(diào)試） 5.USB供電 上圖： 圖1：PCB的3D效果圖,，實(shí)物圖就不上了，太丑,，測試版,，先追求調(diào)通得出姿態(tài)角： 第二部分：軟件 1.使用keil，uvision4.1.0,，工具鏈：RealView MDK-ARM Version4.12,； 2.庫文件：ST官方庫STM32F10x_StdPeriph_Driver version V3.3.0； 3.驅(qū)動：官方的MPU6050驅(qū)動inv_mpu.c和inv_mpu_dmp_motion_driver.c,； 先看幾個圖,，然后再說坐標(biāo)軸的設(shè)定和算法部分。 上圖： 圖1：系統(tǒng)初始化,，順序從上到下依次是：初始化MPU,、設(shè)置需要使用哪些傳感器、設(shè)置陀螺儀測量范圍（我設(shè)的是正負(fù)500度/s）,、設(shè)置加速度計測量范圍（我設(shè)的是正負(fù)4g）,、配置fifo、設(shè)置采樣率,、裝載DMP,、設(shè)置陀螺儀軸向(比較重要)、使能DMP的一些玩意兒,、設(shè)置DMP的FIFO,、自校準(zhǔn)陀螺儀和加速度計,、開啟DMP、開始姿態(tài)融合,，見下圖： 圖2：由四元數(shù)求出的最終姿態(tài)角,，其中Yaw為航向角，表示機(jī)頭偏離正北方多少度,，范圍-180到+180,；Pitch為俯仰角，表示機(jī)頭正方向與水平線的夾角,，范圍-90到+90,；Roll為翻滾角,，表示機(jī)翼與水平線的夾角,，范圍：-180到+180。下圖為機(jī)身水平,，且機(jī)頭正北偏西37度左右的數(shù)據(jù)： 圖3：下圖為機(jī)翼水平,，機(jī)頭指向正北，且機(jī)頭向下25度的數(shù)據(jù)： 圖4：下圖為機(jī)頭指向正北,，保持水平,，且機(jī)翼的右翼向下傾斜23度的數(shù)據(jù)： 圖5：看下歐拉角的奇異點(diǎn)，在奇異點(diǎn)處一個轉(zhuǎn)動狀態(tài)對應(yīng)無窮多組自由度值,，當(dāng)物體轉(zhuǎn)到這些奇異點(diǎn)附近,，便沒法求解。圖中當(dāng)Pitch為+90度時,，機(jī)體的姿態(tài)便沒法控制,，Roll的軸向發(fā)生了變化。如下圖： 第三部分：如何確定自己的軸向 首先,，軸向的定義跟初始化四元數(shù)和最后結(jié)算的歐拉角有關(guān),，跟IMUUpdate四元數(shù)更新算法無關(guān)，換句話說,，不管你的軸向如何定義,，IMUUpdate隨便用，但是初始化四元數(shù)的公式和最后結(jié)算歐拉角的公式要做適當(dāng)?shù)母淖?，這個后面算法中有說,。加速度計也好，陀螺儀也好,，磁力計也好,，他們的軸向都要滿足右手定理，如下圖： 再附上一段注釋用于解釋如何定義合理的軸向,，以及如何正確旋轉(zhuǎn)傳感器的軸向,，解釋這么多其實(shí)就是說定義好的軸向要滿足右手定理,，如下圖： 下圖，旋轉(zhuǎn)前是[x y z],，旋轉(zhuǎn)后就是[-y x z]： 下圖是如何確定旋轉(zhuǎn)角度的正方向,，用右手握住坐標(biāo)軸，拇指指向軸向的正方向,，四個指頭彎曲的方向就是旋轉(zhuǎn)角度的正方向,，在初始化四元數(shù)時，計算出的歐拉角的正方向也要滿足這個條件： 我的程序使用的軸向如下圖所示,，未作任何改變： 第四部分：算法 第一步是校準(zhǔn),，加速度計和陀螺儀我用的是MPU9150內(nèi)部自校準(zhǔn)，磁力計的校準(zhǔn)采用如下方法： 第二步是初始化四元數(shù),，常見的軸向定義是繞x軸旋轉(zhuǎn)是Roll,，繞y軸旋轉(zhuǎn)是Pitch，繞z軸旋轉(zhuǎn)是Yaw,，我的程序也是這樣定義的,，詳細(xì)說明見程序，這里舉個另外一種軸向定義來對初始化四元數(shù)進(jìn)行說明,，方便比較,。 下面我們來定義繞x軸旋轉(zhuǎn)是Pitch，繞y軸旋轉(zhuǎn)是Roll,，繞z軸旋轉(zhuǎn)是Yaw,，軸向的正方向如上圖一樣，不變,。 先對加速度計和磁力計的數(shù)據(jù)進(jìn)行處理,，得到init_xx來供我們使用如下圖： 然后通過公式計算出初始化的Roll、Pitch,、Yaw,，注意加負(fù)號保證旋轉(zhuǎn)角度的正方向，如下圖： 然后由上面的歐拉角求出初始化四元數(shù),，這時要注意旋轉(zhuǎn)順序的不同,，公式也不同，大部分旋轉(zhuǎn)順序是Z-Y-X,，我的程序里也用的這個順序,，在這里我們按Z-X-Y的順序來旋轉(zhuǎn)，并得出求四元數(shù)的公式以做比較,，其旋轉(zhuǎn)矩陣: q=qyaw*qpitch*qroll= (cos(0.5*Yaw)+ksin(0.5*Yaw)) * (cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll)) 得出初始化四元數(shù)計算公式如下圖所示： 其中i,，j，k之間相乘的順序不能隨意變，在前的先計算,，在后的后計算相乘的公式如下圖： 至此初始化四元數(shù)完成,。 第三步就是使用IMUUpdate算法了，用完以后再根據(jù)公式計算出歐拉角,，此公式跟旋轉(zhuǎn)順序和旋轉(zhuǎn)使用的軸向有關(guān),，我們的旋轉(zhuǎn)順序是Z-X-Y，且繞Z是Yaw,，繞X是Pitch,，繞Y是Roll，推到過程如下圖： 首先得出3個方向余旋矩陣： 下圖繞Z軸Yaw： 下圖繞X軸pitch： 下圖繞Y軸Roll： 然后按照我們的Z-X-Y順序求得C=Croll * Cpitch * Cyaw,，如下圖： 將上圖的方向余旋矩陣C與下圖的四元數(shù)姿態(tài)矩陣做對比,，即可求出歐拉角，注意上圖的方向余旋矩陣C是隨著我們對坐標(biāo)軸的定義變化而變化的,，而下圖的四元數(shù)姿態(tài)矩陣是固定的： 最后一步就是求出歐拉角,，公式如下圖： 至此，算法完成,，參考如下： //本程序的讀寫mpu9150部分是在作者_spetrel的基礎(chǔ)上修改而來,； //本程序Yaw的互補(bǔ)濾波出自作者john800422,； //本程序采用的gyro accel融合算法出自作者Madgwick,； 在此感謝以上作者的無私分享和幫助！ 附上自己的代碼和AHRS IMU原始代碼： ---------------------------------------------------------------------------------------------------------------------------- 2013年11月28日更新-采用AHRSUpdate算法進(jìn)行姿態(tài)融合： 這段時間一直沒有太多時間搞這玩意,，不過苦惱多日的AHRSUpdate算法問題終于解決了,，該算法直接融合9軸數(shù)據(jù)，包括加速度計,、陀螺儀和磁力計,，其中0°<yaw<360°, -90°<pitch<+90°, -180°<roll<180°，并且我定義：Yaw北偏西為正,，pitch往上為正,，roll“右翼”下沉為正。傳感器坐標(biāo)軸：繞y旋轉(zhuǎn)是roll,，繞x旋轉(zhuǎn)為pitch,，繞z旋轉(zhuǎn)為yaw。代碼里只有姿態(tài)更新,，更新頻率接近400Hz,，這還只是跑72Mhz頻率下的數(shù)據(jù)，想要跑108M可以自己修改,。 說下改進(jìn)： 1.由于IMUupdate算法只融合了加速度計和陀螺儀的數(shù)據(jù),，因此我用了互補(bǔ)濾波來融合磁力計以修正偏航角，但是這種互補(bǔ)濾波有個小問題，就是假如定義偏航角的范圍是0-360度,，那么當(dāng)機(jī)頭大概從北偏西1度轉(zhuǎn)到北偏東364度時,，機(jī)頭會經(jīng)過0度(360度)這個點(diǎn)，那么這時,，yaw不會直接1-0-364這么變化,，而是會被逆向積分從1-20-180-270-364這么轉(zhuǎn)一圈，這是個不好的現(xiàn)象,，實(shí)驗(yàn)了半天也沒有解決,，而AHRSupdate很好的解決了這個問題。 2.放棄了DMP輸出,，DMP輸出會有部分?jǐn)?shù)據(jù)丟失現(xiàn)象,，輸出頻率只有200Hz，雖然也夠用,，但還是太低了,，采用I2C直接讀取raw數(shù)據(jù)，做了簡單校準(zhǔn),，下一步打算邊做精確校準(zhǔn)邊搞PID控制,，然后再加上加速度計的平滑濾波和磁力計的橢圓擬合校準(zhǔn)，最好還有陀螺儀的自動零漂校準(zhǔn),。 3.考慮到電機(jī)的轉(zhuǎn)動頻率是100Hz以上,，因此我采用加速度計的低通濾波(DLPF)為98Hz，可以有效濾掉電機(jī)振動帶來的影響,，結(jié)合平滑濾波效果應(yīng)該不錯,。 說下AHRSUpdate算法的缺點(diǎn)： 由于把磁力計的數(shù)據(jù)融合到了pitch和roll里面，那么當(dāng)磁力計突然收到強(qiáng)磁場干擾時,，會導(dǎo)致四軸失控,。親測過拿手機(jī)等物品靠近飛控板時，pitch,、roll和yaw的數(shù)據(jù)有明顯變化,，變化程度跟物品具有的磁場強(qiáng)度成正比，不過應(yīng)該是有算法可以濾掉這些固定的外部干擾的,。 而IMUupdate只把磁力計融合進(jìn)了yaw,，那么即便有強(qiáng)磁場干擾也不會造成全方位的失控。 按照慣例,，上代碼,，代碼里有詳細(xì)注釋： |
|
|
