基于激光跟蹤的機(jī)器人三維空間定位方法

賀 驥 胥 健 鄭 斌 桂仲成

（上海圭目機(jī)器人有限公司，上海 200437）

0 引言

目前，機(jī)器人在室內(nèi)常以激光雷達(dá)、機(jī)器視覺攝像頭作為主要傳感器，基于SLAM 算法進(jìn)行定位導(dǎo)航[1]。在室外則常利用GPS 衛(wèi)星信號(hào)，結(jié)合編碼器里程計(jì)和陀螺儀慣導(dǎo)進(jìn)行定位導(dǎo)航。無論室內(nèi)還是室外都存在一些特殊情況和場(chǎng)景，導(dǎo)致常見的定位傳感器失效（例如，過于空曠或單調(diào)的環(huán)境會(huì)導(dǎo)致激光雷達(dá)無法找到足夠的參照物；隧道、橋底以及地下室等場(chǎng)景存在沒有GPS 信號(hào)或GPS 信號(hào)微弱等問題），在這種情況下，傳統(tǒng)的機(jī)器人定位方式無法正常工作，因此需要一種新的基于激光跟蹤的機(jī)器人三維空間定位方法[2]。

1 工程概況

重慶蘇家溝特大橋是重慶沿江高速主城至涪陵段項(xiàng)目的重點(diǎn)工程，全長1 208 m，是全線最長的橋梁，橋體采用整體式30 m×40 m 預(yù)制預(yù)應(yīng)力T 型梁搭建。在全橋31 個(gè)墩臺(tái)中，高度在40 m 以上的高墩12個(gè)，高差較大，地形條件復(fù)雜。

重慶蘇家溝特大橋的檢測(cè)工作采用負(fù)壓吸附爬壁機(jī)器人來對(duì)橋墩橋塔進(jìn)行表觀圖像采集和病害識(shí)別，需要對(duì)機(jī)器人進(jìn)行精確定位。在橋梁上GPS 信號(hào)很差或者沒有GPS 信號(hào)，橋墩橋塔表面空曠，特征不明顯，顏色單調(diào)，激光雷達(dá)或機(jī)器視覺也沒有足夠的標(biāo)識(shí)來定位。基于上述背景，該文在機(jī)器人背部固定位置安裝反射棱鏡，讓全站儀自動(dòng)跟蹤這個(gè)棱鏡，通過該文提出的標(biāo)定方法得到的坐標(biāo)系轉(zhuǎn)換關(guān)系即可實(shí)時(shí)獲得機(jī)器人在工作坐標(biāo)系的坐標(biāo)，從而精確地對(duì)爬壁機(jī)器人在橋墩橋塔的位置進(jìn)行定位。

2 基于激光跟蹤的機(jī)器人三維空間定位方法

2.1 全站儀坐標(biāo)系和機(jī)器人工作坐標(biāo)系的構(gòu)建

將全站儀安裝在合適位置，一般放置在三腳架上，然后調(diào)水平。全站儀輸出的是三維極坐標(biāo)（d，ha，va），坐標(biāo)原點(diǎn)在全站儀本體正中央，d為被測(cè)點(diǎn)距離原點(diǎn)的距離，ha為全站儀沿垂直地平面的軸旋轉(zhuǎn)的角度（偏航角），va為全站儀的俯仰角度。建立1 個(gè)三維直角坐標(biāo)系作為全站儀坐標(biāo)系，這個(gè)坐標(biāo)系原點(diǎn)依然在全站儀本體正中央，垂直地平面向上作為z軸正方向，在平行于地平面并通過原點(diǎn)的平面上設(shè)定任意一個(gè)方向作為y軸正方向，則在該平面上垂直y軸向右的方向是x軸正方向，x軸、y軸和z軸均滿足左手定則。

如圖1 所示，假設(shè)被測(cè)點(diǎn)的極坐標(biāo)為（dt，hat，vat），其對(duì)應(yīng)在全站儀直角坐標(biāo)系中的坐標(biāo)為（xt，yt，zt），那么（xt，yt，zt）的計(jì)算關(guān)系式如公式（1）～公式（3）所示[3]。

