四軸飛行器的結構是由兩個支架交叉形成機架，然后在兩個支架的兩端配備螺旋槳如圖2-1。根據牛頓第三定律中作用力與反作用力的關系，螺旋槳產生向下的力，而對飛行器自身會得到一個向上的升力，從而去克服重力作用。當四個螺旋槳產生的升力大于重力時，飛行器將會脫離地面，根據牛頓第二定律中物體加速度跟質量與受力的關系，四軸飛行器將會得到一個向上的加速度使得飛行器在垂直方向上運動得越來越快，但是當飛行器脫離地面后，當升力與重力相等時，飛行器將會保持之前的上升速度做勻速上升。因此四旋翼飛行器通過改變四個電機的轉速來實現飛行器的姿態和位置控制，然而飛行器由于機械結構的差異和環境影響，若只通過電機得到升力來達到穩定飛行是遠遠不夠的，所以需要加入微型計算機來實時控制各個電機的轉速。
圖2-1 四旋翼飛行器模型
2.2 四軸飛行器的數學模型在不計飛行器的彈性形變和震動的情況下，四旋翼飛行器當作一個具有六自由度剛體，并輸出四個電機的轉速。令飛行器相對于地面坐標系的三個歐拉角分別為滾動角
、俯仰角
、偏航角
，因此地面坐標系與剛體坐標系的旋轉矩陣R（推導見4.3章節）和平移矩陣為T（推導見4.3章節）
（2.1）
（2.2）
假設飛行器三個軸所受的擾動
，并且飛行器為“X”型，根據牛頓第二定律有公式（2.3）
（2.3）
式中
表示飛行器在地面坐標系三個軸上的合力；F表示飛行器四個旋翼的整體升力大小；R表示從飛行器坐標系（剛體坐標系）映射到地面坐標系的旋轉矩陣；
表示F在飛行器坐標系的方向。再根據角動量定理得到最終的系統動力學模型為：
（2.4）
（2.5）
（2.6）
（2.7）
（2.8）
（2.9）
其中，
是四個旋翼的轉速，b為阻力因素；m為飛行器質量；g為重力加速度；l為電機到四旋翼飛行器幾何中心的距離；
為螺旋槳繞軸的轉動慣量；
為各方向上的轉動慣量；
為槳葉整體的速度；d為風阻系數。
而在實際控制中是寫成系統狀態方程
的形式，其中狀態慣量
與控制量輸入
可表示為
（2.10）
（2.11）
四旋翼作為一個四輸入六輸出的欠驅動系統，只需要控制四個電機的轉速，便可以有效的控制飛行器的姿態，需特別注意的是為了抵消電機旋轉產生的力矩，遵循相鄰電機的旋轉方向相反，對角電機旋轉方向相同的原則。
圖2-2垂直運動模型圖 圖2-3前后運動模型圖
圖2-4左右運動模型圖 圖2-5偏航運動模型圖
在上圖里，電機1、3做順時針旋轉，電機2、4做逆時針旋轉，令正方向為飛行器前方。
2.3.1 垂直運動在圖2-2中，同時增加四個電機的轉速，使得飛行器的總體升力增大，當總的升力大于飛行器重力時，就能使四旋翼垂直上升；相反，若四個電機的速度同時減小，四旋翼飛行器就能夠垂直向下運動；因此只有當飛行器的升力剛好抵消重力時才滿足懸停條件。
2.3.2 前后運動 在圖2-3中，電機1、2的轉速下降，電機3、4的轉速增加，使得飛行器產生向前的傾斜，對此姿態進行受力分析如圖2-6，F為四旋翼槳葉產生的合力，為四旋翼的俯仰角，忽略空氣阻力，將F沿著水平方向和豎直方向進行分解后，豎直方向的力為F*cos，為避免垂直運動F*cos=mg；在水平方向的分力為F*sin，因此會飛行器會向水平方向運動。
圖2-6 受力分析圖
2.3.3 左右運動在圖2-4中，電機2、3的轉速下降，電機1、4的轉速增加，使得飛行器的橫滾角發生改變，與圖2-6類似，飛行器的升力在側向會產生一個水平分力，因此飛行器對發生左右運動。
2.3.4 偏航運動在圖2-5中，電機1、3轉速增加，電機2、4轉速減小，使得兩個方向力矩不平衡，讓飛行器自旋，從而引起四旋翼飛行器的頭部方位發生改變，為了使飛行器升力與重力平衡，電機1、3轉速增加的同時需減少另外兩個電機的轉速。
2.4 四軸飛行器的位置控制解析在圖2-3、2-4中表明，可以通過姿態控制來控制飛行器的水平位置變化，然而位置變化量取決于四旋翼飛行器的俯仰角，橫滾角的大小和持續時間。以前進運動發生位置改變為例進行說明，在發生前進運動是飛行器的高度保持不變，初始狀態為懸停（水平方向速度為0），在圖2-6中進行受力分析后便可以的到飛行器的加速度值
，值可以通過陀螺儀測得，再根據已經設定的位移值s求解到水平飛行時間
，在加速飛行過后由于飛行器慣性存在減速過程產生的位移量，在此忽略不計，飛行器過程中受到空氣阻力也忽略不計。因此可以通過設定的位置量來使飛行器到達目標點。
本章節完成了四旋翼飛行器的動力學建模，分別對飛行器的上升運動、左右運動、前后運動、偏航運動姿態進行模型分析，得到針對不同姿態各個電機的轉速變化趨勢，并對位置控制進行受力分析及理論闡述。
方案一：做微型飛行器，其優點是重量輕，耗電少，一般手機電池都能夠驅動，易于調試，危險性低，可以用飛控板搭建，很大程度的減少了開發周期，但是有致命的缺點抗由于重量輕抗干擾能力弱，載重能力低，一般只用于單純的飛行，用途較少。
方案二：做小型飛行器，其優點在于防干擾能力強，具有一定的載重能力，因此用途比較廣泛，但是有重量大、耗電高、電機轉速快、不易調試、危險性高的缺點。
方案確定：飛行器之所以能夠迅速發展，就是因為用途廣泛，涉及的領域多，單純的飛行并沒有多大使用價值，因此決定采用方案二進行設計。
3.2 總體方案設計 
圖3-1 總體結構圖
圖3-2 總體電路圖
電池：為整個飛行器提供電能。
電源轉化模塊：內含無線繼電器開關，控制整個飛行器是否供電，同時有DC-DC模塊，為控制器和各個單元模塊供電。
按鍵輸入模塊：方便在線調試，以及設置位置坐標。
姿態檢測模塊：采用六軸傳感器，實時檢測飛行器的加速度數據、陀螺儀數據，通過濾波算法得到穩定準確的姿態角。
高度檢測模塊：獲得飛行器的距離地面高度數據。
無線接收模塊：配合遙控器使用接收遙控器傳輸過來的油門、俯仰、橫滾、偏航等數據。
數據顯示模塊：配合鍵盤設置目的坐標，同時能夠在調試時顯示重要數據值。
電子調速器：將主控輸出的PWM（脈沖寬度調制）波形轉化成電機實際的輸入電壓，相當于電機驅動器。
3.3 模塊電路1.電池
雖然電池容量大，但是電池重量較重。鋰電池是同樣電池容量中最輕的、起飛效率最高的，綜合考慮到整體飛行器的重量和續航，采用鋰電池型號為2200mah、35C、3S如圖3-3。
2200mah表示電池容量，如果該容量電池以2200ma放電，可以持續放電1小時，若以1000ma放電可持續放電2.2小時；
3S表示電池節數，鋰電池一節標準電壓3.7V，3S就有11.1V；
35C代表電池的放電能力，這是普通鋰電池和動力鋰電池的重要區別，動力鋰電池需要很大的電流放電，這個放電能力就用 C來表示，如果電池是1000mah、10C，那么得出電池的放電電流可以為1000*10mh，若低C電池大電流放電會損壞電池甚至自燃。
圖3-3 電池實物
2.降壓電路
圖3-4 降壓電路圖
降壓電路由兩部分組成，第一部分為無線繼電器開關，控制整個飛行器的供電。第二部分為降壓，將電池電壓降到適合各個模塊正常工作的電壓值（5V）。
（1）為了控制四旋翼飛行器在飛行過程中的安全，防止意外事故發生加入了直流單路遙控開關繼電器如圖3-5。
圖3-5 12V直流單路遙控開關繼電器實物
（2）由于電池輸出的電壓在11-12.6V之間，無法直接給主控芯片和各個模塊電路供電，因此需用DC-DC降壓模塊將電池電壓降到5V，降壓模塊如圖3-6，該模塊輸入電壓3-40V，調節電位器改變輸出電壓，輸出功率高達10W。
圖3-6 LM2596直流降壓模塊實物
3.3.2 按鍵輸入及顯示模塊
圖3-7 按鍵顯示電路圖
1. 由于在整個系統設計中，需要調試PID參數和設置位置信息，因此加入了五個按鍵進行更改值，方便調試。如圖3-8最右方按鍵是增加數值，最左方按鍵時減小數值，上方和下方按鍵用來切換更改的變量，中間是確認鍵。
圖3-8 按鍵模塊實物
2. 顯示部分采用oled12864顯示，其優點重量輕，體積小，分辨率高，較TFT屏幕比較廉價，通信方式為SPI。顯示屏如圖3-9。
圖3-9 oled顯示屏實物
3.3.3 微型計算機選用的MCU為ST公司的stm32f407zgt6如圖3-10。
圖3-10 stm32f407zgt6最小系統
3.3.4 姿態檢測模塊
圖3-11 姿態檢測電路圖
姿態檢測采用常用的mpu-6050傳感器，它整合了陀螺儀、加速度傳感器處理組件，相對于其它多組件方案，消除組合陀螺儀于與加速度時之軸間差的問題，縮小包裝空間，它還含有第二個IIC端口連接其他加速度、磁力傳感器或其他數位運動處理硬件加速引擎，由主要IIC端口以單一數據流的形式，向主控輸出完整的九軸融合演算數據。可輸出中斷，支持姿態識別、搖攝、畫面放大縮小、滾動、快速下降中斷、high-G中斷、零動作感應、觸擊感應等功能。自帶1024字節FIFO，有助于降低系統功耗。InvenSence的運動處理資料庫，可處理運動感測的復雜數據，降低了運動處理運算對操作系統的負荷，并為應用開發提供架構的API。Mpu-6050的角速度全格感測范圍為
，
，
，
/sec(dps)，可準確追蹤快速和慢速動作，并且可以用程序控制加速度全格感測范圍為
，
，
和
，并且它可在不同電壓下工作。模塊實物如圖3-12。
圖3-12 MPU-6050實物
3.3.5 高度檢測模塊
圖3-13 高度檢測電路圖
高度檢測方案主要有以下三種：通過氣壓計檢測，它的檢測高度范圍大，但是精度不高，并且螺旋槳產生的氣流影響氣壓計；通過紅外線檢測，精度高，檢測的高度有限，價格昂貴，并且會對人體造成傷害；最后就是通過超聲波檢測，其方案精度高，檢測距離有限，不易受外界干擾，對人體無害。綜合各個因素采用超聲波測距，超聲波模塊如圖3-14。
圖3-14 超聲波模塊實物
VCC電源，Trig 輸入控制信號端，Echo 接收端，Gnd 共地。
采用IO觸發測距、給trig至少10us的高電平信號，然后模塊會自動發送8個40KHZ的方波、自動檢測是否有信號返回，有信號返回、就通過echo端口輸出一段時間的高電平信號，高電平的持續時間就是超聲波從發射到返回的時間，因此距離=（高電平時間*聲速*0.5）。
3.3.6 無線傳輸模塊無線傳輸方案主要有：通過接收機數傳，傳輸距離比較遠，但需要配套輸出及遙控器，買成套成品價格比較昂貴；通過2.4G的nrf24l01+模塊進行搭建傳輸系統，造價低，但是傳輸距離短；通過WiFi傳輸信號，可用手機應用作為遙控器，只需要在購買WiFi接收模塊，傳輸距離足夠，信號質量好，不易被干擾。綜合各個因素選擇WiFi無線傳輸方案。
1. 采用ATK-WIFI-MODULE是ALIENTEK推出的一款百萬高清wifi航拍攝像頭模塊，此模塊支持在平板/手機端 1280*720 分辨率錄像和拍照，以及圖像實時回傳。模塊使用也較為簡單，只需要在主控端配置一個串口（數據格式：波特率19200，一個起始位，一個終止位，間隔40MS發送一次，一次發送8個字節）就能實現手機與該模塊的數據傳輸。該模塊如圖3-15，該模塊電路圖如圖3-16
圖3-15 ATK-WIFI-MOUDLE 實物
圖3-16 WiFi傳輸電路圖
2.采用nrf24l01+模塊進行搭建遙控器，nrf24l01+是一款工作在2.4-2.5G通用ISM頻段的單片手法芯片，此模塊包括：頻率發射器、模式控制器、功率放大器、晶體放大器、調制器、解調器、輸出功率頻段選取、協議的設置可以通過SPI接口進行設置低的電流功耗，當工作在發射模式下發射功率為6dBm時電流為9mA，接收模式為12.3mA，掉電模式和待機模式下電流消耗更低。支持六通道的數據接收。
圖3-17 nrf24l01+模塊實物
圖3-18 nrf24l01+傳輸電路圖
3.3.7 電機、槳葉、機架及電子調速器首先是機架，機架是一架飛行器的核心硬件，要求硬度高、質量輕、在劇烈震蕩的情況下不易發生形變，因此采用F330四軸機架如圖3-19。
圖3-19 機架實物
其次是槳葉，要求軸心穩定，質量輕不易發生形變，漿的尺寸需與電機的kv值匹配，一般kv值的無刷電機配低尺寸漿葉，但是考慮到機身的總體重量選擇配大尺寸的槳葉8045。需注意的是高kv值配大尺寸電機，不宜讓電機長時間工作在高轉速，容易造成電機負載過大損壞電機。電機如圖3-20，槳葉如圖3-21。
圖3-20 電機實物
圖3-21 槳葉實物
最后電子調速器作為無刷電機的驅動器，只需要輸入PWM波形就可輸出相應的電壓驅動電機，PWM波形的頻率為50HZ，最大油門為一個周期里面2MS高電平時間，最小油門設置為一個周期里面1MS的高電平時間。電子調速器實物如圖3-22。
圖3-22 電子調速器實物
圖3-23 電機驅動電路圖
3.3.8 遙控器1.對于使用WiFi模塊傳輸數據時，采用配套的APP使用。首先在手機端下載相應APP然后打開飛行器電源啟動WiFi模塊（大約在10s左右），接著用手機連接名字為“minifly”的網絡，連接成功后打開APP可以看到攝像頭畫面，打開操作界面時，數據就發送到WiFi模塊，在設置中可以切換不同的操作界面。在調試過程中主要用APP控制飛行器。
2.對于用nrf24l01+模塊需要自己搭建遙控器，因此需要用到單片機來控制無線傳輸模塊，發送數據。同時控制飛行器的數據用兩個雙軸搖桿，搖桿處于不同位置具有不同的電壓值，因此需要將電壓值用單片機進行AD轉化成數字量，再通過無線傳輸模塊將數據發送個飛行器上的接收模塊。雙軸搖桿如圖3-24，遙控器框架如圖3-25，遙控器電路如圖3-26。
圖3-24 雙軸搖桿實物 圖3-25 遙控器硬件框圖
圖3-26 遙控器電路圖
3.4 本章小結本章節闡述了多種方案優缺點，搭建硬件系統框圖和電路原理圖，以及各個單元電路的選取，分析各個單元電路的電氣特征。最后完成整體電路的實物搭建，對電路進行測試，再上電檢測，觀察供電情況，各個模塊都能正常工作。
第4章 四軸飛行器軟件設計MDK5是由keil公司發行的一個包括C編譯器、鏈接器、宏匯編、庫管理的強大集成IC開發環境，支持多種語言編程，編譯后能夠快速生成執行效率較高的匯編代碼，開發環境支持在線調試、仿真，支持斷點調試，能夠實時觀測到程序執行過程中每個量和內存里面數據的變化情況，keil公司雖然沒有發布中文的開發環境，但keil系列的產品一直是電子類行業工作人員的常用軟件[7]。
4.2調試軟件主要使用的程序調試軟件是“ANO_Tech匿名四軸上位機_V2.6”，此軟件需在程序中搭配串口程序才能夠將飛行控制器中的數據通過串口發送到電腦端的軟件上，再在軟件中連接該端口就能使用。通過調試軟件可以觀察飛行器的姿態數據，實時呈現飛行器的歐拉角，同時有數據變化曲線顯示的功能。也可以觀察遙控器的傳輸數據是否正常。也能夠在上位機中設置相應的PID參數，對飛行器進行測試，可以看到電機的實時PWM數據變化情況。
4.3 模塊程序設計如圖4-1，程序開始先對所有程序中要用到的中斷優先級、串口配置、定時器進行初始化，然后在對飛行器重要硬件進行檢測，若硬件有引腳松動、脫落等問題程序不會向后執行，硬件檢查通過后就是電機自檢設置最大油門和最小油門，在通過按鍵顯示程序來設置目標位置的坐標，顯示的初始坐標為當前飛行器的坐標。在通過姿態檢測程序獲得飛行器的歐拉角，高度檢測程序獲得飛行器距離地面的高度，再接收遙控器的控制信號，通過控制信號的不同使飛行器工作在不同的三種狀態：僅姿態控制狀態，定高狀態，位置控制狀態。最后執行完一次又返回到姿態檢測程序中循環執行。
圖4-1 總體程序流程圖
4.3.1 姿態檢測程序設計四元數姿態解算過程：
先從一次平面的旋轉（繞Z軸）如圖4-2，假如坐標系旋轉角度后得到的坐標系，在空間中有一矢量，分別在兩個坐標系的坐標分別為
、
，因此兩坐標關系如下：
圖4-2 坐標系變換關系圖
（4.1）
（4.2）
（4.3）
轉換成矩陣表達形式后為：
（4.4）
同理再依次繞Y軸、X軸旋轉得到歐拉角方向余弦矩陣：
（4.5）
（4.6）
采用與推導歐拉角方向余弦矩陣相同的思路可以得到坐標系平移的平移矩陣T:
（4.7）
由于四元數可表示一個三維坐標系的旋轉坐標對應關系為公式(4.8)[16]
（4.8）
其中
,
表示兩個四元數相乘又稱四元數的格拉斯曼積滿足分配律、結合律；V1=
、V=
;
因為四元數
可逆，需要
=1所以||=1；
= 
= 
= 
；根據四元數的性質有
2
2
2
-1;
= 
、
= -
、
= 
、
= -、
= 
、
= -。
解公式（4.8）得到公式（4.9）
（4.9）
轉換為矩陣表達形式后為公式（4.10）
（4.10）
從公式（4.10）中得到四元數方向余弦矩陣為公式（4.11）
（4.11）
最后有公式（4.6）和公式（4.11）RT=R1得到四元數轉化成歐拉角公式（4.12）
（4.12）
設計思路：利用模塊自帶的數字運動處理器（DMP: Digital motion processor）硬件加速引擎，通過IIC接口輸出六軸姿態融合人演算數據，利用DMP可以使用InvenSence公司的運動處理資料庫，能夠將原始數據直接轉化成四元數[4]。得到四元數后根據公式（4.12）計算出歐拉角。降低了程序開發難度，提高系統的實時性。程序流程圖如圖4-3.
圖4-3 姿態檢測流程圖
初始化IIC接口：在使用MPU-6050時需要把傳感器的數據傳送給控制器，因此需要配置一個通信接口，這里配置的IIC接口用于數據通信。
復位MPU-6050：由電源管理寄存器1（0x6b）控制。
設置滿量程范圍：即是設置角速度傳感器和加速度傳感器的量程范圍，由陀螺儀配置寄存器（0x1b）和加速度傳感器配置寄存器（0x1c）設置。同時設置陀螺儀采樣率，由采樣率分頻寄存器（0x19）控制；設置數字低通濾波器，由寄存器（0x1c）控制。
設置系統時鐘：由電源管理寄存器（0x6b）控制，一般選擇X軸陀螺PLL作為時鐘源，以獲得更高精度的時鐘。
使能傳感器：使能角速度傳感器和加速度傳感器，由電源管理寄存器2（0x6c）控制。
程序調試：把模塊程序編寫完成后，首先是要對程序進行測試，在測試是采用匿名四軸上位機軟件，將mpu-6050采集到的角速度、加速度和歐拉角通過控制器配置的串口發送到軟件，用三角板實際測量傳感器的角度數據去對比傳感器的測量數據，觀測兩者是否吻合，同時在上位機中觀察數據是否出現較大的波動。若出現誤差需在程序中進行角度補償，來提高測量的準確性。
4.3.2 高度檢測程序設計程序流程如圖4-4，使用的超聲波模塊，測量高電平時間利用TIM5定時器的捕獲功能，利用公式
（s:高度，單位m；t:時間，單位s）得到距離[2]。控制信號由TIM3定時器每200ms中斷一次，在中斷中發出15us的高電平給trig引腳，觸發TIM5捕獲中斷計算出高電平的持續時間。
測序測試：將程序于硬件相結合，用oled12864顯示測出的高度值，并將測量值于實際值作對比，觀測數據是否準確，同時觀測數據是否發生較大范圍的波動。若出現問題需更改程序進行校正。
圖4-4 高度檢測流程圖
4.3.3 電機驅動程序設計電機作為四旋翼飛行器的動力來源，無刷電機需要與電子調速器配合才能夠用單片機的定時器輸出不同占空比的PWM來控制電機的轉速，在上電是需要對電子調速器進行初始化的程序如圖4-5。
圖4-5 電子調速器控制流程圖
4.3.4 無線傳輸設計1.WiFi模塊
程序流程如圖4-6，采用無線wifi傳輸方案需要配置一個串口中斷程序，APP中的數據每間隔40ms發送一次，每次發送八個字節。因此通過串口來接收WiFi模塊發送過來的字節數據，再利用中斷進行數據處理，篩選出每次發送的8個字節數據。
Byte[0]：數據頭，固定為0x66；
Byte[1]：副翼：中間值0x80，左邊最大為 0x00,右邊最大為0xff，即在0x00-0x80-0xff線性變化。
Byte[2]：升降舵：中間值0x80，后邊最大為 0x00,前邊最大為0xff，即在0x00-0x80-0xff線性變化。
圖4-6 串口中斷數據處理流程圖
Byte[3]：油門：0x00為最小，0xff為最大。
Byte[4]：方向舵：中間值0x80，左轉最大為 0x00,右轉最大為0xff，即在0x00-0x80-0xff線性變化。
Byte[5]：標志位：含定高使能位，含追蹤目的坐標使能位。
Byte[6]：校驗字節。
Byte[7]：數據尾，固定為0x99。
程序測試：連接好相應的硬件，連接好WiFi，打開APP內的控制界面，用上位機觀測手機發送的初始數據是否準確的發送到主控芯片，再滑動油門、姿態搖桿、觀測對應的數據是否正確，若數據未發送到上位機，檢查硬件來連接；若數據出錯，檢查串口中斷內數據篩選程序。最終調試出正確的程序。
2. nrf24l01+模塊
（1）接收機
圖4-7 nrf24l01+接收程序流程圖
（2）遙控器
程序流程如圖4-8，使用nrf24l01+模塊，不僅需要搭建接收部分，還要自制遙控器，兩者要匹配才能夠使用，nrf24l01+模塊主要是配置spi通信接口來與主控芯片通信。AD轉化主要使用主控芯片內部集成的AD 轉化器進行識別模擬量，并轉化成數字量，在AD采樣時還利用均值濾波來減小數據的誤差。
程序測試：搭建好接收電路和遙控器電路，接上供電電源，將飛行器上串口與電腦端的上位機連接。首先觀測上位機中出現遙控器的初始數據，再撥動雙軸搖桿到最大、最小角度，觀察上位機中的數據是與程序中的目標數值吻合，然后緩慢滑動搖桿，觀察搖桿數據是為線性變化，證明遙控器的數據能夠成功的發送到飛行器的控制器中。
圖4-8 遙控器程序流程圖
4.3.5 PID控制器設計PID控制器流程圖如圖4-9，控制器的輸入是期望的俯仰角、橫滾角、航向角、高度這四個數據，姿態控制器的設計是用串級PID搭建，外環是控制飛行器的角度，傳感器反饋量是角度數據，外環的輸出作為內環的輸入，內環的反饋是姿態傳感器的角速度，內環是控制飛行器角度變化快慢的環節，增加飛行器的穩定性。
對于姿態控制使用串級PID控制相對于單級的姿態PID控制器，有方便調試、響應迅速、輸出穩定的優點。在通過高度PID控制閉合回路，實現在姿態控制的同時穩定飛行器的高度，若不穩定飛行器的飛行高度，飛行器在調節姿態時可能使得電機的PWM值低于飛行器正常懸停的油門值，使得飛行器會出現掉高的現象。PID控制器參數介紹：P參數是使系統對變化量做出快速響應，在三個參數中對系統的調節能力是最強的；I參數是使系統能夠準確調到目標值；D參數是控制系統的波動，控制系統在目標值附近出現波動，增加系統的穩定性。
程序測試：對于此部分模塊程序的測試，需要用到姿態檢測傳感器的數據、高度檢測數據和無線傳輸的數據。在前面的模塊程序都測試通過的情況下才能夠進行測試。由于此部分程序直接影響飛行器的飛行質量，將在調試章節詳細講解測試過程。
圖4-9 PID控制程序流程圖
4.4 本章小結簡要介紹了開發環境和調試軟件的使用，搭建軟件系統的流程圖，以及介紹重要模塊程序的流程，結合硬件測試各個模塊程序的功能，證明各個硬件模塊結合軟件能夠實現相應模塊的功能。最后聯合模塊程序進行系統綜合測試，觀察各個關鍵量的變化情況，完成程序系統設計。
由于姿態PID是采用串級的思路，在調節參數時需要先調節內環參數再調整外環參數。對于質量分布對稱的四旋翼飛行器，理論上它的俯仰角和橫滾角的PID控制參數是相同的，所以在搭建實物時盡量讓四旋翼的重心在模型的幾何中心，這樣有利于調試，若重心與幾何中心偏離很遠，不僅需要多調節一個軸的PID參數，還會導致重心偏向側的電機長時間處于較高速旋轉，容易引起電機損壞。
在調節姿態PID前，需要飛行器脫離地面，才能進行姿態觀測，因此需要估計飛行器的大致起飛油門。
首先將外環所有參數置零，將打舵量作為內環的期望，加上內環P參數，如果P太小，表現為不能修正角速度誤差，不能夠調節傾斜的情況，如果P值過大，便會引起飛行器的震蕩，該震蕩強烈時會引起飛行器無法正常起飛。因此在調節P參數時一般從小到達改變，依次觀察飛行器的擺動情況，合適的P值就是能夠根據打舵量快速做出反應，同時又不會太震蕩。
P參數調到合適后，系統可能不會在目標位置震蕩，此時加上I參數，I參數的作用就是使系統能夠到達目標值附近，大的I值會使得系統在高于目標值的區域發生輕微震蕩，過小的I值，會使得系統響應慢穩定時間長，因此合適的I值使讓系統在我們預想的目標值附近。
對于飛行器姿態在期望角度發生震蕩的情況，就需要系統能夠預測下一時刻的姿態趨勢，來減小震蕩，因此加入D參數，D參數的使用會使得飛行器姿態不會出現擺動，看上去飛行器十分的穩定。
然后就是在已有的內環參數上調節外環參數，外環就是給系統輸入一個期望的姿態角度，在通過外環PID控制，將飛行器穩定在期望的姿態角度。對于外環PID參數的調節，由于飛行器的姿態角并非水平，飛行器會隨著搖桿的方向發生水平移動，因此采用“烤四軸”的方案如圖5-1，對每個軸進行調節，調節過程與內環相同，觀測飛行器在期望角度的飛行情況。在分別調整好三個軸的PID參數后，由于每個軸之間存在魯棒性，每個軸的參數之間會相互影響，因此需要三個軸一起調節如圖5-2。最終調試出適合飛行的所有PID參數。
圖5-1 “烤四軸”調試方案圖
圖5-2 整體調試圖
5.1.2 高度PID調試調節高度PID時需要將油門加大到飛行器能夠脫離地面，打開遙控器的定高按鈕，再來觀察飛行器的高度變化。
首先調節P參數，將I、D參數置零，在程序調試時，在程序內部設置目標高度為50cm。P參數從小到大調整，在調節過程中，若出現飛行器偏離目標高度較遠，飛行器也沒有明顯的上升下降趨勢的情況，此時說明P參數過小調節力度不夠。若出現飛行器高度震蕩范圍越來越大，說明P值過大，引起系統發散，合適大小的P值是當飛行器偏離目標高度較遠時，飛行器能夠迅速反應上升或者下降，同時在一定高度范圍內震蕩。
然后確定I參數，使飛行器能夠調節的目標高度，若飛行器很久才穩定在目標高度，說明I參數值過小，若飛行器出現較小幅度震蕩，說明I參數過大，因此合適的I參數就是使得飛行器能夠調整到目的高度。
最后確定D參數增加高度控制的穩定性，該值從小到大依次調試，合適大小的D值使得飛行器在目標高度十分穩定。
5.1.3 位置控制調試用油門控制飛行器脫離地面，開啟定高模式，再按下位置鍵進行位置控制。由于位置控制是結合姿態控制的持續時間來控制飛行器水平移動的距離，因此在固定的俯仰角、橫滾角下進行位置移動，在調節的過程中需要根據實際距離修改控制程序中的循環次數。
先調整水平方向的位置控制程序，固定飛行器的目標飛行距離，根據實際飛行距離調整相應的循環次數。然后改變目標具體，觀測實際飛行距離是否準確，若有誤差，微調循環次數即可
后調節高度控制，調節步驟與水平位置調節類似。
最后將兩個環節相結合，在程序中飛行器先是水平位置控制，再是高度控制。
5.1.4 調試結果經過對串級姿態PID參數的整定，得到表5-1和表5-2數據對飛行器的姿態控制起到很明顯的作用，能夠有效的控制飛行器的飛行姿態如圖5-3、5-4、5-5、5-6、5-7、5-8、5-9、5-10、5-11、5-12，而對于單級PID控制的高度環節，能夠調節到期望高度，但是在期望高度附近有明顯的波動。位置控制環節受高度環的影響，位置控制上還存在一定誤差，若高度控制環調好，位置控制誤差將能夠有效降低。
表5-1 姿態外環PI參數表
| 俯仰角 | 橫滾角 | 偏航角 |
P值 | 200 | 200 | 1.5 |
I值 | 0.2 | 0.2 | 0.2 |
表5-1 姿態內環PD參數表
| 俯仰角 | 橫滾角 | 偏航角 |
P值 | 0.004 | 0.004 | 0.046 |
D值 | 0.003 | 0.003 | 0.012 |
圖5-3 三軸加速度和三軸陀螺儀數據曲線圖
在圖5-3中，橫坐標是時間軸，單位是
，時間數據被擴大了100倍；縱坐標是加速度和陀螺儀數據，單位分別是
、
。
圖5-4 X軸加速度數據曲線圖
在圖5-4中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是X加速度數據被擴大了16384倍，單位是。
圖5-5 Y軸加速度數據曲線圖
在圖5-5中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是Y加速度數據被擴大了16384倍，單位是。
圖5-6 Z軸加速度數據曲線圖
在圖5-6中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是Z加速度數據被擴大了16384倍，單位是。
圖5-7 X軸陀螺儀數據曲線圖
在圖5-7中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是X陀螺儀數據被擴大了16.4倍，單位是。
圖5-8 Y軸陀螺儀數據曲線圖
在圖5-8中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是Y陀螺儀數據被擴大了16.4倍，單位是。
圖5-9 Z軸陀螺儀數據曲線圖
在圖5-9中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是Z陀螺儀數據被擴大了16.4倍，單位是。
圖5-10 橫滾數據曲線圖
在圖5-10中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是橫滾角度數據被擴大了100倍，單位是度。
圖5-11 俯仰數據曲線圖
在圖5-11中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是俯仰角度數據被擴大了100倍，單位是度。
圖5-12 偏航數據曲線圖
在圖5-12中，橫坐標是時間軸，單位是，時間數據被擴大了100倍；縱坐標是偏航角度數據被擴大了100倍，單位是度。
從圖5-10、5-11、5-12中可以看出飛行器在自穩狀態下，俯仰角和橫滾角都控制很好，曲線都在零度上。然而偏航角度控制曲線離零度漸漸變遠，可見飛行器存在緩慢的自旋，方向都是逆時針。因此在飛行器的姿態控制器中偏航角控制還存在一定的誤差。
5.2 疑難問題及解決方案在實際測試程序的過程中，出現WiFi模塊的初始數據小概率被干擾，以至于在初始化飛行器后，準備起飛前，飛行器會出現失控。
解決方案：飛行器失控是出現在加入無線繼電器開關后出現，但無線繼電器的品那段處于315M，WiFi傳輸頻段在2.4G，從理論上來講兩個頻段的信號是不會發生干擾，因此便在程序中進行優化，在進入起飛前加入WiFi初始數據判定函數，對干擾信號進行攔截，只有當遙控器的初始數據發送到飛行器上才進入起飛就緒階段。這樣就能夠兼并使用無線繼電器模塊，又能確保不會出現數據錯誤。
5.2.2 主控板震動劇烈將主控板安裝在機架上，由于電機的劇烈轉動和機架的機械結構使得電路板上的震動十分劇烈，對MPU-6050姿態傳感器的影響較大。
解決方案：用泡沫膠連接電路板與機架，能夠避免電路板與機架的直接接觸，通過泡沫膠將機架的震動抵消掉一部分，有效的減小MPU-6050上的震動。
5.2.3 部分電機自檢失敗在加入按鍵顯示程序后，某些電機無法自檢，影響正常的轉動。
解決方案：自檢失敗的電機是隨機的、不固定，最初認為是電子調速器的控制線未連接好，但是很少出現四個電機完全自檢通過的情況。因此考慮到程序結構問題，去掉按鍵顯示程序后，四個電機都能夠成功自檢，將按鍵顯示程序放在電機自檢程序后也能避免這個問題，因此更改程序初始化順序就能解決。
5.2.4 飛行器脫離地面發生飄移飛行器在油門的驅動下能夠平穩的脫離地面，但是在穩定油門后飛行器總是朝某一方向移動，無法比較好的穩定在初始位置。
解決方案：檢查飛行器的初始角度俯仰角0度、橫滾角0度、航向角0度，都屬于正常值；檢查遙控器的姿態數據也都是中值數據，所以在程序中的數據并未發現異常。在測試中發現出現飄逸的飛行器可以通過遙控器輸入姿態來矯正，因此對姿態數據進行補償，可以有效的避免飛行器的明顯飄移。
5.2.5 油門控制在飛行器最初測試中，油門滑桿太過靈敏，不利于控制飛行器的起飛油門。
解決方案：這個問題源于自己使用的電機轉速較高，槳葉較大造成，電機在低轉速是飛行器也能夠獲得較大的升力，在實際測試中，滑桿在油門的四分之一處就能飛離地面，因此為降低油門的靈敏度，把[1000-2000]的油門范圍只利用[1000-1500]。便能夠很好的控制起飛、降落。
5.3 實物如圖5-13是自制的遙控器，需要外接USB供電，左邊搖桿是方向控制，右邊搖桿是油門、航向角控制。四旋翼飛行器如圖5-14，將所有的電路布局在一塊洞洞板，只需要接上電池和給電子調速器供電就能使飛行器正常工作，洞洞板上的立桿是用來輔助調試。
圖5-13 遙控器實物圖
圖5-14 飛行器實物圖
5.4 本章小結本章介紹了四旋翼飛行器的實物調試過程及最終調試結果，調試中遇到的問題及解決方案，證明通過本文的設計思路結合調試過程是能夠完成四旋翼飛行器的姿態控制。
由于飛行器是一個比較大的系統，在硬件選取時需要仔細考慮硬件配套的問題，特別是電池、電子調速器、電機、槳葉的型號組合，這部分與控制部分無關，若型號選取不好很容易導致硬件燒毀。然后就是飛行器上控制部分硬件電路，這部分設計需要結合軟件設計來搭建各個模塊的硬件電路，通過軟硬件聯合測試各個模塊的功能。完成整體軟硬件的設計。
在此次飛行器的設計中最重要的是控制器的設計，對于姿態控制的設計采用串級PID的架構，通過實際實物的制作、調試，證明通過這種架構不僅調試方便，而且控制器的輸出比較穩定的姿態，因此采用串級PID的姿態控制策略是可行的。對于高度控制器采用單極PID設計思路，在實際調試過程中單級PID確實不好調試，較串級的PID需要花更多的時間去整定PID參數，而對于輸出也能達到較好的穩定效果。對于位置控制，由于未使用光流元件進行定位，飛行器不能建立自己的位置坐標系，亦無法將位置控制做成閉環，而對于自己設計的方案，只是開環控制飛行器的大概位置，調試過程也相對閉環簡單，但是開環下若只是單純的位置改變，也還是能獲得不錯的控制效果。
在本設計中還有很多地方值得完善，高度PID還不能夠穩定準確的將飛行器高度控制在期望高度，若將高度PID調試好，便能有效的控制飛行器的高度，并且飛行器不會掉高，就能夠實現懸停的功能。在位置控制上也存在很大的誤差，飛行器不能夠感知外界的障礙物，若將位置控制器作成閉環系統，就能夠有效準確的控制飛行器的位置，再結合蔽障等功能就能實現脫離遙控器完全意義上的自主飛行。還有采用的WiFi無線傳輸方案距離小，飛行器的可操作范圍不高，飛行器中也未做自動返航的功能，因此容易發生墜機。用手機APP控制飛行器較用手炳控制飛行器的難度更高，用手機控制時感覺不到搖桿的具體偏離程度，在實際操作中還需要一邊觀測手機一邊觀察飛行器，還不能夠做到很好的操作飛行器。
相信未來的飛行器，無論從材料、做工、結構上都會更加成熟，更加有利于飛行器的飛行控制，抗干擾能力、負重能力越來越強，更能適應在復雜環境下工作。隨著傳感器的精度越高，飛行器在姿態控制和位置控制上的穩定性能越好、誤差也越小。同時隨著電池行業的進步，四旋翼飛行器的續航能力也會更強，實現更久的作業時間。
keil5_code.7z
(434.19 KB, 下載次數: 167)
小型四旋翼飛行姿態及位置控制系統設計與實現.doc
(11.93 MB, 下載次數: 135)
gow1 發表于 2021-4-17 20:53
stm32f407zgt6最小系統如何下手呀
月下丘比特 發表于 2021-4-24 23:39
請問為什么不用電調的5V輸出直接給控制板供電呢
12332323 發表于 2022-10-17 12:29
樓主，您好！目前看了下遙控器的代碼發現NRF24101引腳定義不詳能否說明下，另外遙控器上的PB14是接哪里的NR ...
12332323 發表于 2022-10-17 12:29
樓主，您好！目前看了下遙控器的代碼發現NRF24101引腳定義不詳能否說明下，另外遙控器上的PB14是接哪里的NR ...
| 歡迎光臨 (http://www.zg4o1577.cn/bbs/) | Powered by Discuz! X3.1 |