基于虛擬連桿機(jī)構(gòu)的類圓柱體目標(biāo)位姿獲取

羅磊，陳懇，馬振書，穆希輝

(1. 清華大學(xué) 機(jī)械工程系，北京100084;2. 總裝備部 軍械技術(shù)研究所，河北 石家莊050000)

0 引言

隨著國際反恐形勢的日趨嚴(yán)峻和反恐斗爭的深入，世界各國和軍隊(duì)十分關(guān)注危險(xiǎn)彈藥處理問題。由于危險(xiǎn)彈藥的保險(xiǎn)狀態(tài)、質(zhì)量狀況不明、放置的位置與姿態(tài)隨機(jī)性較大，搬運(yùn)過程中隨時(shí)有爆炸的可能［1］，因此，采用危險(xiǎn)彈藥處理機(jī)器人進(jìn)行彈藥處理作業(yè)，降低危險(xiǎn)彈藥的處理難度，避免工作人員的傷亡，對提高彈藥處理技術(shù)水平和作業(yè)效率具有重要意義［2-3］。國內(nèi)外一些公司、大學(xué)及研究院所，如美國的Remotec、iRobot，英國的Allen、ABP，德國的Telerob，中國科學(xué)院沈陽自動化研究所，上海交通大學(xué)，華南理工大學(xué)等，都對排爆機(jī)器人相關(guān)技術(shù)進(jìn)行了大量研究，并取得了突破性的進(jìn)展，在不適于人類作業(yè)的危險(xiǎn)環(huán)境作業(yè)、爆炸物處理過程中，均發(fā)揮了重要作用。時(shí)至今日，國際上已有數(shù)十種排爆機(jī)器人問世［4-5］，但普遍存在目標(biāo)位姿獲取和自主抓取的智能化程度不夠、作業(yè)效率不高、對操作人員要求高等問題。

1 虛擬連桿位姿測量機(jī)構(gòu)設(shè)計(jì)

作為被抓取物通常是彈藥彈丸等類圓柱體目標(biāo)的排爆機(jī)器人，其目標(biāo)位姿確定是實(shí)現(xiàn)機(jī)械手路徑規(guī)劃與自主抓取的基礎(chǔ)和關(guān)鍵環(huán)節(jié)［6］。

1.1 類圓柱體目標(biāo)位姿獲取研究現(xiàn)狀

目前，排爆機(jī)器人對類圓柱體目標(biāo)的位姿獲取主要通過以下途徑實(shí)現(xiàn):

1)在精度要求不高的情況下，通過多個(gè)不同角度攝像頭的2D 圖像信息，依靠人的目測與經(jīng)驗(yàn)進(jìn)行機(jī)械手末端執(zhí)行器的控制。該方法對操作者的經(jīng)驗(yàn)和心理素質(zhì)要求較高，末端執(zhí)行器的位置與實(shí)際偏差不能滿足需求，抓取效果不夠理想;由于該方法無法獲得目標(biāo)物的位姿信息，因此不可能實(shí)現(xiàn)自主抓取。

2)在手爪或腕部等部位安裝測距傳感器，輔助自主抓?。?］。這種方法能夠得到機(jī)器人手爪與抓取目標(biāo)之間的距離信息，但無法得到其姿態(tài)信息，由于測距傳感器安裝在機(jī)械手上，對準(zhǔn)目標(biāo)物的操作存在一定困難，其初始位姿誤差受到機(jī)械手多關(guān)節(jié)累積誤差影響較大，需要在抓取過程中進(jìn)行多次測量和調(diào)整，能夠在一定程度上消除操作人員的緊張程度。

3)采用雙目立體視覺系統(tǒng)，進(jìn)行位姿確定，作為機(jī)械手自主抓取閉環(huán)控制的反饋環(huán)節(jié)，實(shí)現(xiàn)自動抓取和路徑規(guī)劃［8］。該方法由于需要進(jìn)行攝像機(jī)標(biāo)定、目標(biāo)圖像分割處理、抓取點(diǎn)分析等處理，算法復(fù)雜，精度不夠理想，特別是在復(fù)雜背景中提取出目標(biāo)物并準(zhǔn)確識別較為困難，對操作人員要求也較高。

