了得網工業技術_四旋翼無人飛行器設計

編輯推薦

本書繫統論述了四旋翼飛行器的基本原理，基本控制方法，本書從四旋翼飛行器的遙控器設計、四旋翼飛行器核心板設計以及陀螺儀、氣壓計等相關電路的設計和原理講解，從零開始一點一點實現四旋翼飛行器的設計，同時還給出了上位機的軟件代碼，項目都提供了全部源代碼，可直接拿來二次開發，用於課程設計或科研項目。本書配套提供了全部設計源代碼、設計圖紙及演示視頻，方便讀者學習使用，提供了學習交流QQ群（185156135），歡迎讀者參與進來，和廣大的STM32愛好者共同學習，及時解決設計中遇到的問題。

內容簡介

四軸飛行器是一種無人飛行器，也是一種智能機器人，“四軸”指飛行器的動力由4個旋翼式的飛行引擎提供。人們對於四軸飛行器的研究從軍用到民用、商用領域都有涉及。近幾十年來，隨著現代控制理論與電子控制技術的發展，運用現代控制技術，使用電機代替油動力引擎進行四軸飛行器控制研究。本書利用主流控制器STM32繫列微處理器平臺，從設計的方案論證、器件選型、代碼調試的全過程對四軸飛行器設計透徹細致地講解，讀者可以根據書中給出的電路和代碼自行設計。本書可作為電子、通信及控制等相關專業的參考書，也可以作為相關技術人員的技術參考書。

作者簡介

馮新宇：從事嵌入式繫統項目開發和相關教學工作。曾主持或參與嵌入式相關課題與項目開發20餘項；近10年來一直作為指導教師參與全國電子設計大賽、飛思卡爾電子設計大賽、黑龍江省電子設計大賽等工作，所指導的學生多次獲得各種賽事重大獎勵。個人代表性著作有《ADS2009射頻電路設計與仿真》《ARM 9嵌入式開發基礎與實例進階》與《ARM Cortex-M3體繫結構與編程》）。

目錄

第1章簡介

1.1四旋翼飛行器發展歷史

1.2四旋翼飛行器的研究現狀

1.3四旋翼飛行器的主要應用

第2章四旋翼飛行器的控制原理

2.1四旋翼飛行器的結構

第1章簡介

1.1四旋翼飛行器發展歷史

1.2四旋翼飛行器的研究現狀

1.3四旋翼飛行器的主要應用

第2章四旋翼飛行器的控制原理

2.1四旋翼飛行器的結構

2.2四旋翼飛行器的運動控制方法

2.3四旋翼飛行器各部分的工作原理

2.3.1飛行姿態與升力關繫

2.3.2飛行姿態的測量

2.3.3加速度傳感器工作原理及角度測量

2.3.4陀螺儀傳感器工作原理及角度測量

2.3.5磁力計傳感器工作原理及測量方法

2.4姿態解算方法

2.4.1互補濾波算法

2.4.2卡爾曼濾波算法

2.4.3DMP姿態數據獲取

2.5PID控制算法

2.5.1PID概述

2.5.2四軸飛行器PID控制器設計

第3章硬件設計

3.1協議預備知識

3.1.1SPI總線

3.1.2I2C總線

3.1.3USART總線

3.2總體設計

3.2.1遙控器電路基本框架

3.2.2飛行器主控電路基本框架

3.3飛行器主控電路小繫統設計

3.3.1基本原理

3.3.2硬件電路設計

3.4姿態傳感器模塊

3.4.1基本原理

3.4.2硬件電路設計

3.5無線通信模塊

3.5.1基本原理

3.5.2硬件電路設計

3.6定高模塊

3.6.1超聲波定高模塊

3.6.2氣壓計定高模塊

3.7電機及驅動模塊

3.7.1基本原理

3.7.2硬件電路設計

3.8遙控器模塊設計

3.8.1基本原理

3.8.2硬件電路設計

3.9電源模塊選擇

3.10四軸飛行器的組裝

3.10.1電機、漿、電池、機型的相互關繫

3.10.2機架的組裝

第4章軟件設計

4.1軟件預備知識

4.1.1剛體的空間角位置描述

4.1.2用歐拉角描述定點轉動剛體的角位置

4.1.3控制與濾波算法

4.2主控程序初始化設置及說明

4.2.1SPI的I/O口初始化實現

4.2.2IIC的I/O口初始化實現

4.2.3定時器初始化實現

4.2.4電子調速器初始化實現

4.3姿態傳感器軟件設計

4.3.1軟件設計基本思路

4.3.2DMP

4.3.3代碼實現及解析

4.4氣壓計軟件設計

4.4.1軟件設計基本思路

4.4.2代碼實現及解析

4.4.3自主高度控制的實現

4.5遙控器軟件設計

4.5.1軟件設計基本思路

4.5.2無線模塊代碼實現及解析

4.5.3搖杆代碼實現及解析

4.6攝像頭軟件設計

4.6.1軟件設計基本思路

4.6.2攝像頭的數據讀取

4.6.3攝像頭的數據處理

4.7上位機設計

4.7.1幀頭檢測模塊

4.7.23D模型路徑模塊

4.7.33D模型屬性設置模塊

4.7.4陀螺儀3D數據顯示模塊

第5章調試、問題解析及改進方向隨想

附錄ASTM32F4小繫統電路圖

附錄B遙控器電路

附錄C飛控板連接電路

參考文獻

前言

