一種面向srs仿人手臂的自運(yùn)動(dòng)角計(jì)算方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及冗余度機(jī)械臂運(yùn)動(dòng)學(xué)及控制領(lǐng)域，具體來(lái)說(shuō)，是一種面向SRS仿人手 臂的自運(yùn)動(dòng)角計(jì)算方法。
【背景技術(shù)】
[0002] 機(jī)械臂是實(shí)現(xiàn)機(jī)器人系統(tǒng)主體與末端執(zhí)行器連接的重要工具，機(jī)械臂的活動(dòng)范 圍、負(fù)載能力直接影響機(jī)器人末端的作業(yè)能力。冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題是指已知機(jī) 械臂末端位姿，確定機(jī)械臂的關(guān)節(jié)角度。給定冗余自由度機(jī)械臂的末端位姿，關(guān)節(jié)空間有無(wú) 數(shù)個(gè)點(diǎn)與之對(duì)應(yīng)，這些點(diǎn)的集合是關(guān)節(jié)空間的一個(gè)n-m維流形。冗余自由度機(jī)械臂可以在 這個(gè)流形內(nèi)自由變動(dòng)而不影響末端位姿，這個(gè)變動(dòng)稱為冗余自由度機(jī)器人的自運(yùn)動(dòng)。傳統(tǒng) 的冗余自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)解法如梯度投影法耗時(shí)較長(zhǎng)，或人為退化一個(gè)關(guān)節(jié)角轉(zhuǎn)化 為無(wú)冗余自由度問(wèn)題。

【發(fā)明內(nèi)容】

