室內無人機定點懸控系統(tǒng)

孟令康，秦海超，鮑丹陽

(哈爾濱工業(yè)大學(深圳) 機電工程與自動化學院，廣東 深圳 518000)

0 引言

近年來無人機應用市場日趨火熱，無人機開始被應用在多個領域之中[1-4]，比如航拍、植保、運輸、安防等[5]。無人機的自主飛行依賴導航系統(tǒng)提供的定位信息進行控制。當前，為了在保障安全可靠的前提下便于高校及研究所科研人員對無人機控制的關鍵技術展開研究，迫切需要一套成熟的無人機室內定位技術解決方案?；诖?，本文做了相關研究并開發(fā)了基于圖像處理的室內無人機定位系統(tǒng)，可對無人機控制相關的科研進程提供有效可靠的實驗條件，也將加速無人機技術應用到各個行業(yè)中。

當前較為成熟的定位方案包括以下幾種：

1) 全球衛(wèi)星導航系統(tǒng)

如美國的GPS(global positioning system)和中國的BDS(beiDou navigation satellite System))，但此類方案在民用范圍內精度受限制[6]，一般必須與INS(inertial navigation system)[7]的慣性數(shù)據(jù)融合后才能實際使用，且全球衛(wèi)星導航系統(tǒng)在室內的應用效果會因建筑環(huán)境對衛(wèi)星信號的影響而大打折扣[8]，導致更加難以形成可靠的室內定位方案。

2) 光流法定位方案

其原理為采集安裝在機器人底部的微型相機垂直向下拍攝所得的連續(xù)圖像幀序列，通過光流法計算相鄰兩幀圖像之間基于像素的位移[9]。與諸如超聲波、激光、氣壓計等高度傳感器獲得的相對高度數(shù)據(jù)融合后得到基于參考坐標系尺度下機器人的相對位移，進而完成定位的任務。但其有效性對地面紋理要求高且不具有通用性，而光流算法本身及其數(shù)據(jù)融合算法的有效性對定位的精度影響也很大，最重要的是方案本身具有不可避免的長期漂移性[10]。

3) 并發(fā)建圖與定位技術

近年來發(fā)展迅速，一般分為基于視覺和基于激光雷達兩種方案[11]。基于視覺的方案算法復雜度高，即便普通的定位實時性也需要相當程度的硬件支持[12]。而基于激光雷達方案的復雜度相對于視覺方案低，但硬件成本更高[13]。

4) 無線測距定位方案

采用多基站測距后解算目標位置[14]。由于無線通訊的特點，其測距受遮擋干擾大，對機體自身姿態(tài)變化敏感，且動態(tài)性能差。在無人機系統(tǒng)中的應用不具備通用性。

本文采取基于圖像處理的室內定位系統(tǒng)，設計四旋翼無人機實驗平臺，將圖像識別的結果結合目標無人機自身的高度值解算出無人機的水平位置信息，同時基于上述定位系統(tǒng)設計了基于四旋翼無人機的定點懸停控制方案，通過進行室內定點懸停實驗對該系統(tǒng)方案的有效性進行驗證。

1 基于圖像處理的室內定位系統(tǒng)

1.1 定位系統(tǒng)介紹

針對室內無人機，本文提出一種基于圖像處理的定位系統(tǒng)，通過單相機實時采集定位區(qū)域的畫面，對無人機機體的圖像特征進行識別后進行定位，實現(xiàn)無人機的自主飛行。

1.2 相機模型

相機將三維世界中的坐標點(單位為cm)映射到二維圖像平面(單位為像素)的過程能夠用一個幾何模型進行描述。這個模型有很多種，其中最簡單的稱為針孔模型(常用且有效)。如圖1所示，針孔模型描述了一束光線通過針孔(即光心)之后，在針孔背面投影成像的關系。又由于相機鏡頭上透鏡的存在，使光線投影到成像平面的過程中產生畸變。故本文使用針孔和畸變兩個模型來描述整個投影過程，把外部的三維點投影到相機內部成像平面，構成相機內參數(shù)。

設Oxyz為相機坐標系，習慣上z軸指向相機前方，x向右，y向下，O為攝像機的光心，也是針孔模型中的針孔。參考坐標系下的空間點P經過小孔O投影之后，落在物理成像平面Ox'y'上，成像點為P'。

