基于混合層次包圍盒的水下訓練機械臂碰撞檢測方法研究

張 宇， 張得禮*， 張文奇， 王 苑， 王志國， 楊永常， 韓亮亮

(1.南京航空航天大學機電學院， 南京 210016； 2.上海宇航系統工程研究所， 上海 201109；3.中國航天科技集團有限公司空間結構與機構技術實驗室， 上海 201109)

1 引言

空間機械臂在空間站的建設及維護中能夠協助航天員完成大量復雜的艙外活動，或替代航天員執行一些高危任務，提高航天員工作效率和任務安全性[1-2]。 為保證空間站任務安全、順利完成，航天員需要提前在水下模擬失重環境中進行充分的模擬訓練[3-4]。 水下訓練機械臂是航天員水下出艙模擬訓練中必不可少的輔助設備，可以配合航天員進行出艙移動及驗證在空間站的相關任務[5]。 為了保證航天員及水下訓練設備的安全性，水下訓練機械臂的碰撞檢測十分重要。

目前，機械臂碰撞檢測方法主要有基于外部設備和基于3D 模型的2 種檢測方法。 基于外部設備的碰撞檢測方法需要添加額外的傳感器或相機等進行信息采集，增加了設備成本及系統復雜性；基于3D 模型的碰撞檢測方法則是根據模型之間的相交關系判斷是否產生碰撞，不需要額外設備且易于實現及維護，故在實際中得到了更廣泛的運用[6-7]。 目前，國內外已經提出了多種有效的碰撞檢測算法庫，如交互式精確碰撞檢測( Interactive and Exact Collision Detection，I-COLLIDE)、快速搜索隨機樹(Rapid-exploration Random Tree， RAPID)、柔性碰撞庫(Flexible Collision Library， FCL)等。 I-COLLIDE 基于掃描排序算法，利用模型軸對齊包圍盒(Axis-aligned Bounding Box， AABB) 在坐標系的投影判斷AABB 是否相交，然后對所有模型按相關性進行排序，減少不必要的相交測試，適用于檢測物體數量巨多的場景[8-9]。 RAPID 基于方向包圍盒(Oriented Bounding Box， OBB)，建立分層OBB 樹，OBB 之間利用分離軸定理(Separating Axis Theorem， SAT)判斷是否相交，能夠準確檢測多邊形模型之間的碰撞，但OBB 樹的更新及SAT 算法計算量都比較大[10]。 FCL 提供了一個可供各種應用程序使用的統一接口，集成了基于凸包的碰撞檢測，如GJK 算法，基于邊界體積層次的碰撞檢測，如包圍盒、空間分解算法等[11-12]。 GJK 算法[13]能夠運用于各種凸體，但其迭代過程難以避免舍入誤差，且迭代時間無法確定，不滿足實時控制的要求。 以上算法庫基本解決了模型的碰撞檢測問題，但存在算法復雜，計算時間長的問題，不能滿足控制系統的實時性要求。

包圍盒算法計算時間短，通過合理地建立包圍盒，能夠在滿足實時性的要求下提高包裹性能。包圍盒算法的精度與物體幾何外形的逼近程度有關[14]。 由于水下機械臂完全展開長度約為11 m，最大直徑約為0.5 m，最小直徑約為0.3 m。 艙體如核心艙軸線長約為16.5 m，中間艙段的半徑約為4.2 m，兩端艙段的半徑約為1.8 m，機械臂及艙體體積非常大且軸向半徑不斷變化，易導致包圍盒內的多余體積較大，從而難以保證碰撞檢測的精度。 同時，由于水下機械臂的臂展長，質量大，導致其慣性大，減速時間長，為了保證航天員及訓練設備的安全，對碰撞檢測的實時性及準確性要求更高。 干智峰等[15]通過分段建模方式建立AABB 層次樹，對機械臂碰撞部位進行精準判斷，但碰撞檢測的精度由模型的AABB 進行多層劃分保證，計算量較大；鄭義等[16]采用球體及膠囊體包圍盒對機械臂模型進行簡化，提升了包圍盒包裹性能，但采用全部遍歷方式，算法計算量較大，并且采用正解對包圍盒特征點進行定位，對復雜機械臂模型的適應性差；李玉虎等[17]提出一種上、下2 層的混合層次包圍盒樹創建方法，避免對不同種類包圍盒之間進行相交測試，能夠降低算法復雜度；Zhang 等[18]將2 個機械臂OBB 在地面投影，篩除不可能碰撞的OBB，但只能減少機械臂之間在與地面平行的距離較遠時的碰撞檢測計算量，且篩選效果較差；Safeea 等[19]基于QR 分解提出一種計算膠囊體包圍盒之間最小距離的簡單解析解，但計算時間并未得到改善。

