基于線激光傳感器的盾構管片位姿檢測方法

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

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

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

1 管片檢測方案

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

1.1 管片抓取位姿檢測

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

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

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

1.2 管片安裝位姿檢測

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

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

結合待裝管片的內表面幾何特征，計算管片在目標安裝位置時的特征點相對于機械手坐標系的三維坐標值。最后結合已知的特征點在管片目標安裝坐標系的三維坐標值。計算得到管片目標安裝坐標系與機械手坐標系的位姿關系。

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

2 管片位姿計算方法

2.1 計算管片特征點坐標

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

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

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

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

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

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

經代數求解后取其正根得：

其中：

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

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

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

進而求得點E相對于機械手坐標系{t}的三維坐標值[xEt,yEt,zEt]T：

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

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

2.2 計算管片坐標系位姿

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

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

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

由于旋轉矩陣可以分解為[19]

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

則式(15)可描述為

展開式(21)，整理得到

將式(22)簡寫如下：

根據最小二乘原理，得到羅德格里參數：

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

3 算例計算

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

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

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

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

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

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

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

圖4 各組盾構管片位姿計算誤差曲線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°。將各組數據按照平移誤差和旋轉角度誤差分別繪制出曲線見圖4。

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

4 結論

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

2)推導了管片位姿檢測算法，該算法可根據圓弧邊上2個檢測點和側直邊上1個檢測點確定管片坐標系相對于機械手坐標系的位姿關系。

3)在圓弧邊上選擇4個檢測點，在側直邊上選擇2 個測點，隨機組合形成18 組檢測點，算法計算檢測點均能獲得良好的結果。分析算法誤差，發現弧線檢測點越接近右側，弧線上兩檢測點距離越遠，位姿誤差越小；側邊直線上的檢測點距離D點越遠，姿態精度也越高。