無(wú)人機(jī)控制PX4系統(tǒng)傳感器介紹

飛控系統(tǒng)的傳感器1.1飛控系統(tǒng)的傳感器無(wú)人飛行器要求能夠穩(wěn)定飛行，首先最基礎(chǔ)的問(wèn)題是需要確定自己在空間中的位置、速度和姿態(tài)等相關(guān)的系統(tǒng)狀態(tài)。而要的到這些狀態(tài)，就需要通過(guò)不同的安裝在機(jī)身系統(tǒng)上的各種不同的傳感器。我們所處的空間是三維空間，因此主要的飛行器系統(tǒng)狀態(tài)也主要基于這個(gè)三維空間同時(shí)在時(shí)間維度進(jìn)行拓展：1：通過(guò)全球定位系統(tǒng)GNSS來(lái)定位自己的經(jīng)度、維度和高度等三維坐標(biāo)信息，同時(shí)也可以獲取這三維的速度信息2：通過(guò)陀螺儀加速度計(jì)直接獲取三軸加速度信息與旋轉(zhuǎn)角信息的狀態(tài)量，其他的狀態(tài)欄只有通過(guò)姿態(tài)解算3：當(dāng)飛行器需要往某個(gè)方向飛行時(shí)是通過(guò)調(diào)整飛行器的姿態(tài)往對(duì)應(yīng)方向傾斜，飛行器的一部分升力會(huì)分配到該方向上成為該方向的拉力。飛行器要能夠調(diào)整飛行的姿態(tài)，就必須能夠?qū)崟r(shí)的獲得機(jī)體當(dāng)前相對(duì)于慣性坐標(biāo)系的姿態(tài)，在三維空間中同樣姿態(tài)角也是由三個(gè)軸的角度來(lái)表示4：飛行器的三維空間位置信息、三維空間速度信息、三維空間角度信息以及三維空間加速度信息和三維空間的角速度信息，總共有是十五個(gè)系統(tǒng)空間狀態(tài)量需要獲得5：傳感器跟估計(jì)的精度決定了建模辨識(shí)與控制的精度，然后傳感器跟估計(jì)的精度，與建模辨識(shí)，一起決定了控制的精度。因此傳感器的采集精度與飛行控制的控制精度密切相關(guān)1.2I2C簡(jiǎn)介光標(biāo)飛控系統(tǒng)中集成的微機(jī)械六軸傳感器和磁力計(jì)均采用I2C總線(xiàn)接口與主控處理器連接。本章著重介紹I2C接口總線(xiàn)、各傳感器的接口驅(qū)動(dòng)、數(shù)據(jù)采集及處理模型。I2C(Inter—IntegratedCircuit)總線(xiàn)是一種由PHILIPS公司開(kāi)發(fā)的兩線(xiàn)式串行總線(xiàn)，用于連接微控制器及其它的一些外圍設(shè)備。和我們常用的UART通信不同，雖然UART有TX、RX兩個(gè)接口，但是這兩根線(xiàn)都是可以單獨(dú)使用，I2C是由數(shù)據(jù)線(xiàn)SDA和時(shí)鐘SCL構(gòu)成的串行總線(xiàn)，可發(fā)送和接收數(shù)據(jù)。在CPU與被控IC設(shè)備之間、IC設(shè)備與IC設(shè)備之間進(jìn)行雙向傳送，高速I(mǎi)2C總線(xiàn)一般可達(dá)400kbps以上。它的特點(diǎn)是：通信模式為主從式設(shè)備，可以一主多從，也可以多主多從I2C總線(xiàn)組成“線(xiàn)與”的關(guān)系，任何一個(gè)器件都可以拉到低電平I2C總線(xiàn)上可以并連多個(gè)器件I2C總線(xiàn)有起始信號(hào)、數(shù)據(jù)傳輸、停止信號(hào)支持7位/10位芯片地址尋址支持不同的通信速率，標(biāo)準(zhǔn)速度為100kHz，高速速度為400kHzI2C總線(xiàn)在傳送數(shù)據(jù)過(guò)程中一共有三種類(lèi)型的信號(hào)，它們分別是：開(kāi)始信號(hào)、結(jié)束信號(hào)和應(yīng)答信號(hào)。vcc七設(shè)法 從設(shè)備工從設(shè)備2 從設(shè)備4起始信號(hào)與停止信號(hào)?起始信號(hào)：當(dāng)SCL為高期間，SDA由高到低的跳變；啟動(dòng)信號(hào)是一種電平跳變時(shí)序信號(hào)，而不是一個(gè)電平信號(hào)。?停止信號(hào)：當(dāng)SCL為高期間，SDA由低到高的跳變；停止信號(hào)也是一種電平跳變時(shí)序信號(hào)，而不是一個(gè)電平信號(hào)。發(fā)送器每發(fā)送一個(gè)字節(jié)，就在時(shí)鐘脈沖9期間釋放數(shù)據(jù)線(xiàn)，由接收器反饋一個(gè)應(yīng)答信號(hào)。應(yīng)答信號(hào)為低電平時(shí)，規(guī)定為有效應(yīng)答位（ACK簡(jiǎn)稱(chēng)應(yīng)答位），表示接收器已經(jīng)成功地接收了該字節(jié)；應(yīng)答信號(hào)為高電平時(shí)，規(guī)定為非應(yīng)答位（NACK），一般表示接收器接收該字節(jié)沒(méi)有成功。對(duì)于反饋有效應(yīng)答位ACK的要求是，接收器在第9個(gè)時(shí)鐘脈沖之前的低電平期間將SDA線(xiàn)拉低，并且確保在該時(shí)鐘的高電平期間為穩(wěn)定的低電平。如果接收器是主控器，則在它收到最后一個(gè)字節(jié)后，發(fā)送一個(gè)NACK信號(hào)，以通知被控發(fā)送器結(jié)束數(shù)據(jù)發(fā)送，并釋放SDA線(xiàn)，以便主控接收器發(fā)送一個(gè)停止信號(hào)P。