無人直升機，即旋轉翼無人機。從20世紀50年代出現至今，它的發展較為緩慢，但是，隨著與其相關技術的發展和應用，無人機飛行器領域迎來了革命性的巨變，其中無人直升機的研究和發展也迎來了一個全新的時代。無人直升機的功能越來越全面，體積越來越小，在軍事領域和民用領域共同發展。本書的寫作初衷是2015年的全國電子設計大賽和學生選擇了飛控類的題目，把我們參加比賽制作的過程做了一個記錄。比賽後，經過增加一些功能，PCB的全部設計制作，四軸飛行器飛控相對地穩定。結合實驗室的一些項目，把上位機等相關的內容添加進來，鬥膽把自己的一些心得寫成一本書和大家分享。本書是按照無人機制作順序來寫的，更像是一本內容多一些的論文。個人覺得本書與其他眾多書籍的不同是，上位機軟件設計和操作手柄設計沒有使用現有的成品。這麼做是為了達到實踐和鍛煉的目的，特別是針對電子類相關專業的本科生。如果學生能照著做一個，調試出來，就能學到很多知識。例如，利用LabVIEW學習上位機軟件設計，對於非計算機專業的學生還是挺實用的。本書中核心設計的內容應該是姿態數據的獲取和處理，采用了DMP實現解算。稍後筆者編寫的代碼會發布在QQ群內和廣大的程序愛好者一起研究和完善。互聯網有很多優秀開源的代碼，讀者隻要認真地熟讀一個完整代碼，其他的問題就會迎刃而解。入門的過程較難，特別是對於大學二年級與三年級的學生，需要自學的東西較多。本書完成得匆忙，很多東西加工得不是很細致，留個QQ群號，方便讀者交流學習，QQ群： 185156135。本書的很多素材、資料都是來源於一些知名網絡論壇和優秀的參考書，這裡一並感謝！感謝黑龍江科技學院電子技術實驗室支持我的同學和同事。由於時間倉促，書中難免存在不足之處，歡迎讀者批評指正。
編者2017年4月

媒體評論

評論

在線試讀

第1章簡介
1.1四旋翼飛行器發展歷史無人機是一種通過無線電遙控設備或自備程序算法自行控制的不載人飛行器。早在20世紀初，無人機就在多次局部戰爭中施展其無與倫比的優越性，並且為美國、以色列等國在戰爭中取得勝利起了重要作用。近三十年的時間裡，隨著微電子技術、通信技術、材料及推進繫統等技術的迅猛發展，無人飛行器不僅外形上更加獨特新穎，而且性能和功能上更加先進。從一開始通過地面搖杆控制飛行器，到用人眼控制飛行器，近些年又出現了用人腦來控制飛行器。由於無人機在作戰中的優異表現，並且具有先天零傷亡的優勢，各國在軍事研究上都將其置於優先發展的地位，國際上也掀起了研究的熱潮，其中以美國和以色列位居榜首。在無人機迅猛發展的同時，能夠垂直起降的無人機備受關注，其中發展快、研究多的是四旋翼飛行器。它具有其他固定翼無人機無可匹敵的優越性，不僅能夠執行各種非殺傷性任務，同時又能執行如偵察、目標截獲、監視、通信中斷等各種具有軟硬殺傷性的任務。其結構與普通的單旋翼無人機設計不同，飛行器隻能通過改變兩對螺旋槳的轉速來實現各種姿態的變化，通過四個螺旋槳的升力來實現各種飛行軌跡的控制。與傳統的無人機比較，飛行器具有如下幾點明顯的優勢：  體積小、重量輕，並且可以靈活地垂直起降；  易於控制，具有很好的機動性，並且能夠快速地進行姿態控制；  結構簡單、造價低廉，並且適用於一些比較危險的場合。四旋翼飛行器具有相互交叉的兩對旋翼，通過控制旋翼的轉向和轉速，可抵消每對螺旋槳產生的反槳扭矩。此外還可以通過調節兩對旋翼的轉速改變其扭矩大小和升力，實現對飛行器姿態的控制，而不需要調節繁雜的機械裝置，從而減輕了飛行器重量、減少了飛行器的體積、提高了其負載能力。20世紀初就有學者對四旋翼飛行器進行了研究。直到近幾年來，隨著傳感器、處理器以及能源供給等相關技術的發展，同時又出現了新型的飛行控制理論，為解決四旋翼飛行器的各種疑難問題提供了極大的幫助。早期的飛行器如圖11和圖12所示。

圖11Louis Breguet(1907)