圖1 針孔模型

設P的坐標為[X,Y,Z]T，P'為[X',Y',Z']T，并且設物理成像平面到小孔的距離為f，即焦距。根據(jù)三角形相似關系，則有：

(1)

其中負號表示成像為倒立。為簡化模型，如圖2所示，本文對圖像進行預處理，將成像平面對稱至相機前方，和三維空間點一同置于攝像機坐標系的同一側，則有：

(2)

圖2 成像等效轉換圖示

在物理成像平面上固定像素坐標系O-x-y，原點O位于圖像左上角，u軸向右與x軸平行，v軸向下與y軸平行。相對于成像平面，像素坐標系在u軸上縮放了α倍，在v軸上縮放了β倍，同時原點平移了[cx,cy]T，則P'點坐標與像素坐標[u,v]T有如下關系：

(3)

將式(3)代入式(2)，并將α合并為fx，β合并為fy，得:

(4)

其中：f的單位為m；α、β的單位為像素每米；fx，fy的單位為像素。式(4)化為矩陣形式如下：

(5)

其中：K為相機的內參數(shù)矩陣，Pc為三維空間點在相機坐標系下的坐標。

1.3 位置解算模型

P點在相機坐標系下坐標Pc和參考坐標系下坐標Pr的關系由旋轉矩陣R和平移向量t描述如下：

(6)

由公式(4)得：

(7)

由公式(6)得：

(8)

由式(7)、式(8)得，已知相機內參數(shù)及其位姿，則可通過相機識別結果并結合無人機高度即可解算出水平位置信息，用于無人機位置控制。定位系統(tǒng)算法流程如圖3所示。

圖3 定位系統(tǒng)算法流程圖

2 室內定點懸?？刂葡到y(tǒng)設計

2.1 控制系統(tǒng)簡介

四旋翼無人機的電機對稱地安裝在飛行器的支架端，旋翼均處于同一高度平面，且結構和半徑都相同，其結構形式如圖4所示。

圖4 四旋翼結構示意圖

由于四旋翼無人機的特殊結構，使其成為一種六自由度的垂直升降機，通過調節(jié)4個電機轉速來調整無人機姿態(tài)，進而改變4個電機對機體作用力的大小和方向，實現(xiàn)對飛行器位置的控制。

四旋翼無人機室內定點懸?？刂葡到y(tǒng)，將參考坐標系下無人機的姿態(tài)和位置作為系統(tǒng)的控制量，4個電機的轉速作為控制系統(tǒng)的輸出量，可實現(xiàn)垂直起飛及定點懸停等功能。

2.2 姿態(tài)解算

姿態(tài)解算方案如圖5所示，飛行控制器安裝有慣性測量單元，可對機體三軸線加速度與三軸角速度進行測量，利用慣性測量單元的輸出結合機體自身重力采用牛頓梯度下降算法進行姿態(tài)解算，獲得對水平軸對應姿態(tài)角的估計。

另安裝有地磁傳感器，對無人機所在位置的磁場進行測量，結合陀螺儀對豎直軸對應角速度測量值的積分進行互補濾波，最終獲得偏航角估計。

圖5 姿態(tài)解算方案示意圖

2.3 高度控制

在位置控制中，水平兩個方向是基于姿態(tài)的控制方式，其結構相同，而豎直方向即高度控制則基于4個電機合力的控制方式，然而當機體具有一定的水平傾角時，升力的改變會導致機體水平加速度的改變。

這種關系造成了高度控制與水平控制的耦合關系，故在基于模型控制時，需在對水平姿態(tài)角度的控制量中加上升力的影響。但考慮在四旋翼無人機的室內懸?？刂魄闆r下，除垂直起飛過程外，高度控制量無大的干擾或波動且不存在大角度控制量，故本文將忽略其耦合關系，對高度和水平位置分別進行控制。

如圖6所示，通過三環(huán)PID控制提高高度環(huán)的穩(wěn)定性和抗干擾性，其中對加速度環(huán)中的微分項引入了二階低通濾波，豎直高度、豎直速度及豎直加速度反饋量通過傳感器組數(shù)據(jù)融合后獲得，其控制輸出為四電機各自的基礎轉速控制量。