DATA岬UTByTranw?itturDATAtlutpulbyreceiverSCLlasilernotDATA岬UTByTranw?itturDATAtlutpulbyreceiverSCLlasilernot.匚knElcdiscStartcundition.BckniMled^el2C總線(xiàn)劇的應(yīng)數(shù)據(jù)有效性I2C總線(xiàn)進(jìn)行數(shù)據(jù)傳送時(shí)，時(shí)鐘信號(hào)為高電平期間，數(shù)據(jù)線(xiàn)上的數(shù)據(jù)必須保持穩(wěn)定，只有在時(shí)鐘線(xiàn)上的信號(hào)為低電平期間，數(shù)據(jù)線(xiàn)上的高電平或低電平狀態(tài)才允許變化。即：數(shù)據(jù)在SCL的上升沿到來(lái)之前就需準(zhǔn)備好。并在在下降沿到來(lái)之前必須穩(wěn)定。贊求改據(jù)整定贊求改據(jù)整定I2C通訊常用基本寫(xiě)時(shí)序?qū)τ谝话阍O(shè)計(jì)來(lái)說(shuō)，一對(duì)I2C引腳可以?huà)旖雍芏嘀С諭2C通訊的設(shè)備，因此需要對(duì)每個(gè)I2C設(shè)定設(shè)定一個(gè)設(shè)備地址（DEVICEADDRESS，設(shè)備地址設(shè)備里面自帶，不許需要我們自己設(shè)定）。I2C一般寫(xiě)時(shí)序?yàn)椋褐鳈C(jī)發(fā)送起始信號(hào)一》主機(jī)發(fā)送設(shè)備地址編號(hào)（7位地址+寫(xiě)位）+從機(jī)發(fā)送ACK信號(hào)+主機(jī)發(fā)送命令+從機(jī)發(fā)送ACK信號(hào)+主機(jī)發(fā)送數(shù)據(jù)+從機(jī)發(fā)送ACK信號(hào)+主機(jī)發(fā)送停止信號(hào)。ByteWriteSTARTDEVICE

ADDPE&SSDALINEmnWOFinADDRESSLSB前ByteWriteSTARTDEVICE

ADDPE&SSDALINEmnWOFinADDRESSLSB前I2C通訊常用基本讀時(shí)序I2C一般讀時(shí)序?yàn)椋褐鳈C(jī)發(fā)送起始信號(hào)--》主機(jī)發(fā)送設(shè)備地址編號(hào)（7位地址+讀位）+從機(jī)發(fā)送ACK+主機(jī)發(fā)送命令+從機(jī)發(fā)送ACK+主機(jī)發(fā)送起始信號(hào)+主機(jī)發(fā)送設(shè)備地址編號(hào)（7位地址+寫(xiě)位）+從機(jī)發(fā)送ACK+從機(jī)發(fā)送數(shù)據(jù)（根據(jù)主機(jī)的命令）+主機(jī)發(fā)送停止信號(hào)。RandomReadSTARTDEVICE

ADDRESSWRITEWORDADDRESSnSTARTDEVICEADDRESSREADSTOPmsbMSB

ACKSTARTDEVICE

ADDRESSWRITEWORDADDRESSnSTARTDEVICEADDRESSREADSTOPmsbMSB

ACKR/wLSBACK

LSB

MSB

ACK

