光傳輸管道清洗機器人高精度自主行走問題研究

陶茂林，隋春平，陳月玲

（1.中國科學院沈陽自動化研究所，沈陽 110016；2.中國科學院大學，北京 100049）

?

光傳輸管道清洗機器人高精度自主行走問題研究

陶茂林1，2，隋春平1，陳月玲1

（1.中國科學院沈陽自動化研究所，沈陽 110016；2.中國科學院大學，北京 100049）

摘 要：為解決光傳輸管道清洗機器人需沿管道中心線精確行走的問題，基于沿墻導航的策略對此問題展開了研究。運用反饋線性化方法并基于運動學模型設計了糾偏控制器；針對光傳輸管道壁面較光滑的特點，提出了一種新的基于雙線結構光視覺傳感器的機器人位姿精確檢測方法，并建立了位姿偏差計算模型。通過所設計的糾偏控制器實時糾正機器人的位姿偏差來實現高精度沿線行走。最后利用樣機進行了行走實驗，實驗結果表明機器人的穩定行走精度達到±2mm，滿足應用要求。

關鍵詞：管道機器人；自主行走；控制器設計；線結構光

0　引言

針對某大型激光核聚變裝置光傳輸管道拆卸清洗較為困難的問題，采用移動機器人搭載激光清洗部件進行在線清洗是一種有效的解決方法。機器人在這種方形直管道內作業時，為保證清洗效果，要求機器人能精確地沿管道中心線行走，水平面內的偏差≤±5mm。由于光傳輸管道壁面光滑，不能人為設置標記，因此如何實現高精度的自主行走成為需要解決的問題。

目前對于矩形管道內機器人的自主行走研究主要以避免與管壁碰撞為目的［1～4］，而以實現較高行走精度為目的的研究相對較少。文獻［1～3］通過測距傳感器來獲取機器人的位姿信息，并以此設計了相應的控制算法實現了空調通風管道機器人的自主行走，但行走精度較低。文獻［4］采用機械式導向輪的方法沒有閉環控制，無法保證行走精度且不利于保護管道表面。文獻［5］提出的基于慣性導航與立體視覺的方法則比較復雜。

本文基于地面移動機器人沿墻導航的策略，設計了糾偏控制器，提出了一種新的基于雙線結構光視覺傳感器的機器人位姿偏差計算方法。通過糾偏控制器實時糾正機器人相對管壁的位姿偏差來實現高精度沿線行走。最后通過樣機實驗證明該方法的可行性。

1　問題描述

1.1運動學模型

光傳輸管道清洗機器人屬于車式移動機器人類型，其運動學模型如圖1所示。圖中XOY為慣性坐標系，v為驅動輪速度；φ為轉向輪轉角，θ為車身方向與x軸的夾角；L為前后輪軸距；（x，y）為驅動軸中點Pm在慣性坐標系下的坐標。機器人的位姿向量用pp==［x， y， θ， φ］T表示。

圖1　車式移動機器人運動學模型

設轉向輪的角速度為ω，則車式移動機器人的運動學模型可表示如下［6］：

1.2沿中心線行走問題

基于沿墻導航法實現沿管道中心線行走，對這一問題的描述如圖2所示。機器人自一初始位置出發，通過所設計的控制器作用，使機器人最終以期望速度vr沿管道側壁行走并始終與壁面保持w/2的距離，其中w為管道寬度。以Pm為參考點，圖2同時給出了機器人相對管壁的位姿偏差描述。其中de、θee分別表示距離偏差和角度偏差。

圖2　沿中心線行走問題和位姿偏差的描述

按圖2中建立的慣性坐標系，位姿偏差表示如下：

對式求導，并結合式得到偏差模型：

機器人沿管道中心線精確行走問題即是尋求控制輸入u=［v，w］T，使得

2　控制器設計

車式移動機器人是一類典型的強耦合、高度非線性系統。本文采用反饋線性化方法來設計控制器。為使所設計的控制器方便，將控制輸入v視為一與時間無關的非零常量，并且等于期望速度，即v=vr。這樣僅需通過控制轉向輪的轉角φ來達到精確沿線行走的目的。為將偏差模型轉化為鏈式形式，作如下變換：

得到如下鏈式系統：

根據文［6］結論，當參數k1、k2、k3使得多項式s3+k s2+k s+ k滿足Hurwitz穩定條件時，設計如下反饋控制律：

