第5章 機(jī)器人控制技術(shù)-逆運(yùn)動(dòng)學(xué)方程

1、第四章第四章 逆運(yùn)動(dòng)學(xué)方程逆運(yùn)動(dòng)學(xué)方程Chapter Inverse Kinematic Equations4.1 引言4.2 逆運(yùn)動(dòng)學(xué)方程的解4.3 斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解4.4 歐拉變換的逆運(yùn)動(dòng)學(xué)解4.5 RPY變換的逆運(yùn)動(dòng)學(xué)解4.6 球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解4.7 本章小結(jié) 4.1 4.1 引言引言 (Introduction) 所謂逆運(yùn)動(dòng)學(xué)方程的解，就是已知機(jī)械手直角坐標(biāo)空間的位姿（pose）T6，求出各節(jié)變量n or dn 。 T6 = A1 A2 A3 A4 A5 A6 （4.1）逆運(yùn)動(dòng)學(xué)方程解的步驟如下：（1）根據(jù)機(jī)械手關(guān)節(jié)坐標(biāo)設(shè)置確定An An為關(guān)節(jié)坐標(biāo)的齊次坐標(biāo)變換，由關(guān)節(jié)變

2、量和參數(shù)確定。關(guān)節(jié)變量和參數(shù)有：an連桿長(zhǎng)度; n連桿扭轉(zhuǎn)角;dn相鄰兩連桿的距離; n相鄰兩連桿的夾角。 對(duì)于旋轉(zhuǎn)關(guān)節(jié)n為關(guān)節(jié)變量，而對(duì)于滑動(dòng)關(guān)節(jié)dn為關(guān)節(jié)變量。其余為連桿參數(shù)，由機(jī)械手的幾何尺寸和組合形態(tài)決定。（2） 根據(jù)任務(wù)確定機(jī)械手的位姿T6 T6為機(jī)械手末端在直角坐標(biāo)系（參考坐標(biāo)或基坐標(biāo)）中的位姿，由任務(wù)確定，即式（ 3.37 ）給出的表達(dá)式T6 = Z-1 X E-1確定。它是由三個(gè)平移分量構(gòu)成的平移矢量P（確定空間位置）和三個(gè)旋轉(zhuǎn)矢量n，o，a（確定姿態(tài)）組成的齊次變換矩陣描述。（3）由T6和An（n1，2，6）和式（4.1）求出相應(yīng)的關(guān)節(jié)變量n 或 dn。 10006zzzzy

3、yyyxxxxpaonpaonpaonT4.2 逆運(yùn)動(dòng)學(xué)方程的解（逆運(yùn)動(dòng)學(xué)方程的解（Solving inverse kinematic equations）根據(jù)式（4.1）T6 = A1 A2 A3 A4 A5 A6分別用An（n1，2，5）的逆左乘式（4.1）有 A1-1 T6 = 1T6 ( 1T6 = A2 A3 A4 A5 A6 ) （4.2） A2-1 A1-1 T6 = 2T6 ( 2T6 = A3 A4 A5 A6 ) （4.3） A3-1A2-1 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) （4.4） A4-1 A3-1A2-1 A1-1 T6 = 4T6

