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