王 聰， 張 華,2， 陳財(cái)富， 汪 雙， 黎榮華

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽(yáng) 621000；2.西南科技大學(xué) 特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室，四川 綿陽(yáng) 621000)

0 引 言

無(wú)人機(jī)具備機(jī)動(dòng)性強(qiáng)、視角廣等優(yōu)點(diǎn)，近年來(lái)在室內(nèi)應(yīng)急救援、巡檢等領(lǐng)域得到廣泛應(yīng)用。其高度控制作為縱向飛行控制的重要組成部分[1]，是確保穩(wěn)定性的重要環(huán)節(jié)。

現(xiàn)有導(dǎo)航方案，如氣壓計(jì)、慣性導(dǎo)航設(shè)備，在室內(nèi)定高精度差，無(wú)法感知地形變化及上下方障礙物[2,3]。在室內(nèi)，常采用超聲波、激光測(cè)距儀測(cè)高，這類(lèi)傳感器可用于地形估計(jì)，如地形跟隨，即無(wú)人機(jī)隨下方地形高低起伏而起伏，與障礙物保持恒定相對(duì)高度[4,5]。文獻(xiàn)[6,7]分別使用激光雷達(dá)、毫米波雷達(dá)用于坡路和植保中的地形跟隨。但在實(shí)際使用過(guò)程中，無(wú)人機(jī)常需保持恒定絕對(duì)高度，如定距跟蹤目標(biāo)[8]，定高監(jiān)視、巡檢[9]。文獻(xiàn)[10] 加入卡爾曼濾波融合超聲波與加速度計(jì)估計(jì)無(wú)人機(jī)高度，但地形變化會(huì)影響融合效果。文獻(xiàn)[11,12]分別使用雙目視覺(jué)、視覺(jué)即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)進(jìn)行無(wú)人機(jī)定高及定位，但視覺(jué)在光照、紋理特征不足時(shí)表現(xiàn)較差。文獻(xiàn)[13]采用三維激光SLAM用于無(wú)人駕駛，但由于傳感器體積及重量限制，不適用于小型無(wú)人機(jī)平臺(tái)。文獻(xiàn)[14]搭建無(wú)人機(jī)自主飛行系統(tǒng)，使用飛行時(shí)間(time of flight,ToF)傳感器獲取高度，在非平整地面仍需進(jìn)一步處理。文獻(xiàn)[15]使用慣性測(cè)量單元(inertial measurement unit,IMU)、氣壓計(jì)融合進(jìn)行絕對(duì)高度的測(cè)量，能夠一定程度適應(yīng)突變地形，但在室內(nèi)應(yīng)用還需提高精度。

針對(duì)無(wú)人機(jī)在室內(nèi)由于地形變化導(dǎo)致的定高不恒定問(wèn)題，本文提出室內(nèi)非平整地形下無(wú)人機(jī)定高控制方法，所提方法在室內(nèi)應(yīng)急救援，空中機(jī)器人路徑規(guī)劃，地空協(xié)同跟蹤等領(lǐng)域有廣闊應(yīng)用前景。

1 系統(tǒng)描述

無(wú)人機(jī)搭載ToF傳感器測(cè)定高度信息，在非平整地形條件下，高度數(shù)據(jù)經(jīng)過(guò)高度去突變模型處理后，再進(jìn)行自適應(yīng)加權(quán)遞推均值濾波，最后經(jīng)過(guò)無(wú)人機(jī)飛行控制器高度環(huán)處理后用作高度控制。系統(tǒng)框圖如圖1所示。

圖1 系統(tǒng)框圖

2 六旋翼數(shù)學(xué)建模

無(wú)人機(jī)定高去突變系統(tǒng)采用六旋翼飛行器為載體，如圖2為六旋翼飛行器結(jié)構(gòu)示意，其中,o(x,y,z)為機(jī)體坐標(biāo)系，O(X,Y,Z)為地球固連坐標(biāo)系。為簡(jiǎn)化建模過(guò)程，提出以下假設(shè)：1)六旋翼為剛體；2)質(zhì)量與轉(zhuǎn)動(dòng)慣量不變；3)六旋翼幾何中心與質(zhì)心一致。