圖6 高度控制環(huán)

目前可實現(xiàn)對高度進行測量的傳感器包括有GPS、超聲波、激光、氣壓計等，考慮到室內空間的特點，如圖7所示，結合對傳感器自身測量精度、穩(wěn)定性以及輸出頻率等要求，選擇激光測距的方案對機體高度進行測量。豎直加速度由加速度計豎直軸測量值經“體-地”坐標系轉換后獲得，豎直速度采用捷聯(lián)慣導方案融合豎直高度與數(shù)值解速度后獲得，其中參與融合的豎直高度單獨由測量頻率、穩(wěn)定性，平滑度更高的氣壓傳感器獲得。

圖7 高度數(shù)據(jù)融合方案示意圖

2.4 水平位置控制

定位系統(tǒng)向無人機輸出水平位置，無人機對位置數(shù)據(jù)微分后經低通濾波獲得水平方向的速度估計值，對水平位置環(huán)路實現(xiàn)如圖8所示串級PID控制，其中水平兩個方向控制環(huán)路結構相同，其控制輸出為參考坐標系下期望姿態(tài)角。

圖8 水平位置控制環(huán)

2.5 姿態(tài)控制

為簡化控制結構，將偏航角的期望值置0，僅通過水平姿態(tài)角且不影響控制效果，以姿態(tài)解算獲得的姿態(tài)角和陀螺儀對角速度的測量值作為反饋量，設計如圖9所示串級PID控制，其中各姿態(tài)角控制環(huán)路結構相同，其控制輸出為四電機各自的補償控制量。

圖9 姿態(tài)控制環(huán)

3 實驗驗證

本文采用等軸距為33cm的四旋翼無人機作為載體，飛行控制計算機和外部設備均安裝在機體中央位置，在其頂部中央固定有特定顏色的輕量球體，各機臂中央安裝有30 A無刷電調，機臂外端安裝有1200kV無刷電機，并采用5200mAh容量的3S鋰電池，組成無人機平臺的動力配置。無人機通過雙藍牙與上位機進行通訊，分別完成與圖像定位系統(tǒng)的數(shù)據(jù)通訊任務和實時飛行數(shù)據(jù)的采集任務。無人機平臺在實現(xiàn)自主飛行的同時仍保留遙控器配置作為應急方案切換。

上位機配置處理器為Intel(R) Core(TM) i5-4210M，主頻為2.60GHz，安裝RAM 4.00GB，操作系統(tǒng)為Windows 10(1803)教育版。

控制系統(tǒng)結構圖如圖10所示，四旋翼無人機和上位機之間通過藍牙進行無線通訊，無人機將高度數(shù)據(jù)發(fā)送給上位機，上位機結合相機獲得的圖像數(shù)據(jù)解算出水平位置信息，并發(fā)送給無人機進行位置控制。

圖10 室內定點懸停控制系統(tǒng)結構圖

通過實驗給出位置跟蹤曲線(圖11-圖13)和姿態(tài)跟蹤曲線(圖14-圖16)，其中，無人機位置期望均為起飛時坐標即初始位置，初始高度為激光傳感器固定高度，航向角為基于地理坐標系下磁場方向所建立，其具體數(shù)值見表1，相關控制參數(shù)見表2。

表1 系統(tǒng)參數(shù)

表2 控制參數(shù)

圖11 x軸位置跟蹤曲線

圖12 y軸位置跟蹤曲線

圖13 高度跟蹤曲線

圖14 橫滾角跟蹤曲線

圖15 俯仰角跟蹤曲線

圖16 航向角跟蹤曲線

4 結語

本文通過設計基于圖像處理的室內定位方案，為室內無人機系統(tǒng)提供實時的定位數(shù)據(jù)，同時設計了四旋翼無人機實驗平臺，結合基于圖像處理的室內定位方案進行了室內定點懸停實驗，達到了預期的效果，證實了本文提出的室內定位方案的可行性，且該目標定位方案對機體自身負荷小，精度高，實時性好，可移植性強，對其他攝像頭和無人機平臺都具有較好的通用性。