4、 ( 4T6 = A5 A6 ) （4.5） A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) （4.6） 根據(jù)上述五個(gè)矩陣方程對(duì)應(yīng)元素相等，可得到若干個(gè)可解的代數(shù)方程，便可求出關(guān)節(jié)變量n或 dn。 4.3 斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解 （ Inverse solution of Stanford manipulator） 在第三章我們推導(dǎo)出 Stanford Manipulator 的運(yùn)動(dòng)方程和各關(guān)節(jié)齊次變換式。下面應(yīng)用式（4.2）（4.6）進(jìn)行求解：這里 f11 = C1 xS1 y (4.10) f12 = - z (4.11

5、) f13 = - S1 xC1 y (4.12)其中 x = nx ox ax px T， y = ny oy ay py T， z = nz oz az pz T由第三章得到的斯坦福機(jī)械手運(yùn)動(dòng)學(xué)方程式（3.48）為 C2( C4C5C6 - S4S6 ) - S2S5C6 -C2( C4C5S6 + S4C6 )+ S2S5S6 S2( C4C5C6 - S4S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 1T6 = S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0 C2C4S5 + S2C5 S2d3 S2C4S5 - C2C5 -C

6、2d3 S4S5 d2 （4.13） 0 1比較式（4.9）和式（4.13）矩陣中的第三行第四列元素相等得到 f13（p）= d2 （4.14）或 - S1 pxC1 py = d2 （4.15）令 px = r cos （4.16） py = r sin （4.17）其中 （4.18） （4.19）將式（4.16）和式（4.17）代入式（4.15）有 sincon1consin1 d2/r （ 0 d2/r 1 ） （4.20）由式（4.20）可得 sin(1) d2/r （0 1 ） （4.21） con(1) （4.22）這里號(hào)表示機(jī)械手是右肩結(jié)構(gòu)（）還是左肩結(jié)構(gòu)（）。22yxpprxy

7、pp1tan221rd由式（4.21）、（4.22）和（4.18）可得到第一個(gè)關(guān)節(jié)變量1的值 （4.23） 根據(jù)同樣的方法，利用式（4.9）和式（4.13）矩陣元素相等建立的相關(guān)的方程組，可得到其它各關(guān)節(jié)變量如下： 2222111tantandrdppxyzyxppSpC1112tan（4.24）zyxpCpSpCSd21123（4.25）zyxyxaSaSaCCaCaS21121114tan（4.26）zyxyxzyxaCaSaCSaCaSSaSaSaCCC21121142112415tan（4.27）yxzyxzyxyxzyxoCoSCoSoSoCCSoCoSoCSSoCoSSoSoSoC

8、CCC114211242112511421124516tan（4.28）注意：l在求解關(guān)節(jié)變量過(guò)程中如出現(xiàn)反正切函數(shù)的分子和分母太小，則計(jì)算結(jié)果誤差會(huì)很大，此時(shí)應(yīng)重新選擇矩陣元素建立新的方程組再進(jìn)行計(jì)算，直到獲得滿意的結(jié)果為止。同樣，如果計(jì)算結(jié)果超出了機(jī)械手關(guān)節(jié)的運(yùn)動(dòng)范圍，也要重新計(jì)算，直到符合機(jī)械手關(guān)節(jié)的運(yùn)動(dòng)范圍。l由于機(jī)械手各關(guān)節(jié)變量的相互耦合，后面計(jì)算的關(guān)節(jié)變量與前面的關(guān)節(jié)變量有關(guān)，因此當(dāng)前面關(guān)節(jié)變量的計(jì)算結(jié)果發(fā)生變化時(shí)，后面關(guān)節(jié)變量計(jì)算的結(jié)果也會(huì)發(fā)生變化，所以逆運(yùn)動(dòng)方程的解不是唯一的，我們應(yīng)該根據(jù)機(jī)械手的組合形態(tài)和各關(guān)節(jié)的運(yùn)動(dòng)范圍，經(jīng)過(guò)多次反覆計(jì)算，從中選擇一組合理解。由此可見(jiàn)，求解機(jī)

9、械手的逆運(yùn)動(dòng)方程是一個(gè)十分復(fù)雜的過(guò)程。4.4 4.4 歐拉變換的逆運(yùn)動(dòng)學(xué)解歐拉變換的逆運(yùn)動(dòng)學(xué)解 （Inverse solution of Inverse solution of Euler Angles ）由第三章知?dú)W拉變換為Euler (, ,) Rot (z, ) Rot (y, ) Rot (z,) （4.29）我們用T來(lái)表示歐拉變換的結(jié)果，即T Euler (, ,) （4.30）或T Rot (z, ) Rot (y, ) Rot (z,) （4.31）其中 （4.32）1000zzzzyyyyxxxxpaonpaonpaonT（4.33）10000sinsincossin0sins

