基于線激光傳感器的盾構(gòu)管片位姿檢測方法

高翔，陶建峰，王明斗，劉成良，楊正，莊欠偉

(1.上海交通大學(xué)機(jī)械與動力工程學(xué)院，上海，200240；2.上海隧道工程有限公司，上海，200032)

盾構(gòu)掘進(jìn)技術(shù)主要是用于地下隧道挖掘工作，在當(dāng)前城市公路[1]、鐵路[2]和橋梁[3]建設(shè)中占據(jù)關(guān)鍵地位。管片拼裝機(jī)是盾構(gòu)掘進(jìn)機(jī)中的重要子系統(tǒng)，主要負(fù)責(zé)盾構(gòu)施工中的管片拼裝工作[4]。管片拼裝機(jī)運動工況復(fù)雜，人工操縱拼裝機(jī)器人嚴(yán)重影響了管片拼裝的效率和質(zhì)量，且給安全施工帶來隱患[5]，因此全自動管片拼裝成為拼裝機(jī)發(fā)展的方向之一。管片拼裝機(jī)的工作主要分為3個流程，分別為管片抓取、管片遞送和管片安裝[6]。在管片抓取時，管片的擺放位置存在誤差，需要對抓取裝置姿態(tài)進(jìn)行調(diào)整。在安裝時串聯(lián)機(jī)構(gòu)所造成的關(guān)節(jié)累積誤差[7]與已拼裝管片的施工拼裝誤差[8]也會對管片安裝精度造成影響。通過拼裝機(jī)機(jī)構(gòu)優(yōu)化、運動解耦和控制算法的改進(jìn)能夠顯著提升末端執(zhí)行機(jī)構(gòu)的定位精度[9-11]，但無法解決管片初始擺放誤差和目標(biāo)位置誤差問題。因此準(zhǔn)確檢測管片的初始位姿和安裝位姿是實現(xiàn)精準(zhǔn)拼裝的重要前提。國外對于管片位姿檢測的研究較多，其中日本最為領(lǐng)先。 WADA[12]闡述了一種全自動管片系統(tǒng)，該系統(tǒng)采用光學(xué)傳感器測量管片安裝位姿。TANAKA[13]采用了9組激光發(fā)射器和相機(jī)檢測管片高差和間隙，結(jié)合光截面檢測法、數(shù)字伺服技術(shù)和遠(yuǎn)程遙控混合控制技術(shù)，大大減少了拼裝時間，但該方法不適用于管片抓取工作。 HIRASAWA等[14]對全自動拼裝機(jī)偏差補償方法、相機(jī)視距以及圖像處理算法進(jìn)行改進(jìn)，拼裝機(jī)識別管片性能得到了進(jìn)一步完善。日本三菱重工提出利用超聲波裝置對管片位姿進(jìn)行精確測量的方法[15]。時至今日，日本三菱重工業(yè)株式會社、日本川崎重工業(yè)株式會社已成為國際上知名的盾構(gòu)掘進(jìn)機(jī)生產(chǎn)商，但激光檢測、力控導(dǎo)向等技術(shù)仍未被廣泛應(yīng)用于實際施工中[16]。國內(nèi)對于拼裝機(jī)研究主要在于微調(diào)機(jī)構(gòu)和控制算法，對管片姿態(tài)精準(zhǔn)識別技術(shù)的研究較少[6]。SHI 等[17]采用攝動法分析了管片安裝的位置和姿態(tài)精度，建立了管片安裝誤差模型，認(rèn)為誤差源在管片安裝中對位置誤差影響高于姿態(tài)誤差。張龍等[18]基于圖像識別技術(shù)識別布置在盾構(gòu)管片特定位置的靶標(biāo)，從而計算管片位姿，但該方式需要事先在每個待安裝管片上布置靶標(biāo)。本文作者針對待安裝管片的初始擺放位姿的偶然偏差和施工過程中累積的拼裝誤差，提出采用3個線激光檢測管片位姿的方案及算法，該方法適用于管片抓取位姿和管片安裝位姿的精確計算，從而有效提高管片拼裝精度和實現(xiàn)管片拼裝自動化。

1 管片檢測方案

檢測外界物體位姿有相機(jī)視覺、激光測距等方法。盾構(gòu)施工感知系統(tǒng)需要在惡劣環(huán)境下仍有較強的穩(wěn)定性，所以，采用激光傳感器保證施工過程中的確定性。將3個線激光傳感器安裝在機(jī)械手固定板上。通過線激光傳感器檢測管片邊緣的方法，計算待安裝管片初始擺放位姿和管片安裝目標(biāo)位姿。該位姿計算方法同時適用于管片抓取階段的初始擺放位姿和管片安裝階段的目標(biāo)安裝位姿。

