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

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

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

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

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

圖1 工程控制圖

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

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

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

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

圖2 扇面夾角

(2)整平基座

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

(3)接收器

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

(4)坐標(biāo)計(jì)算處理器

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

(5)上位機(jī)軟件

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

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

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

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

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

圖4 接收器安放位置

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

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

1.3 測量原理

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

圖6 前方交會原理圖

計(jì)算公式

通過對 A、B 兩點(diǎn)空間坐標(biāo)，αA、αB、βA、βB角度的測量，可以得到目標(biāo)點(diǎn)P的坐標(biāo)。

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

計(jì)算公式:

其中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ā)出的選通光先后到達(dá)接收器，接收器提取A、B選通光所帶的角速度信息和位置信息，以此判斷不同時刻接收到的選通光來自哪個發(fā)射器。通過無線電模塊將接收到的A、B選通光的時刻信息反饋給發(fā)射器，發(fā)射器記錄此時刻選通光與電子羅盤正北方向的夾角，得到水平角度信息 αA、αB。

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

圖8 系統(tǒng)框圖

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

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

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

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

靜態(tài)誤差是目標(biāo)點(diǎn)即接收器在靜止條件下觀測，由環(huán)境、零部件不穩(wěn)定等因素造成的，具有隨機(jī)性，所以將靜態(tài)誤差視為隨機(jī)誤差處理。設(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種光束獨(dú)立測量得出，所以4個觀測量彼此獨(dú)立，互不相關(guān)，根據(jù)方差合成定理［15］得:

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

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

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

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

可求得 p1，p2，p3，p44 個測量點(diǎn)坐標(biāo):

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

將式(11)～式(14)代入式(1)得目標(biāo)點(diǎn)的實(shí)際測量值(x'，y'，z')。進(jìn)一步計(jì)算得:

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

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

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

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

根據(jù)式(4)進(jìn)行仿真計(jì)算，當(dāng)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)的激光測角精度和在掘進(jìn)工程中隧道標(biāo)高誤差需控制在－20 mm～80 mm，平面誤差控制在±50 mm，水工隧道誤差控制在±150 mm［16］等因素，取測角精度σ≤5″進(jìn)行分配后得:σx1≤20 mm，σy1≤25 mm，σz1≤20 mm。sx2≤45 mm，sy2≤43 mm，sz2≤10 mm。

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

圖11 系統(tǒng)仿真

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

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

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

4 結(jié)論

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

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

［2］ 唐爭氣，趙俊三，彭國新.盾構(gòu)機(jī)實(shí)時姿態(tài)測量和計(jì)算方法的研究［J］.土木工程學(xué)報，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］.同濟(jì)大學(xué)學(xué)報，2010，38(3):459 －463.

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

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

［7］ 王晶晶，童敏明，劉彬.組合導(dǎo)航定位系統(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ù)學(xué)報，1994，7(3):44 －48.

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

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

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

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

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

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