基于位姿誤差補償?shù)娜娔X鑿巖臺車釬桿定位算法

夏毅敏，李正光，羅建利，曾桂英，馬劼嵩

(1.中南大學(xué) 機電工程學(xué)院，湖南 長沙，410083；2.中南大學(xué) 高性能復(fù)雜制造國家重點實驗室，湖南 長沙，410083；3.中國鐵建重工集團有限公司，湖南 長沙，410100)

全電腦鑿巖臺車是一種集機械、電子、液壓等多學(xué)科技術(shù)于一體的隧道施工設(shè)備。與傳統(tǒng)鑿巖設(shè)備相比，全電腦鑿巖臺車能有效控制超挖量[1]，降低因地質(zhì)原因引起的施工事故，保障工作人員的施工安全。在施工過程中，關(guān)鍵技術(shù)之一是釬桿的快速準確定位。釬桿的自動定位精度和定位速度經(jīng)常出現(xiàn)不滿足工程實際需求的情況，嚴重影響了隧道施工效率和施工質(zhì)量。目前，鑿巖臺車在進行釬桿定位時的研究主要包括運動學(xué)誤差分析和誤差補償運動學(xué)求逆解2部分。近年來，國內(nèi)外學(xué)者對鑿巖臺車的運動學(xué)研究較多，如：JIANG等[2-3]在求解6r-2p鑿巖臺車的逆解時，為避免求解非線性系統(tǒng)，改變關(guān)節(jié)輸入條件，計算過程不涉及數(shù)值迭代和操縱矩陣；WANG等[4]利用對偶四元數(shù)法替代DH法進行建模，對六自由度鑿巖臺車進行逆運動學(xué)求解；何清華等[5]建立了多關(guān)節(jié)鉆臂的運動學(xué)方程，根據(jù)鉆臂的實際工作特點和工作范圍提出了一種求逆解的方法；周友行等[6]提出了一種基于運動軌跡的求解多關(guān)節(jié)冗余機器人運動學(xué)逆解方法。上述研究均未充分考慮鑿巖臺車臂架運動學(xué)誤差的影響，并且未分析存在誤差時的逆解計算方法。為此，本文作者借鑒工業(yè)機器人運動學(xué)誤差補償研究[7-10]和遺傳算法在工程中的應(yīng)用[11-14]，以DH法建立臂架正向運動學(xué)模型，通過計算臂架動態(tài)誤差和靜態(tài)誤差，建立運動學(xué)誤差補償模型；在進行釬桿定位時，采用多種群遺傳算法，結(jié)合無誤差補償運動學(xué)逆解結(jié)果、移民算子和人工選擇算子，計算誤差補償模型逆解，保證鑿巖臺車釬桿的定位精度和速度。

1 釬桿定位方法

1.1 臂架結(jié)構(gòu)

全電腦鑿巖臺車臂架包括6個回轉(zhuǎn)關(guān)節(jié)和2個移動關(guān)節(jié)，釬桿安裝在推進梁上，其主要結(jié)構(gòu)如圖1所示。

1.2 釬桿定位方法

影響鑿巖臺車釬桿位姿誤差的主要因素分為動態(tài)誤差和靜態(tài)誤差，其中，動態(tài)誤差包括外力、慣性力等導(dǎo)致的彈性變形和振動誤差，靜態(tài)誤差包括結(jié)構(gòu)參數(shù)誤差、環(huán)境因素(溫度、磨損)導(dǎo)致的尺寸誤差和傳感器誤差等。

在給定釬桿末端位姿情況下，可以根據(jù)無誤差正向運動學(xué)模型求逆解得到對應(yīng)的關(guān)節(jié)變量(角度和距離)，并將其作為計算臂架動態(tài)誤差、靜態(tài)誤差的輸入值和釬桿誤差補償模型求逆解的初始搜索值。同時，為保證計算速度和計算精度，預(yù)先設(shè)定迭代次數(shù)，通過MPGA算法對各關(guān)節(jié)值不斷搜索和誤差補償，得到臂架優(yōu)化關(guān)節(jié)值，使釬桿達到期望位姿。根據(jù)所提的研究思路，對釬桿定位問題進行研究，具體研究路線如圖2所示。