1.1 管片抓取位姿檢測

矩形盾構(gòu)管片拼裝機(jī)器人主要工作過程為抓取管片、遞送管片和安裝管片3個工作過程。其中在抓取管片過程中，末端執(zhí)行機(jī)械手抓取后方傳送過來的管片存在初始擺放誤差。機(jī)械手需要首先檢測到管片的實際擺放位姿，然后沿著已規(guī)劃的軌跡完成管片抓取動作。

隧道拼裝管片為預(yù)制弧形管片，其內(nèi)表面是半徑為R的弧形面。如圖1所示，使用3 個線激光傳感器檢測管片內(nèi)表面邊緣的方法確定管片位姿。

線激光傳感器發(fā)射一道直線激光并照射在管片邊緣上，在管片邊緣上會形成深度突變。線激光傳感器的擺放位置布置為2個線激光傳感器照射管片的內(nèi)表面圓弧邊，1個線激光傳感器照射相鄰的內(nèi)表面?zhèn)戎边?。通過檢測反饋激光深度突變的位置，即可確定被檢測到的邊緣點(點A，點B和點C)分別相對于激光傳感器的三維坐標(biāo)。其中A點和B點為內(nèi)表面圓弧邊上隨機(jī)不同2 點，C點為側(cè)直邊上隨機(jī)一點。然后結(jié)合管片內(nèi)表面幾何特征和檢測的3個邊緣點，計算管片位姿檢測面上的側(cè)直邊頂點和圓弧邊緣圓心等特征點相對于機(jī)械手坐標(biāo)系的空間坐標(biāo)值。再根據(jù)特征點相對于管片抓取坐標(biāo)系的空間坐標(biāo)值，進(jìn)而算得管片抓取坐標(biāo)系與機(jī)械手坐標(biāo)系的相對位姿關(guān)系。

1.2 管片安裝位姿檢測

管片安裝階段主要運動為將管片從安裝位置附近移動至正確的安裝位置。施工過程中的不確定性因素導(dǎo)致已裝好的管片與理論位置相比有所偏差，且在同環(huán)內(nèi)所裝管片位姿誤差具有累積效應(yīng)，若依照理論位置進(jìn)行拼裝可能會造成管片碰撞或產(chǎn)生間隙，因此，管片拼裝機(jī)器人將管片遞送至安裝目標(biāo)位置附近，需要進(jìn)一步檢測實際的安裝位姿，然后再進(jìn)行管片微調(diào)安裝。

同樣，采用安裝在機(jī)械手上的3個線激光傳感器照射至相鄰已安裝好的管片邊緣上。如圖2所示，2個線激光傳感器照射待安裝管片目標(biāo)位置圓弧邊的相鄰管片的弧邊，A點和B點為該弧邊上的隨機(jī)不同兩點。最后1個線激光照射，同環(huán)相鄰管片的側(cè)直邊，C點為該側(cè)直邊上隨機(jī)一點。根據(jù)線激光深度突變現(xiàn)象提取出邊緣點(點A，點B和點C)，并計算出邊緣點在機(jī)械手坐標(biāo)系中的三維坐標(biāo)值。

結(jié)合待裝管片的內(nèi)表面幾何特征，計算管片在目標(biāo)安裝位置時的特征點相對于機(jī)械手坐標(biāo)系的三維坐標(biāo)值。最后結(jié)合已知的特征點在管片目標(biāo)安裝坐標(biāo)系的三維坐標(biāo)值。計算得到管片目標(biāo)安裝坐標(biāo)系與機(jī)械手坐標(biāo)系的位姿關(guān)系。

圖2 激光傳感器檢測管片安裝位姿原理Fig.2 Principle of measuring assembly pose of segment by laser sensors

2 管片位姿計算方法

2.1 計算管片特征點坐標(biāo)

采用線激光傳感器感知管片邊緣位姿，結(jié)合管片內(nèi)表面幾何形狀計算管片特征點的位姿坐標(biāo)。再根據(jù)特征點位姿坐標(biāo)計算得到待裝管片的抓取坐標(biāo)系和安裝坐標(biāo)系的位姿。管片抓取階段的初始擺放位姿和管片安裝階段的目標(biāo)安裝位姿，2種位姿計算方法相同，下面以管片抓取階段的管片初始擺放位姿計算為例進(jìn)行介紹。

