目標(biāo)引導(dǎo)系統(tǒng)模型研究

周斌斌，馬驪群，趙子越

(航空工業(yè)北京長(zhǎng)城計(jì)量測(cè)試技術(shù)研究所，北京 100095)

0 引言

大尺寸測(cè)量在航空航天、船舶、高鐵等大型制造領(lǐng)域中占有舉足輕重的地位[1]。激光跟蹤儀是一種帶有合作靶標(biāo)的高精度測(cè)量設(shè)備，其數(shù)字化水平直接影響了大尺寸測(cè)量的數(shù)字化水平[2]，能夠很好的在不同環(huán)境下解決各種測(cè)量問(wèn)題[3]。但激光跟蹤儀在大尺寸測(cè)量過(guò)程存在如下問(wèn)題：①不能像三坐標(biāo)機(jī)一樣實(shí)現(xiàn)自動(dòng)化測(cè)量，使用時(shí)需要手持反射鏡定位到目標(biāo)點(diǎn)；②在高空環(huán)境測(cè)量時(shí)，靶球的放置難度高，危險(xiǎn)系數(shù)大，不利于人工操作；③在大批量相同樣件的測(cè)量中，工作的重復(fù)性大，人工測(cè)量的效率低下。為了解決以上問(wèn)題，對(duì)于目標(biāo)模型已知的情況，采用機(jī)械臂及加持機(jī)構(gòu)來(lái)引導(dǎo)靶球抵達(dá)目標(biāo)點(diǎn)，從而實(shí)現(xiàn)目標(biāo)的自動(dòng)定位[4-7]。

在使用機(jī)械臂夾持靶球引導(dǎo)到目標(biāo)點(diǎn)的過(guò)程中，需要獲取機(jī)械臂末端執(zhí)行器的位姿和被測(cè)目標(biāo)點(diǎn)的位姿[8]。因此，本文先介紹和分析空間剛體位置和姿態(tài)的描述方法[9]，然后利用這些方法對(duì)激光跟蹤儀、機(jī)械臂、被測(cè)目標(biāo)點(diǎn)建模。最后使用Matlab仿真，驗(yàn)證方案的可行性[10]。

1 描述空間剛體位置和姿態(tài)的基本方法

對(duì)機(jī)械臂、激光跟蹤儀等進(jìn)行目標(biāo)定位時(shí)，需要獲取機(jī)械臂、激光跟蹤儀、模型被測(cè)點(diǎn)的位置和姿態(tài)。坐標(biāo)系中的位置參數(shù)使用(x,y,z) 來(lái)描述，常見(jiàn)的姿態(tài)表達(dá)方式有：正交旋轉(zhuǎn)矩陣法、三角度表示法、雙向量法、單位四元數(shù)法[9]。

1.1 單位四元數(shù)法

坐標(biāo)系的任何旋轉(zhuǎn)都可表示為繞特定軸旋轉(zhuǎn)特定角度，因此坐標(biāo)的旋轉(zhuǎn)可以用單位四元數(shù)表示，將其記作一個(gè)標(biāo)量加上一個(gè)向量

(1)

(2)

在使用單位四元數(shù)法時(shí)，需要已知特定軸和特定角度的數(shù)據(jù)，如果剛體的位姿在隨時(shí)改變，則無(wú)法使用此方法表示。

1.2 三角度表示法

三角度表示法是根據(jù)繞三個(gè)軸依次的旋轉(zhuǎn)的角度描述目標(biāo)姿態(tài)的方法。由于不能繞同一個(gè)軸線連續(xù)旋轉(zhuǎn)兩次，因此根據(jù)旋轉(zhuǎn)順序的不同，分為兩種不同的表達(dá)方式：歐拉式和卡爾丹式。目前廣泛使用的旋轉(zhuǎn)角順序?yàn)閞oll-pitch-yaw，即

R=Rx(θr)Ry(θp)Rz(θy)

(3)

