逆運動學方程

逆運動學方程第一頁，共三十一頁，編輯于2023年，星期五上節(jié)知識點回顧：空間姿態(tài)的描述：1.歐拉角：Euler(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(z,ψ)2.RPY角：RPY(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(x,ψ)空間位置的描述：1.圓柱坐標：Cyl(z,α,r)=Trans(0,0,z)Rot(z,α)Trans(r,0,0)2.球坐標：Sph(α,β,γ)=Rot(z,α)Rot(y,β)Trans(0,0,γ)如果機械手用變換矩陣Z與參考坐標系相聯(lián)系，機械手末端執(zhí)行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標系用X來描述，有X=ZT6E

第二頁，共三十一頁，編輯于2023年，星期五第四章逆運動學方程

ChapterⅣInverseKinematicEquations4.1引言4.2逆運動學方程的解4.3斯坦福機械手的逆運動學解4.4歐拉變換的逆運動學解4.5RPY變換的逆運動學解4.6球坐標變換的逆運動學解4.7本章小結

第三頁，共三十一頁，編輯于2023年，星期五4.1引言(Introduction)所謂逆運動學方程的解，就是已知機械手直角坐標空間的位姿（pose）T6，求出各節(jié)變量θn

ordn

。

T6=A1A2A3A4A5A6（4.1）逆運動學方程解的步驟如下：（1）根據(jù)機械手關節(jié)坐標設置確定AnAn為關節(jié)坐標的齊次坐標變換，由關節(jié)變量和參數(shù)確定。關節(jié)變量和參數(shù)有：

an－連桿長度; αn－連桿扭轉角;

dn－相鄰兩連桿的距離; θn－相鄰兩連桿的夾角。 對于旋轉關節(jié)θn為關節(jié)變量，而對于滑動關節(jié)dn為關節(jié)變量。其余為連桿參數(shù)，由機械手的幾何尺寸和組合形態(tài)決定。第四頁，共三十一頁，編輯于2023年，星期五（2）

根據(jù)任務確定機械手的位姿T6

T6為機械手末端在直角坐標系（參考坐標或基坐標）中的位姿，由任務確定，即式（3.37）給出的表達式T6=Z-1XE-1確定。它是由三個平移分量構成的平移矢量P（確定空間位置）和三個旋轉矢量n，o，a（確定姿態(tài)）組成的齊次變換矩陣描述。（3）由T6和An（n＝1，2，…，6）和式（4.1）求出相應的關節(jié)變量θn或dn。

第五頁，共三十一頁，編輯于2023年，星期五4.2逆運動學方程的解（Solvinginversekinematicequations）根據(jù)式（4.1）T6=A1A2A3A4A5A6分別用An（n＝1，2，…，5）的逆左乘式（4.1）有

A1-1T6=1T6(1T6=A2

A3A4A5A6)（4.2）

A2-1A1-1T6=2T6(2T6=A3A4A5A6)（4.3）

A3-1A2-1A1-1T6=3T6(3T6=A4A5A6)（4.4）

A4-1A3-1A2-1A1-1T6=4T6(4T6=A5A6

)（4.5）

A5-1

A4-1A3-1A2-1A1-1T6=5T6(5T6=A6)（4.6）根據(jù)上述五個矩陣方程對應元素相等，可得到若干個可解的代數(shù)方程，便可求出關節(jié)變量θn或dn。第六頁，共三十一頁，編輯于2023年，星期五

4.3斯坦福機械手的逆運動學解

（InversesolutionofStanfordmanipulator）在第三章我們推導出StanfordManipulator的運動方程和各關節(jié)齊次變換式。下面應用式（4.2）～（4.6）進行求解：第七頁，共三十一頁，編輯于2023年，星期五這里

f11=C1x＋S1y

(4.10)

f12=-z

(4.11)

f13=-S1x＋C1y

(4.12)其中

x=[nxoxaxpx]T，y=[nyoyaypy]T，z=[nzozazpz]T由第三章得到的斯坦福機械手運動學方程式（3.48）為

C2(C4C5C6

-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6

S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S6

1T6=S4C5C6+C4C6-S4C5S6+C4C6

00

C2C4S5+S2C5S2d3S2C4S5-C2C5-C2d3S4S5d2（4.13）

01第八頁，共三十一頁，編輯于2023年，星期五比較式（4.9）和式（4.13）矩陣中的第三行第四列元素相等得到

f13（p）=d2（4.14）或-S1

px＋C1py=d2（4.15）令px

=rcosΦ

（4.16）

py

=rsinΦ

（4.17）其中（4.18）（4.19）將式（4.16）和式（4.17）代入式（4.15）有

sinΦconθ1－conΦsinθ1

＝d2/r

（0<d2/r≤1）（4.20）由式（4.20）可得

sin(Φ－θ1)＝d2/r

（0<Φ－θ1<）（4.21）

con(Φ－θ1)＝（4.22）這里±號表示機械手是右肩結構（＋）還是左肩結構（－）。第九頁，共三十一頁，編輯于2023年，星期五由式（4.21）、（4.22）和（4.18）可得到第一個關節(jié)變量θ1的值（4.23）根據(jù)同樣的方法，利用式（4.9）和式（4.13）矩陣元素相等建立的相關的方程組，可得到其它各關節(jié)變量如下：

（4.24）（4.25）（4.26）（4.27）（4.28）第十頁，共三十一頁，編輯于2023年，星期五注意：在求解關節(jié)變量過程中如出現(xiàn)反正切函數(shù)的分子和分母太小，則計算結果誤差會很大，此時應重新選擇矩陣元素建立新的方程組再進行計算，直到獲得滿意的結果為止。同樣，如果計算結果超出了機械手關節(jié)的運動范圍，也要重新計算，直到符合機械手關節(jié)的運動范圍。由于機械手各關節(jié)變量的相互耦合，后面計算的關節(jié)變量與前面的關節(jié)變量有關，因此當前面關節(jié)變量的計算結果發(fā)生變化時，后面關節(jié)變量計算的結果也會發(fā)生變化，所以逆運動方程的解不是唯一的，我們應該根據(jù)機械手的組合形態(tài)和各關節(jié)的運動范圍，經(jīng)過多次反覆計算，從中選擇一組合理解。由此可見，求解機械手的逆運動方程是一個十分復雜的過程。第十一頁，共三十一頁，編輯于2023年，星期五4.4歐拉變換的逆運動學解

（InversesolutionofEulerAngles

）由第三章知歐拉變換為Euler(?,θ,ψ)＝Rot(z,?)Rot(y,θ)Rot(z,ψ)（4.29）我們用T來表示歐拉變換的結果，即T＝Euler(?,θ,ψ)（4.30）或T＝Rot(z,?)Rot(y,θ)Rot(z,ψ)（4.31）其中

（4.32）第十二頁，共三十一頁，編輯于2023年，星期五（4.33）第十三頁，共三十一頁，編輯于2023年，星期五比較式（4.32）和式（4.33）有（4.34）（4.35）（4.36）（4.37）（4.38）（4.39）（4.40）（4.41）（4.42）第十四頁，共三十一頁，編輯于2023年，星期五由式（4.42）可解出θ角（4.43）由式（4.40）和式（4.43）可解出φ角（4.44）由式（4.36）和式（4.43）可解出Ψ角（4.45）第十五頁，共三十一頁，編輯于2023年，星期五

這里需要指出的是，在我們采用式（4.43）~式（4.45）來計算θ、φ、Ψ時都是采用反余弦函數(shù)，而且式（4.44）和式（4.45）的分母為sinθ，這會帶來如下問題：

1）由于絕對值相同的正負角度的余弦相等，如cosθ＝cos（-θ），因此不能確定反余弦的結果是在那個象限；

2）當sinθ接近于0時，由式（4.43）和式（4.45）所求出的角度φ和Ψ是不精確的；

3）當θ＝0或±180o時，式（4.43）和式（4.45）無數(shù)值解。為此，我們必須尋求更為合理的求解方法。由三角函數(shù)的知識我們知道，反正切函數(shù)θ＝tan－1（x/y）所在的象限空間可由自變量的分子和分母的符號確定（如圖4.1所示），因此如果我們得到歐拉角的正切表達式，就不難確定歐拉角所在的象限。為此，我們采用本章第二節(jié)的方法，用Rot(z,?)－1左乘式（4.31）有Rot－1(z,?)T＝Rot(y,θ)Rot(z,ψ)（4.46）yx＋y＋y－x＋y－xx－y＋x－圖4.1正切函數(shù)所在象限θ第十六頁，共三十一頁，編輯于2023年，星期五即（4.47）將上式寫成如下形式（4.48）式中（4.49）（4.50）（4.51）同樣，上面三個式子中的x、y、z分別表示n、o、a、p矢量的各個分量，如（4.52）第十七頁，共三十一頁，編輯于2023年，星期五比較式（4.48）等號兩邊矩陣的第2行第3列元素可知（4.63）即（4.54）由此可得到（4.55）或（4.56）結果得到（4.57）或（4.58）第十八頁，共三十一頁，編輯于2023年，星期五上述結果相差180o，可根據(jù)實際系統(tǒng)的組合形態(tài)從中選擇一個合理解。如果ay和ax都為0，則式（4.57）和式（4.58）無定義，這是一種退化現(xiàn)象，此時φ值可任意設置，如φ＝0。由于角φ已求出，比較式（4.48）等號兩邊矩陣第1行第3列和第3行第3列元素相等有（4.59）（4.60）或（4.61）（4.62）由此可得（4.63）第十九頁，共三十一頁，編輯于2023年，星期五同樣比較式（4.48）等號兩邊矩陣的第2行第1列和第2行第2列元素可知（4.64）（4.65）或（4.66）（4.67）由此可得（4.68）至此，我們求出了歐拉變換的逆運動學解。第二十頁，共三十一頁，編輯于2023年，星期五例：給定一個直角坐標—歐拉角型機器人的最終期望姿態(tài)，求相

應的歐拉角。第二十一頁，共三十一頁，編輯于2023年，星期五解：由式（4-57）得到

由式（4-63）得到

由式（4-68）得到

第二十二頁，共三十一頁，編輯于2023年，星期五4.5RPY變換的逆運動學解（InversesolutionofRPY）第三章介紹的搖擺、俯仰和偏轉(RPY)變換的表達式如下T=RPY(?,θ,ψ)＝Rot(z,?)Rot(y,θ)Rot(x,ψ)（4.69）用Rot－1(z,?)左乘上式得到Rot－1(z,?)T＝Rot(y,θ)Rot(x,ψ)（4.70）將上式寫成式（4.48）的形式（4.71）式中（4.72）（4.73）（4.74）第二十三頁，共三十一頁，編輯于2023年，星期五由式（4.71）等號兩邊矩陣的第2行第1列元素相等有（4.75）由此得到（4.76）或（4.77）角φ已求出，根據(jù)式（4.71）等號兩邊矩陣的第3行第1列和第1行第1列元素相等有（4.78）（4.79）由此可得（4.80）第二十四頁，共三十一頁，編輯于2023年，星期五進一步比較式（4.71）等號兩邊矩陣元素，由第2行第3列和第2行第2列元素相等有（4.81）（4.82）由此可得（4.83）

至此，我們求出了RPY的逆運動學解。第二十五頁，共三十一頁，編輯于2023年，星期五例：下面給出一個RPY機器人手所吸取的最終位姿，求滾動角、

俯仰角和位移。第二十六頁，共三十一頁，編輯于2023年，星期五解：由式（4-76）得：由式（4-80），得：由式（4-83），得：第二十七頁，共三十一頁，編輯于2023年，星期五4.6