LSBDATAnDUMMYWRITE1.2.2I2C驅(qū)動(dòng)在STM32中的硬件實(shí)現(xiàn)I2C通訊常用基本寫(xiě)時(shí)序STM32芯片中有集成了I2C模塊，通過(guò)I2C模塊，CPU軟件可以不需要關(guān)注I2C總線(xiàn)的協(xié)議實(shí)現(xiàn)以及通信時(shí)的具體會(huì)話(huà)過(guò)程，只需要將待發(fā)送的數(shù)據(jù)放入發(fā)送緩沖區(qū)中，啟動(dòng)I2C傳輸即可，而接收的時(shí)候也可以由硬件觸發(fā)中斷，從接收緩沖區(qū)中讀取數(shù)據(jù)既可。STM32F4的I2C模塊的工作過(guò)程：1：STM32F407的I2C模塊即可以實(shí)現(xiàn)I2C主設(shè)備模式，同時(shí)也能實(shí)現(xiàn)I2C從設(shè)備模式。在飛控系統(tǒng)中，主要采用STM32F407的主設(shè)備模式:在主模式下，I2C模塊會(huì)啟動(dòng)I2C總線(xiàn)上的數(shù)據(jù)傳輸，同時(shí)輸出SCL的時(shí)鐘信號(hào)：當(dāng)控制寄存器I2C_CR1的START位置1時(shí)，模塊自動(dòng)切換到主模式下，同時(shí)在總線(xiàn)上發(fā)出起始位信號(hào)，狀態(tài)寄存器I2C_SR1的SB為會(huì)被硬件置1，并且當(dāng)ITEVFEN位置1時(shí)產(chǎn)生系統(tǒng)中斷：接著內(nèi)部緩沖區(qū)會(huì)將從設(shè)備地址從內(nèi)部并串轉(zhuǎn)換的移位寄存器中發(fā)送到I2C總線(xiàn)上，發(fā)送完畢后狀態(tài)寄存器中的ADDR位會(huì)被置1。接下來(lái)主設(shè)備會(huì)根據(jù)讀寫(xiě)操作進(jìn)入接收模式或者發(fā)送模式，將最后1位置位或者復(fù)位發(fā)送到總線(xiàn)上。在發(fā)送完從地址信息后，主設(shè)備會(huì)檢測(cè)從設(shè)備響應(yīng)的ACK信號(hào)，只有I2C總線(xiàn)上的從地址設(shè)備出現(xiàn)了地址匹配命中的事件并將ACK位拉低后，主設(shè)備才能知道有相應(yīng)的從地址掛載在I2C總線(xiàn)上。SDASCLSMBASDASCLSMBA加速度計(jì)原理慣性測(cè)量元件是一種能夠在慣性系中測(cè)量載體自身三維加速度和三維角度的設(shè)備，主要分為加速度計(jì)和角速度計(jì)兩種，其統(tǒng)稱(chēng)為慣性測(cè)量元件。根據(jù)物理學(xué)原理，加速度的積分是速度，速度的積分是位置信息；角速度的積分是角度信息加速度計(jì)，或稱(chēng)加速度傳感器是一種能夠測(cè)量加速度的設(shè)備。加速度傳感器可以幫助你檢測(cè)到一個(gè)物體或事物此時(shí)此刻的狀態(tài)，是停止，還是在運(yùn)動(dòng)，是在向前、向后、向左、還是向右運(yùn)動(dòng)，以及是在向上還是向下運(yùn)動(dòng)。加速度傳感器甚至可以用來(lái)分析物體的振動(dòng)。慣性元件：慣性測(cè)量元件本身固有的誤差由于時(shí)間積分會(huì)累積誤差，并且受到元件本身的工藝、技術(shù)和成本的緣故，其誤差越大，誤差的累積就會(huì)越快，因此在飛行上萬(wàn)公里后累積的誤差可以達(dá)到幾公里甚至幾十公里，因此現(xiàn)在導(dǎo)航系統(tǒng)都是通過(guò)GNSS全球衛(wèi)星定位系統(tǒng)獲取精準(zhǔn)的定位和速度信息來(lái)對(duì)慣導(dǎo)系統(tǒng)進(jìn)行修正。其實(shí)人體也有慣性測(cè)量元件，人的耳蝸充滿(mǎn)液體，人在運(yùn)動(dòng)的時(shí)候，可以被二種的神經(jīng)感受到，因此可以測(cè)量出人體自身的運(yùn)動(dòng)特征。的慣性測(cè)量元件也是非常差，閉上眼睛，也不摸周?chē)臇|西，只靠耳蝸感受的移動(dòng)，人基本沒(méi)法走直線(xiàn)，這里眼睛的作用就相當(dāng)于GNSS的作用，它可以隨時(shí)修正耳蝸所感應(yīng)的慣性信息。MEMSMEMS是(Micro-Electro-MechanicalSystems)的英文縮寫(xiě)，它是指集成微型機(jī)械結(jié)構(gòu)、微型傳感器、微型執(zhí)行器以及信號(hào)處理和控制電路、直至接口、通信和電源等于一體的微型器件或系統(tǒng)?，F(xiàn)在很多微型加速度傳感器，陀螺儀都是基于MEMS技術(shù)實(shí)現(xiàn)的。微機(jī)械加速度傳感器可以根據(jù)壓電效應(yīng)的原理來(lái)工作。壓電效應(yīng)：所謂壓電效應(yīng)就是“對(duì)于不存在對(duì)稱(chēng)中心的異級(jí)晶體加在晶體上的外力除了使晶體發(fā)生形變以外，還將改變晶體的極化狀態(tài)，在晶體內(nèi)部建立電場(chǎng)，這種由于機(jī)械力作用使介質(zhì)發(fā)生極化的現(xiàn)象稱(chēng)為正壓電效應(yīng)”。還可以根據(jù)壓阻技術(shù)、諧振式、電容效應(yīng)等原理來(lái)制作慣性測(cè)量傳感器元件，但是所有的原理基本都是由內(nèi)部集成的微機(jī)械質(zhì)量塊收到物理機(jī)械力的作用帶來(lái)的某個(gè)介質(zhì)的形變，并將產(chǎn)生形變的量轉(zhuǎn)換為電壓輸出，通過(guò)相應(yīng)的放大和濾波電路進(jìn)行采集，測(cè)量量的大小分別與電阻、電壓、電容的變化成正比。加速度計(jì)測(cè)量信息加速度傳感器分為二軸與三軸加速度傳感器，二軸加速度傳感器：能夠同時(shí)檢測(cè)兩個(gè)方向（x軸，y軸）上的加速度。三軸加速度傳感器：能夠同時(shí)檢測(cè)三個(gè)方向上的加速度，x，y，z。加速度測(cè)量一些特性有如下幾個(gè)方面：傾斜度偵測(cè)傾斜度偵測(cè)儀。加速傳感器在靜止時(shí)，可用來(lái)檢測(cè)傾斜角，傾斜角在90~+90之間變化時(shí)，重力加速度信息會(huì)分配到三個(gè)軸上，加速度傳感器輸出的三軸分量可以計(jì)算出傾斜角度運(yùn)動(dòng)檢測(cè)運(yùn)動(dòng)檢測(cè)可用于運(yùn)動(dòng)控制，計(jì)步器，基本運(yùn)動(dòng)檢測(cè)。定位偵測(cè)定位偵測(cè)用于汽車(chē)導(dǎo)航，防盜設(shè)備，地圖跟蹤。定位偵測(cè)需要測(cè)量的因素包括：加速度的范圍是多少及加速傳感器如何安裝。對(duì)加速度數(shù)據(jù)進(jìn)行二重積分即可得到位置數(shù)據(jù)。震動(dòng)偵測(cè)震動(dòng)偵測(cè)可用于下降記錄，黑盒子/故障記錄儀，硬盤(pán)保護(hù)，運(yùn)輸和處理監(jiān)視器。震動(dòng)偵測(cè)主要收集加速度計(jì)的原始數(shù)據(jù)并進(jìn)行閾值選擇。一般按照被測(cè)量對(duì)象的減速度決定了震動(dòng)檢測(cè)所需的加速傳感器的規(guī)則選取。振動(dòng)偵測(cè)振動(dòng)偵測(cè)可用于地震活動(dòng)監(jiān)視器，家電平衡和監(jiān)測(cè)。振動(dòng)偵測(cè)需要考慮的因素包括：分析振動(dòng)頻率的多少，確定g值的范圍及最適當(dāng)?shù)募铀賯鞲衅靼惭b位置。借助于快速傅立葉變換對(duì)加速度資料的分析可得到振動(dòng)頻率的情況，快速傅立葉變換允許振動(dòng)信號(hào)被分解成它的諧波分量，而每個(gè)電機(jī)振動(dòng)都有它自己的諧波分量信號(hào)。自由下落偵測(cè)自由落下偵測(cè)可用于自由落體保護(hù)，下降記錄，下降檢測(cè)，運(yùn)動(dòng)控制和認(rèn)知等。自由落下偵測(cè)包含線(xiàn)性落下、旋轉(zhuǎn)型落下和拋射落下。

