基于激光雷達的無人機仿地飛行系統(tǒng)設(shè)計與實現(xiàn)*

林 峰，石田松，盧艷軍，張曉東

（沈陽航空航天大學自動化學院，沈陽 110136）

0 引言

仿地飛行系統(tǒng)廣泛應用于無人機的農(nóng)藥噴灑、電力巡檢領(lǐng)域。仿地飛行不僅可以提高工作效率，而且也有助于提高無人機飛行安全。目前針對無人機仿地飛行問題，文獻［1-3］提出基于已知周圍地形高度，預先計算出最優(yōu)的仿地飛行路線的方法，但無人機無法對地形變化做出實時反應。文獻［4-6］提出基于視覺的地形測繪方法，但該方法對無人機飛行控制要求很高，且隨著飛行高度的增加，誤差會增加。近年來，激光雷達系統(tǒng)和激光測距儀在小型無人機上的應用越來越多。文獻［7-8］提出使用旋轉(zhuǎn)激光雷達系統(tǒng)創(chuàng)建一個完整的三維地形圖，實現(xiàn)仿地飛行，但是存在計算量過大的問題。

本文提出基于激光雷達和二軸增穩(wěn)云臺的仿地飛行方法，解決現(xiàn)有仿地飛行的實時性差和計算量大等問題。

1 系統(tǒng)總體設(shè)計

仿地飛行是不改變無人機的飛行路線，在縱向上依靠無人機的機動性保持無人機和地面的相對高度不變，無人機根據(jù)地形實時飛越障礙物［9］。如圖1 所示，其中虛線是無人機理想的仿地飛行軌跡。仿地飛行原理是通過調(diào)節(jié)無人機的實際相對高度與目標高度的差值ΔH，實現(xiàn)仿地飛行效果。仿地飛行的效果可以從無人機對地面改變反應的實時性觀察中得到。

圖1 仿地飛行示意圖

系統(tǒng)總體設(shè)計方案如圖2 所示。在原來飛控基礎(chǔ)上，增加了激光測距模塊和二軸增穩(wěn)云臺模塊。激光測距模塊是仿地飛行系統(tǒng)的核心，它會將精準、穩(wěn)定的高度數(shù)據(jù)快速輸出給飛控，等待飛控對高度數(shù)據(jù)進一步處理。云臺起到了輔助的作用，主控MEGAP32 8P 會將飛控輸出的姿態(tài)數(shù)據(jù)和基座上MPU 6050 的數(shù)據(jù)進行融合，在云臺兩軸系統(tǒng)同時穩(wěn)定的情況下，并對角速度進行實時修正，最終保證了激光雷達測距方向的穩(wěn)定。

圖2 系統(tǒng)設(shè)計方案

為了方便后期實驗的驗證，系統(tǒng)設(shè)計了實驗模式（手動飛行）和正常模式（自主飛行）兩種，具體工作流程如圖3 所示。實驗模式是通過遙控飛行，在油門保持中值不變后自動保持無人機與地面的相對高度不變實現(xiàn)仿地飛行。優(yōu)點在于仿地飛行的高度可以隨時調(diào)節(jié)。正常模式是全程自主飛行，一切任務都在地面站上進行設(shè)置，不同路段可以設(shè)置不同高度。優(yōu)點是操作簡單有更好的安全性和實用性。

圖3 工作流程圖

2 硬件設(shè)計

硬件系統(tǒng)設(shè)計主要包括激光雷達測距系統(tǒng)和二軸云臺增穩(wěn)控制系統(tǒng)。激光雷達可以快速、精準地測量出無人機與地面之間的相對高度，以便無人機及時地做出反應。二軸云臺確保激光雷達測量數(shù)據(jù)和測量方向的穩(wěn)定。激光雷達不是測量斜前方的高度用算法去做感知地形的前瞻性，是因為前瞻性可以通過更好的實時性彌補，并且解決了在無人機只有單測距儀的情況下需要前瞻性，急停時無法測量正下方相對高度的問題。

2.1 基于激光雷達的測距系統(tǒng)

測距激光雷達是基于TOF（飛行時間）原理，能實現(xiàn)穩(wěn)定、精準、高靈敏度和中遠距離測量等功能。并且在仿地飛行的任務中可以通過改變輸出頻率和信號強度的閾值來提高無人機對地面變化的反應速度和飛行效果。測距激光雷達由激光發(fā)射機、激光接收機和激光電源組成［10］。原理框圖如圖4 所示。

圖4 激光雷達測距的原理框圖