圖1 鑿巖臺車臂架結(jié)構(gòu)圖Fig.1 Boom structure of rock-drilling jumbo

圖2 鑿巖臺車釬桿定位研究Fig.2 Positioning research on drill rod for rock-drilling jumbo

2 釬桿運動學(xué)誤差補償模型

2.1 釬桿無誤差運動學(xué)求解

本文只對錨桿油缸固定的一般工況進行討論，即錨桿關(guān)節(jié)為過渡關(guān)節(jié)，關(guān)節(jié)角恒為0°。由于DH法屬于參數(shù)建模方法，坐標系需要建立在關(guān)節(jié)軸的延長線上，因此，需在不改變運動學(xué)結(jié)果的前提下對部分坐標系進行位姿變換。根據(jù)鑿巖臺車結(jié)構(gòu)參數(shù)，建立各關(guān)節(jié)坐標系，將推進梁回轉(zhuǎn)坐標系原點o6沿關(guān)節(jié)軸z6移動距離d6至x7o7z7平面，將錨桿坐標系o7沿關(guān)節(jié)軸x7移動距離a6至y8o8z8平面。臂架D-H參數(shù)如表1所示，臂架運動學(xué)模型如圖3所示。

圖3 釬桿正向運動學(xué)模型Fig.3 Forward kinematics model of drill rod

將參數(shù)代入關(guān)節(jié)坐標變換矩陣，得

所得臂架正向運動學(xué)模型為

式中：i表示第i個關(guān)節(jié)；C(θi)=cos(θi)；S(θi)=sin(θi)；P=[px,py,pz]，為釬桿位置矩陣；R=[n,o,a]，為釬桿的姿態(tài)矩陣。

式中：S24=sin(θ2+θ4) ；C24=cos(θ2+θ4)；Si=sin(θi)；Ci=cos(θi)。在實際施工過程中，為了隨時觀察鑿巖機的工作情況，需要在臂架孔序規(guī)劃后先確定推進梁回轉(zhuǎn)關(guān)節(jié)θ6，共0°，90°，180°和270°這4種工況。

根據(jù)臂架正向運動學(xué)模型進行推導(dǎo)，得

表1 臂架DH參數(shù)Table 1 DH parameters of boom

為防止出現(xiàn)分母為0導(dǎo)致計算溢出，將運動學(xué)求逆解分為S24≠0的一般工況和S24=0的特殊工況。

2.1.1 一般工況求解

當(dāng)S24≠0時，

在一般工況下，運動學(xué)逆解的求解順序為θ1→θ5→d8→d3→θ2→θ4。

2.1.2 特殊工況求解

當(dāng)S24=0時，C24=-1。由于數(shù)值解不封閉，有多組解，故先給定大臂伸縮關(guān)節(jié)d3。

在特殊工況下，運動學(xué)逆解的求解順序為θ2→θ4→θ5→d8→θ1。

2.2 臂架動態(tài)誤差補償

2.2.1 大臂及推進梁動態(tài)誤差計算

將大臂和推進梁分別簡化為一端固定的Bernoulli-Euler梁。臂架等效模型如圖4所示。由于大臂末端到推進梁的部件總質(zhì)量超過2 t，大臂末端最大變形達到0.1 m以上，屬于大撓度變形，經(jīng)典撓度公式不再適用，故重新對撓度公式進行改進。

圖4 臂架等效模型Fig. 4 Equivalent model of boom

1) 當(dāng)臂架末端受集中載荷時，臂架任意一點的扭矩為

