SP-POMDP：堆疊物體抓取場景中的任務規劃方法

關鍵詞：機器人抓取；堆疊場景；POMDP；任務規劃；狀態空間修正中圖分類號：TP391 文獻標志碼：A 文章編號：1001-3695（2025）07-019-2064-08doi：10.19734/j. issn.1001-3695.2024.11.0495

Abstract：Inthe workingscenarioof robots grasping stackedobjects，due tosensorsampling afectedbyclutter andpartial observabilitycaused byobjectocclusion intheenvironment，robotsareunable toachieveaccurateand complete modeling， making itdificulttoeficientlycompletetasks.ThispaperdesignedastatepatchedbasedpartiallyobservableMarkovdecision proceses （SP-POMDP）model to addressthe above isues，and proposed arobot grasping task planning method based on this model.This methodabstractly extracted discrete states，actions，andobservation spaces basedonsamplingresults.Through a statespacecoectionmethod，stateinformationthatcouldn’tbesampledandrecognizedduetopartialobservabilityintheenvironment wasadded tothestate spacebasedonthe inherentcharacteristicsofthecurent stackedscene.Itconstructedabelief treetosolvethemodel.Theresultsofexperimentsshowthatinthegraspingtaskofstackedobjects，thismethodcansignificantly reduce computation time and improve work eficiency while ensuring success rate.

Key words：robot grasping；stacking scene；POMDP；task planning；state space patching

0 引言

在雜亂的物體堆疊場景中搜索并抓取特定物體是工業場景中的場景任務。上世紀末以來，機器人被廣泛應用在工業中，用于組裝、拋光和噴漆等任務[1]。在經典的工業環境中，往往通過對任務進行精確的、完整的建模來實現任務的合理規劃，為此機器人必須根據有限的傳感器信息推理其動作的可能結果。然而，在未知的、非結構化的以及雜亂無章的環境中，或在使用干擾較大的傳感器、與人類協作或在具有難以建模的目標物體的任務中[2]，系統的不確定性和部分可觀測性被放大，難以直接通過控制系統的建模來處理。物體的相互遮擋不僅使得系統無法對環境完全觀測，還導致了目標物體先驗屬性的不確定。此時如何處理系統中的不確定性與部分可觀測性，是具有難度且至關重要的問題。

對于機器人抓取過程中的這類問題，目前的研究主要有兩個方向。一個是優化對環境信息的采樣方式，通過先進的機器學習方法[3-6]，從有限的傳感器信息中提取出盡可能完整的環境信息。例如，薛騰等人[4]針對機器人抓取過程中需要實時評估抓取質量以動態調整抓取構型的問題，提出了一種基于觸覺先驗知識的機器人穩定抓取方法。該方法通過融合視覺圖像和觸覺先驗知識，生成穩定的抓取構型，極大提高了穩定抓取的成功率。針對工業上常見的散亂堆疊零件的抓取問題，徐進等人[3]提出一種基于抓取簇和碰撞體素的抓取姿態檢測算法，解決了傳統方法中因采用離散固定抓取點而導致可抓取點丟失、篩選效率低的問題。值得一提的是，在雜亂環境中抓取特定目標的機器人抓取任務，目前已被推廣到了服務型機器人的研究上。在該問題上，Duan等人[5]提出了一種通過多任務卷積神經網絡的模塊化智能機器人架構，包括一種端到端的語義抓取卷積神經網絡，以及一種對多模態信息進行簡單推理的后處理方法，在相對復雜的環境中具有較好的適應性與魯棒性表現。這類方法均能夠在特定目標與場景中表現出較好的性能，但由于依賴大量的樣本數據進行離線學習，遷移性較弱。而且受限于環境采樣條件，在一些相對惡劣的工況中存在一定挑戰。

