基于四旋翼無人機的單目建模系統

莊曉明，李 濤，付 龍

(中科院合肥物質科學研究院先進制造技術研究所,江蘇 常州 213164)

基于四旋翼無人機的單目建模系統

莊曉明，李 濤，付 龍

(中科院合肥物質科學研究院先進制造技術研究所,江蘇 常州 213164)

為了實現四旋翼無人機的自主飛行、環境單目稠密重建與即時導航功能，文中設計了該單目建模導航系統。飛行控制器采用MPU6050六軸傳感器采集六軸信號，通過算法擬合出飛行器姿態，然后通過STM32F103RBT6芯片分別控制4個電機驅動模塊,即時通過PID算法輸出PWM值進行調速來實現飛行姿態的穩定和改變。建模系統通過控制GPS模塊，可以獲取飛行器的實時位置；飛行器通過圖像采集模塊，可以獲取飛行器的環境信息。設計了上位機軟件，能夠接收圖像并且進行單目稠密重建。稠密重建主要由從運動到結構、基礎表面構建、約束場景流稠密重建、稠密對齊和局部模型集成共五方面組成，然后通過無線串口與無人機通訊，實現無人機的智能自主飛行。實驗結果表明，該系統能夠實現單目建模的功能。

四旋翼無人機；智能導航；三維重建；姿態控制

0 引 言

四旋翼無人機具有垂直起降、自由懸停的特點，并且體積小，操控方便，攜帶方便，可以進入人不易進入的復雜環境。目前，四旋翼無人機在航拍、娛樂、勘測、救災等惡劣環境中發揮著極其重要的作用[1]。

對于復雜不易進入的環境，更多時候可以使用無人機進入。無人機要具有自主飛行的功能，并且能對未知環境進行建模以方便外部人員觀察。傳統的四旋翼飛機需要使用遙控器進行位置和姿態控制，但無法感知環境的圖像細節和深度細節。傳統的三維重建方法主要有結構光方法和立體視覺方法，代表產品如Kinect體感器和Bumblebee相機等。

文中設計了一種基于從運動信息中恢復三維場景結構(structure from motion)的三維重建算法，通過使用單目相機采集圖像，傳輸至遠程電腦進行實時三維重建，四旋翼無人機根據三維環境結合IMU信息進行姿態控制，實現自主智能導航。

1 飛行器的基本工作原理

四旋翼飛行器是一種有四個旋翼并且旋翼呈十字形交叉的飛行器[2]。飛行器的機械結構簡單,旋翼對稱分布在機體的前、后、左、右四個方向，四個旋翼處于同一高度平面，且四個旋翼的結構和半徑都相同。四個電機對稱地安裝在飛行器的支架端，支架中間安放飛行控制計算機和外部設備。常見結構如圖1所示。

圖1 四旋翼飛行器的結構

四旋翼飛行器通過調節四個電機的轉速來改變旋翼轉速[3]，從而實現升力的變化，控制四旋翼飛行器的位姿。四旋翼飛行器是一種六自由度的垂直升降機，但只有四個輸入力，同時卻有六個狀態輸出，所以是一種典型的欠驅動系統。其主要飛行姿態如圖2所示。

(1)垂直運動：同時增加四個電機的旋轉速度使總拉力增大，當拉力克服整機的重量時，四旋翼飛行器就可以垂直上升;反之，同時減小四個電機的旋轉速度，四旋翼飛行器就垂直下降，直至平衡落地，實現了沿z軸的垂直運動。當外界擾動量為零時，當旋翼產生的升力等于飛行器的重力時，飛行器就能保持懸停。

(2)俯仰運動:使電機1的轉速上升，電機3的轉速下降，電機2和4的轉速保持不變。由于旋翼1的升力增大，旋翼3的升力減小，產生的不平衡力矩使機身繞y軸旋轉，實現飛行器的俯仰運動。

(3)滾轉運動：改變電機2和4的轉速，保持電機1和3的轉速不變，就可以使機身繞x軸旋轉，實現飛行器的滾轉運動。

(4)偏航運動：當電機1和3的轉速上升，電機2和4的轉速下降時，機身就在反扭矩的作用下繞z軸轉動，實現飛行器的偏航運動。

(5)前后運動：增加電機3的轉速，并相應減小電機1的轉速，同時保持其他兩個電機轉速不變，以保持反扭矩不變。飛行器就先發生一定程度的傾斜，使旋翼拉力產生水平分量，從而實現飛行器的前后運動。

(6)傾向運動：由于結構對稱，因此傾向飛行的工作原理與前后運動的原理一致。

2 系統整體設計

四旋翼飛行器的系統結構如圖3所示。

圖3 系統結構圖

