《機器人導論》機器人逆運動學

1、 逆運動學逆運動學: 已知工具坐標系相對于工作臺坐標系的期望位置已知工具坐標系相對于工作臺坐標系的期望位置和姿態(tài)，計算一系列滿足期望要求的關節(jié)角和姿態(tài)，計算一系列滿足期望要求的關節(jié)角 為求出要求的關節(jié)角以放置相對于工作臺坐標系為求出要求的關節(jié)角以放置相對于工作臺坐標系S的工的工具坐標系具坐標系T，可將這個問題分為兩部分，可將這個問題分為兩部分: 首先，進行坐標變換求出相對于基坐標系首先，進行坐標變換求出相對于基坐標系B的腕部坐標的腕部坐標系系 W. 應用逆運動學求關節(jié)角應用逆運動學求關節(jié)角. TTTTTTT56453423120106T06101TTTTTTTT5645342312061011

2、112TTTTTTTT5645342306101112211121321222300123456112233445566313233()()()()()()0001xyzrrrprrrpTTTTTTTrrrp1. 解的存在性解的存在性 解是否存在的問題完全取決于操作臂的工作空間解是否存在的問題完全取決于操作臂的工作空間. 靈巧工作空間靈巧工作空間: 機器人的末端執(zhí)行器能夠從各個方向到達機器人的末端執(zhí)行器能夠從各個方向到達的空間區(qū)域的空間區(qū)域.可達工作區(qū)間可達工作區(qū)間:機器人至少從一個方向上有一個方位可以達機器人至少從一個方向上有一個方位可以達到的空間到的空間. 例例: 考慮一個兩連桿操作臂考慮

3、一個兩連桿操作臂. 如果如果 , 則可達工作空間是半徑為則可達工作空間是半徑為 的圓，而靈巧工作空間僅是單的圓，而靈巧工作空間僅是單獨的一點，即原點。如果獨的一點，即原點。如果 ，則不存在靈巧工作空間，而可達工作空，則不存在靈巧工作空間，而可達工作空間為一外徑為間為一外徑為 ，內徑為，內徑為 的圓環(huán)。在可達工作空間內部，末端執(zhí)的圓環(huán)。在可達工作空間內部，末端執(zhí)行器有兩種可能的方位，在工作空間的邊界上只能一種可能的方位。行器有兩種可能的方位，在工作空間的邊界上只能一種可能的方位。12ll12ll12l12ll12ll 當一個操作臂少于當一個操作臂少于6自由度時，它在三維空間內不能達到自由度時，它

4、在三維空間內不能達到全部位姿全部位姿. -操作臂的工作空間是一個子空間操作臂的工作空間是一個子空間. -更簡單的操作臂的工作空間是這個子空間的子集更簡單的操作臂的工作空間是這個子空間的子集. 對于少于對于少于6個自由度的操作臂來說，給定一個確定的一般個自由度的操作臂來說，給定一個確定的一般目標坐標系，什么是最近的可達目標坐標系？目標坐標系，什么是最近的可達目標坐標系？ 一般來說，工具坐標系的變換與操作臂的正逆運動學無一般來說，工具坐標系的變換與操作臂的正逆運動學無關，所以一般常去研究腕部坐標系關，所以一般常去研究腕部坐標系W的工作空間。對于一個的工作空間。對于一個給定的末端執(zhí)行器，定義工具坐標

5、系給定的末端執(zhí)行器，定義工具坐標系T，給定目標坐標系，給定目標坐標系G，去計算相應的腕部坐標系，去計算相應的腕部坐標系W。 例例: 試著描述三連桿操作臂試著描述三連桿操作臂 的子空間的子空間. 利用連桿參數(shù)求得操作臂的運動學方程為利用連桿參數(shù)求得操作臂的運動學方程為: 這里這里 和和 是滿足約束的任意變量是滿足約束的任意變量,因此，子空間就因此，子空間就建立了建立了.連桿長度和關節(jié)的限位決定了操作臂的工作空間連桿長度和關節(jié)的限位決定了操作臂的工作空間. 1231231 12121231231 121200000010001000010001BWcsxcsl cl cscyscl sl sTBW