最新研究進(jìn)展表明，將具有角度反饋的云臺和激光測距傳感器綜合應(yīng)用于位姿測量［9-10］，甚至用于物體三維表面形狀的粗檢測是可行的［11］，該方法不但有效，而且定位效率高。

1.2 虛擬連桿位姿測量機(jī)構(gòu)設(shè)計(jì)

位姿獲取機(jī)構(gòu)主要由測量機(jī)構(gòu)底座、一個(gè)具有兩個(gè)轉(zhuǎn)動自由度的數(shù)字云臺(含兩個(gè)角度編碼器)和一個(gè)激光測距傳感器組成。在本機(jī)構(gòu)設(shè)計(jì)中，將激光測距傳感器發(fā)出的光線作為虛擬連桿，實(shí)現(xiàn)位姿測量，如圖1 所示。

圖1 虛擬連桿位姿測量機(jī)構(gòu)Fig.1 Structure and constitution of measurement mechanism

如圖2 所示，該位姿獲取機(jī)構(gòu)2 的底座通常與機(jī)器人本體1 固聯(lián)，從而與機(jī)械手3 建立相對固定的坐標(biāo)關(guān)系。由于排爆機(jī)器人通常需要遠(yuǎn)程遙控，為了將激光斑點(diǎn)打到需要的位置，通常需要在云臺上安裝雙目攝像頭。為了實(shí)現(xiàn)危險(xiǎn)場所的遠(yuǎn)距離作業(yè)和操縱的方便性，后端控制臺6 通常位于安全距離外，通過無線傳輸設(shè)備5 進(jìn)行云臺的轉(zhuǎn)動和俯仰遙操作控制。云臺的旋轉(zhuǎn)和俯仰角度、激光測距傳感器的數(shù)值等信息可以得到實(shí)時(shí)反饋，從而得到類圓柱體目標(biāo)4 相對于位姿獲取機(jī)構(gòu)的位姿信息。位姿獲取機(jī)構(gòu)和機(jī)械手均固聯(lián)在機(jī)器人機(jī)體上，其相對位置固定，所以可通過坐標(biāo)變換得到抓取目標(biāo)相對于機(jī)械手的位姿，從而實(shí)現(xiàn)自主抓取。

該位姿獲取機(jī)構(gòu)可看作是具有兩個(gè)旋轉(zhuǎn)自由度(即云臺旋轉(zhuǎn)和俯仰)和一個(gè)伸縮連桿的機(jī)械臂(虛擬連桿)。兩個(gè)角度可以通過云臺的角度編碼器反饋的數(shù)值確定，連桿長度可通過激光測距傳感器數(shù)值確定。因此，落在被測目標(biāo)物上的光斑的三維空間位置坐標(biāo)可以通過該機(jī)構(gòu)的運(yùn)動學(xué)方程求得。

圖2 位姿測量機(jī)構(gòu)與排爆機(jī)器人的結(jié)合應(yīng)用Fig.2 Measurement mechanism and EOD robot

顯然，一個(gè)光斑點(diǎn)可以確定三維空間位置，兩個(gè)不重合的光斑點(diǎn)可以確定一條直線，3 個(gè)不在一條直線上的光斑點(diǎn)可以確定一個(gè)平面。對于類圓柱體目標(biāo)物，其軸向旋轉(zhuǎn)自由度可以不加限制。也就是說，用兩個(gè)光斑點(diǎn)就能夠確定彈丸等類圓柱體目標(biāo)物的空間位置和姿態(tài)。

2 位姿獲取原理與計(jì)算

2.1 機(jī)構(gòu)運(yùn)動學(xué)方程建立

1)建立坐標(biāo)系:固定坐標(biāo)系在云臺的底座上，以云臺正面為y 軸正向，垂直向上為z 軸正向，以底座與云臺旋轉(zhuǎn)軸交點(diǎn)O 為坐標(biāo)原點(diǎn)，建立坐標(biāo)系;

