基于加速度信息修正的四旋翼位置估計算法研究*

方家豪，葉 欣，董 偉，盛鑫軍，朱向陽

（上海交通大學(xué)機(jī)械系統(tǒng)與振動國家重點實驗室，上海200240）

基于加速度信息修正的四旋翼位置估計算法研究*

方家豪，葉 欣，董 偉，盛鑫軍*，朱向陽

（上海交通大學(xué)機(jī)械系統(tǒng)與振動國家重點實驗室，上海200240）

針對單一的微傳感器無法準(zhǔn)確進(jìn)行四旋翼無人機(jī)空間定位的問題，設(shè)計了一種多元信息融合的互補(bǔ)濾波算法用于無人機(jī)空間位置估計。該算法的核心思想為利用一類通用的輔助傳感器如氣壓傳感器，全球定位系統(tǒng)（GPS）以及微基站的測量信息對加速度傳感器的測量信息進(jìn)行實時修正，然后利用修正后的加速度信息積分估計四旋翼無人機(jī)的空間位置。本文在自主研制的飛行控制平臺上驗證了這種互補(bǔ)濾波算法的有效性。通過對比實驗驗證，利用本文設(shè)計的互補(bǔ)濾波算法可以使得無人機(jī)運動速度估計值以及空間位置估計值無偏差的收斂。飛行實驗證明，將該互補(bǔ)濾波算法輸出的速度以及位置估計信息應(yīng)用于位置控制器中，可以實現(xiàn)無人機(jī)穩(wěn)定的位置控制。

多元信息融合；位置估計；互補(bǔ)濾波；加速度傳感器；氣壓傳感器；GPS

近年來隨著各類廉價的高性能微控制器以及微傳感器系統(tǒng)的興起，四旋翼無人機(jī)UAV（Unmanned Aerial Vehicle）的研究也得到國內(nèi)外研究機(jī)構(gòu)的廣泛重視［1］。考慮到四旋翼無人機(jī)的控制特點，為了實現(xiàn)無人機(jī)準(zhǔn)確的位置控制需要實時地獲得無人機(jī)當(dāng)前的運動狀態(tài)，其中包括了無人機(jī)當(dāng)前的空間運動速度信息以及空間位置信息［2］。這兩項信息的獲得通常依賴于3類傳感器：MEMS（Micro Electro-Mechanical System）加速度傳感器，氣壓傳感器以及GPS。然而這三類傳感器信號都存在固有的缺陷，單一的傳感器信號無法直接應(yīng)用于無人機(jī)的位置控制［3］。因此需要通過合適的算法融合這三類傳感器信號，達(dá)到互相補(bǔ)償各自信號缺陷的目的。

近些年來，針對于這幾類傳感器信號的信息融合問題，國內(nèi)外許多研究機(jī)構(gòu)開展了大量的研究工作。在傳感器信號分析方面，文獻(xiàn)［4-5］中提到多種影響MEMS加速度傳感器信號質(zhì)量的因素，其中包括了傳感器設(shè)計與加工工藝等內(nèi)在因素以及傳感器工作環(huán)境等外在因素。在文獻(xiàn)［6］中提到，GPS傳感器測量的位置信息存在時間的滯后，這種測量的滯后會影響到組合導(dǎo)航的精度。在空間位置估計算法方面，目前公開的算法主要是基于卡爾曼濾波的思想［7-9］。在文獻(xiàn)［7］中使用了一種擴(kuò)展卡爾曼濾波（EKF）算法用于慣性傳感器與GPS傳感器的信息融合。相比于使用單獨的GPS測量值作為位置估計值，通過該EKF算法有效地減小了位置估計值的噪聲。同時，在文獻(xiàn)［8］中使用了一種SPKF（Sigma-Point Kalman Filters）算法，和EKF算法相比，該SPKF算法減少了30%的位置估計誤差。在文獻(xiàn)［9］中使用了一種UKF（Unscented Kalman Filter）算法，相比于EKF算法，該UKF算法的位置估計均方根（RMS）值更小，位置估計精度更高。

