受限空間監(jiān)測機器人運動控制系統

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

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

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

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

1運動控制傳感器

1.1線激光傳感器

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

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

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

1.2.1棋盤格平面方程計算

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

（1）

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

棋盤格在相機坐標系中的法向量如公式（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ā)現該方法有一定誤差，因此，本文引入PnP問題的經典求解方法：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的轉置矩陣。

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

（6）

式中：ωi為第i個角點的權重；N為總角點數。

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

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

式中：s為相機對應距離參數；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為迭代次數；si為pi處相機對應距離參數；K為內參矩陣。

1.2.2線結構光中心線提取

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

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

（10）

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

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

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

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

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

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

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

2運動控制系統硬件電路

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

2.1無線通信模塊

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

2.2驅動電機控制模塊

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

2.3定位模塊

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

3軟件系統開發(fā)

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

3.1系統初始化

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

3.2傳感器采集

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

3.3網絡通信

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

3.4安全監(jiān)視

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

3.5運動控制

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

4結論

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

參考文獻

[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耦合模型預測下穿高鐵橋梁盾構隧道形變[J]. 湖北民族大學學報（自然科學版），2022，40（4）：445-450.

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

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

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