自行走式地下掘進機器人姿態(tài)測量系統(tǒng)的設(shè)計*

陳雙葉，牛經(jīng)龍，楊汝軍

(北京工業(yè)大學電子信息與控制工程學院，北京100124)

自行走式地下掘進機器人主要應(yīng)用于城市隧(管)道的挖掘，采用自增阻四段式結(jié)構(gòu)［1］，，具有急曲線施工能力及不依賴管壁提供掘進推進反力的技術(shù)特點，是一種創(chuàng)新發(fā)展的新機型。在其掘進過程中受到各種力的作用和一些不定性因素影響，這會與設(shè)計軸線出現(xiàn)偏差。為了將掘進路線與隧道設(shè)計曲線的誤差控制在一定的范圍內(nèi)，需要及時的獲得機器人的位置和姿態(tài)，此時就必須通過導向系統(tǒng)測量機器人在絕對空間中的坐標，具體來說就是獲得機器人各段首尾的中心坐標，計算掘進機器人軸線與隧道設(shè)計軸線的位置偏差(水平方向和垂直方向)和角度偏差(水平偏航角和俯仰角)［2－4］，隨后反饋給位姿控制系統(tǒng)做出控制來減小偏差。

對于掘進中的機器人，要實時的獲取空間位置和姿態(tài)難度較大。主要原因有機器人的動態(tài)性、測量標志設(shè)置困難、測量環(huán)境惡劣等。目前測量方法主要分為人工測量和自動測量兩類。人工測量工作量大、人力投入多、測量頻率低無法實時獲取機器人姿態(tài)。但由于其實現(xiàn)方式有多種、成本較低和操作簡單，國內(nèi)施工中仍被采用。自動導向測量系統(tǒng)主要有德國的VMT，日本Enzan公司的ROBOTEC和日本的GYRO系統(tǒng)［5］，等。前兩者為全站儀激光導航，后者為陀螺儀慣性導航。德國VMT激光導向系統(tǒng)功能齊全、精度高，但只能與德國海瑞克公司的盾構(gòu)配套使用而無法移植，并且操作復雜，無法大規(guī)模采用。對于日本ROBOTEC，采用三個反射棱鏡與一個自動全站儀測量，通過測量三個棱鏡的坐標計算機器人位姿。但在同一時刻全站儀只能檢測一個點的坐標，而測量動態(tài)行進的機器人姿態(tài)需要檢測同一時刻離散分布的若干點的空間坐標，所以它更適合掘進機器人靜態(tài)位姿檢測。對于日本GYRO慣性陀螺儀導航系統(tǒng)，定位誤差會隨時間而增大，不適用于長距離長時間的測量，需要定期的進行校正［6－7］，在施工中陀螺儀僅作為輔助參考。為了使自行走式地下掘進機完成地下自動控制掘進行走，對其地下姿態(tài)的定位提出高的要求，高精度，智能化和開放化。基于此，本文設(shè)計出一種新的姿態(tài)定位系統(tǒng)，可對多目標進行實時動態(tài)的精確測量。圖1為測量系統(tǒng)在工程中的應(yīng)用結(jié)構(gòu)圖。

圖1 工程控制圖

1 姿態(tài)測量系統(tǒng)設(shè)計

1.1 測量系統(tǒng)的組成

(1)發(fā)射器［8－10］

系統(tǒng)中存在兩個發(fā)射器，每臺發(fā)射器以不同角速度旋轉(zhuǎn)，同時發(fā)射帶有位置和角速度等信息的激光信號，包括兩束扇形光和一束選通光。兩束扇形光在水平面的夾角θ為90°，與垂直面的夾角φ為±30°，扇面水平約夾角±30°(如圖2)。同時發(fā)射器內(nèi)部還集成有電子羅盤模塊，配合整平基座可以建立與世界坐標系相符的測量坐標系，南北為坐標系x軸，東西為坐標系y軸，垂直于水平面的軸為z。

圖2 扇面夾角

(2)整平基座

