肩 關(guān)節(jié)中心點和腕關(guān)節(jié)中心點的連線轉(zhuǎn)動的角度。腕關(guān)節(jié)中心點和執(zhí)行末端姿態(tài)已知時，肩 關(guān)節(jié)中心點、肘關(guān)節(jié)中心點和腕關(guān)節(jié)中心點組成的平面可以繞腕關(guān)節(jié)中心點矢量轉(zhuǎn)動，稱 其為自運動平面，對應(yīng)轉(zhuǎn)角稱為自運動角，并規(guī)定當工具坐標點為（x，y，z)時，自運動角為 零時自運動平面的法矢方向為（_y，X，0)。自運動角的大小為其余自運動平面對應(yīng)的自運動 角大小為自運動平面法矢與自運動角為零時自運動平面法矢方向的夾角。
[0053] 3、下面介紹本發(fā)明結(jié)合實際機構(gòu)尺寸的逆運動學計算過程及結(jié)果：
[0054] 如圖2所示，建立SRS構(gòu)型冗余度機械臂的數(shù)學模型，建立坐標系，當腕關(guān)節(jié)在初 始狀態(tài)時，腕關(guān)節(jié)的坐標系由肘關(guān)節(jié)坐標系平移得到，坐標系必須這樣建立才能進一步優(yōu) 化計算；其中肩關(guān)節(jié)轉(zhuǎn)動中心為A，肘關(guān)節(jié)轉(zhuǎn)動中心為B，腕關(guān)節(jié)轉(zhuǎn)動中心為C，AB之間的距 離為I 1, BC之間的距離為12, AC之間的距離為d。
[0055] 如圖1所示，初始狀態(tài)下的機械臂狀態(tài)。機械臂上電后初始化，初始位置自運動角 Θ Q的數(shù)值為零。給定一條運動軌跡的插補點序列〇，1，2, 3···。由初始位置自運動角和插補 點〇計算關(guān)節(jié)空間的運動角度。設(shè)上一步計算的腕關(guān)節(jié)姿態(tài)矩陣的Z軸與腕關(guān)節(jié)中心點矢 量所在的平面M，計算M與大臂小臂平面的夾角Ci 1。新的自轉(zhuǎn)角新的自運動角Q1*如下公 式：θ 1= θ Q+P*a i+lX Θ。- Θ。）求出，其中P = 〇. 2, I = 0. 1。通過調(diào)節(jié)PI可以改善關(guān) 節(jié)空間的運動平滑性。存儲當前插補點〇處的自運動角S1。已知η處自運動角時，由插補 點η處自運動角和插補點η+1處給定末端位姿計算插補點η+1處關(guān)節(jié)空間的運動角度。設(shè) 上一步計算的腕關(guān)節(jié)姿態(tài)矩陣的Z軸與腕關(guān)節(jié)中心點矢量所在的平面Μ，計算M與大臂小臂 平面的夾角α η+1。插補點η+1處的自運動角θη+2由如下公式：θ η+1= θ η+Ρ*αη+1+Ι*(θη-θ 〇)求出，其中P = 〇. 2, I = 0. 1。由初始狀態(tài)的值和遞推公式可以求得所有插補點處的 關(guān)節(jié)運動角度。將計算得到的結(jié)果傳遞給下位機控制器，驅(qū)動機械臂完成給定的末端軌跡。
[0056] 上文所述給定自運動角Θ和執(zhí)行末端位姿時，計算關(guān)節(jié)空間轉(zhuǎn)動角度的步驟如 下：
[0057] 1)設(shè)腕關(guān)節(jié)中心點矢量AC為（x，y，z)，則自轉(zhuǎn)角為0時大臂小臂平面ABC的法向 量 N 為（_y, X，0)。
[0058] 2)將1)中所述矢量N繞腕關(guān)節(jié)中心點矢量AC轉(zhuǎn)動自運動角值Θ得到大臂小臂 平面ABC的法矢量N 1;
[0059] 3)大臂長度I1和小臂長度I2已知，已知AC為（x，y，z)，求得
【主權(quán)項】
1. 一種面向SRS仿人手臂的自運動角計算方法，其特征在于：它包括如下步驟： 步驟一：建立SRS構(gòu)型冗余度機械臂的數(shù)學模型，根據(jù)結(jié)構(gòu)特點建立肩關(guān)節(jié)、肘關(guān)節(jié)、 腕關(guān)節(jié)坐標系； 步驟二：由當前狀態(tài)下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動 角； 步驟由步驟二的自運動角和工具坐標系位姿計算關(guān)節(jié)空間運動角度； 步驟四；將步驟=的求解結(jié)果傳遞給下位機控制器，驅(qū)動機械臂實現(xiàn)給定工具坐標系 位姿的運動。
2. 根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 步驟一所述"建立SRS構(gòu)型冗余度機械臂的數(shù)學模型"，其實現(xiàn)過程如下：將測量肩關(guān) 節(jié)中屯、到肘關(guān)節(jié)中屯、的距離和肘關(guān)節(jié)中屯、到腕關(guān)節(jié)中屯、的距離，其幾何模型為一個=動桿 機構(gòu)。
3. 根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 基于自運動角的SRS構(gòu)型冗余度機械臂逆運動學計算方法，所述SRS構(gòu)型S為球副，R 轉(zhuǎn)動副，文中所述中肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)分別對應(yīng)S球副、R轉(zhuǎn)動副、S球副。
4. 根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 基于自運動角的SRS構(gòu)型冗余度機械臂逆運動學計算方法，坐標系建立如下所述：將 肩關(guān)節(jié)轉(zhuǎn)動中屯、、軸關(guān)節(jié)轉(zhuǎn)動中屯、的連線稱為大臂，肘關(guān)節(jié)轉(zhuǎn)動中屯、、腕關(guān)節(jié)轉(zhuǎn)動中屯、的連 線稱為前臂，在初始狀態(tài)下肩關(guān)節(jié)坐標系Z軸與大臂平行，并由肩關(guān)節(jié)轉(zhuǎn)動中屯、指向肘關(guān) 節(jié)轉(zhuǎn)動中屯、；Y軸平行于肘關(guān)節(jié)轉(zhuǎn)動軸線，肘關(guān)節(jié)坐標系Z軸與小臂平行，并由肘關(guān)節(jié)轉(zhuǎn)動 中屯、指向腕關(guān)節(jié)轉(zhuǎn)動中屯、，Y軸為肘關(guān)節(jié)轉(zhuǎn)動軸線方向，腕關(guān)節(jié)處坐標系為肘關(guān)節(jié)坐標系沿 前臂平移。
5. 根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 基于自運動角的SRS構(gòu)型冗余度機械臂逆運動學計算方法，參數(shù)自運動角0的含義： 腕關(guān)節(jié)中屯、點和執(zhí)行末端姿態(tài)已知時，肩關(guān)節(jié)中屯、點、肘關(guān)節(jié)中屯、點和腕關(guān)節(jié)中屯、點組成 的平面繞腕關(guān)節(jié)中屯、點矢量轉(zhuǎn)動，稱其為自運動平面，對應(yīng)轉(zhuǎn)角稱為自運動角，并規(guī)定當工 具坐標點為（x，y，z)時，自運動角為零時自運動平面的法矢方向為（-y，x，0);其余自運動 平面對應(yīng)的自運動角大小為自運動平面法矢與自運動角為零時自運動平面法矢方向的夾 角。
6. 根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 基于自運動角的SRS構(gòu)型冗余度機械臂逆運動學計算方法，其中步驟二中所述PID算 法具體過程如下： 1) 由當前狀態(tài)的自運動角0。和給定的執(zhí)行末端點的位姿確定各個關(guān)節(jié)運動角度； 2) 將步驟二計算得到的關(guān)節(jié)角度，計算腕關(guān)節(jié)姿態(tài)矩陣的Z軸與腕關(guān)節(jié)中屯、點矢量構(gòu) 成的平面與大臂小臂平面之間的夾角a。； 3) 新的自運動角0。4由如下公式；0。4= 0 n+P*a。+1*( 0。- 0。）求出； 式中符號說明如下： 0。+1為新的自運動角，0。為上一次計算得到的自運動角，0。為初始狀態(tài)的自運動角， P、I為PI可調(diào)節(jié)參數(shù)； 4)在內(nèi)存中存儲自運動角0。+1; 該算法需給定初始狀態(tài)時的自運動角0。，由初始狀態(tài)時的自運動角0。按照上面 1)-4)所述遞推。
7.根據(jù)權(quán)利要求1所述的一種面向SRS仿人手臂的自運動角計算方法，其特征在于： 基于自運動角的SRS構(gòu)型冗余度機械臂逆運動學計算方法，由自運動角和執(zhí)行末端位 姿計算關(guān)節(jié)角度的步驟如下： 步驟1)由腕關(guān)節(jié)中屯、點求出自運動角為0時大臂小臂的法向矢量N ;若腕關(guān)節(jié)中屯、點 為（X，y, Z)，則上面所述矢量N為（-y, X，0); 步驟2)將步驟1)中所述矢量N繞腕關(guān)節(jié)中屯、點（x，y，z)轉(zhuǎn)動自運動角值0。得到大 臂小臂平面的法矢量Ni; 步驟3)通過余弦定理求出肩關(guān)節(jié)中屯、點，肘關(guān)節(jié)中屯、點和腕關(guān)節(jié)中屯、點構(gòu)成的S角 形的S個角度，大臂與肩關(guān)節(jié)、腕關(guān)節(jié)連線的夾角用符號a表示，大臂小臂之間的夾角用 符號0表不； 步驟4)將腕關(guān)節(jié)中屯、點矢量繞步驟2)求出的法矢量Ni轉(zhuǎn)動步驟3)求出的角度a， 得出大臂的矢量方向AB，AB方向即肩關(guān)節(jié)轉(zhuǎn)動后的Z軸方向，步驟2)求出的法矢量Ni為 肩關(guān)節(jié)轉(zhuǎn)動后的Y軸，轉(zhuǎn)動后肩關(guān)節(jié)的坐標確定； 步驟5)肘關(guān)節(jié)的轉(zhuǎn)動角度即步驟3)中所求角度JT-P ; 步驟6)運動后腕關(guān)節(jié)的坐標是通過肩關(guān)節(jié)的旋轉(zhuǎn)矩陣乘W肘關(guān)節(jié)的旋轉(zhuǎn)矩陣，然后 對兩矩陣的乘積求逆，最后將求得的逆矩陣乘W給定執(zhí)行末端的姿態(tài)矩陣得到。
【專利摘要】一種面向SRS仿人手臂的自運動角計算方法，它包括如下步驟：步驟一：建立SRS構(gòu)型冗余度機械臂的數(shù)學模型，根據(jù)結(jié)構(gòu)特點建立肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)坐標系；步驟二：由當前狀態(tài)下的自運動角通過PID算法計算給定工具坐標系位姿時的自運動角；步驟三：由步驟二的自運動角和工具坐標系位姿計算關(guān)節(jié)空間運動角度；步驟四：將步驟三的求解結(jié)果傳遞給下位機控制器，驅(qū)動機械臂實現(xiàn)給定工具坐標系位姿的運動。本發(fā)明給出了一種冗余度機械臂的逆運動學求法，和傳統(tǒng)的計算方法相比，算法簡單，計算速度快，可以得到解析解，在降低了求解難度的同時對關(guān)節(jié)運動做了一定的優(yōu)化。
【IPC分類】G05D3-12
【公開號】CN104635762
【申請?zhí)枴緾N201510017243
【發(fā)明人】胡寧, 胡磊, 胡玥
【申請人】北京航空航天大學
【公開日】2015年5月20日
【申請日】2015年1月13日