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

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

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

[中圖分類號(hào)]TP311.52 [文獻(xiàn)標(biāo)志碼]A [文章編號(hào)]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)說(shuō)明

1.1 無(wú)人機(jī)選型

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

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

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

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

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

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

2.1 動(dòng)力學(xué)建模

無(wú)人機(jī)在工廠環(huán)境中運(yùn)動(dòng)時(shí)，主要是在三維空間SO（3）中發(fā)生坐標(biāo)與姿態(tài)變換，因此在該向量空間中首先建立坐標(biāo)系。建立了兩個(gè)常用的坐標(biāo)系，包括大地坐標(biāo)系和機(jī)體坐標(biāo)系，這兩個(gè)坐標(biāo)系都符合右手定則，其中大地坐標(biāo)系的原點(diǎn)定義為無(wú)人機(jī)質(zhì)心的初始位置，XE指向正北方向，YB指向正西方向，ZB指向地心方向的反方向。機(jī)體坐標(biāo)系與無(wú)人機(jī)固連，原點(diǎn)OB與無(wú)人機(jī)的質(zhì)心重合，XB指向機(jī)頭方向。無(wú)人機(jī)的姿態(tài)常用歐拉角表示，即機(jī)體坐標(biāo)系相對(duì)于大地坐標(biāo)系的姿態(tài)。地球大地坐標(biāo)系與機(jī)體坐標(biāo)系的原點(diǎn)重合，機(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ù)牛頓-歐拉公式，給出無(wú)人機(jī)的動(dòng)力學(xué)模型。

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

2.2 控制器設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

A*算法的計(jì)算公式表示為

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

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

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

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

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

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