本文針對航天員水下訓練超大作業空間的人機安全問題，提出一種快速、準確的機械臂碰撞檢測方法。 首先，基于最小包絡和二分法實現球體及膠囊體包圍盒的包裹性能優化及自動生成；然后，基于二叉樹思想，采用AABB-球體及膠囊體包圍盒2 層結構，建立混合層次包圍盒樹，通過同層優先的遍歷算法對碰撞部位進行快速定位。

2 機械臂及艙體模型包圍盒創建

為了提高包圍盒的包裹性能及測試效率，提高碰撞檢測的精度及效率，需要對機械臂及艙體模型進行簡化建模[20]。 考慮到機械臂整體結構由2 根細長的圓柱連桿和一些近似短圓柱體的零件組成，艙體主要形狀為圓柱體及球體，且圓柱體及球體包圍盒能夠方便、準確計算最小距離，故選用球體及膠囊體包圍盒對機械臂及艙體模型進行簡化建模。 而在機械臂執行任務時，航天員通過手臂對艙體上的關鍵部件進行操作，為了滿足航天員的操作空間，艙體上的部件與人體需要保持一定距離，故實際包圍盒需要在最小包圍盒的基礎上擴大一定范圍，此時機械臂與艙體表面細節已經在實際包圍盒內，故在包圍盒創建時可將這些表面細節進行簡略。

機械臂球體及膠囊體包圍盒的幾何特征點可以利用機械臂DH 參數對運動過程中的機械臂進行正運動學分析求得，但該方法對機械臂模型幾何結構要求比較嚴格，對于零部件結構復雜的機械臂通常還需要進行基于模型的修正計算，其包裹性能難以得到保證，且不易實現包圍盒的自動生成。 本文基于最小包絡和二分法的思想提出了一種模型的最優球體及膠囊體包圍盒的自動化生成方法。

2.1 膠囊體包圍盒創建

最優膠囊體包圍盒的創建旨在使膠囊體左、右半球內的點分別到左、右半球球心的最大距離恰好等于膠囊體半徑，從而得到最小包絡膠囊體，具體方法如下:

1)在三維模型導入階段，對模型進行OBB 建模，從而獲得位于模型最長邊方向上的左右端點P1、P2及中心線P1P2， 如圖1 所示，為了簡化計算，設定向量P1P2方向為模型的x軸方向。

圖1 模型中心軸的創建Fig.1 Creation of model central axis

2)計算模型上所有三角片的頂點到中心線P1P2的最大距離dmax作為膠囊體包圍盒的半徑radius，模型三角片上的頂點(x，y，z) 到中心線P1P2的距離d如式(1)所示:

3)接下來的目標是將膠囊體左右半球的球心盡可能往中心移動。 假設點P1(xmin，0，0)、P2(xmax，0，0) 為求得的OBB 中心線的端點。 首先，令膠囊體包圍盒左右半球的球心P3(x3，0，0)，P4(x4，0，0) 的初始值等于點P1、P2， 以線段P3P4作為膠囊體包圍盒的中心軸線；然后，確定點P3的x3的取值范圍為xmin～(xmin＋radius)，點P4的x4的取值范圍為(xmax-radius) ～xmax；最后，通過二分法快速查找P3、P4的最優解。

