南文虎，徐付民，葉伯生

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

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

1 機(jī)器人抓取系統(tǒng)運(yùn)動(dòng)學(xué)模型

1.1 鉛罐定位系統(tǒng)設(shè)計(jì)

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

其中，fx和fy為單位尺寸的像素?cái)?shù)；Cx和Cy為相機(jī)坐標(biāo)系與圖像坐標(biāo)系的偏移量；Ki為3 × 3 的相機(jī)內(nèi)參矩陣.

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

對(duì)式（3）進(jìn)行矩陣逆運(yùn)算，解出在已知相機(jī)坐標(biāo)值下像素點(diǎn)在世界坐標(biāo)系下的值，轉(zhuǎn)換關(guān)系為

由式（4）確定鉛罐中心位置點(diǎn)的像素坐標(biāo)，進(jìn)而求出該像素在世界坐標(biāo)系下的位置點(diǎn)，為機(jī)器人抓取放射源提供鉛罐定位基礎(chǔ).

1.2 機(jī)器人運(yùn)動(dòng)學(xué)模型建立

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

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

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

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

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

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

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

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

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

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

2.1 小概率抓取學(xué)習(xí)階段

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

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

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

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

2.2 大概率抓取操作

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

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

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

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

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

3）基于對(duì)過(guò)去采樣結(jié)果的推斷，評(píng)估是否空抓取.定義大概率重復(fù)抓取判斷函數(shù)為

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

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

3 仿真研究

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

3.1 排斥系數(shù)對(duì)抓取效率的影響試驗(yàn)

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

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

由圖5 可見(jiàn)，當(dāng)λ＜ 1.5 時(shí)，σ隨著λ的增加而減小；但當(dāng)λ＞1.5 時(shí)，抓取采樣次數(shù)隨λ值的增大而增大；當(dāng)λ＞2.0 時(shí)，算法不再收斂，因此可認(rèn)為λ= 1.5是估計(jì)極值點(diǎn).

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

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

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

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

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

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

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

結(jié) 語(yǔ)

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

此外，由于此類(lèi)黑盒子抓取問(wèn)題需要高質(zhì)量的觸覺(jué)傳感器，未來(lái)可引入類(lèi)似人手觸覺(jué)的傳感器，并結(jié)合強(qiáng)化學(xué)習(xí)技術(shù)，實(shí)現(xiàn)黑盒子空間的豐富特征探索，進(jìn)一步提高鉛罐內(nèi)放射源的抓取效率.