記憶推理的放射源抓取機器人運動規(guī)劃

南文虎，徐付民，葉伯生

1）蘭州理工大學機電工程學院，甘肅蘭州 730050；2）華中科技大學國家數(shù)控工程中心，湖北武漢 430074

目前在工業(yè)中進行礦石密度檢測時，從鉛罐內抓取放射源并進行分裝的過程仍主要是人工和半自動抓取.人工抓取危險性大，而半自動抓取時，因鉛罐是半封閉結構，遠程遙控抓取效率較低.非輻射環(huán)境下的抓取是機器人研究領域的熱點之一，余玉琴等［1-2］提出基于模型的抓取策略；何濤［3］提出基于半模型和無模型的抓取策略.但由于難以估算現(xiàn)實世界物體的形狀，基于模型的方法很難應用到實際抓取環(huán)境中，深度學習算法則為無模型抓取策略提供了廣泛前景.當前基于深度學習的機器人抓取策略主要有端對端策略［4］與采樣評估策略［5］兩類.周祺杰等［6］針對固體放射性廢物分揀作業(yè)，使用Q網(wǎng)絡算法來訓練抓取.薛騰等［7］結合視覺與力覺信息構建數(shù)據(jù)集訓練抓取.崔少偉等［8-9］提出基于觸覺先驗知識的機器人穩(wěn)定抓取方法.FALLAHINIA等［10］利用指甲成像技術對多個手指進行無約束的抓取力測量，研究人類的抓取行為.張磊等［11］采用預抓取技術對機器人所在環(huán)境的地圖信息進行預抓取，再使用自適應樣本的蒙特卡羅定位方法對機器人進行定位.本研究以腕力傳感器為力覺反饋裝置實現(xiàn)機器人與環(huán)境的交互，針對目前抓取鉛罐內放射源顆粒的工程背景，設計基于記憶推理決策的強化學習抓取方法，每次抓取前先比對以前的抓取情況再規(guī)劃當前抓取路徑，以免造成動作浪費，從而實現(xiàn)鉛罐內放射源顆粒的高效的自主抓取.

1 機器人抓取系統(tǒng)運動學模型

1.1 鉛罐定位系統(tǒng)設計

從相機坐標系轉換到圖像坐標系是將3維點轉換到2維點.假設目標物體在相機坐標系的坐標矩陣PC=[XC，YC，ZC]T，在圖像坐標系上的投影坐標矩陣PP=[u，v，1]T，根據(jù)相似三角形法，得到相機坐標系到圖像坐標系的轉換關系為

其中，fx和fy為單位尺寸的像素數(shù)；Cx和Cy為相機坐標系與圖像坐標系的偏移量；Ki為3 × 3 的相機內參矩陣.

式（1）確定了像機坐標系到像素坐標系的轉換關系.裝有放射源的鉛罐放置在世界坐標系中，因此，定位鉛罐時需先將世界坐標系轉換到相機坐標系中.令鉛罐在世界坐標系的坐標矩陣PW=[XW，YW，ZW]T，則轉換到相機坐標系為

對式（3）進行矩陣逆運算，解出在已知相機坐標值下像素點在世界坐標系下的值，轉換關系為

由式（4）確定鉛罐中心位置點的像素坐標，進而求出該像素在世界坐標系下的位置點，為機器人抓取放射源提供鉛罐定位基礎.

1.2 機器人運動學模型建立

本研究以5 自由度串聯(lián)機器人、攝像頭、6 維力傳感器、鉛罐和分裝器搭建機器人抓取系統(tǒng)的仿真模型.結合圓柱形的放射源形狀，設計了圓柱三爪型放射源爪手，并在爪手和腕部的連接部位安裝6 維力傳感器，用于機器人與鉛罐的環(huán)境交互檢測.攝像頭固定在距抓取臺800 mm 高的支架上，視角向下.放射源抓取機器人系統(tǒng)三維模型如圖1，機器人運動學Denavit-Hartenberg（D-H）參數(shù)如表1.其中，li-1為連桿長度；αi-1為連桿扭角；di為連桿偏距；θi為對應連桿的轉角；i為圖1所示機器人的連桿編號，從基座連桿1到腕部共5個連桿.

