一種面向SRS仿人手臂的自運動角計算方法技術

一種面向SRS仿人手臂的自運動角計算方法，它包括如下步驟：步驟一：建立SRS構型冗余度機械臂的數學模型，根據結構特點建立肩關節、肘關節、腕關節坐標系；步驟二：由當前狀態下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動角；步驟三：由步驟二的自運動角和工具坐標系位姿計算關節空間運動角度；步驟四：將步驟三的求解結果傳遞給下位機控制器，驅動機械臂實現給定工具坐標系位姿的運動。本發明專利技術給出了一種冗余度機械臂的逆運動學求法，和傳統的計算方法相比，算法簡單，計算速度快，可以得到解析解，在降低了求解難度的同時對關節運動做了一定的優化。

【技術實現步驟摘要】
一種面向SRS仿人手臂的自運動角計算方法
本專利技術涉及冗余度機械臂運動學及控制領域，具體來說，是一種面向SRS仿人手臂的自運動角計算方法。
技術介紹
機械臂是實現機器人系統主體與末端執行器連接的重要工具，機械臂的活動范圍、負載能力直接影響機器人末端的作業能力。冗余度機械臂的逆運動學問題是指已知機械臂末端位姿，確定機械臂的關節角度。給定冗余自由度機械臂的末端位姿，關節空間有無數個點與之對應，這些點的集合是關節空間的一個n-m維流形。冗余自由度機械臂可以在這個流形內自由變動而不影響末端位姿，這個變動稱為冗余自由度機器人的自運動。傳統的冗余自由度機械臂的逆運動學解法如梯度投影法耗時較長，或人為退化一個關節角轉化為無冗余自由度問題。
技術實現思路
為了解決上述問題，本專利技術提出一種面向SRS仿人手臂的自運動角計算方法。本專利技術通過以下技術方案予以實現：一種面向SRS仿人手臂的自運動角計算方法，它包括如下步驟：步驟一：建立SRS構型冗余度機械臂的數學模型，根據結構特點建立肩關節、肘關節、腕關節坐標系；步驟二：由當前狀態下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動角；步驟三：由步驟二的自運動角和工具坐標系位姿計算關節空間運動角度；步驟四：將步驟三的求解結果傳遞給下位機控制器，驅動機械臂實現給定工具坐標系位姿的運動。其中，步驟一所述“建立SRS構型冗余度機械臂的數學模型”，其實現過程如下：將測量肩關節中心到肘關節中心的距離和肘關節中心到腕關節中心的距離，其幾何模型為一個三動桿機構。其中，基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，所述SRS構型S為球副，R轉動副，文中所述中肩關節、肘關節、腕關節分別對應S球副、R轉動副、S球副。其中，基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，坐標系建立如下所述：將肩關節轉動中心、軸關節轉動中心的連線稱為大臂，肘關節轉動中心、腕關節轉動中心的連線稱為前臂。在初始狀態下肩關節坐標系Z軸與大臂平行，并由肩關節轉動中心指向肘關節轉動中心；Y軸平行于肘關節轉動軸線。肘關節坐標系Z軸與小臂平行，并由肘關節轉動中心指向腕關節轉動中心，Y軸為肘關節轉動軸線方向。腕關節處坐標系為肘關節坐標系沿前臂平移。其中，基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，參數自運動角θ的含義：腕關節中心點和執行末端姿態已知時，肩關節中心點、肘關節中心點和腕關節中心點組成的平面可以繞腕關節中心點矢量轉動，稱其為自運動平面，對應轉角稱為自運動角，并規定當工具坐標點為(x,y,z)時，自運動角為零時自運動平面的法矢方向為(-y,x,0)。其余自運動平面對應的自運動角大小為自運動平面法矢與自運動角為零時自運動平面法矢方向的夾角。其中，基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，其中步驟二中所述PID算法具體過程如下：1)由當前狀態的自運動角θn和給定的執行末端點的位姿確定各個關節運動角度；2)將步驟二計算得到的關節角度，計算腕關節姿態矩陣的Z軸與腕關節中心點矢量構成的平面與大臂小臂平面之間的夾角αn；3)新的自運動角θn+1由如下公式：θn+1＝θn+P*αn+I*(θn–θ0)求出；式中符號說明如下：θn+1為新的自運動角，θn為上一次計算得到的自運動角，θ0為初始狀態的自運動角，P、I為PI可調節參數。4)在內存中存儲自運動角θn+1。該算法需給定初始狀態時的自運動角θ0。由初始狀態時的自運動角θ0按照上面1)-4)所述遞推。其中，基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，由自運動角和執行末端位姿計算關節角度的步驟如下：步驟1)由腕關節中心點求出自運動角為0時大臂小臂的法向矢量N；若腕關節中心點為(x,y,z)，則上面所述矢量N為(-y,x,0)；步驟2)將步驟1)中所述矢量N繞腕關節中心點(x,y,z)轉動自運動角值θn得到大臂小臂平面的法矢量N1；步驟3)通過余弦定理求出肩關節中心點，肘關節中心點和腕關節中心點構成的三角形的三個角度，大臂與肩關節、腕關節連線的夾角用符號α表示，大臂小臂之間的夾角用符號β表示。步驟4)將腕關節中心點矢量繞步驟2)求出的法矢量N1轉動步驟3)求出的角度α，得出大臂的矢量方向AB，AB方向即肩關節轉動后的Z軸方向，步驟2)求出的法矢量N1為肩關節轉動后的Y軸，轉動后肩關節的坐標可以確定；步驟5)肘關節的轉動角度即步驟3)中所求角度π-β；步驟6)運動后腕關節的坐標是通過肩關節的旋轉矩陣乘以肘關節的旋轉矩陣，然后對兩矩陣的乘積求逆，最后將求得的逆矩陣乘以給定執行末端的姿態矩陣得到。本專利技術的優點在于：給出了一種冗余度機械臂的逆運動學求法，和傳統的計算方法相比，算法簡單，計算速度快，可以得到解析解，在降低了求解難度的同時對關節運動做了一定的優化。附圖說明圖1為初始狀態下SRS構型冗余度機械臂機構及坐標系建立示意圖；圖2為一般狀態下SRS構型冗余度機械臂機構及坐標系建立示意圖；圖3為實施本專利技術方法使用的冗余度機械臂結構示意圖；圖4(a)是給定直線軌跡下肩關節X支鏈的移動量軌跡圖圖4(b)是給定直線軌跡下肩關節Y支鏈的移動量軌跡圖圖4(c)是給定直線軌跡下肩關節Z軸轉動量軌跡圖圖4(d)是給定直線軌跡下肘關節轉動量軌跡圖圖4(e)是給定直線軌跡下腕關節X支鏈的移動量軌跡圖圖4(f)是給定直線軌跡下腕關節Y支鏈的移動量軌跡圖圖4(g)是給定直線軌跡下腕關節關節Z軸轉動量軌跡圖圖5(a)是給定圓形軌跡下肩關節X支鏈的移動量軌跡圖圖5(b)是給定圓形軌跡下肩關節Y支鏈的移動量軌跡圖圖5(c)是給定圓形軌跡下肩關節Z軸轉動量軌跡圖圖5(d)是給定圓形軌跡下肘關節轉動量軌跡圖圖5(e)是給定圓形軌跡下腕關節X支鏈的移動量軌跡圖圖5(f)是給定圓形軌跡下腕關節Y支鏈的移動量軌跡圖圖5(g)是給定圓形軌跡下腕關節關節Z軸轉動量軌跡圖圖6是本專利技術流程框圖具體實施方式下面將結合附圖和實例對本專利技術作進一步的詳細說明。1、圖3所示為實現本專利技術所使用的機械臂，該機械臂為SRS構型七自由度機械臂。該機械臂肩關節轉動中心到肘關節轉動中心軸線的距離l1＝270，肘關節轉動中心到腕關節轉動中心的距離l2＝230。2、圖4(a-g)、5(a-g)為實現本專利技術的冗余度機械臂逆運動學關節運動曲線示意圖。在這個具體示例中，機械臂的任務是畫一條圓弧和一條直線。根據插值點的位姿，利用本專利技術所述逆運動學方法求出各關節的運動角度，將計算得到的結果傳送給機械臂控制器，從而控制機械臂完成末端軌跡任務。其中參數自運動角θ表示在給定工具坐標系的位姿時，冗余度機械臂可以繞肩關節中心點和腕關節中心點的連線轉動的角度。腕關節中心點和執行末端姿態已知時，肩關節中心點、肘關節中心點和腕關節中心點組成的平面可以繞腕關節中心點矢量轉動，稱其為自運動平面，對應轉角稱為自運動角，并規定當工具坐標點為(x,y,z)時，自運動角為零時自運動平面的法矢方向為(-y,x,0)。自運動角的大小為其余自運動平面對應的自運動角大小為自運動平面法矢與自運動角為零時自運動平面法矢方向的夾角。3、下面介紹本專利技術結合實際機構尺寸的逆運動學計算過程及結果：如圖2所示，建立SRS構型冗余度機械臂的數學模型，建立坐標系，當腕關節在本文檔來自技高網...

