[發明專利]一種隨動型機械臂的控制方法

有效

• 申請號: 202011046836.7
• 申請日: 2020-09-29
• 公開（公告）號: CN111890372B
• 公開（公告）日: 2021-01-15
• 發明（設計）人: 林嘉;辜紀文;辜正濤
• 申請（專利權）人: 江西明天高科技股份有限公司;南昌嘉研科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J3/00
• 代理公司: 北京清亦華知識產權代理事務所(普通合伙) 11201
• 代理人: 彭琰

1.一種隨動型機械臂的控制方法，其特征在于，所述隨動型機械臂包括肩部組件、上臂、前臂；所述肩部組件包括底座、旋肩電機、旋肩傳動結構、展臂電機、展臂傳動結構、屈肘電機、屈肘傳動結構，所述旋肩電機、所述展臂電機、所述屈肘電機均配備轉子角度編碼器；

所述旋肩電機固定在所述底座上，所述旋肩傳動結構與所述旋肩電機連接，所述展臂電機和所述屈肘電機安裝在所述旋肩傳動結構上，所述旋肩電機用于帶動所述隨動型機械臂除所述底座和所述旋肩電機以外的部分繞豎直的旋肩軸旋轉；

所述展臂傳動結構連接在所述展臂電機的轉子和所述屈肘電機的定子之間，所述展臂電機用于帶動所述上臂和所述前臂繞水平的展臂軸旋轉；

所述上臂的基部與所述屈肘電機的定子連接，所述前臂的一端與所述上臂鉸接于肘關節處，并與所述屈肘傳動結構連接，所述前臂的另一端延伸至持握點；

所述屈肘電機與所述屈肘傳動結構連接，所述屈肘傳動結構用于將所述屈肘電機的動力輸出到所述肘關節，所述屈肘電機用于帶動所述前臂繞水平的肘關節軸旋轉；

所述旋肩軸和所述展臂軸在空間的交點為所述隨動型機械臂的旋轉中心，在所述旋肩電機、所述展臂電機、所述屈肘電機的聯合帶動下，所述持握點可到達一個球殼范圍內的任意位置，球殼的外徑等于所述上臂與所述前臂的長度之和，球殼內徑等于所述上臂與所述前臂長度之差的絕對值，當使用者持握所述持握點時，所述隨動型機械臂在使用者手部的移動帶領下做隨行運動，并同時向人手施加反力；

所述控制方法包括：

獲取所述旋肩電機、所述展臂電機、所述屈肘電機的輸出角；

根據所述旋轉中心的位置、所述旋肩電機的輸出角、所述上臂的長度、所述展臂電機的輸出角，計算所述肘關節的位置；

根據所述展臂電機的輸出角、所述屈肘電機的輸出角計算所述前臂與水平面之間的前臂方向角；

根據所述肘關節的位置、所述前臂方向角、所述前臂的長度計算所述持握點的位置；

根據所述持握點的位置計算所述持握點到所述旋肩電機、所述展臂電機、所述屈肘電機的力臂矢量；

根據各個所述力臂矢量計算所述旋肩電機、所述展臂電機、所述屈肘電機向合力作用點輸出分力的方向矢量；

根據各個所述方向矢量以及需要生成的反饋力合力矢量，計算出所述旋肩電機、所述展臂電機、所述屈肘電機的分力大小；

根據各個所述分力大小計算所述旋肩電機、所述展臂電機、所述屈肘電機的輸出力矩。

2.根據權利要求1所述的隨動型機械臂的控制方法，其特征在于，根據所述旋轉中心的位置、所述旋肩電機的輸出角、所述上臂的長度、所述展臂電機的輸出角，計算所述肘關節的位置的步驟中，采用下式計算所述肘關節的位置：

其中，α₁為所述旋肩電機的輸出角，α₂為所述展臂電機的輸出角，[x₁,y₁,z₁]為所述旋轉中心的位置，L_ua為所述上臂的長度，[x₂,y₂,z₂]為所述肘關節的位置；

根據所述展臂電機的輸出角、所述屈肘電機的輸出角計算所述前臂與水平面之間的前臂方向角的步驟中，采用下式計算所述前臂方向角：

其中，A₃為所述前臂方向角，α₃為所述屈肘電機的輸出角。