機(jī)器人正逆運(yùn)動(dòng)學(xué)

機(jī)器人正逆運(yùn)動(dòng)學(xué)課件第一頁，共五十九頁，2022年，8月28日D-H表示法學(xué)習(xí)目標(biāo)：1.理解D-H法原理

2.學(xué)會(huì)用D-H法對機(jī)器人建模學(xué)習(xí)重點(diǎn)：1.給關(guān)節(jié)指定參考坐標(biāo)系

2.制定D-H參數(shù)表

3.利用參數(shù)表計(jì)算轉(zhuǎn)移矩陣第二頁，共五十九頁，2022年，8月28日背景簡介：

1955年，Denavit和Hartenberg(迪納維特和哈坦伯格)提出了這一方法，后成為表示機(jī)器人以及對機(jī)器人建模的標(biāo)準(zhǔn)方法，應(yīng)用廣泛。

總體思想：首先給每個(gè)關(guān)節(jié)指定坐標(biāo)系，然后確定從一個(gè)關(guān)節(jié)到下一個(gè)關(guān)節(jié)進(jìn)行變化的步驟，這體現(xiàn)在兩個(gè)相鄰參考坐標(biāo)系之間的變化，將所有變化結(jié)合起來，就確定了末端關(guān)節(jié)與基座之間的總變化，從而建立運(yùn)動(dòng)學(xué)方程，進(jìn)一步對其求解。第三頁，共五十九頁，2022年，8月28日1.第一個(gè)關(guān)節(jié)指定為關(guān)節(jié)n,第二個(gè)關(guān)節(jié)為n+1,其余關(guān)節(jié)以此類推。坐標(biāo)系的確定2.Z軸確定規(guī)則：如果關(guān)節(jié)是旋轉(zhuǎn)的，Z軸位于按右手規(guī)則旋轉(zhuǎn)的方向，轉(zhuǎn)角為關(guān)節(jié)變量。如果關(guān)節(jié)是滑動(dòng)的，Z軸為沿直線運(yùn)動(dòng)的方向，連桿長度d為關(guān)節(jié)變量。關(guān)節(jié)n處Z軸下標(biāo)為n-1。第四頁，共五十九頁，2022年，8月28日3.X軸確定規(guī)則情況1：兩關(guān)節(jié)Z軸既不平行也不相交取兩Z軸公垂線方向作為X軸方向，命名規(guī)則同Z軸。情況2：兩關(guān)節(jié)Z軸平行此時(shí)，兩Z軸之間有無數(shù)條公垂線，可挑選與前一關(guān)節(jié)的公垂線共線的一條公垂線。情況3：兩關(guān)節(jié)Z軸相交取兩條Z軸的叉積方向作為X軸。4.Y軸確定原則取X軸、Z軸叉積方向作為Y軸方向。（右手）5.變量選擇原則用θn+1角表示Xn到Xn+1繞Zn軸的旋轉(zhuǎn)角;dn+1表示從Xn到Xn+1沿Zn測量的距離;an+1表示關(guān)節(jié)偏移，an+1是從Zn到Zn+1沿Xn+1測量的距離;角α表示關(guān)節(jié)扭轉(zhuǎn),αn+1是從Zn到Zn+1繞Xn+1旋轉(zhuǎn)的角度。

通常情況下，只有θ和d是關(guān)節(jié)變量。第五頁，共五十九頁，2022年，8月28日斯坦福機(jī)器人斯坦福機(jī)器人開始的兩個(gè)關(guān)節(jié)是旋轉(zhuǎn)的，第三個(gè)關(guān)節(jié)是滑動(dòng)的，最后三個(gè)腕關(guān)節(jié)全是旋轉(zhuǎn)關(guān)節(jié)第六頁，共五十九頁，2022年，8月28日第七頁，共五十九頁，2022年，8月28日例1：Stanford機(jī)器人運(yùn)動(dòng)學(xué)方程第八頁，共五十九頁，2022年，8月28日A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O5d3z6x6y6O6d6z0y0x0O0為右手坐標(biāo)系原點(diǎn)Oi：Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn)zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意xi軸：Zi和Zi-1構(gòu)成的面的法線yi軸：按右手定則ai—沿xi

軸，zi-1軸與xi