盡管上述的這些基于卡爾曼濾波的位置估計算法都在仿真環(huán)境中或者商業(yè)平臺上驗證了算法的有效性，但是這一類算法并不適用于廉價的微控制系統(tǒng)中。這些算法的運算量很大，對執(zhí)行算法的控制器有很高的性能要求。在廉價的微控制系統(tǒng)中執(zhí)行這類算法時，并不能達(dá)到文獻(xiàn)中提及的更新頻率，從而也無法保證位置估計的精度。因此，在工程應(yīng)用中，這一類算法的效果有限。

考慮到實際的工程應(yīng)用，本文使用了一種適用于廉價微控制器的互補(bǔ)濾波算法用于無人機(jī)空間位置的估計。相比于卡爾曼濾波算法，該互補(bǔ)濾波算法迭代簡單，更適用于工程實踐。該算法的核心思想為實時地獲得加速度傳感器測量誤差的無偏估計值，從而對加速度傳感器的測量值進(jìn)行動態(tài)修正，最后利用修正后的加速度信息進(jìn)行積分估計無人機(jī)的空間位置。這種算法具有很強(qiáng)的通用性，不僅僅適用于解決氣壓傳感器以及GPS與加速度傳感器的信息融合問題，同樣也適用于其它具有類似信號特點的傳感器與加速度傳感器的信息融合。在本文的第五節(jié)中將討論一種利用該互補(bǔ)濾波算法融合微基站與加速度傳感器的信息進(jìn)行無GPS環(huán)境下的無人機(jī)空間定位問題的解決方案。

本文的內(nèi)容將以如下順序介紹，第二節(jié)介紹了無人機(jī)的連體坐標(biāo)系與慣性坐標(biāo)系的定義。第三節(jié)介紹了本文設(shè)計的一款無人機(jī)飛行控制平臺的硬件結(jié)構(gòu)，同時對該平臺連接的相關(guān)傳感器信號特點進(jìn)行了分析。第四節(jié)介紹了針對這些傳感器信號特點所設(shè)計的一種用于空間位置估計的互補(bǔ)濾波算法。實驗與結(jié)果在第五節(jié)中進(jìn)行簡要的討論。最后一節(jié)是對本文所有內(nèi)容的總結(jié)。

1 坐標(biāo)系定義

本節(jié)主要介紹了四旋翼無人機(jī)的連體坐標(biāo)系與慣性坐標(biāo)系的定義以及坐標(biāo)系的轉(zhuǎn)化關(guān)系。如圖1定義兩個坐標(biāo)系，坐標(biāo)系{B}定義為無人機(jī)的連體坐標(biāo)系。坐標(biāo)系{G}定義為固連在地面上的慣性坐標(biāo)系。慣性坐標(biāo)系的原點O定義為無人機(jī)解鎖時刻所處的空間位置。地理正東方向定義為慣性坐標(biāo)系的X軸，地理正北方向定義為慣性坐標(biāo)系的Y軸，豎直向上定義為慣性坐標(biāo)系的Z軸。

圖1 坐標(biāo)系定義

無人機(jī)的三個姿態(tài)角定義如下，以繞著體坐標(biāo)系X軸旋轉(zhuǎn)定義為橫滾角?。以繞著體坐標(biāo)系Y軸旋轉(zhuǎn)定義為俯仰角θ。以繞著體坐標(biāo)系Z軸旋轉(zhuǎn)定義為偏航角ψ。據(jù)此得到從慣性坐標(biāo)系到連體坐標(biāo)系的旋轉(zhuǎn)矩陣R

其中s為sin運算，c為cos運算。

2 傳感器信號分析

本文自主研制了一款四旋翼飛行控制硬件平臺。圖2為該硬件平臺以及其傳感器的硬件連接關(guān)系框圖，下文所有的實驗都是基于這個硬件平臺完成的。該硬件平臺使用一款廉價的微處理器AT91SAM7S256芯片作為微控制單元（MCU）。這一型號的芯片不具備當(dāng)前先進(jìn)控制芯片常具有的硬件浮點數(shù)運算單元（FPU），也不支持?jǐn)?shù)字信號處理（DSP）指令集。其較弱的運算處理能力提高了對算法簡潔性的要求。