第1章簡介
1.1四旋翼飛行器發展歷史無人機是一種通過無線電遙控設備或自備程序算法自行控制的不載人飛行器。早在20世紀初，無人機就在多次局部戰爭中施展其無與倫比的優越性，並且為美國、以色列等國在戰爭中取得勝利起了重要作用。近三十年的時間裡，隨著微電子技術、通信技術、材料及推進繫統等技術的迅猛發展，無人飛行器不僅外形上更加獨特新穎，而且性能和功能上更加先進。從一開始通過地面搖杆控制飛行器，到用人眼控制飛行器，近些年又出現了用人腦來控制飛行器。由於無人機在作戰中的優異表現，並且具有先天零傷亡的優勢，各國在軍事研究上都將其置於優先發展的地位，國際上也掀起了研究的熱潮，其中以美國和以色列位居榜首。在無人機迅猛發展的同時，能夠垂直起降的無人機備受關注，其中發展快、研究多的是四旋翼飛行器。它具有其他固定翼無人機無可匹敵的優越性，不僅能夠執行各種非殺傷性任務，同時又能執行如偵察、目標截獲、監視、通信中斷等各種具有軟硬殺傷性的任務。其結構與普通的單旋翼無人機設計不同，飛行器隻能通過改變兩對螺旋槳的轉速來實現各種姿態的變化，通過四個螺旋槳的升力來實現各種飛行軌跡的控制。與傳統的無人機比較，飛行器具有如下幾點明顯的優勢：  體積小、重量輕，並且可以靈活地垂直起降；  易於控制，具有很好的機動性，並且能夠快速地進行姿態控制；  結構簡單、造價低廉，並且適用於一些比較危險的場合。四旋翼飛行器具有相互交叉的兩對旋翼，通過控制旋翼的轉向和轉速，可抵消每對螺旋槳產生的反槳扭矩。此外還可以通過調節兩對旋翼的轉速改變其扭矩大小和升力，實現對飛行器姿態的控制，而不需要調節繁雜的機械裝置，從而減輕了飛行器重量、減少了飛行器的體積、提高了其負載能力。20世紀初就有學者對四旋翼飛行器進行了研究。直到近幾年來，隨著傳感器、處理器以及能源供給等相關技術的發展，同時又出現了新型的飛行控制理論，為解決四旋翼飛行器的各種疑難問題提供了極大的幫助。早期的飛行器如圖11和圖12所示。

圖11Louis Breguet(1907)

圖12George de Bothezat (1922.12)

CurtissWright VZ7是垂直起降飛機，是CurtissWright公司專為美國陸軍設計的，如圖13所示，VZ7通過改變4個螺旋槳的推力實現控制。

圖13CurtissWright VZ7（1958)

1.2四旋翼飛行器的研究現狀在過去的幾十年裡，小型無人機已經應用於許多領域。四旋翼飛行器研究的主要熱點是如何提高機動性和懸停能力。四軸飛行器的四轉子設計使得飛行器設計相對簡單、可靠和易操作。目前四軸飛行器的研究熱點在多機通信、環境探索及可操作性，如果把這些特性融合在一起，它可以完成目前其他車輛所不可能完成的很多任務。目前正在研發的有：  貝爾波音四傾轉旋翼機的概念，提出對C130大型軍用運輸機采用固定直升機概念和傾斜轉子概念進一步結合；  Aermatica Spa Anteos是個旋轉翼遙控飛機，獲得官方許可的民用空域飛行，由意大利民航局設計制造，並首次工作在非隔離空域；  Aeroquad和Arducopter基於Arduino的四旋翼飛行器DIY（Do It Yourself,自己動手做）建設開源硬件和軟件項目；  Parrot AR.Drone是一款帶有攝像機無線控制的四旋翼飛行器，可以通過智能電話和平板設備進行操控，如圖14所示；  Nixie是一款帶小相機配備的飛行器，可以作為一種穿戴設備，如圖15所示。

圖14Parrot AR.Drone飛行器

圖15Nixie飛行器

1.3四旋翼飛行器的主要應用因為四軸飛行器體積小,重量輕，攜帶方便，能輕易進入人不易進入的各種惡劣環境。該飛行器常用來制作模型，也用來執行航拍電影取景、實時監控、地形勘探等飛行任務。目前主要應用在以下幾個方面。 研發平臺：四旋翼飛行器是大學研究人員測試和評估在多個不同領域新思路的一個有用工具，包括飛行控制、導航、實時繫統和機器人。近年來，許多高校已經讓四旋翼飛行器從事越來越復雜的空中演習。成群的四軸飛行器可以懸停在空中，自主執行復雜的飛行程序，如翻轉。 軍事和執法：四旋翼無人機用於軍事偵察和執法機構偵察，以及在城市環境中的搜索和救援任務。由加拿大公司Aeryon Labs研發的小型無人機Scout，可以靜靜地在原地徘徊，並用相機觀察地面上的人和物。用戶在超過視線範圍內3km操作Scout，在距地面100~150m的飛行速度達50km/h，續航25min。它允許飛行在惡劣天氣條件下，所有通信都采用數字加密，從而降低了劫持和視頻截取的風險，如圖16所示。

圖16Scout飛行器

圖17大疆無人機

 商業應用：商業應用成功的是大疆公司，它約占世界民用無人機市場份額的70%，以及全球小型無人機市場份額的50%。隨著大疆公司把目光從無人機本身的平臺，逐步移向建立在飛行器平臺上的增值應用，未來的市場空間對於這家新興公司來說，仍然是個未知數。在美國使用四軸飛行器的領域是航空影像。四旋翼無人機為航拍節約了大量成本。大疆無人機如圖17所示。

第2章四旋翼飛行器的控制原理
2.1四旋翼飛行器的結構如圖21所示，四旋翼飛行器一般是由4個可以獨立控制轉速的外轉子直流無刷電機驅動的螺旋槳提供全部動力的飛行運動裝置，4個固定迎角的螺旋槳分別安裝在兩個十字相交的剛性碳素杆兩端。

圖21四旋翼飛行器結構示意圖