軸交點(diǎn)到Oi的距離αi—繞xi軸，由zi-1轉(zhuǎn)向zidi—沿zi-1軸，zi-1軸和xi交點(diǎn)至Oi–1坐標(biāo)系原點(diǎn)的距離θi—繞zi-1軸，由xi-1轉(zhuǎn)向xi關(guān)節(jié)1坐標(biāo)系0關(guān)節(jié)2坐標(biāo)系1關(guān)節(jié)3坐標(biāo)系2連桿0連桿1連桿2連桿3連桿4連桿5關(guān)節(jié)4坐標(biāo)系3關(guān)節(jié)5坐標(biāo)系4關(guān)節(jié)6坐標(biāo)系5第九頁，共五十九頁，2022年，8月28日解：第十頁，共五十九頁，2022年，8月28日第十一頁，共五十九頁，2022年，8月28日第十二頁，共五十九頁，2022年，8月28日第十三頁，共五十九頁，2022年，8月28日θ1θ2θ3θ4θ5θ6關(guān)節(jié)變量都是θ例2、PUMA560運(yùn)動(dòng)學(xué)方程（六個(gè)自由度，全部是旋轉(zhuǎn)關(guān)節(jié)）第十四頁，共五十九頁，2022年，8月28日PUMA560機(jī)器人的連桿及關(guān)節(jié)編號第十五頁，共五十九頁，2022年，8月28日為右手坐標(biāo)系，Yi軸：按右手定則Zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意Xi軸：Zi和Zi-1構(gòu)成的面的法線，

或連桿i兩端軸線Ai與Ai+1的公垂線(即：Zi和Zi-1的公垂線)原點(diǎn)Oi：Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn)，或Zi與Xi的交點(diǎn)ai—沿xi

軸，zi-1軸與xi

軸交點(diǎn)到Oi的距離αi—繞xi軸，由zi-1轉(zhuǎn)向zidi—沿zi-1軸，zi-1軸和xi交點(diǎn)至Oi–1坐標(biāo)系原點(diǎn)的距離θi—繞zi-1軸，由xi-1轉(zhuǎn)向xiA1A2A3A4A5A6O1O0第十六頁，共五十九頁，2022年，8月28日第十七頁，共五十九頁，2022年，8月28日第十八頁，共五十九頁，2022年，8月28日

對下圖所示簡單機(jī)器人，根據(jù)D-H法，建立必要坐標(biāo)系及參數(shù)表。例3第十九頁，共五十九頁，2022年，8月28日第一步：根據(jù)D-H法建立坐標(biāo)系的規(guī)則建立坐標(biāo)系第二十頁，共五十九頁，2022年，8月28日第二步：將做好的坐標(biāo)系簡化為我們熟悉的線圖形式第二十一頁，共五十九頁，2022年，8月28日第三步：根據(jù)建立好的坐標(biāo)系，確定各參數(shù)，并寫入D-H參數(shù)表#da1009020030040-90500906000第二十二頁，共五十九頁，2022年，8月28日#da1009020030040-90500906000第二十三頁，共五十九頁，2022年，8月28日第四步：將參數(shù)代入A矩陣，可得到第二十四頁，共五十九頁，2022年，8月28日第5步求出總變化矩陣第二十五頁，共五十九頁，2022年，8月28日

依次寫出從基坐標(biāo)系到手爪坐標(biāo)系之間相鄰兩坐標(biāo)系的齊次變換矩陣，它們依次連乘的結(jié)果就是末端執(zhí)行器（手爪）在基坐標(biāo)系中的空間描述，即已知q1,q2,…,qn，求，稱為運(yùn)動(dòng)學(xué)正解；已知，求q1,q2,…,qn，稱為運(yùn)動(dòng)學(xué)反解。上式稱為運(yùn)動(dòng)方程。綜上：第二十六頁，共五十九頁，2022年，8月28日正解反解第二十七頁，共五十九頁，2022年，8月28日§1.5機(jī)器人的逆運(yùn)動(dòng)學(xué)解

給定機(jī)器人終端位姿，求各關(guān)節(jié)變量，稱求機(jī)器人運(yùn)動(dòng)學(xué)逆解。讓我們通過下面這道例題來了解一下機(jī)器人逆運(yùn)動(dòng)學(xué)求解的一般步驟。前面例子最后方程為：求逆運(yùn)動(dòng)學(xué)方程的解第二十八頁，共五十九頁，2022年，8月28日

