【正文】 農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 21 參 考 文 獻(xiàn) 馮智勇，曾瀚，張力等 . 基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量 [J]. 西南師范大學(xué)學(xué)報(bào) (自然科學(xué)版 )， 20xx， 36(4)： 137141. 籍穎，劉兆祥，劉剛等 . 基于 Kalman 濾波農(nóng)用車輛導(dǎo)航定位方法 [J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，20xx,9(40)： 1317. 黃旭，王常虹，伊國(guó)興等 . 利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器 [J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 20xx， 13(2)： 2734. 代剛，李枚，蘇偉等 . 自旋導(dǎo)彈捷聯(lián)式陀螺 /地磁姿態(tài)測(cè)量方法 [J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，20xx， 18(6)： 702705. 施聞明，徐彬，陳利敏 . 捷聯(lián)式航姿系統(tǒng)中四元素算法 Kalman 濾波器的實(shí)現(xiàn)研究 [J]. 自動(dòng)化技術(shù)與應(yīng)用， 20xx， 24(11)： 68. SORG H. From Sensor to Drapertwo centuries of gyroscopic development[J]. Navigation,23(4):19761977. 陶成林，雷必成，楊揚(yáng)戩 . 基于 STM32 的跌倒防護(hù)裝置研究 [J]. 電子設(shè)計(jì)工程， 20xx，20(22)： 6368. 吳軍，鮑其蓮 . 高精度捷聯(lián)慣導(dǎo)姿態(tài)更新優(yōu)化算法研究 [J]. 電子測(cè)量技術(shù)， 20xx， 2(33)：4952. 葉锃鋒，馮恩信 . 基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法 [J]. 傳感技術(shù)學(xué)報(bào)，20xx， 25(4)： 524528. 袁西，陳棟，田湘等 . 三軸數(shù)字加速度計(jì) ADXL345 及其在捷聯(lián)慣導(dǎo)中的應(yīng)用 [J]. 電子設(shè)計(jì)工程， 20xx， 18(3)： 138140. 農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 22 附 錄 STM32F103VCT6 主控模塊原理圖 BOOT094NC73NRST14OSC_IN12OSC_OUT13PA0WKUP23PA124PA225PA326PA429PA530PA631PA732PA867PA968PA1069PA1170PA1271PA13/JTMS/SWDIO72PA14/JTCK/SWCLK76PA15/JTDI77PB035PB136PB2/BOOT137PB3/JTDO89PB4/JNTRST90PB591PB692PB793PB895PB996PB1047PB1148PB1251PB1352PB1453PB1554PC015PC116PC217PC318PC433PC534PC663PC764PC865PC966PC1078PC1179PC1280PC13TAMPERRTC7PC14OSC32_IN8PC15OSC32_OUT9PD081PD182PD283PD384PD485PD586PD687PD788PD855PD956PD1057PD1158PD1259PD1360PD1461PD1562PE097PE198PE21PE32PE43PE54PE65PE738PE839PE940PE1041PE1142PE1243PE1344PE1445PE1546VDD_150VDD_275VDD_3100VDD_428VDD_511VDDA22VREF20VREF+21VSS_149VSS_274VSS_399VSS_427VSS_510VSSA19VBAT6U110pfC310pfC5128MGND10kR110kR4GND1234PA0PA1PA2PA3SPI1_MISOSPI1_MOSISPI1_SCKSPI1_NSSPA8PA9PA10PA11PA12SWCLKSWDIOPA15PB0PB1INT0DRDYPB5PB6PB7SCLSDAPB10PB11PB12PB13PB14PB15NRST3V33V33V33V33V33V3PC0PC1PC2PC3PC4PC5PC6PC7PC8PC9PC10PC11PC12PC13PD0PD1INT1INT2PD4PD5PD6PD7PD8PD9PD10PD11PD12PD13PD14PD15PE0PE1PE2PE3PE4PE5PE6PE7PE8PE9PE10PE11PE12PE13PE14PE15GNDSWCLKSWDIO3V3GND1 23 45 67 8P5GNDSPI1_MISOSPI1_MOSISPI1_NSSSPI1_SCKPC4PC53V3 農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 23 SCL SDA3V33V3SETPSETC3V3C3SETPGNDSCL1VDD2NC3S14NC5NC6NC7SETP8GND9C110GND11SETC123V313NC14DRDY15SDA16U5HMC5883DR_HMC D110KR83V3GND VDD1GND2RESERVED3GND4GND5Vs6CS7INT18INT29NC10RESERVED11SDO/ALT ADDRESS12SDA/SDI/SDIO13SCL/SCLK14U2ADXL345GNDGND3V33V3C4GND3V3R2R3SCLSDAINT1INT2 12P6Header 2104C610uFC7Vout2 1Vin3U3LM1117GND104C810uFC93V3104C10104C11GND Vdd_IO1SCL_SPC2SDA/SDI/SDO3SDO/SA04CS5DRDY/INT26INT7RES8RES9RES10RES11RES12GND13PLLFILT14RES15Vdd16U4L3G42003V3GND3V33V310nFC13470nFC1210KR910uFC15100nfC14SCLSDADRDYINT0 1234567891011121314151617P11234
。但是還有許多可以改進(jìn)的地方，如可以采用 ARM9 處理器，在 Linux 嵌入式系統(tǒng)平臺(tái)上，通過(guò) TFT 建立三維圖像動(dòng)態(tài)顯示目標(biāo)載體的狀態(tài)，將數(shù)據(jù)進(jìn)行圖像化，會(huì)更形象更容易懂。 表 7 偏航角實(shí)驗(yàn)數(shù)據(jù) 參考數(shù)據(jù) 測(cè)量數(shù)據(jù) 絕對(duì)誤差 相對(duì)誤差 6 結(jié)論 本次畢業(yè)設(shè)計(jì)收獲良多，完成了設(shè)計(jì)的要求，實(shí)現(xiàn)對(duì)姿態(tài)角的監(jiān)測(cè)，該系統(tǒng)能夠通過(guò)實(shí)時(shí)采集目標(biāo)載體上的傳感器的數(shù)據(jù)，經(jīng)過(guò)四元數(shù)算法和濾波處理后得到所要的姿態(tài)角數(shù)據(jù)，并用無(wú)線模塊發(fā)送給接收顯示模塊通過(guò)液晶 LCD1602 實(shí)姿態(tài)角。在抽取五個(gè)測(cè)量數(shù)據(jù)中，絕對(duì)誤差保持在的范圍內(nèi)，相對(duì)誤差則不是在一個(gè)小范圍內(nèi)，并且絕對(duì)誤差沒(méi)有隨著變量的規(guī)律變化和絕對(duì)誤差的變化而規(guī)律變化。在測(cè)出的五個(gè)數(shù)據(jù)中，絕對(duì)誤差保持在的范圍內(nèi)，相對(duì)誤差則不是在一個(gè)小范圍內(nèi)，并且絕對(duì)誤差沒(méi)有隨著變量的規(guī)律變化而規(guī)律變化。 通過(guò)四元數(shù)算法所得到俯仰角、翻滾角和偏航角的測(cè)量數(shù)據(jù)與參考數(shù)據(jù)做對(duì)比。 圖 15 翻滾角 Rool 濾波前后對(duì)比圖 農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 19 偏航角 kalman 濾波效果如下圖 16 所示，含有圓點(diǎn)的曲線為濾波前的數(shù)據(jù)曲線，線寬粗的曲線為 kalman 濾波后的數(shù)據(jù)曲線。 測(cè)試數(shù)據(jù) 通過(guò) Usart串口將 kalman 濾波前和濾波后的歐拉角發(fā)送到電腦上的 SerialChart軟件上畫(huà)圖表示，通過(guò)將數(shù)據(jù)做出圖表，如下圖 14 俯仰角 (Pitch)濾波前后對(duì)比圖所示，含有圓點(diǎn)的曲線為俯仰角還沒(méi)經(jīng)過(guò) kalman 濾波的數(shù)據(jù)曲線，線寬粗的曲線表示 kalman 濾農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 18 波后的數(shù)據(jù)曲線。調(diào)試時(shí)先調(diào)試發(fā)送端，禁用自動(dòng)應(yīng)答功能，禁用自動(dòng)重發(fā)功能，通過(guò)讀狀態(tài)寄存器，查看發(fā)送成功標(biāo)志是否置位檢測(cè)是否發(fā)送成功。經(jīng)過(guò)改變變阻器阻值大小調(diào)節(jié)液晶屏幕的對(duì)比度，液晶顯示正常。主要觀察液晶顯示屏是否能正常顯示。數(shù)據(jù)輸出正確后，則將采集的數(shù)據(jù)進(jìn)行四元素算法的轉(zhuǎn)換，每計(jì)算一次則轉(zhuǎn)換成歐拉角， Kalman 濾波后，通過(guò) Usart 串口將未濾波前和濾波后的歐拉角數(shù)據(jù)發(fā)送到電腦上，用 SerialChart 軟件進(jìn)行波形顯示對(duì)比。 硬件調(diào)試結(jié)果 首先調(diào)試陀螺儀和加速度計(jì)傳感器，使其能輸出數(shù)據(jù)，并將數(shù)據(jù)用 Usart 串口發(fā)到電腦串口調(diào)試助手軟件上顯示數(shù)據(jù)。 其中遇到的問(wèn)題很多，如印制電路線不合格，中間有些許短路，造成調(diào)試的失敗。同樣，如果電壓過(guò)低就沒(méi)有能力驅(qū)動(dòng)其負(fù)載。在本系統(tǒng)中我們都進(jìn)行了仔細(xì)的檢查，所以此步驟不會(huì)發(fā) 生故障，這一步如果檢查不細(xì)通電后可能會(huì)造成不可想象的后果，所以這一步也至關(guān)重要。特別注意電源的正負(fù)極以及電源之間是否有短路，并重點(diǎn)檢查地址總線、數(shù)據(jù)總線、控制總線是否存在相互間的短路或其他信號(hào)線的短路。首先在 Altium Designer 軟件中畫(huà)好原理圖和 PCB 板的布線圖，然后轉(zhuǎn)換成 reg 格式，發(fā)給印刷公司定制 PCB 板；根據(jù)已經(jīng)畫(huà)好的 PCB 布線圖，在萬(wàn)用板上焊出自己所設(shè)計(jì)的電路。 開(kāi) 始初 始 化 N R F 2 0 L 0 1 和L C D 1 6 0 2 顯 示 模 塊n R F 2 4 L 0 1 是 否 收 到 數(shù) 據(jù) ？L C D 1 6 0 2 顯 示 數(shù) 據(jù)數(shù) 據(jù) 處 理否是 圖 13 接收端程序流程圖 5 硬件電路 硬件制作 硬件的制作由萬(wàn)用板、 PCB 板和電子元器件組成。各通道接收地址(RX_ADDR_P0~RX_ADDR_P5)與發(fā)送端地址 (TX_ADDR)相同 (其中通道 25 只需設(shè)置低 8 位地址，高 32 位地址與通道 1 相同，無(wú)需設(shè)置 )，各通道接收數(shù)據(jù)長(zhǎng)度設(shè)置為 4 個(gè)字節(jié)，各通道工作頻率設(shè)為 (收發(fā)保持一致 )，各通道發(fā)射速率為 1Mb/s。將本機(jī)地址 (TX_ADDR)通過(guò) SPI 接口寫(xiě)入 nRF24L01，當(dāng) CSN 為低時(shí)數(shù)據(jù)被不斷寫(xiě)入。 開(kāi) 始預(yù) 計(jì) X ( k | k 1 )預(yù) 估 計(jì) 協(xié) 方 差 P 、 Q計(jì) 算 卡 爾 曼 增 益 K g進(jìn) 行 卡 爾 曼 濾 波更 新 X ( k | k 1 ) 和 協(xié) 方 差X ( k | k 1 ) = X ( k | k )P ( k | k 1 ) = p ( k | k ) 圖 12 kalman 濾波算法流程圖 農(nóng)業(yè)機(jī)械車輛的姿態(tài)角監(jiān)測(cè)設(shè)計(jì) 16 接收端流程 初始化液晶顯示 1602 和無(wú)線模塊 nRF24L01：初始化液晶顯示 LCD1602。 增益方程：增益方程是用于更新卡爾曼增益 Kg， P 表示 X(k|k1)的協(xié)方差，則有： (14) 濾波誤差協(xié)方差的推導(dǎo)： (15) (16) 式中： I 為 1 的矩陣，對(duì)于單模型單測(cè)量， I=1； P(k|k1)是 X(k|k1)對(duì)應(yīng)的協(xié)方差， P(k1|k1)是 X(k1|k1)對(duì)應(yīng)的協(xié)方差；表示 A 的轉(zhuǎn)置矩陣； Q 是系統(tǒng)過(guò)程的協(xié)方差 (施聞明， 20xx)。 濾波方程：濾波方程的目的就是利用第 k1 時(shí)刻的狀態(tài)估計(jì)值、第 k 時(shí)刻的觀測(cè)值和第 k 時(shí)刻的