整平基座對發(fā)射器進行整平操作，使發(fā)射器的中心軸線垂直于水平面。

(3)接收器

接收器主要由激光傳感器和放大電路組成［11］，用于接收發(fā)射器發(fā)射出的激光信號，并反饋信息給發(fā)射器，告知其對選通光束的接收，通過發(fā)射器與接收器輸出的信號來實現(xiàn)空間定位。其數(shù)量可以根據(jù)要求進行配置。

(4)坐標計算處理器

坐標計算處理器用于處理發(fā)射器與接收器反饋回來的數(shù)據(jù)，計算得到與目標相關(guān)的角度信息，再根據(jù)角度信息計算得到目標點的三維空間坐標。

(5)上位機軟件

上位機軟件用于匯總各個接收器反饋回來的坐標信息，根據(jù)用戶的需要計算得到機器人空間位姿的信息，顯示于客戶端，方便用戶掌握機器人的路線偏差并對其進行控制。

1.2 測量系統(tǒng)的布局

自行走掘進機器人外徑為1 100 mm，內(nèi)徑為900 mm，共分為4節(jié)，每節(jié)之間通過液壓缸兩兩相連，液壓缸全部處于收縮狀態(tài)時機器全長5 200 mm其中第1節(jié)長度為700 mm，第2、3、4節(jié)長度均為1 500 mm(為機器外側(cè)測量長度，不包括機器各節(jié)搭接部分長度)，當液壓缸全部處于伸長狀態(tài)時機器全長5 560 mm［1］(如圖3)。根據(jù)機器人的機械結(jié)構(gòu)和發(fā)射器的測量范圍對接收器和發(fā)射器進行布局。

圖3 自行走掘進機器人結(jié)構(gòu)圖

在掘進機尾部離內(nèi)壁一定距離處安裝4個接收器，接收器位置即測量點位置，要滿足4個測量點在同一斷面(圖4)且關(guān)于中心對稱。根據(jù)3點確定一個平面的原理，機器人第4段尾部斷面方程和中心點O的坐標可以確定。由于掘進機各節(jié)的材質(zhì)均是圓形鋼桶，便可根據(jù)剛體性質(zhì)通過數(shù)學公式依次推導出各段首尾斷面中心坐標完成位姿的測量［12－14］。

圖4 接收器安放位置

發(fā)射器的扇形光束與水平面的夾角φ為±30°，加上激光的測量距離構(gòu)成發(fā)射器的測量空間(圖5)，測量過程中要求接收器處在發(fā)射器的測量范圍內(nèi)，且兩個發(fā)射器相距一定距離且分布于掘進路線內(nèi)壁的兩側(cè)，則可完成接收器空間坐標的測量。

圖5 發(fā)射器空間測量范圍

1.3 測量原理

利用對自行走式掘進機器人尾段機身上不在同一條直線上的三點坐標的測量，根據(jù)三點確定一個平面和掘進機的剛體性質(zhì)，自行走式掘進機器人的空間位置就可以確定。對于機身上點坐標的測量，采用前方交會原理(圖6)。

圖6 前方交會原理圖

計算公式

通過對 A、B 兩點空間坐標，αA、αB、βA、βB角度的測量，可以得到目標點P的坐標。

其中A、B兩點空間坐標即測量系統(tǒng)兩個發(fā)射器的坐標，在掘進開始前通過全站儀對兩點進行測量。αk(k=A，B)是通過接收裝置接收到選通光后反饋給發(fā)射裝置，記錄此時選通光與電子羅盤正北方向的夾角，得出水平方向夾角。βk(k=A，B)是利用兩束扇形光被接收器接收到的不同時間差值Δt計算得出的(圖7)。

計算公式:

其中t1是發(fā)射器第1束扇形光被接收到的時間，t2是第2束扇形光被接收到的時間。

圖7 角度測量

1.4 系統(tǒng)工作原理