根據(jù)第3行第4列元素對應(yīng)相等可得到依次用左乘上面兩個(gè)矩陣，得到：第二十九頁，共五十九頁，2022年，8月28日根據(jù)1，4元素和2，4元素，可得到：將上面兩個(gè)方程兩邊平方相加，并利用和差化積公式得到第三十頁，共五十九頁，2022年，8月28日已知于是可得到：依次類推，分別在方程2.19兩邊左乘A1~A4的逆，可得到第三十一頁，共五十九頁，2022年，8月28日接下來再一次利用式由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：第三十二頁，共五十九頁，2022年，8月28日最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：θ1θ2θ3θ4θ5θ6關(guān)節(jié)變量都是θ第三十三頁，共五十九頁，2022年，8月28日§2.10機(jī)器人的運(yùn)動(dòng)學(xué)編程

在實(shí)際應(yīng)用中，對運(yùn)動(dòng)學(xué)的求解是相當(dāng)繁瑣和耗時(shí)的，因此需要用計(jì)算機(jī)編程來實(shí)現(xiàn)。并且應(yīng)盡量避免使用矩陣求逆或高斯消去法等相對繁瑣的算法。正確的算法是：第三十四頁，共五十九頁，2022年，8月28日§2.11設(shè)計(jì)項(xiàng)目

利用本書中所介紹的四自由度機(jī)器人，結(jié)合本章所學(xué)的知識進(jìn)行四自由度機(jī)器人的正逆運(yùn)動(dòng)學(xué)分析。

SCARA型機(jī)器人的運(yùn)動(dòng)學(xué)模型的建立，包括機(jī)器人運(yùn)動(dòng)學(xué)方程的表示，以及運(yùn)動(dòng)學(xué)正解、逆解等，這些是研究機(jī)器人控制的重要基礎(chǔ)，也是開放式機(jī)器人系統(tǒng)軌跡規(guī)劃的重要基礎(chǔ)。為了描述SCARA型機(jī)器人各連桿之間的數(shù)學(xué)關(guān)系，采用D-H法。SCARA型機(jī)器人操作臂可以看作是一個(gè)開式運(yùn)動(dòng)鏈。它是由一系列連桿通過轉(zhuǎn)動(dòng)或移動(dòng)關(guān)節(jié)串聯(lián)而成的。為了研究操作臂各連桿之間的位移關(guān)系，可在每個(gè)連桿上固接一個(gè)坐標(biāo)系，然后描述這些坐標(biāo)系之間的關(guān)系。第三十五頁，共五十九頁，2022年，8月28日SCARA（SelectiveComplianceAssemblyRobotArm裝配機(jī)器人臂）機(jī)器人坐標(biāo)系的建立

1.SCARA機(jī)器人坐標(biāo)系建立原則根據(jù)D-H坐標(biāo)系建立方法，SCARA機(jī)器人的每個(gè)關(guān)節(jié)坐標(biāo)系的建立可參照以下的三原則(1)軸沿著第n個(gè)關(guān)節(jié)的運(yùn)動(dòng)軸;基坐標(biāo)系的選擇為:當(dāng)?shù)谝魂P(guān)節(jié)變量為零時(shí)，零坐標(biāo)系與一坐標(biāo)系重合。(2)軸垂直于軸并指向離開軸的方向。(3)軸的方向按右手定則確定。

2.構(gòu)件參數(shù)的確定根據(jù)D-H構(gòu)件坐標(biāo)系表示法，構(gòu)件本身的結(jié)構(gòu)參數(shù)、和相對位置參數(shù)、可由以下的方法確定:(1)為繞軸(按右手定則)由軸到軸的關(guān)節(jié)角。(2)為沿軸，將軸平移至軸的距離。(3)為沿軸從量至軸的距離。(4)為繞軸(按右手定則)由軸到軸的偏轉(zhuǎn)角。第三十六頁，共五十九頁，2022年，8月28日3.變換矩陣的建立全部的連桿規(guī)定坐標(biāo)系之后，就可以按照下列的順序來建立相鄰兩連桿n-1和n之間的相對關(guān)系:(1)繞軸轉(zhuǎn)角。(2)沿軸移動(dòng)。(3)繞軸轉(zhuǎn)角。(4)沿軸移動(dòng)。這種關(guān)系可由表示連桿n對連桿n-1相對位置齊次變換來表征。即：展開上式得

第三十七頁，共五十九頁，2022年，8月28日由于描述第n個(gè)連桿相對于第n-1連桿的位姿，對于SCARA教學(xué)機(jī)器人(四個(gè)自由度)，機(jī)器人的末端裝置即為連桿4的坐標(biāo)系，它與基座的關(guān)系為:第三十八頁，共五十九頁，2022年，8月28日如上圖坐標(biāo)系，可寫出連桿n相對于n-1變換矩陣:

