煤礦用鉆孔機器人鉆臂定位誤差補償研究

梁春苗，姚寧平,2,*，姚亞峰,2，彭 濤

(1.中煤科工西安研究院(集團)有限公司，陜西 西安 710077；2.煤炭科學研究總院，北京 100013)

《煤礦機器人重點研發(fā)目錄》要求探放水、防突和防沖鉆孔機器人具備自動調(diào)整鉆姿、智能鉆孔規(guī)劃和定位功能，目前鉆機位姿調(diào)節(jié)采用開環(huán)控制，受加工誤差、裝配誤差和零部件磨損等影響，自動調(diào)節(jié)時無法達到精確定位，進而影響瓦斯抽采、探放水、防突和防沖效果。煤礦井下鉆孔自動精確定位是實現(xiàn)鉆孔機器人自主施工孔群的難點，也是實現(xiàn)少人化、無人化的關(guān)鍵技術(shù)。

國內(nèi)外少有煤礦井下鉆孔機器人鉆臂定位和位姿補償方面研究，而在鑿巖機器人鉆臂位姿調(diào)節(jié)、控制和定位技術(shù)方面成果較多，已成為自主化鑿巖機器人的關(guān)鍵技術(shù)[1]。中南大學率先開展了鑿巖機器人方向研究，何清華[2]、謝習華[3]等對大臂伸縮、推進梁伸縮、翻轉(zhuǎn)定位產(chǎn)生的誤差進行分析和誤差補償，建立了位姿誤差模型。近幾年中南大學學者將研究成果向產(chǎn)品轉(zhuǎn)化，夏毅敏等[4-6]基于有限元仿真，采用正交試驗法確定影響臂架柔性誤差的主要參數(shù)，得到柔性誤差補償公式，并建立基于位置誤差的運動學誤差標定模型，使釬桿的絕對定位誤差降低了79.6%；并從釬桿末端位姿誤差補償角度出發(fā)，建立基于釬桿運動學誤差補償模型和MPGA 求逆解關(guān)節(jié)變量的釬桿定位算法，實測釬桿定位誤差小于0.1 m，滿足工程實際定位要求。吳昊駿從工程應(yīng)用實際出發(fā)，建立鉆孔位姿參數(shù)和鉆具末端塌落量的函數(shù)關(guān)系，對定位誤差進行補償[7-8]；上述文獻主要針對工程行業(yè)的鑿巖機器人或者全電腦鑿巖臺車，遠不及工業(yè)機器補償時的高精度和實時性，張憲民等[9]研究并聯(lián)機器人時，基于關(guān)節(jié)間隙使用運動學標定識別誤差種類并分析其影響規(guī)律，并在逆運動學模型中補償，提高了重復(fù)定位精度，但是缺乏實時性。因此，史曉佳[10]、李睿[11]、杜亮[12]等借助在機器人末端加裝激光跟蹤儀進行誤差研制或者在線誤差二次補償，提高了機器人控制精度，但是忽視了機械臂因連桿柔性、關(guān)節(jié)柔性等非線性變形的綜合影響導致末端產(chǎn)生偏差，缺乏剛?cè)崂碚撝危S后，譚月勝[13]、李文龍[14]等結(jié)合剛?cè)狁詈辖⒛Ｐ瓦M行快速補償，得到了有效和實用的補償效果。

上述文獻分別針對鑿巖機器人和工業(yè)機器人兩個不同行業(yè)誤差補償?shù)男枨螅眠\動學建模方法，對幾何尺寸誤差、關(guān)節(jié)間隙或幾何尺寸誤差與關(guān)節(jié)間隙耦合產(chǎn)生的誤差進行分析，建立了剛?cè)峤Y(jié)合的補償模型，實現(xiàn)了誤差補償，對于鉆孔機器人鉆臂定位誤差補償研究具有借鑒意義，然而上述計算方法無法保證計算速度和響應(yīng)的實時性。因此，筆者通過分析鉆臂定位精度影響因素，借助激光測距和開孔定位儀實測參數(shù)，建立在線鉆臂誤差補償模型，實現(xiàn)鉆臂實時誤差監(jiān)測與誤差補償，為煤礦井下無人化做好技術(shù)儲備。