對於絕大多數四旋翼飛行器來講，飛行器的結構關於兩根碳素杆的交點對稱，並且兩個相鄰的螺旋槳旋轉方向相反。正是由於這種獨特結構，使四旋翼飛行器抵消了飛機的陀螺效應，更方便建模。與傳統的單旋翼飛行器，特別是直升機相比，四旋翼飛行器沒有尾槳，這使之擁有更高的能量利用率。另外，四旋翼飛行器四個旋翼的轉速比直升機的螺旋槳轉速明顯低很多。因此，它可以近距離地靠近目標物體，適合室內飛行和近地面飛行。
2.2四旋翼飛行器的運動控制方法四旋翼飛行器繫統共有4個輸入，分別為一個上升力和三個方向的轉矩。但是飛行器在空間中卻有6個自由度的輸出坐標，可以進行三個坐標軸方向的平動運動和圍繞三個坐標軸方向的轉動運動。如果沿著任意給定方向的獨立運動，飛行器沒有給予足夠多的運動驅動，那麼該飛行器就是欠驅動的。可見，四旋翼飛行器是欠驅動和動力不穩定的繫統。因此，針對該繫統實現全部的運動控制目標，必然存在旋轉力矩與平移繫統的耦合。傳統的縱列式直升機為了平衡反扭矩，需借助尾槳來實現。四旋翼飛行器采用了4個旋翼的機械結構，4個電機作為飛行的直接動力源，通過改變4個螺旋槳的轉速，進而改變螺旋槳產生的升力來控制飛行器姿態和運動。這種設計理念使飛行器結構和動力學特性得到了很大簡化。四旋翼的前槳1和後槳3逆時針旋轉，左右2、4兩槳順時針旋轉，這種反向對稱結構代替了傳統直升機尾旋翼。在飛行過程中，如圖22所示，改變4個旋翼螺旋槳的轉速，可使四旋翼產生各種飛行姿態，也可使四旋翼飛行器向預定方向運動，完成任務。

圖22四旋翼飛行器飛行運動原理

根據四旋翼飛行器的運動方式的特點將其飛行控制劃分為4種基本的飛行控制方式：  垂直飛行控制；  橫滾控制；  俯仰控制；  偏航控制。下面分別對以上4種飛行控制方式進行闡述。垂直飛行控制主要是控制飛機的爬升、下降和懸停。如圖23所示，弧線箭頭方向表示螺旋槳旋轉的方向。當四旋翼處於水平位置時，在垂直方向上，慣性坐標繫同機體坐標繫重合。同時增加或減小4個旋翼的螺旋槳轉速，4個旋翼產生的升力使得機體上升或下降，從而實現爬升和下降。懸停時，保持4個旋翼的螺旋槳轉速相等，並且保證產生的合推力與重力相平衡，使四旋翼在某一高度處於相對靜止狀態，各姿態角為零。垂直飛行控制的關鍵是要穩定4個旋翼的螺旋槳轉速，使其變化一致。

橫滾控制，如圖24所示。通過增加左邊旋翼螺旋槳轉速，使拉力增大，相應減小右邊旋翼螺旋槳轉速，使拉力減小，同時保持其他兩個旋翼螺旋槳轉速不變。這樣由於存在拉力差，機身會產生側向傾斜，從而使旋翼拉力產生水平分量，使機體向右運動。當Δ2=Δ4時可控制四旋翼飛行器作側向平飛運動。

圖23垂直飛行控制示意圖

圖24橫滾運動控制示意圖

俯仰控制，如圖25所示，與橫滾控制相似，在保持左右兩個旋翼螺旋槳轉速不變的情況下，減少前面旋翼螺旋槳的轉速，並相應增加後面旋翼螺旋槳的轉速，使得前後兩個旋翼存在拉力差，從而引起機身的前後傾斜，使旋翼拉力產生與橫滾控制中水平方向正交的水平分量，使機體向前運動。類似地，當Δ1=Δ3時可控制四旋翼飛行器作縱向平飛運動。

偏航控制，如圖26所示。四旋翼飛行器為了克服反扭矩影響，4個旋翼螺旋槳中的兩個順時針轉，兩個逆時針轉，且對角線上的兩個旋翼螺旋槳轉動方向相同。

圖25俯仰運動控制示意圖

圖26偏航控制示意圖

反扭矩的大小與旋翼螺旋槳轉速有關，當4個旋翼螺旋槳轉速不完全相同時，不平衡的反扭矩會引起機體轉動。根據上面的原理，可以設計四旋翼飛行器的偏航控制，即同時提升一對同方向旋轉的旋翼螺旋槳轉速並降低另一對相反方向旋轉的旋翼螺旋槳轉速，保證轉速增加的旋翼螺旋槳轉動方向與四旋翼飛行器機身的轉動方向相反。

2.3四旋翼飛行器各部分的工作原理2.3.1飛行姿態與升力關繫

為便於進行四軸飛行器運動分析，建立剛體三軸坐標繫，將四軸飛行器置於剛體坐標繫其中如圖27所示，飛行器運動過程中飛行姿態與各螺旋槳所產生升力之間的關繫借助此坐標軸進行分析。
1. 飛行器繞y軸旋轉α角度與升力之間的關繫如圖28所示，飛行器與y軸之間夾角α。主要通過左右螺旋槳產生升力差控制，其控制關繫為

∑M=Ixα¨（21）

式中： M——力矩； Ix——轉動慣量；α¨——飛行器與y軸夾角二階導數，即角加速度。
lxF右-F左=Ixα¨（22）
式中： F右——右側螺旋槳旋轉產生升力； F左——左側螺旋槳旋轉產生升力;lx——螺旋槳與飛行器中心軸距。
α¨=lx（F右-F左）Ix（23）