6、T, x y 例例: 試描述下圖兩自由度操作臂試描述下圖兩自由度操作臂 的子空間的子空間. 已知已知: 這里這里 可以取任意值可以取任意值. 它的方位是確定的，因為它的方位是確定的，因為 的方向取決于的方向取決于 它的姿態(tài)受限，它的姿態(tài)受限， 總是向下，而總是向下，而 的方向是叉乘求得。的方向是叉乘求得。020ORGxPy 02T, x y, x y02Y000222XYZ22220222220001000001yxxxyxyxyyTxyxy02Z 2. 多重解多重解 一個具有一個具有3個旋轉關節(jié)的平面操作臂，由于從任何方位均可到達工作個旋轉關節(jié)的平面操作臂，由于從任何方位均可到達工作空間內的

7、任何位置，因此在平面中有較大的靈巧工作空間（給定適當?shù)目臻g內的任何位置，因此在平面中有較大的靈巧工作空間（給定適當?shù)倪B桿長度和大的關節(jié)運動范圍）連桿長度和大的關節(jié)運動范圍）. 系統(tǒng)最終只能選擇一個解，比較合理的選擇應當是取系統(tǒng)最終只能選擇一個解，比較合理的選擇應當是取“最短行程最短行程”解解. 最短行程的確定：最短行程的確定： 計算最短行程需要加權，使得選擇側重于移動小連桿而不計算最短行程需要加權，使得選擇側重于移動小連桿而不是移動大連桿是移動大連桿. 在存在障礙的情況下，最短行程發(fā)生干涉，這時選擇較長在存在障礙的情況下，最短行程發(fā)生干涉，這時選擇較長行程。行程。 解的個數(shù)取決于操作臂的關節(jié)數(shù)

8、量，它也是連桿參數(shù)和關節(jié)運動范解的個數(shù)取決于操作臂的關節(jié)數(shù)量，它也是連桿參數(shù)和關節(jié)運動范圍的函數(shù)圍的函數(shù). 例子例子: PUMA 到達一個確定目標有到達一個確定目標有8個不同的解個不同的解. 圖中給出了其中的圖中給出了其中的4個個解解.它們對于末端手部運動來說具有相同的位姿。對于圖中所示的每一個它們對于末端手部運動來說具有相同的位姿。對于圖中所示的每一個解存在另外一種解，解存在另外一種解， 其中最后三個關節(jié)變?yōu)榱硗庖环N位形：其中最后三個關節(jié)變?yōu)榱硗庖环N位形： 由于關節(jié)運動的限制，由于關節(jié)運動的限制， 這這8個解中的某些解是不能實現(xiàn)的個解中的某些解是不能實現(xiàn)的.04455066180180 通常

9、，連桿的非零參數(shù)越多，達到某一特定目標的方式也越多通常，連桿的非零參數(shù)越多，達到某一特定目標的方式也越多. 以一個具有以一個具有6個旋轉關節(jié)的操作臂為例，解的最大數(shù)目與等于零的連個旋轉關節(jié)的操作臂為例，解的最大數(shù)目與等于零的連桿長度參數(shù)的數(shù)目相關。非零參數(shù)越多，解的最大數(shù)目就越大桿長度參數(shù)的數(shù)目相關。非零參數(shù)越多，解的最大數(shù)目就越大. 3. 解法解法 與線性方程組不同，非線性方程組沒有通用的求解方法，與線性方程組不同，非線性方程組沒有通用的求解方法，我們把操作臂的全部求解方法分成兩大類：我們把操作臂的全部求解方法分成兩大類：封閉解封閉解: 封閉解是指基于解析形式的算法，或者指對于不封閉解是指基

10、于解析形式的算法，或者指對于不高于四次的多項式不用迭代便可完全求解?？蓪⒎忾]解的高于四次的多項式不用迭代便可完全求解?？蓪⒎忾]解的求解方法分為兩類：代數(shù)解法和幾何解法求解方法分為兩類：代數(shù)解法和幾何解法.數(shù)值解法數(shù)值解法: 數(shù)值解具有迭代性質，所以比封閉解法的求解數(shù)值解具有迭代性質，所以比封閉解法的求解速度慢得多。通常，數(shù)值解的計算也依賴于解的解析形式，速度慢得多。通常，數(shù)值解的計算也依賴于解的解析形式，一般不用數(shù)值解來求解運動學問題，對運動方程的數(shù)值迭一般不用數(shù)值解來求解運動學問題，對運動方程的數(shù)值迭代本身已形成一個完整的研究領域代本身已形成一個完整的研究領域. 關于運動學逆解的幾個結論關于

