基于激光測距的排爆機器人建模
基于激光測距的排爆機器人建模
作者:曾建軍,楊汝清
排爆(Explosive Ordnance Disposal,簡寫 EOD)機器人幫助排爆人員對可疑爆炸物品進行檢查、抓取、搬運和銷毀。排爆操作采用遙操作模式,排爆人員在遠處通過觀察攝像機視頻信息,然后再控制機器人和機械臂運動,排除爆炸物。如果爆炸物難以現場中和和分解,又或者由于擔心傷害人員和建筑而不能將之引爆,就必須操作排爆機器人抓取爆炸物,放入防爆筒,運輸到安全地帶作進一步處理。
排爆人員通過觀察 2D 攝像機視頻來操作機械手,由于 2D 攝像機不具有立體信息,操作者的整個操作過程就如同蒙上一直眼睛而只用一只眼睛觀察爆炸物,所以無法準確地抓取爆炸物,需要多次嘗試后才可能成功,而失敗的嘗試可能會引發觸發爆炸物等嚴重后果,所以非常不安全。即便使用多個攝像機,多個視頻信息也需要人腦的理解分析后才能指導操作,很不直觀。
另外,爆炸物所處的環境是未知的、非結構化的。在這種復雜未知障礙環境下,操作多關節機械臂抓取爆炸物并且不發生碰撞對排爆人員是個很大的挑戰。因為排爆人員無法確切的了解當前機械臂的位置狀態以及機械比各關節與障礙的準確距離,排爆人員要小心翼翼的操作機械臂,擔心手爪或機械臂碰到各種障礙物,而這種碰撞有可能引爆炸彈。這種操作模式不僅不安全,同時也使排爆人員一直處于非常緊張的狀態,勞動強度大,容易疲勞,效率低下。
如何使機械手抓取爆炸物作業能夠安全、快捷和輕松地完成成為排爆作業的重要研究內容。
英國的 ABP公司在 EOD 機器人機械臂各關節上安裝了角度傳感器,可以在控制顯示屏上顯示出機械臂的當前位置,這極大的幫助了操作者了解機械臂的狀態,而不是控制攝像機轉來轉去,通過視頻來獲取信息。德國的 TELEROB[2]公司將立體攝像機引入操作過程,方便操作者能方便的操作機械臂。韓國的ROBHAZ-DT2[3]搭建了一個基于觸覺的可穿戴的多模型的遙操作平臺。它將手控器、立體視覺和聲音控制結合起來,并設計成可穿戴的,試圖使排爆操作簡單、直觀和有效。但是在現階段,這種立體視覺太過昂貴,無法適應商業要求。上海交大針對排爆間歇性工作特點設計出機械臂電機的準極限理論[4]。沈陽自動化所開發了適合排爆的訓練平臺[5]。華南理工大學則在爆炸物視覺提取,手眼協調方面作了研究,結果不詳。
一些學者在未知非結構環境下機械臂自主探索環境并到達目標物方面做了深入的研究:E. Kruse[6]在 C 空間把目標象素作為探索的方向或者把阻擋目標的象素作為探索的方向;P. Renton[7]在物理空間把探索方向放在最大限度降低物理區域的點。 Belen[8]等提出了一種通用的 C 空間評價方法,并介紹了它在關節機械臂中的應用。Gupta[9]在忽略模型誤差、傳感器精度誤差的前提下,提出了基于C-space 和 W-space 的自適應觀察規劃。因為機械臂的拓展過程相當耗時,根據環境復雜程度不同,它需要 7~25 個掃描循環,而每個循環需要 1~2.5分鐘,所以現階段,將它應用到實際也是相當困難。
在國家 863 計劃和自然科學基金的資助下,上海交通大學機器人研究所開發出了基于激光測距的排爆機器人系統 SUPER-PLUS,采用該系統能安全準確的半自主抓取爆炸物。本文著重介紹該系統基于激光測距特點的機械手、爆炸物和障礙環境建模。
1 機械手建模
開放性圖形庫OpenGL是由SGI公司為其圖形工作站開發的IRIS GL演變而來的三維計算機圖形和模型庫。 OpenGL 最顯著的特點是與硬件系統的無關性,并能直接面向硬件調用3D 處理功能,其處理3D 圖形速度特別快。因此使用OpenGL 開發庫來建立排爆機械手仿真模型。
地排爆機器人SUPER-PLUS由一個關節輪式移動平臺和一個其裝載的機械手構成。移動平臺詳見文獻[10],本文不予討論。該機械臂的三個自由度分別是上臂俯仰,前臂俯仰和腕部轉動,末端安裝一個二指手爪。該機械臂是平面RR機械臂。
使用 OpenGL 庫的另一個重要原因是機械臂的各關節運動能夠通過其逐級疊加局部坐標變換來實現,非常便于實現各關節的牽連運動,其偽指令為:
glPushMatrix (); // Push Matrix Onto Stack
Coordinate Transform (transfer, rotation and scale);
Draw 1st arm;
glPushMatrix ();
Coordinate Transform (transfer, rotation and scale);
Draw 2nd arm;
…
glPopMatrix ();
glPopMatrix ();
…
對于機械臂模型,簡化為包絡整個真實機械臂(包括一些輔助設施,如激光距離傳感器和攝像機)的矩形,這樣做的好處是減少路徑規劃的計算量并保證真實機械臂運動軌跡同障礙保持一定余度的距離,而缺點是喪失了部分可用空間。機械臂運動學見第 2 節。
根據機械臂的幾何尺寸和各關節的實際工作區間繪制如圖 2 所示的機械臂工作范圍曲線。如果沒有實時顯示該工作范圍曲線,在排爆作業的前期,操作者操作移動平臺靠近爆炸物,由于 2D 攝像機沒有距離信息,操作員只能憑感覺操作,無法判斷爆炸物是否在機械臂的工作范圍內,只能伸出機械臂試探,一旦爆炸物超出范圍,又必須縮回機械臂,再操作移動平臺調整位置,再試探,如此反復,比較麻煩和費時。有了該曲線,機器人通過激光測距測出爆炸物距離,并和工作范圍曲線一同實時地顯示,爆炸物是否處于機械臂工作范圍就一目了然了。
2 激光測距
激光距離傳感器是基于 TOF(Time of Flight)原理的高精度距離傳感器。與圖像傳感器相比,它具有受光線影響小、處理方法簡單和數據精度高等優點。與移動機器人研究中廣泛使用的超聲波傳感器相比較,激光測距具有更高的距離和角度分辨率,而且其極短的采樣周期和較低的測量噪聲使其成為當前移動機器人測量的主要手段。
本系統激光傳感器采用 DIMATIX 公司的DLS-A15,其工作范圍為 0.2~200 m, 重復精度為1 mm, 絕對精絕對精度為 3 mm。該傳感器的 0.2 m盲區可以通過向后安裝來避免。它有 RS422 和 AD兩種通訊方式可選。
3 障礙環境和爆炸物建模
采用第2 節激光距離傳感器測量障礙物的方法,操作者操作機械臂在較遠距離掃描出整個與排爆操作相關的障礙環境。由于 SUPER-PLUS 不僅可以在關節空間操作,還能夠在笛卡兒空間操作,所以在距離障礙物和爆炸物較遠距離的前提下,操作該機器人很方便的,不必擔心機械臂會碰到它們。
由于激光測距傳感器的測量頻率受到限制,因此測量出的障礙物邊界是一些離散的點,如圖 4 所示。這些點用來近似地表示障礙物邊界,相鄰兩點之間的距離不能太大,否則機械臂或者部分機械臂可能穿過這兩點,導致碰撞發生。相鄰兩點的間距可通過更換更高測量頻率的測距傳感器和減小測量時機械臂的運動速度來調整,這能保證大多數點的間距在規定范圍內。由于障礙物的不規則性,障礙物表面與測距方向可能角度很小,從而導致兩點的距離很大。另外,由于障礙物的分布可能會突變,從而會使激光測距點從當前點跳到很遠的一個相鄰點(圖 4 所示的 P1 和 P2 兩點),也會使兩點距離很大。解決方法有:
(1)判斷兩點間距離,若大于某閾值(與機械臂寬度 d 相關,本文取 d/2),則將該線段離散,并將這些離散點插入測量障礙物邊界點序列。
(2)將簡化機械臂的矩形尺寸適當增加,犧牲可用空間保障安全性。
因為激光測量和模型誤差的存在,測量的障礙邊界與實際障礙邊界有些差別。接下來考慮爆炸物的建模。操作者操作機械臂使激光光斑大致落在爆炸物的表面中心上。沿該測量方向延長 e 得到 Obomb。如果 O3 與 Obomb 重疊,手爪長度 c 與 e 保持合適比例(e=0.8*c),另外機械臂不與障礙物相碰,機械臂就能安全的抓取爆炸物。因為在障礙環境的激光掃描過程中,爆炸物被視為障礙物,第三個條件無法滿足,機械臂無法抓取爆炸物,所以爆炸物必須被從障礙環境中“挖出”。
一個半徑為 R 的圓被用來“挖出”爆炸物。R與 max(d/2,c)應保持適當比例值,如果該值太小,手爪將無法伸入該挖出的區域,如果太大,部分障礙環境將被當作無碰空間。按照經驗:位于“挖出”圓內部的原實測障礙邊界變成無效,它被新生成的挖出的障礙邊界取代。
4 路徑規劃
現在我們可以開始解決空間映射的問題了??臻g映射的目的是得到自由位姿空間(Free C-space),現階段主要的方法有:針掃描法、點評價法、邊界方程法、體積掃描法、模板法、雅可比矩陣法等。前四種方法是解決低維度空間映射問題的常用算法,對于高自由度機械臂問題,由于循環檢測會耗去大量的時間和空間,可以考慮后兩種算法。在這里我們采用針掃描法。
由于機械臂上臂和前臂的地位并不相同,上臂旋轉限制可以制約前臂的旋轉限制,因此我們在位姿空間上按列(各個上臂旋轉角)討論,在固定的每一列,計算各個前臂旋轉角的碰撞情況。從上臂旋轉角度 0 度到 360 度,每一度作一次上臂碰撞檢測,如果通過(上臂沒有發生碰撞)還要作前臂的碰撞檢測。設位姿空間循環過程中作碰撞檢測的最大可能次數為 MAX,最小可能次數為 MIN,則 MAX=橫向離散點數*縱向離散點數 MIN=橫向離散點數 其中 MAX 在上臂每次碰撞檢測都通過時達到,MIN 在上臂每次碰撞檢測都不能通過時達到。
建立了自由位姿空間之后,現在的任務就是如何獲取一條初始點到目標點的無碰軌跡。這可以通過人工智能的啟發搜索來解決。A*算法[13]是一種典型的啟發式搜索算法,其原理是設計一個估價函數,由該函數可獲得各點的代價,每次搜索時,通過估價函數對下一步可能到達的點逐一進行評價,并取出代價最小的點繼續往下搜索,那些沒被選上的點就不會被繼續搜索了。
橫坐標代表上臂,縱坐標代表前臂。深色區域是碰撞空間,淺色區域是無碰自由空間。采用 A*算法能夠搜索出一條從給定初始點到目標點的無碰軌跡。
5 實驗結果
按照前面介紹的模型,建立了如圖 1 所示的排爆機器人系統。實驗平臺的 PC 為 IBM T42,其配置為 Pentium(R) 1.7G Hz, 1G RAM。機載 DSP 采用 TI TMS-LF2407。采用 0.25 度將 C 工作區間離散化,得到 840*680 的網格 C 空間。T42 與 DSP 采用USB 塊傳輸通訊,每 20 ms 互換 512B 數據。關節角度檢測采用 2000 線增量編碼器,并通過 QEP 電路四倍頻。
采用將機械臂簡化為矩形框,將障礙物簡化為離散的障礙物邊界點,這樣,機械臂與障礙環境的碰撞檢測就簡化為判斷這些離散的障礙邊界點是否在矩形框內,這將大大地簡化計算。同時對爆炸物的簡化模型可以方便快捷地將爆炸物從未知的障礙環境中分離出來。
在前述的硬件條件下,掃描未知障礙環境大約需要 30 s,建立 C 空間和并生成無碰軌跡大約需要50s,而自動抓取爆炸物則只需要 15 s 左右。實驗證明,本文建立的模型是簡潔可行的。
7 結論
針對排爆作業的非結構化環境、全手動控制導致的安全性低,操作者勞動緊張度高,效率低的缺點,提出了一種基于激光測距的排爆機器人模型,可以實現爆炸物的半自主抓取。本文提出的模型同樣適用其它多關節機械臂,為排爆操作的自動化提出了一個新的解決方案。
本文章轉自愛學術(aixueshu.com),如有侵權,請聯系刪除