[發明專利]一種應用于六軸機械手臂的空間平滑停止控制方法

有效

• 申請號: 201710747184.1
• 申請日: 2017-08-25
• 公開（公告）號: CN107598921B
• 公開（公告）日: 2018-07-03
• 發明（設計）人: 許可;胡春旭;潘北川;黃邵波
• 申請（專利權）人: 深圳星河智能科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 深圳市順天達專利商標代理有限公司 44217
• 代理人: 郭偉剛

1.一種應用于六軸機械手臂的空間平滑停止控制方法，其特征在于，包括以下步驟：

測得并輸入六軸機械手臂開始減速的初始點信息，位置P₀，速度V₀，加速度A₀，并且假設六軸機械手臂經過時間段T，減速后停止點信息是速度為0，加速度為0，位置P_T；

采用五次方程式作為減速階段的位置插補算法，

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵, (1)

其中，t是減速時間段T內任一插補采樣時間點，P(t)為任一插補采樣時間點對應的位置，而a₀，a₁，a₂，a₃，a₄和a₅是系數；

假設位置曲線對稱，則在時間t＝0時，機械手臂處于初始狀態(P₀，V₀),

在時間t＝2T時，機械手臂處于狀態(P₀，-V₀)，

而在時間t＝T時，機械手臂處于狀態(P_T，0)；

在時間段T內任一插補采樣時間點t的位置P(t)、速度V(t)以及加速度A(t)如下，從而實現對六軸機械手臂的空間平滑停止控制：

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵, (2)

V(t)＝P’(t)＝a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴, (3)

A(t)＝P”(t)＝2a₂+6a₃t+12a₄t²+20a₅t³, (4)

其中，a₀＝P₀, (5)

a₁＝V₀， (6)

2.根據權利要求1所述的應用于六軸機械手臂的空間平滑停止控制方法，其特征在于，基于公式(2)，(3)和(4)得到如下約束條件：

P(0)＝P₀， (11)

P’(0)＝V₀， (12)

P”(0)＝A₀， (13)

P’(T)＝0， (14)

P”(T)＝0， (15)

P’(2T)＝-V₀， (16)

根據公式(11)，(12)，(13)，(14)，(15)和(16)聯立求解得到公式(5)，(6)，(7)，(8)，(9)和(10)。

3.根據權利要求2所述的應用于六軸機械手臂的空間平滑停止控制方法，其特征在于，在時間t＝T時，機械手臂處于狀態(P_T，0)，該狀態為減速到完全停止，速度為0的減速終止狀態。