可使閉環子系統：

根據極點配置理論，可取k1= λ3、k2= 3 λ2、k3= 3 λ，其中λ＞0，從而將控制參數減少為一個。由式（4）～式（6）將控制輸入表達如下：

3　偏差檢測

機器人在管道內行走時需實時檢測相對管壁的位姿偏差de、θe，從而輸給糾偏控制器進行偏差糾正。精確的偏差檢測是實現高精度自主行走的保證。當前基于沿墻導航策略的自主移動機器人普遍采用測距傳感器（紅外測距傳感器或超聲波測距傳感器）來獲取機器人的位姿偏差［1～3，7］。但測距傳感器易受干擾、穩定性差，尤其當壁面較光滑而機器人又與壁面不平行時測量誤差較大，甚至無法測量。結構光視覺傳感器具有精度高、實時性好、抗干擾強等優點。本文借鑒文［8］提出的方法，采用雙線結構光視覺傳感器進行偏差檢測。傳感器安裝在機器人右側，如圖3所示，主要由相機和兩個激光投射器構成。

3.1測量模型

線結構光視覺測量模型的建立有多種方法，文獻［8］中是基于簡單三角法建立的，但該方法需準確調整激光投射器和相機的安裝位置［9］，給使用造成不便。因此這里將基于應用更方便的透視投影模型進行測量，該模型的詳細建立過程見文獻［9］，這里只進行必要的概述。如圖4所示，Oc-xcyczc為相機坐標系，O1-XY為圖像物理坐標系，O1-uv為圖像像素坐標系。z軸為相機光軸，X、Y軸分別與x、y軸平行。O1點為光軸與像平面的交點，O1點的像素坐標為（u0，v0）。光平面與壁面相交形成投射光條，P點為投射光條上的任意點，該點在像平面上的透視投影點為p。

圖3　雙線結構光視覺傳感器

圖4　單線結構光視覺測量模型

設P點相機坐標為（xc，yc，zc），投影點p的像素坐標為（u，v），則單線結構光視覺測量模型可表示為：

式中：s為比例因子；ax、ay為u軸和v軸上的尺度因子；a、b、c、d為光平面方程系數，通過光平面標定獲得。ax、ay、u0、v0為相機內部參數，通過相機標定獲得。

對于雙線結構光，只需再對另一個激光投射器的光平面進行標定，在式（9）基礎上再增加一個光平面方程即可。

3.2偏差計算模型

由于不需要yc軸方向的信息，因此將基于平面幾何關系來建立偏差計算模型。假設經中心提取后的兩條結構光光條圖像如圖5所示（投射光條不需要嚴格平行）。取各光條上對應于縱坐標為v=v0的像素點p、q，設兩點的像素坐標分別為（up，v0）、（uq，v0），并設兩點對應的位于壁面上的空間點及其坐標分別為P（xcp，ycp，zcp）、Q（xcp，ycp，zcp）。將p、q兩點的像素坐標代入式（9）即可解得對應的P、Q點坐標，并且有ycp=ycq=0。由此，將偏差計算問題轉化為xcOczc平面內的幾何問題，如圖6所示。

圖5　線結構光光條圖像

圖6　偏差計算模型

由圖6可知，根據幾何關系易求得角度偏差如下：

設相機光心與管道壁面的距離為dc，則有：

設參考點Pm與相機ycOczc平面的距離為m，相機光心與機器人縱向對稱平面的距離為n，則點Pm的相機坐標為（m，-n）。其中，m由結構設計確定，n通過測量實驗獲取。點Pm的慣性系坐標為（x，y），則根據圖6有以下變換關系有：

式中：xO、yOc為相機坐標原點Oc的慣性系坐標，w為管道寬度。

綜合式（2）、式（10）～式（12）得到偏差計算模型如下：

4　樣機實驗

所研制的光傳輸管道清洗機器人工程樣機如圖7所示。受制于客觀條件，無法在實際的光傳輸管道中進行實驗，因此首先在一段模擬管道進行沿中心線行走實驗。管道截面尺寸為500mm×500mm，長2m。為便于實驗和觀察，樣機卸去了吸塵扒，并設計了一簡易自適應畫線裝置，固定在靠近驅動軸一端的中間位置，用以描繪機器人的行走軌跡。

圖7　機器人工程樣機

