[發明專利]一種機械臂碰撞檢測方法

有效

• 申請號: 202010892066.1
• 申請日: 2020-08-28
• 公開（公告）號: CN112179602B
• 公開（公告）日: 2021-06-25
• 發明（設計）人: 陳鋼;李俊杰;曾曉旭;王一帆;邵文
• 申請（專利權）人: 北京郵電大學
• 主分類號: G01M7/08
• 分類號: G01M7/08
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種機械臂碰撞檢測方法，其特征在于，所述方法包括：

利用表面柵格化技術分別對虛擬仿真場景模型和機械臂模型進行預處理，獲得虛擬仿真場景模型對應的點云文件和機械臂模型對應的點云文件；

利用八叉樹分割技術對所得點云文件分別進行空間分割，以確定每個點云所處的子空間位置，得到點云文件的八叉樹空間；

利用球包圍盒和方向包圍盒對所得八叉樹空間進行層次包裹，得到混合包圍盒樹；

利用深度優先搜索算法和雙向廣度搜索算法，搜索混合包圍盒樹，對混合包圍盒樹的方向包圍盒進行相交計算，得到機械臂單一構型碰撞檢測結果；

在虛擬仿真場景中，對機械臂單一構型與仿真場景做單次碰撞檢測時，搜索機械臂與仿真場景兩棵混合包圍盒樹，判斷兩棵混合包圍盒樹中是否存在相交子節點；

當兩棵混合包圍盒樹中除根節點外所有葉子節點數量之差大于第二指定值時，對兩棵混合包圍盒樹中高度較小的包圍盒樹進行深度優先搜索，搜索是否存在相交子結點，得到機械臂單一構型碰撞檢測結果；

當兩棵混合包圍盒樹中除根節點外所有葉子節點數量之差小于第二指定值時，對兩棵混合包圍盒樹進行雙向廣度搜索，搜索是否存在相交子結點，得到機械臂單一構型碰撞檢測結果；

利用機械臂運動特性、動態規劃算法和上述機械臂單一構型檢測方法，得到機械臂連續運動時碰撞檢測結果；

當機械臂處于運動初始時刻時，對機械臂做機械臂單一構型碰撞檢測，并保存此次碰撞檢測結果，所述碰撞檢測結果包括碰撞結果、相交點云索引、最小點云距離、碰撞深度；

當機械臂處于非運動初始時刻時，若上一時刻不發生碰撞且最小點云距離大于第三指定值時，判斷出當前時刻不發生碰撞；

或者，當機械臂處于非運動初始時刻時，若上一時刻不發生碰撞且最小點云距離小于第三指定值時，判斷出當前時刻可能發生碰撞，在當前狀態下對機械臂做機械臂單一構型碰撞檢測，得到當前時刻的碰撞結果；

或者，當機械臂處于非運動初始時刻時，若上一時刻發生碰撞且碰撞深度大于第四指定值時，判斷出當前時刻發生碰撞；

或者，當機械臂處于非運動初始時刻時，若上一時刻發生碰撞且碰撞深度小于第四指定值時，判斷出當前時刻可能發生碰撞，在當前狀態下對機械臂做機械臂單一構型碰撞檢測，得到當前時刻的碰撞結果；

或者，當機械臂運動過程中所有時刻都不發生碰撞時，判斷出該次運動過程中無碰撞，反之，該次運動過程中發生了碰撞，獲取發生碰撞的碰撞時刻；

其中，第三指定值大于第四指定值。

2.根據權利要求1所述的碰撞檢測方法，其特征在于，所述利用表面柵格化技術分別對虛擬仿真場景模型和機械臂模型進行預處理，包括：

利用OSG三維引擎的線段求交方法及預設的掃描精度，對虛擬仿真場景模型的表面進行散點采樣，得到虛擬仿真場景模型對應的散點的點云文件；

利用OSG三維引擎的線段求交方法及預設的掃描精度，對機械臂模型的表面進行散點采樣，得到機械臂模型對應的散點的點云文件。

3.根據權利要求1所述的碰撞檢測方法，其特征在于，所述利用八叉樹分割技術對所得點云文件分別進行空間分割，以確定每個點云所處的子空間位置，得到點云文件的八叉樹空間之前，還包括：

判斷待分割空間的點云數量是否小于第一指定數量；

如果空間內點云數量小于第一指定數量，不再對該空間進行分割；

如果空間內點云數量不小于第一指定數量，執行利用八叉樹分割技術對所得點云文件分別進行空間分割的步驟。

4.根據權利要求3所述的碰撞檢測方法，其特征在于，八叉樹的根節點是點云文件的全部點云；所述利用八叉樹分割技術對所得點云文件分別進行空間分割，已確定每個點云所處子空間位置，得到點云文件的八叉樹空間，包括：

從八叉樹根節點開始，遞歸向下進行平均分割，平均分割得到的八個子空間為根節點的子節點，所有子節點包含子空間內全部點云；

當節點滿足繼續分割條件，對節點進行空間分割，直到節點不滿足分割條件為止。

5.根據權利要求1所述的碰撞檢測方法，其特征在于，所述利用球包圍盒和方向包圍盒對所得八叉樹空間進行層次包裹，得到混合包圍盒樹包括：

使用球包圍盒與方向包圍盒兩種包圍盒技術，對八叉樹空間進行包裹，從八叉樹根節點開始向下構建混合包圍盒樹，混合包圍盒樹緊密包裹所有點云；

其中，使用球包圍盒對八叉樹根節點進行包裹，成為混合包圍盒樹的根節點，球包圍盒的構建公式為：

R＝{(x,y,z)|(x-o_x)²+(y-o_y)²+(z-o_z)²r²}

(o_x,o_y,o_z)表示球包圍盒的球心坐標，r表示球包圍盒的球體半徑，(x,y,z)表示所有點云坐標集合，R表示點云文件所有點云集合；

其中，使用方向包圍盒對八叉樹空間所有子節點包含的點云進行包裹，成為混合包圍盒樹的子節點，方向包圍盒的構建公式為：

M＝{C+ar₁v₁+br₂v₂+cr₃v₃|a,b,c∈(-1,1)}

C點為包圍盒的中心點，v₁,v₂,v₃為過C點且互相垂直的三個向量，r₁,r₂,r₃分別為C點在v₁,v₂,v₃三個方向上的延伸長度，a,b,c為三個向量預設標值，M表示子節點內所有點云集合。