<a)制量加速度為。 (⑴"I量工速度為刊 2測(cè)量印理發(fā)為1加速度計(jì)原始數(shù)據(jù)采集MPU6050是一個(gè)六軸運(yùn)動(dòng)處理傳感器，包含了三軸加速度和三軸陀螺儀以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP(DigitalMotionProcessor)。MPU-60X0對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。1：陀螺儀可測(cè)范圍為士250，±500，±1000，±2000°/秒(dps)2：加速度計(jì)可測(cè)范圍為±2，±4，±8，±16g1.4.2加速度計(jì)校準(zhǔn)3：上還內(nèi)嵌了一個(gè)溫度傳感器和在工作環(huán)境下僅有±1%變動(dòng)的振蕩器，溫度傳感器可以實(shí)時(shí)感知傳感器所處的環(huán)境溫度，以便對(duì)傳感器進(jìn)行溫度補(bǔ)償校準(zhǔn)1.4.2加速度計(jì)校準(zhǔn)其中其中am是校準(zhǔn)前的原始測(cè)量值，由于芯片生產(chǎn)和裝配的因素該值一般都會(huì)\bf加速度計(jì)的原理特性，它的誤差模型為:ax有零點(diǎn)偏移誤差，在公式中用拉=匕篇來(lái)表示零點(diǎn)偏移補(bǔ)償，此外還有量程比aaybrTOC\o"1-5"\h\zLaZ l r5皿 00例的縮放導(dǎo)致的尺度準(zhǔn)確性，公式中用尺度因子與=05叼0來(lái)補(bǔ)償，0 0Sl az」

最后由于安裝誤差和芯片內(nèi)部感應(yīng)器件軸的扭曲因素，還需要一個(gè)旋轉(zhuǎn)因子矩納-A3/。來(lái)補(bǔ)償。加速度計(jì)的校準(zhǔn)就是要得到這幾個(gè)未知矩1陣。加速度計(jì)的校正方法可以采用六面校準(zhǔn)方法。基于的原理就是在靜止?fàn)顟B(tài)下，加速度計(jì)各軸輸出的矢量和等于重力加速度g。加速度計(jì)校準(zhǔn)---6個(gè)特殊位置在這六個(gè)特殊位置記錄下測(cè)量的數(shù)據(jù)。再計(jì)算不同角度的三維模與重力加速度g的方差的最小值。該方差值達(dá)到最小值時(shí)的各參量矩陣的數(shù)值即為校準(zhǔn)值。加九t（%Sa（%+"）"-g）2i=1其中最簡(jiǎn)單的校準(zhǔn)方法為：找到三軸的中心零點(diǎn)的位置。即需要將裝配了加速度計(jì)的飛控板水平放置，Z軸朝正下方，此時(shí)理論上X與Y軸的重力分量為0，Z軸為1g的加速度測(cè)量量。這時(shí)X與Y軸所讀到的數(shù)據(jù)近似認(rèn)為是零點(diǎn)漂移，而Z軸的數(shù)據(jù)減去1g得到的誤差也認(rèn)為是Z軸的零點(diǎn)飄移。這種方法其實(shí)是忽略了三軸旋轉(zhuǎn)誤差并將這些誤差近似到零點(diǎn)飄移中。Cb）倒立Cb）倒立S）后惻S）前裂］（e）右根」（f）左側(cè)陀螺儀的原理陀螺儀又稱(chēng)為角速度傳感器，用于檢測(cè)旋轉(zhuǎn)角速度的大小。角速度的單位deg/s（度/每秒）。MEMS陀螺儀有多種設(shè)計(jì)以及多種原理（內(nèi)/外框架驅(qū)動(dòng)式、梳狀和電磁驅(qū)動(dòng)式等）來(lái)實(shí)現(xiàn)陀螺儀的功能，但是基本上都是采用振動(dòng)相互正交振動(dòng)和轉(zhuǎn)動(dòng)引起的交變科里奧利力。它的基本原理為兩個(gè)有質(zhì)量的質(zhì)量塊運(yùn)動(dòng)速度方向相反，大小相同，所產(chǎn)生科里奧利力相反，從而壓迫兩塊對(duì)應(yīng)的電容板移動(dòng)，產(chǎn)生電容差分變化。因?yàn)榧铀俣茸兓荒艽偈箖蓚€(gè)質(zhì)量塊朝相同方向移動(dòng)，不會(huì)帶來(lái)電容差分變化，因此電容差分變化與旋轉(zhuǎn)角速度成正比?？评飱W利力（Coriolisforce）有些地方也稱(chēng)作哥里奧利力，簡(jiǎn)稱(chēng)為科氏力，是對(duì)旋轉(zhuǎn)體系中進(jìn)行直線(xiàn)運(yùn)動(dòng)的質(zhì)點(diǎn)由于慣性相對(duì)于旋轉(zhuǎn)體系產(chǎn)生的直線(xiàn)運(yùn)動(dòng)的偏移的一種描述。FFgrioiis工-2mClxv陀螺儀的測(cè)量信息三自由度陀螺儀具有內(nèi)、外兩個(gè)框架，使轉(zhuǎn)子自轉(zhuǎn)軸具有兩個(gè)轉(zhuǎn)動(dòng)自由度。在沒(méi)有任何力矩裝置時(shí)，它就是一個(gè)自由陀螺儀。陀螺儀作用于航空、航天、航海、兵器、汽車(chē)、生物醫(yī)學(xué)、環(huán)境監(jiān)控等領(lǐng)域。陀螺儀可以根據(jù)需要提供準(zhǔn)確的方位、水平、位置、速度和加速度等信號(hào)。主要應(yīng)用于：導(dǎo)航。配合GPS，手機(jī)的導(dǎo)航能力將達(dá)到前所未有的水準(zhǔn)。實(shí)際上，目前很多專(zhuān)業(yè)手持式GPS上也裝了陀螺儀，如果手機(jī)上安裝了相應(yīng)的軟件，其導(dǎo)航能力絕不亞于目前很多船舶、飛機(jī)上用的導(dǎo)航儀。各類(lèi)游戲的傳感器，比如飛行游戲，體育類(lèi)游戲，甚至包括一些第一視角類(lèi)射擊游戲，陀螺儀完整監(jiān)測(cè)游戲者手的位移，從而實(shí)現(xiàn)各種游戲操作效果。可以用作輸入設(shè)備，陀螺儀相當(dāng)于一個(gè)立體的鼠標(biāo)，這個(gè)功能和第三大用途中的游戲傳感器很類(lèi)似，甚至可以認(rèn)為是一種類(lèi)型。陀螺儀校準(zhǔn)? 三軸陀螺儀在生產(chǎn)與實(shí)際應(yīng)用中同樣也會(huì)引入三軸偏差，以及零點(diǎn)偏差的因素。因此同樣需要對(duì)陀螺儀傳感器進(jìn)行精確校準(zhǔn)。類(lèi)似加速度傳感器，三軸陀螺儀的誤差模型可以用式3=RS（3,+b'）m^^m3其中3八是校準(zhǔn)前的原始測(cè)量值，由于芯片生產(chǎn)和裝配的因素該值一般都\bf3%會(huì)有零點(diǎn)偏移誤差，在公式中用枚=%來(lái)表示零點(diǎn)偏移補(bǔ)償，此外還3 3〉brl3Z」有量程比例的縮放導(dǎo)致的尺度準(zhǔn)確性，公式中用尺度因子Sg=Sax 0 00 s 0來(lái)補(bǔ)償，最后由于安裝誤差和芯片內(nèi)部感應(yīng)器件軸的扭3V00s3Z」1納曲因素，還需要一個(gè)旋轉(zhuǎn)因子矩陣此=一紳 1』6 -J0陀螺儀傳感器的校準(zhǔn)就是要得到這幾個(gè)未知矩陣。陀螺儀的校正方法可以采用積分推算校準(zhǔn)方法?；诘脑砭褪怯捎谌S角速度的積分可以得到角度信息，因此使用已經(jīng)校準(zhǔn)的三軸加速度計(jì)信息得到的角度信息可以用來(lái)校準(zhǔn)陀螺儀的未知校準(zhǔn)參數(shù)。積分推算校準(zhǔn)的主要思路就是采用根據(jù)陀螺儀的包含校準(zhǔn)參數(shù)的數(shù)據(jù)轉(zhuǎn)換為四元數(shù)旋轉(zhuǎn)矢量迭代到上一時(shí)刻已校準(zhǔn)過(guò)的三軸加速度計(jì)，獲得旋轉(zhuǎn)的角度進(jìn)而獲得更新時(shí)刻的加速度值，該加速度值與新時(shí)刻的校準(zhǔn)加速度數(shù)據(jù)的方差應(yīng)該達(dá)到最小。該方差值達(dá)到最小值時(shí)的各參量矩陣的數(shù)值即為校準(zhǔn)值。其中最簡(jiǎn)單的校準(zhǔn)方法為：找到三軸陀螺儀的中心零點(diǎn)的位置。即需要將裝配了陀螺儀的飛控板水平放置，Z軸朝正下方，此時(shí)理論上X、Y與Z軸的旋轉(zhuǎn)矢量均為0。這時(shí)X、Y與Z軸所讀到的數(shù)據(jù)近似認(rèn)為是零點(diǎn)漂移，這種方法其實(shí)是忽略了三軸旋轉(zhuǎn)誤差并將這些誤差近似到零點(diǎn)飄移中。磁力計(jì)的原理? 磁力計(jì)指的是各種用來(lái)測(cè)量磁場(chǎng)的儀器，也稱(chēng)磁力儀、高斯計(jì)。飛行器的機(jī)頭朝向是指向哪個(gè)方位，這個(gè)狀態(tài)量稱(chēng)為航向，也是飛行器三維姿態(tài)信息中的一維。磁力計(jì)通過(guò)測(cè)量大地的磁場(chǎng)強(qiáng)度可以獲得載體的航向信息、。磁力計(jì)之所以由于能夠獲得航向信息是因?yàn)榈厍蚩臻g周?chē)兄艌?chǎng)，磁場(chǎng)強(qiáng)度大約為0.5?0.6高斯，磁力計(jì)可以測(cè)量出穿過(guò)大地的三維地磁強(qiáng)度，從而獲得載體相對(duì)于地磁線(xiàn)的偏轉(zhuǎn)方位。d^pgrfephk.NorthMeNorthM^HneEk6d^pgrfephk.NorthMeNorthM^HneEk6滬鮮這種磁力計(jì)采用各向異性的磁致電阻(AnisotropicMagneto-Resistance)材料來(lái)檢測(cè)空間中磁感應(yīng)強(qiáng)度的大小。這種具有晶體結(jié)構(gòu)的合金材料對(duì)外界的磁場(chǎng)非常敏感，磁場(chǎng)的強(qiáng)弱變化會(huì)導(dǎo)致AMR電阻值發(fā)生變化。在制造過(guò)程中，將一個(gè)強(qiáng)磁場(chǎng)加在AMR上使其在某一方向上磁化，建立起一個(gè)主磁域，與主磁域垂直的軸被稱(chēng)為該AMR的敏感軸；為了使測(cè)量結(jié)果以線(xiàn)性的方式變化，AMR材料上的金屬導(dǎo)線(xiàn)呈45°角傾斜排列，電流從這些導(dǎo)線(xiàn)上流過(guò)；由初始的強(qiáng)磁場(chǎng)在AMR材料上建立起來(lái)的主磁域和電流的方向有45°的夾角。AMR材料示意圖AMR材料示意圖角排列的導(dǎo)線(xiàn).當(dāng)有外界磁場(chǎng)Ha時(shí)，AMR上主磁域方向就會(huì)發(fā)生變化而不再是初始的方向了，那么磁場(chǎng)方向和電流的夾角。也會(huì)發(fā)生變化。對(duì)于AMR材料來(lái)說(shuō)，。角的變化會(huì)引起AMR自身阻值的變化，并且呈線(xiàn)性關(guān)系。磁力計(jì)中再利用惠斯通電橋檢測(cè)AMR阻值的變化，如圖5-13所示。R1/R2/R3/R4是初始狀態(tài)相同的AMR電阻，但是R1/R2和R3/R4具有相反的磁化特性。當(dāng)檢測(cè)到外界磁場(chǎng)的時(shí)候，R1/R2阻值增加AR而R3/R4減少AR。這樣在沒(méi)有外界磁場(chǎng)的情況下，電橋的輸出為零；而在有外界磁場(chǎng)時(shí)電橋的輸出為一個(gè)微小的電壓AV。當(dāng)R1=R2=R3=R4=R，在外界磁場(chǎng)的作用下電阻變化為AR時(shí)，電橋輸出AV正比于AR。I. C磁力計(jì)的測(cè)量信息雖然大地磁場(chǎng)強(qiáng)度在除了南北磁極的地方是垂直于大地水平面，而在其他位置都是平行于水平面，但磁力計(jì)一般都為三軸磁力計(jì)，因?yàn)樵谑褂眠^(guò)程中，我們不能保證磁力計(jì)完全水平，而三軸磁力計(jì)就可以同時(shí)測(cè)量慣性系三維空間的三個(gè)方向上的磁力磁力計(jì)原始數(shù)據(jù)采集HMC5883L包括最先進(jìn)的高分辨率HMC118X系列磁阻傳感器，并附帶霍尼韋爾專(zhuān)利的集成電路包括放大器、自動(dòng)消磁驅(qū)動(dòng)器、偏差校準(zhǔn)、能使羅盤(pán)精度控制在1°?2°的12位模數(shù)轉(zhuǎn)換器，簡(jiǎn)易的I2C系列總線(xiàn)接口。磁力計(jì)校準(zhǔn)地球磁場(chǎng)的強(qiáng)度本身極其微弱，只有0.5?0.6高斯，它很容易受到外界金屬及電磁設(shè)備的干擾和影響，甚至機(jī)載的電機(jī)旋轉(zhuǎn)的過(guò)程中都會(huì)對(duì)磁場(chǎng)產(chǎn)生強(qiáng)烈的干擾。因此磁力計(jì)是一個(gè)極易受干擾的脆弱的傳感器件。在很多應(yīng)用于電力巡線(xiàn)等復(fù)雜電磁場(chǎng)環(huán)境應(yīng)用場(chǎng)合的大型工業(yè)無(wú)人機(jī)的設(shè)備中，甚至采用雙