以點P3為例，算法流程如圖2 所示，其中Pmax為模型在P3點左側(x ＜x3) 所有點到P3的最大距離，P3.x為P3點的x軸坐標值，SD為給定的P3到最佳點(Pmax＝radius)的誤差可接受范圍，SD越小，包圍盒包裹性能越好，耗費的計算時間相應增長。

圖2 最優解算法流程圖Fig.2 Flow chart of optimal solution algorithm

圖3 為用最優解算法獲得的滿足精度要求的模型最優膠囊體包圍盒。

圖3 膠囊體包圍盒Fig.3 Capsule enclosure

2.2 球體包圍盒創建

球體包圍盒的創建旨在找到一個最小包絡球面能夠使模型內所有點到球心的距離小于球體的半徑，具體方法如下:

1)在模型導入階段，對模型進行OBB 建模，從而獲得模型的中心點center(x0，y0，z0)， 如圖4中點P所示。

圖4 模型中心點的創建Fig.4 Creation of model center point

2)計算模型所有點到中心點center的距離，模型三角片上的頂點(x，y，z) 到中心點center的距離d如式(2)所示:

3)比較得到所有頂點到球心距離的最大值dmax，則球體包圍盒的半徑radius等于dmax。

圖5 為得到的最優球體包圍盒。 球體及膠囊體包圍盒只需在模型導入時創建一次，之后可以通過旋轉平移坐標變換矩陣實現與機械臂模型的同步運動。

圖5 球體包圍盒Fig.5 Sphere bounding box

3 包圍盒碰撞檢測算法

3.1 AABB 碰撞檢測算法

AABB 的碰撞檢測只需要將2 個包圍盒投影到x，y，z坐標軸上，若2 個包圍盒在3 個坐標軸上的投影均相交，則說明2 個AABB 相交，相交測試只需要進行6 次比較運算[21]。

3.2 OBB 碰撞檢測算法

OBB 之間采用分離軸算法進行碰撞檢測，即給定2 個凸物體，若這2 個物體不相交，則一定存在一個平面能夠將2 個物體隔開，分離軸即為該平面的法線，將2 個物體上的點投影到分離軸上，2 個物體的投影不重疊，則能夠判定2 個物體不相交[22]。 OBB 總共有3 個面法矢以及3 個邊法矢，2 個包圍盒總共有6 個面法矢，加上2 個包圍盒之間的邊法矢叉乘得到的9 個面法矢，總共進行15 次分離軸測試[23]。

3.3 膠囊體及球體包圍盒碰撞檢測算法

膠囊體與球體包圍盒之間的碰撞檢測是基于最小距離的原理，即如果2 個物體之間的最小距離大于0，則能夠說明2 個物體未相交，基于這種思想，可得到膠囊體及球體包圍盒碰撞檢測算法[24]。 球體與球體之間的最小距離簡化為求解2 個球心點之間的距離，球體與膠囊體之間的最小距離簡化為求解球心點到膠囊體中心線段之間的最小距離，膠囊體與膠囊體之間的最小距離簡化為求解膠囊體中心線段到膠囊體中心線段之間的最小距離。 本文膠囊體及球體包圍盒碰撞檢測方法采用文獻[6]中計算線段與線段以及點與線段之間最小距離的方法。

4 水下機械臂碰撞檢測算法

層次包圍樹是基于二叉樹，按照自頂向下的順序對物體進行剖分，從而建立層次包圍盒樹，自頂向下的節點與包圍盒緊密程度相對應[25]。 層次包圍盒樹隨著機械臂的運動進行實時更新，其碰撞檢測遍歷遵循同層優先的原則，對機械臂包圍盒進行快速篩選，最終確定機械臂實際碰撞位置。

4.1 混合層次包圍盒樹創建