圖27飛行器坐標軸建立

圖28飛行器繞y軸的角度α與F左、F右的關繫

2. 飛行器繞x軸旋轉β角度與升力之間的關繫如圖29所示，飛行器與x軸夾角β主要通過前後兩個螺旋槳所產生升力差值進行控制，其控制關繫為

∑M=Iyβ¨(24)
ly(F前-F後)=Iyβ¨(25)
β¨=ly（F前-F後）Iy(26)

圖29飛行器繞x軸的角度β與F前、F後的關繫

3. 飛行器繞z軸旋轉γ角度與升力之間的關繫如圖210所示，飛行器繞z軸旋轉γ角度，使螺旋槳產生扭矩及升力與旋轉角度之間的關繫為
∑M=Izγ¨(27)
M右 M左-M前-M後=Izγ¨(28)

圖210飛行器繞z軸旋轉角度γ與M前、M後、M左、M右之間的關繫

γ¨=M右 M左-M前-M後Iz(29)
由於螺旋槳所產生的升力和力矩之間存在關繫M=cF，所以上式可以表示為γ與升力之間的關繫：
γ¨=c右F右 c左F左-c前F前-c後F後Iz(210)
假定各個螺旋槳性能參數一致，則可以認為c前=c後=c左=c右=c，上式可以簡化為
γ¨=c(F左 F右-F前-F後)Iz(211)

4. 飛行器飛行速度與螺旋槳升力之間的關繫根據牛頓第二定律：
∑F=z¨ml(212)
F前 F後 F左 F右-mtg=z¨ml(213)
z¨=F前 F後 F左 F右-mtgml(214)
2.3.2飛行姿態的測量飛行姿態是一個真實飛行物體與參考坐標繫之間的角度關繫。如2.3.1節中分析使用到的α、β、γ角，這三個角度也稱為歐拉角，對應Pitch、Yaw、Roll。常用姿態測量傳感器有加速度傳感器、角速度傳感器、磁力傳感器、氣壓傳感器、超聲波及GPS（Global Positioning System,全球定位繫統）等。若需要獲取比較精確姿態定位數據，則需要融合計算上述多個傳感器測量數據。對於嵌入式平臺應用，多種傳感器數據融合計算對微處理器的運算能力要求較高。選擇與實際開發平臺相符合的姿態傳感器尤為重要，本設計采用加速度與角速度測量飛行器姿態，兩者測量數據互補融合計算姿態角，可以滿足飛行姿態穩定性要求。2.3.3加速度傳感器工作原理及角度測量加速度傳感器是測量由物體重力加速度引起的加速度量。物理靜止或運動過程中，受重力作用，會產生物體相對於三個坐標軸方向上的重力分量，通過對重力分量進行量化，運用三角函數可計算出物體相對於三個坐標軸的傾角。圖211所示為加速度傳感器測量時重力慣性矢量的三軸分量Rx、Ry、Rz。利用三角函數即可求出重力加速度與三個坐標軸夾角α、β、γ。

α=arccosRyR(215)
β=arccosRxR(216)
γ=arccosRzR(217)
R=R2x R2y R2z(218)
式中： α——重力矢量與y軸夾角； β——重力矢量與x軸夾角； γ——重力矢量與z軸夾角； Rx——加速度計測量重力加速度x軸分量； Ry——加速度計測量重力加速度y軸分量； Rz——加速度計測量重力加速度z軸分量； R——重力矢量。

圖211加速度測量

2.3.4陀螺儀傳感器工作原理及角度測量角速度傳感器（陀螺儀）用來測量一段時間內角度變化速率。對兩次測量時間差值進行積分可得到角度增量值。增量值可正可負，正值表示向角度增大方向旋轉，負值表示向原角度減小反方向旋轉，積分後與測量前初始角度求和可計算出當前角度。
θ=θ0 ∫t0ωdt(219)
式中： θ——旋轉角度值； θ0——上一次旋轉角度值； ω——角速度測量值； t——測量間隔時間。實際使用中，需要得到更加精確的角速度值，可以使角速度測量值ω取前一次測量值與後一次測量值進行求平均，且兩次測量時間應盡量短。但是角速度測量值在多次積分之後會引入很大的誤差，誤差一部分為積分時間間隔誤差; 另一部分即陀螺儀本身存在一定誤差（漂移）。為盡量減小誤差可采取兩個措施，減小測量時間間隔和一段時間間隔內重新校準陀螺儀。
2.3.5磁力計傳感器工作原理及測量方法地球的磁場就像一個偶極子，地球的南北極為這個偶極子的兩極。在地球極地處，地球磁場的磁場強度為0.6高斯，赤道處的磁場強度為0.3高斯。但是，偶極子隻是對地球磁場的簡單比喻。對於地球磁場來說，國際參考磁場是一個更加準確的模型。此模型中包含一繫列球諧條款，根據球諧條款對應一個繫數，利用這個繫數可以計算出當地的磁場強度，由於地球磁場隨著時間發生漂移，所以這些繫數每5年被國際地磁與高空物理學協會更新一次。一些情況下地球磁場會發生變化，日常由於太陽輻射產生的電離層會導致地球磁場發生 0.0001~0.001高斯的變化。每個月發生的幾次太陽耀斑磁暴可產生高達0.01高斯強度的磁場變化。這些因素在一定程度上使地球磁場的強度和方向發生變化。目前用來測量地球磁場的磁力計主要有三種：磁通門式：磁通門式磁力計在1928年問世，一直沿用至今。磁通門式磁力計基於磁飽和法，利用被測磁場中磁芯在交變磁場飽和勵磁下其磁感應強度與磁場強度的非線性關繫來測量弱磁場的一種方法。這些設備往往是笨重的，而且不耐用，作為較小的集成傳感器，響應時間慢。霍爾效應式：霍爾效應磁力計的工作原理為通過感測附近的交變磁場而產生輸出電壓的傳感器。這種磁力計的設計簡單，價格低廉，適用於對強磁的測量，但由於靈敏度低，噪聲大而不適用於測量地磁場。磁阻式：利用磁阻效應的傳感器利用電阻組成惠斯登電橋測量磁場。磁阻式傳感器的靈敏度高、體積小、響應時間快。磁力計作為測試磁場密度的傳感器，廣泛應用於科研和工程等領域。在導航領域中，磁力計用於求取載體姿態中航向角的估計，航向角為磁力計用測量信息在水平方向上的分量求得。三軸磁力計在地球磁場坐標繫下的測量值為
hb=［hbxhbyhbz］T
其中，上標b表示了向量h為地球磁場坐標繫下的向量，下標x，y，z，表示三軸磁力計各軸的分量。當磁力計水平放置水平面上時，可以利用向量h在水平方向上的兩個分量航向hx和hy求出磁力計坐標繫x與地球磁場北極的夾角，即航向角推導公式為：
ψ=arctan hbyhbx(220)
單獨地使用磁力計測量航向角時，由於磁力計非水平放置，導致磁力計產生傾斜角誤差，因此磁力計常與加速度計一起組成電子羅盤。當磁力計與慣性傳感器組合使用進行姿態測試時，磁力計用來估算運動目標的航向角，用以校正陀螺儀漂移誤差。當外界磁場發生突變時，磁力計對載體的航向角估算值失真。2.4姿態解算方法2.4.1互補濾波算法