圖2 六旋翼結(jié)構(gòu)示意

由機(jī)體坐標(biāo)系到地球固連坐標(biāo)系的旋轉(zhuǎn)矩陣為

(1)

式中C與S分別為cos()和sin()函數(shù)；θ，ψ，φ分別為俯仰角、偏航角與滾轉(zhuǎn)角。系統(tǒng)輸入如下

(2)

式中U為六旋翼4個(gè)獨(dú)立通道的輸入，U1,U2,U3,U4分別為升力、滾轉(zhuǎn)、俯仰、偏航輸入量；Ni(i=1,2,3,4,5,6)為各個(gè)電機(jī)的轉(zhuǎn)速；L為旋翼中心到機(jī)體質(zhì)心的距離；a和d分別為升力、阻力系數(shù)。

由牛頓—?dú)W拉方程可得出所關(guān)注的飛行高度及決定多旋翼穩(wěn)定性的姿態(tài)角的運(yùn)動(dòng)方程[16]

(3)

式中J為總轉(zhuǎn)動(dòng)慣量，Ix，Iy，Iz分別為x，y，z軸的轉(zhuǎn)動(dòng)慣量，m為飛行器質(zhì)量，g為重力加速度，fi(i=1,2,3,4)為系統(tǒng)其他干擾量。

3 高度去突變模型

3.1 地形突變情況分析

ToF傳感器測(cè)量無(wú)人機(jī)與地面之間的相對(duì)距離，在地形突變情況下該距離發(fā)生突變。如圖3是無(wú)人機(jī)在地形突變情況下的高度變化情況。無(wú)人機(jī)先后處于突變前后的地面上空，產(chǎn)生的突變高度為Δd，通過(guò)建立高度去突變模型大幅削減地形突變對(duì)高度的影響。

圖3 地形突變情況的無(wú)人機(jī)高度變化

考慮到地形存在高低起伏，設(shè)定高度突變閾值t，并提出以下判據(jù)判斷地形是否產(chǎn)生突變

|dc-dl|>t

(4)

式中t為高度突變閾值，綜合考慮傳感器刷新速率及室內(nèi)應(yīng)用需求，本文中高度突變閾值取0.1 m；dc為當(dāng)前高度；dl為上一時(shí)刻高度。若滿(mǎn)足式(4)，則產(chǎn)生了地形突變，須進(jìn)一步進(jìn)行高度去突變處理。

3.2 單一地形突變

當(dāng)滿(mǎn)足式(4)，突變高度大于閾值，首先取得突變高度，再利用當(dāng)前高度減去突變高度得到相對(duì)穩(wěn)定高度，建立如下高度去突變模型

d=dc-Δd,Δd=dc-dl

(5)

式中 Δd為突變高度。d為去突變處理后的高度。如式(5)所示的高度去突變模型，在低復(fù)雜度的單一地形突變環(huán)境，突變過(guò)程固定，前后突變高度一致，能夠滿(mǎn)足運(yùn)用需求。

3.3 連續(xù)地形突變

室內(nèi)環(huán)境地形情況十分復(fù)雜，往往存在地形連續(xù)突變的情況。式(5)中的高度去突變模型不滿(mǎn)足應(yīng)用需求。在式(5)的基礎(chǔ)上，對(duì)突變高度模型進(jìn)行優(yōu)化，首次產(chǎn)生突變后，對(duì)突變高度進(jìn)行累加，隨地形的連續(xù)變化而調(diào)整。建立如下高度去突變模型

(6)

4 自適應(yīng)加權(quán)遞推均值濾波

經(jīng)過(guò)高度去突變模型的處理，無(wú)人機(jī)高度數(shù)據(jù)仍存在小幅波動(dòng)，通過(guò)引入自適應(yīng)加權(quán)遞推均值濾波，減小高度誤差，并使無(wú)人機(jī)高度變化更加平滑。

4.1 加權(quán)均值濾波

加權(quán)均值濾波取n個(gè)數(shù)據(jù)，根據(jù)數(shù)據(jù)時(shí)間序列先后等因素賦予相應(yīng)權(quán)重，再進(jìn)行加權(quán)平均，能夠減小數(shù)據(jù)波動(dòng)。加權(quán)均值濾波用于無(wú)人機(jī)高度數(shù)據(jù)處理過(guò)程如下