圖1 放射源抓取機器人系統(tǒng)三維模型Fig.1 Three-dimensional model of radiation source grasping robot system

2 基于記憶推理決策的抓取方法

鉛罐是半封閉結構，這令攝像機難以對鉛罐內的放射源成像.抓取是一種模糊探索過程，建立機器人抓取過程的馬爾科夫決策過程（Markov decision process，MDP），記為 MDP ={S，A，P，R，γ}.其中，抓取位置點狀態(tài)為S=[x，y]；動作空間為爪手移動量A=[Vx，Vy]；P為從當前狀態(tài)轉移到下一個狀態(tài)的概率；R為抓取回報值，γ為折扣因子.圖2 為R的狀態(tài)示意圖.其中，虛線圓圈為爪手內部輪廓；實線圓圈為放射源外部輪廓.采用歸一化尺寸，令放射源直徑為1，則爪手的歸一化直徑為爪手直徑與放射源直徑的比值.采用此假設，根據(jù)爪手與放射源的位置關系，將R分為以下3種情況：

圖2 抓取回報值狀態(tài)示意Fig.2 The reward value status diagram of grasping

1）包含(R= 1)：抓取位置剛好在放射源的抓取包絡體內.通過爪手傳感器判斷為抓取成功.

2）交叉(R=-1)：在實際作業(yè)中，通過機器人爪手高度及底部碰撞檢測力，測得抓取位置部分與放射源位置發(fā)生交集，此情況判斷為抓取失敗.但是，此區(qū)域內抓取成功的概率較大，該位置的抓取數(shù)據(jù)S可為后續(xù)抓取提供參考.

3）空采樣(R= 0)：實際作業(yè)中，通過分析機器人爪手高度及爪手與鉛罐底部碰撞力，檢測抓取過程中機器人爪手位置是否與鉛罐底部發(fā)生接觸碰撞.若發(fā)生接觸碰撞，則視為抓取失敗，說明此區(qū)域內無放射源，即此區(qū)域抓取成功概率為0，同時將此位置相關數(shù)據(jù)存儲到歷史數(shù)據(jù)庫中，避免下次進入其鄰域，造成重復抓取.

本研究提出基于歷史數(shù)據(jù)記憶推理學習的抓取方式，每次抓取前先比對以前的抓取情況，若有重復，則取消此次動作并重新規(guī)劃，以免造成動作浪費，流程如圖3.其中，n是小概率抓取個數(shù)；m是大概率抓取個數(shù)；N是總的放射源個數(shù).抓取分為兩階段：①小概率抓取階段判斷放射源在鉛罐中的粗略位置，并存儲記憶數(shù)據(jù)庫，為第2階段抓取做準備；②大概率抓取階段則是基于第1階段的粗定位，高效完成局部抓取操作任務.

圖3 基于記憶推理的抓取流程流程圖Fig.3 The grasping flow diagram based on memory reasoning

2.1 小概率抓取學習階段

由機器人圖像定位系統(tǒng)，找到鉛罐底部的中心位置，記為P0=[x0y0].根據(jù)蒙特卡羅原則，定義均勻抓取采樣函數(shù)為

其中，r為采樣點的極半徑，r=rd× rand(1)，rd為采樣半徑；α為采樣點的極角，α= 2π × rand(1)，函數(shù)rand（1）生成0 ～1的隨機數(shù).小概率抓取階段旨在探索放射源的大概位置，每次探索完畢會記錄當次抓取的狀態(tài)及回報值，并構造狀態(tài)集S={S1，S2，…，Sn}，然后通過式（8）的小概率重復抓取檢測函數(shù)，實現(xiàn)最優(yōu)抓取策略的選擇.