該表示方法在描述船舶、飛機(jī)和車輛的位姿時(shí)十分直觀。roll,pitch,yaw分別表示繞x,y,z軸的旋轉(zhuǎn)。但是，三角度表示法的缺點(diǎn)是存在奇異點(diǎn)，該問(wèn)題會(huì)導(dǎo)致無(wú)法正確的描述目標(biāo)姿態(tài)。

1.3 雙向量表示法

(4)

對(duì)于機(jī)械臂的末端執(zhí)行器，定義接近向量比定義歐拉角或橫滾-俯仰-偏航角更方便。因此，本文采用雙向量法描述機(jī)械臂末端執(zhí)行器的位姿。

1.4 正交旋轉(zhuǎn)矩陣法

正交旋轉(zhuǎn)矩陣法使用參考坐標(biāo)系的坐標(biāo)軸單位向量來(lái)描述目標(biāo)的姿態(tài)。對(duì)于三維坐標(biāo)系，每一個(gè)單位向量有3個(gè)元素，它們組成了3×3階正交矩陣ARB

(5)

1.5 平移和旋轉(zhuǎn)的組合

為了表示兩個(gè)坐標(biāo)系之間位置變換和旋轉(zhuǎn)變換的組合，常使用齊次變換矩陣，其表達(dá)方式如式(6)所示。

(6)

為了保證建模的準(zhǔn)確性與高效性，本文中剛體的姿態(tài)采用雙向量表示法和正交旋轉(zhuǎn)矩陣法描述，而最終的位姿統(tǒng)一使用齊次變換矩陣方法描述。

2 目標(biāo)引導(dǎo)模型的建立

機(jī)械臂夾持靶球放置到目標(biāo)點(diǎn)的過(guò)程中，整個(gè)目標(biāo)引導(dǎo)系統(tǒng)中必須存在激光跟蹤儀、機(jī)械臂及夾持機(jī)構(gòu)和模型已知的被測(cè)目標(biāo)。激光跟蹤儀作為測(cè)量設(shè)備，建立測(cè)量坐標(biāo)系。機(jī)械臂作為執(zhí)行機(jī)構(gòu)，需要在已知機(jī)械臂的基坐標(biāo)和各個(gè)軸角度的情況下求解出末端軸的坐標(biāo)系。角耦目標(biāo)反射鏡作為被抓取的機(jī)構(gòu)，需要通過(guò)控制機(jī)械臂的6個(gè)角度調(diào)節(jié)靶球的位姿。被測(cè)目標(biāo)點(diǎn)的理論位置和姿態(tài)在設(shè)計(jì)圖紙中已經(jīng)給出。通過(guò)坐標(biāo)系的轉(zhuǎn)換，實(shí)現(xiàn)靶球和被測(cè)點(diǎn)的位姿統(tǒng)一，使測(cè)量系統(tǒng)中的所有坐標(biāo)系轉(zhuǎn)換到激光跟蹤儀的測(cè)量坐標(biāo)系中[11]。目標(biāo)引導(dǎo)系統(tǒng)組成及關(guān)聯(lián)如圖1所示。

圖1 目標(biāo)引導(dǎo)系統(tǒng)組成及關(guān)聯(lián)

角耦目標(biāo)反射鏡是激光跟蹤儀的合作靶標(biāo)，機(jī)械臂通過(guò)執(zhí)行機(jī)構(gòu)攜帶角耦目標(biāo)反射鏡并將其放置到目標(biāo)點(diǎn)，之后激光跟蹤儀即可測(cè)量被測(cè)目標(biāo)。在整個(gè)自動(dòng)引導(dǎo)測(cè)量系統(tǒng)中，激光跟蹤儀占據(jù)主導(dǎo)地位，它所建立的坐標(biāo)系為基坐標(biāo)系，其它坐標(biāo)系需要轉(zhuǎn)換到基坐標(biāo)系中。機(jī)械臂的任務(wù)是改變攜帶反射鏡的執(zhí)行機(jī)構(gòu)的位置和姿態(tài)，從而改變角耦目標(biāo)反射鏡的位姿。