1 經(jīng)典鉆臂運動學誤差補償方法

1.1 鉆臂運動學分析與正逆解求解

1.1.1 鉆臂結(jié)構(gòu)分析與運動學建模

鉆臂是鉆進機器人的主要工作機構(gòu)，鉆臂的主要動作包含方位角調(diào)節(jié)、升降調(diào)節(jié)、傾角調(diào)節(jié)和孔口位置的平移調(diào)節(jié)。圖1 為鉆臂結(jié)構(gòu)，主要由平移機構(gòu)、主機、上穩(wěn)固裝置、傾角調(diào)節(jié)機構(gòu)、升降機構(gòu)和方位回轉(zhuǎn)機構(gòu)6 部分組成。

圖1 鉆臂結(jié)構(gòu)Fig.1 Structure of drilling arm

分析對鉆臂結(jié)構(gòu)和動作可知，鉆臂有傾角回轉(zhuǎn)θ2、方位回轉(zhuǎn)θ1、機身舉升d1和機身平移d34 個自由度。利用D-H 方法建立鉆進機器人鉆臂運動學模型，鉆臂坐標系如圖2 所示。鉆臂D-H 參數(shù)見表1[15-16]。

表1 鉆臂D-H 參數(shù)Table 1 D-H parameters of drilling arm

圖2 鉆臂坐標系Fig.2 Coordinate system of drilling arm

1.1.2 鉆臂正運動學求解

兩相鄰關(guān)節(jié)坐標系之間的齊次變換矩陣為：

可得到鉆臂執(zhí)行機構(gòu)末端與鉆臂基坐標系的關(guān)系矩陣為：

矩陣中含有θ1、θ2、d1、d3共4 個未知數(shù)，當這4 個未知的關(guān)節(jié)變量已知時，即可通過式(2)得到鉆臂末端執(zhí)行機構(gòu)相對于鉆臂基坐標系的位姿正解。

1.1.3 鉆臂逆運動學求解

鉆臂有兩個轉(zhuǎn)動和兩個移動關(guān)節(jié)，滿足Pieper 結(jié)構(gòu)特點，因此，采用代數(shù)解法對鉆臂進行逆運動學求解[17-19]。求解用左乘式(2)的兩邊有：

式(3)和式(4)中兩個矩陣對應(yīng)矩陣元素相等，因此，可以列方程依次求θ1、θ2、d1、d3。

1.2 鉆臂定位誤差原因分析

機構(gòu)因加工誤差、變形、傳感器誤差、裝配間隙等，實際控制時與理論有偏差。鉆臂屬串聯(lián)機構(gòu)，從方位、舉升、傾角調(diào)節(jié)、平移油缸等誤差會逐級傳遞并放大，在鉆臂末端形成位姿總誤差，位姿誤差的存在會影響開孔位置精度[20-21]。鉆臂的傾角和方位角調(diào)節(jié)關(guān)鍵部件是回轉(zhuǎn)式減速器，回轉(zhuǎn)減速器為蝸輪蝸桿式結(jié)構(gòu)，其嚙合時存在間隙，一般回轉(zhuǎn)式減速器精度控制在10 弧分以內(nèi)，同時托板裝置采用導軌方式與機身連接，連接處間隙對機身末端定位精度將會有較大影響。因此，機身在平移時，傾角和方位角都會存在誤差。如圖3a 機身從0～450 mm 移動時，不同傾角時的傾角誤差；圖3b 機身從0～450 mm 移動時，不同方位時的誤差。

圖3 機身平移引起的傾角和方位角誤差Fig.3 Dip angle and azimuth errors caused by body translation

