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

關(guān)鍵詞：受限空間；監(jiān)測(cè)機(jī)器人；定位傳感器；運(yùn)動(dòng)控制；精確定位

中圖分類號(hào)：TP242""""""""" 文獻(xiàn)標(biāo)志碼：A

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

1運(yùn)動(dòng)控制傳感器

1.1線激光傳感器

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

1.2線激光定位傳感器標(biāo)定原理

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

1.2.1棋盤(pán)格平面方程計(jì)算

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

（1）

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

棋盤(pán)格在相機(jī)坐標(biāo)系中的法向量如公式（2）所示。

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

記法向量表達(dá)式為ni=（nix，niy，niz），棋盤(pán)格在相機(jī)坐標(biāo)系中的平面方程如公式（3）所示。

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

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

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

式中：Gi為點(diǎn)pi處的梯度；Pi為人工定義的坐標(biāo)軸上第i個(gè)角點(diǎn)的三維坐標(biāo)。

將公式（4）移項(xiàng)展開(kāi)，使用最小二乘法進(jìn)行計(jì)算，如公式（5）所示。

p=（GiTGi）-1GiTGipi（5）

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

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

（6）

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

以亞像素角點(diǎn)為起始角點(diǎn)進(jìn)行迭代計(jì)算，能獲取更精確的角點(diǎn)坐標(biāo)。

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

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

計(jì)算公式（7）可以得到2個(gè)約束，如公式（8）所示。

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

式中：P為空間點(diǎn)的齊次坐標(biāo)。

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

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

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

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

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

（10）

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

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

步驟一：將所有的中心點(diǎn)作為一個(gè)集合M，從中隨機(jī)選取n個(gè)中心點(diǎn)作為集合N。

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

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

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

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

2運(yùn)動(dòng)控制系統(tǒng)硬件電路

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

2.1無(wú)線通信模塊

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

2.2驅(qū)動(dòng)電機(jī)控制模塊

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

2.3定位模塊

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

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

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

3.1系統(tǒng)初始化

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

3.2傳感器采集

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

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

網(wǎng)絡(luò)通信主要基于ESP8266模塊的透?jìng)髂Ｊ剑琒TM32連接Wi-Fi，接收上位機(jī)控制指令，同時(shí)反饋當(dāng)前狀態(tài)給上位機(jī)。

3.4安全監(jiān)視

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

3.5運(yùn)動(dòng)控制

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

4結(jié)論

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

參考文獻(xiàn)

[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ù)測(cè)下穿高鐵橋梁盾構(gòu)隧道形變[J]. 湖北民族大學(xué)學(xué)報(bào)（自然科學(xué)版），2022，40（4）：445-450.

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

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

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