2)D-H 參數(shù):機(jī)構(gòu)參數(shù)包括云臺水平旋轉(zhuǎn)角度θ1與俯仰角度θ2，云臺俯仰中心距坐標(biāo)系原點(diǎn)z 向距離d1，激光傳感器與云臺俯仰中心z 向距離d2，激光束虛擬連桿距離參數(shù)d3，激光傳感器相對于云臺旋轉(zhuǎn)中心x 向側(cè)移距離m，如圖3 所示。該位姿獲取機(jī)構(gòu)的D-H 參數(shù)見表1。

3)運(yùn)動學(xué)方程建立:根據(jù)D-H 參數(shù)表，建立本機(jī)構(gòu)的機(jī)器人運(yùn)動學(xué)方程，可得激光束在目標(biāo)物體上的光斑點(diǎn)的三維坐標(biāo):

圖3 位姿測量機(jī)構(gòu)參數(shù)Fig.3 Structure parameters of measurement mechanism

表1 位姿測量機(jī)構(gòu)的D-H 參數(shù)表Tab.1 D-H parameters of measurement mechanism

則有

可得光斑點(diǎn)坐標(biāo)為

當(dāng)機(jī)構(gòu)確定時(shí)，d1、d2、m 為定值。(1)式、(2)式中的參數(shù)d3、θ1、θ2值由激光測距傳感器、云臺角度編碼器反饋值確定。

2.2 姿態(tài)計(jì)算

根據(jù)兩個(gè)不重合目標(biāo)點(diǎn)相對于位姿獲取機(jī)構(gòu)底座的坐標(biāo)信息，以及位姿獲取機(jī)構(gòu)和機(jī)械手的相對位置關(guān)系，計(jì)算目標(biāo)物相對于機(jī)械手的位姿。

設(shè)實(shí)際測量得到的A、B 兩點(diǎn)對應(yīng)的水平角度θ1、俯仰角度θ2和激光測距d4值分別為(θ1A，θ2A，d4A)、(θ1B，θ2B，d4B)，代入(2)式可得，兩點(diǎn)相對于測量坐標(biāo)系的坐標(biāo)值PA(xA，yA，zA)、PB(xB，yB，zB)。

則有

轉(zhuǎn)化為單位長度矢量:

該矢量表示了目標(biāo)物軸線的姿態(tài)。

2.3 位置計(jì)算

A、B 兩點(diǎn)的中點(diǎn)D 在目標(biāo)物軸線上的投影C點(diǎn)為機(jī)械手自主抓取中心，求解C 點(diǎn)坐標(biāo)。顯然，C點(diǎn)位于OAB 平面內(nèi)，距離實(shí)測A、B 點(diǎn)相等且距離AB 線段的中點(diǎn)D 距離為目標(biāo)物的半徑R，如圖4 所示。

圖4 位置計(jì)算示意圖Fig.4 Position calculation

即滿足:

滿足上述條件的解有兩個(gè)，應(yīng)取y 坐標(biāo)較大的一個(gè)解。

2.4 反向驗(yàn)證

為確保操作的安全性，通常在抓取前需對位姿測量結(jié)果進(jìn)行驗(yàn)證。對于排爆機(jī)器人，通常取兩個(gè)測量點(diǎn)的中點(diǎn)進(jìn)行驗(yàn)證。其方法是:通過反向計(jì)算中點(diǎn)對應(yīng)的云臺水平角度、俯仰角度和激光測距值，將云臺轉(zhuǎn)動到相應(yīng)角度，觀察激光束斑點(diǎn)是否落在期望位置。

若已知期望點(diǎn)坐標(biāo)P(x，y，z)，機(jī)構(gòu)參數(shù)d1、d2、d3、m，則任意空間點(diǎn)對應(yīng)的機(jī)構(gòu)參數(shù)可通過以下公式計(jì)算:

3 應(yīng)用實(shí)例

3.1 測量平臺搭建