(7)

式中di為總數(shù)n的數(shù)據(jù)序列中第i個(gè)高度去突變模型處理后的數(shù)據(jù)，考慮傳感器幀率及應(yīng)用需求，本文中n取10；wi為第i個(gè)數(shù)據(jù)的加權(quán)系數(shù)；dstable為加權(quán)平均濾波后的穩(wěn)定高度。

該方法存在時(shí)間滯后，加權(quán)系數(shù)適應(yīng)性不強(qiáng)的缺點(diǎn)。為減少時(shí)間滯后，動(dòng)態(tài)分配加權(quán)系數(shù)，在加權(quán)均值濾波的基礎(chǔ)上，引入一種自適應(yīng)加權(quán)遞推均值濾波算法。

4.2 建立自適應(yīng)加權(quán)遞推均值濾波模型

首先,遞推選取n個(gè)數(shù)據(jù)，確定對(duì)應(yīng)加權(quán)系數(shù)，根據(jù)估計(jì)高度與期望高度之間的誤差動(dòng)態(tài)調(diào)整加權(quán)系數(shù)。誤差越大，權(quán)值應(yīng)越小，采用反比例函數(shù)確定加權(quán)因子，如式(8)所示，取誤差的絕對(duì)值與正實(shí)數(shù)之和作為分母，避免誤差為0及負(fù)數(shù)時(shí)導(dǎo)致權(quán)值無(wú)窮大的情況,即

(8)

然后進(jìn)行歸一化處理,得到加權(quán)系數(shù)為

(9)

式中wi為第i個(gè)數(shù)據(jù)對(duì)應(yīng)的歸一化加權(quán)系數(shù)。

最后得出自適應(yīng)加權(quán)遞推濾波后的高度數(shù)據(jù)

(10)

式中dstable為對(duì)高度去突變數(shù)據(jù)進(jìn)行自適應(yīng)加權(quán)遞推均值濾波后的穩(wěn)定高度。

5 高度控制模型

ToF傳感器的測(cè)量數(shù)據(jù)先后經(jīng)過(guò)高度去突變模型、自適應(yīng)加權(quán)遞推均值濾波處理后的穩(wěn)定高度dstable，經(jīng)無(wú)人機(jī)位置控制環(huán)中的高度通道處理后用作高度控制。

如圖4所示的高度通道，采用雙閉環(huán)串級(jí)比例—積分—微分(proportional-integral-differential,PID)控制算法，可分為外環(huán)高度控制環(huán)及內(nèi)環(huán)高度速度控制環(huán)，外環(huán)輸入為期望高度，內(nèi)環(huán)輸入為外環(huán)輸出，即期望速度[17]。

圖4 高度通道

高度控制環(huán)采用比例控制器得到期望速度用作高度速度控制環(huán)的輸入

(11)

式中Velsp為期望速度；Zerr為高度誤差；Zsp為期望高度;kp為比例系數(shù)。

高度速度控制環(huán)采用PID控制器，得到期望拉力輸入控制分配器對(duì)無(wú)人機(jī)電機(jī)進(jìn)行控制

(12)

式中fsp為期望拉力；m為無(wú)人機(jī)質(zhì)量；g為重力加速度；kP，kI，kD為比例、積分、微分系數(shù)；Velerr為速度誤差。

6 實(shí)驗(yàn)平臺(tái)搭建與測(cè)試

ToF傳感器采用北醒光子的低成本、高精度TFmini-Plus激光雷達(dá)，基于ToF即飛行時(shí)間原理，測(cè)量范圍0.1～12 m，準(zhǔn)確度為±5 cm，重量12 g；無(wú)人機(jī)采用X600-X6六軸機(jī)架；板載計(jì)算機(jī)NVIDIA TX2 用于高度數(shù)據(jù)的處理；飛行控制器使用開(kāi)源PIXHAWK一代，固件版本為v1.11.0；ToF傳感器朝下安裝于無(wú)人機(jī)底部，用于獲取無(wú)人機(jī)與地面之間的相對(duì)距離。