互補濾波器作為一種頻域特性濾波器，常用於融合來自不同傳感器測量得到的數據。一般地，互補濾波器包含至少兩種頻率特性互補的輸入信號。例如，對於陀螺儀和加速度計解算姿態這一雙輸入繫統，兩個輸入量都能分別對姿態角進行解算，其中加速度計輸入量包含高頻噪聲，應通過低通濾波器來濾除；陀螺儀則包含低頻噪聲（積分漂移），應采用高頻濾波器濾除。兩者的頻率特性互補，可用互補濾波思想進行姿態解算，終輸出較準確信號，其工作原理如圖212所示。

圖212互補濾波姿態融合原理

設運用加速度計和陀螺儀分別解算出的飛行器姿態角x的值為
x1=x u1x2=x u2(221)
其中，u1、u2分別為加速度計高頻噪聲和陀螺儀低頻噪聲。取互補濾波器由低通濾波器FL(s)=1fs 1和高通濾波器FH(s)=fsfs 1兩部分構成， f為濾波器常數，有
FL(s) FH(s)=1(222)
則姿態角x的估計值X^可表示為
X^s=FLsX1s FHsX2s=Xs FLU1s FHU2s(223)
式(223)對應差分方程表達式為
X^k=1-fX^k-1 X2k-X2k-1 fX1k(224)
式中，X2k為k時刻陀螺儀角速度積分值； X1k為加速度計解算的角度值。相對於單傳感器方案，互補濾波可以避免加速度計精度和動態性能不足的問題，也能避免陀螺儀的漂移誤差。由式(224)可見互補濾波器的結構簡單、計算量小，其編程思想可以描述為
angle=1-A×angle_last gyro×dt A×acc(225)
式中： angle當前融合而成的姿態角； angle_last為上一次姿態角融合結果值； gyro為當前陀螺儀測量的角速度； dt為積分時間； acc為加速度計解算出的姿態角。濾波器繫數為A，一般取A小於0.1，表示每次解算結果中陀螺儀積分角度占比重較大，加速度計解算角度占比重較小，這也就相當於對陀螺儀信號高通濾波而對加速度計信號低通濾波。由此可以看出，雖然加速度計信號在每次姿態解算中所占權值較小，但隨著運行時間的增加，加速度計信號實際上是在不斷對陀螺儀積分進行緩慢的矯正，從而減小因陀螺儀積分時間增加而引起的漂移誤差。圖213是取互補濾波器繫數A為0.02時對四軸飛行器俯仰角進行姿態解算的結果。

圖213俯仰角互補濾波融合結果

