摘要：針對監(jiān)測機器人運動控制定位不精準等問題，本文研究了受限空間監(jiān)測機器人運動控制系統(tǒng)的定位傳感器、硬件和軟件系統(tǒng)。本文研制了一款小尺寸激光位移傳感器，提出基于直線線性變換（DLT）和非線性優(yōu)化的外參計算方法以及改進灰度重心法的中心線提取算法。在硬件方面改進了機器人電器電路，提出基于大疆M3508設(shè)計的里程計程序和紅外傳感技術(shù)（RFID）聯(lián)合軌道定位的機器人定位方法。在軟件方面，根據(jù)受限空間工作要求設(shè)計了機器人軟件系統(tǒng)。為隧道受限空間監(jiān)測機器人的整體設(shè)計提供參考。

關(guān)鍵詞：受限空間；監(jiān)測機器人；定位傳感器；運動控制；精確定位

中圖分類號：TP242""""""""" 文獻標志碼：A

隨著經(jīng)濟發(fā)展和社會進步，我國的隧道建設(shè)總長逐年增加，然而在隧道的使用過程中，受地質(zhì)災(zāi)害的影響易產(chǎn)生橫向變形和縱向不均勻沉降，容易造成隧道管片接縫張開與錯動，從而影響隧道的防水效果[1-2]，嚴重威脅生命和財產(chǎn)安全[3]。因此，定期監(jiān)測隧道病害必不可少。目前，部分國家采用智能監(jiān)測機器人對隧道受限空間進行檢測和維護，輪式管道機器人具有運行穩(wěn)、移動快和效率高等優(yōu)勢，應(yīng)用廣泛。“巡游者”機器人利用圖像識別進行機器人定位導(dǎo)航，高壓電力廊道自動巡檢機器人結(jié)合激光傳感器和RFID紅外傳感技術(shù)進行地圖構(gòu)建、定位導(dǎo)航。由于目前研究多集中于對隧道內(nèi)的運行情況進行監(jiān)測，因此存在受限空間監(jiān)測機器人的運動控制定位不精準、在軌運行不穩(wěn)定等問題。因此，本文對受限空間監(jiān)測機器人運動控制系統(tǒng)的定位傳感器、硬件和軟件系統(tǒng)進行了研究，為隧道受限空間監(jiān)測機器人的整體設(shè)計提供參考。

1運動控制傳感器

1.1線激光傳感器

為監(jiān)測管片接縫張開量，提高傳感器效能，本文研制了一款小尺寸線激光傳感器。其采用嵌入式邁德威視MV-SUM134GM相機，激光傳感器為中科院長春光機所研制的MDL-SL激光模組，傳感器樣機如圖1所示。線激光傳感器和相機按照一定的角度擺放，線結(jié)構(gòu)光發(fā)出一束線激光投射至被測物體上，相機捕捉線結(jié)構(gòu)光條紋在被測物體上的投影，使用標定計算得線結(jié)構(gòu)光平面在相機坐標系中的方程，提取線結(jié)構(gòu)光投影的中心線，利用線結(jié)構(gòu)光條紋在光平面上的約束得到線結(jié)構(gòu)光條紋的三維空間坐標。掃描整個物體得到被測物體表面輪廓的三維坐標信息。

1.2線激光定位傳感器標定原理

在樣機研制過程中，本文提出一種基于直線線性變換（DLT）和非線性優(yōu)化的外參計算方法以及改進灰度重心法的中心線提取算法。此外，在光平面的計算過程中，采用隨機采樣一致性算法（RANSAC）以及非線性優(yōu)化得到精確的光平面方程。

1.2.1棋盤格平面方程計算

計算棋盤格平面在相機坐標系中的平面方程來標定結(jié)構(gòu)光系統(tǒng)，本文采用張氏標定法進行計算。某個位姿中的棋盤格，其坐標系使用旋轉(zhuǎn)矩陣Ri和平移向量ti轉(zhuǎn)換至相機坐標系，已知3個棋盤格中非共線標定點坐標，如公式（1）所示。

（1）

式中：A、B、C為世界坐標系，A'、B'、C'為相機坐標系。ti為平移向量；Ri為旋轉(zhuǎn)矩陣。

棋盤格在相機坐標系中的法向量如公式（2）所示。

ni=A'B'×A'C'（2）

記法向量表達式為ni=（nix，niy，niz），棋盤格在相機坐標系中的平面方程如公式（3）所示。

nix（x-X'ai）+niy（y-Y'ai）+niz（z-Z'ai）（3）

在試驗過程中發(fā)現(xiàn)該方法有一定誤差，因此，本文引入PnP問題的經(jīng)典求解方法：DLT以及光束平差法（Bundle Adjustment）來得到比較精確的相機外參。在提取像素角點的過程中，采用亞像素角點提取算法來保證角點提取的精度。亞像素角點提取如圖2所示，p為待計算的亞像素角點。如公式（4）所示。

（pi-p）·Gi=0（i=1，2）（4）

式中：Gi為點pi處的梯度；Pi為人工定義的坐標軸上第i個角點的三維坐標。

將公式（4）移項展開，使用最小二乘法進行計算，如公式（5）所示。

p=（GiTGi）-1GiTGipi（5）

式中：GiT為Gi的轉(zhuǎn)置矩陣。

在原始角點處選擇1個矩形窗口，計算窗口內(nèi)所有像素點的和以及權(quán)重，得到亞像素角點，如公式（6）所示。

（6）

式中：ωi為第i個角點的權(quán)重；N為總角點數(shù)。

以亞像素角點為起始角點進行迭代計算，能獲取更精確的角點坐標。

采用DLT求解3D點與2D點之間的運動（不通）。通過旋轉(zhuǎn)矩陣R和平移向量t從世界坐標系轉(zhuǎn)移到攝像機坐標系，角點可表示為公式（7）。

式中：s為相機對應(yīng)距離參數(shù)；x，y為平面坐標系未知變量；[X，Y，Z]T為世界坐標系中的坐標；t1～t12為12個平移向量矩陣元素；R、t為外參。為了與前文變換矩陣區(qū)分，采用“T”增廣矩陣，并定義增廣矩陣表達式為T-[R|t]。

計算公式（7）可以得到2個約束，如公式（8）所示。

t1'TP-t3'TPx=0，t2'TP-t3'TPy=0（8）

式中：P為空間點的齊次坐標。

采用非線性優(yōu)化算法進行迭代優(yōu)化，得到更精確的位姿，如公式（9）所示。

式中：R*、t*為優(yōu)化后外參；xi為第i個角點像素值的歸一化坐標；n為迭代次數(shù)；si為pi處相機對應(yīng)距離參數(shù)；K為內(nèi)參矩陣。

1.2.2線結(jié)構(gòu)光中心線提取

灰度重心法原理簡單，計算速度較快，但是曲折形狀結(jié)構(gòu)光投影精度不高；Steger算法精度較高，但是后續(xù)處理煩瑣，實時性差。本文根據(jù)這2種方法的特點，提出了一種快速計算法向量、改進灰度重心的中心線提取算法，使用RANSAC算法剔除外點，得到更加準確的中心線。

本文在室內(nèi)光照環(huán)境中進行大量試驗后發(fā)現(xiàn)，雖然光強具有近似高斯分布的特征，但是無法避免環(huán)境噪聲等因素。如果在一定閾值下進行分割，就可能會造成光條紋不完整，從而直接影響后續(xù)中心線提取的結(jié)果。因此，在本文提出的中心線提取算法中，將原始圖像只進行高斯濾波以消除高斯噪聲，并不對其進行閾值分割，這樣可以避免由于光條紋分割不完整導(dǎo)致中心線提取的誤差。由于線結(jié)構(gòu)光的光強比背景光強烈，因此沿著光條紋方向?qū)D像的每一列進行遍歷，將灰度值最高的像素點作為結(jié)構(gòu)光的中心線的初始值。在此基礎(chǔ)上，本文提出一種快速計算光條紋法向量的算法，并在法線方向上對其利用灰度重心法進行加權(quán)，得到更加精確的中心線。為了更加準確地確定中心線的坐標，在每個中心點的法向量方向上使用采取上述方法得到的法向量，以灰度重心法得到更精確的中心線坐標，如公式（10）所示。

（10）

式中：i為圖像上像素點的橫坐標；j為圖像上像素點的縱坐標；W（i，j）為坐標的像素值；x0、y0分別為經(jīng)過權(quán)重計算得到的中心線在像素坐標系下的橫坐標和縱坐標。

利用極值計算近似的中心線，結(jié)合相鄰點計算得到各中心點的法向量，在法向量方向上采用灰色重心法得到更精確的中心線。這樣計算得到的中心點多數(shù)比較準確，使用最小二乘法進行擬合，得到精確的光條紋方程。本文利用MATLAB自帶的曲線擬合工具箱對光條紋直線部分進行擬合計算，大多數(shù)中心點與擬合的中心線的距離偏差不超過0.35ppi。由于環(huán)境噪聲或反光的影響，因此還有一些中心點會偏離擬合的直線。這些離群值會直接影響擬合結(jié)果，對線結(jié)構(gòu)光的標定精度造成影響。為了消除這些離群值，提高標定精度，采用基于RANSAC的方法來得到光條紋的直線方程。其具體流程如下。

步驟一：將所有的中心點作為一個集合M，從中隨機選取n個中心點作為集合N。

步驟二：使用最小二乘法將集合N中的點進行擬合，得到這些點的直線方程。

步驟三：計算集合N外的所有中心點至由步驟二擬合得到的直線方程的距離d。設(shè)定一個固定的閾值T，如果d小于T，則將該中心點當(dāng)作1個內(nèi)點，并將其添加至集合N中。計算更新后的集合N中點的數(shù)量。

步驟四：設(shè)定一個固定值Tc，重復(fù)步驟二和步驟三Tc次，得到Tc個不同的集合N，選擇這些集合中具有最多中心點數(shù)量的集合，記作Nmax。使用最小二乘法將Nmax集合中的所有中心點擬合為最佳直線方程。

利用上述方法可以有效避免離群點導(dǎo)致的擬合直線誤差，在后續(xù)光平面的計算過程中，提高了標定精度。

2運動控制系統(tǒng)硬件電路

隧道管片接縫張開與錯動是隧道中容易出現(xiàn)的問題[4]，需要定期監(jiān)測[5]。為了進行機器人遠程控制以及自動化檢測，對機器人電器電路進行改造，如圖3（a）所示，機器人的主控板采用STM32F407單片機，搭載了紅外熱成像相機、激光位移傳感器和視覺相機，對隧道管片接縫張開和滲漏水病害進行監(jiān)測。為延長工作時間，選擇了容量為10000MA36V10A的鋰電池，并采用電源模塊分壓24V進行供電。

2.1無線通信模塊

無線通信模塊采用ESP8266芯片聯(lián)立上機位和下機位，保證上機位對下機位的指令下達和下機位對上機位的數(shù)據(jù)反饋（包括位置信息和姿態(tài)信息），如圖3（b）所示。ESP8266芯片包括電源電路、重啟復(fù)位、指令、射頻電路和串口5個模塊。ESP8266芯片的接口采用UART方式，可以利用Wi-Fi對相關(guān)數(shù)據(jù)進行高速、高質(zhì)傳輸，同時獲得SOCKET和串口功能，STM32具有無線通信功能，電壓為3.3V。ESP8266芯片用STM32的UART4通信，接收連接STM32的PC11，發(fā)送連接STM32的PC10。

2.2驅(qū)動電機控制模塊

為了保證機器人移動的驅(qū)動力以及合理的功耗，電機采用大疆M3508直流無刷電機，F(xiàn)407和電機之間利用CAN通信，兩路伺服電機和六路舵機利用PWM信號操控傘形機械臂工作。F407利用C620電調(diào)控制直流電機，同時電機也會向主控板反饋電機轉(zhuǎn)速、角度和位置等相關(guān)信息，以達到精準定位的目的，驅(qū)動電機控制模塊如圖3（c）所示。

2.3定位模塊

本文在上述2種機器人定位系統(tǒng)的啟發(fā)下，提出一種基于M3508設(shè)計的里程計程序和RFID聯(lián)合軌道定位的移動機器人的定位方法。利用CAN通信得到M3508直流伺服電機反饋數(shù)據(jù)，建立里程計模塊；將RFID讀取的信息融合至里程計模塊中建立融合定位模塊，對機器人位置進行精確定位，定位模塊如圖3（d）所示。CAN屬于總線式串行通信網(wǎng)絡(luò)，數(shù)據(jù)傳送的速度快、可靠且靈活，CAN總線結(jié)構(gòu)主要包括物理層以及數(shù)據(jù)鏈路層，數(shù)據(jù)鏈路層主要為媒體訪問控制子層MAC和邏輯鏈路控制子層LLC，符合ISO/0SI的標準建設(shè)模型要求，具有更好的靈活性以及透明度。LLC子層可以提供數(shù)據(jù)傳遞服務(wù)以及遠程數(shù)據(jù)請求服務(wù)；MAC子層可以提供控制幀結(jié)構(gòu)、錯誤檢驗、執(zhí)行仲裁、故障界定和出錯標定等傳送規(guī)則服務(wù)。由于STM32無法直接與CAN信號通信，因此在設(shè)計過程中添加TJA1050芯片進行數(shù)據(jù)傳輸，將CAN信號傳遞給STM32。

3軟件系統(tǒng)開發(fā)

本文對監(jiān)測機器人的嵌入式軟件層面進行設(shè)計。其中，嵌入式編程利用C語言進行模塊化編程。根據(jù)隧道巡檢機器人現(xiàn)狀以及當(dāng)前隧道環(huán)境的工作要求，在機器人嵌入式系統(tǒng)架構(gòu)中，主要功能包括機器人系統(tǒng)初始化、傳感器實時采集、網(wǎng)絡(luò)通信收發(fā)、運動控制和安全監(jiān)視等，機器人軟件模塊組成如圖4所示。

3.1系統(tǒng)初始化

系統(tǒng)初始化主要包括GPIO初始化、中斷優(yōu)先級設(shè)置、定時器設(shè)置和CAN初始化。

3.2傳感器采集

分別采集機器人的電機溫度數(shù)據(jù)、位置數(shù)據(jù)、速度數(shù)據(jù)、電流數(shù)據(jù)，可見光和熱成像圖像數(shù)據(jù)以及與障礙物之間的距離。

3.3網(wǎng)絡(luò)通信

網(wǎng)絡(luò)通信主要基于ESP8266模塊的透傳模式，STM32連接Wi-Fi，接收上位機控制指令，同時反饋當(dāng)前狀態(tài)給上位機。

3.4安全監(jiān)視

安全監(jiān)視主要包括機器人的電量監(jiān)視和避障監(jiān)視。如果出現(xiàn)受限空間的避障要求，就會發(fā)出信號，使機器人避障。

3.5運動控制

受限空間監(jiān)測機器人主要有驅(qū)動機構(gòu)運動和伸縮機構(gòu)運動。在嵌入式軟件中分別對驅(qū)動機構(gòu)和伸縮機構(gòu)進行模塊化設(shè)計。機器人運動采用分層控制，分為決策、協(xié)調(diào)和執(zhí)行3層（運動分層控制模型如圖5所示）。決策層需要結(jié)合遠程指令與融合定位信息對監(jiān)測機器人做出運動指令；協(xié)調(diào)層根據(jù)傳感器反饋的執(zhí)行器數(shù)據(jù)協(xié)調(diào)決策層給出的決策信息發(fā)出最終指令；執(zhí)行層由直行電機、升降電機、CHARGE繼電器和剎車機構(gòu)等單元執(zhí)行上位機給出的指令。協(xié)調(diào)層利用運動控制電路控制執(zhí)行層，例如，運動控制單元根據(jù)PMN信號為直行電機和升降電機供電，使用開關(guān)機構(gòu)控制CHARGE繼電器和剎車機構(gòu)。

4結(jié)論

本文研究受限空間監(jiān)測機器人運動控制系統(tǒng)的定位傳感器、硬件和軟件系統(tǒng)。本文研制了一款小尺寸激光位移傳感器，提出基于DLT和非線性優(yōu)化的外參計算方法以及改進灰度重心法的中心線提取算法。在硬件方面，改進了機器人電器電路，提出基于M3508設(shè)計的里程計程序和RFID聯(lián)合軌道定位的機器人定位方法。在軟件方面，根據(jù)受限空間工作要求設(shè)計了機器人軟件系統(tǒng)。

參考文獻

[1]TALMON AM，BEZUIJENA. Analytical model for the beam action of atunnel lining during construction[J].International journal for numerical and analytical methods in geomechanics，2013，37（2）：181-200.

[2]李靖銘，李小雨，王鍥，等.Kalman-BP耦合模型預(yù)測下穿高鐵橋梁盾構(gòu)隧道形變[J]. 湖北民族大學(xué)學(xué)報（自然科學(xué)版），2022，40（4）：445-450.

[3]肖明清，封坤，周子揚，等. 盾構(gòu)隧道施工期管片錯臺影響因素研究[J]. 巖土工程學(xué)報，2023，45（7）：1347-1356.

[4]張琨，陳定方，高銘鑫，等. 有監(jiān)督學(xué)習(xí)下的市域鐵路隧道結(jié)構(gòu)裂縫邊緣識別方法[J]. 中國機械工程，2021，32（4）：446-453.

[5]張志偉，梁榮柱，李忠超，等. 盾尾非對稱推力作用下盾構(gòu)隧道縱向變形分析[J]. 巖土力學(xué)，2023，44（1）：88-98.