陶茂林，隋春平，陳月玲

（1.中國科學(xué)院沈陽自動(dòng)化研究所，沈陽 110016；2.中國科學(xué)院大學(xué)，北京 100049）

?

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

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

（1.中國科學(xué)院沈陽自動(dòng)化研究所，沈陽 110016；2.中國科學(xué)院大學(xué)，北京 100049）

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

關(guān)鍵詞：管道機(jī)器人；自主行走；控制器設(shè)計(jì)；線結(jié)構(gòu)光

0　引言

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

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

本文基于地面移動(dòng)機(jī)器人沿墻導(dǎo)航的策略，設(shè)計(jì)了糾偏控制器，提出了一種新的基于雙線結(jié)構(gòu)光視覺傳感器的機(jī)器人位姿偏差計(jì)算方法。通過糾偏控制器實(shí)時(shí)糾正機(jī)器人相對管壁的位姿偏差來實(shí)現(xiàn)高精度沿線行走。最后通過樣機(jī)實(shí)驗(yàn)證明該方法的可行性。

1　問題描述

1.1運(yùn)動(dòng)學(xué)模型

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

圖1　車式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

設(shè)轉(zhuǎn)向輪的角速度為ω，則車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型可表示如下［6］：

1.2沿中心線行走問題

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

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

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

對式求導(dǎo)，并結(jié)合式得到偏差模型：

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

2　控制器設(shè)計(jì)

車式移動(dòng)機(jī)器人是一類典型的強(qiáng)耦合、高度非線性系統(tǒng)。本文采用反饋線性化方法來設(shè)計(jì)控制器。為使所設(shè)計(jì)的控制器方便，將控制輸入v視為一與時(shí)間無關(guān)的非零常量，并且等于期望速度，即v=vr。這樣僅需通過控制轉(zhuǎn)向輪的轉(zhuǎn)角φ來達(dá)到精確沿線行走的目的。為將偏差模型轉(zhuǎn)化為鏈?zhǔn)叫问剑魅缦伦儞Q：

得到如下鏈?zhǔn)较到y(tǒng)：

根據(jù)文［6］結(jié)論，當(dāng)參數(shù)k1、k2、k3使得多項(xiàng)式s3+k s2+k s+ k滿足Hurwitz穩(wěn)定條件時(shí)，設(shè)計(jì)如下反饋控制律：

可使閉環(huán)子系統(tǒng)：

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

3　偏差檢測

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

3.1測量模型

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

圖3　雙線結(jié)構(gòu)光視覺傳感器

圖4　單線結(jié)構(gòu)光視覺測量模型

設(shè)P點(diǎn)相機(jī)坐標(biāo)為（xc，yc，zc），投影點(diǎn)p的像素坐標(biāo)為（u，v），則單線結(jié)構(gòu)光視覺測量模型可表示為：

式中：s為比例因子；ax、ay為u軸和v軸上的尺度因子；a、b、c、d為光平面方程系數(shù)，通過光平面標(biāo)定獲得。ax、ay、u0、v0為相機(jī)內(nèi)部參數(shù)，通過相機(jī)標(biāo)定獲得。

對于雙線結(jié)構(gòu)光，只需再對另一個(gè)激光投射器的光平面進(jìn)行標(biāo)定，在式（9）基礎(chǔ)上再增加一個(gè)光平面方程即可。

3.2偏差計(jì)算模型

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

圖5　線結(jié)構(gòu)光光條圖像

圖6　偏差計(jì)算模型

由圖6可知，根據(jù)幾何關(guān)系易求得角度偏差如下：

設(shè)相機(jī)光心與管道壁面的距離為dc，則有：

設(shè)參考點(diǎn)Pm與相機(jī)ycOczc平面的距離為m，相機(jī)光心與機(jī)器人縱向?qū)ΨQ平面的距離為n，則點(diǎn)Pm的相機(jī)坐標(biāo)為（m，-n）。其中，m由結(jié)構(gòu)設(shè)計(jì)確定，n通過測量實(shí)驗(yàn)獲取。點(diǎn)Pm的慣性系坐標(biāo)為（x，y），則根據(jù)圖6有以下變換關(guān)系有：

式中：xO、yOc為相機(jī)坐標(biāo)原點(diǎn)Oc的慣性系坐標(biāo)，w為管道寬度。

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

4　樣機(jī)實(shí)驗(yàn)

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

圖7　機(jī)器人工程樣機(jī)

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

圖8　樣機(jī)行走軌跡圖

圖9　模擬管道內(nèi)行走實(shí)驗(yàn)的偏差變化曲線

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

為進(jìn)一步驗(yàn)證機(jī)器人能否實(shí)現(xiàn)高精度的自主行走，避免管道壁面不平整的干擾，又進(jìn)行了兩次沿直墻行走

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

圖10　沿墻行走實(shí)驗(yàn)的偏差變化曲線

圖10表明，機(jī)器人在經(jīng)過一段時(shí)間的偏差糾正后能穩(wěn)定行走在±2mm區(qū)間內(nèi)，滿足水平面內(nèi)偏差≤±5mm的要求。由此證明機(jī)器人在光滑的光傳輸管道內(nèi)可以實(shí)現(xiàn)沿管道中心線精確行走。

5　結(jié)論

1）所提出的基于雙線結(jié)構(gòu)光視覺傳感器的機(jī)器人位姿檢測方法具有較高的測量精度和準(zhǔn)度，可應(yīng)用于管道或地面移動(dòng)機(jī)器人的簡單導(dǎo)航；2）所設(shè)計(jì)的糾偏控制器能實(shí)現(xiàn)對位姿偏差的快速收斂；3）所采用的基于沿墻導(dǎo)航的方法，可以使管道機(jī)器人實(shí)現(xiàn)沿矩形管道中心線精確行走。

參考文獻(xiàn)：

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

［2］ 王永雄，葉青.管道機(jī)器人自主導(dǎo)航系統(tǒng)設(shè)計(jì)［J］.制造業(yè)自動(dòng)化，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］ 王聰，孫煒，卜德旭，等.基于慣性導(dǎo)航與立體視覺的風(fēng)管清掃機(jī)器人同時(shí)定位與地圖創(chuàng)建方法［J］.機(jī)械工程學(xué)報(bào)，2013，49（23）：59-67.

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

［7］ 袁宇龍.室內(nèi)自主移動(dòng)機(jī)器人沿墻導(dǎo)航研究［D］.重慶：重慶大學(xué)，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］.北京：科學(xué)出版社，2008.

設(shè)計(jì)與應(yīng)用

加工與制造

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

文獻(xiàn)標(biāo)識碼：A

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

收稿日期：2016-03-01

基金項(xiàng)目：國家自然科學(xué)基金（51375476）

作者簡介：陶茂林（1992 -），男，湖北武穴人，碩士研究生，研究方向?yàn)闄C(jī)器人技術(shù)及應(yīng)用。