從圖213(a)中可以看出，隻用加速度計進行姿態結算雖然沒有靜態誤差，但其波形是含有一定量的噪聲，尤其是在圖中15~20s時，由於飛行器姿態改變，其外力加速度將使解算角度波動較大。圖213(b)中的陀螺儀直接積分解算結果波形雖然平滑，即使在飛行器姿態改變時，其積分角度也沒有波動，但隨著時間的增加，將出現積分角度的漂移。圖中僅進行了25s姿態解算，陀螺儀漂移誤差就達到了將近2°。因此，進行姿態融合是十分必要的。在圖213(c)中，互補濾波取得了較好的效果，不僅波形平滑，而且陀螺儀漂移誤差也得到有效矯正。互補濾波姿態融合雖然計算簡單，運算量小，也能夠取得較理想的融合效果，但對於參數A選取卻沒有比較好的解決辦法，一般是通過不斷調試和反復修改進行確定。若A過大，則角度收斂較慢，動態性能降低；若A過小，則角度波動較大，濾波效果降低。因此一般還需要運用模糊算法或其他自適應算法對參數值進行實時調整，以滿足繫統對靜態和動態時的性能要求。2.4.2卡爾曼濾波算法卡爾曼濾波算法是由匈牙利數學家魯道夫·卡爾曼於1960年提出的一種遞歸更新濾波算法。該算法為了描述整個計算更新的過程，提供了一組有效的遞歸推算方程組來估計過程的狀態量，其間使估計均方誤差小化。由於卡爾曼濾波器的概率原型解釋及推導過程等更完整的討論相對煩瑣，不是本文研究重點，下面僅簡單介紹離散卡爾曼濾波算法。設離散時間控制繫統狀態變量X∈Rn，可由以下離散隨機差分方程描述：
Xk=AX(k-1) BUk W(k)(226)
觀測反饋量即繫統輸出方程可描述為
Zk=HX(k) V(k)(227)
其中，W(k)和V(k)均為隨機信號，分別表示過程白噪聲和觀測白噪聲且相互獨立，服從正態分布：
pw~N（0，Q）pv~N（0，R）(228)
式中，噪聲協方差繫數R與Q的取值，關繫到終濾波的效果和響應速度。但兩者相互制約，R取值越小，濾波響應和收斂越迅速； Q取值越小，抑制、濾除噪聲的能力越強。卡爾曼濾波的基本公式為(1) 狀態預測方程：由繫統狀態變量k-1時刻的值X(k-1|k-1)和繫統輸入U(k)可以求出k時刻繫統預測值X(k|k-1)為
X(k|k-1)=AXk-1|k-1 BU(k)(229)
(2) 協方差預測方程：根據k-1時刻繫統協方差矩陣Pk-1|k-1預測k時刻繫統協方差矩陣Pk|k-1為
Pk|k-1=APk-1|k-1AT Q(230)
(3) 卡爾曼增益計算方程：根據狀態變量預測值和協方差矩陣預測值計算卡爾曼增益 Kg(k)為
Kg(k)=Pk|k-1HT/(HPk|k-1HT R)(231)
(4) 值更新方程：由狀態變量預測值和繫統測量值計算k時刻狀態變量值X(k|k)為
X(k|k)=Xk|k-1 Kg(k)(Zk-HXk|k-1)(232)
(5) 協方差更新方程：更新k時刻協方差矩陣P(k|k)為
P(k|k)=(1-Kg(k)H)P(k|k-1)(233)
值和協方差更新方程都計算結束後，整個濾波過程進入下一輪循環，重復(1)~(5)步。遞推算法

圖214卡爾曼濾波器工作流程

的本質體現在估計過程，每次隻需根據以前的測量值遞歸計算，就能得到當前時刻的狀態估計。卡爾曼濾波器的工作流程如圖214所示。以四軸飛行器俯仰角狀態變量為例，進行卡爾曼融合姿態解算。令狀態變量X為俯仰角度θ，繫統狀態方程為和測量方程分別為
Xk=θk=1×Xk-1 Ts×Wgyro Ts×wg(k)(234)
Zk=1×Xacc(k) wa(k)(235)
式中，Ts為繫統采樣周期，Wgyro為陀螺儀測量角速度，Xacc為加速度計計算的俯仰角度，wg、wa分別為陀螺儀和加速度計測量的噪聲。取繫統采樣周期Ts=0.005s，繫統噪聲協方差矩陣Q=0.001，測量誤差協方差矩陣R=0.5，濾波器初始條件X(0)=0、P(0)=1，根據卡爾曼濾波基本公式（式(229)~式(233)）設計俯仰角姿態融合算法，得到姿態解算結果如圖215所示。

圖215俯仰角卡爾曼濾波融合結果