GPS模塊和六軸傳感組成姿態測量系統，主控制器將姿態測量獲得的數據進行卡爾曼濾波得到當前姿態，再與從無線模塊發送過來的自主導航指令一起作為姿態控制器的輸入，姿態控制器通過PID算法輸出對應的PWM值[4]，驅動四個電調，進而改變4個電機的轉速實現姿態控制。

同時系統搭建了基于單目建模的SLAM系統，可以實時單目稠密重建并在PC端進行渲染和導航。

3 飛行控制系統設計

四旋翼飛行器能夠沿著規劃的軌跡飛行，并且能夠在飛行過程中同步進行SLAM，實時修正軌跡，并且能夠返回初始位置[5]。導航系統主要是由GPS模塊、六軸傳感模塊和圖像采集模塊組成。該系統具有精確修正系統誤差的功能，從而提高導航精度[6]。

主控制芯片要進行數據的采集和濾波、圖像的采集和傳輸等大量復雜運算，故選用了STM32F103-RBT6作為主控芯片，STM32F103XX基于高性能32位RISC的ARMCortex-M3核，工作頻率為72MHz。片上集成了高速存儲器，通過APB總線連接了豐富、增強的外設和I/O。所有的設備都提供標準的通信接口[7]。

3.1 GPS模塊

GPS模塊型號為LEA-5T，作為導航系統的主要模塊，GPS對導航精度及靈敏度起著十分重要的作用，所以必須使用高性能、高可靠性的GPS定位模塊。該系統采用的GPS模塊具有以下特點：

(1)擁有一個-160 dBm捕獲和跟蹤靈敏度的Super Sense;

(2)具備Kick Start功能，信號微弱時可實現加速啟動；

(3)熱啟動和輔助啟動首次定位時間小于1 s;

(4)支持AssistNow Online和AssistNow Offline等A-GPS服務；

(5)GPS、GALILEO、SBAS(WAAS、 EGNOS、MSAS、GAGAN)混合引擎。

3.2 六軸傳感模塊

六軸傳感模塊使用的是MPU6050,MPU6050的角速度全格感測范圍為±250、±500、±1 000與±2 000 °/sec(dps)，可準確追蹤快速與慢速動作，并且用戶可程式控制的加速器全格感測范圍為±2 g、±4 g、±8 g與±16 g。產品傳輸可透過最高至400 kHz的IC。

3.3 圖像采集模塊

圖像采集模塊使用的是0V2640。0V2640像素輸出最大支持200萬像素，支持QCIF(176*144)、QVGA(320*240)、VGA(640*480)、1 027*768、1 600*1 200等像素輸出。具備兩種輸出格式：

(1)原始數據如RGB565，RGB RAW，YUV422；

(2)JPEG壓縮圖像格式(可極大減少傳輸帶寬，例如640*480分辨率的原圖片大小在300 kB左右，JPEG編碼輸出后大小僅約為16 kB)。

在該系統中，由于使用原始數據格式，圖像文件過大(以RGB RAW為例，一幀640*480的圖像文件大小為640*480*3=900 Kbytes)，影響數據傳輸速率和更新圖像的頻率。TM32f103RBT6處理能力有限，無法做復雜的圖像壓縮算法，所以選擇0V2640內部DSP壓縮后的JPEGS圖像格式是最好的選擇。

MCU與0V2640通信采用串行與并行結合，0V2640帶有SCCB(Serial Camera Control Bus)雙線串行接口，MCU通過SCCB接口配置和讀取0V2640的信息；MCU通過并行總線的方式接收0V2640圖像數據。

4 系統軟件分析

文中提出了一種可以使用單個實時相機進行場景的快速稠密重建的方法。使用基于點的實時從運動中恢復結構(SFM)作為起始點，生成精確的3D相機姿態估計和稀疏點云。主要創新在于使用了一種源于SFM的基礎網格生成方法，預測在一束自動選擇的跨越場景的參考幀附近的姿態中的視野，然后使用一種視野預測的光流和有約束的場景流更新的方法將基礎網格變形為高度精確的深度圖[8]。由于產生深度圖的高質量，全局場景模型可以簡單將它們并列排放,然后去除重疊區域。筆者展示了一個現實環境可以通過一個實時相機在數秒內重建，其處理使用的僅是普通桌面級別的硬件[9]。

4.1 從運動到結構

與所有稠密重建系統類似，都需要在全局幀中獲得高質量的相機位姿估計。使用SFM來提供實時相機位姿的估計和作為稠密場景重建的起始點的稀疏點云。Davison介紹了一種簡單的相機追蹤和映射系統，其使用了連續概率濾波器來構建一致的場景圖和實施相機位姿估計,但是絕對精確對受限于每幀所追蹤到的數量較少的特征點。Klein和Murray的平行追蹤和映射系統(PTAM)轉而依賴基于每幀幾百個特征點的測量的幀率位姿估計，并迭代使用最優化平行方法的重復束調整。它能夠實現幾千個場景點位置和歷史攝像幀序列的位姿的高精度估計，因此能夠滿足需求。

