煉焦化工廠復(fù)雜環(huán)境下的無人機(jī)巡檢控制與規(guī)劃

宮則強(qiáng) 鄭曦星 趙偉

[摘 ? ?要]針對煉焦化工廠環(huán)境下無人機(jī)巡檢問題進(jìn)行了研究。針對工廠復(fù)雜的管線通道分布，提出了三維坐標(biāo)系下的無人機(jī)動力學(xué)建模、基于PID控制方法的機(jī)體運(yùn)動控制及以A*算法為基礎(chǔ)的巡線規(guī)劃算法。經(jīng)仿真測試與現(xiàn)場實測都達(dá)到了良好的巡檢效果，實現(xiàn)了煉焦化工廠復(fù)雜環(huán)境下的無人機(jī)智能巡檢。

[關(guān)鍵詞]煉焦化工廠;無人機(jī);巡檢;PID;A*算法

[中圖分類號]TP311.52 [文獻(xiàn)標(biāo)志碼]A [文章編號]2095–6487（2022）01–00–03

[Abstract]This article studies the problem of drone inspection in the environment of a coking chemical plant. Aiming at the complex pipeline channel distribution in the factory， the UAV dynamic modeling in the three-dimensional coordinate system， the body motion control based on the PID control method， and the line-following planning algorithm based on the A* algorithm are proposed. Both the simulation test and the on-site test have achieved good inspection results， and the intelligent inspection of the drone in the environment of the coking chemical plant has been realized.

[Keywords]coking and chemical plant; UAV; inspection; PID; A* algorithm

1 系統(tǒng)說明

1.1 無人機(jī)選型

本文研究的算法使用了深圳大疆創(chuàng)新制造的Matrice 300RTK（以下簡稱M300）無人機(jī)作為飛行平臺。該型號無人機(jī)是X型四旋翼布局，機(jī)臂能夠折疊收縮。該機(jī)型能夠很好地用于管線巡檢，并安裝有遠(yuǎn)程操作接口，可用于遠(yuǎn)程地面站控制與數(shù)據(jù)回傳。

1.2 機(jī)載氣體傳感儀器

機(jī)載氣體傳感儀器主要基于STM32和泵吸式氣體傳感器，安裝于M300的機(jī)腹位置，用于巡檢時檢測泄漏的一氧化碳?xì)怏w并回傳至遠(yuǎn)端的管理軟件/手機(jī)APP，并在遠(yuǎn)端超過報警閾值后予以示警。

1.3 巡檢管理系統(tǒng)架構(gòu)

巡檢管理系統(tǒng)將無人機(jī)GPS自動導(dǎo)航飛行能力與高精度一氧化碳檢測技術(shù)進(jìn)行結(jié)合，實現(xiàn)對管道的巡線和泄漏焦?fàn)t煤氣的檢測。硬件設(shè)備主要包括無人機(jī)、數(shù)據(jù)庫服務(wù)器、機(jī)載一氧化碳檢測裝置和數(shù)據(jù)傳輸設(shè)備，軟件系統(tǒng)包括CO檢測系統(tǒng)管理軟件、移動APP。

2 無人機(jī)控制算法設(shè)計

2.1 動力學(xué)建模

無人機(jī)在工廠環(huán)境中運(yùn)動時，主要是在三維空間SO（3）中發(fā)生坐標(biāo)與姿態(tài)變換，因此在該向量空間中首先建立坐標(biāo)系。建立了兩個常用的坐標(biāo)系，包括大地坐標(biāo)系和機(jī)體坐標(biāo)系，這兩個坐標(biāo)系都符合右手定則，其中大地坐標(biāo)系的原點定義為無人機(jī)質(zhì)心的初始位置，XE指向正北方向，YB指向正西方向，ZB指向地心方向的反方向。機(jī)體坐標(biāo)系與無人機(jī)固連，原點OB與無人機(jī)的質(zhì)心重合，XB指向機(jī)頭方向。無人機(jī)的姿態(tài)常用歐拉角表示，即機(jī)體坐標(biāo)系相對于大地坐標(biāo)系的姿態(tài)。地球大地坐標(biāo)系與機(jī)體坐標(biāo)系的原點重合，機(jī)體坐標(biāo)系先后繞ZB軸、YB和XB軸旋轉(zhuǎn)一定角度與大地坐標(biāo)系重合。其中，繞XB軸旋轉(zhuǎn)的角度為滾轉(zhuǎn)角，繞YB軸旋轉(zhuǎn)的角度為俯仰角，繞ZB軸旋轉(zhuǎn)的角度為偏航角。這些角度的正方向，由右手螺旋定則定義。

在上述坐標(biāo)系定義基礎(chǔ)上，根據(jù)牛頓-歐拉公式，給出無人機(jī)的動力學(xué)模型。