在圖215中使用的傳感器數據與之前互補濾波使用的數據相同，可以看出，采用卡爾曼濾波進行姿態融合也可以得到較好的融合效果，其波形平滑度和靜態漂移誤差比互補濾波稍好。在濾波融合算法設計過程中，主要對協方差Q和R的取值進行設計，R取值越小，濾波響應和收斂越迅速； Q取值越小，抑制、濾除噪聲的能力越強。因此，其具體取值也需要反復實際調試進行權衡確定。
2.4.3DMP姿態數據獲取運用互補濾波與卡爾曼濾波思想進行姿態融合的過程歸根結底都是利用加速度計解算出的姿態角去修正陀螺儀積分的漂移誤差。這兩種方法在姿態融合過程中姿態角的表示形式都是歐拉角表示。用歐拉角進行姿態解算在大角度計算時會出現萬向節鎖（角度為90°時加速度計進行姿態解算的反三角函數無解），為了避免該問題，可數來解算姿數姿態解算流程如下。(1) 初數：設當前的坐標繫為機體坐標繫數列向量：
q=［q0q1q2q3］T=［1000］T(236)
(2）獲取角速度、加速度：讀取MPU6050三軸加速度計和三軸陀螺儀的信號並經過IIR低通濾波器濾波濾除振動噪聲，得到重力加速度分量accx、accy、accz和角速度分量ωx、ωy、ωz。(3) 將加速度計測量值accx、accy、accz轉化為三維的單位向量(歸一化)：
ax=accxacc2x acc2y acc2zay=accyacc2x acc2y acc2zaz=acczacc2x acc2y acc2z(237)
(4)數表示三軸的重力分量Vx、Vy、Vz：
Vx=2（q1q3-q0q2）Vy=2（q1q0-q3q2）Vz=q20-q21-q22 q23(238)
式中Vx、Vy、Vz：即重力單位向量在機體坐標繫中的分量。 (5)數所求重力分量與加速度計測量值的誤差值：
ex=ay×Vz-az×Vyey=az×Vx-ax×Vzez=ax×Vy-ay×Vx(239)
在機體坐標繫中，加速度計測量的重力加速度分量為accx、accy、accz；陀螺儀積分後推算得到的重力向量是Vx、Vy、Vz。兩者之間的誤差，即陀螺儀積分誤差。此處，向量間的誤差ex、ey、ez是用向量積(外積、叉乘)表示，該誤差向量仍位於機體坐標繫中。
(6) 利用所得的誤差修正陀螺儀的測量值：
exint=e^xint ki×exω·x=ωx kp×ex exint(240)
eyint=e^yint ki×eyω·y=ωy kp×ey eyint(241)
ezint=e^zint ki×ezω·z=ωz kp×ez ezint(242)
式中，參數ki、kp用以控制加速度計修正陀螺儀誤差的速度。(7) 利用修正後的陀螺儀值ω·x 、ω·y、ω·z數：
q0=q^0 dt2(-q1ω·x-q2ω·y-q3ω·z)q1=q^1 dt2(q0ω·x q2ω·z-q3ω·y)q2=q^2 dt2(q0ω·y-q1ω·z q3ω·x)q3=q^3 dt2(q0ω·z q1ω·y-q2ω·x)(243)
(8) 將得到更新數規範化：
q0=q^0q20 q21 q22 q23q1=q^1q20 q21 q22 q23q2=q^2q20 q21 q22 q23q3=q^3q20 q21 q22 q23(244)
公式（240）~（244）中e^xint,e^yint,e^zint,q^0~q^4表示更新後的值。(9) 得數後即完成了數法姿態融合的運算。數作為下數運算的數，再從步驟(1)開始下一數運算。為了直觀表示飛行器的姿態，可數轉化成為三個歐拉角：
=arctan2(q2q3 q0q1)q20-q21-q22 q23(245)
θ=arcsin(-2(q1q3-q0q2))(246)
ψ=arctan2(q2q1 q0q3)q20 q21 q22 q23(247)
總結以上9個步數算法流程如圖216所示。
數姿態解算的流程可以看出，其主要思想還是利用加速度計對陀螺儀進行修正，其修正的快慢程度由參數kp和ki進行控制。由於使用該方法步驟較為煩瑣，涉及中間變量轉換較多，且計算量較大，占用內存也較大，使用起來很不方便。為解決該問題，運動控制傳感器MPU6050提供了DMP數解算功能，可以直接數數據，從而省略了煩瑣的計算步驟，給設計帶來了極大便利。運動處理傳感器MPU6050除了提供三軸陀螺儀和三軸加速度計傳感器的16位ADC（AnalogtoDigital Converter,模/數轉換器）信號采集功能之外，還集成了數字低通濾波器和數字運動處理器DMP（Digital Motion Processor），可以直接輸出經低通濾波處數姿態解算數數據。數轉換為歐拉角，可以得到準確的俯仰角和橫滾角。配置MPU6050內部DMP功能的流程如圖217所示。

圖2數法姿態解算流程

圖217MPU6050內部DMP功能配置流程

按如圖217所示的流程對DMP功能進行配置後，即可調用庫函數直接數姿態角數據。由於沒有融合磁力計數據，因此偏航角的計算仍然是由陀螺儀積分得到。對偏航角數據的融合，也可采用之前介紹的互補濾波、卡爾曼濾數法，其具體實現過程與用加速度計修正陀螺儀誤差類似，在此不再贅述。對於同一組飛行姿態數據，圖218所示展示了互補濾波姿態融合、卡爾曼濾波姿態融合以及MPU6050內部DMP姿態解算俯仰角波形對比結果。

圖218不同解算方法姿態解算結果對比圖

可以看出，三種姿態解算方法都能取得較好的解算效果，其中DMP內部解算在飛行器靜止時穩態噪聲小，在姿態角動態改變時，互補濾波和卡爾曼濾波姿態解算受加速度計影響更大，而DMP解算則收斂相對較慢。總的來說，DMP解算姿態數據更平穩，動態性能也比較理想。從設計難度上來說，互補濾波和卡爾曼濾波姿態解算都需要根據實際調試反復修改濾波器參數纔能達到較好的解算效果，而DMP解算直接數數據，隻需要將其轉換為歐拉角即可用於姿態控制，大大簡化了四軸飛行器姿態解算難度。綜合考慮，本書設計的四軸飛行器姿態解算方法采用MPU6050內部姿態解算。2.5PID控制算法2.5.1PID概述
PID即比例、積分、微分控制器,作為早實用化的控制器已有近百年歷史，也是現在應用為廣泛的工業控制器,其基本結構如圖219所示。

圖219PID控制器基本結構

如圖219所示，PID控制器由、和組成，其輸入是期望值r(t)和輸出測量值y(t)之間的誤差e(t)。早期的PID控制器通過硬件實現稱為模擬PID，其基本公式為
u(t)=Kpet 1Ti∫t0etdt Tde(t)dt(248)
式中，Kp為控制器比例繫數，Ti為積分繫數，Td為微分繫數。對應傳遞函數為

書摘插畫

商品搜索

商品分类

【醫學】

【各大出版社】