測量系統(tǒng)啟動后，兩臺發(fā)射器以不同角速度ωk(k=A，B)旋轉(zhuǎn)，旋轉(zhuǎn)過程中各自發(fā)射出一束選通光和兩束扇形光，A、B兩發(fā)射器發(fā)出的選通光先后到達接收器，接收器提取A、B選通光所帶的角速度信息和位置信息，以此判斷不同時刻接收到的選通光來自哪個發(fā)射器。通過無線電模塊將接收到的A、B選通光的時刻信息反饋給發(fā)射器，發(fā)射器記錄此時刻選通光與電子羅盤正北方向的夾角，得到水平角度信息 αA、αB。

以接收器接收到選通光的時刻為計時零點，存在兩個計時零點A0、B0，4束扇形光先后到達接收器，根據(jù)扇形激光所載的信息特征判斷該時刻以哪個計時零點計時。得到4組時刻其中為A發(fā)射器的兩束扇形激光被接收時刻，得到為B發(fā)射器的兩束扇形激光被接收的時刻，得到根據(jù)式(2)計算出βA、βB。同理多個接收器計算方法相同，但反饋給發(fā)射器的信息有標識，可以區(qū)分不同接收器反饋回的信息。當 αA、αB、βA、βB測量完畢后，一周期的測量結(jié)束，開始下一周期的測量。圖8為系統(tǒng)結(jié)構(gòu)框圖，圖9為系統(tǒng)運行流程圖。

圖8 系統(tǒng)框圖

圖9 系統(tǒng)流程圖

2 系統(tǒng)測量誤差分析

系統(tǒng)的坐標測量誤差從原理上可分為靜態(tài)誤差和動態(tài)誤差。靜態(tài)誤差主要源于發(fā)射器和接收器本身的觀測誤差，是儀器觀測誤差通過系統(tǒng)測量相互作用，相互影響的結(jié)果。動態(tài)誤差主要是接收器接收發(fā)射器信號時間上不同步和載體運動共同作用的結(jié)果?？紤]到測量技術(shù)的成熟，發(fā)射器空間坐標在開始用全站儀定位所產(chǎn)生的誤差可以忽略不計。

2.1 靜態(tài)誤差分析

靜態(tài)誤差是目標點即接收器在靜止條件下觀測，由環(huán)境、零部件不穩(wěn)定等因素造成的，具有隨機性，所以將靜態(tài)誤差視為隨機誤差處理。設(shè)發(fā)射器和接收器觀測量 αA、αB、βA、βB的觀測精度為 σ1、σ2、σ3、σ4，誤差為 Δα1、Δα2、Δβ1、Δβ2。靜態(tài)誤差ΔK1=(Δx1，Δy1，Δz1)，測量精度為 σk1=(σx1，σy1，σz1)。對式(1)全微分得:

由于4個角度的測量分別通過兩個發(fā)射器共4種光束獨立測量得出，所以4個觀測量彼此獨立，互不相關(guān)，根據(jù)方差合成定理［15］得:

2.2 動態(tài)誤差分析

在系統(tǒng)測量過程中，由于目標點接收器的運動和發(fā)射器A、B不同角速度的旋轉(zhuǎn)，對4個角的測量會出現(xiàn)2個時刻的不同步，即測量水平角αA、αB時2發(fā)射器各自的選通光束在不同時刻被接收器接收，接收時間間隔為ΔT1，同理各自扇形光束測量俯仰角βA、βB，被接收器接收的時間間隔為ΔT2，測量水平角和俯仰角之間的時間間隔記為ΔT3，建立動態(tài)誤差模型(圖10)。測量過程中目標點相對于接收器靜止，接收器又是測量系統(tǒng)的一部分。所以將接收器的運動作為測量裝置內(nèi)部的運動，它所造成的誤差等同于系統(tǒng)本身測量裝置的誤差，而對目標點的測量則可看作在目標點靜止時的測量。又由于動態(tài)誤差的大小和方向隨接收器的載體即掘進機器人的掘進距離，掘進方向和掘進速度大小變化而變化，具有不確定性，不能確切的掌握，所以將動態(tài)誤差視為未定系統(tǒng)誤差處理。

圖10 模型動態(tài)誤差原理

