摘 要：針對工廠智能輸送機自主巡航場景中對不同路徑點進行導(dǎo)航等問題，采用深度攝像頭及多種傳感器以及視覺即時定位和地圖構(gòu)建技術(shù)（SLAM），通過深度攝像頭采集圖像信息和所處位置對應(yīng)的景深信息，構(gòu)造點云，使機器人能夠獲取當前所處位置的信息，再通過對點云的分析還原計算機器人的位姿，實現(xiàn)機器人的定點巡航及路徑規(guī)劃。

關(guān)鍵詞：工廠智能輸送機；傳感器；深度攝像頭；地圖構(gòu)建技術(shù)（SLAM）；移動線程；定點巡航；路徑規(guī)劃

中圖分類號：TP31；TP39 文獻標識碼：A 文章編號：2095-1302（2024）06-0-03

0 引 言

隨著人工智能和現(xiàn)代工業(yè)制造技術(shù)近年來的迅速發(fā)展，對于一些重復(fù)性比較強或者一些極其精密的工作，機器人能夠代替人類完成，在某些程度上比人類做得更好[1-2]。我國也將逐步進入工業(yè)4.0時代，國家出臺了相關(guān)政策來引導(dǎo)傳統(tǒng)制造智能化提升，推廣使用多種先進技術(shù)，如物聯(lián)網(wǎng)、大數(shù)據(jù)、機器視覺、人工智能等，進一步實現(xiàn)工業(yè)制造的高效化和智能化[3-4]?；谌斯ぶ悄芎湍Ｊ阶R別技術(shù)的不斷進步，機器人的智能化程度越來越高，其對外界的感知能力進一步加強，結(jié)合物聯(lián)網(wǎng)、傳感器等技術(shù)的綜合運用，能夠適應(yīng)各種復(fù)雜的工作場景和任務(wù)，應(yīng)用范圍逐漸從工業(yè)領(lǐng)域擴展到軍事、娛樂、航天、醫(yī)療等領(lǐng)域。隨著SLAM技術(shù)重要性的凸顯，國內(nèi)越來越多科技人員研發(fā)和應(yīng)用SLAM技術(shù)，并將其廣泛應(yīng)用于定位導(dǎo)航模塊等產(chǎn)品[5-6]。張飛等

人[7]利用機器人設(shè)計原理，針對傳統(tǒng)輸送帶偏差監(jiān)測技術(shù)不能準確檢測輸送帶偏差且監(jiān)測成本高的問題，設(shè)計了一種基于圖像處理技術(shù)的輸送帶偏差監(jiān)測系統(tǒng)。系統(tǒng)中有配備網(wǎng)絡(luò)監(jiān)控攝像頭的移動機器人，融合了多種傳感器，利用輸送帶運行狀態(tài)的實時圖像對可能出現(xiàn)的輸送帶偏差進行實時監(jiān)控和預(yù)警。

文中將物聯(lián)網(wǎng)、機器視覺以及SLAM技術(shù)應(yīng)用于工廠智能輸送機應(yīng)用場景。由于工廠智能輸送機需要對巡視的場所進行圖像等參數(shù)的采集，故可采用視覺SLAM技術(shù)。攝像頭既可以采集所需圖像信息，也可作為工廠智能輸送機的場景建圖感知傳感器。工廠智能輸送機可自主移動，完成對場景的建圖，采用人工控制，實現(xiàn)對場景的遍歷。建圖后由人工在地圖上標注要巡檢位置在工廠智能輸送機所建地圖上對應(yīng)的坐標點，讓工廠智能輸送機自動實現(xiàn)對建圖場景的遍歷[8-9]。與其他具有代表性的視覺和視覺慣性系統(tǒng)相比，ORB-SLAM2是一種適用于單目、立體和RGB-D相機的SLAM系統(tǒng)，包括地圖重用、閉環(huán)和重新定位功能。ORB-SLAM3是對短期、中期、長期數(shù)據(jù)關(guān)聯(lián)的視覺、視覺慣性系統(tǒng)，能夠使用單眼、立體和RGB-D相機執(zhí)行視覺、視覺慣性和多地圖SLAM系統(tǒng)任務(wù)。所使用的針孔和魚眼鏡頭模型具有廣泛的應(yīng)用場景[10-11]。文中使用ORB-SLAM技術(shù)，還提出了一種移動線程設(shè)計方法，使工廠智能輸送機達到預(yù)期設(shè)計目標。