11、運動學逆解的幾個結論: 所有包含轉動關節(jié)和移動關節(jié)的串聯(lián)型所有包含轉動關節(jié)和移動關節(jié)的串聯(lián)型6自由度操作臂都自由度操作臂都是可解的，但這種解一般是數(shù)值解是可解的，但這種解一般是數(shù)值解. 對于對于6自由度操作臂來說，只有在特殊情況下才有解析解。自由度操作臂來說，只有在特殊情況下才有解析解。這種存在解析解（封閉解）的操作臂具有如下特性：存在這種存在解析解（封閉解）的操作臂具有如下特性：存在幾個正交關節(jié)軸或者有多個幾個正交關節(jié)軸或者有多個 為為0或或 . 具有具有6個旋轉關節(jié)的操作臂存在封閉解的充分條件個旋轉關節(jié)的操作臂存在封閉解的充分條件（sufficient condition）是相鄰的三個關節(jié)

12、軸線相交于一）是相鄰的三個關節(jié)軸線相交于一點點. i090 為了介紹運動學方程的求解方法，這里用兩種為了介紹運動學方程的求解方法，這里用兩種不同方法對一個簡單的平面三連桿操作臂進行求解不同方法對一個簡單的平面三連桿操作臂進行求解. 1. 代數(shù)解法代數(shù)解法 該操作臂的運動方程為該操作臂的運動方程為: 1231231 12121231231 1212030000100001BWcsl cl cscl sl sTT 研究的是平面操作臂研究的是平面操作臂,通過確定三個量通過確定三個量 就可以容易確定目標點就可以容易確定目標點的位置的位置 : . 所有可達目標點均位于上式描述的子空間內所有可達目標點均位

13、于上式描述的子空間內. 0000100001BWcsxscyT, ,x y1231231 12121231231 12120300000010001000010001BWcsxcsl cl cscl sl sscyTT 得到四個非線性方程得到四個非線性方程: 上式有解的條件是上式有解的條件是 的值必須在的值必須在-1和和+1之間。在這個之間。在這個解法中，可用這個約束來檢查解是否存在。如果約束條件不解法中，可用這個約束來檢查解是否存在。如果約束條件不滿足滿足 ,則操作臂離目標點太遠則操作臂離目標點太遠. 1231231 12121 1212ccssxl cl cyl sl s2222121 2

14、222221221 2121 21 2121 21 222,xylll l cxyllcl lcc cs ssc ss c2c 假設目標點在工作空間內，則假設目標點在工作空間內，則: 上式是多解的，可以選擇正或者負上式是多解的，可以選擇正或者負.2222221,tan2(,)scAs c 為便于計算引入新的變量引入新的變量: 式中式中: 為了求解這種形式的方程，進行變量代換為了求解這種形式的方程，進行變量代換:令令 那么那么 于是有于是有 1 12121 1212xl cl cyl sl s1 1211 121xk ck syk sk c1122222kll ckl s221221,tan2(

15、,)rkkAk k 12cos,sinkrkr1111cos cossin sincos sinsin cosxryr11cos()sin()xryr 得到得到 最后，我們解出最后，我們解出 : 總之，用代數(shù)解法求解運動學方程是求解操作臂的基本方法之一，在求解方程時，總之，用代數(shù)解法求解運動學方程是求解操作臂的基本方法之一，在求解方程時，解的形式已經確定?？梢钥闯?，對于許多常見的幾何問題，經常會出現(xiàn)幾種形式解的形式已經確定。可以看出，對于許多常見的幾何問題，經常會出現(xiàn)幾種形式的超越方程。的超越方程。 注：超越方程：等號兩邊至少有一個含有未知數(shù)的初等超越函數(shù)式的方程。如指注：超越方程：等號兩邊至