其中：以下相同。相應(yīng)連桿初始位置及參數(shù)列于表2.4，表中、為關(guān)節(jié)變量。

構(gòu)件1000102001030010400010第三十九頁，共五十九頁，2022年，8月28日

各連桿變換矩陣相乘，可得到SCARA機(jī)器人末端執(zhí)行器的位姿方程(正運(yùn)動(dòng)學(xué)方程)為下式它表示了SCARA手臂變換矩陣，它描述了末端連桿坐標(biāo)系{4}相對基坐標(biāo)系{0}的位姿。SCARA機(jī)器人的正運(yùn)動(dòng)學(xué)分析

第四十頁，共五十九頁，2022年，8月28日

SCARA機(jī)器人的逆運(yùn)動(dòng)學(xué)分析

1.求關(guān)節(jié)變量為了分離變量，對方程的兩邊同時(shí)左乘,得:即：第四十一頁，共五十九頁，2022年，8月28日

左右矩陣中的第一行第四個(gè)元素(1.4)，第二行第四個(gè)元素(2.4)分別相等。即:

由以上兩式聯(lián)立可得:

式中：

第四十二頁，共五十九頁，2022年，8月28日2求關(guān)節(jié)變量由式(2.87)可得:

式中：第四十三頁，共五十九頁，2022年，8月28日3求關(guān)節(jié)變量再令左右矩陣中的第三行第四個(gè)元素(3.4)相等，可得:

4求關(guān)節(jié)變量再令左右矩陣中的第一行第一個(gè)元素、第二行第一個(gè)元素(1.1，2.1)分別相等，即：由上兩式可求得:

第四十四頁，共五十九頁，2022年，8月28日

至此，機(jī)器人的所有運(yùn)動(dòng)學(xué)逆解都已求出。在逆解的求解過程中只進(jìn)行了一次矩陣逆乘，從而使計(jì)算過程大為簡化，從的表達(dá)式中可以看出它有兩個(gè)解，所以SCARA機(jī)器人應(yīng)該存在兩組解。運(yùn)動(dòng)學(xué)分析提供了機(jī)器人運(yùn)動(dòng)規(guī)劃和軌跡控制的理論基礎(chǔ)。第四十五頁，共五十九頁，2022年，8月28日對機(jī)器人相關(guān)概念的補(bǔ)充退化：當(dāng)機(jī)器人失去一個(gè)自由度，并因此不按所期望的狀態(tài)運(yùn)動(dòng)時(shí)即稱為退化。退化發(fā)生條件：1.機(jī)器人達(dá)到物理極限，不能進(jìn)一步運(yùn)動(dòng)2.兩個(gè)相似關(guān)節(jié)共線不靈巧區(qū)域：能對機(jī)器人定位不定姿的區(qū)域稱為不靈巧區(qū)域。D-H法的局限性：無法表示關(guān)于y軸的運(yùn)動(dòng)。第四十六頁，共五十九頁，2022年，8月28日退化狀態(tài)下的機(jī)器人第四十七頁，共五十九頁，2022年，8月28日總結(jié)1用矩陣表示點(diǎn)，向量，坐標(biāo)系及變換的方法2正逆運(yùn)動(dòng)學(xué)方程的建立3用D-H法建立坐標(biāo)系及變化方程4正逆運(yùn)動(dòng)學(xué)方程的求解第四十八頁，共五十九頁，2022年，8月28日§9.2機(jī)器人桿件，關(guān)節(jié)和它們的參數(shù)

§9.2.1桿件與關(guān)節(jié)操作機(jī)由一串用轉(zhuǎn)動(dòng)或平移（棱柱形）關(guān)節(jié)連接的剛體（桿件）組成每一對關(guān)節(jié)桿件構(gòu)成一個(gè)自由度，因此N個(gè)自由度的操作機(jī)就有N對關(guān)節(jié)—桿件。0號桿件（一般不把它當(dāng)作機(jī)器人的一部分）固聯(lián)在機(jī)座上，通常在這里建立一個(gè)固定參考坐標(biāo)系，最后一個(gè)桿件與工具相連關(guān)節(jié)和桿件均由底座向外順序排列，每個(gè)桿件最多和另外兩個(gè)桿件相聯(lián)，不構(gòu)成閉環(huán)。

