智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn)

南 陽 理 工 學 院 本科生畢業(yè)設計（論文） 學 院：電子與電氣工程學院 專 業(yè)： 電子信息工程 學 生： 劉 一 峰 指導教師： 徐 源 完成日期 2014 年 5 月 南陽理工學院本科生畢業(yè)設計（論文） 智能三維空間姿態(tài)解算系統(tǒng) 設計與實現(xiàn) Design and Implementation of Intelligent Three-dimensional Attitude Calculation System 總 計： 30 頁 表 格： 2 個 插 圖： 21 幅 南南 陽陽 理理 工工 學學 院院 本本 科科 畢畢 業(yè)業(yè) 設設 計（論文）計（論文） 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) Design and Implementation of Intelligent Three-dimensional Attitude Calculation System 學 院： 電子與電氣工程學院 專 業(yè)： 電子信息工程 學 生 姓 名： 劉一峰 學 號： 105090440057 指 導 教 師（職稱）： 徐源（講師） 評 閱 教 師： 完 成 日 期： 南陽理工學院 Nanyang Institute of Technology 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn)智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 電子信息工程專業(yè) 劉一峰 摘摘 要要 本系統(tǒng)采用 STM32 作為微控制處理器，主要完成智能三維空間姿態(tài)解 算系統(tǒng)的設計。該系統(tǒng)主要實現(xiàn)以下功能。首先微控制處理器對加速度傳感器、陀螺 儀傳感器、磁力計傳感器進行數(shù)據(jù)采集，然后對采集到的數(shù)據(jù)進行數(shù)據(jù)濾波和算法解 算以得到姿態(tài)數(shù)據(jù)，再將目標物體的三維空間姿態(tài)數(shù)據(jù)在上位機上實時顯示，通過不 斷重復以上過程最終實現(xiàn)智能三維空間姿態(tài)解算的功能。 關鍵詞關鍵詞 姿態(tài)解算；微機電系統(tǒng)；四元數(shù)；歐拉角 Design and Implementation of Intelligent Three-dimensional Attitude Calculation System Electronic Information Engineering Specialty LIU Yi-feng Abstract: The system uses a STM32 as the micro-control processor, mainly to complete the design of intelligent three-dimensional attitude calculation system. The system has realized the following functions. The micro-processor collected the data of triaxial accelerometer, three-axis gyroscope and three-axis geomagnetic sensor through sensor circuits. And then it filtrated and calculated the data to obtain three-dimensional attitude data, afterwards displayed the calculated attitude data on the upper computer in real time, eventually to realize the intelligent three-dimensional attitude calculation system by repeating the above process. Key words: Attitude algorithm; micro-electromechanical systems; quaternion; Euler angle 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 目目 錄錄 1 引言1 1.1 三維空間姿態(tài)解算算法的發(fā)展歷程.1 1.2 國內外研究現(xiàn)狀與應用前景.1 1.3 論文組織結構安排.2 2 系統(tǒng)總體設計2 2.1 姿態(tài)航向參考系統(tǒng)的設計.2 2.1.1 MEMS 技術的介紹.3 2.1.2 MEMS 傳感器的工作原理.3 2.2 智能三維空間姿態(tài)解算系統(tǒng)的設計方案.4 3 智能三維空間姿態(tài)解算系統(tǒng)的硬件設計4 3.1 微控制處理器最小系統(tǒng)設計.4 3.1.1 微控制處理器的選型4 3.1.2 微控制器最小系統(tǒng)原理圖5 3.2 供電電路設計.6 3.2.1 供電電路芯片選型6 3.2.2 供電電路芯片特點及原理圖6 3.3 姿態(tài)航向參考系統(tǒng)模塊設計.7 3.3.1 MPU-6050 工作電路 8 3.3.2 HMC5883L 工作電路.9 3.3.3 傳感器模塊設計10 4 智能三維空間姿態(tài)解算系統(tǒng)的軟件設計13 4.1 軟件開發(fā)平臺.13 4.2 軟件下載及調試方式.14 4.3 軟件底層驅動程序設計.15 4.3.1 傳感器驅動程序的通信協(xié)議15 4.3.2 傳感器驅動程序的設計15 4.4 串口通信驅動程序設計.17 4.4.1 串口通信原理設計17 4.4.2 串口底層驅動設計流程圖18 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 4.5 數(shù)據(jù)處理方案.18 4.5.1 傳感器數(shù)據(jù)采集濾波設計18 4.5.2 三維空間姿態(tài)解算算法18 5 系統(tǒng)調試21 5.1 調試方法.21 5.2 調試結果及分析.23 結束語27 參考文獻28 附錄29 致謝30 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 1 引言 2014 年 3 月 8 日馬來西亞飛往北京的航班 MH370 失事，隨后確定已墜落 于南印度洋海域。為了搜尋此架飛機的黑匣子耗費了巨大的人力物力財力。飛 機上安裝的有兩種黑匣子，分別是駕駛艙語音記錄器和飛行數(shù)據(jù)記錄器。駕駛 艙語音記錄器主要用于記錄駕駛艙機組人員的言談和機務人員同地面控制人員 的對話等，而飛行數(shù)據(jù)記錄器主要記錄飛機的飛行參數(shù)、飛機操縱狀況、發(fā)動 機狀態(tài)、飛行姿態(tài)及各種儀表數(shù)據(jù)等1。因此，找到黑匣子就意味著能夠了解 到飛機在失事前發(fā)生的事情的全過程。其中，飛行姿態(tài)就是通過三維空間姿態(tài) 解算來實現(xiàn)的。研究智能三維空間解算系統(tǒng)的設計與實現(xiàn)不僅在航空領域有重 大的應用，而且在航海等其他領域也有很好的應用。 1.1 三維空間姿態(tài)解算算法的發(fā)展歷程 近現(xiàn)代以來，科學技術高速發(fā)展，智能的時代一直在更新前進，人們的生 活也正在一步一步地發(fā)生變化。人們出行從遠古時代的步行或者騎馬，到現(xiàn)在 的帶有導航系統(tǒng)的汽車以及民航飛機和油輪客輪，這些變化為我們的生活帶來 了極大的便利。而這些，離不開三維空間姿態(tài)解算技術的發(fā)展與應用。 采集目標的仰俯角、航偏角和滾動角是確定目標三維姿態(tài)的必要數(shù)據(jù)。我 們可以對角速度做積分來取得姿態(tài)數(shù)據(jù)，但是由于陀螺儀本身的誤差會隨溫度 變化，并且長時間對角速度積分等同于放大誤差，加上角速度所計算出來的角 度其中有一個為 90或 270時，轉回原始坐標時就會產生萬向節(jié)死鎖的問題， 導致轉不回去，這是一個很大算法缺陷。 1843 年愛爾蘭數(shù)學家哈密頓提出了四元數(shù)的基本概念，但是由于沒有實際 的應用而并沒有得到人們的重視。四元數(shù)方法開始獲得應用是在理論力學中。 因為單位四元數(shù)算法相似于旋轉矩陣的運算，在處理剛體運動學的實際問題時 四元數(shù)和四元數(shù)矩陣發(fā)揮了很大的作用。自此，人們對四元數(shù)產生了濃厚的興 趣。后來，四元數(shù)在光學領域、計算機領域和航空航天領域等都得到了廣泛的 應用。對四元數(shù)的研究與應用，使科學事業(yè)又向前邁進一步。用四元數(shù)解算出 歐拉角，可以避免產生萬向節(jié)死鎖，使三維空間姿態(tài)解算算法得到了進一步的 完善。 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 1.2 國內外研究現(xiàn)狀與應用前景 我國國內發(fā)展比較晚，近幾年才開始發(fā)展，隨著微傳感器技術的飛速發(fā)展， 三軸加速度計、三軸陀螺儀、三軸磁力計等之類的微傳感器也走向成熟與穩(wěn)定。 三維空間姿態(tài)解算這一技術也越來越受到大家的重視，近年來取得了較快的發(fā) 展，在三維空間姿態(tài)解算這方面取得了較大的進步。 在國外，三維空間姿態(tài)解算技術已經(jīng)在民用和軍用領域得到了廣泛的認可。 民用方面，精細農業(yè)用的機械與車輛、用于農藥噴灑與林業(yè)防火的無人飛機等 已部分分裝該類型組合導航系統(tǒng)；軍用方面，它在無人偵察機和衛(wèi)星探測器等 上也已被多數(shù)發(fā)達國家應用。 隨著時代的變化和科技的迅速發(fā)展，把微傳感器做的更加小型化、智能化 和集成化也成為了現(xiàn)代社會的必要。采用體積小、可靠性高、成本低的微傳感 器不僅可以優(yōu)化國民經(jīng)濟生活，還可以促進國防事業(yè)的發(fā)展。智能三維空間姿 態(tài)解算系統(tǒng)除了應用在航空航天領域中，同時在海洋探測領域同樣有重要意義， 比如說監(jiān)測潛艇運行姿態(tài)和控制水下自動設備的穩(wěn)定性等。不僅如此，在機器 人運動領域、汽車工業(yè)領域、人體運動測量等方面也有很好的應用前景。以后 的國民生活很可能離不開智能三維空間解算系統(tǒng)了，因為它將會遍布國民生活 的各個角落。 1.3 論文組織結構安排 論文圍繞智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn)展開論證。第 1 節(jié)從三維 空間姿態(tài)解算的發(fā)展歷程、國內外發(fā)展現(xiàn)狀和應用前景方面進行了闡述。第 2 節(jié)主要從姿態(tài)航向參考系統(tǒng)的組成和智能三維空間姿態(tài)解算系統(tǒng)的設計方案方 面介紹了系統(tǒng)總體設計。第 3 節(jié)主要從系統(tǒng)微控制處理器最小系統(tǒng)、傳感器采 集電路、系統(tǒng)供電電路的設計入手，介紹智能三維空間姿態(tài)解算系統(tǒng)的整體硬 件設計。第 4 節(jié)主要介紹軟件設計平臺、驅動程序設計以及姿態(tài)解算的算法等。 第 5 節(jié)主要介紹系統(tǒng)的調試方法和設計結果與分析。 2 系統(tǒng)總體設計 2.1 姿態(tài)航向參考系統(tǒng)的設計 為了獲取空間目標物體的姿態(tài)信息，就需要建立 AHRS（Attitude and Heading Reference System） ，即姿態(tài)航向參考系統(tǒng)，它主要包含了基于 MEMS 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 的三軸加速度傳感器、三軸陀螺儀傳感器、三軸電磁羅盤傳感器，把它們合稱 為九軸傳感器。隨著科學技術的不斷進步，電子行業(yè)也取得了飛速的發(fā)展。 2.1.1 MEMS 技術的介紹 MEMS（micro-electromechanical systems）是微機電系統(tǒng)的英文縮寫，是一 種先進的制造技術平臺。它是指可以批量制造的，由微型傳感器，微型執(zhí)行器， 信號處理和控制電路，通訊接口和電源等部件組成的一體化的微型器件或系統(tǒng)， 其目的是把信息的提取、處理和執(zhí)行集成在一起，組成具有多功能的微型系統(tǒng)， 集成于打尺寸系統(tǒng)中，從而大幅度地提高系統(tǒng)的自動化、智能化和可靠性水平2。 MEMS 涉及到了很多學科，包括電子、材料、物理、化學、生物和機械等。二 十一世紀 MEMS 將逐步從實驗室走向實用化，對工農業(yè)、信息、環(huán)境、生物工 程、醫(yī)療、空間技術、國防和科學發(fā)展產生重大影響。 MEMS 有如下幾點特點： （1）微型化。 （2）以硅為主要材料，機械電器性能良好。 （3）批量生產。 （4）集成化。 （5）多學科交叉。 MEMS 器件具有由于體積微小，功耗低，響應快，靈敏度高，質量輕，成 本低的優(yōu)點，因此在運動測量領域也逐漸開始采用微電子器件3。它作為新一 代的慣性器件，在三維姿態(tài)解算系統(tǒng)中發(fā)揮著越來越重要的作用。 2.1.2 MEMS 傳感器的工作原理 （1）加速度傳感器的工作原理：采用彈簧質量系統(tǒng)的結構形式，質量塊與 電極之間的距離會隨質量所受慣性力的變化而改變，因而電容值也隨之改變且 和加速度成正比關系。電容式加速度傳感器具有很多優(yōu)點，比如高靈敏度、容 易適應環(huán)境以及零頻率響應等。雖然它也有像輸入輸出呈非線性的關系等缺點， 但在本系統(tǒng)中影響不是很大，基本上可以忽略，而主要充分利用了它的優(yōu)點。 （2）陀螺儀傳感器的工作原理：陀螺儀是測角速度的傳感器。MEMS 陀 螺儀依賴轉動坐標系角振動產生科里奧利力效應來檢測角速度4。它通常有兩 個方向的可移動電容板，徑向的電容板加震蕩電壓迫使導體作徑向運動，橫向 的電容板測量由于橫向科里奧利運動帶來的電容變化。角速度可以依據(jù)科里奧 利力和角速度成正比的關系計算出。 （3）電磁羅盤傳感器的工作原理：利用磁阻傳感器測量地磁場強度，根據(jù) 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 其水平分量的比值來確定載體相對北向的航向角。一般情況下，安裝在機體上 的電子磁羅盤的坐標系和機體坐標系重合。 2.2 智能三維空間姿態(tài)解算系統(tǒng)的設計方案 本系統(tǒng)的設計主要包括硬件設計和軟件設計，其中姿態(tài)解算是依據(jù)數(shù)學和 物理原理來實現(xiàn)的，并通過 C 語言編寫成相應的程序，并結合實際電路進行調 試，以實現(xiàn)將空間目標物體的瞬時姿態(tài)數(shù)據(jù)顯示精確地在上位機上。設計本系 統(tǒng)需要做的三個方面。 （1）系統(tǒng)硬件平臺的設計，主要有供電電路、單片機最小系統(tǒng)、傳感器采 集電路、串口通信這幾部分構成。 （2）系統(tǒng)軟件的設計，主要是對傳感器獲取的數(shù)據(jù)，進行濾波，四元數(shù)、 歐拉角算法的處理，最終輸出結果。 （3）三維空間姿態(tài)解算的調試工作，主要是通過串口把數(shù)據(jù)發(fā)到上位機進 行調試，最終實現(xiàn)系統(tǒng)任務。 二十一世紀是信息的時代，是科技的時代。只有及時地獲得可靠的精確的 信息數(shù)據(jù)才能夠掌握先機，使自己處于優(yōu)勢的地位。因此，設計本系統(tǒng)，要做 到能夠快速并連續(xù)地提取姿態(tài)數(shù)據(jù)，并且保證姿態(tài)數(shù)據(jù)的精確性和準確性。不 僅如此，在得到這些數(shù)據(jù)的同時，要及時地傳送到上位機，以供檢測和控制。 然而在現(xiàn)實中，會存在不同程度的噪聲干擾，并且通過姿態(tài)解算原理獲得的更 新數(shù)據(jù)也會存在些許誤差，提高系統(tǒng)的精確度是完善智能三維空間姿態(tài)解算系 統(tǒng)的必需之處。 3 智能三維空間姿態(tài)解算系統(tǒng)的硬件設計 硬件是一個電子系統(tǒng)的基礎，一個良好的硬件平臺是系統(tǒng)穩(wěn)定型的標志， 所以硬件平臺的搭建是不容忽視的。智能三維空間姿態(tài)解算在硬件設計方面主 要分為以下幾個模塊：供電模塊，微控制器最小系統(tǒng)模塊，傳感器采集模塊， 串口通信模塊，這幾部分構成了整體的硬件設計平臺。 考慮到系統(tǒng)的穩(wěn)定性，因此對于傳感器這部分電路，可以采用現(xiàn)有的模塊 來實現(xiàn)，這樣避免了由于人工焊接導致在一定程度上損傷傳感器的穩(wěn)定性和精 確度，因此我們選擇了 GY-86 這個九軸傳感器模塊。 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 3.1 微控制處理器最小系統(tǒng)設計 3.1.1 微控制處理器的選型 微控制處理器的選型關系到整個系統(tǒng)設計是十分重要的，對于三維空間姿 態(tài)解算這個系統(tǒng)的設計需要考慮到以下三個方面。 （1）微控制器的執(zhí)行速度，在本系統(tǒng)設計中由于要對多個傳感器進行數(shù)據(jù) 采集，并且采集的速率較高，因此微控制的時鐘要高。 （2）微控制器的數(shù)據(jù)處理能力，在本系統(tǒng)要通過四元數(shù)等算法去處理一些 數(shù)據(jù)。其中尤其是關于浮點型數(shù)據(jù)的處理。 （3）微控制器的穩(wěn)定性，對于每一個綜合電子系統(tǒng)，這一點都是必不可少 的。系統(tǒng)的穩(wěn)定性決定系統(tǒng)的成敗。 由于上述因素，最終選取了 STM32 作為智能三維空間姿態(tài)解算系統(tǒng)的微控 制處理器。STM32F103x 增強型系列芯片使用高性能的 ARM Cortex-M3 32 位的 RISC 內核，工作頻率為最高可達 72MHz，內置高速存儲器(高達 128K 字節(jié)的 FLASH 和 20K 字節(jié)的 SRAM)，豐富的增強 I/O 端口和連接到兩條 APB 總線的 外設。所有型號的器件都包含 2 個 12 位的 ADC、3 個通用 16 位定時器和一個 PWM 定時器，還包含標準和先進的通信接口：多達 2 個 I2C 和 SPI、3 個 USART、一個 USB 和一個 CAN5。它的工作電壓為 3.3V。該智能三維空間姿 態(tài)解算系統(tǒng)的硬件電路是以 STM32F103C8T6 為微控制器，最終選擇 C8T6 是 因為它還具有以下優(yōu)點： STM32 增強型系可以與所有的 ARM 工具和軟件兼容。 STM32 的內部 FLASH 是在線可編程的。 STM32 有優(yōu)秀的功耗控制。 STM32 擁有強大的庫函數(shù)。 除此之外，它只有 48 個引腳，不僅滿足了本次設計的需要，而且價格便宜， 性價比高。 3.1.2 微控制器最小系統(tǒng)原理圖 本設計的最小系統(tǒng)原理圖如圖 1 所示。主要有以下幾部分構成：微控制處 理器，SWD 調試下載接口，外圍晶振電路，以及復位電路這幾部分構成。這保 證了本系統(tǒng)的能夠完成單片機運作的基本功能，是實現(xiàn)三維空間姿態(tài)解算的基 礎。 （1）微控制器：在本次設計中由于對 STM32 系列的單片機引腳和片內存 儲器容量要求較小，因此選用 STM32F103C8T6。 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) （2）供電電路：工作電壓為 2.0V 到 3.6V ，在本次設計中采用 3.3V 供電。 （3）晶振電路：單片機內部自帶 8MhzRC 震蕩器，本系統(tǒng)使用外接 8Mhz 的石英晶振。 （4）復位電路：采用經(jīng)典的按鍵復位電路，通過按鍵按下拉低 Reset 引腳， 使其硬件復位。 （5）下載電路：本次采用 SWD 方式和 ISP 方式下載程序，采用 SWD 模 式也可以進行軟件硬件仿真。 +3.3V GND TO05 TO06 TO07 TO04 UART1-TX UART1-RX UART2-RX UART2-TX BOOT0 R3 10K GND H-SDA H-SCL FSYNC INTA DRDY MO SCK CSN CE IRQ GND 22pF C10 22pF C9 1 Y1 8MHz 10K R1 0.1uF C11 GND +3.3V S1 switch ADC-4 ADC-5 R2 10K GND SWDIO SWCLK Beep LED2 LED1 power NRST MI PA0/TIM2-CH1 10 PA1/TIM2-CH2 11 PA2/TIM2-CH3 12 PA3/TIM2-CH4 13 PA6/TIM3-CH1 16 PA7/TIM3-CH2 17 PB0/TIM3-CH3 18 PB1/TIM3-CH4 19 PA4/ADC12-IN4 14 PA5/ADC12-IN5 15 PC13-ANTI-TAMP 2 PC14-OSC32-IN 3 PC15-OSC32-OUT 4 PA9/USART1-TX 30 PA10/USART1-RX 31 OSC-IN 5 OSC-OUT 6 PB6/TIM4-CH1 42 PB7/TIM4-CH2 43 PB8/TIM4-CH3 45 PB9/TIM4-CH4 46 PA11/TIM1-CH4 32 PA8/TIM1-CH1 29 PB10/I2C2-SCL 21 PB11/I2C2-SDA 22 PB12/SPI2-NSS 25 PB13/SPI2-SCK 26 PB14/SPI2-MISO 27 PB14/SPI2-MOSI 28 PB5 41 PA12 33 PB3/JTDO 39 PB4/JNTRST 40 PA15/JTDI 38 PB2/BOOT1 20 BOOT0 44 PA14/JTCK/SWCLK 37 PA13/JTMI/SWDIO 34 NRST 7 VBRT 1 VDDA 9 VDD-1 24 VDD-2 36 VDD-3 48 VSSA 8 VSS-2 23 VSS-3 35 VSS-4 47 STM 32F103C 8T6 * STM32F103C8T6(XIN) 1 2 3 4 5 P1 SWD +3.3V GND BOOT0 UART1-RX UART1-TX 圖 1 STM32F103C8T6 最小系統(tǒng)原理圖 3.2 供電電路設計 3.2.1 供電電路芯片選型 供電芯片的選型，這個要考慮系統(tǒng)的總體硬件設計需求。 （1）本系統(tǒng)主要有微控制器，傳感器，串口這幾個部分需要供電。而在本 次設計中，微控制器需要 3.3V 供電，傳感器需要 3.3V 供電，串口也需要 3.3V 供電。因此本次設計需要產生 3.3V 的電源。同時也要估計到供電電流和輸出電 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 流。 （2）由于智能三維空間姿態(tài)解算系統(tǒng)這個設計一般是用在飛行器等對對空 間姿態(tài)要求較高的地方使用，因此本次的輸入電壓設定為 12V。 經(jīng)過考慮最終選取了 LM2940-5.0 和 ASM1117-3.3 ，其中 LM2940-5.0 把 12V 轉化到 5V，ASM1117-3.3 把 5V 轉化輸出為 3.3V。 3.2.2 供電電路芯片特點及原理圖 （1）LM2940-5.0 此芯片是輸出電壓固定的低壓差三端穩(wěn)壓器。它的電氣 特性如下： 輸出電壓：5V 輸出電流為 1A，輸出電流為 1A 時，最小輸入輸出電壓差小于 0.8V。 最大輸入電壓：26V 工作溫度：-40+125 內含靜態(tài)電流降低電路、電流限制、過熱保護和反插入保護電路等。 因此穩(wěn)壓器完全可以滿足本次設計的需要。 （2）ASM1117-3.3 這個芯片固定電壓 3.3V 輸出電流為 1A 線路調整率； 0.2%（MAX）負載調整率：0.4%（MAX） 。它的電氣特性如下： 輸出電壓：3.2673.333V（0= IOUT=1A , 4.75V=VIN=12V） 線路調整（MAX）：10mV（4.75V=VIN=12V） 負載調節(jié)（MAX）：15mV（VIN=5V，0= IOUT=1A） 電壓差（MAX）：1.3V 電流限制：9001500mA 靜態(tài)電流（最大） ，10mA 紋波抑制（最小）： 60dB 通過對這兩種穩(wěn)壓器的了解，參考對應芯片的芯片手冊，設計了供電電路 如圖 2 所示，此供電方案達到了本系統(tǒng)的供電需求。 10uF C3 Cap Pol1 22uF C4 Cap Pol1 GND +3.3V GND1 OUT 2 IN 3 AMS1117-3.3 電源電路 GND +12V 30K R9 Res2 10K R10 Res2 GND+12V power 12 D5 1N5822 1 2 P11 串串 GND3 OUT 2 IN 1 LM2940-5.0 GND 47uF C1 22uF C2 +5V 圖 2 系統(tǒng)供電原理圖 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 3.3 姿態(tài)航向參考系統(tǒng)模塊設計 姿態(tài)航向參考系統(tǒng) AHRS(Attitude and Heading Reference System)主要包含 了基于 MEMS 的三軸加速度計，三軸陀螺儀和三軸磁阻傳感器?？臻g目標物體 的三軸加速度和三軸角速度分別由加速度計和三軸陀螺儀測量出。三軸磁阻傳 感器可以感應目標物體姿態(tài)變化時磁場在各測量軸向上的強度變化，并由此可 以計算出偏航角6。傳感器部分主要采用了 MPU-6050 和 HMC5883L 這兩個芯 片，由它們兩個組成了姿態(tài)航向參考系統(tǒng)的硬件部分。三軸陀螺儀傳感器和三 軸加速度計傳感器集成在芯片 MPU-6050 中，而 HMC5883L 包含了一個三軸地 磁傳感器。這兩個芯片通過上拉電阻掛接在 I2C 總線上。圖 3 表示了姿態(tài)航向 參考系統(tǒng)的結構框圖。 圖 3 姿態(tài)航向參考系統(tǒng)的結構框圖 3.3.1 MPU-6050 工作電路 MPU-6050 集成了三軸 MEMS 陀螺儀和三軸 MEMS 加速度計，以及一個 可擴展的數(shù)字運動處理器 DMP（Digital Motion Processor） 。MPU-6050 分別用 了三個 16 位的 ADC 將三軸陀螺儀和三軸加速度計采集到的模擬量數(shù)據(jù)轉化成 可輸出的數(shù)字量。用戶可控傳感器的測量范圍能夠更加精確地跟蹤快速和慢速 的運動。電源范圍，MPU-6050 可支持 VDD 范圍為 2.5V5%，3.0V5%，3.3V5%。另 MPU-6050 還有 VLOGIC 引腳，可為 I2C 輸出提供邏輯電平，VLOGIC 電壓可取 1.8V5%或者 VDD。 其他的參數(shù)如下： 陀螺儀運作電流：5mA 陀螺儀待命電流：5A 加速度計運作電流：500A 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 加速度計省電模式電流：40A10Hz 芯片 MPU-6050 采用 5V 或 3.3V 電源輸入，XDA/XCL 用于外接其他 I2C 接口傳感器。 SDA 和 SCL 分別連接到單片機。INT 產生中斷信號，連接至單 片機。AD0 接地。 注意使用 IO 口模擬 I2C 信號時候，要注意 SDA 所連接的單 片機 IO 口輸入輸出方向設置。MPU-6050 使用 I2C 接口和芯片連接，并且總是 作為從設備。連接主設備的邏輯電平用 VLOGIC 引腳設置。MPU-6050 的應用 電路如 4 所示。 CLKIN 1 AUX_DA 6 AUX_CL 7 VLOGIC 8 AD0 9 REGOUT 10 FSYNC 11 INT 12 VDD 13 GND 18 RESV 19 CPOUT 20 RESV 21 CLKOUT 22 SCL 23 SDA 24 U2 MPU-6050 GND XDA XCL VCC_3.3V0.01uF C18 0.1uF C20 FSYNC INTA 1K R16 GND SDA SCL 0.1uF C21 GND VCC_3.3V 0.1uF C19 GND 圖 4 MPU-6050 應用電路 3.3.2 HMC5883L 工作電路 HMC5883L 包括最先進的高分辨 HMC118X 系列磁阻傳感器。磁阻傳感器 是利用各向異性磁電阻效應制作的傳感器，各向異性磁阻效應指的是鐵磁金屬 （例如：鉆、鐵、鎳等及其合金）的磁化方向和電流方向平行時的電阻率和兩 者垂直是的電阻率有明顯差別7。采用霍尼韋爾各向異性磁電阻（Anisotropic Magneto Resistive）技術的 HMC5883L，是一種表面貼裝的高集成模塊，并帶 有數(shù)字接口的弱磁傳感器芯片，而且它內部附帶了霍尼韋爾專利的集成電路， 具有在軸向高靈敏度和線性高精度的特點。在地磁場傳感器行業(yè)中，霍尼韋爾 的磁傳感器是靈敏度最好和可靠性最好的傳感器。 HMC5883L 具有以下特點： 三軸磁阻傳感器和 ASIC 都被封裝在 3.03.00.9mm LCC（Leadless Chip Carriers）表面裝配中 12-bit ADC 與低干擾 AMR 傳感器，能在8 高斯的磁場中四線 5 毫高斯 分辨率 內置自檢功能 低電壓工作（2.16-3.6V）和超低功耗（100A） 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 內置驅動電路 I2C 數(shù)字接口 無引線封裝結構 磁場范圍廣(+/-80e) 有相應軟件及算法支持 最大輸出頻率可達 160Hz 根據(jù) HMC5883L 的各個管腳需要這樣設計電路。管腳 2、4 和 13 并聯(lián)并分 兩條支路，一支接 3.3V 的供電電路，一支和 0.1F 的電容 C12 串聯(lián)并接地。 XCL 和 XDA 用于接其他 I2C 接口傳感器。STEP 和 SETC 通過 C16 串聯(lián)起來 用于驅動傳感器。管腳 10 是需要串聯(lián)電容并接地。DRDY 連接到 STM32C8T6 的第 39 管腳 PB3/JTDO 處，它的內部被拉高，當數(shù)據(jù)位于輸出寄存器時會在 低電位上停 250sec。HMC5883L 的應用電路如圖 5 所示。 VDD_IO 13 S1 4 VDD 2 SCL 1 SDA 16 STEP 8 SETC 12 C1 10 GND 9 GND 11 DRDY 15 U1 HMC5883L VCC_3.3V 0.1uF C12 GND XCL XDA 224 C16 DRDY 475 C17 GND 圖 5 HMC5883L 應用電路 3.3.3 傳感器模塊設計 智能三維空間姿態(tài)解算系統(tǒng)的設計，要提高系統(tǒng)的精確度和穩(wěn)定性，采用 現(xiàn)有的傳感器模塊是十分有必要的。 采用模塊有以下兩個優(yōu)點： （1）減少了焊接的難度，三軸加速度，三軸陀螺儀，三軸電磁羅盤傳感器 都是 BGA 封裝，如果自己焊接難度很大，并且在焊接的過程中對傳感器的精 度產生影響。 （2）減少了機械布局導致的誤差，對于元件的擺放，如果元件的擺放位置 誤差較大，會影響到系統(tǒng)的精確度，并且對于軟件設計也會帶來較多的麻煩。 此模塊主要有以下幾部分構成，模塊的供電電路，在本模塊中由于考慮到 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 用戶供電電壓一般為 3.3V 或 5V，因此該模塊同時提供了這兩種電源接口設計。 另外該模塊上面還用工作指示燈，當該模塊正常工作時，此指示燈點亮，如指 示燈處于熄滅狀態(tài)，則要立即檢查硬件設計電路，避免意外事件發(fā)生。另外該 模塊主要用 MPU-6050、HMC5883L 傳感器進行數(shù)據(jù)采集，而 MPU-6050 有支 持外接 I2C 接口器件，因此在此處選擇了 HMC5883L 傳感器掛接在 MPU-6050 模塊上，對于外接傳感器的工作模式，可以對 MPU-6050 的寄存器進行配置來 確定。傳感器模塊原理圖如圖 6 所示。 VDD_IO 13 S1 4 VDD 2 SCL 1 SDA 16 STEP 8 SETC 12 C1 10 GND 9 GND 11 DRDY 15 U1 HMC5883L I 1 GND 2 en 3 by 4 O 5 U3 LDO CLKIN 1 AUX_DA 6 AUX_CL 7 VLOGIC 8 AD0 9 REGOUT 10 FSYNC 11 INT 12 VDD 13 GND 18 RESV 19 CPOUT 20 RESV 21 CLKOUT 22 SCL 23 SDA 24 U2 MPU-6050 0.1uF C13 GNDGND 10uF C14 0.1uF C15 GND 1K R11 D6 LED0 GND 4.7K R12 4.7K R13 Q2 N_MOS 4.7K R14 4.7K R15 Q3 N_MOS VCC_5VVCC_3.3V VCC_3.3VVCC_5V H_SCL VCC_5VVCC_3.3V H_SDASDA VCC_3.3V 0.1uF C12 GND XCL XDA 224 C16 DRDY 475 C17 GND GND XDA XCL VCC_3.3V0.01uF C18 0.1uF C20 FSYNC INTA 1K R16 GND 1 2 3 4 5 6 7 8 P14 Header 8H VCC_5V VCC_3.3V GND H_SCL H_SDA FSYNC INTA DRDY SDA SCL 0.1uF C21 GND VCC_3.3V 0.1uF C19 GND SCL 圖 6 傳感器模塊原理圖 以上主要從微控制處理器最小系統(tǒng)、供電電路和姿態(tài)航向參考系統(tǒng)的硬件 設計方面具體的介紹了智能三維空間姿態(tài)解算系統(tǒng)的幾個模塊。接下來，需要 對智能三維空間姿態(tài)解算系統(tǒng)做一個整體性的闡述。 （1）最小系統(tǒng)微控制器的設計，這是系統(tǒng)的控制核心，對于此部分的設計 要考慮 CPU 的供電電壓、晶振電路、復位電路、下載電路等的設計，尤其是對 于下載電路的設計要注意布線規(guī)則，否則有可能導致芯片無法下載程序，進而 導致設計失敗。 （2）系統(tǒng)的供電電源，這部分主要保證整個系統(tǒng)的供電需求以及系統(tǒng)的穩(wěn) 定性，因此要考慮系統(tǒng)的供電電壓是否滿足，電壓的紋波是否過大，還有最重 要的一點就是系統(tǒng)的整體供電電流的大小，否則會導致系統(tǒng)傳感器數(shù)據(jù)采集出 現(xiàn)異常現(xiàn)象。 （3）傳感器模塊的設計，在這一部分主要考慮傳感器的工作電壓，以及傳 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 感器的數(shù)據(jù)接口，這兩點可以保證傳感器的數(shù)據(jù)正確采集。另外還有一點就是 各個傳感器的機械布局，因為本設計主要是進行姿態(tài)解算，傳感器芯片的布局 會影響到軟件算法的設計，這三點考慮到就可以達到設計的需求了，最終本設 計選擇了成品傳感器模塊。 另外在整個系統(tǒng)的硬件設計中，考慮到以后的擴展應用，又添加了一些 LED 和蜂鳴器聲光調試電路，AD 電壓采集接口以及無線收發(fā)電路等，方便該 系統(tǒng)以后的二次開發(fā)使用，提高硬件設計的潛在價值。結合以上三方面介紹， 該系統(tǒng)的硬件設計原理圖如圖 7 所示。 +3.3V GND 1 2 3 P3 1串串串 1 2 3 P4 2串串串 1 2 3 P5 3串串串 1 2 3 P6 4串串串 GNDGNDGNDGND TO04TO05TO06TO07 四路電調(PWM) +5V+5V+5V+5V 1 2 3 4 5 P1 串串1 1 2 3 4 P2 串串2 +3.3V 串口 GNDGND BOOT0 UART1-RX UART1-TX UART2-RX +3.3V TO05 TO06 TO07 TO04 UART1-TX UART1-RX UART2-RX UART2-TX BOOT0 R3 10K GND H-SDA H-SCL 1 2 3 4 5 6 7 8 P8 10DOF +5V +3.3V H-SCL H-SDA INTA DRDY FSYNC 10DOF接口 GND FSYNC INTA DRDY SCK MI MO CSN IRQ CE SPI(24L01) +3.3VGND MO SCK CSN CE IRQ GND 22pF C10 22pF C9 1 Y1 8MHz 10K R1 0.1uF C11 GND +3.3V S1 switch C5 0.1uF C6 0.1uF C7 0.1uF C8 0.1uF GND +3.3V 10uF C3 Cap Pol1 22uF C4 Cap Pol1 GND +3.3V GND1 OUT 2 IN 3 AMS1117-3.3 電源電路 ADC-4 ADC-5 1 2 P9 ADC-4串5 模擬口 R2 10K GND ADC-4 ADC-5 SWDIO SWCLK SWDIO SWCLK SW調試接口 D1 LED1 1K R4 Res2 D2 LED1 1K R5 Res2 +5V +3.3V GND GND D3 LED1 1K R6 Res2 +3.3V LED +5V 560 R8 Res2 GND Beep Beep LED1 蜂鳴 D4 LED1 1K R7 Res2 +3.3V LED2 LED2 LED1 GND +12V 12 34 56 P12 Header 3X2 12 34 56 P13 Header 3X2 +5V+3.3V GNDGND 引出電源接口 30K R9 Res2 10K R10 Res2 GND+12V power power +3.3V NRST A 1 K 2 LS1 串串串 Q1 J3Y UART2-TX MI GND 1 2 3 4 5 6 P10 Header 3X2A NRST PA0/TIM2-CH1 10 PA1/TIM2-CH2 11 PA2/TIM2-CH3 12 PA3/TIM2-CH4 13 PA6/TIM3-CH1 16 PA7/TIM3-CH2 17 PB0/TIM3-CH3 18 PB1/TIM3-CH4 19 PA4/ADC12-IN4 14 PA5/ADC12-IN5 15 PC13-ANTI-TAMP 2 PC14-OSC32-IN 3 PC15-OSC32-OUT 4 PA9/USART1-TX 30 PA10/USART1-RX 31 OSC-IN 5 OSC-OUT 6 PB6/TIM4-CH1 42 PB7/TIM4-CH2 43 PB8/TIM4-CH3 45 PB9/TIM4-CH4 46 PA11/TIM1-CH4 32 PA8/TIM1-CH1 29 PB10/I2C2-SCL 21 PB11/I2C2-SDA 22 PB12/SPI2-NSS 25 PB13/SPI2-SCK 26 PB14/SPI2-MISO 27 PB14/SPI2-MOSI 28 PB5 41 PA12 33 PB3/JTDO 39 PB4/JNTRST 40 PA15/JTDI 38 PB2/BOOT1 20 BOOT0 44 PA14/JTCK/SWCLK 37 PA13/JTMI/SWDIO 34 NRST 7 VBRT 1 VDDA 9 VDD-1 24 VDD-2 36 VDD-3 48 VSSA 8 VSS-223 VSS-335 VSS-447 STM 32F103C 8T6 * STM32F103C8T6(XIN) GND 1 VCC 2 CE 3 CSN 4 SCK 5 MOSI 6 MISO 7 IRQ 8 P7 NRF24L01 12 D5 1N5822 1 2 P11 串串 +3.3V GND3 OUT 2 IN 1 LM2940-5.0 GND 47uF C1 22uF C2 +5V 1 2 3 4 5 P1 SWD +3.3V GND BOOT0 UART1-RX UART1-TX 圖 7 硬件設計原理圖 根據(jù)系統(tǒng)硬件的原理圖可以利用 Protel 軟件可以畫出系統(tǒng)硬件的 PCB 板如 圖 8 所示. 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 圖 8 系統(tǒng)硬件的 PCB 板 智能三維空間姿態(tài)解算系統(tǒng)的實物如圖 9 所示。 圖 9 智能三維空間姿態(tài)解算系統(tǒng)實物 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 4 智能三維空間姿態(tài)解算系統(tǒng)的軟件設計 智能三維空間姿態(tài)解算系統(tǒng)的軟件設計是系統(tǒng)的靈魂。軟件設計的好壞決 定了設計的成敗。這里主要從軟件的編程環(huán)境，程序的下載及調試方式，傳感 器驅動的設計和三維姿態(tài)解算控制算法的設計來展開介紹。 4.1 軟件開發(fā)平臺 在本系統(tǒng)中采用了 Keil uVision4 作為系統(tǒng)軟件的開發(fā)工具。Keil uVision4 引入靈活的窗口管理系統(tǒng)，使開發(fā)人員能夠使用多臺監(jiān)視器，并提供了視覺上 的表面對窗口位置的完全控制的任何地方。新的用戶界面可以更好地利用屏幕 空間和更有效地組織多個窗口，提供一個整潔，高效的環(huán)境來開發(fā)應用程序。 新版本支持更多最新的 ARM 芯片，還添加了一些其他新功能。在以下幾個方 面體現(xiàn)出來。 （1）uVision4 引入了窗口管理系統(tǒng)靈活方便，支持多顯示器窗口。 （2）uVision4 在 uVision3 IDE 的基礎上，增加了更多大眾化的功能。 （3）多顯示器和靈活的窗口管理系統(tǒng)。 （4）系統(tǒng)瀏覽器窗口的顯示設備外設寄存器信息。 （5）調試還原視圖創(chuàng)建并保存多個調試窗口布局。 （6）多項目工作區(qū)簡化與眾多的項目。 因此在此版本下可以方便簡潔的完成系統(tǒng)軟件開發(fā)的需求，一個穩(wěn)定易用 的軟件開發(fā)平臺可以大大的提高我們的開發(fā)效率。 4.2 軟件下載及調試方式 STM32F10x 系列單片機有三種下載方式，分別是 J-Flash 下載（需要用到 J-link） ，MDK 配置下載（需要用到 J-link） ，ISP 下載（通過串口下載） 。這三中 下載方式各有特點。以下對這幾部分進行介紹。 （1）J-Flash：J-Link 是 SEGGER 公司為支持仿真 ARM 內核芯片推出的 JTAG 仿真器。配合 IAR EWAR，ADS，KEIL，WINARM，RealView 等集成 開發(fā)環(huán)境支持所有 ARM7/ARM9/ARM11,Cortex M0/M1/M3/M4, Cortex A4/A8/A9 等內核芯片的仿真，與 IAR,Keil 等編譯環(huán)境無縫連接，操作方便、 連接方便、簡單易學，是學習開發(fā) ARM 最好最實用的開發(fā)工具。 （2）MDK：Keil MDK，也稱 MDK-ARM、uVision4 等。目前三家國內代 理商為 Keil MDK 提供技術支持和相關服務。MDK-ARM 軟件為基于 Cortex- 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) M、Cortex-R4、ARM7、ARM9 處理器設備提供了一個完整的開發(fā)環(huán)境。 MDK-ARM 專為微控制器應用而設計，不僅易學易用，而且功能強大，能夠滿 足大多數(shù)苛刻的嵌入式應用。其環(huán)境配置部分圖如圖 10 所示。 圖 10 MDK 下載配置 （3）ISP：ISP（In-System Programming）即在線系統(tǒng)編程，一種無需將存 儲芯片（如 EPROM）從嵌入式設備上取出就能對其進行編程的過程。在系統(tǒng) 編程需要在目標板上有額外的電路完成編程任務。其優(yōu)點是，即使器件焊接在 電路板上，仍可對其重新進行編程。在系統(tǒng)可編程是 Flash 存儲器的固有特性 （通常無需額外的電路） ，F(xiàn)lash 幾乎都采用這種方式編程8。 經(jīng)過對比可以看到，在 Keil uVision4 軟件開發(fā)平臺下，使用 MDK 結合 J- Link 的下載方式十分方便，簡介只要配置好軟件的下載環(huán)境，就可以方面使用， 并且 J-Link 可以實現(xiàn)硬仿真的功能，更有利于我們找到問題的所在。其中 J- link 下載有幾種接口，這次我們選用了 SWD 這種接口方式，SWD 這種接口占 用的 IO 管腳少，并且下載方便快速穩(wěn)定。 4.3 軟件底層驅動程序設計 4.3.1 傳感器驅動程序的通信協(xié)議 在智能三維空間姿態(tài)解算系統(tǒng)中，我們主要用了三軸加速度、三軸陀螺儀 以及三軸電磁羅盤，整體上簡稱為九軸傳感器。九軸傳感器都是通過 I2C 通信 的，通過 I2C 協(xié)議對傳感器進行讀寫操作，最終獲取到相應的傳感器數(shù)據(jù)。 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) I2C（Inter-Integrated Circuit）總線是由 PHILIPS 公司開發(fā)的兩線式串行總 線，用于連接微控制器及其外圍設備。是微電子通信控制領域廣泛采用的一種 總線標準。它是同步通信的一種特殊形式，具有接口線少，控制方式簡單，器 件封裝形式小，通信速率較高等優(yōu)點9。其有如下幾個突出特點： 只要求兩條總線線路：一條串行數(shù)據(jù)線 SDA，一條串行時鐘線 SCL； 每個連接到總線的器件都可以通過唯一的地址和一直存在的簡單的主機 /從機關系軟件設定地址，主機可以作為主機發(fā)送器或主機接收器； 它是一個真正的多主機總線，如果兩個或更多主機同時初始化，數(shù)據(jù)傳 輸可以通過沖突檢測和仲裁防止數(shù)據(jù)被破壞； 串行的 8 位雙向數(shù)據(jù)傳輸位速率在標準模式下可達 100kbit/s，快速模式 下可達 400kbit/s，高速模式下可達 3.4Mbit/s； 連接到相同總線的 I2C 數(shù)量只受到總線的最大電容 400pF 限制。 4.3.2 傳感器驅動程序的設計 傳感器驅動程序的設計分為以下幾部分：STM32 內部模塊的初始化，傳感 器接口模塊的初始化。其具體有以下幾個步驟： （1）STM32 系統(tǒng)時鐘的初始化。 （2）STM32 對應傳感器模塊的 GPIO，I2C 模塊的初始化。 （3）傳感器內部寄存器的初始化，這個初始化之后就可以讀取傳感器的數(shù) 據(jù)了。 （4）周期性獲取傳感器的數(shù)據(jù)。整體上傳感器驅動的原理流程框圖設計如 圖 11 所示。 智能三維空間姿態(tài)解算系統(tǒng)設計與實現(xiàn) 圖 11 傳感器驅動原理圖 4.4 串口通信驅動程序設計 4.4