一種6自由度機械手自主抓取逆解工程算法
【技術領域】
[0001] 本發明屬于機器人控制技術領域，具體涉及一種6自由度機械手自主抓取逆解工 程算法。
【背景技術】
[0002] 隨著科學技術不斷發展，以及機器人取代人工的領域迅速增加，各類機器人的研 制已成為世界各國和軍隊共同關注的課題。由于機器人的作業目標質量狀況不明，放置的 位置隨機性較大，抓取過程中隨時有意外發生的可能，因此，采用機器人進行作業，降低處 理難度，避免工作人員的意外發生，對提高科技整體技術水平和作業效率具有重要意義。
[0003] 世界上已有的車載機械手型機器人包括履帶式"手推車"、"超級手推車" MPR-800 型多功能智能機器人、"安德魯斯HD-I"機器人、"靈蜥A"和"靈蜥B"排爆機器人、 "RAPT0P-E0D"中型排爆機器人等。而控制系統是機器人中至關重要的一部分，控制系統的 核心是逆運動算法，其好壞程度直接影響著機器人的功能、可靠程度及操作性能等。
[0004] 針對機器人作業環境的特殊情況，可靠性和效率是它的一個重要因素。如果機器 人的控制系統不可靠，將會引入新的不安全因素，不但沒能解決問題，反而會促發矛盾問題 的升級。在6自由度的機械手操作時，采用單關節的遠程遙控方式，這對機械手末端手爪的 定位抓取造成了極大困難。該種方式對操作人員的訓練程度要求高且造成的精神負擔較 大，容易疲勞而且操作效率較低。如果采用逆矩陣求解方法，不僅計算復雜，而且計算過程 中多解可能性不斷出現，必須靠約束條件去掉增根，且求解不直觀。
【發明內容】
[0005] 本發明針對現有技術中機械手的控制系統采用單關節的遙控方式，導致機械手末 端定位抓取困難，造成操作人員的精神負擔較大，容易疲勞而且操作效率較低等問題，提供 一種能夠實現機械手多關節聯動的逆運動求解工程算法。
[0006] 本發明通過以下技術方案實現該目的：
[0007] -種6自由度機械手自主抓取逆解工程算法，包括以下步驟：
[0008] (1)根據實際的目標圖像判斷抓取目標點（X，Y，Z)，且不考慮目標姿態；
[0009] (2)根據機械手的正解方法，設定6自由度機械手的各關節參數為 (^，9 2, 93, 94, 95, ％)，第4、5、6關節依靠人工調整，因此求出目標點與前3個關節的 關系，即固定后三個關節的初始角度；
[0010] (3)求逆解時先轉腰，求出0 i，后面的求解工程為平面求解問題；
[0011] ⑷計算 02，03。
[0012] 進一步，所述步驟（1)中，目標點P(X，Y，Z)由人工設定或者由機器人視覺提供。
[0013] 進一步，所述步驟（2)中，不考慮最后3個關節的旋轉，將目標點P(X，Y，Z)定位 于機械手爪中心。在機械手臂上固定一個平面坐標系Ixy}，這是為了將空間坐標系的問題 工程到平面坐標系解決，減少了變量，使得空間變量求解更加簡單。該平面坐標系Ixy}的 原點與空間基坐標系{XYZ}的原點重合，即固定在機器人的基坐標系的原點；平面坐標系 {xy}的y軸與空間坐標系{XYZ}的Z軸重合。
[0014] 無論機械臂繞腰關節怎么旋轉，坐標系{xy}始終與機械手處于同一平面內，該坐 標系是輔助的動態平面坐標系，那么點P在該坐標系Ixy}中的坐標記為（x，y)，根據幾何 學知識可以求得該坐標如下：
[0015]
[0016] 最終要求得點P在機器人基坐標系{XYZ}中的坐標（X，Y，Z)，點P在平面動坐標系 Ixy}與機器人基坐標系{X，Y，Z}滿足以下關系式
，所以點P在機器人基坐標 系下的坐標關
[0017] 進一步，所述步驟（3)、（4)中，已知機器人末端的三維坐標（X，Y，Z)的值，求解關 節9 1，9 2，9 3的值。
[0018] 在機器人基坐標系下可以解的0 i的表達式為：0 i=arctan(Y/X)(根據機械臂 的空間位置信息求取角度）；
[0019] 由正解的結論可得
[0020] X2+Y2+Z2=L 12+L22+2L1L 2(cos( 93-9 2)cos(92)_sin( 93-9 2)sin( 9 2)),
[0021 ] X2+Y2+Z2= L 12+L22+2L1L 2cos ( 9 3- 9 2~*~ ?2) = L2
[0022] 可得◎
[0023] 利用三角形的余弦定理可解得
f，根據實際可取
[0024] 將包含9 2, 9 3的y-0-x平面
[0025]
[0026]
[0027] 其中，求出0i，0 2, 0 3后，根據目標姿態，人工調整手爪姿態，實現合理位姿抓取。
[0028] 相對于現有技術，本發明的有益效果為：本發明的6自由度機械手自主抓取逆解 工程算法，在求取各關節角度時，采用先轉腰且不考慮手爪姿態，再求取其他關節角度的方 法，先求出腰部回轉角度，進而轉化為平面問題進行求解其他關節角度，大大提高了求解效 率，并且避免了逆矩陣求解方式帶來的復雜性，以及操作人員的精神負擔較大、容易疲勞等 問題，進一步提高了抓取效率。
【附圖說明】
[0029] 圖1是本發明的末端不包括擺動關節的機械手結構示意圖。
[0030] 圖2是本發明的6自由度機械手運動學正解幾何圖解示意圖。
[0031] 圖3是本發明的工程成平面的關節參數求解示意圖。
【具體實施方式】
[0032] 以下結合附圖及具體實施例對本發明進行詳細描述。
[0033] 實施例1。
[0034] 一種6自由度機械手自主抓取逆解工程算法包括以下步驟：
[0035] (1)根據實際的目標圖像判斷抓取目標點（X，Y，Z)，且不考慮目標姿態；
[0036] (2)根據機械手的正解方法，設定6自由度機械手的各關節參數為 (^，9 2, 93, 94, 95, ％)，第4、5、6關節依靠人工調整，因此求出目標點與前3個關節的 關系，即固定后三個關節的初始角度；
[0037] (3)求逆解時先轉腰，求出0 i，后面的求解工程為平面求解問題；
[0038] (4)計算 02,03。
[0039] 進一步，所述步驟（1)中目標點P(X，Y，Z)由人工設定或者由機器人視覺提供。
[0040] 進一步，所述步驟（2)中，不考慮最后3個關節的旋轉，將目標點P(X，Y，Z)定位手 爪中心，如圖1所示。在機械手臂上固定一個平面坐標系{xy}，這是為了將空間坐標系的問 題工程到平面坐標系解決，減少了變量，使得空間變量求解更加簡單。該平面坐標系Ixy} 的原點與空間基坐標系{XYZ}的原點重合，即固定在機器人的基坐標系的原點；平面坐標 系{xy}的y軸與空間坐標系{XYZ}的Z軸重合，坐標系的關系如圖2所示。
[0041] 無論機械臂繞腰關節怎么旋轉，坐標系Ixy}始終與機械手處于同一平面內，該坐 標系是輔助的動態平面坐標系，那么點P在該坐標系Ixy}中的坐標記為（x，y)，根據幾何 學知識可以求得該坐標如下：
[0042]
[0043] 最終要求得點P在機器人基坐標系{XYZ}中的坐標（X，Y，Z)，點P在平面動坐標系 X =^cos：0j Ixy}與機器人基坐標系{X，Y，Z}滿足以下關系式貧，所以點P在機器人基坐標 Z. - y 系下的坐標7
[0044] 進一步，所述步驟（3):
[0045] 已知機器人末端的三維坐標（X，Y，Z)的值，求解關節0 d 0 2, 0 3的值，多功能水 下爬行機器人運動學逆解幾何圖解圖如圖1所示。
[0046] 在機器人基坐標系下可以解的0 :的表達式0 arctan(Y/X)(根據機械臂的空 間位置信息求取角度）；
[0047] 由正解的結論可得
[0048] X2+Y2+Z2= L 12+L22+2L1L 2(cos( 93-9 2) cos (92) _sin ( 9 3- 9 2) sin (92))
[0049] ，
[0050] X2+Y2+Z2= L 12+L22+2L1L 2cos ( 9 3- 9 2~*~ ? 2) = L2
[0056] 其中，求出0i，0 2, 0 3后，根據目標姿態，人工調整手爪姿態，實現合理位姿抓取。
[0057] 以上所述實施例僅表達了本發明的幾種實施方式，其描述較為具體和詳細，但并 不能因此而理解為對本發明專利范圍的限制。應當指出的是，對于本領域的普通技術人員 來說，在不脫離本發明構思的前提下，還可以做出若干變形和改進，這些都屬于本發明的保 護范圍。因此，本發明專利的保護范圍應以所附權利要求為準。
【主權項】
1. 一種6自由度機械手自主抓取逆解工程算法，其特征在于，包括W下步驟： (1)根據實際的目標圖像判斷抓取目標點P狂，Y，幻，且不考慮目標姿態； 似根據機械手的正解方法，設定6自由度機械手的各關節參數為 (0 1，0 2, 0 3, 0 4, 0 5, 0 6)，第4、5、6關節依靠人工調整，因此求出目標點P化Y，口與前3 個關節的關系，即固定后=個關節的初始角度； (3) 求逆解時先轉腰，求出0 1的數值，后面的求解工程為平面求解問題； (4) 計算02,03的數值。2. 根據權利要求1所述的6自由度機械手自主抓取逆解工程算法，其特征在于，所述步 驟（1)中目標點P狂，Y，幻由人工設定或者由機器人視覺提供。3. 根據權利要求1所述的6自由度機械手自主抓取逆解工程算法，其特征在于，所述 步驟（2)中，不考慮最后3個關節的旋轉，將目標點P〇(，Y，幻定位于機械手爪中屯、，在機械 手臂上固定一個平面坐標系Ixy}，該平面坐標系{xy}的原點與空間基坐標系找Y幻的原點 重合，即固定在機器人的基坐標系的原點；平面坐標系Ixy}的y軸與空間坐標系找Y幻的 Z軸重合； 無論機械臂繞腰關節怎么旋轉，坐標系Ixy}始終與機械手處于同一平面內，該坐標系 是輔助的動態平面坐標系，那么點P在該坐標系Ixy}中的坐標記為（x，y)，求得該坐標如 下：最終要求得點P在機器人基坐標系找Y幻中的坐標狂，Y，Z)，點P在平面動坐標系{xy} 與機器人基坐標系化Y，幻滿足W下關系式斤W點P在機器人基坐標系下 的坐標為4. 根據權利要求1所述的6自由度機械手自主抓取逆解工程算法，其特征在于，所述步 驟（3)中，已知機器人末端的S維坐標化Y，幻的值，求解關節0 1，0 2, 9 3的值，根據機械 臂的空間位置信息求取角度，在機器人基坐標系下可W求得e1的表達式為： 白1=arctan(Y/X)； 由正解的結論可得0 2,e3的關系式為：5. 根據權利要求1所述的6自由度機械手自主抓取逆解工程算法，其特征在于，求出 0 1，0 2, 0 3后，根據目標姿態，人工調整手爪姿態，實現合理位姿抓取。6. 根據權利要求4所述的6自由度機械手自主抓取逆解工程算法，其特征在于，計算 0 2,e3不使用逆矩陣求解方法，采用先轉腰且不考慮手爪姿態，再求取其他關節角度的方 法。
【專利摘要】本發明涉及一種6自由度機械手求逆解算法，在求取各關節角度時，先求出腰部回轉角度，進而轉化為平面問題，有利于求解效率效率，并且避免了逆矩陣求解方式帶來的復雜性，以及操作人員的精神負擔較大、容易疲勞等問題，進一步提高了抓取效率。此外，本發明中的目標位置采用雙目視覺攝像單元獲取或者人工判斷，以供上位機控制系統確定所述可疑目標相對于所述車體的三維坐標，并反饋回相應的控制指令，可以進一步實現對目標的精確定位及抓取，從而也進一步提高了抓取效率。
【IPC分類】G06F17/50
【公開號】CN105005656
【申請號】CN201510408520
【發明人】蔣梁中, 張 成
【申請人】廣州霞光技研有限公司
【公開日】2015年10月28日
【申請日】2015年7月13日