機(jī)械手坐標(biāo)系即拼裝機(jī)工作坐標(biāo)系{t}，3 個線激光傳感器安裝在機(jī)械手上其坐標(biāo)系分別是坐標(biāo)系{s1}、坐標(biāo)系{s2}和坐標(biāo)系{s3}，其與機(jī)械手坐標(biāo)系{t}的轉(zhuǎn)移矩陣分別為Tts1，Tts2和Tts3。如圖3所示，點A和點B為弧形邊緣上的隨機(jī)不重合兩點，其相對于各自傳感器坐標(biāo)為[xAs1,yAs1,zAs1]T和[xAs2,yAs2,zAs2]T。點C為側(cè)面直線邊緣上的隨機(jī)一點，其相對激光傳感器3 的坐標(biāo)為[xAs3,yAs3,zAs3]T。假設(shè)B點距離C點距離較近，此3點相對于機(jī)械手坐標(biāo)系具有如下式關(guān)系：

圖3 激光傳感器計算管片位姿示意圖Fig.3 Calculation of position and position of segment by laser sensors

使用線激光檢測管片位姿問題即：已知前面圓弧邊上2 點A和B坐標(biāo)分別為[xAt,yAt,zAt]T和[xBt,yBt,zBt]T，側(cè)面圓弧邊上點C坐標(biāo)為[xCt,yCt,zCt]T。點D為管片端點，其中向量垂直于面ABD，以及內(nèi)表面圓弧半徑R。求解管片坐標(biāo)系相對于機(jī)械手坐標(biāo)系的轉(zhuǎn)換矩陣Ttseg。

線激光傳感器通過高度突變誤差提取出的管片邊緣點僅表示該點在該管片邊緣上，并無該點所在管片上的具體位置信息。所以，需要結(jié)合檢測面幾何特征約束獲得已知管片具體位置的特征點。通過特征點在抓取管片坐標(biāo)系{seg}和機(jī)械手坐標(biāo)系{t}的三維坐標(biāo)值，計算抓取管片坐標(biāo)系{seg}和機(jī)械手坐標(biāo)系{t}的相對位置關(guān)系。管片上的特征點是相對管片抓取坐標(biāo)系位置已知的點。通過本節(jié)方法可以計算出圖3中的點D，點E和點O在機(jī)械手坐標(biāo)系{t}中的三維坐標(biāo)值。

在圖3的三角形△BCD和三角形△ACD運用勾股定理得式(2)前2個方程。圓弧︵對應(yīng)圓心角與圓弧︵對應(yīng)圓心角之和等于圓弧︵對應(yīng)的圓心角，采用三角函數(shù)兩角和公式得式(2)第3 個方程，得方程組如下：

經(jīng)代數(shù)求解后取其正根得：

其中：

至此圖3中四面體ABCD中的角度和長度即可完全求出。下面根據(jù)上述已知參數(shù)，計算點D、點E和點O在機(jī)械手坐標(biāo)系{t}中的三維坐標(biāo)值。

根據(jù)式(11)求解得到點D相對于機(jī)械手坐標(biāo)系{t}的三維坐標(biāo)值[xDt,yDt,zDt]T。

已知側(cè)邊長度為lside,計算向量上的另一個端點E：

進(jìn)而求得點E相對于機(jī)械手坐標(biāo)系{t}的三維坐標(biāo)值[xEt,yEt,zEt]T：

根據(jù)點A，B和D外接圓心O幾何關(guān)系，計算點A，B和D的外接圓心O相對于機(jī)械手坐標(biāo)系{t}的三維坐標(biāo)值[xOt,yOt,zOt]T：

至此已求出管片頂點D、管片頂點E、管片圓心點O在機(jī)械手坐標(biāo)系{t}中的三維坐標(biāo)值。

2.2 計算管片坐標(biāo)系位姿

已求出管片頂點D、管片頂點E、管片圓心點O在機(jī)械手坐標(biāo)系{t} 中的三維坐標(biāo)值[xDt,yDt,zDt]T，[xEt,yEt,zEt]T和[xOt,yOt,zOt]T，結(jié)合三點在在管片坐標(biāo)系{seg}中的坐標(biāo)值[xDseg,yDseg,zDseg]T，[xEseg,yEseg,zEseg]T和[xOseg,yOseg,zOseg]T。計算管片坐標(biāo)系{seg}在機(jī)械手坐標(biāo)系{t}的位姿。位姿關(guān)系采用旋轉(zhuǎn)矩陣和平移向量表示，和有如下關(guān)系：

首先計算點D，E和O在坐標(biāo)系{t}中的重心坐標(biāo)Pct，以及這3 點在坐標(biāo)系{seg}中的重心坐標(biāo)Pcseg：

重心化可減少平移向量的影響，進(jìn)而可以先計算旋轉(zhuǎn)矩陣。因此，對于點D，E和O在2個坐標(biāo)系中的坐標(biāo)進(jìn)行重心化處理得：

由于旋轉(zhuǎn)矩陣可以分解為[19]

其中：I為單位陣；S為反對稱矩陣；a，b和c為羅德格里參數(shù)。