而因回轉(zhuǎn)減速器間隙存在，當轉(zhuǎn)動時對傾角和方位角誤差影響較大，當傾角從-30°～+40°變化時，最大誤差可達0.85°。若回轉(zhuǎn)中心距離鉆桿開孔位置2 000 mm，那么高度方向z將引起29.6 mm 的偏差，方位角在從0°～+15°變化時，最大誤差可達0.79°，若鉆桿開孔位置在2 000 mm 遠處，那么x方向?qū)⒁?7.5 mm 的偏差。機身平移會使得傾角發(fā)生較大偏移，尤其是平移至300 mm 行程時，所有傾角狀態(tài)下誤差最大，都將直接影響鉆孔作業(yè)精度，因此，需對鉆臂進行誤差的檢測與補償，從而提高定位精度。

1.3 鉆臂靜、動態(tài)誤差補償及逆解求解

影響鉆臂位姿誤差的靜態(tài)誤差包括結(jié)構(gòu)參數(shù)誤差和制造誤差等，動態(tài)誤差即受外力和慣性力，使其柔性變形的誤差。機構(gòu)誤差分析中的微分矩陣法是采用全站儀和角度儀標定相鄰關(guān)節(jié)偏移距離和偏轉(zhuǎn)角，得到結(jié)構(gòu)參量和運動參量的誤差后，求位姿變換矩陣全微分建立誤差模型。對式(1)各關(guān)節(jié)傳遞矩陣結(jié)構(gòu)參數(shù)求偏導得：

因此，鉆臂靜態(tài)誤差補償模型為：

利用材料力學和理論力學相關(guān)知識，根據(jù)機身截面型心計算慣性矩可得推進行程的撓性變形。當平移油缸推進長度l從0 到450 mm 變化時，轉(zhuǎn)角和撓度隨平移長度的變化曲線如圖4 所示。

圖4 機身轉(zhuǎn)角和撓度隨平移長度變化曲線Fig.4 Variation of body rotating angle and deflection with translation length

將撓度f和轉(zhuǎn)角θ兩個誤差量代入傳遞矩陣0T'3，對鉆臂末端進行誤差補償變換如圖5 所示。

圖5 動態(tài)誤差補償坐標變換Fig.5 Dynamic error compensation coordinate transformation

圖5 中坐標系O2先沿著x軸移動塌落量f值，再沿著y軸旋轉(zhuǎn)了θ，變換至O'3，變換關(guān)系為：

由于存在動態(tài)誤差和靜態(tài)誤差，因此，鉆臂運動學總誤差補償模型式如下：

采用基于RBF 神經(jīng)網(wǎng)絡(luò)法求逆解的方法對鉆臂總誤差補償模型進行逆運動學求解，使鉆桿開孔位置達到期望位姿[18-19]。

為了簡化輸入樣本矩陣，采用歐拉變換RPY 角的方式表達位姿，計算式如下：

在訓練中選取1 000 組進行訓練，基于鉆臂運動學誤差補償模型所得到的鉆臂末端鉆桿位姿計算逆解，并取100 組作為測試樣本數(shù)據(jù)，使用RBF 神經(jīng)網(wǎng)絡(luò)訓練得到的逆解與測試值角度誤差不超過0.028°，位移誤差不超過0.5 mm。

施工中根據(jù)地層信息設(shè)計鉆孔得到期望位姿，基于鉆臂誤差補償模型的逆運動學求解，求逆后的關(guān)節(jié)值輸入驅(qū)動鉆臂，利用全站儀測量鉆臂各關(guān)節(jié)變化后達到的實際位姿。圖6 是采用靜態(tài)、動態(tài)補償后，回轉(zhuǎn)中心距離鉆桿開孔位置2 000 mm 時，鉆臂期望位姿與實際位姿誤差，x方向平均誤差為9.6 mm(對應(yīng)方位誤差0.28°)，y方向平均誤差為18.2 mm，z方向平均誤差為16 mm(對應(yīng)傾角誤差0.46°)。

圖6 鉆臂期望位姿與實際位姿誤差Fig.6 Errors between the expected and actual pose of drilling arm

