煉焦化工廠復雜環境下的無人機巡檢控制與規劃

宮則強 鄭曦星 趙偉

[摘 ? ?要]針對煉焦化工廠環境下無人機巡檢問題進行了研究。針對工廠復雜的管線通道分布，提出了三維坐標系下的無人機動力學建模、基于PID控制方法的機體運動控制及以A*算法為基礎的巡線規劃算法。經仿真測試與現場實測都達到了良好的巡檢效果，實現了煉焦化工廠復雜環境下的無人機智能巡檢。

[關鍵詞]煉焦化工廠;無人機;巡檢;PID;A*算法

[中圖分類號]TP311.52 [文獻標志碼]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 系統說明

1.1 無人機選型

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

1.2 機載氣體傳感儀器

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

1.3 巡檢管理系統架構

巡檢管理系統將無人機GPS自動導航飛行能力與高精度一氧化碳檢測技術進行結合，實現對管道的巡線和泄漏焦爐煤氣的檢測。硬件設備主要包括無人機、數據庫服務器、機載一氧化碳檢測裝置和數據傳輸設備，軟件系統包括CO檢測系統管理軟件、移動APP。

2 無人機控制算法設計

2.1 動力學建模

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

在上述坐標系定義基礎上，根據牛頓-歐拉公式，給出無人機的動力學模型。

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

2.2 控制器設計

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

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

（3）控制器總體設計

根據實際的工廠環境需要，將姿態控制作為控制內環，位置控制作為外環。將二者按照閉環控制方法將飛行控制器設計為內外環結構，其中外環的位置控制器控制輸出作為內環姿態控制器的控制輸入。整體控制器設計（圖1）如下。

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

3 無人機規劃算法設計

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

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

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

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

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

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

式中，f*（n）為從初始狀態經由節點n到目標節點的最小代價估計，g*（n）為在狀態空間中從初始節點到節點n的最小代價，h*（n）為從節點n到目標節點的路徑的最小估計代價。

算法主要流程是估計值基礎上對實際h（n）的選取：

保證找到最短路徑（最優解的）條件，關鍵在于估價函數f*（n）的選取（或者說h（n）的選取）。

以h（n）表達節點n到目標節點的估計距離。那么h（n）的選取分為以下三種情況進行討論：

（1）當h（n）