為給定模型創建OBB 樹所需的時間大約是創建AABB 樹所需時間的3 倍[26]。 上層層次包圍盒相交測試次數最多，需要構建簡單、測試速度快，從而實現對碰撞部位快速定位；底層則為了滿足碰撞監測精度要求，需要包裹性好的包圍盒[27]。 因此，雖然AABB 緊密性較差，但創建簡單，相交測試計算量小，能夠實現層次樹的快速刷新及對碰撞部位的快速定位。 球體及膠囊體包圍盒計算量比AABB 大，比OBB 小，并且對于水下機械臂和艙體模型而言，包裹性能優于AABB 和OBB，故層次樹上層節點選用AABB，層次樹葉子節點則選用球體及膠囊體包圍盒。 而不同種類的包圍盒之間的相交檢測算法復雜，難以滿足機械臂碰撞檢測的實時性要求，所以對于機械臂模型及艙體模型均采用AABB 球體及膠囊體2 層結構，上層結構為AABB，下層結構為球體及膠囊體包圍盒。 機械臂和艙體層次結構圖如圖6 所示。

圖6 機械臂和艙體層次結構圖Fig.6 Hierarchy diagram of manipulator and cabin

其中AABB 層只與AABB 層進行相交檢測，球體與膠囊體層只與球體與膠囊體層進行相交檢測，降低了相交檢測算法復雜度。 AABB 層可以實現包圍盒的實時刷新，能夠快速排除掉沒有相交的物體，提升算法計算效率。 球體及膠囊體層不僅能夠滿足算法效率的要求，還能夠保證包圍盒的緊密性。 假設二叉樹層數為n(n＞0)，則其葉子節點最大數量為2n-1。 故假設葉子節點數為m，則層次樹層數D如式(3)所示:

針對水下訓練機械臂及航天員模型的結構特點，對模型的零部件進行拆分，拆分后得到的零部件去除水下機械臂基座、關節1 等不可能產生碰撞的部分，獲得機械臂模型層次包圍盒樹的葉子節點，葉子節點數為13，故層次樹實際層數為5。

層次樹的創建流程按照自頂向下的順序逐層將機械臂的父節點劃分為子節點，按照從末端航天員到關節2 的左右順序的排列，建立起基于AABB-球體及膠囊體包圍盒2 層結構的水下機械臂碰撞檢測混合層次包圍盒樹。 機械臂混合層次包圍盒樹如圖7 所示。 水下機械臂葉子節點采用球體及膠囊體包圍盒進行包圍盒建模，其余節點均采用AABB 進行包圍盒建模。 水下機械臂葉子節點球體及膠囊體包圍盒如圖8 所示。

圖7 機械臂碰撞檢測混合層次包圍盒樹Fig.7 Hybrid hierarchical bounding box tree for collision detection of manipulator

圖8 葉子節點球體及膠囊體包圍盒示意圖Fig.8 Schematic diagram of leaf nodes’ sphere and capsule bounding box

4.2 混合層次包圍盒樹的更新

混合層次包圍盒樹采用由葉子節點到根節點的更新順序。 混合層次包圍盒樹葉子節點的AABB 和球體及膠囊體包圍盒在模型導入階段已經創建完成，在機械臂運動期間通過旋轉平移變換矩陣實現葉子節點包圍盒的更新。 其余層次的AABB 更新則是通過下一層次的左、右子節點的AABB 來創建上一層次的父節點的AABB，逐層創建，直到創建完成最高層次根節點的AABB，則實現了整個混合層次包圍盒樹的更新。

假設父節點的左、右子節點的AABB 的16 個頂點分別為Pi(xi，yi，zi)(i＝1，2，…，16)，比較得到xi、yi、zi的最大值和最小值，將最大、最小值組成 點Pmax(Xmax，Ymax，Zmax) 和Pmin(Xmin，Ymin，Zmin)，這2 個點即為父節點的AABB 的2 個頂點，父節點的AABB 包圍盒通過頂點Pmax以及Pmin來創建，父節點包圍盒的長L、寬W、高H如式(4)所示:

更新過程總體示意圖如圖9 所示，通過4 號和5 號葉子節點的AABB 創建2 號節點的AABB，再通過6 號和7 號葉子節點的AABB 創建3 號節點的AABB，最后由2 號和3 號節點的AABB 創建1 號根節點的AABB，完成混合層次包圍盒樹的更新。

