[發明專利]一種空間機器人末端接觸碰撞力計算方法

有效

• 申請號: 201710700714.7
• 申請日: 2017-08-16
• 公開（公告）號: CN107609222B
• 公開（公告）日: 2020-10-23
• 發明（設計）人: 張海博;何英姿;魏春嶺;魏承;王勇;徐拴鋒
• 申請（專利權）人: 北京控制工程研究所
• 主分類號: G06F30/20
• 分類號: G06F30/20;G06F30/17;G06F119/14
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 范曉毅

1.一種空間機器人末端接觸碰撞力計算方法，其特征在于，所述方法包括如下步驟：

步驟1：將捕獲裝置及被捕獲裝置的機械設計三維CAD模型進行簡化，基于簡化模型分別對捕獲裝置及被捕獲裝置的表面建立三角片元化模型；

步驟2：對三角片元化模型進行特征幾何要素的提取以及對捕獲裝置及被捕獲裝置發生碰撞區域的點、線、面特征標定及提取，得到能夠用于快速碰撞檢測的捕獲裝置及被捕獲裝置的幾何特征模型；

步驟3：根據捕獲裝置及被捕獲裝置的幾何特征模型，采用碰撞檢測算法對捕獲裝置和被捕獲裝置進行碰撞檢測，得到捕獲裝置及被捕獲裝置的碰撞點位置及嵌入深度量；

步驟4：由捕獲裝置及被捕獲裝置的碰撞點位置及嵌入深度量，建立捕獲裝置和被捕獲裝置的碰撞模型；

步驟5：根據捕獲裝置和被捕獲裝置的碰撞模型，通過碰撞嵌入量和碰撞嵌入速度得到碰撞力；

步驟6：根據捕獲裝置及被捕獲裝置的碰撞模型，通過碰撞點嵌入速度的切向速度、碰撞力和摩擦系數得到接觸摩擦力；

步驟7：結合步驟5中的碰撞力和步驟6中的接觸摩擦力得到碰撞力模型；其中，

在步驟5中，碰撞力的公式如下：

其中，F_c為碰撞力，K_c為接觸碰撞剛度系數，n為接觸點公法線方向，δ為沿接觸點公法線方向n的碰撞嵌入量，C_c為接觸碰撞阻尼系數，為接觸點的碰撞嵌入速度；

在所述步驟6中，接觸摩擦力的公式如下：

f_t＝μ(|v_t|)F_c，

其中，v_t為碰撞點嵌入速度的切向速度，f_t為接觸摩擦力，μ為摩擦系數，F_c為碰撞力。

2.根據權利要求1所述的空間機器人末端接觸碰撞力計算方法，其特征在于：在所述步驟1中，三角片元為根據一個表面上的相鄰三個節點的坐標定義一個三角片。

3.根據權利要求1所述的空間機器人末端接觸碰撞力計算方法，其特征在于：在所述步驟2中，對三角片元化模型進行特征幾何要素的提取包括對捕獲裝置的三角片元化模型進行點、線和面的提取和對被捕獲裝置的三角片元化模型進行點、線和面的提取。

4.根據權利要求1所述的空間機器人末端接觸碰撞力計算方法，其特征在于：在所述步驟3中，碰撞檢測算法為采用沿坐標軸的包圍盒AABB碰撞檢測算法。

5.根據權利要求4所述的空間機器人末端接觸碰撞力計算方法，其特征在于：包圍盒AABB碰撞檢測算法包括：

首先判斷2棵樹的根節點的包圍盒是否相交，如果不相交，那么這2個物體肯定不發生碰撞；否則遞歸遍歷2棵樹，對相應的子節點進行求交判斷；

如果參與求交運算的2個子節點中至少有一個為非葉節點，則檢測2節點的包圍盒是否相交，如果不相交則不必再判斷子節點；

若2節點均為葉節點，且檢測出它們的包圍盒相交，則還要進一步判斷這2個包圍盒內所包含的多邊形是否發生碰撞，如果相交檢測都判斷完，沒有發現相交的情況，則2個物體不碰撞；

若檢測到2個物體發生碰撞，則記錄捕獲裝置和被捕獲裝置的碰撞點位置及嵌入深度量。

6.根據權利要求1所述的空間機器人末端接觸碰撞力計算方法，其特征在于：在所述步驟7中，得到碰撞力模型包括：

將碰撞力與接觸摩擦力進行求和得到碰撞力的數學模型，即得到捕獲裝置與被捕獲裝置的碰撞力模型。