其中，λ為小概率抓取的排斥系數(shù).若φ(Sn) ＞ 0，說明第n次采樣是重復采樣，無需進行實際抓取，只需重新決策規(guī)劃；若φ(Sn)= 0，表示第n次采樣非重復采樣，可進行實際抓取.每次實際抓取采樣前，都要計算φ(Sn)，以達到學習歷史數(shù)據(jù)，探索下一次抓取空間的目的.

小概率抓取算法的程序代碼請掃描論文末頁右下角二維碼見補充材料圖S1 和圖S2.首先，在抓取操作前，建立機器人抓取操作的環(huán)境模型.然后，進行抓取決策規(guī)劃，若φ(Sn)= 0，則表示采樣成功，先將狀態(tài)抓取Sn和對應的回報值分別存入小概率抓取數(shù)據(jù)庫H和抓取狀況歷史數(shù)據(jù)庫K中，然后進行實際抓取.循環(huán)采樣直到抓取成功個數(shù)與交叉抓取個數(shù)的和等于總放射源數(shù)N時，表示小概率采樣抓取成功.若經過指定步數(shù)后，抓取成功個數(shù)與交叉抓取個數(shù)的和小于N，則表示抓取失敗，需重新抓取.

2.2 大概率抓取操作

小概率抓取雖然全部抓取成功的可能性很小，但得到放射源鄰近位置的概率很大，且能記憶歷史操作.在大概率抓取作業(yè)階段，機器人通過查詢記憶庫中的抓取情況來完成抓取任務，抓取過程偽代碼請掃描論文末頁右下角二維碼查看補充材料圖S3和圖S4，算法步驟為：

1）初始化已抓取數(shù)據(jù)庫h= ?，判斷2.1節(jié)的抓取狀態(tài)歷史數(shù)據(jù)庫K的第i行，若K(i，：) = 1，表示機器人已經抓取了該位置的放射源.

2） 判斷K(i，：) =-1 時，若此時h= ?，則用均勻抓取采樣函數(shù)locate（H（i，：））進行采樣，獲得新的抓取位置坐標，并存入h；若h≠?，則采用中心移動采樣函數(shù)進行采樣.中心移動采樣函數(shù)為

其中，g= sum(([h；H(i，：)]，1)/size(h，1) + 1)，g為新采樣中心的位置矩陣，函數(shù)sum（［h；H（i，：）］，1）表示對矩陣按行求和，函數(shù)size（h，1）求得h的行數(shù).中心移動式采樣過程如圖4.首先，機器人圍繞放射源進行隨機采樣，得到第1 次采樣結果S1.隨后，采用中心偏移法將采樣中心移至g1，在以g1為中心的圓C1內進行第2 次采樣，得到采樣結果S2.再次將采樣中心偏移到g2位置，對以g2為中心的C2區(qū)域內以同樣方法進行第3次采樣.這樣，每次采樣都向放射源的方向移動，采樣成功的概率逐漸加大，直至最后抓到放射源.

圖4 中心偏移采樣示意Fig.4 The schematic diagram of center offset sampling

3）基于對過去采樣結果的推斷，評估是否空抓取.定義大概率重復抓取判斷函數(shù)為

其中，δ為大概率抓取的排斥系數(shù).若φ(h，S) ＞ 0，說明發(fā)生了重復采樣，需重新采樣.每次采樣都要進行式（10）計算，以避免抓取歷史空位置.

4）循環(huán)步驟1）—3），若在規(guī)定時間內抓取到所有放射源，則停止抓??；否則，重新抓取.

3 仿真研究

由于鉛罐的半封閉性和強輻射性，機器視覺難以應用于鉛罐內部放射源的定位，因此分別采用基于記憶推理決策的強化學習抓取方法和蒙特卡洛隨機采樣抓取方法［11］，對不同數(shù)量的放射源進行抓取仿真實驗，以驗證本研究方法的有效性.實驗基于機器人操作系統(tǒng)（robot operating system，ROS）及GAZEBO 仿真器插件，規(guī)劃算法通過上層C++程序實現(xiàn)，運動執(zhí)行與控制由ros_control 軟件包實現(xiàn)，采用ROS Moveit軟件進行仿真.

3.1 排斥系數(shù)對抓取效率的影響試驗