16、少有一個含有未知數(shù)的初等超越函數(shù)式的方程。如指數(shù)方程、對數(shù)方程、三角方程、反三角方程等。具有未知量的對數(shù)函數(shù)、指數(shù)函數(shù)方程、對數(shù)方程、三角方程、反三角方程等。具有未知量的對數(shù)函數(shù)、指數(shù)函數(shù)、三角函數(shù)、反三角函數(shù)等的方程。超越方程一般沒有解析解，而只有數(shù)值解數(shù)、三角函數(shù)、反三角函數(shù)等的方程。超越方程一般沒有解析解，而只有數(shù)值解或近似解，只有特殊的超越方程才可以求出解析解來?；蚪平猓挥刑厥獾某椒匠滩趴梢郧蟪鼋馕鼋鈦?。1121tan2(,)tan2( , )tan2( , )tan2(,)y xAAy xAy xAk krr123312tan2(,)As c 2. 幾何解法幾何解法 為求出操作

17、臂的解，將操作臂的空間幾何參數(shù)分解成為平面幾何參數(shù)，為求出操作臂的解，將操作臂的空間幾何參數(shù)分解成為平面幾何參數(shù)，然后應用平面幾何方法求出關節(jié)角度。對于例子中的然后應用平面幾何方法求出關節(jié)角度。對于例子中的3自由度操作臂，有自由度操作臂，有于操作臂是平面的，因此利用平面幾何關系直接求解。于操作臂是平面的，因此利用平面幾何關系直接求解。 We have:2222121 222cos(180)xylll l2222221221 2cos(180)cos()2xyllcl l 為了使該三角形成立，到目標點的距離為了使該三角形成立，到目標點的距離 必須滿足小于等于兩個連桿長度之和必須滿足小于等于兩個連

18、桿長度之和 , 12ll22xy 應用反正切公式: 應用余弦定理: Here, the arccosine must be solved so that , in order that the geometry which lead to the equation will be preserved. Then we have: 平面內的角度是可以相加的，因此三個連桿的角度之和即為最后一個連桿的姿態(tài): This equation is solved for to complete our solution.00180tan2( , )Ay x222212221cos2xylllxy11233

19、3. 通過化簡為多項式的代數(shù)解法 萬能公式: 22212sintan,cos,sin,tan21121 cosuuuuu 例子: 求解超越方程 的 . 利用: 得到: 取u的冪函數(shù)形式: 得到: 如果 , 那么 . 0accossinabc018022(1)2(1)aubucu22212cos,sin11uuuu2()2()0ac ubuca2222221,2tan ()bbacbbacuacac 盡管一般具有6自由度的操作臂沒有封閉解，但在某些特殊情況下還是可解的.PEIPER研究了3個相鄰的軸相交于一點的6自由度操作臂。PEIPER的方法主要針對6各關節(jié)均為旋轉關節(jié)的操作臂，且后面3個軸相

20、交。該成果廣泛應用于產品化的機器人中。 當最后3根軸相交時，連桿坐標系4、5、6的原點均位于這個交點上，這點的基坐標如下： 3134323001230120141234123124333()()()111ORGORGafxd sfyPT T T PT T TT Tzd cf 111111111100001 iiiiiiiiiiiiiiiiiiicsas cc cssdTs sc sccd 3134323001230120141234123124333()()()111ORGORGafxd sfyPT T T PT T TT Tzd cf 1333323234332322234323334332

21、3222343()0()()1100011 facsaafd ss cc cssdd sTfd csscsccdd c13 343 32232 3432 342332332 3432 342332fa cd ssafa csd sccd scd sfa ssd sscd ccd c 利用 ,得到: 0112,TT1101322110100012321211120421211121001101331112111230()0()()0001000111 ORGcsafcsas cc cssdfscccssdPsscsccds scsccdfc gs gs gc gg121121221 121213

22、21321 12121321gc fs gags cfc cfsfd sgs sfc sfcfd c We now write an expression for the squared magnitude of , which we will denote as : We now write this equation, along with the Z-component equation, as a system of two equations in the form: where: 2222222212312312231212222 ()rgggfffadd fa c fs f04O

23、RGP( ) 1 22 2131 22214()2()rk ck sakzk sk c sk222rxyz11222222231231223431212kfkfkfffadd fkf cd c Equation * is useful because dependence on has been eliminated and because dependence on takes a simple form. Now consider the solution of for : 1. If , then , where r is known. is a function of only. Af