通過(guò)以上分析可知，在整個(gè)過(guò)程中需要建立激光跟蹤儀、機(jī)械臂和被測(cè)目標(biāo)的模型。

2.1 激光跟蹤儀模型的建立

激光跟蹤儀作為工業(yè)測(cè)量中常用的設(shè)備，能夠?qū)崿F(xiàn)對(duì)目標(biāo)的自動(dòng)跟蹤和測(cè)量，具備測(cè)量精度高、測(cè)量方法簡(jiǎn)單等優(yōu)點(diǎn)，適用于大尺寸工件配裝測(cè)量[2-3]。激光跟蹤儀由跟蹤儀頭、控制器、用戶計(jì)算機(jī)、目標(biāo)角耦反射鏡(靶鏡)及測(cè)量附件(靶座、三腳架)等組成[3]，如圖2所示。其測(cè)量原理如圖3所示。

圖2 激光跟蹤儀的基本組成

在激光跟蹤儀工作時(shí)，其測(cè)距系統(tǒng)測(cè)得精確距離L，其測(cè)角系統(tǒng)測(cè)得目標(biāo)方位角α和俯仰角β。之后由測(cè)量組合(α,β,L) 經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換，獲得空間三維直角坐標(biāo)(X,Y,Z)。即

(7)

在激光跟蹤儀的測(cè)量過(guò)程中，通常搭配SA軟件的使用，該軟件可配合激光跟蹤儀建立測(cè)量坐標(biāo)系。因此，以SA軟件建立的測(cè)量坐標(biāo)系{O0}為基坐標(biāo)系，本文后面的坐標(biāo)均轉(zhuǎn)換到該坐標(biāo)系下。

2.2 機(jī)械臂模型的建立

機(jī)械臂末端軸坐標(biāo)系{O6}與基坐標(biāo)系{O1}的關(guān)系可以通過(guò)齊次變換矩陣求解。由于正交旋轉(zhuǎn)矩陣參數(shù)過(guò)多，故先求解機(jī)械臂的D-H參數(shù)，再求解末端軸坐標(biāo)系與基坐標(biāo)的關(guān)系[4]。

2.2.1機(jī)械臂D-H參數(shù)

本系統(tǒng)中采用的機(jī)械臂為KUKA KR60HA，適用于激光測(cè)量、部件測(cè)量等領(lǐng)域。該機(jī)械臂的腕軸具有極高的準(zhǔn)確性和速度。KUKA KR60HA機(jī)械臂6個(gè)軸的旋轉(zhuǎn)范圍如表1所示。

表1 旋轉(zhuǎn)軸的旋轉(zhuǎn)范圍

為了確定機(jī)械臂的D-H參數(shù)模型，還需要已知機(jī)械臂的基本尺寸參數(shù)，如圖4所示。

圖4 KUKA機(jī)械臂的參數(shù)

根據(jù)對(duì)機(jī)械臂的旋轉(zhuǎn)范圍和尺寸參數(shù)的分析，可以得到D-H參數(shù)模型，如表2所示。

表2 KUKA KR60HA的D-H參數(shù)

需要注意的是：本文中采用的D-H參數(shù)模型，均為標(biāo)準(zhǔn)的D-H參數(shù)模型。

2.2.2機(jī)械臂正運(yùn)動(dòng)學(xué)模型

采用齊次變換矩陣來(lái)表示機(jī)械臂的模型。對(duì)于該6軸機(jī)械臂，根據(jù)以上求出的機(jī)械臂D-H參數(shù)，其末端軸相對(duì)于基坐標(biāo)系的表達(dá)式為

(8)

Ai=Rotz,θiTransz,diTransx,aiRotx,αi

(9)

