第三章-工業(yè)機器人運動學-3逆運動學

,Click to edit Master title style,Click to edit Master text styles,Second Level,Third Level,Fourth Level,Fifth Level,*,第三章 工業(yè)機器人的運動學,-3,主要內(nèi)容,數(shù)學基礎,齊次坐標變換,機器人運動學方程的建立（正運動學）,機器人逆運動學分析（逆運動學）,三、逆運動學方程,（,Inverse Kinematic Equations,）,3.1,引言,3.2,逆運動學方程的解,3.3,斯坦福機械手的逆運動學解,3.4,歐拉變換的逆運動學解,3.5 RPY,變換的逆運動學解,3.6,球坐標變換的逆運動學解,3.7,本章小結,3.1,引言,(Introduction),所謂逆運動學方程的解，就是已知機械手直角坐標空間的位姿（,pose）T,6,，,求出各節(jié)變量,n,or,d,n,。,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,（,3.1,）,逆運動學方程解的步驟如下,：,（,1,）根據(jù)機械手關節(jié)坐標設置確定,A,n,A,n,為關節(jié)坐標的齊次坐標變換，由關節(jié)變量和參數(shù)確定。關節(jié)變量和參數(shù)有：,a,n,連桿長度;,n,連桿扭轉(zhuǎn)角;,d,n,相鄰兩連桿的距離;,n,相鄰兩連桿的夾角。,對于旋轉(zhuǎn)關節(jié),n,為關節(jié)變量，而對于滑動關節(jié),d,n,為關節(jié)變量。其余為連桿參數(shù)，由機械手的幾何尺寸和組合形態(tài)決定。,（2）,根據(jù)任務確定機械手的位姿,T,6,T,6,為機械手末端在直角坐標系（參考坐標或基坐標）中的位姿，由任務確定，即式,（2.37）,給出的表達式,T,6,=Z,-1,X E,-1,確定。它是由三個平移分量構成的平移矢量,P（,確定空間位置）和三個旋轉(zhuǎn)矢量,n，o，a（,確定姿態(tài)）組成的齊次變換矩陣描述。,（3）由,T,6,和,A,n,（n1，2，6）,和式（,4.1,）求出相應的關節(jié)變量,n,或,d,n,。,3.2,逆運動學方程的解（,Solving inverse kinematic equations,）,根據(jù)式,（3.1）,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,分別用,A,n,（n1，2，5）,的逆左乘式（,3.1,）有,A,1,-1,T,6,=,1,T,6,(,1,T,6,=,A,2,A,3,A,4,A,5,A,6,),（,3.2,）,A,2,-1,A,1,-1,T,6,=,2,T,6,(,2,T,6,=A,3,A,4,A,5,A,6,),（,3.3,）,A,3,-1,A,2,-1,A,1,-1,T,6,=,3,T,6,(,3,T,6,=A,4,A,5,A,6,),（,3.4,）,A,4,-1,A,3,-1,A,2,-1,A,1,-1,T,6,=,4,T,6,(,4,T,6,=A,5,A,6,),（,3.5,）,A,5,-1,A,4,-1,A,3,-1,A,2,-1,A,1,-1,T,6,=,5,T,6,(,5,T,6,=A,6,),（,3.6,）,根據(jù)上述五個矩陣方程對應元素相等，可得到若干個可解的代數(shù)方程，便可求出關節(jié)變量,n,或,d,n,。,3.3,斯坦福機械手的逆運動學解,（,Inverse solution of Stanford manipulator,）,在第三章我們推導出,Stanford Manipulator,的運動方程和各關節(jié)齊次變換式,。,下面應用式（,3.2,）（,3.6,）進行求解：,這里,f,11,=,C,1,x,S,1,y,(,3.10,),f,12,=,-,z,(,3.11,),f,13,=,-,S,1,x,C,1,y,(,3.12,),其中,x,=,n,x,o,x,a,x,p,x,T,，,y,=,n,y,o,y,a,y,p,y,T,，,z,=,n,z,o,z,a,z,p,z,T,由前節(jié)得到的斯坦福機械手運動學方程式（,2.48,）為,C,2,(,C,4,C,5,C,6,-,S,4,S,6,)-,S,2,S,5,C,6,-,C,2,(,C,4,C,5,S,6,+S,4,C,6,),+S,2,S,5,S,6,S,2,(,C,4,C,5,C,6,-,S,4,S,6,),+C,2,S,5,C,6,-,S,2,(,C,4,C,5,S,6,+S,4,C,6,)-,C,2,S,5,S,6,1,T,6,=S,4,C,5,C,6,+C,4,C,6,-,S,4,C,5,S,6,+C,4,C,6,0 0,C,2,C,4,S,5,+S,2,C,5,S,2,d,3,S,2,C,4,S,5,-,C,2,C,5,-,C,2,d,3,S,4,S,5,d,2,（3.13）,0 1,比較式（,3.9,）和式（,3.13,）矩陣中的第三行第四列元素相等得到,f,13,（,p,）,=,d,2,（,3.14,）,或,-,S,1,p,x,C,1,p,y,=d,2,（,3.15,）,令,p,x,=,r cos,（,3.16,）,p,y,=,r sin,（,3.17,）,其中,（,3.18,）,（,3.19,）,將式（,3.16,）和式（,3.17,）代入式（,3.15,）有,sin,con,1,con,sin,1,d,2,/r,（,0,d,2,/r,1,）（,3.20,）,由式（,3.20,）可得,sin,(,1,),d,2,/r,（,0,1,）（,3.21,）,con,(,1,),（,3.22,）,這里,號表示機械手是右肩結構（）還是左肩結構（）。,由式（,3.21,）、（,3.22,）和（,3.18,）可得到第一個關節(jié)變量,1,的值,（,3.23,）,根據(jù)同樣的方法，利用式（,3.9,）和式（,3.13,）矩陣元素相等建立的相關的方程,組，可得到其它各關節(jié)變量如下：,（,3.24,）,（,3.25,）,（,3.26,）,（,3.27,）,（,3.28,）,注意：,在求解關節(jié)變量過程中如出現(xiàn)反正切函數(shù)的分子和分母太小，則計算結果誤差會很大，此時應重新選擇矩陣元素建立新的方程組再進行計算，直到獲得滿意的結果為止。同樣，如果計算結果超出了機械手關節(jié)的運動范圍，也要重新計算，直到符合機械手關節(jié)的運動范圍。,由于機械手各關節(jié)變量的相互耦合，后面計算的關節(jié)變量與前面的關節(jié)變量有關，因此當前面關節(jié)變量的計算結果發(fā)生變化時，后面關節(jié)變量計算的結果也會發(fā)生變化，所以逆運動方程的解不是唯一的，我們應該根據(jù)機械手的組合形態(tài)和各關節(jié)的運動范圍，經(jīng)過多次反覆計算，從中選擇一組合理解。由此可見，求解機械手的逆運動方程是一個十分復雜的過程。,3.4,歐拉變換的逆運動學解 （,Inverse solution of,Euler Angles,）,由前節(jié)知歐拉變換為,Euler(,),Rot(,z,)Rot(,y,)Rot(,z,),（,3.29,）,我們用,T,來表示歐拉變換的結果，即,T,Euler(,),（,3.30,）,或,T,Rot(,z,)Rot(,y,)Rot(,z,),（,3.31,）,其中,（,3.32,）,（,3.33,）,比較式（,3.32,）和式（,3.33,）有,（,3.34,）,（,3.35,）,（,3.36,）,（,3.37,）,（,3.38,）,（,3.39,）,（,3.40,）,（,3.41,）,（,3.42,）,由式（,3.42,）可解出,角,（,3.43,）,由式（,3.40,）和式（,3.43,）可解出,角,（,3.44,）,由式（,3.36,）和式（,3.43,）可解出,角,（,3.45,）,這里需要指出的是，在我們采用式（,3.43,）,式（,3.45,）來計算,、,、,時都是采用反余弦函數(shù)，而且式（,3.43,）和式（,3.45,）的分母為,sin,，這會帶來如下問題：,1,）由于絕對值相同的正負角度的余弦相等，如,cos,cos,（,-,），因此不能確定反余弦的結果是在那個象限；,2,）當,sin,接近于,0,時，由式（,3.43,）和式（,3.45,）所求出的角度,和,是不精確的；,3,）當,0,或,180,時，式（,3.43,）和式（,3.45,）無數(shù)值解。,為此，我們必須尋求更為合理的求解方法。,由三角函數(shù)的知識我們知道，反正切函數(shù),tan,1,（,x/y,）所在的象限空間可由自變量的分子和分母的符號確定（如圖,3.1,所示），因此如果我們得到歐拉角的正切表達式，就不難確定歐拉角所在的象限。,為此，我們采用前節(jié)的方法，用,Rot(,z,),1,左乘式（,3.31,）有,Rot,1,(,z,)T,Rot(,y,)Rot(,z,),（,3.46,）,y,x,y,y,x,y,x,x,y,x,圖,3.1,正切函數(shù)所在象限,即,（,3.47,）,將上式寫成如下形式,（,3.48,）,式中,（,3.49,）,（,3.50,）,（,3.51,）,同樣，上面三個式子中的,x,、,y,、,z,分別表示,n,、,o,、,a,、,p,矢量的各個分量，如,（,3.52,）,比較式（,3.48,）等號兩邊矩陣的第,2,行第,3,列元素可知,（,3.63,）,即,（,3.54,）,由此可得到,（,3.55,）,或,（,3.56,）,結果得到,（,3.57,）,或,（,3.58,）,上述結果相差,180,，可根據(jù)實際系統(tǒng)的組合形態(tài)從中選擇一個合理解。如果,a,y,和,a,x,都為,0,，則式（,3.57,）和式（,3.58,）無定義，這是一種退化現(xiàn)象，此時,值可任意設置，如,0,。,由于角,已求出，比較式（,3.48,）等號兩邊矩陣第,1,行第,3,列和第,3,行第,3,列元素相等有,（,3.59,）,（,3.60,）,或,（,3.61,）,（,3.62,）,由此可得,（,3.63,）,同樣比較式（,3.48,）等號兩邊矩陣的第,2,行第,1,列和第,2,行第,2,列元素可知,（,3.64,）,（,3.65,）,或,（,3.66,）,（,3.67,）,由此可得,（,3.68,）,至此，我們求出了歐拉變換的逆運動學解。,3.5 RPY,變換的逆運動學解,（,Inverse solution of,RPY,）,第三章介紹的搖擺、俯仰和偏轉(zhuǎn),(RPY),變換的表達式如下,T=RPY(,),Rot(,z,)Rot(,y,)Rot(,x,),（,3.69,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T,Rot(,y,)Rot(,x,),（,3.70,）,將上式寫成式（,3.48,）的形式,（,3.71,）,式中,（,3.72,）,（,3.73,）,（,3.74,）,由式（,3.71,）等號兩邊矩陣的第,2,行第,1,列元素相等有,（,3.75,）,由此得到,（,3.76,）,或,（,3.77,）,角,已求出，根據(jù)式（,3.71,）等號兩邊矩陣的第,3,行第,1,列和第,1,行第,1,列元素相等有,（,3.78,）,（,3.79,）,由此可得,（,3.80,）,進一步比較式（,3.71,）等號兩邊矩陣元素，由第,2,行第,3,列和第,2,行第,2,列元素相等有,（,3.81,）,（,3.82,）,由此可得,（,3.83,）,至此，我們求出了,RPY,的逆運動學解。,3.6,球坐標變換的逆運動學解,（,Inverse solution of,Spherical Coordinates,）,前節(jié)介紹的球坐標變換的表達式如下,T=Sph(,)=Rot(,z,)Rot(,y,)Trans(0,0,),（,3.84,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T=Rot(,y,)Trans(0,0,),（,3.85,）,將上列矩陣方程的第,4,列元素寫出有,（,3.86,）,由上式第,2,行元素相等有,（,3.87,）,由式（,3.87,）可得到,（,3.88,）,或,（,3.89,）,由式（,3.86,）第,1,行和第,3,行元素相等有,（,3.90,）,（,3.91,）,由此可得,（,3.92,）,為了獲得平移量,，我們用,Rot,1,(,y,),左