上述靜態(tài)和動態(tài)誤差補償對于自重和外部產(chǎn)生載荷導致的彈性變形、加工制造和檢測產(chǎn)生的誤差有效，可滿足工程實際要求。而使用一段時間后，因結(jié)合面的磨損間隙發(fā)生變化，補償效果不明顯。同時，經(jīng)典的全站儀誤差檢測法較為復(fù)雜，對測試人員專業(yè)水平要求較高，因此，本文提出了一種不借助專業(yè)全站儀和測角儀方法的激光測距組合定位誤差在線檢測的方法。

2 基于激光測距組合的鉆臂定位誤差檢測

2.1 激光測距組合位姿誤差檢測原理

如圖7 所示，鉆臂鉆孔施工前方放置一個測試靶板，動力頭后部鉆桿中心處安裝高精度激光測距傳感器，確保激光穿過鉆桿和動力頭內(nèi)孔，調(diào)整位置使激光發(fā)射和接收信號工作正常。將開孔定向儀放置在傾角和方位角回轉(zhuǎn)中心位置，手動調(diào)節(jié)傾角和方位角，找到方位和傾角的零位，并得到初始時的激光測距距離，再通過改變傾角和方位角后再次測得距離，采用該方法可獲得數(shù)組傾角、方位角輸入值和實際計算值之間的關(guān)系，可得鉆臂在位姿變化時的誤差量。

圖7 激光測距發(fā)射、接收與激光斑點位置投射Fig.7 Laser ranging transmission,reception and projection of laser spot position

鉆孔機器人較頻繁的施工傾角θ2范圍是-30°～+45°、方位角θ1是-15°～+15°，鉆孔時鉆臂位姿精度會因平移油缸平移距離d3從0～450 mm 推進變化而變化。因此，試驗中擬采集方位角-15°、0°和+15°，傾角-30°、-20°、-10°、0°、+10°、+20°、+30°、+45°，在每種姿態(tài)下，將機身從舉升立柱的中間部位推進到最前端，也就是平移油缸推著機身沿著托板平移450、300、150 和0 mm 時的4 種狀態(tài)下測試，與不同傾角和方位角誤差值組合，即方位測試12 組，傾角測試32 組。

2.2 激光測距組合位姿誤差檢測

測試分兩種狀態(tài)，一種是當方位角θ1=0°時，在不同傾角θ2變化下，測試平移油缸在4 種推進狀態(tài)下的試驗數(shù)據(jù)，此時獲取傾角誤差；另外一種是當傾角θ2=0°時，在不同方位角θ1變化下，測試平移油缸在4 種推進狀態(tài)下的試驗數(shù)據(jù)，此時獲取方位角誤差。

1)激光測距組合法測傾角誤差

圖8 是基于激光測距組合位姿誤差檢測計算實際傾角誤差的原理，O2和O3點分別是舉升和平移變化后傾角轉(zhuǎn)動中心，A1點是激光測距傳感器安裝點，該安裝位置在Y方向與轉(zhuǎn)動中心有Ld1距離，Lc1是鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點激光測距測定的距離，H0是鉆臂舉升初始距離，舉升高度為ΔH1，Hz是在傾角改變和平移油缸平移后，激光測距傳感器投射點距離車體地面的高度，可作為計算角度的復(fù)測值。鉆臂平移油缸平移距離為Ly，當在開孔定位軟件界面的將輸入傾角θ2變化后，Lz1是傾角變化后測距傳感器測定距離，實際傾角θ'2可由下式計算。

圖8 實際傾角誤差量計算原理Fig.8 Calculation principle of inclinaiton error

圖9 是測試得到的傾角θ2、平移距離d3（分別為450、300、150 和0 mm）和傾角誤差Δθ2之間的關(guān)系，4 條線就是4 種平移情況下對應(yīng)傾角變化時的誤差曲線。可以發(fā)現(xiàn)平移油缸在同一推進距離時，傾角越大，誤差量越大，隨著平移推進，同一傾角時誤差量逐漸增大，說明機身相對于托板裝置的間隙、傾角調(diào)節(jié)裝置裝配間隙都會引起末端誤差的產(chǎn)生。