式中：Me為臂架末端所受集中扭矩；F為末端所受集中力。xA和zA分別為末端位置x方向和z方向的位移；θA為轉(zhuǎn)角變形；Me和F受推進梁重心位置影響，其影響參數(shù)為大臂俯仰值θ2、推進梁俯仰值θ4和推進梁伸縮值d8。

臂架上任意位置的變形為

式中：z為撓度變形；θ為端截面轉(zhuǎn)角變形；x為軸向變形；E為臂架彈性模量；I為等效轉(zhuǎn)動慣量。

經(jīng)計算，得到臂架末端角度變形為

式中：L為臂架的全長。綜合式(18)～(20)得

2) 臂架受均布載荷時，根據(jù)式(19)，令

代入邊界條件s=0，θ=α(其中，α為末端角度變形)，。賦值a0=α，a1=0，得

由式(19)得

式中：

影響臂架撓度和端截面轉(zhuǎn)角的參數(shù)為大臂伸縮值d3、大臂俯仰值θ2、推進梁伸縮值d8和推進梁俯仰值θ4。其中，主要影響參數(shù)為大臂俯仰值θ2和大臂伸縮值d3。在旋轉(zhuǎn)關(guān)節(jié)θ2和移動關(guān)節(jié)d3的可變范圍內(nèi)對大臂變形求解，設(shè)定θ4和d8處于零位，對2種載荷施加方式進行線性疊加，撓度變形Δz和端截面轉(zhuǎn)角Δθ變形結(jié)果分別如圖5和圖6所示。撓度變形Δz分布于0.02～0.12 m之間，端截面轉(zhuǎn)角Δθ分布于0.46°～1.86°之間，變形與大臂長度成正相關(guān)。大臂軸向變形 Δx較小，數(shù)量級為10-5～10-4，在實際誤差補償中可以忽略。推進梁動態(tài)誤差規(guī)律與大臂的動態(tài)誤差規(guī)律相同，但是數(shù)量級過小，在動態(tài)誤差補償過程中可忽略。

圖5 大臂撓度變形Fig.5 Deflection of large arm

圖6 大臂端截面轉(zhuǎn)角Fig.6 Cross rotation angle of large arm

2.2.2 臂架動態(tài)誤差補償

對大臂移動關(guān)節(jié)d3進行動態(tài)誤差補償，在進行臂架定位時，基于無誤差補償模型求逆解所得各關(guān)節(jié)值，進而得到撓度變形Δz和端截面轉(zhuǎn)角Δθ，并將數(shù)值代入傳遞矩陣，變換方式如圖7所示。

圖7 動態(tài)誤差補償坐標變換Fig.7 Coordinate transformation of dynamic error compensation

圖7中，坐標系o3先沿x軸移動撓度值Δz，再沿y′軸旋轉(zhuǎn)端截面轉(zhuǎn)角 Δθ，變換至坐標系，進行坐標系傳遞變換至。變換關(guān)系為

式中：TΔ為動態(tài)誤差補償矩陣。

2.3 臂架靜態(tài)誤差補償

基于工業(yè)機器人離線標定方法，利用全站儀和角度儀對相鄰關(guān)節(jié)軸進行距離標定和偏轉(zhuǎn)角度標定，修正各關(guān)節(jié)結(jié)構(gòu)參數(shù)(α，a，θ，d)。根據(jù)攝動微分理論，對式(1)傳遞矩陣求偏導(dǎo)，得

2.4 釬桿誤差補償運動學(xué)模型建立

結(jié)合臂架動態(tài)誤差和靜態(tài)誤差補償，得到釬桿運動學(xué)誤差補償模型：

3 釬桿誤差補償模型逆解算法

根據(jù)釬桿的期望位姿和建立的鑿巖臺車釬桿運動學(xué)誤差補償模型，采用多種群遺傳算法(MPGA)對誤差補償模型求逆解，對各關(guān)節(jié)變量不斷搜索和誤差補償，計算各關(guān)節(jié)優(yōu)化值，使釬桿達到期望位姿。