24、ter the substitution, a quadratic equation in may be solved for . 2. If , then we have , where z is known. Again, after substituting, a quadratic equation arises that can be solved for . 3. Otherwise, eliminate and , the equation results in an equation of degree 4, which can be solved for .3121 22 2

25、131 22214()2()rk ck sakzk sk c sk310a 3rk3k33tan(/2)310s4zk32s2c222234122211()()4rkzkkkas Having solved for , we can solve equation * for and * for . To complete our solution, we need to solve for . These axes intersect, so these joint angles affact the orientation of only the last link. We can comp

26、ute them from nothing more than the rotation portion of the specified goal, . 456, 1 22 2131 22214()2()rk ck sakzk sk c sk32111120111243*1ORGc gs gs gc gPg06R 得到 之后 , 我們還需要求 ，由于這些軸相交，所以這些關節(jié)角只影響最后連桿的方位，我們只需計算指定目標的方向: 求出 后，當 時，可以由連桿坐標系4相對于基坐標的方位計算出 。坐標系6的期望方位與連桿坐標系4的方位差別僅在于最后三個關節(jié)的作用。 對于大多數(shù)操作臂來說，完全可以將之

27、前介紹的 Z-Y-Z歐拉角解法應用于 . 123, 404040|R4440106040 6|RRR456, 06R123, 4460|R Example: Z-Y-Z 歐拉角的求法 描述一個坐標系 B : 與參考坐標系 A重合，首先繞 旋轉 然后繞 旋轉 ，最后繞 旋轉 . 000( , )001000010001 ABZ Y ZcscscsRscscscc c cs sc c ss cc ss c cc ss c sc cs ss cs scBZBYBZ 解: 如果 或 , 解就簡化為: 111213212223313233 c c cs sc c ss cc srrrs c cc ss

28、c sc cs srrrs cs scrrr018002231323323133231tan2(,)tan2(/,/)tan2(/,/)ArrrArsrsArsrs121100tan 2(,)Arr012111800tan 2(,)Arr 例子：puma560機器人逆解: 將含有 的部分移到方程的左邊 11121321222300123456112233445566313233( )()()()()()0001xyzrrrprrrpTTTTTTTrrrp101 0123451162233445566( )()()()()()TTTTTTT 轉置 : 令元素 (2,4) 相等，得到: 進行三角恒

29、等變換: 其中1112131121222311163132330000001 0000 10001xyzrrrpcsrrrpscTrrrp113xys pc pd01Tcos,sinxypp22,tan(,)xyxyppApp We obtain If equate both the (1,4) and (3,4) elements: We can obtain113/s cc sd3 34 3a cd sK2233331122tan2(,1),tan2(,)tan2(,1)yxddddAAppA3 234232 2xpa sd ca s113 234 2322xyc ps pa cd sa

30、c233112sin(),cos()1dd2222222233422xyxpppaaddKa Solved by the same kind of trigonometric substitution: Rewrite again (left-hand side is a function of only knowns and ): 222233434tan2(,)tan2(,)Aa dAKadK01 0345326445566()()()()TTTTT1112131 231 23232 32122231 231 23232 336113313233000010001xyzrrrpc cs c

31、sa crrrpc ss sca sTscdrrrp Equating both the (1,4) and (2,4) elements: We can get We can solve2332 31142 32 3432 311tan2()()(),()()()zxyzxyAaa cpc ps pda sa sdpaa cc ps p2 3432 311232211()()()()zxyzxya sdpaa cc ps pcpc ps p1 231 23232 33xyzc c ps c ps pa ca32 3112 34232211()()()()zxyzxyaa cpc ps pa

32、sdspc ps p1 231 23232 34xyzc s ps s pc pa sd2233 Equating both the (1,3) and (3,3) elements: As long as , we can solve for as: When , the manipulator is in a singular configuration in which joint axes 4 and 6 line up and cause the same motion of the last link. In this case, all that matters (and all that can be solved for) is the sum or difference of and . This situation is detected by checking whether both arguments of the Atan2 are near zero. If so, is chosen arbitrarily, and when is computed