圖1 被測(cè)點(diǎn)極坐標(biāo)與全站儀坐標(biāo)之間的換算關(guān)系

選取機(jī)器人的工作區(qū)域，在工作區(qū)域建立工作坐標(biāo)系，機(jī)器人的工作區(qū)域不一定是地面，也可以是任意表面，例如豎直墻面、斜坡面、隧道拱頂以及橋梁表面等，要在這些區(qū)域?qū)C(jī)器人進(jìn)行定位，就需要先建立工作坐標(biāo)系。工作坐標(biāo)系的選取需要分別確定原點(diǎn)、x軸和y軸，x軸、y軸可以確定1 個(gè)平面，z軸垂直這個(gè)平面。如果機(jī)器人在平面中運(yùn)動(dòng)，那么可以將工作坐標(biāo)系簡(jiǎn)化為平面坐標(biāo)系，z軸坐標(biāo)恒為0。如果有條件，就可以給工作坐標(biāo)系的原點(diǎn)、x軸和y軸做上標(biāo)記，方便全站儀采點(diǎn)[4]。

2.2 基于激光跟蹤的機(jī)器人三維空間坐標(biāo)計(jì)算

圖2 被測(cè)點(diǎn)極坐標(biāo)與全站儀坐標(biāo)之間的換算關(guān)系

此時(shí)，點(diǎn)P在2 個(gè)坐標(biāo)系中的坐標(biāo)也已獲取。

此時(shí)，點(diǎn)Q在2 個(gè)坐標(biāo)系中的坐標(biāo)也已獲取。

至此，已經(jīng)有3 組不在同一直線上的點(diǎn)坐標(biāo)，理論上已經(jīng)可以計(jì)算全站儀坐標(biāo)系A(chǔ) 和工作坐標(biāo)系B 的轉(zhuǎn)換矩陣，不過為了使擬合計(jì)算的結(jié)果更精確，還可以繼續(xù)采集更多的點(diǎn)坐標(biāo)，此處假設(shè)采集更多的點(diǎn)。

這樣，就獲得了點(diǎn)R在2 個(gè)坐標(biāo)系中的坐標(biāo)。需要指出的是，這里為了方便說明，默認(rèn)將點(diǎn)P取在工作坐標(biāo)系B 的x軸正半軸上，點(diǎn)Q取在工作坐標(biāo)系B 的y軸正半軸上，點(diǎn)R取在工作坐標(biāo)系B 中z軸坐標(biāo)為正的區(qū)域，即dx、dy和dz均大于0，實(shí)際上也可以將點(diǎn)P或點(diǎn)Q取到工作坐標(biāo)系B 的坐標(biāo)軸負(fù)半軸，點(diǎn)R取到z軸坐標(biāo)為負(fù)的區(qū)域，那么就需要根據(jù)情況改變dx、dy和dz的符號(hào)。至此，已經(jīng)獲得了4 組點(diǎn)坐標(biāo)，如果想獲得更精確的轉(zhuǎn)換矩陣，還可以繼續(xù)采點(diǎn)，重復(fù)上述采集點(diǎn)R的步驟。

假設(shè)現(xiàn)在已經(jīng)采集了n個(gè)點(diǎn)（n≥3），每個(gè)點(diǎn)對(duì)應(yīng)2組坐標(biāo)，分別是全站儀坐標(biāo)系A(chǔ) 和工作坐標(biāo)系B 中的坐標(biāo)。該文采用一個(gè)成熟的SVD 剛體變換方法求解坐標(biāo)系A(chǔ)到坐標(biāo)系B 的轉(zhuǎn)換矩陣，簡(jiǎn)要說明如下。