圖2 四旋翼飛行控制器硬件系統(tǒng)

2.1 加速度傳感器信號分析

主芯片通過集成電路總線接口（IIC）連接MPU9150型號的MEMS慣性傳感器。加速度傳感器測量信號的數(shù)學(xué)模型可以表達(dá)為

其中aIMU為加速度的測量值，R為旋轉(zhuǎn)矩陣，a為加速度的真實值，g為重力加速度，ba為測量的靜態(tài)偏差，ηa為測量的隨機(jī)噪聲。

圖3為靜止?fàn)顟B(tài)下，該加速度傳感器分別在圖1所示的體坐標(biāo)系{B}的X，Y，Z三軸方向上測量得到的原始加速度數(shù)據(jù)。實驗結(jié)果顯示該加速度傳感器的測量隨機(jī)噪聲的大小為0.05 m/s2。同時該加速度傳感器信號測量的靜態(tài)偏差因各個軸而異。在文獻(xiàn)［10］中提到即使對加速度傳感器進(jìn)行合理的校正也無法完全消除測量的靜態(tài)偏差。因此直接利用加速度傳感器積分估計無人機(jī)的空間位置，估計值的誤差將隨著時間而逐步累積［11］。

圖3 加速度傳感器X，Y，Z三軸原始加速度數(shù)據(jù)

2.2 氣壓傳感器信號分析

主控制芯片通過串行外設(shè)接口（SPI）連接MS5611型號的氣壓傳感器。氣壓與相對高度的轉(zhuǎn)換關(guān)系為

其中P為當(dāng)前高度的氣壓測量值，RP為高度零點的氣壓值，H為相對高度值。氣壓傳感器測量信號的數(shù)學(xué)模型表達(dá)為

其中Hbaro為當(dāng)前氣壓傳感器測量高度值，H為當(dāng)前的真實高度，bbaro為氣壓傳感器的時變漂移，ηbaro為隨機(jī)測量噪聲。如圖4所示，該氣壓傳感器的隨機(jī)測量噪聲為0.5 m。

圖4 氣壓傳感器測量壓強(qiáng)及對應(yīng)測量高度

表1 傳感器信號特點

2.3 GPS信號分析

主控制芯片通過串口（USART）連接M8N型號的GPS模塊，通過配置修改該模塊的測量更新頻率為5 Hz。GPS信號測量的數(shù)學(xué)模型可以表達(dá)為

其中PGPS(t)為當(dāng)前時刻GPS傳感器的測量值。假設(shè)GPS測量值存在一個dt大小的時間滯后［6］，所以將P(t-dt)定義為dt時刻之前無人機(jī)所處的真實位置。bGPS(t)為GPS傳感器測量的時變漂移。

圖5為靜止?fàn)顟B(tài)下，在空曠的室外環(huán)境中該GPS模塊測量得到的當(dāng)前無人機(jī)所處位置的原始經(jīng)緯度數(shù)據(jù)。將該經(jīng)緯度測量數(shù)據(jù)投影到X-Y平面坐標(biāo)系中得到如圖6所示的位置分布。投影后的位置數(shù)據(jù)顯示該GPS模塊在X軸與Y軸方向上的測量精度為2 m。同時由于無線通信以及測量與計算的時間要求，GPS模塊輸出的測量信息通常存在時間滯后。

最后，表1總結(jié)了這3類傳感器的信號的特點。

圖5 靜止?fàn)顟B(tài)下GPS模塊測量經(jīng)緯度數(shù)據(jù)

圖6 X-Y平面投影圖

3 多元信息融合算法

根據(jù)上一小節(jié)中所述的三類傳感器信號的特點，本小節(jié)設(shè)計了一種基于互補(bǔ)濾波器的多元信息融合算法用于估計四旋翼無人機(jī)的相對高度以及水平位置。

3.1 高度估計算法

在慣性坐標(biāo)系的Z軸方向上，采用融合氣壓傳感器以及加速度傳感器的數(shù)據(jù)進(jìn)行相對高度的估計。圖7為該方向上傳感器數(shù)據(jù)融合算法框圖。