[0003] 為了解決上述問(wèn)題，本發(fā)明提出一種面向SRS仿人手臂的自運(yùn)動(dòng)角計(jì)算方法。
[0004] 本發(fā)明通過(guò)以下技術(shù)方案予以實(shí)現(xiàn)：
[0005] 一種面向SRS仿人手臂的自運(yùn)動(dòng)角計(jì)算方法，它包括如下步驟：
[0006] 步驟一：建立SRS構(gòu)型冗余度機(jī)械臂的數(shù)學(xué)模型，根據(jù)結(jié)構(gòu)特點(diǎn)建立肩關(guān)節(jié)、肘關(guān) 節(jié)、腕關(guān)節(jié)坐標(biāo)系；
[0007] 步驟二：由當(dāng)前狀態(tài)下的自運(yùn)動(dòng)角通過(guò)PID算法計(jì)算給定工具坐標(biāo)系位姿時(shí)的自 運(yùn)動(dòng)角；
[0008] 步驟三：由步驟二的自運(yùn)動(dòng)角和工具坐標(biāo)系位姿計(jì)算關(guān)節(jié)空間運(yùn)動(dòng)角度；
[0009] 步驟四：將步驟三的求解結(jié)果傳遞給下位機(jī)控制器，驅(qū)動(dòng)機(jī)械臂實(shí)現(xiàn)給定工具坐 標(biāo)系位姿的運(yùn)動(dòng)。
[0010] 其中，步驟一所述"建立SRS構(gòu)型冗余度機(jī)械臂的數(shù)學(xué)模型"，其實(shí)現(xiàn)過(guò)程如下：將 測(cè)量肩關(guān)節(jié)中心到肘關(guān)節(jié)中心的距離和肘關(guān)節(jié)中心到腕關(guān)節(jié)中心的距離，其幾何模型為一 個(gè)三動(dòng)桿機(jī)構(gòu)。
[0011] 其中，基于自運(yùn)動(dòng)角的SRS構(gòu)型冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算方法，所述SRS構(gòu)型S 為球副，R轉(zhuǎn)動(dòng)副，文中所述中肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)分別對(duì)應(yīng)S球副、R轉(zhuǎn)動(dòng)副、S球副。
[0012] 其中，基于自運(yùn)動(dòng)角的SRS構(gòu)型冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算方法，坐標(biāo)系建立如 下所述：將肩關(guān)節(jié)轉(zhuǎn)動(dòng)中心、軸關(guān)節(jié)轉(zhuǎn)動(dòng)中心的連線稱為大臂，肘關(guān)節(jié)轉(zhuǎn)動(dòng)中心、腕關(guān)節(jié)轉(zhuǎn) 動(dòng)中心的連線稱為前臂。在初始狀態(tài)下肩關(guān)節(jié)坐標(biāo)系Z軸與大臂平行，并由肩關(guān)節(jié)轉(zhuǎn)動(dòng)中 心指向肘關(guān)節(jié)轉(zhuǎn)動(dòng)中心；Y軸平行于肘關(guān)節(jié)轉(zhuǎn)動(dòng)軸線。肘關(guān)節(jié)坐標(biāo)系Z軸與小臂平行，并由 肘關(guān)節(jié)轉(zhuǎn)動(dòng)中心指向腕關(guān)節(jié)轉(zhuǎn)動(dòng)中心，Y軸為肘關(guān)節(jié)轉(zhuǎn)動(dòng)軸線方向。腕關(guān)節(jié)處坐標(biāo)系為肘 關(guān)節(jié)坐標(biāo)系沿前臂平移。
[0013] 其中，基于自運(yùn)動(dòng)角的SRS構(gòu)型冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算方法，參數(shù)自運(yùn)動(dòng) 角Θ的含義：腕關(guān)節(jié)中心點(diǎn)和執(zhí)行末端姿態(tài)已知時(shí)，肩關(guān)節(jié)中心點(diǎn)、肘關(guān)節(jié)中心點(diǎn)和腕 關(guān)節(jié)中心點(diǎn)組成的平面可以繞腕關(guān)節(jié)中心點(diǎn)矢量轉(zhuǎn)動(dòng)，稱其為自運(yùn)動(dòng)平面，對(duì)應(yīng)轉(zhuǎn)角稱為 自運(yùn)動(dòng)角，并規(guī)定當(dāng)工具坐標(biāo)點(diǎn)為（x，y，z)時(shí)，自運(yùn)動(dòng)角為零時(shí)自運(yùn)動(dòng)平面的法矢方向?yàn)?(_y, X，0)。其余自運(yùn)動(dòng)平面對(duì)應(yīng)的自運(yùn)動(dòng)角大小為自運(yùn)動(dòng)平面法矢與自運(yùn)動(dòng)角為零時(shí)自運(yùn) 動(dòng)平面法矢方向的夾角。
[0014] 其中，基于自運(yùn)動(dòng)角的SRS構(gòu)型冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算方法，其中步驟二中 所述PID算法具體過(guò)程如下：
[0015] 1)由當(dāng)前狀態(tài)的自運(yùn)動(dòng)角θη和給定的執(zhí)行末端點(diǎn)的位姿確定各個(gè)關(guān)節(jié)運(yùn)動(dòng)角 度；
[0016] 2)將步驟二計(jì)算得到的關(guān)節(jié)角度，計(jì)算腕關(guān)節(jié)姿態(tài)矩陣的Z軸與腕關(guān)節(jié)中心點(diǎn)矢 量構(gòu)成的平面與大臂小臂平面之間的夾角α η;
[0017] 3)新的自運(yùn)動(dòng)角θη+1由如下公式：θ η+1= θ η+Ρ*αη+Ι*(θη- Θ。）求出；式中符 號(hào)
[0018] 說(shuō)明如下：
[0019] θη+1為新的自運(yùn)動(dòng)角，θ η為上一次計(jì)算得到的自運(yùn)動(dòng)角，Θ ^為初始狀態(tài)的自運(yùn) 動(dòng)角，P、I為PI可調(diào)節(jié)參數(shù)。
[0020] 4)在內(nèi)存中存儲(chǔ)自運(yùn)動(dòng)角θη+1。
[0021] 該算法需給定初始狀態(tài)時(shí)的自運(yùn)動(dòng)角Qtl。由初始狀態(tài)時(shí)的自運(yùn)動(dòng)角Qtl按照上 面1)-4)所述遞推。
[0022] 其中，基于自運(yùn)動(dòng)角的SRS構(gòu)型冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)計(jì)算方法，由自運(yùn)動(dòng)角和 執(zhí)行末端位姿計(jì)算關(guān)節(jié)角度的步驟如下：
[0023] 步驟1)由腕關(guān)節(jié)中心點(diǎn)求出自運(yùn)動(dòng)角為0時(shí)大臂小臂的法向矢量N ;若腕關(guān)節(jié)中 心點(diǎn)為（X，y, ζ)，則上面所述矢量N為（_y, X，0);
[0024] 步驟2)將步驟1)中所述矢量N繞腕關(guān)節(jié)中心點(diǎn)（X，y, ζ)轉(zhuǎn)動(dòng)自運(yùn)動(dòng)角值Θ ^導(dǎo) 到大臂小臂平面的法矢量N1;
[0025] 步驟3)通過(guò)余弦定理求出肩關(guān)節(jié)中心點(diǎn)，肘關(guān)節(jié)中心點(diǎn)和腕關(guān)節(jié)中心點(diǎn)構(gòu)成的 三角形的三個(gè)角度，大臂與肩關(guān)節(jié)、腕關(guān)節(jié)連線的夾角用符號(hào)α表示，大臂小臂之間的夾 角用符號(hào)β表不。
[0026] 步驟4)將腕關(guān)節(jié)中心點(diǎn)矢量繞步驟2)求出的法矢量&轉(zhuǎn)動(dòng)步驟3)求出的角度 α，得出大臂的矢量方向AB，AB方向即肩關(guān)節(jié)轉(zhuǎn)動(dòng)后的Z軸方向，步驟2)求出的法矢量N1 為肩關(guān)節(jié)轉(zhuǎn)動(dòng)后的Y軸，轉(zhuǎn)動(dòng)后肩關(guān)節(jié)的坐標(biāo)可以確定；
[0027] 步驟5)肘關(guān)節(jié)的轉(zhuǎn)動(dòng)角度即步驟3)中所求角度π-β ;
[0028] 步驟6)運(yùn)動(dòng)后腕關(guān)節(jié)的坐標(biāo)是通過(guò)肩關(guān)節(jié)的旋轉(zhuǎn)矩陣乘以肘關(guān)節(jié)的旋轉(zhuǎn)矩陣， 然后對(duì)兩矩陣的乘積求逆，最后將求得的逆矩陣乘以給定執(zhí)行末端的姿態(tài)矩陣得到。
[0029] 本發(fā)明的優(yōu)點(diǎn)在于：
[0030] 給出了一種冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求法，和傳統(tǒng)的計(jì)算方法相比，算法簡(jiǎn)單，計(jì) 算速度快，可以得到解析解，在降低了求解難度的同時(shí)對(duì)關(guān)節(jié)運(yùn)動(dòng)做了一定的優(yōu)化。
【附圖說(shuō)明】
[0031] 圖1為初始狀態(tài)下SRS構(gòu)型冗余度機(jī)械臂機(jī)構(gòu)及坐標(biāo)系建立示意圖；
[0032] 圖2為一般狀態(tài)下SRS構(gòu)型冗余度機(jī)械臂機(jī)構(gòu)及坐標(biāo)系建立示意圖；
[0033] 圖3為實(shí)施本發(fā)明方法使用的冗余度機(jī)械臂結(jié)構(gòu)示意圖；
[0034] 圖4(a)是給定直線軌跡下肩關(guān)節(jié)X支鏈的移動(dòng)量軌跡圖
[0035] 圖4(b)是給定直線軌跡下肩關(guān)節(jié)Y支鏈的移動(dòng)量軌跡圖
[0036] 圖4(c)是給定直線軌跡下肩關(guān)節(jié)Z軸轉(zhuǎn)動(dòng)量軌跡圖
[0037] 圖4(d)是給定直線軌跡下肘關(guān)節(jié)轉(zhuǎn)動(dòng)量軌跡圖
[0038] 圖4 (e)是給定直線軌跡下腕關(guān)節(jié)X支鏈的移動(dòng)量軌跡圖
[0039] 圖4 (f)是給定直線軌跡下腕關(guān)節(jié)Y支鏈的移動(dòng)量軌跡圖 [0040] 圖4 (g)是給定直線軌跡下腕關(guān)節(jié)關(guān)節(jié)Z軸轉(zhuǎn)動(dòng)量軌跡圖 [0041] 圖5(a)是給定圓形軌跡下肩關(guān)節(jié)X支鏈的移動(dòng)量軌跡圖
[0042] 圖5(b)是給定圓形軌跡下肩關(guān)節(jié)Y支鏈的移動(dòng)量軌跡圖
[0043] 圖5(c)是給定圓形軌跡下肩關(guān)節(jié)Z軸轉(zhuǎn)動(dòng)量軌跡圖 [0044] 圖5 (d)是給定圓形軌跡下肘關(guān)節(jié)轉(zhuǎn)動(dòng)量軌跡圖
[0045] 圖5 (e)是給定圓形軌跡下腕關(guān)節(jié)X支鏈的移動(dòng)量軌跡圖
[0046] 圖5 (f)是給定圓形軌跡下腕關(guān)節(jié)Y支鏈的移動(dòng)量軌跡圖
[0047] 圖5 (g)是給定圓形軌跡下腕關(guān)節(jié)關(guān)節(jié)Z軸轉(zhuǎn)動(dòng)量軌跡圖
[0048] 圖6是本發(fā)明流程框圖
【具體實(shí)施方式】
[0049] 下面將結(jié)合附圖和實(shí)例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說(shuō)明。
[0050] 1、圖3所示為實(shí)現(xiàn)本發(fā)明所使用的機(jī)械臂，該機(jī)械臂為SRS構(gòu)型七自由度機(jī)械臂。 該機(jī)械臂肩關(guān)節(jié)轉(zhuǎn)動(dòng)中心到肘關(guān)節(jié)轉(zhuǎn)動(dòng)中心軸線的距離I1 = 270,肘關(guān)節(jié)轉(zhuǎn)動(dòng)中心到腕關(guān) 節(jié)轉(zhuǎn)動(dòng)中心的距離I2= 230。
[0051] 2、圖4(a-g)、5(a_g)為實(shí)現(xiàn)本發(fā)明的冗余度機(jī)械臂逆運(yùn)動(dòng)學(xué)關(guān)節(jié)運(yùn)動(dòng)曲線示意 圖。在這個(gè)具體示例中，機(jī)械臂的任務(wù)是畫(huà)一條圓弧和一條直線。根據(jù)插值點(diǎn)的位姿，利 用本發(fā)明所述逆運(yùn)動(dòng)學(xué)方法求出各關(guān)節(jié)的運(yùn)動(dòng)角度，將計(jì)算得到的結(jié)果傳送給機(jī)械臂控制 器，從而控制機(jī)械臂完成末端軌跡任務(wù)。
[0052] 其中參數(shù)自運(yùn)動(dòng)角Θ表示在給定工具坐標(biāo)系的位姿時(shí)，冗余度機(jī)械臂可以繞