GPS來(lái)提取機(jī)頭航向信息，而避免使用電子羅盤(pán)的信息。因此在常用的機(jī)載磁力計(jì)傳感器上，必須要針對(duì)磁力計(jì)進(jìn)行校準(zhǔn)，以避免對(duì)慣導(dǎo)算法造成太大的影響。如果不存在外部磁場(chǎng)干擾的情況下，傳感器只應(yīng)該能夠感應(yīng)到地球磁場(chǎng)，由于地球磁場(chǎng)是一個(gè)恒定方向(指北)，那么如果將傳感器水平旋轉(zhuǎn)一周的時(shí)候，地球磁場(chǎng)矢量將均勻分布在X,Y兩軸上，形成一個(gè)標(biāo)準(zhǔn)的圓形，如果將傳感器按空間各方向旋轉(zhuǎn)360度的情況下，地球磁場(chǎng)矢量將均勻分布在X,Y,Z三軸上，形成一個(gè)標(biāo)準(zhǔn)的三維圓球。校準(zhǔn)之前數(shù)據(jù)擬合校準(zhǔn)之后數(shù)據(jù)擬合校準(zhǔn)之前數(shù)據(jù)擬合校準(zhǔn)之后數(shù)據(jù)擬合1.7.1磁力計(jì)校準(zhǔn)原理首先建立數(shù)值模型，定義傳感器測(cè)量值1.7.1磁力計(jì)校準(zhǔn)原理首先建立數(shù)值模型，定義傳感器測(cè)量值m，=[xmymZm"，校準(zhǔn)之后的值為m=[xcsmx為m=[xcsmx[000smy0yc Zc]T，圓心偏移參數(shù)b，=[bx00]。smZbybZ]T，比例尺度參數(shù)Sm他們之間的關(guān)系如下所示m=Sm(m，+b，)(x=(x+b),scmxmxyc=(ym+by)"myZc=(Zm+bz),smz校正后的校正值應(yīng)該近似分布在一個(gè)圓球上，圓球的公式為：x2+y2+z2=R2，所以我們將校正值帶入圓球公式，與理論的圓球半徑求方差，構(gòu)建誤差u=x2+y2+Z2—R2c,cc將校正值用測(cè)量值替換：u=s2x2+b2s2+2xbs2+s2y2+b2s2+2ybs2+s2Z2+b2s2mxmxmxmxmxmymymymymymZmzmZ+2zbs2—R2mzmz可以看到，形態(tài)類(lèi)似橢圓公式V=[x2y2Z2xyz 1]tm"mmm"mm

P=[abcdefg]T

a=s2mxb=S2my其中C=S2mzd=2bs2其中xmxe=2bs2ymyf=2bs2zmzg=bxsmx+b2smy+b2smz-R2則誤差計(jì)算為：u=axm+bym+czm+dxm+eym+fzm+g=VtxP這里的W是對(duì)于所有的采樣測(cè)試點(diǎn)求和。u是一個(gè)二次函數(shù)，其有極小值點(diǎn)。通過(guò)將u的函數(shù)對(duì)于所有變量求偏導(dǎo)，并計(jì)算偏導(dǎo)為0時(shí)，u可取得最小極值點(diǎn)，即1.9磁力計(jì)傳感器定義W（^x〃）=｛（^X^7X尸）=（W^X^7）X尸=3X尸=0其中，B是測(cè)量點(diǎn)矩陣，P為恒定的待求參數(shù)矩陣，因此可以通過(guò)高斯消元法求齊次線(xiàn)性方程組，來(lái)求得P的各個(gè)參數(shù)的解可以將上式改寫(xiě)成3xkP=0,其中k是一個(gè)任意常數(shù)即P=k4=k4M"kqMdmqk/M%]T通過(guò)解線(xiàn)性方程組求得的只是這個(gè)解系中的一個(gè)基本解，所以首先求出這個(gè)基本解的k值。經(jīng)算是可以推導(dǎo)如下，帶入a,b,c,d,e,f,g即可求解：（上+M+匯-4*g）7 'a萬(wàn)c?）4A2k= 4A2求得的最后解為:

mxmx該校準(zhǔn)方法運(yùn)算量較大，編程較為復(fù)雜。當(dāng)在一定精度上確保傳感器水平時(shí)，可以采用簡(jiǎn)化的方法來(lái)進(jìn)行校準(zhǔn)，即只校準(zhǔn)水平方向上的量。此外由于飛控算法僅需要磁力計(jì)的矢量方向信號(hào)對(duì)四元數(shù)進(jìn)行矩陣，因此實(shí)際使用時(shí)都將矢量歸一化，因此比例因素可以忽略。超聲波傳感器原理人耳朵能聽(tīng)到的聲波頻率為20?20000Hz，當(dāng)聲波的震動(dòng)頻率大于20000Hz時(shí)，人耳無(wú)法聽(tīng)到。因此，我們把頻率高于20000赫茲的聲波稱(chēng)為超聲波特點(diǎn)：超聲波的波長(zhǎng)相對(duì)來(lái)說(shuō)比聲波要短，通常的障礙物都會(huì)比超聲波的波長(zhǎng)大很多，所以說(shuō)超聲波的衍射能力不是很強(qiáng)，在介質(zhì)一定密度不變的情況下，超聲波能夠沿著波的方向一致沿直線(xiàn)傳波，超聲波的波長(zhǎng)相對(duì)來(lái)說(shuō)越短的話(huà)，直射能力就越好超聲波傳感器簡(jiǎn)介在制作無(wú)人機(jī)的時(shí)候，需要實(shí)時(shí)采集有關(guān)自身位置的相關(guān)數(shù)據(jù)，雖然可以通過(guò)GPS來(lái)采集有關(guān)無(wú)人機(jī)相關(guān)位置的數(shù)據(jù)，但是一旦有建筑甚至是樹(shù)木