PV-OSIMr: A Lowest Order Complexity Algorithm for Computing the Delassus Matrix

作者: Ajay Suresha Sathya, Wilm Decré, Jan Swevers · IEEE Robotics and Automation Letters (RA-L) 2024

授權條款

• 開放取用
• 論文: CC BY-SA 4.0

以下內容皆依據論文全文，依其 CC BY-SA 4.0 授權條款使用；圖表已標示作者。

一句話摘要

重新安排 PV-OSIM 的計算順序，讓傳播的約束永遠不超過六個維度，從而以 O(n + m²) 計算 Delassus 矩陣，作者證明這在漸近上是最佳的。

為何重要

具有許多接觸點的機器人，例如具備雙腳與多指手的人形機器人，需要以高頻率計算這個矩陣來進行模擬與控制，因此運算量越少，求解器就越快。由於建構 m × m 矩陣以及走訪全部 n 個關節無法避免，作者主張 O(n + m²) 是最佳階數。此演算法以 CasADi 程式碼生成實作，而且根據論文，也以 C++ 實作於 Pinocchio 函式庫中。

問題

Delassus 矩陣，也就是逆操作空間慣性矩陣，連結約束力與約束加速度，是約束與接觸動力學模擬以及操作空間控制所必需的，在 MPC 與 RL 中它往往是成本最高的步驟。直接計算的成本為 O(n³ + m²n)。已知最好的演算法 EFPA 可達 O(n + md + m²)，而作者先前提出的 PV-OSIM 為 O(n + m²d)，當許多約束必須穿過許多關節傳播時，其擴展性不佳。

提出的方法

PV-OSIMr 修改 PV-OSIM 的反向走訪：它不再傳播會隨約束數量變大的約束空間矩陣，而是在相鄰的分支連桿（集合 N）之間傳播固定大小的 6×6 擴展力傳播子與空間逆慣性。在 N 上進行的有限前向走訪計算逆慣性，有限反向走訪計算每個末端執行器的約束空間傳播子，而 Delassus 矩陣的每個區塊都以常數時間組合完成。如此可得到 O(n + m²) 的複雜度。

關鍵見解

• 當一個連桿承受超過六個約束維度時，先傳播 6D 空間量、稍後再還原約束空間項，會比直接傳播約束空間項更省運算。
• 高成本的量只需要在受約束連桿的最近共同祖先處計算，因此額外的走訪只在縮減後的連桿集合上進行。
• 較低的漸近複雜度並不保證在小型機器人上運算更少：由於 EFPA 採用三次走訪的結構，並需要逐一連桿的座標系轉換，它在幾乎所有測試機構上的成本都高於 PV-OSIM 與 PV-OSIMr。
• LTL-OSIM 雖然複雜度較高，但在單一 Shadow 手上勝過 EFPA，因為該手的分支結構產生了它能利用的稀疏性。

主要結果

• 複雜度：PV-OSIMr 為 O(n + m²)，相較於 PV-OSIM 的 O(n + m²d)、EFPA 的 O(n + md + m²)、KJR 的 O(n + m²d)、LTL-OSIM 的 O(nd² + m²d + md²)，以及直接計算的 O(n³ + m²n)。
• Atlas 站立、每隻腳有四個 3D 點接觸（每隻腳 12D）：PV-OSIM 比 PV-OSIMr 多需要 19.9% 的運算；每隻腳使用一個 6D 焊接約束時，兩種演算法完全相同。
• 配備 Shadow 手的 Atlas（每個指尖一個 3D 約束，每隻手 15D）：第 IV-D 節報告的運算量比次有效率的演算法 PV-OSIM 最多少約 34%，而結論一節則稱在 Atlas 範例上最多改善 24%。
• 在每個分支末端都有 6D 焊接約束的分支測試機構上，PV-OSIMr 在單一分支時與 PV-OSIM 相同，從兩個分支起則增長得較緩和；對於每側有十個分支的長機構，EFPA 最終勝過 PV-OSIM。
• 有一種情況 PV-OSIM 所需運算約少 1.5%：雙手與雙腳都有 6D 焊接約束的 Atlas，其約束在抵達浮動基座之前只穿過三個關節。
• 所有基準測試都是 CasADi 實作中的運算次數，而非實際執行時間。

限制

• 當超過六個約束維度只穿過少數（三個或更少）單 DoF 關節時，PV-OSIM 可能成本較低，因為其秩 1 更新優於 PV-OSIMr 的兩次秩 6 計算。
• 當約束只在浮動基座處交會，或機構是單一鏈時，PV-OSIMr 相較於 PV-OSIM 沒有任何增益。
• 結果是以運算次數呈現，而非實測的執行時間。
• 此演算法是針對運動學樹提出；C++ 的 Pinocchio 版本原訂在論文獲接受後才釋出。

對領域的影響

此論文獲得 RA-L 2025 最佳論文獎，被引用 4 次。作者在 GitHub 上以原始碼形式釋出其 CasADi 實作，並表示 C++ 版本已實作於 Pinocchio 函式庫中，將於論文獲接受後釋出。

摘要

我們提出 PV-OSIMr，一種為運動學樹計算 Delassus 矩陣（又稱逆操作空間慣性矩陣）的高效率演算法，其計算複雜度為文獻中已知的最低階數。PV-OSIMr 是藉由力傳播子與運動傳播子的可組合性，將 Popov-Vereshchagin（PV）求解器的計算加以最佳化而推導出來。其計算複雜度為 O(n + m²)，相較於原始 PV-OSIM 演算法的 O(n + m²d) 與擴展力傳播子演算法（EFPA）的 O(n+md+m²)，其中 n 為關節數，m 為約束數，d 為運動學樹的深度。由於計算 Delassus 矩陣需要建構一個 m × m 大小的矩陣，且必須至少考慮全部 n 個關節一次，PV-OSIMr 的漸近計算複雜度是最佳的。我們進一步對演算法進行基準測試，發現它在實務上經常比 PV-OSIM 與 EFPA 更有效率。

連結

• arXiv 2310.03676: https://arxiv.org/abs/2310.03676
• DOI 10.1109/LRA.2024.3469829: https://doi.org/10.1109/LRA.2024.3469829
• PDF: https://arxiv.org/pdf/2310.03676v1
• 程式碼: https://github.com/AjSat/spatial_V2