圖7 高度方向傳感器數(shù)據(jù)融合算法框圖

圖7所示的高度估計算法可以描述為：

加速度傳感器以500 Hz的頻率更新得到運動加速度測量值au，減去加速度的靜態(tài)誤差估計值，得到修正后的運動加速度值。修正后的運動加速度值積分得到速度的預(yù)測值，減去當(dāng)前速度的比例修正值，得到修正后的速度估計值。積分得到高度的預(yù)測值，減去當(dāng)前高度的比例修正值，得到修正后的高度估計值。氣壓傳感器更新測量得到測量高度值z，更新下一時刻速度的比例修正值，高度的比例修正值。同時利用積分環(huán)節(jié)計算下一時刻加速度的靜態(tài)誤差值修正量，積分更新下一時刻的加速度靜態(tài)誤差。該算法數(shù)學(xué)表達(dá)式可以表達(dá)為：

其中Kp，Kvp，Ki為比例以及積分修正系數(shù)值，修正系數(shù)在實驗調(diào)試過程中調(diào)整至最佳值，參數(shù)選擇依據(jù)參考第4.1小節(jié)。

3.2 水平位置估計算法

GPS/MEMS的傳感器組合導(dǎo)航是四旋翼無人機(jī)獲得位置信息的一種重要方法［12］。因此，本文采用融合GPS以及MEMS加速度傳感器的信號進(jìn)行水平位置的估計。圖8為在慣性坐標(biāo)系的X軸與Y軸方向上的信息融合算法框圖，X軸與Y軸方向上算法一致。

圖8 水平方向傳感器數(shù)據(jù)融合算法框圖

圖8所示的水平位置估計算法可以描述為：

加速度傳感器更新測量得到運動加速度測量值au，減去

其中Kpr，Kpv與Kir，Kiv分別為比例以及積分修正系數(shù)，修正系數(shù)在實驗調(diào)試過程中調(diào)整至最佳值，參數(shù)選擇依據(jù)參考第4.1小節(jié)。

4 實驗與結(jié)果

本節(jié)首先進(jìn)行一組對比實驗，比較無人機(jī)在是否執(zhí)行上述互補(bǔ)濾波算法的不同情況下的位置估計值。然后，為了證明上述互補(bǔ)濾波算法的有效性，本節(jié)將討論利用上述互補(bǔ)濾波算法進(jìn)行室外飛行測試的結(jié)果。最后，為了證明該算法的通用性，在本節(jié)的末尾將討論一種利用上述互補(bǔ)濾波算法融合微基站與加速度傳感器的信息進(jìn)行無GPS環(huán)境下無人機(jī)定位問題的解決方案。

4.1 算法驗證

為了驗證上述算法的有效性，本文設(shè)計了一個對比實驗比較3種不同的高度預(yù)測算法。在該對比實驗中，無人機(jī)都處于靜止?fàn)顟B(tài)下。算法1僅僅依靠加速度傳感器進(jìn)行積分預(yù)測高度。算法2在算法1的基礎(chǔ)上添加了利用氣壓傳感器對高度預(yù)測值的比例修正。算法3為本文所采用的互補(bǔ)濾波算法，在算法2的基礎(chǔ)上添加了利用氣壓傳感器對高度預(yù)測值的積分修正。在算法3中為了獲得最優(yōu)的位置以及速度估計值，需要對比例參數(shù)以及積分參數(shù)值進(jìn)行調(diào)整。參數(shù)的調(diào)整依據(jù)為：最優(yōu)參數(shù)應(yīng)使得位置估計值以及速度估計值無偏差的收斂，同時應(yīng)避免引入超調(diào)和震蕩。上述對比實驗的實驗結(jié)果如圖9、圖10所示。

圖9 3種算法得到的高度估計值

圖10 3種算法得到的運動速度估計值