在基于記憶推理決策的強化學習算法中，無論是小概率抓取排斥系數(shù)還是大概率抓取排斥系數(shù)都對學習效率有較大影響.圖5給出了放射源數(shù)N分別為5、10 和12 個的情況下，不同λ值時執(zhí)行500次抓取任務后平均抓取采樣次數(shù).

圖5 排斥系數(shù)對抓取采樣次數(shù)的影響Fig.5 The influence of repulsion coefficient λ on sample grasping times

由圖5 可見，當λ＜ 1.5 時，σ隨著λ的增加而減??；但當λ＞1.5 時，抓取采樣次數(shù)隨λ值的增大而增大；當λ＞2.0 時，算法不再收斂，因此可認為λ= 1.5是估計極值點.

不同δ值對學習效率有較大影響.圖6 為N= 5、10 和12 時，不同δ值下采用基于記憶推理決策的強化學習抓取方法抓取500次后σ的變化曲線.由圖6 可見，隨著δ值的增加，所需采樣次數(shù)減小，但當δ＞2.5時，算法不再收斂.

圖6 排斥系數(shù)δ對抓取采樣次數(shù)的影響Fig.6 The influence of repulsion coefficient δ on sample grasping times

設N= 6，進行500 次抓取試驗，分析平均抓取次數(shù)與λ和δ值的關系，結果如圖7.由圖7 可見，相比小概率抓取排斥系數(shù)λ，大概率抓取排斥系數(shù)δ值對平均抓取次數(shù)σ的影響更大，隨著δ值的增加，σ逐漸減小，但δ= 2.5是極值點，超過會導致算法不收斂.綜合圖5至圖7可見，當λ= 1.5且δ= 2.5時，算法收斂性最好.

圖7 排斥系數(shù)分布Fig.7 The distribution diagram of repulsive coefficient

3.2 不同環(huán)境下抓取試驗

分別采用蒙特卡羅隨機采樣算法（以下簡稱采樣法）和本研究提出的基于記憶推理決策的強化學習方法，對不同數(shù)量的放射源進行抓取試驗，每種方法重復500 次，記錄兩種算法的平均抓取次數(shù)，并計算本研究方法相對蒙特卡羅采樣法的抓取次數(shù)減少率（r），結果如表2.由表2可見，對應不同放射源個數(shù)，基于記憶推理決策的強化學習方法的平均抓取次數(shù)都少于蒙特卡羅采樣法，這是由于前者在每次抓取后，都能利用歷史數(shù)據(jù)進行推理，使下次決策更有效.該方法類似人類在封閉環(huán)境下的抓取活動，每次抓取的歷史過程，都是學習和探索的過程.當N= 9 時，本研究方法的抓取次數(shù)比蒙特卡羅采樣法減少了77.33%；當N= 1時，本研究方法的抓取效率比蒙特卡羅采樣法提高了89.85%，綜合抓取效率平均提高了84.67%，實驗說明所提能高效地解決鉛罐特殊工況下放射源的自主抓取問題.

表2 兩種方法不同放射源數(shù)量下500次抓取試驗的平均抓取次數(shù)Table 2 The average number of 500 grasping tests under different number of radioactive sources with two methods

結 語

設計了放射源容器及分裝容器的自主定位系統(tǒng)，通過6維力傳感器的反饋實現(xiàn)機器人和鉛罐環(huán)境的交互，提出基于記憶推理的強化學習策略，實現(xiàn)機器人抓取模式的自主記憶學習.該抓取方法比蒙特卡羅隨機采樣法，平均抓取效率提高了84.67%，避免了因長時間抓取試探造成的機器人結構疲勞損傷和能量消耗.該系統(tǒng)穩(wěn)定性高，對此類抓取問題的泛化性更好.

此外，由于此類黑盒子抓取問題需要高質量的觸覺傳感器，未來可引入類似人手觸覺的傳感器，并結合強化學習技術，實現(xiàn)黑盒子空間的豐富特征探索，進一步提高鉛罐內放射源的抓取效率.