圖9 傾角、平移距離和傾角誤差之間關(guān)系Fig.9 Relationship of dip angle,translation distance and dip angle error

2)激光測距組合法測方位誤差

圖10 是基于激光測距組合位姿誤差檢測計算實際方位誤差的原理，O0點是方位角轉(zhuǎn)動中心，A2點是激光測距傳感器安裝點，該安裝位置在x方向與轉(zhuǎn)動中心有Ld2距離，在x方向與方位角水平線有h距離，Lc2是鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點激光測距測定的距離，平移油缸平移距離為Ly，在開孔定位軟件界面，將輸入方位角θ1改變，Lz2是方位角變化后測距傳感器測定距離，Δl是因方位角變化h增加段的距離，實際方位角θ'1可由下3 式計算。

圖10 方位角誤差量計算原理Fig.10 Calculation principle of azimuth error

存在關(guān)系式：

因此，有：

通過下式可以計算得到實際方位角θ'1。

由實際方位角θ'1可通過下式計算x、y、z的具體坐標值，其中Lc2、Ly、Lz2、h2、L1、H0、ΔH1都是已知量，當鉆機鉆臂距煤臂某一位置時，實際方位角下鉆孔開孔位置末端x坐標減去理論傾角x坐標值，就是誤差在x方向的偏移量Δx，實際方位角下鉆孔開孔位置末端θ'1減去輸入方位角θ1，即方位角偏移量Δθ1，因此，依據(jù)激光測距組合位姿誤差的測試數(shù)據(jù)和下式計算數(shù)據(jù)。

圖11 是測試得到的方位角θ1、平移距離d3（分別為450、300、150 和0 mm）和方位角誤差Δθ1之間的關(guān)系，4 條線就是4 種平移情況下對應(yīng)方位角變化時的誤差曲線。圖中可以發(fā)現(xiàn)平移油缸在同一推進距離時，方位角越大，誤差量越大，隨著平移推進，同一方位角時誤差量逐漸增大，說明機身相對于托板裝置的間隙、方位角調(diào)節(jié)裝置裝配間隙都會引起末端誤差產(chǎn)生。

圖11 方位角、平移距離和方位角誤差之間關(guān)系Fig.11 Relationship among azimuth,translation distance and azimuth error

2.3 激光測距組合位姿誤差補償模型

通過激光測距組合定位誤差檢測方法測得了方位角和傾角的誤差，而這兩種誤差變化量比較大，若把這些大的變化歸為桿件結(jié)構(gòu)參數(shù)和運動變量誤差中的微小旋轉(zhuǎn)和微小位移較為困難，因此，不適用微分矩陣方法來補償，應(yīng)該引入虛擬關(guān)節(jié)，類似增加了一個剛性桿，在正解矩陣中增加坐標轉(zhuǎn)化矩陣，若在機身平移時致使傾角引起誤差，就在平移處增加補償矩陣，來補償油缸平移和傾角轉(zhuǎn)動引起的傾角誤差，若因方位回轉(zhuǎn)關(guān)節(jié)引起的，就在方位關(guān)節(jié)處增加補償矩陣。

對利用激光測距組合定位誤差檢測方法測得的位姿誤差進行補償，當計入各關(guān)節(jié)誤差時，鉆臂末端的桿3 相對于基坐標O0的坐標變換矩陣為：

1)傾角誤差補償

測試中發(fā)現(xiàn)改變傾角，末端開孔位置會產(chǎn)生一個方位上的偏角Δθ2，同時還會產(chǎn)生x軸的偏移量Δz，如圖12 所示，因此，在O2之后引入虛關(guān)節(jié)，增加一個坐標變換矩陣補償傾角關(guān)節(jié)引起的誤差，對傾角的位姿誤差進行補償?shù)茫?/p>

圖12 傾角誤差補償坐標變換Fig.12 Coordinate transformation of dip angle error compensation

2)方位誤差補償