則式(15)可描述為

展開式(21)，整理得到

將式(22)簡寫如下：

根據(jù)最小二乘原理，得到羅德格里參數(shù)：

將求得的a，b和c代入式(20)得到旋轉(zhuǎn)矩陣，將旋轉(zhuǎn)矩陣代入式(15)中，求得平移向量。至此，管片坐標(biāo)系{seg}在機(jī)械手坐標(biāo)系{t}的位姿已求出。

3 算例計算

在SolidWorks 軟件中建立比例為1:1 的管片三維模型，建立坐標(biāo)系{seg}，在管片外側(cè)弧線和右側(cè)直線選取若干測點模擬線激光檢測到的邊緣點，其中P為圓弧邊上的測點，Q為側(cè)直邊上測點，各個測點坐標(biāo)如表1所示。

管片坐標(biāo)系{seg}相對于機(jī)械手坐標(biāo)系{t}，按照RPY 角描述方法進(jìn)行旋轉(zhuǎn)和平移[20]。設(shè)旋轉(zhuǎn)參數(shù)為[10°, 8°, 6°]，即先繞坐標(biāo)系{t}的Z軸旋轉(zhuǎn)10°，再繞坐標(biāo)系{t}的Y軸旋轉(zhuǎn)8°,最后繞坐標(biāo)系{t}的軸旋轉(zhuǎn)6°。然后分別沿著軸，軸和軸平移500 mm，100 mm 和900 mm。得到管片坐標(biāo)系{seg}相對于機(jī)械手坐標(biāo)系{t}的位姿矩陣如下式所示：

表1 邊緣點在坐標(biāo)系{seg}中的坐標(biāo)值Table 1 Coordinates of edge points in coordinate system{seg}

則表1中的邊緣點在坐標(biāo)系{t}中的坐標(biāo)如表2所示。

對盾構(gòu)管片位姿進(jìn)行計算時，根據(jù)算法在前弧線測點Pti(i=1,2,3,4)中任意選擇2個坐標(biāo)數(shù)據(jù)作為圖3中傳感器1 和傳感器2 的測點數(shù)據(jù)，側(cè)直線測點Qti(i=1,2,3)中任意選擇1個數(shù)據(jù)作為傳感器3 的測點數(shù)據(jù)，參考Dseg，Eseg和Oseg這3 個特征點在坐標(biāo)系{seg}中的參數(shù)。采用第2節(jié)的計算方法對各個管片的位姿進(jìn)行計算，并與真實姿態(tài)比較并計算誤差，結(jié)果如表3所示。

表2 邊緣點在坐標(biāo)系{t}中的坐標(biāo)值Table 2 Coordinates of edge points in coordinate system{t}

表3 盾構(gòu)管片位姿計算誤差Table 3 Calculation error of position and posture of shield segment

圖4 各組盾構(gòu)管片位姿計算誤差曲線Fig.4 Calculation error of pose of segment for every group

從表3可知：誤差絕對值最大分別為0.007 1 mm，0.006 3 mm，0.090 7 mm，0.000 9°，0.000 9°和0.010 3°。將各組數(shù)據(jù)按照平移誤差和旋轉(zhuǎn)角度誤差分別繪制出曲線見圖4。

從圖4可知：弧線檢測點越接近右側(cè)，弧線上檢測點距離越遠(yuǎn)，位姿誤差越小。同時，側(cè)邊直線上的檢測點距離D點越遠(yuǎn)，姿態(tài)精度也越高。因此，在保證弧線檢測點距離不至于過小的情況下，選擇檢測靠近管片頂點D處弧線檢測點，選擇檢測靠近管片定點E處直線檢測點，能夠獲得較好的檢測精度。

4 結(jié)論

1)提出了采用3個線激光傳感器檢測待抓管片和檢測待裝管片位姿的方法。該方法只需要檢測管片邊緣，無需在預(yù)制管片上做其他改動，實施方便可靠。

2)推導(dǎo)了管片位姿檢測算法，該算法可根據(jù)圓弧邊上2個檢測點和側(cè)直邊上1個檢測點確定管片坐標(biāo)系相對于機(jī)械手坐標(biāo)系的位姿關(guān)系。

3)在圓弧邊上選擇4個檢測點，在側(cè)直邊上選擇2 個測點，隨機(jī)組合形成18 組檢測點，算法計算檢測點均能獲得良好的結(jié)果。分析算法誤差，發(fā)現(xiàn)弧線檢測點越接近右側(cè)，弧線上兩檢測點距離越遠(yuǎn)，位姿誤差越?。粋?cè)邊直線上的檢測點距離D點越遠(yuǎn)，姿態(tài)精度也越高。