采用FY-SP2515 型室外數(shù)字云臺、DLS-C 型激光測距傳感器、兩個(gè)日立VK-S888N 型一體化攝像機(jī)、無線數(shù)字音視頻傳輸系統(tǒng)等設(shè)備進(jìn)行系統(tǒng)搭建，該系統(tǒng)由現(xiàn)場數(shù)據(jù)采集子系統(tǒng)、無線傳輸子系統(tǒng)、遠(yuǎn)程監(jiān)控子系統(tǒng)三部分組成，如圖5 所示。由于排爆機(jī)器人作業(yè)過程中有發(fā)生爆炸的可能，因此需要在視覺圖像引導(dǎo)下進(jìn)行遙操作測量。通過該系統(tǒng)，可實(shí)現(xiàn)雙目視覺引導(dǎo)下的三維空間點(diǎn)遙操作位置測量、類圓柱體位姿獲取，進(jìn)而引導(dǎo)機(jī)械手進(jìn)行自主抓取。

建立的測量機(jī)構(gòu)結(jié)構(gòu)參數(shù)為d1=326.3 mm，d2=266.3 mm，d3=d0+d4，其中:d0是指激光測距傳感器前端平面(發(fā)射點(diǎn))到云臺俯仰軸垂直平面的水平距離，是測距距離的偏置量，為安裝參數(shù)(60 mm);d4為激光測距傳感器反饋數(shù)值。激光光軸偏離俯仰軸中心的距離m 為0 mm，θ1取值范圍為0 ～2π rad，θ2取值范圍為

3.2 位姿測量與誤差

圖5 應(yīng)用系統(tǒng)總體結(jié)構(gòu)圖Fig.5 Overall structure of practical system

3.2.1 初始變換與位姿理論值將口徑為152 mm(R=76 mm)某型彈藥彈丸垂直放置，然后繞y 軸旋轉(zhuǎn)1.329 rad，再繞x 軸旋轉(zhuǎn)0.64 rad，最后進(jìn)行的平移量為(- 72.70 mm，1 447.90 mm，-967.10 mm). 此時(shí)z 向單位矢量(0，0，1)經(jīng)過以上變換后對應(yīng)的標(biāo)矢為nz=(0.971，-0.143，0.192);該型彈丸兩個(gè)定心位置高度分別為125 mm、304 mm，則其中點(diǎn)C 坐標(biāo)為PC(0 mm，0 mm，214.50 mm)，經(jīng)旋轉(zhuǎn)和平移變換后理論坐標(biāo)值為 PCt(- 20.90 mm，1 440.90 mm，-958.30 mm)。

3.2.2 姿態(tài)測量與誤差

經(jīng)過上述坐標(biāo)變換后，對其軸向兩處直徑為152 mm 的部位進(jìn)行測量，并使得激光束與彈藥軸線基本相交(允許存在一定誤差)，如圖1 所示。實(shí)際測得的A、B 兩點(diǎn)對應(yīng)的水平角度θ1、俯仰角度θ2和激光測距d4值分別為(2.36°，42.45°，1 550.80 mm)、(355.22°，42.80°，1 596.20 mm)，代入(2)式可得，兩點(diǎn)相對于測量坐標(biāo)系的坐標(biāo)為PA(56.34 mm，1 367.13 mm，- 890.71 mm)，PB(-116.34 mm，1 391.28 mm，-929.90 mm)。

代入(3)式、(4)式得AB = (172.68 mm，-24.15 mm，39.19 mm)，n = (0.966，- 0.135，0.219).

3.2.3 位置測量與誤差

將A、B 點(diǎn)坐標(biāo)值代入(5)式，并取y 值較大解得實(shí)測值:PCm= (- 12.37 mm，1 442.06 mm，-949.23 mm).

位置誤差:δ=PCt-PCm=(-8.53 mm，-1.16 mm，-9.07 mm).

位置綜合誤差:|δ| =12.50 mm.

3.3 誤差分析

1)實(shí)測姿態(tài)誤差為2.87%，1 400 mm 測量距離時(shí)的位置綜合誤差為12.50 mm，完全能夠滿足排爆機(jī)器人的使用要求。