上述激光測距組合定位誤差測試方法中發(fā)現(xiàn)方位角誤差主要來自關(guān)節(jié)轉(zhuǎn)動，末端開孔位置會產(chǎn)生一個方位上的偏角Δθ1，即此在O0和O1之間引入虛關(guān)節(jié)，繞Z軸旋轉(zhuǎn)角度Δθ1的齊次變換，變換矩陣如下：

因此，將傾角和方位角的誤差匯總的誤差補償為：

再利用RBF 神經(jīng)網(wǎng)絡(luò)對總誤差補償矩陣進行逆解求解[17-18]，得到的逆解即為誤差補償后的新傾角和方位角的控制輸入量。

3 激光測距組合位姿誤差補償算法的驗證

為了驗證激光測距組合位姿誤差檢測與補償方法的準確性，通過激光測距組合法建立誤差補償模型，對鉆臂位姿進行誤差補償，采用全站儀檢測進行驗證，以測試補償效果。

3.1 位姿誤差檢測試驗設(shè)計

鉆孔機器人頻繁施工傾角θ2范圍是-30°～+45°、方位角θ1是-15°～+15°，油缸平移距離d3從0～450 mm 推進，因此，試驗中擬采集方位角-15°、0°和+15°，傾角-30°、-20°、-10°、0°、+10°、+20°、+30°、+45°，在上述姿態(tài)下，平移油缸推動機身沿托板平移450、300、150和0 mm 下測試，傾角和方位角組合后，即方位測試12 組，傾角測試32 組。

分別用激光測距組合位姿誤差檢測方法與全站儀測試方法進行對比。

全站儀測試原理如圖13 所示：將機身簡化為一條線，在平行于機身上取3 點A、B、C，A點位于機身后端，B點位于機身中前部，C點位于機身前端，用全站儀記錄機身上A、B、C點坐標，通過計算3 點z坐標所成直線斜率得到傾角，通過3 點坐標x坐標所成直線斜率得到方位角。

圖13 全站儀驗證方法原理示意和測試照片F(xiàn)ig.13 Total station verification method and test photos

A、B、C3 點與鉆桿中心在x和z方向都有一個偏移量，利用A、B、C3 點向外插值法，可計算A、B、C的延伸至孔口位置的坐標值，經(jīng)過坐標變換，可以間接得到全站儀法推算的鉆桿開孔位置坐標。

3.2 誤差補償算法的驗證

全站儀測試也分兩種狀態(tài)，一種是當方位角θ1=0°時，在不同傾角θ2變化下，測試平移油缸在4 種推進狀態(tài)下的試驗數(shù)據(jù)，此時獲取傾角誤差；另外一種是當傾角θ2=0°時，在不同方位角θ1變化下，測試平移油缸在4 種推進狀態(tài)下的試驗數(shù)據(jù)，此時獲取方位角誤差。

表2 列出了使用全站儀，在不同傾角θ2變化下，平移油缸4 中推進狀態(tài)下，測得部分A、B、C3 點坐標值。從表中可以看出經(jīng)過激光測距組合位姿誤差檢測與補償后，與全站儀測試結(jié)果接近。

表2 全站儀檢測傾角驗證Table 2 Verification of inclination detected by a total station

圖14 是采用激光測距組合定位誤差檢測法與全站儀法誤差對比曲線，激光測距組合定位法傾角最大誤差差值在±0.5°以內(nèi)，比未采用誤差補償前的±0.85°提高了41.1%。

圖14 全站儀與激光測距法傾角檢測誤差對比Fig.14 Comparison of dip angle detection errors by total station and laser ranging method

表3 列出了使用全站儀，在不同方位角θ1變化下，平移油缸4 種推進狀態(tài)下，測得部分A、B、C3 點坐標值。從表中可以看出經(jīng)過激光測距組合位姿誤差檢測與補償后，與全站儀測試結(jié)果接近。

表3 全站儀檢測方位角驗證Table 3 Verification of azimuth detected by a total station