圖9 混合層次包圍盒樹更新示意圖Fig.9 Schematic diagram of update of hybrid hierarchical bounding box tree

4.3 混合層次包圍盒樹相交檢測

混合層次包圍盒樹的相交檢測采用同層優先的遍歷算法。 首先從層次樹的根節點出發進行遍歷，如果父節點檢測到相交，則進入到左右子節點的相交檢測中，直到對所有相交包圍盒的左右子節點完成相交測試。 在相交檢測過程中若檢測到葉子節點相交，則表明機械臂跟艙體之間產生了碰撞。 若未檢測到葉子節點相交，則表明機械臂與艙體之間未產生碰撞，開始對機械臂下一位置的層次樹進行更新重建。 假設帶豎線的方塊代表檢測到相交的包圍盒，空白的方塊代表未檢測到相交的包圍盒，則從根節點開始遍歷尋找產生相交的準確位置的過程如圖10 中箭頭所示。

圖10 層次樹遍歷示意圖Fig.10 Schematic diagram of hierarchical tree traversal

采用同層優先的遍歷算法，即針對未發生碰撞的部位，只要父節點沒有檢測到相交，則跳到同層次的節點進行下一次相交檢測，能夠快速排除未相交的部位，大幅減少相交檢測時間，實現碰撞部位的快速定位。 同時，在機械臂距離艙體較遠時，能夠將遍歷過程鎖定在AABB 層的前幾層，減少大量無意義的相交檢測計算。

4.4 碰撞檢測流程

碰撞檢測算法流程圖如圖11 所示。 ①程序開始運行后，進行機械臂及艙體模型的導入；②通過模型三角片的頂點信息創建層次包圍盒樹葉子節點的球體及膠囊體包圍盒和AABB；③利用創建好的葉子節點的包圍盒進行一次混合層次包圍盒樹的創建；④對層次包圍盒樹進行相交檢測遍歷；⑤遍歷完成后，輸出機械臂碰撞狀態，如果檢測到最小距離低于碰撞檢測閾值，則給操作人員發送預警信號；⑥若程序不關閉，則讀取機械臂下一步的每個關節的運動位置；⑦更新下一位置的層次包圍盒樹；⑧最后重復④～⑦，實現機械臂的實時碰撞檢測。

圖11 碰撞檢測算法流程圖Fig.11 Flow chart of collision detection algorithm

5 試驗驗證

為了驗證本文機械臂碰撞檢測算法，選用航天訓練水下機械臂作為試驗對象，為滿足碰撞檢測實時性要求，碰撞檢測總計算時間要求小于0.5 ms。 實驗平臺如圖12 所示，左圖為水下機械臂及艙體實物，右圖為水下機械臂操控臺，操控臺控制器為倍福高性能嵌入式工控機CX2040，設備配置為CPU: Intel? Corei7，2.1 GHz，雙核；內存4 GB。

圖12 水下訓練環境和操控臺Fig.12 Underwater training environment and console

將本文碰撞檢測算法集成到自主研發的水下訓練機械臂操控系統，該操控系統利用OpenGL和MFC 實現虛擬環境和人機交互，集成了機械臂運動學模型，能夠實現機械臂及艙體模型的導入及位置標定、機械臂的仿真運動軌跡規劃以及機械臂模型跟隨機械臂實體進行同步運動等功能，操控臺控制器運行虛擬平臺代碼，機械臂及艙體模型導入后通過本文算法自動創建包圍盒樹并在模型上顯示球體及膠囊體包圍盒，效果圖如圖13所示。

圖13 模型導入效果圖Fig.13 Effect drawing of model import

5.1 精度驗證

包圍盒的緊密性越好，碰撞檢測算法精度越高。 機械臂和艙體的OBB、球體及膠囊體包圍盒的總體積如表1 所示，球體及膠囊體包圍盒總體積比OBB 的總體積減少了28%，說明采用球體及膠囊體包圍盒作為層次包圍盒樹的葉子節點能夠顯著減少包圍盒的無效體積，提高碰撞檢測算法的精度。