3.1 釬桿運動學(xué)MPGA模型

3.1.1 構(gòu)造初始種群

初始種群為MPGA算法的搜索出發(fā)點。本文構(gòu)造的初始種群數(shù)為10個，每個種群個體數(shù)為100個。設(shè)定每個關(guān)節(jié)值在個體的染色體中占據(jù)20個基因位置，保證算法的求解精度。

3.1.2 構(gòu)造目標函數(shù)

已知釬桿期望位姿為

式中：Rg為釬桿期望姿態(tài)矩陣；Pg為期望位置矩陣。

將種群中各個個體的關(guān)節(jié)值代入臂架運動學(xué)誤差補償模型，得到個體的當(dāng)前釬桿位姿矩陣：

式中：Rc為釬桿當(dāng)前姿態(tài)矩陣；Pc為當(dāng)前位置矩陣。

構(gòu)造目標函數(shù)：

式中：ΔR=Rg-Rc，為各種群中個體的釬桿計算姿態(tài)和期望姿態(tài)的姿態(tài)誤差； ΔP=Pg-Pc，為釬桿計算位置和期望位置的位置誤差。

3.1.3 設(shè)計約束條件

根據(jù)正向運動學(xué)求逆結(jié)果，將各旋轉(zhuǎn)關(guān)節(jié)角范圍縮減為θi±10°，將各移動關(guān)節(jié)距離范圍縮減為di±0.2 m，優(yōu)化了各關(guān)節(jié)變量的搜索范圍，提高了算法的求解速度。

交叉率Pc決定了遺傳算法的全局搜索能力，傳統(tǒng)交叉率Pc取值范圍為0.7～0.9。MPGA采用多點交叉方式，增強了算法搜索能力，避免早熟收斂。

變異率Pm決定了局部搜索能力，傳統(tǒng)Pm取值范圍為0.001～0.050。由于搜索范圍已經(jīng)相對減小，需要增強局部搜索能力，因此，變異率Pm相對增大，取Pm為0.05。

3.1.4 構(gòu)造適應(yīng)度函數(shù)

采用輪盤賭法選擇適應(yīng)度高的個體進入下一代，構(gòu)造適應(yīng)度函數(shù)F為

式中：a和b分別為姿態(tài)誤差和位置誤差的權(quán)重系數(shù)。姿態(tài)參數(shù)單位為 rad，位置參數(shù)單位為 m，兩者數(shù)量級相同。經(jīng)過大量測試確定權(quán)重系數(shù)為a=1，b=1.5～2.5。

3.1.5 優(yōu)化方案

采用移民算子，打破各種群的獨立性，將各種群的最優(yōu)個體引入其他種群，替代其他種群中的最劣個體，實現(xiàn)各種群協(xié)同進化。

采用人工選擇算子，將每代中最優(yōu)個體放入精華種群，精華種群不進行選擇、交叉和變異等操作，只記錄和更新每一代最優(yōu)種群。

3.2 求解精度分析

給定一組關(guān)節(jié)值(θ1=20°，θ2=-45°，d3=0.4 m，θ4=-15°，θ5=0°，d8=2.0 m)，推進梁回轉(zhuǎn)關(guān)節(jié)角θ6和錨桿關(guān)節(jié)角θ7為0°，采用MATLAB進行算法編程，基于運動學(xué)誤差補償模型所得到的釬桿位姿計算 20次逆解。對比計算關(guān)節(jié)值和給定關(guān)節(jié)值相對誤差平均值，結(jié)果如表2所示。旋轉(zhuǎn)關(guān)節(jié)的相對誤差遠小于移動關(guān)節(jié)的相對誤差，兩者相差1個數(shù)量級。這是因為鑿巖臺車結(jié)構(gòu)尺寸較大，旋轉(zhuǎn)關(guān)節(jié)對位姿誤差的影響要遠大于移動關(guān)節(jié)的影響。大臂伸縮關(guān)節(jié)d3和推進梁伸縮關(guān)節(jié)d8具有明顯互補關(guān)系，可以適當(dāng)提高適應(yīng)度函數(shù)中位置誤差的權(quán)重系數(shù)b，增大移動關(guān)節(jié)對末端位姿的影響。

