基于虛擬連桿機構的類圓柱體目標位姿獲取

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

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

0 引言

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

1 虛擬連桿位姿測量機構設計

作為被抓取物通常是彈藥彈丸等類圓柱體目標的排爆機器人，其目標位姿確定是實現機械手路徑規劃與自主抓取的基礎和關鍵環節［6］。

1.1 類圓柱體目標位姿獲取研究現狀

目前，排爆機器人對類圓柱體目標的位姿獲取主要通過以下途徑實現:

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

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

3)采用雙目立體視覺系統，進行位姿確定，作為機械手自主抓取閉環控制的反饋環節，實現自動抓取和路徑規劃［8］。該方法由于需要進行攝像機標定、目標圖像分割處理、抓取點分析等處理，算法復雜，精度不夠理想，特別是在復雜背景中提取出目標物并準確識別較為困難，對操作人員要求也較高。

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

1.2 虛擬連桿位姿測量機構設計

位姿獲取機構主要由測量機構底座、一個具有兩個轉動自由度的數字云臺(含兩個角度編碼器)和一個激光測距傳感器組成。在本機構設計中，將激光測距傳感器發出的光線作為虛擬連桿，實現位姿測量，如圖1 所示。

圖1 虛擬連桿位姿測量機構Fig.1 Structure and constitution of measurement mechanism

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

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

圖2 位姿測量機構與排爆機器人的結合應用Fig.2 Measurement mechanism and EOD robot

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

2 位姿獲取原理與計算

2.1 機構運動學方程建立

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

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

3)運動學方程建立:根據D-H 參數表，建立本機構的機器人運動學方程，可得激光束在目標物體上的光斑點的三維坐標:

圖3 位姿測量機構參數Fig.3 Structure parameters of measurement mechanism

表1 位姿測量機構的D-H 參數表Tab.1 D-H parameters of measurement mechanism

則有

可得光斑點坐標為

當機構確定時，d1、d2、m 為定值。(1)式、(2)式中的參數d3、θ1、θ2值由激光測距傳感器、云臺角度編碼器反饋值確定。

2.2 姿態計算

根據兩個不重合目標點相對于位姿獲取機構底座的坐標信息，以及位姿獲取機構和機械手的相對位置關系，計算目標物相對于機械手的位姿。

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

則有

轉化為單位長度矢量:

該矢量表示了目標物軸線的姿態。

2.3 位置計算

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

圖4 位置計算示意圖Fig.4 Position calculation

即滿足:

滿足上述條件的解有兩個，應取y 坐標較大的一個解。

2.4 反向驗證

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

若已知期望點坐標P(x，y，z)，機構參數d1、d2、d3、m，則任意空間點對應的機構參數可通過以下公式計算:

3 應用實例

3.1 測量平臺搭建

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

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

3.2 位姿測量與誤差

圖5 應用系統總體結構圖Fig.5 Overall structure of practical system

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

3.2.2 姿態測量與誤差

經過上述坐標變換后，對其軸向兩處直徑為152 mm 的部位進行測量，并使得激光束與彈藥軸線基本相交(允許存在一定誤差)，如圖1 所示。實際測得的A、B 兩點對應的水平角度θ1、俯仰角度θ2和激光測距d4值分別為(2.36°，42.45°，1 550.80 mm)、(355.22°，42.80°，1 596.20 mm)，代入(2)式可得，兩點相對于測量坐標系的坐標為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 點坐標值代入(5)式，并取y 值較大解得實測值: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)實測姿態誤差為2.87%，1 400 mm 測量距離時的位置綜合誤差為12.50 mm，完全能夠滿足排爆機器人的使用要求。

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

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

4 結論

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

References)

［1］孔新立，金豐年，蔣美蓉. 恐怖爆炸襲擊方式及規模分析［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］.機床與液壓，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］.機器人技術與應用，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.