TOF 原理表述如下。設(shè)待測距離為R，光脈沖往返經(jīng)過時間為t，光速為c，則

式（1）中，t 是光脈沖從發(fā)射器到目標時間的2倍。待測距離是通過計算時間t 內(nèi)進入計數(shù)器內(nèi)鐘頻脈沖的個數(shù)而得出的。設(shè)在時間t 內(nèi)，進入計數(shù)器鐘頻脈沖的個數(shù)為n，鐘頻脈沖的振蕩頻率為f =1/t，則

式（2）中，l 是一個鐘頻脈沖代表的距離，再乘以鐘頻脈沖個數(shù)n，就得到距離R，l 的數(shù)值可以確定測量精度。

激光雷達由于接收角很小，這樣很大地減少了探測的有效邊長，進一步增加了測量精度。激光雷達的有效測距邊長如式（3）所示。

式（3）中，d 表示有效測距邊長，D 表示探測距離，β 為激光雷達的接收半角1.8°，一般的有效測距邊長與探測距離的對應關(guān)系，見表1。

表1 測距距離對應的被測目標有效邊長

2.2 二軸增穩(wěn)云臺系統(tǒng)

二軸增穩(wěn)云臺在無人機仿地飛行中也起到了關(guān)鍵的作用，將激光雷達安裝在云臺上，使其不受無人機姿態(tài)的變化而受到干擾，保證激光雷達始終測量的是無人機與正下方地面的相對高度。

二軸增穩(wěn)云臺顧名思義需要兩個無刷電機來應對無人機俯仰和滾轉(zhuǎn)姿態(tài)角的變化，在驅(qū)動電機之前需要得到無人機的姿態(tài)角，并且增穩(wěn)云臺是將姿態(tài)反饋元件安裝在云臺基座上，并用無刷電機來彌補姿態(tài)角變化，形成穩(wěn)定平臺。當無人機姿態(tài)發(fā)生變化使云臺受到干擾力作用而偏離預設(shè)方位時，安裝在基座上的感應模塊就會將此時的姿態(tài)角反饋到控制核心，經(jīng)過卡爾曼濾波和姿態(tài)解算等處理后，將處理過的數(shù)據(jù)送給電機驅(qū)動，通過無刷電機產(chǎn)生補償力矩與干擾力矩進行綜合，從而保持云臺的平穩(wěn)，間接的保持激光雷達測距的穩(wěn)定和準確。二軸云臺增穩(wěn)原理圖如圖5 所示。

圖5 二軸云臺增穩(wěn)原理圖

云臺的控制數(shù)據(jù)是由無人機飛控傳出的飛行姿態(tài)數(shù)據(jù)和MPU6050 的數(shù)據(jù)融合的。在閉環(huán)控制系統(tǒng)中卡爾曼濾波器聯(lián)合處理飛行姿態(tài)數(shù)據(jù)和MPU6050 測量的數(shù)據(jù)，然后將估計結(jié)果用于反饋估計角速度的計算，以便對PID 控制器的各個穩(wěn)定軸進行校正?？柭鼮V波器的測量矢量由飛行姿態(tài)與MPU6050 的差值構(gòu)成，由于它們都由真實角速度和各自的誤差組成，飛行姿態(tài)和MPU6050 之間的區(qū)別在于誤差的組合?？柭鼮V波器估計MPU6050的剩余速度，并將其提供給誤差更新器的輸入。誤差更新、累加并計算總估計速度誤差，然后從MPU6050 的速度中減去這個誤差，從而提供估計的速度。

在控制器中R（t）是控制系統(tǒng)給定的參考值，Y（t）是被控對象的實際輸出值，R（t）與Y（t）做差得到控制器偏差值E（t）：

控制器的輸出：

經(jīng)過拉普拉斯變換后，控制器的傳遞函數(shù)為：

控制器的比例系數(shù)為Kp1，積分時間常數(shù)為Ti，微分時間常數(shù)為Td。控制器就是根據(jù)給定值與實際輸出值的偏差大小E（t）分別進行比例、積分和微分運算后，再進行一定線性組合來對被控對象進行控制的。

在算法中的卡爾曼濾波是基于一個離散模型的代數(shù)觀測方程和動力學差分方程：

x 是狀態(tài)向量，λ 是向量隨機過程的干擾，σx^是向量確定的輸入，Φ 是狀態(tài)轉(zhuǎn)換矩陣，w 是測量噪聲的向量，Z 是測量矩陣，Y 是測量向量。狀態(tài)轉(zhuǎn)換矩陣Φ 具有以下形式：

