逆運(yùn)動(dòng)學(xué)方程

1、逆運(yùn)動(dòng)學(xué)方程第1頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三上節(jié)知識(shí)點(diǎn)回顧：空間姿態(tài)的描述：1.歐拉角： Euler(,) ot(z,)ot(y,)ot(z,) 2.RPY角：RPY(,)ot(z,)ot(y,)ot(x,)空間位置的描述：1.圓柱坐標(biāo)： Cyl(z, ,r) = Trans(0,0,z)Rot(z, ) Trans(r,0,0)2.球坐標(biāo)： Sph(,) = Rot(z,) Rot(y,) Trans(0,0,)如果機(jī)械手用變換矩陣Z與參考坐標(biāo)系相聯(lián)系，機(jī)械手末端執(zhí)行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標(biāo)系用X來 描述，有 X = Z T6 E 第2頁

2、，共31頁，2022年，5月20日，19點(diǎn)35分，星期三第四章 逆運(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é) 第3頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三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é)方程解

3、的步驟如下：（1）根據(jù)機(jī)械手關(guān)節(jié)坐標(biāo)設(shè)置確定An An為關(guān)節(jié)坐標(biāo)的齊次坐標(biāo)變換，由關(guān)節(jié)變量和參數(shù)確定。關(guān)節(jié)變量和參數(shù)有：an連桿長度; n連桿扭轉(zhuǎn)角;dn相鄰兩連桿的距離; n相鄰兩連桿的夾角。 對于旋轉(zhuǎn)關(guān)節(jié)n為關(guān)節(jié)變量，而對于滑動(dòng)關(guān)節(jié)dn為關(guān)節(jié)變量。其余為連桿參數(shù)，由機(jī)械手的幾何尺寸和組合形態(tài)決定。第4頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三（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)矢

4、量n，o，a（確定姿態(tài)）組成的齊次變換矩陣描述。（3）由T6和An（n1，2，6）和式（4.1）求出相應(yīng)的關(guān)節(jié)變量n 或 dn。 第5頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三4.2 逆運(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

5、 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) （4.4） A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( 4T6 = A5 A6 ) （4.5） A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) （4.6） 根據(jù)上述五個(gè)矩陣方程對應(yīng)元素相等，可得到若干個(gè)可解的代數(shù)方程，便可求出關(guān)節(jié)變量n或 dn。第6頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三 4.3 斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解 （ Inverse solution of Stanford manipulator） 在第三章我們推導(dǎo)出 Stanford M

6、anipulator 的運(yùn)動(dòng)方程和各關(guān)節(jié)齊次變換式。下面應(yīng)用式（4.2）（4.6）進(jìn)行求解：第7頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三這里 f11 = C1 xS1 y (4.10) f12 = - z (4.11) 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 - S4

7、S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 1T6 = S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0 C2C4S5 + S2C5 S2d3 S2C4S5 - C2C5 -C2d3 S4S5 d2 （4.13） 0 1第8頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三比較式（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）將

8、式（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）這里號表示機(jī)械手是右肩結(jié)構(gòu)（）還是左肩結(jié)構(gòu)（）。第9頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三由式（4.21）、（4.22）和（4.18）可得到第一個(gè)關(guān)節(jié)變量1的值 （4.23） 根據(jù)同樣的方法，利用式（4.9）和式（4.13）矩陣元素相等建立的相關(guān)的方程組，可得到其它各關(guān)節(jié)變量如下： （4.24）（4.25）（4.26）（4.27）（4.28）第10

9、頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三注意：在求解關(guān)節(jié)變量過程中如出現(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)范圍。由于機(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)過多次反覆計(jì)算，從中選擇一組合理解。由此可見，求解機(jī)械手的逆運(yùn)動(dòng)方程是

10、一個(gè)十分復(fù)雜的過程。第11頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三4.4 歐拉變換的逆運(yùn)動(dòng)學(xué)解 （Inverse solution of Euler Angles ）由第三章知?dú)W拉變換為Euler (, ,) Rot (z, ) Rot (y, ) Rot (z,) （4.29）我們用T來表示歐拉變換的結(jié)果，即T Euler (, ,) （4.30）或T Rot (z, ) Rot (y, ) Rot (z,) （4.31）其中 （4.32）第12頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三（4.33）第13頁，共31頁，2022年，5月20日，19點(diǎn)35分，星

11、期三比較式（4.32）和式（4.33）有 （4.34） （4.35） （4.36） （4.37） （4.38） （4.39） （4.40） （4.41） （4.42）第14頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三由式（4.42）可解出角 （4.43）由式（4.40）和式（4.43）可解出角 （4.44）由式（4.36）和式（4.43）可解出角 （4.45）第15頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三 這里需要指出的是，在我們采用式（4.43）式（4.45）來計(jì)算、時(shí)都是采用反余弦函數(shù)，而且式（4.44）和式（4.45）的分母為sin，這會(huì)帶來如下問題： 1

