室內無人機定點懸控系統

孟令康，秦海超，鮑丹陽

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

0 引言

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

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

1) 全球衛星導航系統

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

2) 光流法定位方案

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

3) 并發建圖與定位技術

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

4) 無線測距定位方案

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

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

1 基于圖像處理的室內定位系統

1.1 定位系統介紹

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

1.2 相機模型

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

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

圖1 針孔模型

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

(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為相機的內參數矩陣，Pc為三維空間點在相機坐標系下的坐標。

1.3 位置解算模型

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

(6)

由公式(4)得：

(7)

由公式(6)得：

(8)

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

圖3 定位系統算法流程圖

2 室內定點懸停控制系統設計

2.1 控制系統簡介

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

圖4 四旋翼結構示意圖

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

四旋翼無人機室內定點懸停控制系統，將參考坐標系下無人機的姿態和位置作為系統的控制量，4個電機的轉速作為控制系統的輸出量，可實現垂直起飛及定點懸停等功能。

2.2 姿態解算

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

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

圖5 姿態解算方案示意圖

2.3 高度控制

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

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

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

圖6 高度控制環

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

圖7 高度數據融合方案示意圖

2.4 水平位置控制

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

圖8 水平位置控制環

2.5 姿態控制

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

圖9 姿態控制環

3 實驗驗證

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

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

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

圖10 室內定點懸停控制系統結構圖

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

表1 系統參數

表2 控制參數

圖11 x軸位置跟蹤曲線

圖12 y軸位置跟蹤曲線

圖13 高度跟蹤曲線

圖14 橫滾角跟蹤曲線

圖15 俯仰角跟蹤曲線

圖16 航向角跟蹤曲線

4 結語

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