其中，P（k + 1|k）為預測誤差的協(xié)方差矩陣，Q 是隨機過程擾動的協(xié)方差矩陣。

其中，K（k +1）為卡爾曼增益矩陣，R 為協(xié)方差矩陣。

校正如下：

3 仿地飛行算法

仿地飛行首先依賴于GPS 的Loite 模式的穩(wěn)定飛行，然后需要激光雷達提供穩(wěn)定、準確、可靠的距離信息，最后通過模糊PID 控制對無人機相對地面的高度進行調(diào)節(jié)［11］，可以有效提高系統(tǒng)的仿地性能和魯棒性。仿地飛行算法總體流程圖如圖6 所示。

首先根據(jù)仿地飛行的實時性可以對激光雷達的輸出頻率進行設(shè)置，一般設(shè)置在1～1 000 Hz 之間，不同的輸出頻率代表激光雷達每秒輸出距離數(shù)據(jù)的個數(shù)。伴隨著距離數(shù)據(jù)激光雷達還可以測出當前目標的信號強度，因此，激光雷達可以通過對信號強度的判斷來決定距離數(shù)據(jù)的可信度。信號強度會在0～65 535 之間。當測距檔位一定時，信號強度會隨著測量距離的增加而降低，并且在相同距離下目標物反射率越低，信號強度越低。當信號強度大于100 且不等于65 535 時，認為距離的測量值可信。在不同的應用場景下可以對可信區(qū)間進行調(diào)整。

圖6 仿地飛行算法總體流程圖

云臺始終保持激光雷達測得的高度是豎直向下的，當激光雷達遇到如圖7 所示的場景時，即測量范圍內(nèi)出現(xiàn)類似臺階一樣不同的高度信息時，高度會經(jīng)過信號強度的加權(quán)處理，最終所得到的高度H 會在H2和H1之間。在得到測量高度后，無人機會將測量高度和目標高度進行對比得出ΔH，再根據(jù)模糊PID 控制算法對無人機高度進行調(diào)節(jié)。

圖7 激光雷達不同高度測距示意圖

模糊PID 控制算法是仿地飛行效果達到既穩(wěn)定又精準的核心。系統(tǒng)首先需要建立模糊推理模型［12］。本系統(tǒng)采用的是系統(tǒng)誤差e 和誤差變化率ed作為輸入變量，控制量作為輸出量U 的二維模糊控制器。建立e，ed，U 的梯形隸屬函數(shù)。

ke和ked為誤差e 和誤差變化率ed的比較系數(shù)。根據(jù)上述模糊推理模型，結(jié)合實際情況，適當調(diào)整ke和ked，建立模糊控制器。當偏差大于一定值時，可以減小曲線的密度，從而提高系統(tǒng)的響應速度；當偏差小于一定值時，可以增大曲線的密度，以提高系統(tǒng)精度并減少超調(diào)。將模糊控制與PID 控制相結(jié)合，當偏差較大時，采用模糊推理方法對系統(tǒng)控制量U 進行調(diào)整，而偏差較小時采用PID 控制。根據(jù)給定的e0變化范圍，進行自動轉(zhuǎn)換。模糊PID 控制結(jié)構(gòu)如圖8 所示。

圖8 模糊PID 控制結(jié)構(gòu)圖

e0是設(shè)定的轉(zhuǎn)換參數(shù)，決定了系統(tǒng)的實時控制算法，偏差如果大于e0將采取模糊控制，如果小于e0則采取PID 控制。e0的大小也將決定仿地飛行系統(tǒng)的反應速度，e0越小無人機反應速度將會越快。

圖9 增穩(wěn)反饋回路

如圖9 所示，在模糊PID 控制后設(shè)計增穩(wěn)反饋回路來消除穩(wěn)態(tài)分量，提高系統(tǒng)動態(tài)性能。

在反饋增穩(wěn)系統(tǒng)wz和α 中，kwz和kα是增益系數(shù)，λ1和λ2是增穩(wěn)系數(shù)，U 是模糊PID 控制器輸出。

整個系統(tǒng)中，模糊控制加快了無人機的反應速度，PID 控制保證了無人機高度變化的準確和穩(wěn)定。最終將模糊控制和PID 控制相結(jié)合有效地提高了系統(tǒng)的仿地性能和魯棒性。在控制器中減小轉(zhuǎn)換參數(shù)e0和增大比例系數(shù)Kp2都會增加無人機的反應速度，增加積分系數(shù)Ki將減小系統(tǒng)的穩(wěn)態(tài)誤差，增加微分系數(shù)Kd將抑制系統(tǒng)的超調(diào)并改善系統(tǒng)的動態(tài)性能，一般情況下Ki是Kp2的1.5 倍，具體參數(shù)可以根據(jù)實際環(huán)境進行設(shè)定。