圖15 是采用激光測距組合定位誤差檢測法與全站儀法誤差對比曲線，激光測距組合定位法方位角最大誤差差值在±0.5°以內(nèi)，比未采用誤差補償前的±0.8°提高了37.5%。

圖15 全站儀與激光測距法方位角檢測誤差對比Fig.15 Comparison of azimuth detection errors by total station and laser ranging method

4 結(jié)論

a.鑒于煤礦用探放水、防突和防沖鉆孔機器人精確開孔定位與孔群全自動施工的迫切需求，本研究提出了一種激光測距組合誤差檢測方法，實現(xiàn)了鉆臂實時誤差監(jiān)測與誤差自動補償。

b.該方法不借助全站儀，通過測距實時計算實際和理論傾角、方位角差值，作為誤差補償后的新傾角和方位角的控制輸入量，簡化了測試方法，提高了測試精度，使鉆臂精度補償實現(xiàn)了工程應(yīng)用。

c.通過傳統(tǒng)全站儀測量方法對激光測距組合位姿誤差檢測方法進行了驗證，試驗表明：采用激光測距組合定位誤差檢測法，傾角最大誤差差值在±0.5°以內(nèi)，方位角最大誤差在±0.5°以內(nèi)，比未采用誤差補償前分別提高了41.1%和37.5%。

d.激光測距組合位姿誤差檢測與補償法仍需豐富現(xiàn)場試驗數(shù)據(jù)，不斷完善補償方法。同時，后續(xù)還可針對不同的誤差影響因素，搭建相應(yīng)的補償模型，更加精細化鉆臂開孔定位誤差補償算法。

符號注釋：

A1為激光測距傳感器安裝點；A2為激光測距傳感器安裝點；C1為cosθ1；C2為cosθ2；d1為機身舉升距離；d2為鉆臂方位回轉(zhuǎn)中心到機身平移之間垂直距離；d3為機身平移距離；f為機身撓性變形后的撓度；h為x方向與方位角水平線的垂直距離；H0為鉆臂舉升初始距離；ΔH1為舉升高度；Hz為傾角改變和平移油缸平移后，激光測距傳感器投射點距離車體地面的高度；l為平移油缸推進長度；Δl為因方位角變化h增加段的距離；Lc1為鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點激光測距測定的距離；Lc2為鉆臂在經(jīng)過高精度開孔定位儀尋找的初始水平點激光測距測定的距離；Ld1為激光測距傳感器安置點在Y方向與轉(zhuǎn)動中心的距離；Ld2為激光測距傳感器安裝點在x方向與轉(zhuǎn)動中心的距離；Ly為鉆臂平移油缸平移距離；Lz1為傾角變化后測距傳感器測定距離；Lz2為方位角變化后測距傳感器測定距離；O0為鉆臂方位回轉(zhuǎn)中心；O1為鉆臂舉升后最末端；O2為鉆臂傾角回轉(zhuǎn)中心；O3為機身平移后最末端；px、py、pz分別為鉆臂執(zhí)行機構(gòu)末端與鉆臂基坐標系的關(guān)系矩陣中x，y，z方向的位置值；P為末端手爪位姿設(shè)定為輸入樣本矩陣；S1為sinθ1；S2為sinθ2，T為各關(guān)節(jié)變量設(shè)定為樣本輸出矩陣；TΔ為動態(tài)誤差補償矩陣；θ為機身撓性變形后的轉(zhuǎn)角；θ1為方位回轉(zhuǎn)角度；θ1j為激光測距組合位姿誤差檢測結(jié)果；θ1l為理論輸入值；θ1q為全站儀測試后計算結(jié)果；為實際計算的方位角；θ2為傾角回轉(zhuǎn)角度；θ2j為激光測距組合位姿誤差檢測結(jié)果；θ2l為理論輸入值；θ2q為全站儀測試后計算結(jié)果；為實際計算的傾角；αi、ai、θ、di為機器人中各關(guān)節(jié)結(jié)構(gòu)參數(shù)；(α，β，γ)為歐拉變換RPY。