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

(1.南京航空航天大學(xué)機(jī)電學(xué)院， 南京 210016； 2.上海宇航系統(tǒng)工程研究所， 上海 201109；3.中國(guó)航天科技集團(tuán)有限公司空間結(jié)構(gòu)與機(jī)構(gòu)技術(shù)實(shí)驗(yàn)室， 上海 201109)

1 引言

空間機(jī)械臂在空間站的建設(shè)及維護(hù)中能夠協(xié)助航天員完成大量復(fù)雜的艙外活動(dòng)，或替代航天員執(zhí)行一些高危任務(wù)，提高航天員工作效率和任務(wù)安全性[1-2]。 為保證空間站任務(wù)安全、順利完成，航天員需要提前在水下模擬失重環(huán)境中進(jìn)行充分的模擬訓(xùn)練[3-4]。 水下訓(xùn)練機(jī)械臂是航天員水下出艙模擬訓(xùn)練中必不可少的輔助設(shè)備，可以配合航天員進(jìn)行出艙移動(dòng)及驗(yàn)證在空間站的相關(guān)任務(wù)[5]。 為了保證航天員及水下訓(xùn)練設(shè)備的安全性，水下訓(xùn)練機(jī)械臂的碰撞檢測(cè)十分重要。

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

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

本文針對(duì)航天員水下訓(xùn)練超大作業(yè)空間的人機(jī)安全問題，提出一種快速、準(zhǔn)確的機(jī)械臂碰撞檢測(cè)方法。 首先，基于最小包絡(luò)和二分法實(shí)現(xiàn)球體及膠囊體包圍盒的包裹性能優(yōu)化及自動(dòng)生成；然后，基于二叉樹思想，采用AABB-球體及膠囊體包圍盒2 層結(jié)構(gòu)，建立混合層次包圍盒樹，通過同層優(yōu)先的遍歷算法對(duì)碰撞部位進(jìn)行快速定位。

2 機(jī)械臂及艙體模型包圍盒創(chuàng)建

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

機(jī)械臂球體及膠囊體包圍盒的幾何特征點(diǎn)可以利用機(jī)械臂DH 參數(shù)對(duì)運(yùn)動(dòng)過程中的機(jī)械臂進(jìn)行正運(yùn)動(dòng)學(xué)分析求得，但該方法對(duì)機(jī)械臂模型幾何結(jié)構(gòu)要求比較嚴(yán)格，對(duì)于零部件結(jié)構(gòu)復(fù)雜的機(jī)械臂通常還需要進(jìn)行基于模型的修正計(jì)算，其包裹性能難以得到保證，且不易實(shí)現(xiàn)包圍盒的自動(dòng)生成。 本文基于最小包絡(luò)和二分法的思想提出了一種模型的最優(yōu)球體及膠囊體包圍盒的自動(dòng)化生成方法。

2.1 膠囊體包圍盒創(chuàng)建

最優(yōu)膠囊體包圍盒的創(chuàng)建旨在使膠囊體左、右半球內(nèi)的點(diǎn)分別到左、右半球球心的最大距離恰好等于膠囊體半徑，從而得到最小包絡(luò)膠囊體，具體方法如下:

1)在三維模型導(dǎo)入階段，對(duì)模型進(jìn)行OBB 建模，從而獲得位于模型最長(zhǎng)邊方向上的左右端點(diǎn)P1、P2及中心線P1P2， 如圖1 所示，為了簡(jiǎn)化計(jì)算，設(shè)定向量P1P2方向?yàn)槟Ｐ偷膞軸方向。

圖1 模型中心軸的創(chuàng)建Fig.1 Creation of model central axis

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

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

以點(diǎn)P3為例，算法流程如圖2 所示，其中Pmax為模型在P3點(diǎn)左側(cè)(x ＜x3) 所有點(diǎn)到P3的最大距離，P3.x為P3點(diǎn)的x軸坐標(biāo)值，SD為給定的P3到最佳點(diǎn)(Pmax＝radius)的誤差可接受范圍，SD越小，包圍盒包裹性能越好，耗費(fèi)的計(jì)算時(shí)間相應(yīng)增長(zhǎng)。

圖2 最優(yōu)解算法流程圖Fig.2 Flow chart of optimal solution algorithm

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

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

2.2 球體包圍盒創(chuàng)建

球體包圍盒的創(chuàng)建旨在找到一個(gè)最小包絡(luò)球面能夠使模型內(nèi)所有點(diǎn)到球心的距離小于球體的半徑，具體方法如下:

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