管道機器人實際作業時，先在非清洗狀態下進入管道并在管道末端停止，然后再慢速退出并在此過程中清洗管道。出于結構設計方面的需要，機器人在進入管道時以倒退的方式進入，而在退出管道時則相當于前進。依據相應技術要求及模擬管道長度，本次樣機的實驗過程設置如下：首先將機器人以一定的初始偏差放置在管道口，然后以-3.6m/min的速度進入管道，行走24s后停止，再從停止位置以1.4m/min的速度退出管道，行走60s后停止。取控制參數λ==88，采樣周期T=0.1s。實驗完成后樣機的行走軌跡如圖8（a）所示，根據傳感器實時記錄的數據繪制的距離偏差隨行走時間的變化曲線如圖9所示。

圖8　樣機行走軌跡圖

圖9　模擬管道內行走實驗的偏差變化曲線

由圖9可知，機器人能快速減小初始偏差，但穩定行走精度并沒有達到所期望的±5mm，并且偏差存在較大波動。經檢測和分析，這是由于模擬管道是由薄不銹鋼板圍焊而成，管道壁表面存在較嚴重的凹凸不平所致。

為進一步驗證機器人能否實現高精度的自主行走，避免管道壁面不平整的干擾，又進行了兩次沿直墻行走

【】【】實驗。沿墻行走實驗中樣機的前進、后退速度均設為3.6m/min，其余參數與管道行走實驗一致。首先進行一次沿直墻前進實驗，設置與墻面應保持的距離為實際管道寬度的一半，即250mm。樣機行走軌跡如圖8（b）所示，偏差變化曲線如圖10（a）所示。然后再選擇另一面墻進行一次往返行走實驗，設置前進、后退時間均為30s。實驗完成后樣機的行走軌跡如圖8（c）所示，偏差變化曲線如圖10（b）所示。

圖10　沿墻行走實驗的偏差變化曲線

圖10表明，機器人在經過一段時間的偏差糾正后能穩定行走在±2mm區間內，滿足水平面內偏差≤±5mm的要求。由此證明機器人在光滑的光傳輸管道內可以實現沿管道中心線精確行走。

5　結論

1）所提出的基于雙線結構光視覺傳感器的機器人位姿檢測方法具有較高的測量精度和準度，可應用于管道或地面移動機器人的簡單導航；2）所設計的糾偏控制器能實現對位姿偏差的快速收斂；3）所采用的基于沿墻導航的方法，可以使管道機器人實現沿矩形管道中心線精確行走。

參考文獻：

［1］ 宋章軍，陳懇，楊向東.基于紅外測距傳感器信息的通風管道清掃機器人控制算法研究［J］.制造業自動化，2006，28（5）：44-47.

［2］ 王永雄，葉青.管道機器人自主導航系統設計［J］.制造業自動化，2010，32（1）：113-115.

［3］ Wootae Jeong.Performance analysis of a mobile duct cleaning robot［J］.International Journal of Advanced Engineering Applications，2014，7（2）：26-32.

［4］ Wang Ya，Zhang Jian-hua. Autonomous air duct cleaning robot system［A］.49th IEEE International Midwest Symposium on Circuits and Systems.New York，NY，USA： IEEE［C］.2006：510-513.

［5］ 王聰，孫煒，卜德旭，等.基于慣性導航與立體視覺的風管清掃機器人同時定位與地圖創建方法［J］.機械工程學報，2013，49（23）：59-67.

［6］ Siciliano Bruno， Khatib Oussama.Handbook of Robotics［M］.New York： Springer，2008：799-826.

［7］ 袁宇龍.室內自主移動機器人沿墻導航研究［D］.重慶：重慶大學，2014.

［8］ K. C. Koh， H. J. ChO， J S. Kim， et al. Sensor-based navigation of air duct inspection mobile robots［A］.Proceeding of the SPIE［C］. 2001.4190.202-211.

［9］ 張廣軍.視覺測量［M］.北京：科學出版社，2008.

設計與應用

加工與制造

Research on high-precision autonomous walking of a laser-beam propagation tube cleaning robot

TAO Mao-lin1，2， SUI Chun-ping1， CHEN Yue-ling1

中圖分類號：TP242

文獻標識碼：A

文章編號：1009-0134（2016）05-0052-05

收稿日期：2016-03-01

基金項目：國家自然科學基金（51375476）

作者簡介：陶茂林（1992 -），男，湖北武穴人，碩士研究生，研究方向為機器人技術及應用。