1 相關(guān)技術(shù)

通過攝像頭這一視覺傳感器，綜合慣性傳感器、超聲波傳感器等可將工廠巡檢場景的物理世界轉(zhuǎn)換成數(shù)字信號，經(jīng)過物聯(lián)網(wǎng)技術(shù)實現(xiàn)相關(guān)設(shè)備數(shù)據(jù)的聯(lián)網(wǎng)和傳輸。由云端對現(xiàn)場數(shù)據(jù)進行處理和分析，相關(guān)結(jié)果再次傳輸?shù)浆F(xiàn)場設(shè)備并指導(dǎo)現(xiàn)場設(shè)備按照技術(shù)要求執(zhí)行相關(guān)操作。

為實現(xiàn)工廠智能輸送機的巡檢功能，需要對場景感知和對自身定位，并對巡邏目標位置的參數(shù)進行監(jiān)控。在傳統(tǒng)操作過程中，帶式輸送機的性能大多是通過人工測試及管控，耗時長且效率低。在智能輸送機解決方案中，輸送帶運行狀態(tài)的全方位可視化監(jiān)控是智能輸送機運維系統(tǒng)的重要組成部分，通過智能化分析方法有助于提高檢測的實時性和有效性，有助于相關(guān)人員在工況現(xiàn)場做出合理決策。針對現(xiàn)有技術(shù)無法實現(xiàn)一體化檢測的現(xiàn)狀，文中介紹了一種新型帶式輸送機智能檢測系統(tǒng)，融合物聯(lián)網(wǎng)、機器視覺及傳感器融合技術(shù)，提出了一種智能輸送機移動線程設(shè)計方法。預(yù)計在不久的將來，帶式輸送機的智能檢測將得到廣泛應(yīng)用[9]。

2 移動線程設(shè)計方法

2.1 SLAM中狀態(tài)估計

視覺SLAM的狀態(tài)估計只包括相機當前位姿，而視覺慣性SLAM還需要計算其他變量[7-8]。坐標系中的相機位姿包括姿態(tài)Ti、速度vi、陀螺儀、加速度計的漂移big和bia，在移動線程設(shè)計中將多個參數(shù)合并為一個狀態(tài)量：

（1）

對于視覺慣性SLAM，通過在連續(xù)視頻幀i和i+1之間預(yù)積分慣性傳感器，得到預(yù)測量的旋轉(zhuǎn)、速度和位置信息，分別表示為ΔRi， i+1、Δvi， i+1、Δpi， i+1，i到i+1的預(yù)積分方差表示為Στi， i+1。已知預(yù)積分量和狀態(tài)量Si和Si+1，得到慣性殘差為rTi， i+1。結(jié)合視覺和慣性殘差公式，視覺慣性SLAM為一個基于關(guān)鍵幀的優(yōu)化問題，表示如下：

（2）

式中：ρHub為Huber核函數(shù)，用于減小誤匹配的影響；rij為幀與3D點之間的視覺殘差。

式（2）優(yōu)化問題的基本框架如圖1所示。該問題需要良好的初始值以確保收斂效果，提高跟蹤和建圖過程中的檢測效率。