2)影響測量的因素主要包括:激光測距傳感器和云臺反饋角度誤差，屬系統(tǒng)誤差;理論位置和姿態(tài)通過其他測量手段獲得，具有一定誤差;激光束延長線不通過彈藥軸線，是誤差主要因素，屬偶然誤差;彈藥表面的銹蝕也會帶來測量誤差。

3)減小誤差的措施:提高激光測距傳感器精度;通過操作訓(xùn)練，使得激光束延長線偏離彈藥軸線較少;可通過多次測量取平均值，降低偶然誤差。

4 結(jié)論

本文設(shè)計(jì)的基于虛擬連桿的目標(biāo)位姿獲取機(jī)構(gòu)，結(jié)構(gòu)簡單，測量精度較高，算法易于編程實(shí)現(xiàn)。該機(jī)構(gòu)和遙操作、臨場感等技術(shù)相結(jié)合，具有良好的應(yīng)用前景與廣闊的發(fā)展空間。通過該機(jī)構(gòu)能夠獲取彈藥彈丸等類圓柱體目標(biāo)物的位姿信息，可直接用于排爆機(jī)器人機(jī)械手的路徑規(guī)劃與自主抓取，能夠極大提高排爆機(jī)器人的作業(yè)效率和智能化程度。

References)

［1］孔新立，金豐年，蔣美蓉. 恐怖爆炸襲擊方式及規(guī)模分析［J］.爆破，2007，24(3):88 -92.KONG Xin-li，JIN Feng-nian，JIANG Mei-rong. Analysis of way and scale of terroristic raid［J］. Blasting，2007，24(3):88 -92.(in Chinese)

［2］范路橋，姚錫凡，祁亨年. 排爆機(jī)器人的研究現(xiàn)狀及其關(guān)鍵技術(shù)［J］.機(jī)床與液壓，2008，36(6):139 -143.FAN Lu-qiao，YAO Xi-fan，QI Heng-nian. Research progress and key techniques of explosive ordnance disposal robot［J］. Machine Tool ＆ Hydraulics，2008，36(6):139 -143. (in Chinese)

［3］金周英，白英.我國機(jī)器人發(fā)展的政策研究報(bào)告［J］.機(jī)器人技術(shù)與應(yīng)用，2009(2):1 -7.JIN Zhou-ying，BAI Ying. Policy research report of robot development［J］. Robot Technique and Application，2009(2):1 -7.(in Chinese)

［4］Dakis，Ann. Human presence detection:the navy has tri-service responsibility for EOD-related science and technology development［J］.CHIPS，2011，29(4):50 -52.

［5］Jean，Grace V. New robots planned for bomb disposal teams［J］.National Defense，2011，96(692):28 -31.

［6］Zuniga A L A，Pedraza O J C，Gorrostieta E，et al. Design and manufacture of a mobile robot applied to the manipulation of explosives［C］∥Proceedings of the 11th IEEE International Conference on Power Electronics Congress，CIEP 2008. Morelos，Mexico:IEEE，2008:84 -89.

［7］Zeng Jian-jun，Yang Ru-qing，Zhang Wei-jun. Research on semiautomatic bomb fetching for an EOD robot［J］. International Journal of Advanced Robotic Systems，2008，4(2):247 -252.

［8］Gonzalez-Galvan，Emilio J，Cesar A. Precise industrial robot positioning and path-tracking over large surfaces using non-calibrated vision［C］∥2011 IEEE International Conference on Robotics and Automation. Shanghai，China:IEEE，2011:5160 -5166.

［9］Ming-Chih Lu，Chen-Chien Hsu，Yin-Yu Lu. Image-based system for measuring objects on an oblique plane and its applications in 2-D localization［J］. IEEE Sensors Journal，2012，12(6):2249 -2261.

［10］Tom Epton，Adam Hoover. Improving odometry using a controlled point laser［J］. Autonomous Robots，2012(32):165 -172.

［11］Harvent J，Coudrin B，Brethes L，et al. Multi-view dense 3D modelling of untextured objects from a moving projector-cameras system［J］. Machine Vision and Applications，2013，24(8):1645 -1659.