|
|
本文研究內(nèi)容和方法
本文研究基于MEMS傳感器的姿態(tài)參考系統(tǒng)，通過對姿態(tài)測量傳感器數(shù)據(jù)的分析，設(shè)計出了有效去噪的濾波方法；通過大量的查找資料對姿態(tài)解算算法和數(shù)據(jù)融合算法有了更深的理解，最后應(yīng)用于設(shè)計的飛行控制器上實現(xiàn)了姿態(tài)角的測量。最后通過大量的實驗驗證了它們的準(zhǔn)確性，實驗數(shù)據(jù)和曲線驗證了該姿態(tài)參考系統(tǒng)能夠穩(wěn)定的工作，具有很好地工作性能。
本文一共分為五章，主要內(nèi)容安排如下：
第一章緒論部分主要介紹了該項目的研究背景及意義、四旋翼飛行器在國內(nèi)外的研究現(xiàn)狀和發(fā)展趨勢。
第二章主要介紹了四旋翼飛行器的飛行原理和系統(tǒng)結(jié)構(gòu)框架。
第三章詳細(xì)介紹了四旋翼無人機控制系統(tǒng)的硬件設(shè)計的工作。介紹了MEMS傳感器的原理、特性和型號的選擇和硬件電路圖。飛行器控制芯片選擇STM32，外圍電路包括有姿態(tài)測量系統(tǒng)、電源模塊、無線通訊、串口通訊、電機驅(qū)動、遙控器控制電路、GPS模塊。
第四章說明了姿態(tài)參考系統(tǒng)的核心算法----捷聯(lián)慣性導(dǎo)航算法的研究和實現(xiàn)過程。
第五章針對軟件實現(xiàn)部分進(jìn)行了介紹，給出了編程的軟件流程圖和串級PID控制和定高控制方法。
2 四旋翼飛行器工作原理2.1 四旋翼飛行器的飛行原理
四旋翼飛行器有兩種模式，也就是X字模式（如圖2.1所示）和十字模式（如圖2.2所示）。其實這兩種模式差別不大，到X模式使用廣泛，因此我們采用X字模式。四旋翼飛行器的四個電機對稱分布在各個軸上，并且同一條軸線上電機的旋轉(zhuǎn)方向要保證相同，相鄰的電機旋轉(zhuǎn)方向相反[。如果電機1、 3按照逆時針方向旋轉(zhuǎn)的話，電機2、4就要按照順時針方向旋轉(zhuǎn)，這樣做為了克服反扭矩的影響。我們要通過控制4個電機的轉(zhuǎn)速來完成飛行器俯仰、橫滾、偏航等動作。
圖 2.1 X型四旋翼飛行器模型 圖 2.2 十字型四旋翼飛行器模型
2.2 四旋翼飛行器系統(tǒng)結(jié)構(gòu)
四旋翼無人機采用模塊化設(shè)計，如圖2.3所示。分別由控制模塊、姿態(tài)測量系統(tǒng)、電源供電系統(tǒng)、無線通信模塊、GPS衛(wèi)星定位系統(tǒng)、遙控器控制模塊、電機驅(qū)動模塊、串口通信模塊、地面站系統(tǒng)。
四旋翼飛行器控制器的核心任務(wù)是姿態(tài)的測量，它的作用是為飛行器控制系統(tǒng)提供實時、精確的飛行狀態(tài)測量數(shù)據(jù)。常見的四旋翼飛行器人們大多是采用基于MEMS傳感器來測量飛行器姿態(tài)數(shù)據(jù)[[ii]。但是這些初始的傳感器數(shù)據(jù)并不能直接應(yīng)用于姿態(tài)解算，需要對傳感器數(shù)據(jù)進(jìn)行濾波處理，并且需要對陀螺儀漂移問題進(jìn)行實時的數(shù)據(jù)補償，這樣做能夠有效提高飛行器姿態(tài)測量精度，確保控制系統(tǒng)的姿態(tài)角的準(zhǔn)確性和穩(wěn)定性。
圖 2.3 四旋翼飛行器系統(tǒng)結(jié)構(gòu)框架
四旋翼飛行器的主控板選擇的是意法半導(dǎo)體公司生產(chǎn)的STM32f103zet芯片，STM32系列的單片機是基于Cortex-M3內(nèi)核的處理器，功耗低，處理速度非常快，最高工作頻率可達(dá)72MHz，7通道DMA控制器，支持定時器、ADC、SPI、IIC、USART等外設(shè)，多達(dá)112個I/O口，8個Timer定時器， 5個串行USART接口，3個SPI接口，2個IIC接口[[iii]。
電源模塊采用11.1V鋰電池外部供電，連接電子調(diào)速器為控制器提供5V電壓。控制器上還有3.3V穩(wěn)壓芯片，為控制芯片供電。
遙控器控制模塊，控制器對遙控器數(shù)據(jù)進(jìn)行捕獲處理該部分我們通過對STM32定時器進(jìn)行輸入捕獲配置，捕獲接收機發(fā)出的PWM信號，把該信號轉(zhuǎn)化成控制量在經(jīng)過PID控制把輸出量給四個電機，進(jìn)而控制飛行器的動作。
GPS衛(wèi)星定位導(dǎo)航系統(tǒng)，配合上位機在上位機上輸入一些GPS坐標(biāo)點，控制系統(tǒng)就會自動生成航線，并且能夠從GPS系統(tǒng)中讀取定位數(shù)據(jù)[[iv]，并且與存儲的定位坐標(biāo)做實時的對比，然后修正航線，將定位坐標(biāo)顯示在上位機上，處理并顯示當(dāng)前位置。
3 四旋翼飛行器硬件系統(tǒng)設(shè)計3.1 微慣性組合系統(tǒng)傳感器組成3.1.1 MEMS陀螺儀傳感器
陀螺儀是一種能用來維持方向與角速度（獲取角速度）的裝置，設(shè)計原理是角動量守恒。簡單的說就是一個高速旋轉(zhuǎn)的物體的旋轉(zhuǎn)軸所指的方向在不受外力影響時不會改變。這種用來保持方向而制造出來的裝置就叫陀螺儀[[v]，如圖3.1所示。陀螺儀多用于導(dǎo)航定位系統(tǒng)中，姿態(tài)控制系統(tǒng)中多采用三軸陀螺儀，如圖3.2所示。
圖3.1 陀螺儀 圖 3.2 MEMS三軸陀螺儀
3.1.2 MEMS加速度計傳感器
能將物體加速度的信息轉(zhuǎn)換為電信號的傳感器稱之為加速度傳感器。在姿態(tài)控制系統(tǒng)中，加速度傳感器用來測量與重力方向的夾角。當(dāng)應(yīng)用到實際中時我們就可以理解加速度傳感器輸出的信號是當(dāng)?shù)刈鴺?biāo)系下加速度在導(dǎo)航坐標(biāo)系下投影。加速度計能夠在沒有加速度存在的條件下可以感應(yīng)重力產(chǎn)生的加速度，然而在有加速度存在時，根本無法測量出姿態(tài)角，需要陀螺儀傳感器的數(shù)據(jù)相結(jié)合，才能夠?qū)崿F(xiàn)動態(tài)條件下的姿態(tài)測量[[vi]。
3.1.3 三軸數(shù)字羅盤傳感器
數(shù)字電子羅盤也叫指南針，顧名思義指南針是用來指示方向的。傳統(tǒng)羅盤通過磁針來感應(yīng)地磁場方向，電子羅盤通過磁阻傳感器測量地磁方向信息，再將所測信息轉(zhuǎn)換為信號輸出。數(shù)字電子羅盤的優(yōu)勢在于它克服了只能夠在水平面使用的缺點，這種數(shù)字電子羅盤內(nèi)部有傾斜補償裝置，這個裝置一般是由加速度傳感器來完成，如果在完全動態(tài)的情況下，也需要陀螺儀檢測姿態(tài)角，通過這個角度和磁場方向信息可以補償?shù)玫綔?zhǔn)確的角度信息，而姿態(tài)測量系統(tǒng)中的電子羅盤實際上就是三軸數(shù)字電子羅盤。
3.2 姿態(tài)測量系統(tǒng)傳感器選型
目前市場上出現(xiàn)的一款I(lǐng)nvenSense公司的MPU6050芯片內(nèi)部集成了三軸加速度計和三軸陀螺儀，這樣做不僅消除了焊接電路時易造成加速度計和陀螺儀之間的對準(zhǔn)誤差的問題，而且因為芯片內(nèi)部結(jié)構(gòu)上有數(shù)字可編程低通濾波器。所以在飛行器經(jīng)受較大震動的時候，可以用軟件設(shè)置適當(dāng)頻率的低通濾波器，濾掉高頻震動，這種方法很有效的減少了四旋翼機身震動對姿態(tài)測量的影響。因此MPU6050被廣泛應(yīng)用于姿態(tài)控制系統(tǒng)之中，其特征如下：
（1）三軸角速度傳感器具有±250、±500、±1000 與±2000（°/s）測量范圍[[vii]；三軸加速度量程控制范圍有±2g、±4g、±8g、和±16g。
（2）具備較低功耗：芯片供電電壓VDD為2.5V±5%、3.0V±5%、3.3V±5%[[viii]；陀螺儀工作電流5mA，待機電流為5uA；加速度計工作電流為500uA，在10Hz低功耗模式下僅需40uA的電流[[ix]。
（3）陀螺儀和加速度計都具備16位ADC 同步采樣功能。
（4）IIC接口傳輸頻率可高達(dá)400KHz，內(nèi)建頻率發(fā)生器在所有溫度范圍只有1%頻率變化。
綜合MPU6050特性，我們采用如圖3.3所示的電路讀取三軸加速度和三軸陀螺儀數(shù)據(jù)。
圖 3.3 MPU6050電路圖
HMC5883傳感器是三軸數(shù)字羅盤，它可以用來測量四軸飛行器所處位置的三軸磁場信息，該傳感器內(nèi)置了三軸磁阻模塊和放大采樣電路，直接輸出數(shù)字信號，用來測量航向角并進(jìn)行姿態(tài)解算，HMC5883電路圖如圖3.4所示。HMC5883的特點如下：
1. IIC 數(shù)字量輸出總線接口，設(shè)計使用簡單，尺寸非常小。
2.有較高的測量精度，內(nèi)置12位 A/D轉(zhuǎn)換。
圖 3.4 HMC5883電路圖
4. 擁有自動校準(zhǔn)功能，簡化了應(yīng)用的步驟。
5. 內(nèi)置有自測試電路，量產(chǎn)測試非常方便，不需要增加額外的高昂測試設(shè)備[[x]。
6. 功耗較低，供電電壓只需要1.8V，睡眠模式功耗-2.5uA，測量模式功耗-0.6mA。
3.3 電源系統(tǒng)設(shè)計
為了滿足飛行控制系統(tǒng)的需要，電源系統(tǒng)為飛行控制器和功能模塊提供了12V、5V和3.3V電壓，電路設(shè)計如圖3.5所示。選擇12V的鋰電池作為供電電源，通過穩(wěn)壓芯片將12V電壓降為5V，為GPS模塊、超聲波傳感器、接收機等提供供電電源；然后通過AMS1117_3.3穩(wěn)壓芯片再把5V電壓降為3.3V，為飛行控制主板、姿態(tài)測量傳感器和無線通信模塊提供電能。
圖 3.5 電源系統(tǒng)電路圖
3.4 其它硬件模塊3.4.1 無線通信模塊
該模塊為上位機和控制器建立了聯(lián)系。通過兩塊NRF24L01進(jìn)行數(shù)據(jù)的傳輸，控制器將姿態(tài)和高度等信息傳輸至地面站，地面站將位置信息和導(dǎo)航信息告訴控制器，從而達(dá)到可控的要求。NRF24L01的工作頻段為2.4 ~ 2.5GHz，而且具備自動重發(fā)功能，有6個數(shù)據(jù)傳輸通道，最大傳輸速率高達(dá)2Mbits[[xi]。STM32主控板可以通過SPI接口對NRF24L01的寄存器進(jìn)行配置，無線通信模塊的電路設(shè)計圖，如圖3.6所示。
圖 3.6 NRF24L01電路圖
3.4.2 電機和電機驅(qū)動模塊
我們常用的直流電機按結(jié)構(gòu)及工作原理可以劃分為無刷電機和有刷電機。無刷電機是航模電機的主流選擇，因為它力量大而且耐用。小型的四旋翼飛行器一般選用空心杯電機，屬于有刷電機一類。由于我們設(shè)計的是大型四旋翼飛行器，故采用的是無刷電機。
無刷電機根據(jù)廠商的不同，種類也是非常的多，市面上的主要有朗宇、新西達(dá)、銀燕等品牌。在這些無刷電機中口碑最好的就是朗宇電機，用的人非常的多，因此采用朗宇無刷電機。
四旋翼飛行器所用的無刷電機主要有電機尺寸和電機KV值兩個參數(shù)。電機尺寸指的是電機轉(zhuǎn)子的直徑和高度，電機KV值是指外加1V電壓時對應(yīng)的每分鐘空載轉(zhuǎn)速。根據(jù)參數(shù)設(shè)置最后選擇采用朗宇A(yù)2212、KV1400無刷電機。電機實物圖，如圖3.9所示。
無刷電機的驅(qū)動就是俗稱的電子調(diào)速器也稱電調(diào)，如圖3.10所示。黑色和紅色的是11.1V電源線，紅色接電源正極，黑色接電源負(fù)極。右邊三根線和電機的三根線相連，如果轉(zhuǎn)向反了，只需將其中任意兩根線互換就可以。白紅黑三根線是和電調(diào)相連的，白色為信號線，紅色為控制板提供5V供電電源，黑色的線接控制器的GND。
3.4.3 機架和螺旋槳的選型
對于機架的選擇，差的機架會使姿態(tài)傳感器讀取到的數(shù)據(jù)噪聲較大，加大四旋翼飛行器在飛行過程中的不穩(wěn)定性，因此選用結(jié)構(gòu)穩(wěn)定，質(zhì)量較輕的碳纖機架，這樣很大程度上也提高了飛行器的載重。機架實物圖如圖3.11所示。
螺旋槳是由電機轉(zhuǎn)動帶動為四旋翼飛行器提供升力的。螺旋槳分為正反槳，順時針方向轉(zhuǎn)的電機需要配正槳，逆時針轉(zhuǎn)的電機需要配反槳。槳的選型需要結(jié)合所選的電機來配備，從槳的型號我們可以讀出槳的直徑和角度，例如1047中的前兩位10表示槳的直徑單位是英寸，后兩位代表槳的角度。的螺旋槳，同樣轉(zhuǎn)速情況下產(chǎn)生的升力就會越大，也就需要更大的電機驅(qū)動，這就需要根據(jù)電機的大小去選擇槳的型號。在設(shè)計時選用1047的槳，槳的實物圖如圖3.12所示。
圖 3.11 四旋翼飛行器機架 圖 3.12 1047正反槳
3.4.4 遙控控制模塊
該遙控器控制模塊是由一個7通道的遙控器和配套的接收機組成的，接收機上的油門、副翼、升降舵、方向舵通道分別與控制器上的接口相連。控制器能通過定時器的捕獲功能，捕獲到遙控器通過接收機傳出的控制信號，遙控器就是這樣控制四旋翼飛行器的飛行動作的。遙控器和接收機的實物圖如圖3.13所示。
圖 3.13遙控器和接收機實物圖
4 四旋翼飛行器姿態(tài)參考系統(tǒng)設(shè)計4.1 姿態(tài)參考系統(tǒng)原理
姿態(tài)參考系統(tǒng)是利用慣性導(dǎo)航器件來測量載體姿態(tài)角的一種慣性導(dǎo)航系統(tǒng)。因為MEMS傳感器的數(shù)據(jù)帶有噪聲，因此姿態(tài)參考系統(tǒng)需要對MEMS傳感器的測量信息進(jìn)行濾波處理。然后對濾波信號進(jìn)行姿態(tài)解算。由于姿態(tài)傳感器各自特點的不同，所以需要對每個傳感器信號進(jìn)行濾波然后進(jìn)行數(shù)據(jù)融合，以此來提高測量的精度和系統(tǒng)的動態(tài)特性。姿態(tài)參考系統(tǒng)的原理圖，如圖4.1所示。
圖 4.1 姿態(tài)參考系統(tǒng)原理圖
從MEMS傳感器采集到的數(shù)據(jù)傳輸?shù)娇刂破魈幚淼倪^程中會受到很多因素的干擾，造成傳輸誤差，為了減小誤差，提高檢測的精度就需要用濾波器進(jìn)行數(shù)據(jù)處理。濾波器可以通過硬件實現(xiàn)，也可以通過軟件實現(xiàn)，具有較大的靈活性[[xii]。
從加速度傳感器提取到的信號經(jīng)過姿態(tài)解算后的姿態(tài)角信號在和陀螺儀數(shù)據(jù)進(jìn)行融合的過程中以低頻信號為主的。根據(jù)這一特性，加速度傳感器的輸出信號可以通過低通濾波器或者均值濾波器提高信噪比。
陀螺儀信號具有高動態(tài)的特點，它的信號噪聲主要為高斯白噪音，我們可以通過卡爾曼濾波器來濾除這種高斯白噪聲。因為需要融合加速度傳感器獲得的姿態(tài)角信號，所以選擇均值濾波來去除噪聲。
電子羅盤傳感器測量的是地磁強度，我們都知道物體在運動時地磁強度變化慢，它在姿態(tài)測量系統(tǒng)中屬于低頻信號，因此采用均值濾波來濾除噪聲信號。
歐拉角和四元數(shù)方法都是針對陀螺儀姿態(tài)測量進(jìn)行處理的數(shù)學(xué)方法，可以有效地解算處姿態(tài)角。把解算出來的姿態(tài)角進(jìn)行數(shù)據(jù)融合。考慮到高動態(tài)下陀螺儀測得的數(shù)據(jù)解算出的姿態(tài)角比較準(zhǔn)確因此在高動態(tài)條件下選用陀螺儀信號，低動態(tài)下加速度信號解算出的姿態(tài)角更穩(wěn)定因此在低動態(tài)條件下選擇加速度信號。
4.2 傳感器信號處理4.2.1 加速度傳感器信號處理
加速度傳感器的輸出信號主要表示直線加速度大小和與重力方向的夾角[[xiii]。 常用的數(shù)字濾波器有很多種，例如：限值濾波、遞推平均濾波法（又稱滑動平均濾波法）、算術(shù)平均濾波法、中位值濾波法和中位值平均濾波法等[[xiv]。
在實際的姿態(tài)解算中一般采用低通濾波器或者均值濾波器去除高頻噪聲干擾信號。均值濾波因其具備低通濾波的特性并且計算簡單，因此選用均值濾波來處理加速度信號。
4.2.2 陀螺儀信號處理
陀螺儀信號濾波要選擇卡爾曼濾波算法。對于卡爾曼濾波器，首先要清楚系統(tǒng)狀態(tài)的描述方法，系統(tǒng)狀態(tài)的更新方法和系統(tǒng)測量值的更新方法，從卡爾曼濾波算法的數(shù)學(xué)公式，我們可以歸納出它的核心思想：首先根據(jù)系統(tǒng)上一次得來的最優(yōu)值計算出當(dāng)前的估計值和協(xié)方差，再根據(jù)協(xié)方差大小計算出卡爾曼增益的大小，最后根據(jù)當(dāng)前估計值和測量值計算出當(dāng)前最優(yōu)值和協(xié)方差。下面分步建立卡爾曼濾波方程式：
預(yù)估計最優(yōu)值方程：
X(k|k-1) = A(k,k-1) * X(k-1|k-1)+ B(k) * U(k) (4-1)
上式中，X(k|k-1)表示的是根據(jù)k-1時刻的最優(yōu)值計算出的k時刻估計值；X(k|k-1)表示k-1時刻的最優(yōu)值；A(k,k-1)是狀態(tài)轉(zhuǎn)移矩陣；B(k)是輸入控制加權(quán)矩陣；U(k)表示k時刻的輸入控制信號，這些參數(shù)需要根據(jù)具體的應(yīng)用場合進(jìn)行設(shè)計。
預(yù)估計最優(yōu)值協(xié)方差：
P(k|k-1) = A(k,k-1) * P(k-1|k-1)* A(k,k-1) + Q(k) (4-2)
其中，P(k|k-1)表示預(yù)估計最優(yōu)值X(k|k-1)對應(yīng)的協(xié)方差；P(k-1|k-1)表示X(k-1|k-1)對應(yīng)的協(xié)方差，表示了對預(yù)測值的信任度；Q(k)表示k時刻系統(tǒng)過程的協(xié)方差，即對上一次測量估計值的信任程度，Q矩陣值越大表示信任度越低，需要根據(jù)具體應(yīng)用進(jìn)行設(shè)計。P和Q矩陣的區(qū)別在于Q矩陣是根據(jù)模型直接得到的，P矩陣是算法過程中通過計算獲得的。
計算卡爾曼增益矩陣：
K(k)=P(k|k-1) * H(k) / (H(k) * P(k|k-1) * H(k) + R(k)) (4-3)
其中，K(k)表示卡爾曼增益；R(k)表示k時刻觀測過程的covariance，即對測量的信任程度；H(k)表示觀測矩陣。
更新估計：
X(k|k)=X(k|k-1)+ K(k) * (Z(k) – H(k) *X(k|k-1)) (4-4)
P(k|k)=(1- K(k)* H(k)) * P(k|k-1) (4-5)
其中，Z(k)表示k時刻的觀測值，I為單位矩陣。
4.2.3 電子羅盤信號處理
電子羅盤信號非常容易受到高頻干擾，在系統(tǒng)應(yīng)用中，應(yīng)當(dāng)過濾系統(tǒng)的高頻噪聲，最好采用去極值濾波算法[[xv]。去極值濾波就是將連續(xù)測量的n個采樣值，按照數(shù)據(jù)的大小順序進(jìn)行排序，去掉最大值和最小值后對剩下的n-2個數(shù)值計算求取平均值，這種方法是最常用的濾波算法[[xvi]。
4.3 坐標(biāo)系
坐標(biāo)系是描述物體在空間的相對位置和運動規(guī)律的，而導(dǎo)航技術(shù)就是為了確定載體的空間位置。只有選定參考坐標(biāo)系，才能對系統(tǒng)的運動進(jìn)行描述。不同的坐標(biāo)系下載體的描述規(guī)律和運動形式也是不同的，選擇合適的坐標(biāo)系是非常重要的[[xvii]。
目前比較常用的坐標(biāo)系有：
(1)地理坐標(biāo)系(g系)
坐標(biāo)系和地球固連，其原點位于地球球心，通常選取東北天坐標(biāo)系，即坐標(biāo)Xg軸指向水平東方，Yg軸指向水平北方，Zg軸垂直于當(dāng)?shù)厮矫妫禺?dāng)?shù)卮咕�向上[[xviii]。
(2)導(dǎo)航坐標(biāo)系(n系)
一般選取當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，坐標(biāo)Xn軸指向地理東方，Yn軸指向地理北方，Zn軸垂直于當(dāng)?shù)厮矫妫禺?dāng)?shù)卮咕�向上[[xix],如圖4.2所示。
(3)載體坐標(biāo)系(b系)
載體坐標(biāo)系，原點位于機體的質(zhì)心位置，通常選取右前上坐標(biāo)系，其Xb軸沿機
體橫軸向右，Yb軸沿機體縱軸向前，Zb軸沿機體豎軸向上，如圖4.2所示。
圖 4.2 載體坐標(biāo)系和導(dǎo)航坐標(biāo)系
4.4 姿態(tài)角定義
姿態(tài)角也是我們常說的歐拉角，是根據(jù)導(dǎo)航坐標(biāo)系和載體坐標(biāo)系之間的關(guān)系來定義的。歐拉角是飛行器的三個姿態(tài)角即俯仰角、橫滾角和偏航角，根據(jù)歐拉旋轉(zhuǎn)定律可以用三次旋轉(zhuǎn)使得飛行器本身的坐標(biāo)系與地理參考系重合，每一次的旋轉(zhuǎn)以機體坐標(biāo)系的x、y、z軸中的一個坐標(biāo)軸來轉(zhuǎn)動，轉(zhuǎn)過的角度即為歐拉角，三次坐標(biāo)的變換矩陣相乘的積就是歐拉姿態(tài)矩陣[[xx]，形如4-1：
(4-6)
最終的姿態(tài)矩陣與這三次轉(zhuǎn)動的先后順序是有關(guān)系的，通常我們都按照Z-X-Y軸的順序。定義機體繞本體系x軸轉(zhuǎn)動的角度為俯仰角；機體繞本體系y軸轉(zhuǎn)動的角度為橫滾角；機體繞本體系z軸轉(zhuǎn)動的角度為航向角；三個角當(dāng)轉(zhuǎn)動方向與旋轉(zhuǎn)軸符合右手定則為正方向[[xxi]。俯仰角、橫滾角和航向角合稱歐拉角。得到如下姿態(tài)矩陣4-2：
(4-7)
4.5 四元數(shù)姿態(tài)解算算法
四元數(shù)其實是 1個單位實數(shù)和3個虛數(shù)單位、 和 的線性組合，一般可表示為d + a + b + c， a、b、c、d代表實數(shù)[[xxii]。
四元數(shù)乘法運算關(guān)系如下：
式中，表示的是四元數(shù)的乘法。
四元數(shù)與姿態(tài)矩陣之間的關(guān)系：
設(shè)有參考坐標(biāo)系R，坐標(biāo)軸X0、 Y0、 Z0，坐標(biāo)軸方向的單位向量為、 、[[xxiii]。剛體相對于坐標(biāo)系R作定點轉(zhuǎn)動，定點為O，把坐標(biāo)系b與剛體固聯(lián)，b系的坐標(biāo)軸為x、y、z，坐標(biāo)軸方向的單位向量為、 、 [[xxiv]。假設(shè)初始時刻b系與R系重合。在剛體上任取一點A，從 O點向該點引向量,如圖4.3所示。則該位置向量描述了剛體的空間角位置[[xxv]。
圖4.3 剛體的等效轉(zhuǎn)換
設(shè)剛體以= 相對R系旋轉(zhuǎn)，初始時刻位置處于=，經(jīng)過時間t后位置向量處于。根據(jù)歐拉定理，剛體從位置轉(zhuǎn)到位置的轉(zhuǎn)動可等效成繞瞬軸轉(zhuǎn)過角一次完成[[xxvi]。這樣，位置向量做圓錐運動，和位于同一圓上，和位于同一圓錐面上。
在圓上取一點B，是，由圖4.3可得。
所以
由三重矢量計算公式：
即
所以
將上式向R系內(nèi)投影：
記
，，
根據(jù)叉乘關(guān)系表達(dá)式：
記
(4-8)
則
所以
(4-9)
令
(4-10)
則式(4-4)可以寫成：
(4-11)
記剛體固聯(lián)坐標(biāo)系為b系。
所以
而在轉(zhuǎn)動過程中，位置向量和b系都同剛體固聯(lián)，所以位置和b系的相對位角位置不變，即有
因此可得：
該式說明即為b系至R系的坐標(biāo)變換矩陣，根據(jù)式(4-3)和(4-5)
即
(4-12)
令
(4-13)
并以、、、構(gòu)造四元數(shù)：
(4-14)
可得如下結(jié)論：
(1)四元數(shù) 描述了剛體的定點運動[[xxvii]， 包含了等效旋轉(zhuǎn)的全部信息，為旋轉(zhuǎn)瞬軸的旋轉(zhuǎn)方向，為轉(zhuǎn)過的角度。
(2)四元數(shù)可以確定出b系至R系的坐標(biāo)變換矩陣。將(4-8)代入(4-7)得：
(4-15)
由于，所以可進(jìn)一步得出如下結(jié)論：
(4-16)
如果將向量和看作零標(biāo)量的四元數(shù)，則和間的變換關(guān)系可采用四元數(shù)乘法表示：
該式稱為坐標(biāo)變換的四元數(shù)乘表示方法，其中為R系至b系的旋轉(zhuǎn)四元數(shù)。證明如下：
(4-17)
對比式(4-11)知上式矩陣中右下角的33方塊即為，所以式(4-12)可寫成：
即
該式稱為坐標(biāo)變換的矩陣表示法。所以四元數(shù)乘法表示法和矩陣表示法是等價的。
如果參考坐標(biāo)系R是導(dǎo)航坐標(biāo)系n,剛體的固聯(lián)坐標(biāo)系b為機體坐標(biāo)系，則坐標(biāo)變換矩陣就是姿態(tài)矩陣，而由姿態(tài)矩陣可計算出姿態(tài)角[[xxviii]。
記，由于在坐標(biāo)系旋轉(zhuǎn)的過程中坐標(biāo)系始終保持直角坐標(biāo)系，所以為正交矩陣
對比上式可得姿態(tài)角
(4-18)
因此，四元數(shù)包含了所有的姿態(tài)信息，姿態(tài)解算實際上是如何計算四元數(shù)。下面來介紹如何計算四元數(shù)：
(1)初始化四元數(shù)
假設(shè)當(dāng)前的坐標(biāo)系為地理坐標(biāo)系，則四元數(shù)列向量
(2)從傳感器獲取載體加速度和角速度
從MPU6050讀取三軸加速度計的測量值即加速度、、，陀螺儀的測量值即角速度，，。
(3)將加速度計得出來三個軸的加速度值、、轉(zhuǎn)化為三維的單位向量得到：
(4-19)
(4)將地理坐標(biāo)系的重力向量轉(zhuǎn)換到機體坐標(biāo)系可得三軸的重力量，，：
(4-20)
(5)將地理坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系下的重力向量和載體坐標(biāo)系測量的加速度向量外積，得到兩坐標(biāo)系的誤差[[xxix]：
(4-21)
(6)陀螺儀誤差是導(dǎo)致機體坐標(biāo)系誤差的根本原因，因此用兩坐標(biāo)系誤差的PI來補償陀螺儀使得機體坐標(biāo)系更加準(zhǔn)確。
(4-22)
其中：和是調(diào)整參數(shù)，在實際調(diào)試中確定。其中可以等于0，可以以0為初始值，0.01步進(jìn)調(diào)節(jié)。
(7)四元數(shù)姿態(tài)更新方程[[xxx]。
四元數(shù)微分方程為：
(4-23)
其中，
將上式寫成矩陣形式
(4-24)
對四元數(shù)一階微分方程進(jìn)行一階畢卡算法可得：
(4-25)
(8)對四元數(shù)進(jìn)行規(guī)范化處理，于是得下式4-21：
(4-26)
(9)以上得到的新的四元數(shù)代表完成了一次四元數(shù)的運算，將此四元數(shù)回到開頭，將舊的四元數(shù)更新為新四元數(shù)，作為下一次四元數(shù)運算的初始數(shù)，再從(1)開始下一次的四元數(shù)運算[[xxxi]。與此同時將新的四元數(shù)更新規(guī)范化后轉(zhuǎn)化成三個歐拉角得下式(4-22)，完成了姿態(tài)的初步運算[[xxxii]：
(4-27)
4.6 校準(zhǔn)載體航向角
通過三軸數(shù)字電子羅盤可以校準(zhǔn)陀螺儀積分獲得的航偏角以消除累計誤差。如果電子設(shè)備干擾強烈的情況下需要暫停數(shù)字羅盤的數(shù)據(jù)融合[[xxxiii]，因為這樣測到的數(shù)據(jù)誤差非常的大。對地磁傳感器常用的校準(zhǔn)方法之一是平面校準(zhǔn)法[[xxxiv]：使用數(shù)字羅盤前，對其進(jìn)行初始化，然后進(jìn)行磁場校準(zhǔn)，將傳感器水平放置并旋轉(zhuǎn)一周得到新的圓周圓心位置 [[xxxv]。這是對水平的平面的校準(zhǔn)，校準(zhǔn)前后對比如圖4.4所示。
圖 4.4 xy平面校準(zhǔn)前后對比
同理需要對豎直平面進(jìn)行校準(zhǔn)，校準(zhǔn)前后的對比圖，如圖4.5所示。
圖 4.5 xz平面校準(zhǔn)前后對比
當(dāng)載體靜止時，設(shè)地球磁場強度在載體坐標(biāo)系b系各個軸的分量為，在導(dǎo)航坐標(biāo)系R系各軸的分量，根據(jù)
可得：
(4-28)
將4-22式求得的、代入上式，即可求出和。航向角可由式4-24得出：
(4-29)
5 四旋翼飛行器系統(tǒng)軟件設(shè)計5.1 系統(tǒng)程序設(shè)計5.1.1 姿態(tài)參考系統(tǒng)軟件設(shè)計
姿態(tài)參考系統(tǒng)的軟件部分主要是對MEMS傳感器數(shù)據(jù)和三軸數(shù)字電子羅盤傳感器的數(shù)據(jù)進(jìn)行處理和數(shù)據(jù)融合，圖5.1給出了姿態(tài)參考系統(tǒng)的軟件流程圖。
圖 5.1 姿態(tài)參考系統(tǒng)程序流程圖
通過軟件流程圖可以清楚地看到，系統(tǒng)首先完成MCU內(nèi)部初始化的工作，即配置STM32使用到的外設(shè)，包括時鐘系統(tǒng)、NVIC中斷控制器、USART串口通信、I2C接口、定時器；然后進(jìn)行硬件初始化以及系統(tǒng)的姿態(tài)解算，采用模塊化的設(shè)計，具體包括以下4個部分：(1)初始化硬件；(2) MEMS慣性測量單元的數(shù)據(jù)采集模塊；(3)多傳感器數(shù)據(jù)融合的姿態(tài)解算模塊；(4)姿態(tài)信息輸出模塊。
首先開啟定時器中斷，在2.5ms的計時中斷實現(xiàn)通過I2C接口讀取MEMS傳感器和三軸數(shù)字電子羅盤的測量數(shù)據(jù)，根據(jù)傳感器的特點進(jìn)行濾波后，再利用初始校正所得到的校正數(shù)據(jù)對讀取的數(shù)據(jù)作校正，校正完后轉(zhuǎn)換成實際的物理量，再對其做加權(quán)平均減小誤差，然后進(jìn)行四元數(shù)姿態(tài)解算和地磁數(shù)據(jù)校準(zhǔn)，最后進(jìn)行數(shù)據(jù)融合輸出姿態(tài)角信息。
5.1.2 PID控制算法設(shè)計
根據(jù)四旋翼飛行器的工作原理知，因為其結(jié)構(gòu)對稱，所以改變電機的轉(zhuǎn)速差能夠?qū)崿F(xiàn)對飛行器的姿態(tài)控制，而飛行器的位置控制要靠電機的總升力和飛行器姿態(tài)改變的[[xxxvi]。所以，將飛行控制系統(tǒng)分為位置控制系統(tǒng)和姿態(tài)控制系統(tǒng)分別進(jìn)行控制設(shè)計。由于飛行器姿態(tài)會直接影響到飛行器的位置，認(rèn)為飛行器姿態(tài)控制器為內(nèi)環(huán)控制器，位置控制器為外環(huán)控制器。這種數(shù)字式PID控制有位置式PID控制算法和增量式PID控制算法兩種形式，這里采用的是增量式PID算法[[xxxvii]，其表達(dá)式為：
(5-1)
其中，為比例系數(shù)，為積分系數(shù)，微分系數(shù)。是第次采樣時刻計算的PID輸出值，是第次采樣時刻控制器輸入的偏差。
，
四旋翼飛行器的姿態(tài)可分為俯仰角、橫滾角和偏航角，針對每一個自由度都是一個二階系統(tǒng)[[xxxviii]。對每個姿態(tài)角都進(jìn)行PID控制，這樣就可以將復(fù)雜的非線性多變量輸入多變量輸出控制問題化簡為兩變量輸入單變量輸出的問題[[xxxix]。如式5-2所示。
(5-2)
這里、、、代表的是控制四個電機所需要的PWM值，表示的是飛行器停留在空中需要的PWM值，可以由遙控器輸入或者自動高度修正，、、分別表示的是俯仰角偏差、橫滾角偏差和航向角偏差、及其變化率所需的PWM值，把PWM限制在一定的范圍之內(nèi)。
、、采用串級PID控制算法，內(nèi)回路是角速度控制回路，外回路是角度控制回路。三個角度的控制規(guī)律是一致的，以俯仰角的控制為例，如圖5.2所示，其中為控制飛行器的期望俯仰角，和分別表示飛行器俯仰角角度和俯仰角角速度。是外環(huán)PID的輸出值，是內(nèi)環(huán)PID控制器的輸出值，且。
圖 5.2 串級PID控制圖
首先需要求出四旋翼飛行器姿態(tài)誤差信號也就是期望值的姿態(tài)角與當(dāng)前獲取到的姿態(tài)角的差值，然后通過串級PID控制算法求得各個電機的調(diào)整量，將調(diào)整信號傳遞給四個旋轉(zhuǎn)電機，改變電機的轉(zhuǎn)速來控制整個系統(tǒng)的姿態(tài)，使其姿態(tài)誤差始終趨于最小，形成雙級閉環(huán)回路控制系統(tǒng)。
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