表1 不同包圍盒體積對比Table 1 Volume comparison of various bounding boxes

表2 為模型真實體積與球體及膠囊體包圍盒體積和兩者之間的體積比，機械臂整體體積比為49.2%，核心艙體積比為82.8%，節點艙體積比為96.9%，實驗艙體積比為93.1%，可以看到球體及膠囊體模型整體包絡效果較好，特別是對于艙體模型包絡效果非常好，對于機械臂部件模型，組裝成整體后，膠囊體包圍盒部分重疊部位較多，故實際體積比大于49.2%。

表2 模型體積和包圍盒體積對比Table 2 Comparison of model volume and bounding box volume

5.2 實時性驗證

由于機械臂及艙體設備成本昂貴，而且機械臂的質量大、臂展長，導致其慣性大，所需要的減速時間長，為航天員及試驗設備的安全性考慮，將AABB 及OBB 長寬高以中心為基準增大50 mm，球體及膠囊體包圍盒半徑提升50 mm，即將碰撞檢測的閾值設為(100 mm，100 mm)，這樣既可以保證安全性，又在航天員手動操作空間允許的范圍內，不會影響其進行訓練操作。 機械臂以0.1 m/s 的最大速度進行S 型加減速直線運動。表3 為4 種算法計算時間t的統計數據，圖14 為4 種算法的計算時間曲線圖。 可以看到本文算法能夠滿足水下機械臂碰撞檢測實時性要求，且計算時間較其他算法縮短了45%以上，大幅減少了碰撞檢測計算時間，提高了碰撞檢測效率。

圖14 4 種算法計算時間對比Fig.14 Comparison of calculation time of four algorithms

表3 算法計算時間統計分析Table 3 Statistical analysis of algorithm calculation time

5.3 準確性驗證

當將包圍盒碰撞檢測閾值設置為100 mm時，圖15 為機械臂末端航天員訓練用操作臺與艙體之間產生碰撞時的位置，可以看到操作臺包圍盒在此時做出了碰撞反應，操作臺模型及其包圍盒變成了紅色，此時計算得到的機械臂與艙體包圍盒之間的最小距離dmin為68.00 mm，小于碰撞檢測的閾值100 mm，說明包圍盒已經產生了相交。

圖15 包圍盒碰撞位置Fig.15 Collision position of bounding box

通過在機械臂末端安裝超聲測距傳感器，令機械臂末端以水平姿態，沿豎直方向接近艙體，測量機械臂運動過程中機械臂末端與艙體的最小距離。 結果如圖16、圖17 所示。 圖16 為通過算法計算得到的機械臂包圍盒與艙體包圍盒之間的最小距離和通過傳感器實際測得的最小距離，圖17為本文算法得出的碰撞信號和通過實際距離確定的碰撞信號。 圖17 中出現一個周期的延遲是由于傳感器測距本身存在誤差。 結合圖16、圖17可得，當將碰撞檢測距離設置為100 mm 時，在產生碰撞信號的部位，其最小距離均小于100 mm，故本文算法可以準確定位碰撞部位，滿足碰撞檢測準確性要求。

圖16 機械臂與艙體最小距離Fig.16 Minimum distance between manipulator and cabin

圖17 機械臂碰撞檢測信號Fig.17 Collision detection signal of manipulator

6 結論

針對航天員水下訓練超大作業空間人機安全問題，提出一種快速準確的碰撞檢測方法。

1)實現簡化模型膠囊體包圍盒自動生成；

2)建立混合層次包圍盒樹，通過層次優先的遍歷能夠實現碰撞部位的快速篩選以及準確定位，計算時間較其他算法縮短了45%以上；

3)將該方法集成于自主研發的水下訓練機械臂操控系統，并在長時間的運行中證明了算法的穩定性及可靠性。

本文算法通用性強，也可運用于其他類型的機械臂或者其他工業設備的碰撞檢測。