令W=qB·（qA）T，W是一個(gè)3×3 的矩陣，可以證明，只要這n組點(diǎn)中有任意3 點(diǎn)不在一條直線上，則W滿秩。當(dāng)W滿秩時(shí)，可對(duì)W進(jìn)行SVD 分解，得到W=UΣVS（U為n階酉矩陣；Σ為n×d階非負(fù)實(shí)數(shù)對(duì)角矩陣；V為右奇異矩陣；S為旋轉(zhuǎn)矩陣），那么旋轉(zhuǎn)矩陣S=U·VE；求解平移向量t=pB-SpA，S和t合成變換矩陣R，如公式（11）所示。

其中，z=（0，0，0）。根據(jù)全站儀坐標(biāo)系中的坐標(biāo)與工作坐標(biāo)系中的坐標(biāo)得出坐標(biāo)轉(zhuǎn)換矩陣。在機(jī)器人表面安裝反射棱鏡，全站儀通過激光自動(dòng)跟蹤反射棱鏡，隨著機(jī)器人的移動(dòng)，全站儀輸出機(jī)器人在全站儀坐標(biāo)系下的坐標(biāo)。根據(jù)坐標(biāo)轉(zhuǎn)換矩陣得出機(jī)器人在工作坐標(biāo)系下的坐標(biāo)，完成機(jī)器人的實(shí)時(shí)定位工作。

3 基于激光跟蹤的機(jī)器人三維空間定位方法應(yīng)用效果

為了驗(yàn)證基于跟蹤的機(jī)器人三維空間定位方法的應(yīng)用效果，該文結(jié)合重慶蘇家溝特大橋檢測(cè)項(xiàng)目，采用負(fù)壓吸附爬壁機(jī)器人對(duì)橋墩橋塔進(jìn)行表觀圖像采集和病害定位，在機(jī)器人背部固定位置安裝反射棱鏡，讓全站儀自動(dòng)跟蹤該棱鏡，通過該文提出的標(biāo)定方法得到的坐標(biāo)系轉(zhuǎn)換關(guān)系即可實(shí)時(shí)獲得機(jī)器人在工作坐標(biāo)系的坐標(biāo)，選取20 個(gè)監(jiān)測(cè)點(diǎn)對(duì)爬壁機(jī)器人在橋墩橋塔的位置進(jìn)行精確定位，并與橋梁的工程測(cè)繪結(jié)果進(jìn)行對(duì)比，結(jié)果見表1。由表1 可知，兩者的平面坐標(biāo)誤差最大值為4.27 cm，平面坐標(biāo)誤差最小值為0.62 cm，兩者的高程誤差最大值為4.14 cm，高程誤差最小值為0.71 cm。

表1 激光跟蹤的機(jī)器人定位實(shí)測(cè)結(jié)果與橋梁工程測(cè)繪結(jié)果對(duì)比

4 結(jié)語

該文以重慶蘇家溝特大橋?yàn)槔捎美碚摲治龅姆椒ㄓ?jì)算機(jī)器人在工作坐標(biāo)系的坐標(biāo)，將激光跟蹤的機(jī)器人定位實(shí)測(cè)結(jié)果與橋梁工程測(cè)繪結(jié)果進(jìn)行對(duì)比，研究分析方法的可行性，得到以下2 個(gè)結(jié)論：1）在機(jī)器人表面安裝反射棱鏡，全站儀通過激光自動(dòng)跟蹤反射棱鏡，隨著機(jī)器人的移動(dòng)，全站儀輸出機(jī)器人在全站儀坐標(biāo)系下的坐標(biāo)，根據(jù)提出的標(biāo)定方法可以得到坐標(biāo)系轉(zhuǎn)換關(guān)系，即可實(shí)時(shí)獲得機(jī)器人在工作坐標(biāo)系的坐標(biāo)。2）采用激光跟蹤的機(jī)器人定位實(shí)測(cè)結(jié)果與橋梁工程測(cè)繪結(jié)果的對(duì)比表明，兩者的平面坐標(biāo)誤差最大值為4.27 cm，平面坐標(biāo)誤差最小值為0.62 cm，兩者的高程誤差最大值為4.14 cm，高程誤差最小值為0.71 cm。