上述實驗結(jié)果證明，由于加速度傳感器數(shù)據(jù)存在測量的靜態(tài)誤差，在僅僅利用加速度傳感器進(jìn)行積分無修正的情況下，位置估計值以及速度估計值將隨著時間而逐步發(fā)散。在引入了氣壓傳感器數(shù)據(jù)的比例修正項之后估計值逐步收斂，但是存在明顯的穩(wěn)態(tài)誤差，因此還需要引入積分修正項，積分修正項的作用為消除這個穩(wěn)態(tài)誤差。在使用積分環(huán)節(jié)后，隨著時間的增加，誤差的累積項會增大，從而可以推動對加速度測量誤差的進(jìn)一步修正，進(jìn)而通過對修正后的加速度進(jìn)行積分，可以消除估計值的穩(wěn)態(tài)誤差。

4.2 飛行測試（Flight Test）

由于準(zhǔn)確的位置估計是實現(xiàn)無人機(jī)位置控制的基礎(chǔ)［2］，所以本文采用飛行控制實驗驗證該空間位置預(yù)測算法的有效性。本文利用自主研制的控制器搭建了如圖11的測試平臺，該測試平臺總重量 0.905 kg。控制算法采用常用的一組串級PID控制器。其中包含了一個位置控制器，一個速度控制器，一個角度控制器以及一個角速度控制器［13］。

圖11 飛行測試平臺

為了驗證該位置估計算法的實時性與有效性，在室外環(huán)境中分別進(jìn)行高度控制以及位置控制飛行測試。測試實驗的結(jié)果如圖 12～圖 14所示。

圖12 目標(biāo)高度追蹤效果

如圖12所示，實線為利用本文的高度估計算法融合了加速度傳感器以及氣壓傳感器數(shù)據(jù)估計到的無人機(jī)當(dāng)前所處高度，虛線為目標(biāo)高度。實驗結(jié)果證明，基于本文的高度估計算法，利用絕對精度為0.5 m氣壓傳感器，可以實現(xiàn)四旋翼飛行器在高度方向上0.2 m的控制精度。

圖13 X方向目標(biāo)位置追蹤效果

圖14 Y方向目標(biāo)位置追蹤效果

如圖13、圖14所示，實線為基于上述的水平位置估計算法分別在慣性坐標(biāo)系的X軸以及Y軸方向上估計到的飛行器的水平位置，虛線為在這兩個方向上的目標(biāo)位置。實驗結(jié)果證明，基于本文的多元信息融合的水平位置估計算法，利用絕對精度為2 m的GPS傳感器，可以實現(xiàn)四旋翼飛行器在水平方向上1 m的控制精度。

4.3 微基站系統(tǒng)

在無人機(jī)的眾多應(yīng)用場景中不可避免會遇到在無GPS環(huán)境下如何進(jìn)行空間定位的問題。如前文所述，僅僅依靠加速度傳感器估計的位置信息是不可信的，因此在無GPS的環(huán)境下也需要一類輔助的定位傳感器。本文提出的一種解決方案為使用一類微基站定位系統(tǒng)作為輔助的定位系統(tǒng)。如圖15、圖16所示，該微基站系統(tǒng)由兩部分組成，第一部分為安裝在地面的基站系統(tǒng)，采用4個基站為一組安裝在一個矩形區(qū)域的四個頂點位置。第二部分為無人機(jī)機(jī)載的定位標(biāo)簽，該定位標(biāo)簽可以通過與地面基站系統(tǒng)的通信計算出當(dāng)前標(biāo)簽所處的空間位置。同時，該機(jī)載標(biāo)簽通過串口與無人機(jī)的控制器進(jìn)行定位數(shù)據(jù)的通信。

圖15 地面基站

圖16 機(jī)載標(biāo)簽

圖17 微基站系統(tǒng)X-Y平面定位信息

如圖17所示，在實際測試中發(fā)現(xiàn)，該微基站系統(tǒng)輸出的定位信息存在一定的測量噪聲，在X與Y軸上的定位精度分別為0.5 m。這樣的定位信息無法直接應(yīng)用于無人機(jī)的位置控制器。然而，由于微基站的定位信息與GPS的定位信息有許多共性，因此可以利用本文的互補(bǔ)濾波器融合微基站與加速度傳感器的信息進(jìn)行無GPS環(huán)境下的無人機(jī)空間定位。