慣性傳感器初始化的目標是為慣性變量獲得良好的初始值：本體速度、重力方向等?？紤]到傳感器的不確定性，我們將慣性傳感器初始化問題描述為MAP估計問題，分為三個步驟：（1）純視覺MAP估計，在初始化時進行這項操作，以4 Hz插入關(guān)鍵幀，得到規(guī)模為k10個相機姿態(tài)和數(shù)百個點組成的有尺度的圖，如圖1（b）所示；（2）純慣性MAP估計，目標是獲得慣性變量的最優(yōu)估計，使用關(guān)鍵幀之間的軌跡和慣性測量值；（3）視覺-慣性MAP估計。一旦對慣性和視覺參數(shù)有了良好的估計，就可以執(zhí)行視覺-慣性聯(lián)合優(yōu)化，細化解決方案。所有關(guān)鍵幀都有相同的偏差，并且包含與純慣性步驟相同的偏差先驗信息。

2.2 局部地圖構(gòu)建線程

對跟蹤線程送來的關(guān)鍵幀進行處理，更新關(guān)鍵點和幀之間的關(guān)聯(lián)并刪除一些無效匹配。通過制作共視關(guān)鍵幀對一些匹配進行三角測量，并將當前幀中具有最佳共視關(guān)系的幀稱為第一幀和第二幀。將第一和第二級幀指定投影到當前幀，并將關(guān)鍵幀指定投影到第一和第二級幀。在融合方法中，如果MapPoint投影的特征點已有MapPoint，則將2個MapPoint合并（選擇多個觀測值）來匹配關(guān)鍵幀的特征點。若該點無對應(yīng)MapPoint，則將MapPoint添加到該點上。在匹配特征點時，利用當前幀的位姿投影到當前幀的圖像坐標，利用深度確定尺度和搜索半徑，在當前幀的網(wǎng)格中找到滿足匹配條件的特征點。

2.3 回環(huán)檢測線程

在Sphere-SLAM2系統(tǒng)中，準確可靠的定位和測繪是自動駕駛實現(xiàn)路徑規(guī)劃的關(guān)鍵前提。然而在大規(guī)模復(fù)雜動態(tài)環(huán)境下，傳統(tǒng)的LiDAR閉環(huán)檢測方法依賴于半徑搜索，容易產(chǎn)生假陰性，不能有效糾正累積誤差。ORB-SLAM使用ORB特性，其描述符提供中短期數(shù)據(jù)關(guān)聯(lián)，構(gòu)建公共視圖限制跟蹤和映射的復(fù)雜性，使用同義詞庫DBoW2進行閉環(huán)和重定位[9]，實現(xiàn)長期數(shù)據(jù)關(guān)聯(lián)。它集成了3種類型數(shù)據(jù)關(guān)聯(lián)的可視化SLAM系統(tǒng)，該系統(tǒng)在跟蹤丟失時啟動新地圖，并使用新的位置識別方法提高了召回率，從而提高了其在循環(huán)場景中的準確性。ORB-SLAM3在ORB-SLAM2和ORB-SLAM-VI的基礎(chǔ)上發(fā)展起來[11]，它是一個完整的多地圖和多會話系統(tǒng)，能夠在純視覺或視覺慣性模式下工作，具有單眼、立體或RGB-D傳感器，使用針孔和魚眼相機模型。具體流程如下：先找到跟當前幀有可能閉環(huán)的候選關(guān)鍵幀，進行幾何一致性驗證；根據(jù)BOW的相似性給出一定范圍的閾值，給出與當前關(guān)鍵幀最有可能形成閉環(huán)幀的候選關(guān)鍵幀，利用候選幀組的連續(xù)性，完成幾何一致性驗證，相信與幾個候選幀關(guān)鍵組連續(xù)的候選幀是閉環(huán)關(guān)鍵幀。

當閉合回環(huán)達到一定程度時，利用通用圖優(yōu)化框架優(yōu)化回環(huán)位姿圖，如圖2（a）所示，再更新地圖點，融合重復(fù)的地圖點，位姿圖優(yōu)化如下：

（3）

式中：，Xi， j是位姿X， j到位姿X， i

的相似變換；logsim（3）將轉(zhuǎn)移矩陣的位姿誤差映射到7維歐氏空間R7；Δi， j為邊的信息矩陣。