只收集從PTAM獲得的高質量的點，并將其送入稠密重建流水線，期間丟棄那些異常值大于10的點。另一種從PTAM獲得的有用的值是在關鍵幀集中場景點的聯合可見性。通過對關鍵幀中的每個點的光軸方向取平均的方法來進行每個點粗略的初始表面法線估計。在上述的關鍵幀中，這些點都是可見的。

4.2 基礎表面構建

已有從SFM中得到的起始3D點云，現在想要估計起始連續場景表面來獲得稠密細化的基礎。從有向點采樣進行稠密3D表面重建的問題已經在計算機圖形學社區獲得了足夠的關注度[10]，并且這個問題是模型曲面補洞、優化表現模式和模型美化的基礎，隱式曲面重建技術更是獲得了足夠的關注。通過這些方法，表面可以通過將函數擬合成點數據的方法估計得到，其中，R3→R,f(x)=0。可以通過多邊形化函數的零點集合(zerolevelset)的方法來提取重建網格[11]。在獲得即時的基礎模型中，速度是至關重要的。而全局優化的無參數的表面擬合技術需要極大的計算代價用于解很大的稠密系統的方程組。近年來，重建的計算消耗有了大幅度的減少，主要是將全局優化算法替換為分層和由粗到精的方法[12]。尤其是有限支持徑向基函數可以使有約束的稠密系統變得稀疏。

文中使用了一種最先進的多尺度緊支撐徑向基函數的方法用于3D離散數據內插[13]。該方法結合了全局和局部估計方法的優點，適合于稀疏的、方向估計的從SFM過程中獲得的點云，特別是保留了在IRef低稠密度的大區域插值的能力。

零點集合的多邊形化，使用了Bloomenthal的方法，也由MSCSRBF支持，因為當估計隱式曲面時計算的大部分是基于上一層的函數層次。實際上，可以在每次生成一個新的關鍵幀時都進行基礎表面重建，以便維護即時的基礎模型。

4.3 約束場景流稠密重建

在該系統中，使用了簡單迭代稠密深度圖重建算法。該算法依賴于高質量的相機追蹤與在上一章節中獲得的基礎模型。

假設有n個靜止的攝像機觀察一個可變形的表面。目標是運用稠密的跨攝像機束的圖像測量來使基礎表面估計變形到一個更加新而準確的形狀上去。

(1)

稠密的基礎網面提供了一個關于在攝像機束中每幀可見的表面元素的位置和法向量的預測。希望使用圖像對應信息來獲取端點更新Δxj以用來將當前基礎模型的每個端點x變形到一個新的準確體現真實場景的估計中。

Δxi=riλi

(2)

(3)

最小二乘解通過下列標準方程求得：

(4)

形成稠密的逐端點的表面更新：

(5)

這個計算只包括了兩個規模為2n的內積運算，以及一個標量除法，可以在GPU硬件上進行并行化運算。在實踐中，通過使用之前的式(1)，獲取最后的端點更新。在初始基礎模型的端點已經無解的情況下，優化過程繼續最多3次迭代，或者直至平均重投影誤差小于e=1e-4個歸一化像素。

4.4 稠密對齊

4.5 局部模型集成

已經有大量算法可以用于多個深度圖的融合，這些深度圖傳統上是從結構光和范圍掃描器中獲得的[14]。這些方法可大致分為兩類。第一類方法使用計算深度圖、密集點樣本或者體素符號距離函數來擬合全局連續函數，此函數可用于多邊形化。但是很難用于大規模的重建，因為受到普通PC內存和CPU的限制。第二類方法直接在局部網格上進行重建。鏈式多邊形網格,作為最原始的方法，通過去除網格之間的重疊區域的方法來獲得連續拓撲網格。文獻[9]貢獻了有噪聲的深度圖實時融合的方法。該方法可以獲取一系列的深度圖，同時可以降低這些深度圖的誤差并且去除空間碰撞，然后將其多邊形化。

因為前面的工作保證了深度圖的連續高質量，只要使用一種非常簡單的方法就可以滿足需求。

假定已知一個從參考幀Pref獲得的三角化網格，將剛集成的稠密重建深度圖渲染至Pref并且在誤差范圍內去除鄰近頂點。

5 仿真與調試

為了驗證四旋翼無人機的飛行與單目導航功能建模，首先制作了樣機，在地面調試PID參數，最后在室內進行實際飛行測試，并且在上位機上進行同步建模。飛行效果圖如圖4～6所示。