式中：Rotz,θi為繞著z軸旋轉(zhuǎn)θ度的變換矩陣；Transz,di為沿z軸方向，位移連桿長(zhǎng)度為d的變換矩陣；Transx,ai為沿x軸方向,連桿長(zhǎng)度為a的變換矩陣；Rotx,αi為繞x軸旋轉(zhuǎn)α角度的變換矩陣。

把得到的KUKA機(jī)械臂的D-H參數(shù)帶入到旋轉(zhuǎn)矩陣中，得到機(jī)械臂的變換矩陣模型，即

(10)

(11)

式(10),(11)中分別給出了A1和A2的值，同理可求出A3～A6的值

2.2.3末端執(zhí)行器與機(jī)械臂末端軸的轉(zhuǎn)換

末端執(zhí)行器的姿態(tài)采用雙向量法描述，如式(4)所示。末端執(zhí)行器{O7}和機(jī)械臂軸末端{(lán)O6}的姿態(tài)變換矩陣bAm可以通過(guò)末端執(zhí)行器的設(shè)計(jì)圖紙求出

(12)

因此，末端執(zhí)行器所建立的坐標(biāo)系{O7}與機(jī)械臂基坐標(biāo)系{O1}之間的轉(zhuǎn)換關(guān)系為

7Tb=7A66A55A44A33A22A11Ab

(13)

2.2.4激光跟蹤儀坐標(biāo)系與機(jī)械臂基坐標(biāo)系轉(zhuǎn)換

在目標(biāo)引導(dǎo)系統(tǒng)中，機(jī)械臂基坐標(biāo)系{O1}與激光跟蹤儀的坐標(biāo)系{O0}的相互轉(zhuǎn)換至關(guān)重要。機(jī)械臂基坐標(biāo)系{O1}[11-12]和激光跟蹤儀坐標(biāo)系{O0}的齊次變換矩陣為

(14)

因此，從末端執(zhí)行器所建立的坐標(biāo)系{O7}到激光跟蹤儀的坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

7Tm=7A66A55A44A33A22A11AbbAm

(15)

2.3 被測(cè)目標(biāo)模型建立

被測(cè)目標(biāo)模型是根據(jù)設(shè)計(jì)圖紙建立的。不同的被測(cè)物體的模型不同，任何一批被測(cè)物體在第一次被測(cè)之前都需要根據(jù)設(shè)計(jì)圖紙建模。本文以棱柱體為例建立被測(cè)目標(biāo)模型，如圖5所示。

圖5 目標(biāo)點(diǎn)模型

該目標(biāo)有8個(gè)棱角，從上面的4個(gè)棱角中，任意選取一點(diǎn)A作為被測(cè)點(diǎn)來(lái)求取目標(biāo)點(diǎn)的位置和姿態(tài)信息。

2.3.1目標(biāo)點(diǎn)的位置模型

A點(diǎn)在被測(cè)目標(biāo)坐標(biāo)系{O7}中的位置為(x1,y1,z1)。為了確定被測(cè)目標(biāo)點(diǎn)在激光跟蹤儀坐標(biāo)系{O0}下的位置信息，需要建立激光跟蹤儀坐標(biāo)系{O0}和被測(cè)目標(biāo)坐標(biāo)系{O7}之間的齊次變換矩陣。本文采用四點(diǎn)法求取齊次變換矩陣，這4個(gè)點(diǎn)分別為：激光跟蹤儀在被測(cè)目標(biāo)坐標(biāo)系{O7}中的原點(diǎn)、x軸上的一點(diǎn)、y軸上的一點(diǎn)、z軸上的一點(diǎn)。求取從被測(cè)目標(biāo)點(diǎn)的坐標(biāo)系{O7}到激光跟蹤儀的坐標(biāo)系{O0}之間的齊次變換矩陣為

(16)

A點(diǎn)在激光跟蹤儀坐標(biāo)系{O0}下的坐標(biāo)由式(17)計(jì)算

(17)

2.3.2被測(cè)目標(biāo)點(diǎn)的姿態(tài)模型