由圖2可以看出，圖的匹配點較多，主要原因是當前的兩張圖片場景高度相似，我們可以利用方向一致性進行匹配：統(tǒng)計兩張圖像所有匹配對中兩個特征點主方向的差，構(gòu)建一個直方圖。由于兩張圖像整體發(fā)生了運動，所以特征點匹配對主方向整體會有一個統(tǒng)一的一致變化，一般認為直方圖里前三個最大的格子里就是正常的匹配點對，可以刪除一些誤匹配特征點。剔除錯誤匹配后的特征點匹配如圖2（b）所示。

2.4 自主導(dǎo)航

通過以上操作能夠?qū)崿F(xiàn)工廠智能輸送機在導(dǎo)航地圖中的定位。在地圖中的導(dǎo)航采用常用A*（A Star）算法實現(xiàn)路徑的最優(yōu)規(guī)劃。A*算法是一種常用的路徑查找和圖形遍歷算法，具有較好的性能和準確度，適用于靜態(tài)場景。該算法是綜合廣度優(yōu)先搜索算法、Dijkstra算法和最佳優(yōu)先搜索算法特點的一種改進算法，能夠計算每個節(jié)點的優(yōu)先級：

（4）

式中：G（n）為節(jié)點n的綜合優(yōu)先級，在選擇下一個要遍歷的節(jié)點時，總選擇綜合優(yōu)先級最高的節(jié)點（即G（n）數(shù)值最低的節(jié)點）；η（n）為到節(jié)點n的起點的距離代價；（n）為到節(jié)點n的端點的距離期望代價，是A*算法的啟發(fā)式函數(shù)。

A*算法使用2個集合來表示要遍歷的節(jié)點和已經(jīng)遍歷的節(jié)點，稱為open_set和close_set。

2.5 在地圖中的定位和跟蹤線程

要使工廠智能輸送機能夠?qū)Ш?，需要使用融合環(huán)境的路標點信息以及視覺信息的特征點定位圖層，以此達到快速定位的目的。要實現(xiàn)該功能，利用ORB關(guān)鍵點快速對圖像提取特征點及其路標信息，并對兩者建立對應(yīng)的hash表，構(gòu)建用于工廠智能輸送機定位的圖層。要快速找到工廠智能輸送機采集的圖像在導(dǎo)航圖層中對應(yīng)的關(guān)鍵幀，與定位圖層中N個關(guān)鍵幀比較，并計算其相似度s，找到與該圖像相似度最高的關(guān)鍵幀；然后對工廠智能輸送機當前的位姿進行估計，通過找到與工廠智能輸送機當前圖像最接近的關(guān)鍵幀，計算當前圖像與關(guān)鍵幀特征點的漢明距離并匹配；在速度空間（v， w）中采樣，假設(shè)當前工廠智能輸送機位于（x， y），由不同的采樣速度推算出多條軌跡，表示為：

（5）

式中：（x'， y'）為推算出來的工廠智能輸送機的位置；θ為推算的航向偏轉(zhuǎn)角度；w為角速度；Δt為采樣時間的間隔。

由式（5）得到運動模型，然后利用評價函數(shù)對工廠智能輸送機的運動軌跡進行評估，得到工廠智能輸送機在當前速度下，軌跡末端姿態(tài)與目標點之間的角度差異。評價函數(shù)表示為：

（6）

式中：heading（v， w）為工廠智能輸送機根據(jù)DWA算法推演出的最終航向角與目標點航向角的偏差；dist（v， w）為工廠智能輸送機處于預(yù)測軌跡末端點位置時與地圖上最近障礙物的距離。對于靠近障礙物的采樣點進行懲罰，確保工廠智能輸送機的避障能力，降低工廠智能輸送機與障礙物發(fā)生碰撞的概率；velocity（v， w）為當前工廠智能輸送機的線速度，為了促進工廠智能輸送機快速到達目標；α、β、γ、σ為權(quán)重。