圖4 四旋翼飛行器樣機調試

圖5 四旋翼飛行器傳輸圖片

圖6 單目稠密重建效果圖

6 結束語

文中設計了一種基于四旋翼無人機的單目導航建模系統。該系統能夠通過攝像頭對環境進行單目稠密重建，在PC端顯示并發送給四旋翼無人機，結合無人機的GPS和慣導系統，能夠使無人機對極端環境進行探測。設計并制作了四旋翼無人機樣機,并進行了實際飛行測試。試飛結果表明：該系統可以用于四旋翼無人機系統進入復雜環境并且進行建模。

[1] 詹 鐳，賀人慶，謝 陽，等.基于微型四旋翼無人機的智能導航系統[J].電子測量技術,2011，34(6):1-3.

[2] 王曉明．電動機的單片機控制[M]．第2版．北京：北京航空航天大學出版社，2007.

[3] 楊明志,王 敏．四旋翼微型飛行器控制系統設計[J]．計算機測量與控制，2008，16(4)：485-487．

[4] 劉金琨．先進PID控制及其MATLAB仿真[M]．北京：電子工業出版社，2004．

[5] 聶博文,馬宏緒,王 劍,等．微小型四旋翼飛行器的研究現狀與關鍵技術[J]．電光與控制，2007，14(6)：113-117．

[6] 劉慧英,范寶山.基于STM32的多步進電機控制系統研究[J]．測控技術，2010，29(6)：54-57．

[7] 符 冰，方宗德，侯 宇．一種新型微旋翼飛行器的設計與控制[J]．航空制造技術，2006(5)：91-94．

[8]WangL,MordohaiP,FrahmJM,etal.Real-timevisibility-basedfusionofdepthmaps[C]//Proceedingsoftheinternationalconferenceoncomputervision.[s.l.]:[s.n.],2007.

[9]NewcombeRA,StrasdatH,KellyPHJ,etal.SLAM++:simultaneouslocalisationandmappingatthelevelofobjects[C]//Procof2013IEEEconferenceoncomputervisionandpatternrecognition.[s.l.]:IEEE,2013:1352-1359.

[10]WuHao,BuJiajun,ChenChun,etal.Locallydiscriminativetopicmodeling[J].PatternRecognition,2012,45(1):617-625.

[11]KleinG,MurrayDW.ParalleltrackingandmappingforsmallARworkspaces[C]//Proceedingsoftheinternationalsymposiumonmixedandaugmentedreality.[s.l.]:[s.n.],2007.

[12]YuF,GallupD.3dreconstructionfromaccidentalmotion[C]//Proceedingsof27thIEEEconferenceoncomputervisionandpatternrecognition.[s.l.]:IEEE,2014.

[13]WagmanRS,BarmpoutisA,BoziaE.Anovelframeworkfor3dreconstructionandanalysisofancientinscriptions[J].JournalofMachineVisionandApplications,2010,21(6):989-998.

[14]ZhangLijun,ChenChun,BuJiajun,etal.Activelearningbasedonlocallylinearreconstruction[J].IEEETransactionsonPatternAnalysisandMachineIntelligence，2011,33(10)：2026-2038.

Monocular Modeling System Based on Four Rotors UAV

ZHUANG Xiao-ming,LI Tao,FU Long

(Institute of Advanced Manufacturing Technology,Hefei Institute of Physical Science, Chinese Academy of Science,Changzhou 213164,China)

In order to realize autonomous flight,monocular dense reconstruction and auto navigation of four rotors UAV,the monocular reconstruction and modeling system is designed.The flight controller uses six axis sensors,MPU-6050,to detect the attitude of the four rotors UAV;the speed of the 4 motor drive module is controlled by the STM32F103RBT6 chip to change the attitude of the UAV with PID algorithm.The monocular reconstruction system acquires real-time position and environment information of the UAV by controlling the GPS module and image acquisition module.The software on PC can receive the images and make monocular dense reconstruction.Dense reconstruction contains five main part including structure from motion,basic surface construction,constrained scene flow dense reconstruction and local model integration.The intelligent autonomous flight of UAV is realized through communications between the PC and the UAV with wireless serial port.Experiments show that the system can realize monocular dense reconstruction.

four-rotor micro UAV;smart navigation;3D reconstruction;attitude control

2015-06-16

2015-09-23

時間：2016-06-22

國家青年科學基金項目(61401437);中國科學院合肥物質科學研究院院長基金青年火花項目(YZJJ201323)

莊曉明(1987-),男，碩士研究生，研究方向為控制工程與機器視覺。

http://www.cnki.net/kcms/detail/61.1450.TP.20160621.1701.008.html

TP302

A

1673-629X(2016)07-0104-05

10.3969/j.issn.1673-629X.2016.07.022