【技術保護點】
一種面向SRS仿人手臂的自運動角計算方法，其特征在于：它包括如下步驟：步驟一：建立SRS構型冗余度機械臂的數學模型，根據結構特點建立肩關節、肘關節、腕關節坐標系；步驟二：由當前狀態下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動角；步驟三：由步驟二的自運動角和工具坐標系位姿計算關節空間運動角度；步驟四：將步驟三的求解結果傳遞給下位機控制器，驅動機械臂實現給定工具坐標系位姿的運動。

【技術特征摘要】
1.一種面向SRS仿人手臂的自運動角計算方法，其特征在于：它包括如下步驟：步驟一：建立SRS構型冗余度機械臂的數學模型，根據結構特點建立肩關節、肘關節、腕關節坐標系；步驟二：由當前狀態下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動角；步驟三：由步驟二給定工具坐標系位姿時的自運動角和工具坐標系位姿計算關節空間運動角度；步驟四：將步驟三的求解結果傳遞給下位機控制器，驅動機械臂實現給定工具坐標系位姿的運動；其中，步驟二中所述PID算法具體過程如下：1)由當前狀態的自運動角θn和給定的執行末端點的位姿確定各個關節運動角度；2)將得到的關節運動角度，計算腕關節姿態矩陣的Z軸與腕關節中心點矢量構成的平面與當前狀態下的大臂小臂平面之間的夾角αn；3)新的自運動角θn+1由如下公式：θn+1＝θn+P*αn+I*(θn–θ0)求出；式中符號說明如下：θn+1為新的自運動角，θn為上一次計算得到的自運動角，θ0為初始狀態的自運動角，P、I為可調節參數；4)在內存中存儲自運動角θn+1；該算法需給定初始狀態時的自運動角θ0，由初始狀態時的自運動角θ0按照上面1)-4)所述遞推。2.根據權利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于：步驟一所述“建立SRS構型冗余度機械臂的數學模型”，其實現過程如下：測量肩關節中心到肘關節中心的距離和肘關節中心到腕關節中心的距離，其幾何模型為一個三動桿機構。3.根據權利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于：基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，所述SRS構型S為球副，R轉動副，所述肩關節、肘關節、腕關節分別對應S球副、R轉動副、S球副。4.根據權利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于：基于自運動角的SRS構型冗余度機械臂逆運動學計算方法，坐標系建立如下所述：將肩關節轉動中心、軸關節轉動中心的連線稱為大臂，肘關節轉動中心、腕關節轉動中心的連線稱為前臂，在初始狀...

【專利技術屬性】
技術研發人員：胡寧，胡磊，胡玥，
申請(專利權)人：北京航空航天大學，
類型：發明
國別省市：北京;11