模型圖表明時間差 ΔT1、ΔT2、ΔT3的存在引起測量系統(tǒng)觀測量的誤差，實際測量值為 α'A、α'B、β'A、β'B，分別對應(yīng)的測量點 p1、p2、p3、p4。由于地下掘進機器人行進速度緩慢，對于以秒級為測量周期的測量定位系統(tǒng)而言，測量周期內(nèi)的4點在一條直線上，設(shè)目標點速度v方向為測量采樣點之間的連線方向(xn+1－xn，yn+1－yn，zn+1－ zn)，方向角為 γ、ρ，精度為sk2=(sx2，sy2，sz2)。

可求得 p1，p2，p3，p44 個測量點坐標:

根據(jù)上述公式可求得各測量角:

將式(11)～式(14)代入式(1)得目標點的實際測量值(x'，y'，z')。進一步計算得:

動態(tài)誤差 ΔK2=(Δx2，Δy2，Δz2)。

3 系統(tǒng)誤差仿真

ΔK1為靜態(tài)誤差，即隨機誤差，ΔK2為動態(tài)誤差，即系統(tǒng)誤差。根據(jù)系統(tǒng)誤差與隨機誤差合成定理［15］:

為保證測量的精度，應(yīng)選擇合適的結(jié)構(gòu)。對于時間同步則受限于硬件設(shè)備性能，所以需要適當縮小觀測誤差范圍，擴大時間同步誤差范圍。

根據(jù)式(4)進行仿真計算，當0＜αA+αB＜180°，0 ＜βA，βB＜30°時，測角精度為 σ≤10″，b≤1 000 mm時，σx1＜27.78 mm，σy1＜43.9 mm，σz1＜38.75 mm。測角精度為 σ≤5″，b≤1 000 mm 時，σx1＜13.89 mm，σy1＜20.87 mm，σz1＜17.49 mm?？紤]到目前系統(tǒng)的激光測角精度和在掘進工程中隧道標高誤差需控制在－20 mm～80 mm，平面誤差控制在±50 mm，水工隧道誤差控制在±150 mm［16］等因素，取測角精度σ≤5″進行分配后得:σx1≤20 mm，σy1≤25 mm，σz1≤20 mm。sx2≤45 mm，sy2≤43 mm，sz2≤10 mm。

通過動態(tài)測量誤差模型可知，目標點速度與誤差成正比，發(fā)射器角速度與誤差成反比，對模型進行仿真，對模型中的輸入?yún)⒘咳O限值，設(shè)定實際路線。目標行進速度為v≤1.5 mm/s，發(fā)射器A的角速度為ωA≥π/4 rad/s，發(fā)射器B的角速度為ωB≥π/3 rad/s。A 點坐標為(xA，yA，zA)，B 點坐標為(xB，yB，zB)。其中|yB－yA|≤1 000 mm。圖11 為極限值情況下的仿真誤差。由圖可知，動態(tài)誤差滿足上述誤差分配要求。該導航定位方法在掘進過程位姿測量中可行，且精度較高。

圖11 系統(tǒng)仿真

慣性導航定位方法與其相比運用的是陀螺儀對掘進機進行方位和傾斜角檢測，同時利用其中的加速度計測量掘進機的加速度并對其進行兩次積分求得掘進距離。通過對方位角，傾斜角和掘進距離的計算得到掘進機器人的位置坐標。圖12表示機器人位置的計算模型。其中θi表示測量得到的方位角與傾斜角。從已知點A1出發(fā)，沿θ1方向掘進距離L1，由此算出點A2的坐標。再從A2出發(fā)，用同樣方法算出A3。以此類推，可算出直到An的坐標。以日本Tolimec公司制造的MG－32B系統(tǒng)為例，系統(tǒng)方位角靜止點精度 ±0.2°，俯仰角精度 ±0.1°［17］。設(shè)定實際路線與上述相同，不考慮陀螺儀加速度計誤差對時間積分的影響，對慣性系統(tǒng)進行仿真。圖13可知隨著掘進距離的增加，定位誤差逐漸增大。該仿真是在較理想的情況下，在實際測量過程中由于加工工藝誤差、信號檢測誤差引起的陀螺漂移會嚴重影響測量的精度。此外，環(huán)境溫度的變化對陀螺的機械、電學性能影響也較大，有的甚至是質(zhì)的變化，這無疑也會導致陀螺精度的下降。這些都會使得測量結(jié)果的誤差大于圖中仿真結(jié)果。與上述測量系統(tǒng)相比它的精度和穩(wěn)定性顯得不足。