圖4 模型中心點(diǎn)的創(chuàng)建Fig.4 Creation of model center point

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

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

圖5 為得到的最優(yōu)球體包圍盒。 球體及膠囊體包圍盒只需在模型導(dǎo)入時(shí)創(chuàng)建一次，之后可以通過旋轉(zhuǎn)平移坐標(biāo)變換矩陣實(shí)現(xiàn)與機(jī)械臂模型的同步運(yùn)動(dòng)。

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

3 包圍盒碰撞檢測(cè)算法

3.1 AABB 碰撞檢測(cè)算法

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

3.2 OBB 碰撞檢測(cè)算法

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

3.3 膠囊體及球體包圍盒碰撞檢測(cè)算法

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

4 水下機(jī)械臂碰撞檢測(cè)算法

層次包圍樹是基于二叉樹，按照自頂向下的順序?qū)ξ矬w進(jìn)行剖分，從而建立層次包圍盒樹，自頂向下的節(jié)點(diǎn)與包圍盒緊密程度相對(duì)應(yīng)[25]。 層次包圍盒樹隨著機(jī)械臂的運(yùn)動(dòng)進(jìn)行實(shí)時(shí)更新，其碰撞檢測(cè)遍歷遵循同層優(yōu)先的原則，對(duì)機(jī)械臂包圍盒進(jìn)行快速篩選，最終確定機(jī)械臂實(shí)際碰撞位置。

4.1 混合層次包圍盒樹創(chuàng)建

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

圖6 機(jī)械臂和艙體層次結(jié)構(gòu)圖Fig.6 Hierarchy diagram of manipulator and cabin

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

針對(duì)水下訓(xùn)練機(jī)械臂及航天員模型的結(jié)構(gòu)特點(diǎn)，對(duì)模型的零部件進(jìn)行拆分，拆分后得到的零部件去除水下機(jī)械臂基座、關(guān)節(jié)1 等不可能產(chǎn)生碰撞的部分，獲得機(jī)械臂模型層次包圍盒樹的葉子節(jié)點(diǎn)，葉子節(jié)點(diǎn)數(shù)為13，故層次樹實(shí)際層數(shù)為5。

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

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

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

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

混合層次包圍盒樹采用由葉子節(jié)點(diǎn)到根節(jié)點(diǎn)的更新順序。 混合層次包圍盒樹葉子節(jié)點(diǎn)的AABB 和球體及膠囊體包圍盒在模型導(dǎo)入階段已經(jīng)創(chuàng)建完成，在機(jī)械臂運(yùn)動(dòng)期間通過旋轉(zhuǎn)平移變換矩陣實(shí)現(xiàn)葉子節(jié)點(diǎn)包圍盒的更新。 其余層次的AABB 更新則是通過下一層次的左、右子節(jié)點(diǎn)的AABB 來創(chuàng)建上一層次的父節(jié)點(diǎn)的AABB，逐層創(chuàng)建，直到創(chuàng)建完成最高層次根節(jié)點(diǎn)的AABB，則實(shí)現(xiàn)了整個(gè)混合層次包圍盒樹的更新。

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

更新過程總體示意圖如圖9 所示，通過4 號(hào)和5 號(hào)葉子節(jié)點(diǎn)的AABB 創(chuàng)建2 號(hào)節(jié)點(diǎn)的AABB，再通過6 號(hào)和7 號(hào)葉子節(jié)點(diǎn)的AABB 創(chuàng)建3 號(hào)節(jié)點(diǎn)的AABB，最后由2 號(hào)和3 號(hào)節(jié)點(diǎn)的AABB 創(chuàng)建1 號(hào)根節(jié)點(diǎn)的AABB，完成混合層次包圍盒樹的更新。

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

4.3 混合層次包圍盒樹相交檢測(cè)

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

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

采用同層優(yōu)先的遍歷算法，即針對(duì)未發(fā)生碰撞的部位，只要父節(jié)點(diǎn)沒有檢測(cè)到相交，則跳到同層次的節(jié)點(diǎn)進(jìn)行下一次相交檢測(cè)，能夠快速排除未相交的部位，大幅減少相交檢測(cè)時(shí)間，實(shí)現(xiàn)碰撞部位的快速定位。 同時(shí)，在機(jī)械臂距離艙體較遠(yuǎn)時(shí)，能夠?qū)⒈闅v過程鎖定在AABB 層的前幾層，減少大量無意義的相交檢測(cè)計(jì)算。

4.4 碰撞檢測(cè)流程

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

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

5 試驗(yàn)驗(yàn)證

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