4 實驗結(jié)果及分析

實驗是讓無人機在坡路上進行往復飛行，觀察飛行效果并對無人機的飛行日志進行分析。飛行日志如下頁圖10～圖12 所示。圖10 反應的是無人機在飛行階段高度的改變不是手動遙控的，而是通過仿地飛行系統(tǒng)自動調(diào)節(jié)的。圖11 中氣壓高度的變化反映出無人機在上坡或者下坡，此時無人機與地面的相對高度始終保持不變，很好地實現(xiàn)了仿地飛行的效果。在圖12 中經(jīng)過實際相對高度與理想相對高度的對比可以看出系統(tǒng)具備很好的穩(wěn)定性。

圖10 中縱軸分別是無人機通道2 的俯仰值和通道3 的油門值；橫軸是無人機飛行時的當?shù)貢r間。通過圖10 可以看出在無人機從起飛到降落，通道3 油門值主要分3 段，第1 段曲線上升為無人機起飛，第2 段曲線保持在1 500 中值為飛行階段，第3 段曲線下降為無人機降落。第2 段始終保持中值，反映出無人機在飛行階段內(nèi)如果高度發(fā)生變化并不是通過手動遙控油門控制的。圖10 中通道2 的俯仰值的變化反應出無人機在飛行階段是做前后的往復運動，當俯仰值小于1 500 時無人機向前行駛，當俯仰值大于1 500 時無人機向后行駛。

圖11 中縱軸分別是通過激光雷達測量的無人機與地面的相對高度HAGL、氣壓計測量的無人機的氣壓高度BAlt 和無人機實現(xiàn)仿地飛行時做的高度補償offset。橫軸是無人機飛行時的當?shù)貢r間。由于氣壓計在低空是不穩(wěn)定的所以對應曲線有一定的波動，氣壓高度增加說明無人機在上坡，反之無人機在下坡。結(jié)合相對高度和氣壓高度的曲線可以看出，無論無人機上、下坡與地面的相對高度都是不變的，并且實時性很高，很好地實現(xiàn)了仿地飛行的效果。補償曲線進一步地反映出了無人機在氣壓高度發(fā)生變化時，無人機為了保持與地面相對高度的不變而做的高度補償。

圖12 中無人機發(fā)生了高度補償，說明無人機正在做仿地飛行任務。此時無人機應該與地面的相對高度是不變的，因此，理想高度曲線是一條直線，通過實際飛行的相對高度曲線與理想相對高度曲線對比，實際相對高度曲線波動很小，與理想飛行高度基本一致，從而可以看出仿地飛行具有很好的穩(wěn)定性。

結(jié)合飛行日志圖10～圖12，可以清晰地看出無人機對地面高度改變的反應實時性很高，無人機在坡路上通過仿地飛行系統(tǒng)能非常好地實現(xiàn)仿地飛行效果，并且經(jīng)過實際飛行表明該系統(tǒng)具有良好的穩(wěn)定性。

圖10 飛行日志（通道2 俯仰值、通道3 油門值曲線）

圖11 飛行日志（相對高度、氣壓高度、高度補償曲線）

圖12 飛行日志（相對高度、理想相對高度、高度補償曲線）

5 結(jié)論

本文充分考慮無人機近地面作業(yè)時迅速應對環(huán)境的多變性，設(shè)計了一種基于激光雷達的仿地飛行系統(tǒng)。激光雷達可以達到高精度測距的效果，并且可以改變輸出頻率來調(diào)節(jié)無人機對地面變化的感知力，改變接收激光信號強度閾值來適合不同應用環(huán)境。云臺經(jīng)過數(shù)據(jù)融合提高了MPU6050 給出角速度的精度，這種融合還消除了無人機飛行姿態(tài)偏置對估計角速度的影響。最后在仿地飛行算法中利用模糊PID 控制算法取代傳統(tǒng)的PID 控制，大幅度提高了系統(tǒng)的反應速度和控制精度。試飛實驗結(jié)果表明，無人機能夠非常好地實現(xiàn)仿地飛行效果，對地面高度改變的應對實時性很高。仿地飛行系統(tǒng)最終得到了實現(xiàn)，希望該系統(tǒng)能在某些仿地飛行應用中得到應用。