[發明專利]一種五自由度機械手控制方法

有效

• 申請號: 201610443388.1
• 申請日: 2016-06-17
• 公開（公告）號: CN106003041B
• 公開（公告）日: 2018-01-30
• 發明（設計）人: 楊亮亮;劉權慶;時軍;歐陽博;應思齊;胡鑫杰
• 申請（專利權）人: 浙江理工大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 浙江翔隆專利事務所(普通合伙)33206
• 代理人: 戴曉翔

技術領域

本發明涉及一種五自由度機械手控制方法，屬于機械設備控制領域。

背景技術

機械手能模仿人手和臂的某些動作功能，可以按固定程序抓取、搬運物件或操作工具的自動操作裝置。機械手是最早出現的工業機器人，也是最早出現的現代機器人，它可代替人的繁重勞動以實現生產的機械化和自動化，能在有害環境下操作以保護人身安全，因而廣泛應用于機械制造、冶金、電子、輕工和原子能等部門。它可通過編程來完成各種預期的作業任務，在構造和性能上兼有人和機器各自的優點，尤其體現了人的智能和適應性。機械手作業的準確性和各種環境中完成作業的能力，在國民經濟各領域有著廣闊的發展前景。但是機械手的控制方法比較繁瑣，需要預先獲取機械臂末端運動軌跡曲線，根據軌跡曲線反算各個機械手關節的轉動量，將上述得到的轉動量下發至運動控制卡，通過伺服驅動系統驅動各臂轉動，各連桿聯動完成給定的運動軌跡。但是現有技術的控制方法繁瑣，控制精度不高，不能有效控制機械手的運動。針對目前現有技術中存在的上述缺陷，實有必要進行研發，解決現有技術中存在的缺陷；

發明內容

針對現有技術的缺陷，本發明的目的在于提供一種控制方法簡單、控制精度高的五自由度機械手控制方法。

為實現上述目的，本發明的技術方案為：

一種五自由度機械手控制方法，包括如下步驟：

S1.獲取機械臂末端運動軌跡曲線，根據曲線特點以及要求選取N個末端位置坐標P(px，py，pz)和接近矢量a(ax，ay，az)，方位矢量o(ox，oy，oz)，法向矢量n(nx，ny，nz)；

S2.將獲取的N個點進行運動學反解的計算，得到的每一個軌跡坐標點相對應的各個驅動關節所對應的一個角度值，獲得機械手的運動軌跡與各關節驅動角度之間的一個關系，五個關節角度分別是θ₁，θ₂，θ₃，θ₄，θ₅；

S3.由于反解存在多解的情況，故而需要對反解得到的各個解進行分析和判斷，進而選取合理的解，但是取解的情況需要考慮到實際的工作情況和人為的需求；

根據基座的運動范圍，將機械手分為左臂與右臂，左臂的運動范圍是：0°～180°，右臂的運動范圍是：-180°～0°；其次，根據小臂的運動范圍將機械臂分為上肘θ₃＜90°與下肘θ₃≥90°；最后，對于最后兩個決定末端姿態的驅動關節將其分為翻轉-180°～0°與非翻轉0°～180°；

因此，可將反解的多解情況分為八組然后選擇其合理的一組，分別是：左臂上肘翻轉、左臂上肘非翻、左臂下肘翻轉、左臂下肘非翻、右臂上肘翻轉、右臂上肘非翻、右臂下肘翻轉、右臂下肘非翻；

S4.將上述得到的轉動角度下發至運動控制卡，通過伺服驅動系統驅動各臂轉動，通過傳感器檢測并反饋轉動信號，各連桿聯動完成給定的運動軌跡。

進一步地，左臂上肘翻轉、左臂上肘非翻、左臂下肘翻轉、左臂下肘非翻、右臂上肘翻轉、右臂上肘非翻、右臂下肘翻轉、右臂下肘非翻，

每一組的情況都是通過坐標的旋轉、平移得到的齊次變換矩陣：

進一步地，

a)首先解出θ₁，可用逆變換左乘方程兩邊來求出θ₁：

即可通過上式解出該位置坐標下P(px，py，pz)的關節角度θ₁；

b)再通過上面的轉換矩陣，可得關節角度θ₃的求解方程：

其中l₂，d₄分別表示的是大臂和小臂的長度，px、py、pz表示的是在對應角度下的位置坐標；

c)通過在齊次變換矩陣方程兩邊左乘逆變換可求得：

代入之前求解得到的第一關節和第三關節的轉動角度，通過上式解得第二關節和第三關節的轉動角度之和；

d)根據θ₂＝θ₂₃-θ₃，得到相對應的坐標下的第二關節的轉動角度值；

e)在步驟c得到的矩陣中，解得：

求得的在相應坐標下的第四關節的轉動角度；

f)在齊次變換矩陣兩邊同時乘以得到第五關節的轉動角度：

求得的在相應坐標下的第五關節的轉動角度。