圖12 慣性系統(tǒng)位置計算模型

圖13 慣性導航測量系統(tǒng)仿真

4 結(jié)論

針對目前自行走式地下掘進機器人地下姿態(tài)測量方法的不足，本文提出的新型地下姿態(tài)測量方法更加合理。該系統(tǒng)操作簡便，實時性好，精度較高，對地下機器人掘進過程中的姿態(tài)定位具有一定的應(yīng)用價值。

［1］ 盧清國，王慶學，張延欽，等.自行走式地下管道掘進機工業(yè)樣機試驗［J］.建筑機械，2010(7):81－86.

［2］ 唐爭氣，趙俊三，彭國新.盾構(gòu)機實時姿態(tài)測量和計算方法的研究［J］.土木工程學報，2007，40(11):92 －97.

［3］ 馮寶新，王解先.盾構(gòu)姿態(tài)測量方法［J］.公路隧道，2011(1):63－65.

［4］ 潘國榮，王穗輝，陳傳林，等.盾構(gòu)姿態(tài)自動測量系統(tǒng)的開發(fā)與應(yīng)用［J］.同濟大學學報，2010，38(3):459 －463.

［5］ 潘明華.盾構(gòu)自動導向系統(tǒng)的研究與實現(xiàn)［D］.湖北:華中科技大學，2005.

［6］ 馬建倉，陳靜.MEMS SINS-GPS組合導航系統(tǒng)設(shè)計［J］.傳感技術(shù)學報，2009，22(10):1437 －1441.

［7］ 王晶晶，童敏明，劉彬.組合導航定位系統(tǒng)研究［J］.Software，2011，32(5):82 －84.

［8］ Thomas M Hedges，Hiro Takgl，Michael J Sobel.Position Measurement System and Using Cone Math Calibration［P］.美國專利:6535282B2，2003－3－18.

［9］ Schwendemann J，Tilman Müller，Krautschneider R.Indoor Navigation of Machines and Measuring Devices with iGPS［C］//IEEE International Conference on Indoor Positioning and Indoor Navigation，Zürich，Swizerland，15 －17 September 2010.

［10］ Robert Schmitt，Nish S，Schonberg A，et al.Performance Evaluation of iGPS for Industrial Application［C］//IEEE International Conference on Indoor Positioning and Indoor Navigation，Zürich，Swizerland，15 －17 September 2010.

［11］李孝軒.傳感器微弱信號低噪聲斬波放大器［J］.傳感技術(shù)學報，1994，7(3):44 －48.

［12］張厚美，古力.盾構(gòu)機姿態(tài)參數(shù)的測量及計算方法研究［J］.現(xiàn)代隧道技術(shù)，2004，41(2):14 －20.

［13］李鵬程.盾構(gòu)機姿態(tài)測量方法的精度比較［J］.安徽建筑，2008，15(6):174 －176.

［14］馮東健，潘慶林，張鳳梅.地鐵盾構(gòu)施工中盾構(gòu)機姿態(tài)定位測量的研究［J］.工程勘探，2005，5:57 －61.

［15］吳石林，張玘.誤差分析與數(shù)據(jù)處理［M］.北京:清華大學出版社，2010:108－113.

［16］孫更生等.中國土木工程師手冊(中)［M］.上海:上?？茖W技術(shù)出版社，2001:55 －60.

［17］于睿坤.盾構(gòu)掘進機掘進過程中姿態(tài)的智能控制系統(tǒng)研究［D］.上海:同濟大學，2008.