如圖18、圖19所示，分別為X與Y方向上原始的微基站測量數(shù)據(jù)與利用本文的互補(bǔ)濾波算法輸出的濾波數(shù)據(jù)。實驗結(jié)果顯示，利用本文的互補(bǔ)濾波算法有效的抑制了原始測量數(shù)據(jù)中的高頻噪聲。

圖18 微基站X方向測量數(shù)據(jù)與濾波數(shù)據(jù)

圖19 微基站Y方向測量數(shù)據(jù)與濾波數(shù)據(jù)

5 結(jié)論

根據(jù)四旋翼無人機(jī)位置控制器的要求，本文設(shè)計了一種適用于廉價微控制器的互補(bǔ)濾波算法用于無人機(jī)運動速度以及空間位置的估計。為了驗證該算法的有效性，本文以一塊不具備FPU單元，不支持DSP指令集的廉價微控制芯片作為主控制單元，自主研制了一款四旋翼飛行控制平臺。通過對該平臺所連接的傳感器信號特點的分析，本文設(shè)計了一種簡單有效的互補(bǔ)濾波算法用于融合這些傳感器的信號。通過一系列的實驗驗證，本文得出了如下結(jié)論：

①通過比較不同的位置估計算法證明，利用本文所設(shè)計的互補(bǔ)濾波算法可以實時修正加速度傳感器的測量誤差，從而使得空間位置估計值無偏差的收斂。

②由于準(zhǔn)確的位置估計是實現(xiàn)無人機(jī)位置控制的基礎(chǔ)，所以本文采用室外飛行控制實驗證明該位置估計算法的有效性。實驗結(jié)果顯示采用本文的互補(bǔ)濾波算法，使用垂直方向上0.5 m測量精度的廉價氣壓傳感器以及水平方向上2 m測量精度的民用GPS，可以實現(xiàn)無人機(jī)垂直方向上0.2 m，水平方向上1 m的控制精度。

③此類互補(bǔ)濾波算法具有很強(qiáng)的通用性，不僅僅適用于解決氣壓傳感器以及GPS與加速度傳感器的信息融合問題，也適用于其它具有類似信號特點的傳感器與加速度傳感器融合的組合導(dǎo)航問題。在未來的工程應(yīng)用中，該互補(bǔ)濾波算法可以進(jìn)一步擴(kuò)展應(yīng)用于微基站系統(tǒng)與加速度傳感器的組合定位問題，從而可以解決無GPS環(huán)境下的無人機(jī)空間定位問題。

［1］陳海濱，殳國華.四旋翼飛行器的設(shè)計［J］.實驗室研究與探索，2013，03：41-44.

［2］Mahony R，Kumar V，Corke P.Multirotor Aerial Vehicles：Model?ing，Estimation，and Control of Quadrotor［J］.IEEE Robotics& Automation Magazine，2012，19（3）：20-32.

［3］Tailanian M，Paternain S，Rosa R，et al.Design and Implemen?tation of Sensor Data Fusion for an Autonomous Quadrotor［C］//Instrumentation and Measurement Technology Conference（I2MTC）Proceedings，2014 IEEE International.IEEE，2014：1431-1436.

［4］李榮冰，劉建業(yè)，段方，等.MEMS-INS微型飛行器姿態(tài)確定系統(tǒng)的實現(xiàn)研究［J］.應(yīng)用科學(xué)學(xué)報，2006（6）：618-622.

［5］于婷，孫偉，文劍.MEMS慣性組件的誤差特性分析與標(biāo)定［J］.傳感技術(shù)學(xué)報，2016，29（6）：859-864.

［6］楊波，王躍鋼，徐洪濤.高速飛行環(huán)境下衛(wèi)星導(dǎo)航信息滯后補(bǔ)償方法［J］.中國慣性技術(shù)學(xué)報，2011，19（3）：293-297.

［7］Mammarella M，Campa G，Napolitano M R，et al.Machine Vision/GPS Integration Using EKF for the UAV Aerial Refueling Problem［J］.Systems，Man，and Cybernetics，Part C：Applications and Re?views，IEEE Transactions on，2008，38（6）：791-801.

