[發明專利]機械臂控制方法、機械臂控制裝置及終端設備

有效

• 申請號: 201911330302.4
• 申請日: 2019-12-20
• 公開（公告）號: CN113001537B
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 林澤才;安昭輝;劉益彰;張美輝;熊友軍;龐建新
• 申請（專利權）人: 深圳市優必選科技股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/00
• 代理公司: 深圳中一聯合知識產權代理有限公司 44414
• 代理人: 李娟

1.一種機械臂控制方法，其特征在于，包括：

獲取機械臂的7個關節所分別對應的當前關節角度，其中，所述機械臂具有1個冗余自由度，所述7個關節分別控制所述機械臂在對應的7個自由度上運動，所述7個關節為第一關節、第二關節、第三關節、第四關節、第五關節、第六關節和第七關節；

根據機械臂的7個關節所分別對應的當前關節角度，獲得參考夾角值，其中，所述參考夾角值為參考平面與指定平面之間的夾角，所述參考平面為所述當前關節角度所對應的平面，所述指定平面為所述7個關節處于指定狀態時所對應的平面；

以所述參考夾角值為初始夾角值，并以預設角度間隔遍歷目標角度范圍，獲得所述目標角度范圍中的各個待評價夾角值，其中，所述目標角度范圍包括所述參考夾角值；

對于任一待評價夾角值，根據所述待評價夾角值和所述機械臂的末端的期望位姿，計算所述第四關節所對應的第一預估關節角度；

若所述第四關節所對應的第一預估關節角度大于第一預設角度閾值，則確定所述待評價夾角值所對應的待評價關節包括所述第六關節和所述第七關節；

若所述第四關節所對應的第一預估關節角度小于第一預設角度閾值，則計算所述第三關節所對應的第二預估關節角度和所述第五關節所對應的第三預估關節角度；

若所述第二預估關節角度與所述第三關節所對應的關節運動范圍的上限或者下限的第一差值小于第一差值閾值，并且，所述第三預估關節角度與所述第五關節所對應的關節運動范圍的上限或者下限的第二差值小于第二差值閾值，則確定所述待評價夾角值所對應的待評價關節包括所述第三關節和第五關節；

根據所述待評價夾角值，確定所述待評價夾角值所對應的待評價關節分別對應的第一關節角度；

根據各個第一關節角度和所述待評價夾角值所對應的待評價關節所分別對應的關節運動范圍，通過預設的關于夾角的評價函數獲得所述待評價夾角值所對應的評價值；

根據各個待評價夾角值所分別對應的評價值，確定在目標角度范圍內所述機械臂所對應的期望夾角值；

根據所述期望夾角值，確定所述7個關節分別對應的目標關節角度；

根據所述7個關節的目標關節角度，控制所述機械臂。

2.如權利要求1所述的機械臂控制方法，其特征在于，若每一個所述待評價關節所對應的第一關節角度都在對應的關節運動范圍內，則所述評價函數為：

若存在至少一個待評價關節所對應的第一關節角度不在對應的關節運動范圍內，則所述評價函數為：

其中，所述V_j為所述任一待評價夾角值ψ_j的評價值，q_i為所述待評價夾角值所對應的待評價關節中第i個關節的待評估關節角度，U_i為所述待評價夾角值所對應的待評價關節中第i個關節所對應的關節活動范圍，為所述第i個關節所對應的關節活動范圍的上限，為所述第i個關節所對應的關節活動范圍的下限。

3.如權利要求1或2所述的機械臂控制方法，其特征在于，所述根據所述期望夾角值，確定所述7個關節分別對應的目標關節角度，包括：

將所述期望夾角值輸入預設濾波器，獲得所述預設濾波器輸出的濾波后的期望夾角值；

根據所述濾波后的期望夾角值和所述機械臂的末端的期望位姿，計算所述7個關節分別對應的待驗證關節角度；

若所述7個關節分別對應的待驗證關節角度均在相應的關節運動范圍內，則將所述7個關節分別對應的待驗證關節角度作為所述7個關節分別對應的目標關節角度；

若所述7個關節中存在至少一個關節所對應的待驗證關節角度不在相應的關節運動范圍內，則根據濾波前的所述期望夾角值和所述機械臂的末端的期望位姿，計算得到所述7個關節分別對應的目標關節角度。

4.如權利要求3所述的機械臂控制方法，其特征在于，所述預設濾波器為卡爾曼濾波器，所述卡爾曼濾波器的觀測模型為：

X(K|K-1)＝X(K-1|K-1)

其中，X(K|K-1)指示當前時刻的預測夾角值，X(K-1|K-1)指示上一時刻的期望夾角值。