表2 逆解計算關(guān)節(jié)值及相對誤差平均值Table 2 Inverse calculation of joint value and average relative error value

分別選取算法中最快和最慢種群的進化過程進行對比，結(jié)果如圖8所示。MPGA算法在400代內(nèi)使適應(yīng)度達到0.99以上，求解能力強，收斂速度快，計算結(jié)果穩(wěn)定可靠，不會發(fā)生早熟收斂現(xiàn)象。因此，將算法結(jié)束條件設(shè)置為計算到第400代種群。

4 試驗驗證

圖8 MPGA遺傳算法進化曲線Fig.8 Evolution curves of MPGA genetic algorithm

圖9 隧道炮眼圖Fig.9 Borehole of tunnel

根據(jù)鑿巖臺車施工時的實際停車位置和炮眼設(shè)計，給定18組鑿巖臺車釬桿期望位姿值，期望位姿如圖9所示。其中，圓點位置代表釬桿定位位置，其上引出的線段為釬桿需要達到的姿態(tài)在掌子面上的投影。設(shè)定直徑為11 m級隧道掌子面距離鑿巖臺車大臂橫擺中心距離為12 m，圖示坐標系水平向左為y軸，豎直向上為z軸，垂直掌子面向里為x軸。進行基于誤差補償模型的逆運動學(xué)求解，得到各關(guān)節(jié)對應(yīng)數(shù)值。在車間使用全站儀于大臂橫擺中心設(shè)站，驅(qū)動臂架各關(guān)節(jié)至計算關(guān)節(jié)變量，利用全站儀測量釬桿位置，測量結(jié)果如表3所示(其中，[x,y,z]為期望坐標，為臂架移動后的釬桿實際坐標)。

表3 釬桿測量結(jié)果Table 3 Measuring results of drill rod

對比測試位置與期望位置的位置誤差，結(jié)果如圖10所示。從圖10可見：x方向平均誤差為0.031 m，y方向平均誤差為0.028 m，z方向平均誤差為0.029 m，總距離平均誤差為 0.056 m，并且總距離誤差均小于0.100 m，滿足工程實際定位要求。

圖10 釬桿末端位置誤差Fig.10 End position errors of drill rod

5 結(jié)論

1) 臂架負載和自重產(chǎn)生的動態(tài)誤差不容忽略，大臂撓度變形范圍為 0.04～0.12 m，端截面轉(zhuǎn)角范圍為0.46°～1.86°；軸向變形數(shù)量級為10-5～10-4，變形較小。推進梁的動態(tài)誤差規(guī)律與大臂的動態(tài)誤差規(guī)律相同。

2) 針對工程中全電腦鑿巖臺車釬桿快速定位問題，提出了一種基于運動學(xué)誤差補償模型和MPGA算法求逆解的釬桿定位方法，根據(jù)無誤差正向運動學(xué)逆解結(jié)果優(yōu)化各關(guān)節(jié)變量的搜索范圍，結(jié)合移民算子和人工選擇算子，提高算法的求解速度和收斂精度，得到給定位姿下的優(yōu)化關(guān)節(jié)變量。

3) 在車間對全電腦鑿巖臺車進行釬桿定位試驗，根據(jù)釬桿期望位姿矩陣求逆解得到優(yōu)化關(guān)節(jié)變量，驅(qū)動臂架各關(guān)節(jié)至對應(yīng)關(guān)節(jié)變量，實測釬桿位置與期望位姿值誤差小于0.100 m，滿足工程實際定位要求。