一般情況下，點(diǎn)是不存在姿態(tài)信息的，但是因?yàn)楸鞠到y(tǒng)中存在激光跟蹤儀和反射鏡，故根據(jù)系統(tǒng)的實(shí)際情況，為被測(cè)目標(biāo)點(diǎn)定義方向，即將點(diǎn)到激光跟蹤儀頭的方向向量定義為被測(cè)目標(biāo)點(diǎn)的姿態(tài)。

(18)

式中：xmm=xm-x1m；ymm=ym-y1m；zmm=zm-z1m。

2.4 總結(jié)

對(duì)于整個(gè)測(cè)量系統(tǒng)，機(jī)械臂的末端執(zhí)行機(jī)構(gòu)在激光跟蹤儀坐標(biāo)系{O0}下的最終位姿需和被測(cè)目標(biāo)點(diǎn)在激光跟蹤儀坐標(biāo)系{O0}的位姿相匹配，從而計(jì)算機(jī)械臂各個(gè)軸需要調(diào)整的角度。整個(gè)系統(tǒng)模型組成如圖6所示。

圖6 系統(tǒng)組成

3 仿真和驗(yàn)證

通過(guò)坐標(biāo)系的齊次變換矩陣，將建立的激光跟蹤儀、機(jī)械臂、被測(cè)目標(biāo)的模型轉(zhuǎn)換到激光跟蹤儀的坐標(biāo)系{O0}中。利用Matlab中RTB工具箱對(duì)整個(gè)模型進(jìn)行仿真分析，驗(yàn)證其準(zhǔn)確性。

圖7是由Matlab畫出的示意圖。坐標(biāo)系{O}代表的是激光跟蹤儀所建立的坐標(biāo)系{O0}。坐標(biāo)系(x1,y1,z1)代表被測(cè)目標(biāo)點(diǎn)的位姿。KUKA末端坐標(biāo)系代表機(jī)械臂末端軸坐標(biāo)系{O7}，該坐標(biāo)系的z軸為靶球的開(kāi)口方向，因已知目標(biāo)點(diǎn)的位姿(坐標(biāo)系(x1,y1,z1)的原點(diǎn)代表被測(cè)點(diǎn)的位置信息，z軸方向表示距離姿態(tài)信息) ，故通過(guò)Matlab求出機(jī)械臂各個(gè)軸的角度，如圖7所示。

圖7 坐標(biāo)系的統(tǒng)一

圖7中末端軸坐標(biāo)系{O7}的z軸朝向激光跟蹤儀頭，在不同的視角下的圖像如圖8所示。

從圖8中可以看出，機(jī)械臂的末端執(zhí)行器的位姿與目標(biāo)點(diǎn)的位姿相同，從而證明了模型的準(zhǔn)確性。

圖8 不同視角下的圖像

4 結(jié)論

為了實(shí)現(xiàn)激光跟蹤儀的自動(dòng)測(cè)量，建立了一種利用機(jī)械臂抓取靶球的目標(biāo)引導(dǎo)系統(tǒng)。詳細(xì)分析了剛體位姿描述的方法，并根據(jù)目標(biāo)導(dǎo)引系統(tǒng)的實(shí)際情況，分別建立了激光跟蹤儀、機(jī)械臂及被測(cè)目標(biāo)的理論模型。將所建立的激光跟蹤儀坐標(biāo)系{O0}作為基礎(chǔ)坐標(biāo)系，將機(jī)械臂的基坐標(biāo)系{O1}和被測(cè)目標(biāo)的坐標(biāo)系{O7}進(jìn)行統(tǒng)一，最后通過(guò)Matlab仿真分析，驗(yàn)證了該模型的可靠性和方案的準(zhǔn)確性。本文介紹的目標(biāo)引導(dǎo)系統(tǒng)可解決目前激光跟蹤儀無(wú)法全自動(dòng)連續(xù)測(cè)量的問(wèn)題，其中模型的建立和仿真實(shí)驗(yàn)為未來(lái)相關(guān)研究工作提供了參考依據(jù)。