另一個方向是通過調整機器人的抓取策略來減少感知不確定性，在動態的過程中完成任務。相較于前一個方向，該策略更符合直覺，同時由于對環境采樣的要求相對寬松，能夠處理絕大多數復雜場景的任務。該策略的難點在于如何對問題任務進行建模。李鑫等人[基于場景建立馬爾可夫決策過程（Markovdecisionprocess，MDP），使用深度強化學習來解決密集多物體場景中的機械臂抓取問題。然而如果存在物體遮擋這類系統雜波，則MDP模型將無法確定環境物體的狀態。針對該問題所提出的部分可觀測馬爾可夫決策過程（partiallyobservableMarkovdecisionprocess，POMDP）是在不確定性下進行決策的事實模型，已被廣泛應用在機器人抓取任務規劃的研究中。

POMDP早期多被用于解決單個物體的狀態獲取問題。例如在基于觸覺傳感器的機器人抓取場景中，Hsiao等人[8在POMDP的基礎上使用了一種環境模型，將機器人的運動限制在一個離散的動作空間集合中，并劃分狀態空間為離散單元，以處理自然連續的動作和狀態空間。后續通過從候選集合中選擇軌跡的方法，將其擴展到更大規模的問題中[9]。而在多物體的操作問題中，POMDP需要考慮由于傳感器噪聲以及物體之間相互遮擋而導致的部分可觀測性，主要挑戰在于對象數量增加導致的狀態空間的指數增長。圖像渲染或物理模擬等技術可用于觀測和場景動力學的建模，但要找到適合POMDP形式，同時在實踐中保持計算可處理性[1]，是一個挑戰。對此，Mons6等人2根據實際任務中對象的固有物理屬性，通過離散值來獲取狀態、動作和觀測的近似值，成功解決了布料分離的問題。

現在研究會將環境中觀測到的每個物體對象轉換為單獨的狀態變量來處理。例如，Pajarinen等人[1]考慮了一個用RGB-D攝像頭觀測桌面上的若干個物體的機器人工作場景。每個狀態變量表現為對象的屬性，例如顏色或位置。動作為移動對象的操作，觀測包括被移動對象后面的對象屬性的信息。將RGB-D場景分割成對象，來獲取相互遮擋的程度。利用這些遮擋信息估計POMDP求解的抓取成功概率和觀測概率。Li等人[11]也使用了類似的方法，基于離散化的狀態來解決物體的搜索與轉移問題。Xiao等人[12]在此基礎上解決了使用POMDP進行對象搜索的問題，同時考慮了對象完全遮擋的情況。此外，Pajarinen等人[13.14]將環境的不確定性融合到POMDP的信念狀態中，允許對對象模擬進行操縱動作的規劃，以應對隨機分布的未知物體造成的不確定性。

在現有研究中，對場景的實際狀態空間的獲取均建立在采樣方式穩定可靠的基礎上，即通過攝像機能獲取所有存在于視野中的物體信息。而這在實際工況中，受復雜的光照、溫度等條件影響，該條件可能無法滿足。而通過遮擋程度來估計位置物體的狀態屬性，是在采樣受阻的情況下進行模型中部分參數估計的可靠思路[10＼～14]。受 Zhao 等人[15]對 POMDP 的抽象提取方法的啟發，本文設計了一種基于狀態修正的部分可觀測馬爾可夫決策過程，并基于該模型，提出了一種針對堆疊物體抓取場景的機器人抓取任務規劃方法：SP-POMDP。與傳統方法相比，該方法在考慮系統部分可觀測性的基礎上，引入了一種狀態空間修正方法，將環境中無法觀測的物體狀態抽象地添加到系統的狀態空間中，一定程度上削弱了系統的部分可觀測性。并通過一種部分可觀測的蒙特卡羅規劃（partiallyobservableMonteCarloplanning，POMCP）算法的變式，實現所提模型的求解。經過仿真實驗，與傳統方法相比，該方法在堆疊物體的抓取任務中表現出極佳的性能，具有一定的優越性。

1 SP-POMDP構建方法

1.1 POMDP概述

在有限時域中，一個POMDP問題可表示為一個包含8個元素的元組 。若無特別說明，本文中字母下標表示該元素位于所在集合中的序號，上標表示該元素的屬性，且對應符號的含義在全文保持一致。其中： 分別表示系統的狀態空間、動作空間以及觀測空間； T，O，R 分別表示狀態轉移函數、概率觀測函數以及回報函數； 0lt;γ?1 是衡量即時回報與未來回報的折算系數，一般取0.95以上。 b₀ 為初始信念狀態，以一個初始概率質量函數來表示，如 b₀（s） 表示初始狀態為 s 時的概率。

在任務過程中，機器人在系統狀態為 s∈S 的情況下執行動作 a∈A ，存在概率 T（s^′，s，a）=P（s^′∣s，a） 使得系統狀態由 s 變為 s^′ ，此時得到即時回報 r=R（s，a） ，進一步地，存在概率O（ω^′，s^′，a）=P（ω^′|s^′，a） 獲得一個對改變后系統的觀測結果

由于觀測的不確定性，即部分可觀測性，機器人無法保證獲取的系統狀態為真實狀態，故引人一個信念函數 b（s） 來表示任意時刻系統狀態為 s 的概率，記信念狀態的集合為信念空間 B ，即 b∈B 。任何一個時間點的信念狀態被定義為給定過去行動和觀察歷史的狀態的條件概率分布。如上所述，在采取任何行動或感知任何觀察之前，初始信念狀態均為 b₀ 。

在給定當前信念狀態 b 、動作 a 以及觀測結果 ω^′ 后，信念狀態可由貝葉斯規則進行更新：

其中： 為觀測 ω^′ 的先驗概率。式（1）可簡寫為貝葉斯濾波器 ω'）[16]

記POMDP 問題的解為策略序列 π=（π₁，π₂，…，π_h） ，策略 π_t 為信念狀態 b 到動作 a 的映射，即 a=π_t（b） ，其中， t∈ [1，h] 。最優價值函數 為當 χ_t 策略執行時，系統從信念狀態 b 執行一個最優策略 π^* 獲得的期望總回報。

由于該問題滿足最優化原則，即所選擇的最優策略保證其后部任意子策略是最優的，故根據貝爾曼方程[17]可得

由于最優價值函數 沒有直接體現信念狀態 b 到動作 Ψ_a 的映射關系，為描述最優策略 π^* ，需要引入最優動作-價值函數 Q_t^*（b，a） 。其定義為當 χ_t 策略執行時，系統從信念狀態b 執行動作 a 的期望總回報，由式（2）可得

最優策略 ，理論上 π_t^*（b） 唯一。原始的POMDP考慮的系統狀態為連續空間，對于計算的負載過大。從中提取出一個離散的POMDP，降低模型的規模，以提高效率是常見的思路[8]。

1.2 SP-POMDP構建

SP-POMDP表示為 ，從一個原始的POMDP模型中提取獲得，后續稱該過程為抽象化或抽象提取。

對原始屬性 的處理，可以理解為將原始POMDP中的該屬性進行離散化。以狀態空間 s 為例，將任意時刻的原始狀態空間 s^cur 劃分為若干個互斥子集 s_i^cur 。任意一個子集 s_i^cur 對應一個抽象提取 ，且對于任意 s_r^cur∈s_i^cur ，對應相同的抽象提取 。所有抽象提取 的集合構成該時刻的抽象狀態 。此過程可定義為如下所示抽象提取函數 F_s 。

在任務場景固定的情況下，動作空間 A 在任意時刻均不變，其元素 a_i 表示任意動作，故可定義如下所示抽象提取函數 F_a 。

觀測空間 通過概率觀測獲取，由于動作與狀態已經轉化為離散的形式，故可直接輸出離散的觀測 。

在機器人抓取任務中，由于動作 a 的選擇由策略 π 確定，存在一定的偏向性。狀態 s^′ 受到動作 a 影響，也無法保證其均勻分布，故無法利用貝葉斯公式直接從原始POMDP中的狀態轉移函數 T 以及概率觀測函數 o 中提取出 與 由于對原始屬性的抽象提取使問題變為離散的情況，需將 與 轉換為離散化后的所有 引發的獨立事件的變化規則。

的設計不依賴于 R ，需要保證任意時刻的即時回報優于下一時刻，使POMDP的求解收斂。即對于任意時刻 χ_t ，需滿足 ，具體的 根據實際問題確定。信念狀態為系統狀態的映射，故可直接將式（4）代人，得

在實際工作場景中，可能存在因被遮擋而無法被觀測到狀態的物體。由于SP-POMDP的屬性在提取過程中被離散化，位置狀態之間的差異被縮小，此時可將被物體 c 遮擋而無法被識別的物體視為物體 c 在狀態空間上的疊加，其位置狀態相同，但存在一個獨立的疊加計數 n_l ，反映該狀態被疊加的次數，用于區分各個物體。

基于以上原理，可以將抽象化后的狀態空間進行補全。記抽象化后的初始狀態空間為 ，抽象提取后可能遮擋未識別物體的 n 個外層物體 C_i 的狀態為 ，補全后的狀態空間為 ，定義如式（7）所示狀態空間修正函數 f

對于獲得的抽象化后的初始狀態空間 ，利用狀態空間修正函數可添加那些無法通過采樣設備識別，但可推斷其存在的物體狀態到 中，減小部分可觀測性對系統的影響。不同的場景中疊加計數 n_l 的獲取規則由場景中物體性質確定。

2場景建模

2.1 任務描述

為了進一步描述本文方法的原理，設計如下任務場景。若干物體隨機堆疊擺放在桌面上，機器人可將物體從桌面上拾取并放在指定區域。根據機械臂及桌面、物體的尺寸設置場景中各物體的相對位置，在確保不會發生碰撞的前提下，使機械臂的動作空間能夠覆蓋桌面。在桌面附近設置一個區域，要求機器人將指定物體拾取并移動到目標區域，桌面尺寸如圖1所示。其中桌面長 len=400mm ，寬 wid=300mm 。同時設置動作閾值，當執行動作數達到閾值時，無論任務是否完成，機器人均終止任務[15]。設待抓取物體為直徑 d=80mm 高 h= 10mm 的圓盤。

機器人通過感知觀測來獲取系統中物體的狀態分布，根據垂直方向上的物體的遮擋率來判斷物體是否可被觀測。在原始POMDP中，經過初始采樣，可獲得所有物體的狀態空間，此時存在三種情況。

a）若遮擋率較低，則物體可被觀測并識別其類型；

b）若遮擋率較高，由于環境的部分可觀測性，物體的特征無法被完全觀測，其類型無法確定，視為無法識別，需要在移動其上方物體后，對先驗信念再次觀測才能判斷其類型；

c）若遮擋率極高，考慮到實際場景中采樣設備的硬件限制及環境干擾，視為無法觀測。

由于SP-POMDP利用狀態空間修正函數 f 對初始狀態空間的觀測進行了補全，根據觀測到的物體狀態推理出其是否遮擋物體，并將被遮擋的物體作為上方物體的疊加進行補全，以此視為觀測到物體。故情況c）被消除，僅需考慮物體類型能否被識別，通過一個識別概率來表征物體為目標類型的可能性。識別概率函數 P₀ 如式（8）所示，表示物體類型被識別的概率，其中 s_r 表示物體的遮擋率， r_s 表示設定的可被識別的閥值。 r_s 的取值反映的是具體任務中的遮擋對觀測物體類型的影響程度，該任務中的取值在0.7以上。

2.2 構建SP-POMDP

2.2.1 實例化屬性

在實際場景中，需要將任意時刻中物體的各項屬性實例化。為保證每個物體均落在唯一的單元格中，且每個單元格在同一水平面內僅容納一個物體，故單元格的邊長應在 [d 1.7d）。由此將該場景分割成如圖1中的 3×4 的12個離散單元。每個單元格表示為 ，邊長為 100mm ，目標區域的位置為（0，-1），共13個單元格，所有可放置物體的單元集合表示為field。記 OBJ={o_i|i∈[1，n]} 為所有物體組成的集合，其中 n 為物體數量，物體的編號 i 在采樣階段根據采樣順序確定。

系統任意時刻的原始狀態可直接表示為 s={l_i|i∈[1 n]} ，其中 l_i=（x_i，y_i，z_i） 表示物體 o_i 的笛卡爾坐標。當使用單元格而不是笛卡爾坐標來表示物體的位置時，無法反映對象之間的遮擋和位置關系，因此需要將這些信息顯式包含在抽象狀態中。在SP-POMDP中，任意時刻的系統狀態表示為 ^T，E） 。其中 ΨC=[c₁c₂…c_n]^T 為物體的位置狀態組成的 n 階向量，其元素 表示物體 o_i 所在的中心距離最近的單元格。 T=[?bτ₁τ₂…τ_n]^Γ 為物體的識別概率組成的 n 階向量，其元素 τ_i∈[0，1] 表示物體 o_i 的類型能夠被識別的概率，由式（8）直接求得。 E=[ε₁ε₂…E_n]^T 為表示物體遮擋關系的 n 階向量，其元素 ，其中 z_iup 表示 o_i 是否被物體遮擋， z_idown 表示 σ_oi 是否遮擋物體，若是，則對應的元素為1，否則為0。

系統原始動作表示為 a=（o_i，l） ，指將物體 o_i 移動到空間位置 ξ_l 的操作，動作空間的規模由采樣分辨率決定。在SPPOMDP中，完整動作空間表示為 13]}，其元素 表示將物體 o_i 從當前位置移動到單元格 c_j 的動作。由于物體之間相互遮擋，不能保證 OBJ 內的物體 o_i 均能被抓取，故在實際選擇動作時，以待抓取物體所在單元格為動作的起始元素，即 o_i=c_i 。

系統任意時刻的原始觀測表示為 ω={（t_i，l_i）∣i∈[1 m]}，其中 m 為識別到的物體數 ，t_i 表示物體的估計類型， l_i 表示物體的估計位置。與狀態空間類似，抽象化需要顯式表示出物體間的遮擋和位置關系。在SP-POMDP中，任意時刻的系統觀測表示為 。其中 為被識別的物體的估計位置組成的 m 階向量。 為物體被識別到的類型組成的 m 階向量，其元素 表示識別到的物體的類型，由于該場景任務中給定了目標物體的類型，若觀測到物體為目標類型的物體，則取 ，若觀測到物體不是目標類型，則 ，部分物體由于被遮擋，其屬性由狀態空間修正函數f補全，無法直接觀測到類型，其 的值為0＼～1的一個浮動數，在求解過程中更新。 為表示物體遮擋關系的 ?_m 階向量，其具體表示形式與 中的對應元素相同。由于狀態空間修正函數 f 補全了所有物體的位置狀態，故 m=n 。可以證明， 且 ，故 $＼hat { ＼omega } = （ C ， ＼overbar { T } ， E ）$ 。

抽象化前后模型的各屬性如表1所示。

2.2.2狀態轉移函數與概率觀測函數

根據實例化的SP-POMDP屬性，將處于相同單元格 cell_k 內的物體儲存在同一個順序棧中，可以利用如圖2所示的數據結構來維護OBJ，以表征場景中各物體狀態。

構建階段進行狀態空間的遍歷，在進行狀態轉移時，可實現常數級時間復雜度的運算，同時有效解決了當前研究中對于物體是否可進行抓取操作的判斷問題[12.15]，優化了計算的效率。

概率觀測函數 描述了系統環境在執行動作 后，狀態 到觀測 的映射規則。由于 ，故只需討論 T 到 的轉換方法。由2.2.1節中給出的狀態空間與觀測空間的表示形式可以發現，不同于狀態，觀測將物體的類型信息由識別概率轉換為一個較為模糊的預測（可理解為物體的期望類型），以下分為兩種情況討論。a）若采樣時未檢測到目標類型的物體，則系統空間的 應與 T 負相關。如最上方的物體的識別概率為1，但其不是目標物體，識別類型為0，則目標物體存在于被完全遮擋或由狀態空間修正函數補全的物體中，其識別類型應大于 若采樣時檢測到目標類型的物體，則系統空間的 與 T 無關，且對應物體的識別類型為1。

根據以上分析，可以歸納并設計出如下概率觀測函數 。其中 P_n 表示目標物體存在于無法被觀測的物體中的概率，在該場景中均勻分布，故其數值等于向量 T 中值為0的元素個數的倒數： _;I 為所有元素均為1的 n 階向量； e 表示是否檢測到目標類型的標識符，若檢測到取1，否則取 0;T_a 表示目標類型物體的識別類型向量。 e 與 T_a 在采樣階段確定。

此時被完全遮擋或由狀態空間修正函數補全物體的識別類型為 P_n ，被部分遮擋的物體的識別類型為小于 P_n 的值。

當上方物體被轉移時，下方物體不再被遮擋，理論上可直接確定其類型，即識別類型必須為0或1，此時智能體會以 的概率隨機獲取0或1。

2.2.3回報函數與信念狀態更新

狀態轉移函數 描述了系統環境在執行動作 后，狀態 的變化規則。進行狀態轉移時，智能體會從輸入的動作 中提取該動作移動物體的起點與終點的單元格。對于狀態屬性， c 可直接修改被移動物體的對應元素， T 則需要根據被移動物體的遮擋關系 E ，更新其后方物體的識別概率 τ_j+1oE 的更新相對復雜，具體方法為判斷被移動的物體是否遮擋其他物體，更新當前元素以及下一個元素的 ε 值。 的一般形式如式（9）所示，其中 為執行動作 來實現狀態轉移的線性變換 Δ，j 為 c 中被轉移的元素下標。

以上方法在每次進行狀態轉移時，均需要構造特定的線性變換 T_a ，且需要遍歷分析當前狀態 ，其性能受狀態空間規模影響較大，在進行連續的狀態轉移時尤為明顯。現基于圖2的數據結構，構造如圖3的方法來實現 該方法僅在數據結構的

回報函數 需滿足設計條件。這里取任意抽象動作 執行后產生的運動代價 。此外，在抓取任務完成后，產生一個最終的獎勵 ，隨后任務終止。

該模型以信念狀態來表征系統所處的狀態，在該任務場景中表現為每個單元格內存在目標物體的概率。SP-POMDP中，觀測空間 包含了物體的類型信息，式（10）將系統內物體的識別類型以概率數值的形式來表示，將所有物體的識別類型映射到所在單元格。根據貝葉斯規則，可將每個單元格內所有物體的識別類型之和作為該單元格信念狀態的更新權重，來實現信念狀態的更新。由此給出以下信念更新方程，其中 b_cellk 表示單元格 cell_k 的信念值。

由式（11）獲得每個單元格內的信念權重的集合，對其進行歸一化后獲得更新后的信念狀態 b^′ 。初次提取時 b_cellk 取1。

2.2.4狀態空間修正函數

對于一般的平面遮擋場景，如圖4所示。對于物體被觀測到，但被部分遮擋的情況。假定物體形狀未知，此時物體的類型可確定，但真實遮擋率無法獲得。將其外側物體的形狀的一半累加在已觀測到的物體部分上，以粗略估計其遮擋率，并且外側物體所在順序棧的疊加計數 n_l 加1。而當目標物體未被觀測到時，其可能存在與已被觀測到的每個物體后方，故對每一個物體所在順序棧的疊加計數 n_l 加1。

而在該任務描述的堆疊場景中，物體的狀態空間分布在三維坐標上，上層物體疊放在下層物體上，即上層物體的存在受制于下層物體。同樣地，若識別到物體位于最上層，可根據物理性質推斷出其下方物體的屬性。在一個穩定的靜態環境中，上層物體的重心必然位于下層物體的投影上。假設通過RGBD攝像頭采集的物體 c 的位置狀態為三維坐標 （x，y，z） 。若 z 近似于 h ，則說明物體 c 沒有遮擋物體，否則其下方存在多個未被識別的物體 c_i ，且物體 c 下方第一個物體 c₁ 的二維坐標（x^′，y^′） 滿足 （x^′-x）²+（y^′-y）²≤（d/2）² 。故只要設計的場景單元格尺寸能保證每個單元格最多只有一個物體的幾何中心在其投影上，就能實現表示的唯一性。由此可直接根據式（7）進行狀態空間修正。其中各個物體 C_i 的疊加計數 n_l=z/h 11，結果取整。

在圖2所示數據結構中，表現為在棧頂元素下方增加 n_l 個拷貝的操作。需要說明的是，對于遮擋率的獲取并不意味著需要感知層的高性能。這是由于遮擋率僅作為判斷物體類型的初級依據，需要經過后續的計算映射到0/1的值，因而并不要求其數值的精確。

2.3 SP-POMDP求解

2.3.1 信念樹概述

由于2.2節中構建的SP-POMDP模型不是顯式的概率模型，難以設計出全局的最優價值函數，故無法用常規的值迭代方法[18]來求解。

蒙特卡羅樹搜索（MonteCarlotreesearch，MCTS）是一種用于樹搜索的在線算法，在人工智能方向，尤其是計算機博弈領域已經得到廣泛應用[19]。POMCP[20]基于MCTS，被廣泛用于大型POMDP的在線規劃求解。本文在此算法的基礎上進行調整，使其適用于SP-POMDP的求解。

首先是信念樹的構建方法。信念狀態的具體形式為針對每個系統狀態粒子的加權，所有權重之和為1。在任務開始時，根據第一次觀察結果對初始信念進行采樣。在后續每個步驟之后，可以通過式（11）來更新當前信念[20]。

由于被轉換成信念狀態 b ，狀態空間 s 在信念樹中沒有直接體現，而是以觀測 ω 的形式來表征環境的狀態。以所有系統狀態粒子的初始信念狀態為根節點，依次擴展所有可能的動作分支，并按照可能的觀測分支，更新信念狀態，以此為樹搜索的一個步長。

為實現SP-POMDP的抽象提取，需要在創建信念樹前，完成初始狀態空間 s 的抽象化，之后所有的分支節點元素均以抽象化后的形式保存，抽象后的信念樹如圖5所示。具體的抽象化過程在2.2.1節中已經說明。此時信念樹不僅作為求解算法的基礎，更是歷史信息的載體。

分析抽象信念樹的結構可以發現，信念樹可視為多個以信念狀態 b 為根節點，觀測 ω 為葉節點的子信念樹拼接而成，故稱圖5中虛線框內區域為信念樹的一個單元節點，記作 b_i^j ，其中 i 表示單元節點所在分支序號 Δ，j 表示單元節點所在層數，因此信念樹可表示為單元節點的集合 B={b_i^j|i，j=0，1，2，…} 。每個節點內保存的內容包括信念狀態、搜索次數以及回報，即b_i^j=?b，n_i，V_i? 。這樣的數據結構使得智能體能夠通過選擇不同的節點作為根節點來裁剪信念樹，這有利于歷史信息的處理。

2.3.2 算法說明

根據POMCP，設計出算法1對SP-POMDP進行求解。算法將信念樹的根節點 B={b⁰} 與目標信念狀態 b_goal 作為輸入，從初始信念狀態開始向下搜索節點，并擴展信念樹，在節點的信念狀態達到 b_goal 前不斷向下傳播。以下是對算法的一些說明。

算法用上限置信區間（upperconfidencebound，UCB）[21來定量描述節點被選擇后獲得的回報，實現貪婪的節點選擇。在算法1第2行，智能體返回當前節點下UCB最大分支的節點下標，以實現節點的選擇，其中 ∣b_i∣ 表示該分支節點個數。UCB由式（12）確定。

其中： 表示該節點的平均回報； c 為常數項，這里取 2;N 表示信念樹的總搜索次數； n_i 表示當前節點搜索次數。

每次搜索時，智能體會先判斷當前節點是否為葉節點，若不是，則選擇UCB值最高的節點分支向下傳遞，若是，且該節點被搜索過（ n_i≠0 ），則會先在當前信念狀態下枚舉所有可行的動作分支，擴展出一個完整的單元節點，再向下傳遞，見算法1第4、5行。

當傳播到葉節點且該節點未被搜索過（ n_i=0 ），智能體會進行隨機仿真，通過模擬來獲取當前分支的預期回報，見算法1第6＼～8行。該過程中，智能體會在當前節點下隨機生成一條可行的動作分支，重復進行節點擴展與信念狀態更新，直到達到目標信念狀態 b_goal ，此處參考POMCP實現[20]，并將動作的選擇過程分離，將其與信念狀態作為輸入。循環結束時返回本次隨機仿真的預期價值 ，由式（13）獲得。

value（b）=V+γ^t?value（b^′）

在搜索結束后，信念樹會更新此次搜索遍歷的每個節點信息，增加每個節點的搜索次數 n_i 以及總搜索次數 N ，并將value（b^j） 作為回報增加到每個節點上。此過程為反向傳播，見算法1第9行。由于該過程與常見算法相同，故不在本文算法中詳細描述。

對于非目標物體，本文算法將所有可抓取的物體所在的單元格作為動作的起始，隨機選擇一個信念值最低的單元格作為動作的終點，由此構建可行動作集合，見算法1第 11～18 行。由于SP-POMDP的信念狀態表征了各個單元格內存在目標物體的概率，可以此作為動作選擇的依據，但無法保證局部的最優解。經過測試，僅依據概率，會導致同一個動作的重復選擇，造成結果過擬合。

故采取基于抓取優先級的動作選擇策略。在隨機仿真前，會根據信念值，對所有可行動作進行優先級排序。每次選擇仿真動作時，都會選擇優先級最高的動作，并且在動作執行完后，減小動作的優先級。這樣的處理確保所有動作均被考慮，避免了結果的過擬合，同時減小了空間的遍歷次數，提高了效率。動作選擇函數的實現見算法1第19＼～25行。

算法1 SP-POMDP求解器輸人：初始信念樹 B={b⁰} ，目標信念狀態 b_goal ，折算系數γ。

輸出：更新后的信念樹 B_new 。

1 while b^j+1≠φ do

2

3 （20號 jj+1 （204

4 if n_i≠0 then

5 （2號 EXPANSION（b_i^j）

6 while b^j≠b_goal do

7 a^j ←ACTION_CHOOSE（AVAILABLE_ACTION）

8 b ←SIMULATE （b^j，a^j，γ） （2

9 Bnew←—BACKPROPAGATION（value（））

10 retum B_new

11 procedure AVAILABLE_ACTION（b）

12 for temp← ?b₁ ，.，bi do

13 if temp eq0 then

14 （204號 a₁cell（temp） //temp對應的單元格

15 else

16 a₂←cell（temp）

17 return Aenum（a₁，a₂） //枚舉所有 a_i 組合

18 end procedure AVAILABLE_ACTION（）

19 procedure ACTION_CHOOSE（A）

20 staticpriority←{（a1，P1），…，（ai，Pi）}

21 a，argmax（priority）

22 if p_tgt;minp then

23 （2號 p_t←minp+1 and update priority

24 return a_t

25 end procedure ACTION_CHOOSE（A）

基于以上算法，給出輸出單步動作的完整過程，見算法2。其中 H 為任務歷史信息序列，包括動作、觀測以及信念狀態信息，在算法2第1行中可理解為當前時刻桌面的實際狀態， 為設定的最大迭代次數，根據任務的復雜程度確定。

算法2任務規劃完整過程

輸入：任務歷史序列 H ，目標信念狀態 b_goal ，折算系數 γ ，最大迭代次數 0

輸出：單步策略動作 a_real 。

1S＼～H

2 （204

3 （204

4BCONSTRUCT_TREE（ δ，H）

5whileepoch do

6 B←sovler（B，y，bgoal）

7 epoch←epoch +1

8

9 return areal

3實驗

3.1 實驗環境介紹

實驗采用了4個測試場景，分別為5、6、7、8個物體構成的50個隨機擺放場景。利用顏色來區分物體類型，其中紅色物體為需要移動的圓盤，其余物體顏色為紅色以外的隨機顏色。利用Python等腳本語言的相關工具生成 {l_i=（x_i，y_i，z_i）|i∈ 三 格式的數據作為樣本。在生成隨機場景時給出以下限制，首先確保存在物體被遮擋的情況，且桌面上的物體滿足式（9）兩種情況的數量均勻。其次在所有場景中，目標物體可被一次動作抓取到目標區域的情況的樣本數量小于總數的三分之一。

在Ubuntu22.04系統中，基于ROS2構建仿真環境，利用Gazebo搭建如圖1所示尺寸的機器人抓取場景，利用RGB-D攝像頭插件實現環境的感知。選用機器人末端執行器為吸盤。機械臂的運動規劃由MoveIt2模塊實現，使用RRT-Connect規劃機械臂的運動路徑。實驗系統架構如圖6所示。

取折算系數 γ=0.99 ，遮擋率閾值 r_s=0.76 ，最大迭代次數 ，同時設置動作閾值，當執行動作數達到場景物體數減2時（樣本的理論最大動作數），無論任務是否完成，機器人均終止任務。以上參數均在前文中說明。

3.2 實驗方案設計

除本文方法外，設計以下兩組實驗進行對照。

a）為了驗證本文提出的狀態空間修正方法的可行性，針對任務場景，根據本文所設計的方案從原始POMDP中抽象提取出離散的POMDP，但不對無法被觀測的物體信息進行補全。具體操作為，基于式（8），增加 r_s?s_rlt;0.9 的情況為遮擋率較大、物體能被觀測但無法被識別的情況，同時不對初次采樣結果進行修正。該方案表示為 Π_nPOMDP 。

另外設置兩組貪心抓取策略作為基線方法。若未識別到目標物體，則逐一移除物體，直到識別到物體且為可抓取狀態，最后將物體抓取并轉移至指定區域。該方案不采用任務規劃的思想，而直接遍歷所有的情況，來獲得所有物體的位置及類型信息，為實際工業場景中的常規解決思路。該兩組方案分別采用廣度優先和深度優先的搜索方式。

根據仿真結果，通過以下幾個指標來評價本組方案的性能：（a）完成的任務總數；（b）完成一個任務所花費的平均時間，包含運動規劃所花費的時間，且僅對成功完成的任務進行統計，單位為s;

b）為了測試本文方法的綜合計算性能，與目前較為先進的算法在該堆疊場景中的表現進行比較。針對任務場景構建POMDP，分別利用 DESPOT[22]、PA-POMCP[12]、ADVT[23]進行求解。適用于對照組的POMDP的基本屬性構建方法在2.2.1節中已經說明，會根據具體方法的要求進行簡單調整，這里給出顯式的轉移概率與觀測概率信息，如表2所示。

根據仿真結果，通過以下幾個指標來評價本組方案的性能：（a）完成的任務總數；（b）輸出一個動作所花費的平均時間，單位為s；（c）信念樹搜索的平均時間，單位為s。

實驗中的代碼實現不考慮觀測到的物體總數對信念分布的影響。以此消除物體數量較少的場景中，由于復雜度過低導致的性能表現較好的情況，用以放大模型本身的表現。

例如，已知只有5個物體，當執行動作后，可以觀測并確定4個物體的類型不是目標物體。此時雖然主觀上可以直接確定剩余的狀態被修正的物體為目標物體，但仍使用任務規劃完成任務，如圖7所示。

圖8為5個物體的抓取實驗中，單次任務的信念樹搜索過程的一個示例（見電子版）。在初始采樣中，各個物體的離散狀態表示原則上為離該物體幾何中心最近的單元格位置，但由于式（8）將被堆疊物體表示為最上方物體的疊加狀態。故如圖8中初始狀態中的黃色物體，其最近的單元格為（1，1），但由于其被黑色物體疊放，故其位置狀態表示為（1，2），與黑色物體相同。

3.3 實驗結果分析

3.3.1 可行性實驗結果及分析

可行性實驗中各組方案的實驗結果如表3所示。由實驗結果可知，本文方法具有最優的性能，包括任務成功率以及任務完成速度。在任務過程中，POMDP的求解一般只存在兩種結果，成功求解或循環重復兩個動作。本實驗中未成功的實驗組，大部分是因為超出實驗設置的最大動作數。

分析nPOMDP的實驗結果可以發現，由于其未對無法觀測的物體狀態進行修正，導致初次采樣獲得的信息有限，故其任務完成速度沒有太好的表現，同時表現出最低的任務成功率。由此可以看出狀態空間修正的可行性以及優越性。由于本文方法對場景的狀態、動作以及觀測空間進行了抽象提取，優化了算法的計算效率，故即便是任務成功率最低的nPOM-DP，也有相對良好的完成速度表現。

由于該實驗對動作閾值的限制較為寬松，兩組貪心抓取方案均具有相對穩定的任務成功率，但粗暴的解決方式導致了較差的時間性能以及動作效率，不適合對工作效率要求較高的場景。其最大動作數由最大物體數決定，因為較大的物體數量使得目標物體被遮擋的概率增大，基本需要將上方物體移開才能搜索到目標物體。在本實驗中，機器人執行動作帶來的能耗反映在執行的動作數上。在實際工況中，過多無用動作帶來的能耗在一些執行時間長、任務量大的工作中會被不斷放大，故無論是性能還是能耗，本文方法均具有相對更好的表現。

3.3.2綜合性能實驗結果及分析

綜合性能實驗中各組方案的實驗結果如表4所示。由實驗結果可知，在堆疊場景中，本文方法具有較優的綜合性能表現。在該組實驗中，主要體現在算法的求解速度和任務成功率上。

DESPOT是基于優化POMCP設計的在線POMDP求解器，利用一種確定的稀疏部分觀測樹[22]，能夠在相對少的抽樣下評估策略，避免POMCP中極差的情況出現，在一些大型POMDP問題的求解上有著極佳的表現，是目前較為常用的求解算法。但在該組實驗中，受限于動作閾值，部分任務無法完成。通過分析任務過程中的日志可以發現，成功率低的主要原因在于該場景的信息有效性不足，導致信念狀態所反映的目標物體所在位置的概率于真實情況不符。同樣的情況也發生在ADVT的實驗中。ADVT在MCTS的基礎上，采用了一種稱為Voronoi樹的結構來對動作空間進行自適應離散化[22]，以提高連續高維動作空間下的求解性能，但對于求解性能的優化在本實驗中未能彌補信息缺失帶來的影響。而該問題在SP-POMDP中已通過狀態空間修正被消除，故在任務成功率上均略遜于本文方法。

PA-POMCP同樣是對POMCP的一個擴展。該方法在建模過程中考慮了針對物體不同遮擋程度的擴展性，故相較于DESPOT與ADVT，其對任務中出現完全遮擋對象的情況具有一定的魯棒性。除此之外，該方法在求解器中引入了動作空間參數化的操作[12]來加快求解。從實驗結果可以看出，PA-POMCP具有較優的求解速度。由于本文方法的求解器主要基于POMCP，與目前的主流求解器在效率上沒有優勢。但在建模過程中對于求解過程中狀態存儲的一些優化設計，使得本文方法在數據處理時具有更佳的效率，故從輸出策略動作的整個規劃周期上比較，本文方法具備最佳的表現。

4結束語

本文提出了一種針對堆疊場景的POMDP建模方法，能夠在考慮物體相互遮擋的情況下，對無法觀測物體的狀態進行修正，減小系統部分可觀測性對常規抓取任務規劃的影響。并基于POMCP，設計了所提模型的求解算法。實驗結果表明，本文方法在針對堆疊物體的機器人抓取任務規劃問題中具有良好的表現。與現有方法相比，本文方法主要的優勢與創新在以下幾個方面：a）提出了一種狀態空間修正方法，一定程度上消除了環境的不確定性與部分可觀測性；b）利用創新的順序棧來維護系統狀態屬性，使得算法對其的訪問及操作更加靈活；c）在POMCP的基礎上，設計了適應于該模型的求解算法，使用了基于優先級的動作選擇策略，避免了結果過擬合，并簡化了算法的計算規模，加快了模型的求解。本文方法在計算效率上的良好表現使其在實際工程應用中具備一定的優勢。本文方法雖然基于堆疊場景，但又不局限于該場景，狀態空間修正函數在一些環境固有特性相對易得的場景，如定量物體的搜索問題中仍具備一定優勢。后續需要針對場景特性調整狀態空間修正函數，如采用基于概率的模型來進行狀態空間的修補，擴大本文方法的應用范圍。

參考文獻：

[1]Lauri M，HsuD，PajarinenJ.PartiallyobservableMarkovdecisionprocesses in robotics：a survey[J].IEEE Trans on Robotics，2023，39（1）：21-40.

[2]MonsóP，Alenya G，TorrasC.POMDPapproach to robotizedclothesseparation[C]//Proc of IEEE/RSJ International Conference on In-telligentRobotsand Systems.Piscataway，NJ：IEEEPress，2012：1324-1329.

[3]徐進，柳寧，李德平，等．一種基于抓取簇和碰撞體素的工業零件抓取姿態檢測算法[J].機器人，2022，44（2）：153-166.（XuJin，LiuNing，LiDeping，etal.Agraspingposesdetectionalgorithmfor industrial workpieces based on grasping cluster and collision voxels[J].Robot，2022，44（2）：153-166.）

[4]薛騰，劉文海，潘震宇，等．基于視覺感知和觸覺先驗知識學習的機器人穩定抓取[J].機器人，2021，43（1）：1-8.（XueTeng，LiuWenhai，Pan Zhenyu，et al.Stable robotic graspbased onvisual perceptionand prior tactileknowledge learning[J].Robot，2021，43（1）：1-8.）

[5]Duan Shengqi，Tian Guohui，Wang Zhongli，et al.A semantic roboticgrasping framework based on multi-task learning in stacking scenes[J].EngineeringApplicationsofArtificial Intelligence，2023，121：106059.

[6]馬倩倩，李曉娟，施智平．輕量級卷積神經網絡的機器人抓取檢測研究［J].計算機工程與應用，2020，56（10）：141-148.（MaQianqian，Li Xiaojuan，Shi Zhiping.Research onlight-weight convo-lutional neural network for robotic grasp detection [J].ComputerEngineering and Applications，2020，56（10）：141-148.）

[7]李鑫，沈捷，曹愷，等．深度強化學習的機械臂密集場景多物體抓取方法[J]．計算機工程與應用，2024，60（23）：325-332.（LiXin，ShenJie，Cao Kai，etal.Deepreinforcement learning formanipulator multi-object grasping in dense scenes [J].ComputerEngineering and Applications，2024，60（23）：325-332.）

[8]．Hsiao K，KaelblingLP，Lozano-Perez T. Grasping POMDPs[C]//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ： IEEE Press，2007：4685-4692.

[9]Hsiao K，Lozano-Pérez T， Kaelbling L P. Robust belief-based execu-tion of manipulation programs[C]//Proc of the 8th InternationalWorkshop ontheAlgorithmic Foundations of Robotics.Cham：Springer，2008.

[10]Pajarinen J，Kyrki V.Roboticmanipulationof multipleobjectsasaPOMDP[J].Artificial Intelligence，2017，247：213-228.

[11]Li Juekun，Hsu D，Lee W S. Act to see and see to act： pomdp plan-ning for objects search in clutter[C]//Proc of IEEE/RSJ Interna-tional ConferenceonIntellgentRobotsand Systems.Piscataway，NJ：IEEEPress，2016：5701-5707.

[12]Xiao Yuchen，Katt S，Pas A，et al.Online planning for target objectsearch in clutter under partial observability[C]//Proc of InternationalConference on Roboticsand Automation.Piscataway，NJ：IEEEPress ，2019： 8241-8247.

[13]Pajarinen J，Kyrki V.Robotic manipulation in object compositionspace[C]//Proc of IEEE/RSJ Intermational Conference on Inteli-gent Robots and Systems.Piscataway，NJ： IEEE Press，2014：1-6.

[14]Pajarinen J，Lundell J，Kyrki V.POMDP planning under object com-position uncertainty：application to robotic manipulation[J]．IEEETrans on Robotics，2023，39（1） ：41-56.

[15] Zhao Wenrui， Chen Weidong.Hierarchical POMDP planning forob-ject manipulation in clutter[J].Robotics and Autonomous Sys-tems，2021，139：103736.

[16]Chen Z.Bayesian filtering：from Kalman filters to particle filters，andbeyond[J].Statistics，2003，182（1）：1-69.

[17]Bellman R.Dynamic programming [J]. Science，1966，153（3731） ： 34-37.

[18］朱榮鑫，王鐶，劉峰，等．基于環境狀態分布優化的POMDP值迭代求解算法［J]．計算機應用研究，2022，39（2）：374-378.（Zhu Rongxin，Wang Xuan，Liu Feng，et al. Probability-based valueiteration on optimal state distribution algorithm for POMDP[J].ApplicationResearch ofComputers，2022，39（2）：374-378.）

[19］朱舟，閔華松．利用統計數據并行蒙特卡羅樹搜索算法的中國象棋博弈［J]．計算機工程與應用，2024，60（23）：340-348.（ZhuZhou，Min Huasong. Chinese chess game using statistical data paral-lel Monte Carlo tree search algorithm[J].Computer Engineeringand Applications，2024，60（23）： 340-348.）

[20]Silver D，Veness J. Monte-Carlo planning in large POMDPs[C]//Advances in Neural Information Processing Systems.2010：2164-2172.

[21]Auer P，Cesa-Bianchi N，Fischer P.Finite-time analysis of the mul-tiarmed bandit problem[J].Machine Learning，2002，47（2）：235-256.

[22]Ye Nan，Somani A，Hsu D，et al．DESPOT：online POMDP plan-ning with regularization [J].Journal of Artificial Intellgence Re-search，2017，58：231-266.

[23]Hoerger M，Kurniawati H，Kroese D，etal.Adaptive discretizationusing voronoi trees for continuous-action POMDPs[M]//LaValle SM，O'kane JM，Ote M，et al.Algorithmic Foundations of RoboticsXV.Cham： Springer，2022：170-187.