12、）由于絕對值相同的正負(fù)角度的余弦相等，如coscos（-），因此不能確定反余弦的結(jié)果是在那個(gè)象限； 2）當(dāng)sin接近于0時(shí)，由式（4.43）和式（4.45）所求出的角度和是不精確的； 3）當(dāng)0或180時(shí)，式（4.43）和式（4.45）無數(shù)值解。 為此，我們必須尋求更為合理的求解方法。 由三角函數(shù)的知識(shí)我們知道，反正切函數(shù)tan1（x / y）所在的象限空間可由自變量的分子和分母的符號確定（如圖4.1所示），因此如果我們得到歐拉角的正切表達(dá)式，就不難確定歐拉角所在的象限。 為此，我們采用本章第二節(jié)的方法，用Rot (z, )1左乘式（4.31）有Rot1(z,) T Rot (y, ) Rot

13、(z, ) （4.46）yxyyxyxxyx圖4.1 正切函數(shù)所在象限第16頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三即（4.47）將上式寫成如下形式（4.48）式中 （4.49） （4.50） （4.51）同樣，上面三個(gè)式子中的x、y、z分別表示n、o、a、p矢量的各個(gè)分量，如 （4.52）第17頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三比較式（4.48）等號兩邊矩陣的第2行第3列元素可知 （4.63）即 （4.54）由此可得到 （4.55）或 （4.56）結(jié)果得到 （4.57）或 （4.58）第18頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三

14、上述結(jié)果相差180，可根據(jù)實(shí)際系統(tǒng)的組合形態(tài)從中選擇一個(gè)合理解。如果ay和ax都為0，則式（4.57）和式（4.58）無定義，這是一種退化現(xiàn)象，此時(shí)值可任意設(shè)置，如0。 由于角已求出，比較式（4.48）等號兩邊矩陣第1行第3列和第3行第3列元素相等有 （4.59） （4.60） 或 （4.61） （4.62） 由此可得 （4.63）第19頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三同樣比較式（4.48）等號兩邊矩陣的第2行第1列和第2行第2列元素可知 （4.64） （4.65）或 （4.66） （4.67）由此可得 （4.68）至此，我們求出了歐拉變換的逆運(yùn)動(dòng)學(xué)解。第20頁，共3

15、1頁，2022年，5月20日，19點(diǎn)35分，星期三例：給定一個(gè)直角坐標(biāo)歐拉角型機(jī)器人的最終期望姿態(tài)，求相應(yīng)的歐拉角。第21頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三解：由式（4-57） 得到 由式（4-63） 得到 由式（4-68） 得到 第22頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三4.5 RPY變換的逆運(yùn)動(dòng)學(xué)解（Inverse solution of RPY）第三章介紹的搖擺、俯仰和偏轉(zhuǎn)( RPY )變換的表達(dá)式如下T = RPY ( , ,) Rot ( z, ) Rot ( y, ) Rot ( x, ) （4.69）用Rot1( z, )左乘上式得到

16、Rot1( z, ) T Rot ( y, ) Rot ( x, ) （4.70）將上式寫成式（4.48）的形式 （4.71）式中 （4.72） （4.73） （4.74）第23頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三由式（4.71）等號兩邊矩陣的第2行第1列元素相等有 （4.75）由此得到 （4.76）或 （4.77）角已求出，根據(jù)式（4.71）等號兩邊矩陣的第3行第1列和第1行第1列元素相等有 （4.78） （4.79）由此可得 （4.80）第24頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三 進(jìn)一步比較式（4.71）等號兩邊矩陣元素，由第2行第3列和第2行第

17、2列元素相等有 （4.81） （4.82） 由此可得 （4.83） 至此，我們求出了RPY的逆運(yùn)動(dòng)學(xué)解。第25頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三例：下面給出一個(gè)RPY機(jī)器人手所吸取的最終位姿，求滾動(dòng)角、俯仰角和位移。第26頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三解：由式（4-76） 得：由式（4-80） ，得：由式（4-83） ，得：第27頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三4.6 球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解 （Inverse solution of Spherical Coordinates ）第三章介紹的球坐標(biāo)變換的表達(dá)式如下T

18、= Sph (, , ) = Rot ( z, ) Rot (y, ) Trans( 0, 0, ) （4.84）用Rot1(z,)左乘上式得到Rot1( z, ) T = Rot ( y, ) Trans ( 0, 0, ) （4.85）將上列矩陣方程的第4列元素寫出有 （4.86）由上式第2行元素相等有 （4.87）第28頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三由式（4.87）可得到 （4.88）或 （4.89）由式（4.86）第1行和第3行元素相等有 （4.90） （4.91）由此可得 （4.92）第29頁，共31頁，2022年，5月20日，19點(diǎn)35分，星期三為了獲得平移量，我們用Rot1( y, )左乘式（4.85）Rot1( y, ) Rot1( z, ) T = Trans ( 0, 0, ) （4.9