圖12 水下訓(xùn)練環(huán)境和操控臺(tái)Fig.12 Underwater training environment and console

將本文碰撞檢測(cè)算法集成到自主研發(fā)的水下訓(xùn)練機(jī)械臂操控系統(tǒng)，該操控系統(tǒng)利用OpenGL和MFC 實(shí)現(xiàn)虛擬環(huán)境和人機(jī)交互，集成了機(jī)械臂運(yùn)動(dòng)學(xué)模型，能夠?qū)崿F(xiàn)機(jī)械臂及艙體模型的導(dǎo)入及位置標(biāo)定、機(jī)械臂的仿真運(yùn)動(dòng)軌跡規(guī)劃以及機(jī)械臂模型跟隨機(jī)械臂實(shí)體進(jìn)行同步運(yùn)動(dòng)等功能，操控臺(tái)控制器運(yùn)行虛擬平臺(tái)代碼，機(jī)械臂及艙體模型導(dǎo)入后通過本文算法自動(dòng)創(chuàng)建包圍盒樹并在模型上顯示球體及膠囊體包圍盒，效果圖如圖13所示。

圖13 模型導(dǎo)入效果圖Fig.13 Effect drawing of model import

5.1 精度驗(yàn)證

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

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

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

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

5.2 實(shí)時(shí)性驗(yàn)證

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

圖14 4 種算法計(jì)算時(shí)間對(duì)比Fig.14 Comparison of calculation time of four algorithms

表3 算法計(jì)算時(shí)間統(tǒng)計(jì)分析Table 3 Statistical analysis of algorithm calculation time

5.3 準(zhǔn)確性驗(yàn)證

當(dāng)將包圍盒碰撞檢測(cè)閾值設(shè)置為100 mm時(shí)，圖15 為機(jī)械臂末端航天員訓(xùn)練用操作臺(tái)與艙體之間產(chǎn)生碰撞時(shí)的位置，可以看到操作臺(tái)包圍盒在此時(shí)做出了碰撞反應(yīng)，操作臺(tái)模型及其包圍盒變成了紅色，此時(shí)計(jì)算得到的機(jī)械臂與艙體包圍盒之間的最小距離dmin為68.00 mm，小于碰撞檢測(cè)的閾值100 mm，說明包圍盒已經(jīng)產(chǎn)生了相交。

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

通過在機(jī)械臂末端安裝超聲測(cè)距傳感器，令機(jī)械臂末端以水平姿態(tài)，沿豎直方向接近艙體，測(cè)量機(jī)械臂運(yùn)動(dòng)過程中機(jī)械臂末端與艙體的最小距離。 結(jié)果如圖16、圖17 所示。 圖16 為通過算法計(jì)算得到的機(jī)械臂包圍盒與艙體包圍盒之間的最小距離和通過傳感器實(shí)際測(cè)得的最小距離，圖17為本文算法得出的碰撞信號(hào)和通過實(shí)際距離確定的碰撞信號(hào)。 圖17 中出現(xiàn)一個(gè)周期的延遲是由于傳感器測(cè)距本身存在誤差。 結(jié)合圖16、圖17可得，當(dāng)將碰撞檢測(cè)距離設(shè)置為100 mm 時(shí)，在產(chǎn)生碰撞信號(hào)的部位，其最小距離均小于100 mm，故本文算法可以準(zhǔn)確定位碰撞部位，滿足碰撞檢測(cè)準(zhǔn)確性要求。

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

圖17 機(jī)械臂碰撞檢測(cè)信號(hào)Fig.17 Collision detection signal of manipulator

6 結(jié)論

針對(duì)航天員水下訓(xùn)練超大作業(yè)空間人機(jī)安全問題，提出一種快速準(zhǔn)確的碰撞檢測(cè)方法。

1)實(shí)現(xiàn)簡(jiǎn)化模型膠囊體包圍盒自動(dòng)生成；

2)建立混合層次包圍盒樹，通過層次優(yōu)先的遍歷能夠?qū)崿F(xiàn)碰撞部位的快速篩選以及準(zhǔn)確定位，計(jì)算時(shí)間較其他算法縮短了45%以上；

3)將該方法集成于自主研發(fā)的水下訓(xùn)練機(jī)械臂操控系統(tǒng)，并在長(zhǎng)時(shí)間的運(yùn)行中證明了算法的穩(wěn)定性及可靠性。

本文算法通用性強(qiáng)，也可運(yùn)用于其他類型的機(jī)械臂或者其他工業(yè)設(shè)備的碰撞檢測(cè)。