將無人機(jī)位置和姿態(tài)都用三維向量表示。位置坐標(biāo)表示為（x，y，z），姿態(tài)使用用歐拉角形式，表示為（φ，θ，ψ）。機(jī)體轉(zhuǎn)動角速度在機(jī)體坐標(biāo)系下表示為（p，q，r）。所以根據(jù)牛頓-歐拉方程可以建立無人機(jī)在空間中的平動與轉(zhuǎn)動方程：

2.2 控制器設(shè)計

在本文研究中，主要是在煉焦化工廠中使用，環(huán)境復(fù)雜且干擾因素多，很難得到精確模型。因此，選擇使用不依賴模型的控制方法。

而這其中經(jīng)典比例-積分-微分（Proportional Integral Derivative，PID）控制方法[1]是不依賴模型控制方法中工程應(yīng)用最廣泛的，效果比較突出的一類方法。通過參考輸入與實際輸出之間的誤差作為控制器的輸入信號，輸出控制量消除被控對象的誤差[2]。PID方法可用如下公示表示

（3）控制器總體設(shè)計

根據(jù)實際的工廠環(huán)境需要，將姿態(tài)控制作為控制內(nèi)環(huán)，位置控制作為外環(huán)。將二者按照閉環(huán)控制方法將飛行控制器設(shè)計為內(nèi)外環(huán)結(jié)構(gòu)，其中外環(huán)的位置控制器控制輸出作為內(nèi)環(huán)姿態(tài)控制器的控制輸入。整體控制器設(shè)計（圖1）如下。

位置控制器根據(jù)期望位置和實際位置計算出期望總拉力fd、期望滾轉(zhuǎn)角φd、期望俯仰角θd;姿態(tài)控制器根據(jù)由外部控制指令發(fā)送的期望偏航角ψd、實際姿態(tài)以及位置控制器的輸出計算出力矩期望τd;控制分配根據(jù)fd和τd計算出4個旋翼的期望轉(zhuǎn)速ωd，i;最后，電機(jī)控制器根據(jù)ωd，i計算出每個電機(jī)輸出功率σd，i。

3 無人機(jī)規(guī)劃算法設(shè)計

在實際的煉焦化工廠環(huán)境中，整體的環(huán)境是一個三維空間，其中管道走向以橫平豎直的方式為主，但周圍的建筑物等會構(gòu)成一些障礙點。當(dāng)無人機(jī)進(jìn)行巡檢航跡規(guī)劃時，需要將管線的走向與障礙點的位置坐標(biāo)進(jìn)行全盤規(guī)劃計算。所以一般首先會選擇利用全局靜態(tài)規(guī)劃參考相關(guān)的信息來生成多個航跡點，以此來形成一條完整的飛行軌跡，實現(xiàn)自主跟蹤航跡飛行。在規(guī)劃算法生成航跡以后，無人機(jī)按照指定的軌跡從第一個航跡點出發(fā)，根據(jù)航跡點順序依次移動，直至飛行目標(biāo)完成到達(dá)最后一個航跡點，但是無人機(jī)在執(zhí)行飛行任務(wù)的過程中存在遇到突發(fā)性障礙的可能性，因此需要同時結(jié)合局部動態(tài)規(guī)劃來進(jìn)行實時規(guī)劃。

本文采用的是A*（又稱A-Star）算法[3]，該算法是在靜態(tài)網(wǎng)絡(luò)中規(guī)劃最短路徑的直接搜索方法[4-5]。將整個無人機(jī)規(guī)劃的地圖抽象為圖的表示形式

G=（V，E） ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（8）

式中，V為節(jié)點集合，E為節(jié)點間邊的集合，邊的和就是不同節(jié)點間的距離。

A*算法的計算公式表示為

f*（n）=g*（n）+h*（n） ? ? ? ? ? ? ? ? ? （9）

式中，f*（n）為從初始狀態(tài)經(jīng)由節(jié)點n到目標(biāo)節(jié)點的最小代價估計，g*（n）為在狀態(tài)空間中從初始節(jié)點到節(jié)點n的最小代價，h*（n）為從節(jié)點n到目標(biāo)節(jié)點的路徑的最小估計代價。

算法主要流程是估計值基礎(chǔ)上對實際h（n）的選?。?/p>

保證找到最短路徑（最優(yōu)解的）條件，關(guān)鍵在于估價函數(shù)f*（n）的選?。ɑ蛘哒fh（n）的選取）。

以h（n）表達(dá)節(jié)點n到目標(biāo)節(jié)點的估計距離。那么h（n）的選取分為以下三種情況進(jìn)行討論：

（1）當(dāng)h（n）