關(guān)節(jié)桿件末端操作手機(jī)座兩自由度關(guān)節(jié)第四十九頁，共五十九頁，2022年，8月28日關(guān)節(jié)：一般說來，兩個(gè)桿件間是用低副相聯(lián)的只可能有6種低副關(guān)節(jié)：旋轉(zhuǎn)（轉(zhuǎn)動(dòng)）、棱柱（移動(dòng)）、圓柱形、球形、螺旋和平面，其中只有旋轉(zhuǎn)和棱柱形關(guān)節(jié)是串聯(lián)機(jī)器人操作機(jī)常見的，各種低副形狀如下圖所示：旋轉(zhuǎn)棱柱形柱形球形螺旋形平面第五十頁，共五十九頁，2022年，8月28日§9.2.2桿件參數(shù)的設(shè)定

條件關(guān)節(jié)串聯(lián)每個(gè)桿件最多與2個(gè)桿件相連，如Ai與Ai-1和Ai+1相連。第i關(guān)節(jié)的關(guān)節(jié)軸Ai

位于2個(gè)桿件相連接處，如圖所示，

i-1關(guān)節(jié)和i+1關(guān)節(jié)也各有一個(gè)關(guān)節(jié)軸Ai-1

和Ai+1。AiAi+1Ai-1第五十一頁，共五十九頁，2022年，8月28日

桿件參數(shù)的定義——、、和—li和li-1在Ai軸線上的交點(diǎn)之間的距離。AiAi+1Ai-1—

li和li-1之間的夾角，按右手定則由li-1轉(zhuǎn)向li。

由運(yùn)動(dòng)學(xué)的觀點(diǎn)來看，桿件保持其兩端關(guān)節(jié)間的形態(tài)不變，這種形態(tài)由兩個(gè)參數(shù)決定：桿件長度li

和桿件扭轉(zhuǎn)角。桿件的相對位置關(guān)系，由另外兩個(gè)參數(shù)決定：桿件的距離di和桿件的回轉(zhuǎn)角。

li—關(guān)節(jié)Ai軸和Ai+1軸線公法線的長度。

—關(guān)節(jié)i軸線與i+1軸線在垂直于li平面內(nèi)的夾角。第五十二頁，共五十九頁，2022年，8月28日

上述4個(gè)參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位置關(guān)系。在轉(zhuǎn)動(dòng)關(guān)節(jié)中，li,αi,di是固定值，θi是變量。在移動(dòng)關(guān)節(jié)中，li,αi,θi是固定值，di是變量。第五十三頁，共五十九頁，2022年，8月28日§9.3機(jī)器人關(guān)節(jié)坐標(biāo)系的建立對于每個(gè)桿件都可以在關(guān)節(jié)軸處建立一個(gè)正規(guī)的笛卡兒坐標(biāo)系（xi,yi,zi），（i=1,2,…,n），n是自由度數(shù)，再加上基座坐標(biāo)系，一共有（n+1）個(gè)坐標(biāo)系?；鴺?biāo)系∑O0定義為0號坐標(biāo)系（x0,y0,z0）,它也是機(jī)器人的慣性坐標(biāo)系，0號坐標(biāo)系在基座上的位置和方向可任選，但z0軸線必須與關(guān)節(jié)1的軸線重合，位置和方向可任選；最后一個(gè)坐標(biāo)系（n關(guān)節(jié)），可以設(shè)在手的任意部位，但必須保證zn與zn-1

垂直。第五十四頁，共五十九頁，2022年，8月28日機(jī)器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機(jī)器人各桿件和終端之間的相對運(yùn)動(dòng)，對建立運(yùn)動(dòng)方程和動(dòng)力學(xué)研究是基礎(chǔ)性的工作。為了描述機(jī)器人各桿件和終端之間轉(zhuǎn)動(dòng)或移動(dòng)關(guān)系，Denavit和Hartenberg于1955年提出了一種為運(yùn)動(dòng)鏈中每個(gè)桿件建立附體坐標(biāo)系的矩陣方法（D-H方法），建立原則如下：

§9.3.1D-H關(guān)節(jié)坐標(biāo)系建立原則右手坐標(biāo)系原點(diǎn)Oi：設(shè)在li與Ai+1軸線的交點(diǎn)上Zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意Xi軸：與公法線Li重合，指向沿Li由Ai軸線指向Ai+1軸線Yi軸：按右手定則第五十五頁，共五十九頁，2022年，8月28日

§9.3.2關(guān)節(jié)坐標(biāo)系的建立方法AiAi+1Ai-1原點(diǎn)Oi：設(shè)在li與Ai+1軸線的交點(diǎn)上zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意xi軸：與公法線li重合，指向沿li由Ai軸線指向Ai+1軸線yi軸：按右手定則

桿件長度li

—沿x