10、incoscossincossinsincoscoscossin0sincoscossinsincoscossinsincoscoscos1000010000cossin00sincos10000cossin00100sin0cos1000010000cossin00sincos),(),(),(conzRotyRotzRot比較式（4.32）和式（4.33）有 （4.34） （4.35） （4.36） （4.37） （4.38） （4.39） （4.40） （4.41） （4.42）sinsincoscoscosxnsincoscoscossinyncossinzncossinsincosc

11、osxocoscossincossinyosinsinzosincosxasinsinyacosza由式（4.42）可解出角 （4.43）由式（4.40）和式（4.43）可解出角 （4.44）由式（4.36）和式（4.43）可解出角 （4.45）)(cos1zasincos1xasincos1zn 這里需要指出的是，在我們采用式（4.43）式（4.45）來(lái)計(jì)算、時(shí)都是采用反余弦函數(shù)，而且式（4.43）和式（4.45）的分母為sin，這會(huì)帶來(lái)如下問(wèn)題： 1）由于絕對(duì)值相同的正負(fù)角度的余弦相等，如coscos（-），因此不能確定反余弦的結(jié)果是在那個(gè)象限； 2）當(dāng)sin接近于0時(shí)，由式（4.43）和

12、式（4.45）所求出的角度和是不精確的； 3）當(dāng)0或180時(shí)，式（4.43）和式（4.45）無(wú)數(shù)值解。 為此，我們必須尋求更為合理的求解方法。 由三角函數(shù)的知識(shí)我們知道，反正切函數(shù)tan1（x / y）所在的象限空間可由自變量的分子和分母的符號(hào)確定（如圖4.1所示），因此如果我們得到歐拉角的正切表達(dá)式，就不難確定歐拉角所在的象限。 為此，我們采用本章第二節(jié)的方法，用Rot (z, )1左乘式（4.31）有Rot1(z,) T Rot (y, ) Rot (z, ) （4.46）yxyyxyxxyx圖4.1 正切函數(shù)所在象限即（4.47）將上式寫(xiě)成如下形式（4.48）式中 （4.49） （4.5

13、0） （4.51）同樣，上面三個(gè)式子中的x、y、z分別表示n、o、a、p矢量的各個(gè)分量，如 （4.52）10000cossinsincossin00cossin0sinsincoscoscos10001000010000cossin00sincoszzzzyyyyxxxxpaonpaonpaon10000cossinsincossin00cossin0sinsincoscoscos1000)()()()()()()()()()()()(131313131212121211111111pfafofnfpfafofnfpfafofnfyxfsincos11yxfcossin12zf13yxaaaf

14、cossin)(12比較式（4.48）等號(hào)兩邊矩陣的第2行第3列元素可知 （4.63）即 （4.54）由此可得到 （4.55）或 （4.56）結(jié)果得到 （4.57）或 （4.58）0)(12af0cossinyxaaxyaacossintanxyaacossintanxyaa1tanxyaa1tan 上述結(jié)果相差180，可根據(jù)實(shí)際系統(tǒng)的組合形態(tài)從中選擇一個(gè)合理解。如果ay和ax都為0，則式（4.57）和式（4.58）無(wú)定義，這是一種退化現(xiàn)象，此時(shí)值可任意設(shè)置，如0。 由于角已求出，比較式（4.48）等號(hào)兩邊矩陣第1行第3列和第3行第3列元素相等有 （4.59） （4.60） 或 （4.61）

15、（4.62） 由此可得 （4.63）)(sin11af)(cos13afyxaasincossinzacoszyxaaasincostan1同樣比較式（4.48）等號(hào)兩邊矩陣的第2行第1列和第2行第2列元素可知 （4.64） （4.65）或 （4.66） （4.67）由此可得 （4.68）至此，我們求出了歐拉變換的逆運(yùn)動(dòng)學(xué)解。)(sin12nf)(cos12ofyxnncossinsinyxoocossincosyxyxoonncossincossintan14.5 RPY變換的逆運(yùn)動(dòng)學(xué)解變換的逆運(yùn)動(dòng)學(xué)解（Inverse solution of Inverse solution of RPY）

16、第三章介紹的搖擺、俯仰和偏轉(zhuǎn)( RPY )變換的表達(dá)式如下T = RPY ( , ,) Rot ( z, ) Rot ( y, ) Rot ( x, ) （4.69）用Rot1( z, )左乘上式得到Rot1( z, ) T Rot ( y, ) Rot ( x, ) （4.70）將上式寫(xiě)成式（4.48）的形式 （4.71）式中 （4.72） （4.73） （4.74）10000coscossincossin0sincos00cossinsinsincos10000)()()(0)()()(0)()()(131313121212111111afofnfafofnfafofnfyxfsincos

17、11yxfcossin12zf13由式（4.71）等號(hào)兩邊矩陣的第2行第1列元素相等有 （4.75）由此得到 （4.76）或 （4.77）角已求出，根據(jù)式（4.71）等號(hào)兩邊矩陣的第3行第1列和第1行第1列元素相等有 （4.78） （4.79）由此可得 （4.80）0cossinyxnnxynn1tan0180znsinyxnnsincoscosyxznnnsincostan1 進(jìn)一步比較式（4.71）等號(hào)兩邊矩陣元素，由第2行第3列和第2行第2列元素相等有 （4.81） （4.82） 由此可得 （4.83） 至此，我們求出了RPY的逆運(yùn)動(dòng)學(xué)解。yxaacossinsinyxoocossinc

18、osyxyxooaacossincossintan14.6 球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解 （Inverse solution of Inverse solution of Spherical Coordinates ）第三章介紹的球坐標(biāo)變換的表達(dá)式如下T = Sph (, , ) = Rot ( z, ) Rot (y, ) Trans( 0, 0, ) （4.84）用Rot1(z,)左乘上式得到Rot1( z, ) T = Rot ( y, ) Trans ( 0, 0, ) （4.85）將上列矩陣方程的第4列元素寫(xiě)出有 （4.86）由上式第2行元素相等有 （4.87）1cos0sin1cossinsincoszyxyxppppp0cossinyxpp由式（4.87）可得到 （4.88）或 （4.89）由式（4.86）第1行和第3行元素相等有 （4.90） （4.91）由此可得 （4.92）xypp1tan0180yxppsincossinzpcoszyxpppsincostan1為了獲得平移量，我們用Rot1( y, )左乘式（4.85）Rot1( y, ) Rot1( z, ) T = Trans ( 0, 0, ) （4.93）上式第4列元素是 （4.94）由上