利用該評價函數(shù)可以得到最優(yōu)的運動策略。根據(jù)系統(tǒng)需求，工廠智能輸送機上位機需要獲取圖像以及對應(yīng)場景的深度信息，利用深度攝像頭和機器人自身的傳感器信息，兩者相互融合實現(xiàn)SLAM建圖及導(dǎo)航；一般將傳感器安裝在工廠智能輸送機的底盤上，因此該信息由下位機單片機讀取，之后通過串口發(fā)送給上位機；同時，上位機也可通過串口向下位機發(fā)送控制指令。另外，下位機在運行時為防止碰撞還要加裝距離傳感器。

3 結(jié) 語

文中給出了工廠智能輸送機的移動線程設(shè)計思路。采用搭載RGB-D攝像頭的上位機讀取搭載傳感器模塊的工廠智能輸送機信息實現(xiàn)視覺SLAM，利用ORB-SLAM3算法構(gòu)建地圖，建立導(dǎo)航地圖層。在層中設(shè)置導(dǎo)航點，利用工廠智能輸送機視覺圖像信息快速匹配導(dǎo)航層關(guān)鍵幀，用于確定工廠智能輸送機在導(dǎo)航中的位置和姿態(tài)。通過A*算法實現(xiàn)路徑規(guī)劃，由工廠智能輸送機底盤的運動模型求解，并將工廠智能輸送機的線速度和角速度發(fā)送給底盤控制器?？刂破魍ㄟ^控制左右電機引導(dǎo)工廠智能輸送機運動，實現(xiàn)工廠智能輸送機巡航，提高了輸送機的工作效率。

參考文獻

[1]易柯敏，沈艷霞.激光SLAM導(dǎo)航工業(yè)機器人定位算法研究綜述[J].工業(yè)機器人技術(shù)與應(yīng)用，2019，32（5）：25-28.

[2]高超.基于直接法的工業(yè)機器人半稠密地圖構(gòu)建研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2017.

[3] TETYANA B，ERNST K，DONALD C. Wunsch II. Computer Intelligent Systems for Manufacture and Control [M]. Intelligent Automation in Renewable Energy. 2019.

[4] DURRANT W H F. Uncertain geometry in robotics [J]. IEEE journal on robotics amp;automation，1988，4（1）：23-31.

[5] GRISETTI G，STACHNISS C，BURGARD W. Improved techniques for grid mapping with Rao-black wellized particle filters [J]. IEEE transactions on robotics，2007，23（1）：34-46.

[6]王博實.基于SLAM系統(tǒng)的準稠密三維重建[D].北京：北京郵電大學(xué)，2018

[7]張飛，朱鋒，田圣彬，等.基于圖像處理技術(shù)的輸送帶跑偏移動監(jiān)測系統(tǒng)設(shè)計[J].礦山機械，2021，49（11）：16-19.

[8]尚任.一種視覺SLAM單目半稠密建圖方法的實現(xiàn)[J].智能計算機與應(yīng)用，2021，11（1）：89-93

[9] REN R，F(xiàn)U H，XUE H，et al. Towards a fully auto-mated 3D reconstruction system based on LiDAR and GNSS in challenging scenarios [J]. Remote sensing，2021，13（10）：1981.

[10] VIDAL A R，REBECQ H，HORSTSCHAEFER T，et al. Ultimate SLAM-Combining events，images，and IMU for robust visual SLAM in HDR and high speed scenarios [J]. IEEE robotics and automation letters，2018，3（2）：994-1001.

[11] CAMPOS C，ELVIRA R，RODRíGUEZ J，et al. ORB-SLAM3： An accurate open-source library for visual，visual-inertial，and multimap SLAM [J]. IEEE transactions on robotics，2021，37（6）：1874-1890.

作者簡介：郭月飛（1978—），男，碩士學(xué)位，工程師，研究方向為通信與物聯(lián)網(wǎng)技術(shù)。

白海鋒（1984—），男，碩士學(xué)位，研究方向為物聯(lián)網(wǎng)與傳感器技術(shù)。