［8］Van Der Merwe R，Wan E A.Sigma-Point Kalman Filters for Inte?grated Navigation［C］//Proceedings of the 60th Annual Meeting of the Institute of Navigation（ION）.2004：641-654.

［9］Oh S M，Johnson E N.Development of UAV Navigation System Based on Unscented Kalman Filter［C］//AIAA Guidance，Naviga?tion，and Control Conference and Exhibit.Paper Number AIAA-2006-6763，Keystone，Colorado.2006.

［10］Lim C H，Lim T S，Koo V C.A MEMS Based，Low Cost GPS-Aid?ed INS for UAV Motion Sensing［C］//Advanced Intelligent Mecha?tronics（AIM），2014 IEEE/ASME International Conference on.IEEE，2014：576-581.

［11］李耀軍，潘泉，趙春暉，等.基于INS/SMNS緊耦合的無人機(jī)導(dǎo)航［J］.中國慣性技術(shù)學(xué)報，2010（3）：302-306.

［12］蔡磊，牛小驥，張?zhí)嵘?，?MEMS慣導(dǎo)在旋翼飛行器中導(dǎo)航性能的實驗分析［J］.傳感技術(shù)學(xué)報，2016，29（5）：711-715.

［13］Dong W，Gu G Y，Zhu X，et al.Modeling and Control of a Quadro?tor UAV with Aerodynamic Concepts［J］.World Academy of Sci?ence，Engineering and Technology，2013，7：377-382.

方家豪（1992-），男，浙江省衢州人?，F(xiàn)為上海交通大學(xué)機(jī)械與動力工程學(xué)院碩士研究生。主要從事四旋翼無人機(jī)飛行控制系統(tǒng)開發(fā)相關(guān)的研究，fangjia?hao@sjtu.edu.cn；

盛鑫軍（1978-），男，江蘇省江陰人。分別于2000年，2003年，2014年于上海交通大學(xué)獲得學(xué)士學(xué)位，碩士學(xué)位以及博士學(xué)位?，F(xiàn)為上海交通大學(xué)機(jī)器人研究所副研究員，長期從事無人機(jī)，智能機(jī)器人與生機(jī)電一體化的研究，xjsh?eng@sjtu.edu.c

朱向陽（1966-），男，江蘇省揚州人。1992年于東南大學(xué)獲得控制理論與應(yīng)用專業(yè)博士學(xué)位。2002年起擔(dān)任上海交通大學(xué)機(jī)械與動力工程學(xué)院教授，博士生導(dǎo)師?，F(xiàn)為上海交通大學(xué)機(jī)器人研究所所長，長期從事機(jī)器人運動規(guī)劃，人機(jī)接口以及生機(jī)電一體化方向的研究，mexyzhu@sjtu.edu.cn。

Quadrotor Position Estimation Algorithm Research Based on the Acceleration Correction*

FANG Jiahao，YE Xin，DONG Wei，SHENG Xinjun*，ZHU Xiangyang
（State Key Laboratory of Mechanical System and Vibration，Shanghai Jiaotong University，Shanghai 200240，China）

Single sensor couldn't provide accurate position estimation for the quadrotor.To solve this problem，a complementary filter based on multisensory data fusion is proposed to precisely estimate the position of the quadro?tor.The key point of this algorithm is to correct acceleration measurement with a class of auxiliary sensors such as the barometer，GPS and the micro base station.The complementary filter is validated by experiments on a quadrotor control platform.The result shows that，with this complementary filter，the speed estimation and the position estima?tion of the quadrotor is able to converge without steady error.Finally，outdoor flight test is carried out to prove that，with the position and speed estimation provided by the complementary filter，the quadrotor is capable of navigating through user-defined way-points.

multisensory data fusion；position estimation；complementary filter；accelerometer；barometer；GPS

TP212.9

A

1004-1699（2016）11-1684-08

EEACC：7230 10.3969/j.issn.1004-1699.2016.11.010

項目來源：上海張江國家自主創(chuàng)新示范區(qū)專項發(fā)展資金重點項目（201411-PD-JQ-B108-009）

2016-05-12 修改日期：2016-07-07