為定量分析無(wú)人機(jī)在室內(nèi)地形突變環(huán)境下的定高效果，搭建4.5 m×3.7 m×3.1 m的鋼架結(jié)構(gòu)及緩沖網(wǎng)組成實(shí)驗(yàn)場(chǎng)地；使用不同高度的盒子模擬連續(xù)地形突變。如圖5為實(shí)驗(yàn)平臺(tái)。

圖5 實(shí)驗(yàn)平臺(tái)

無(wú)人機(jī)于定點(diǎn)模式保持與地面的相對(duì)距離不變，并飛越下方模擬地形突變環(huán)境，測(cè)試定高效果。如圖6為地形突變狀態(tài)下無(wú)人機(jī)飛越下方障礙物過(guò)程，t0時(shí)刻處于平坦地面上方；t1時(shí)刻飛越0.5 m高障礙物，高度發(fā)生突變；t2時(shí)刻處于0.5,0.25 m高障礙物之間；t3時(shí)刻飛越0.25 m高障礙物；t4時(shí)刻移出0.25 m高障礙物，位于平坦地面上方。

圖6 地形突變狀態(tài)下無(wú)人機(jī)飛行實(shí)驗(yàn)過(guò)程

7 數(shù)據(jù)分析

實(shí)驗(yàn)共分4組：期望高度分別相距平坦地面1，2 m，并以?xún)山M快慢不同的速度飛越下方高度為0.5，0.25 m左右的障礙物，速度大致為4，1 m/s；測(cè)試高度去突變模型及自適應(yīng)加權(quán)遞推均值濾波效果,分析計(jì)算高度誤差。

期望高度為1，2m時(shí)不同速度下的高度估計(jì)曲線如圖7(a)所示。如圖7(b)為期望高度為1,2m時(shí)，不同速度下發(fā)生地形突變后的高度誤差曲線，并比較加入自適應(yīng)加權(quán)遞推均值濾波前后的誤差。

圖7 實(shí)驗(yàn)結(jié)果

由圖7(a)所示不同速度、不同期望高度下高度估計(jì)曲線得出：高度去突變模型能夠大幅削減地形對(duì)高度的影響；自適應(yīng)加權(quán)遞推均值濾波使高度變化曲線更加平滑，相較于快速飛行，慢速飛行的高度去突變效果相對(duì)較差。

由圖7(b)所示的發(fā)生地形突變后的高度誤差曲線得出：相較于高度去突變模型，加入自適應(yīng)加權(quán)遞推均值濾波的高度誤差波動(dòng)幅度更小；相較于快速飛行，慢速飛行的高度誤差更大。相較于快速飛行，慢速飛行去突變效果較差，是由于飛越下方障礙物邊緣時(shí)，檢測(cè)到邊緣高度突變值小于閾值，須在實(shí)際使用過(guò)程中調(diào)整閾值。表1為不同飛行速度及高度下的誤差對(duì)比情況，與期望高度相比，最大平均誤差為7.97 cm，平均誤差為2.92 cm。

表1 高度誤差對(duì)比 cm

相比文獻(xiàn)[15]提出的IMU氣壓計(jì)融合絕對(duì)高度估計(jì)方法，本文方法將高度平均誤差從19.7 cm減小到了2.92 cm，高度精度提升85.2 %。

8 結(jié)束語(yǔ)

本文提出建立基于TOF傳感器的高度去突變、自適應(yīng)加權(quán)遞推均值濾波模型，并應(yīng)用于無(wú)人機(jī)室內(nèi)非平整地形的定高。實(shí)驗(yàn)表明：本文方法能有效規(guī)避地形對(duì)高度的影響，滿(mǎn)足執(zhí)行飛行任務(wù)時(shí)保持恒定絕對(duì)高度的需求；高度估計(jì)精度達(dá)到2.82cm，滿(mǎn)足室內(nèi)飛行時(shí)精確度需求。所提方法已應(yīng)用于應(yīng)急救援環(huán)境下的地空協(xié)同機(jī)器人的定距離跟蹤，滿(mǎn)足實(shí)際應(yīng)用需求。