機(jī)器人軌跡規(guī)劃

1、第四章 機(jī)器人軌跡規(guī)劃本章主要要內(nèi)容4.1機(jī)機(jī)器人人軌跡規(guī)規(guī)劃概述述4.2插插補(bǔ)方方式分類類與軌跡跡控制4.3機(jī)機(jī)器人人軌跡插插補(bǔ)計(jì)算算4.4軌軌跡的的實(shí)時(shí)生生成所謂機(jī)器器人的規(guī)規(guī)劃(P1anning)，指的是是機(jī)器人根根據(jù)自身身的任務(wù)務(wù)，求得得完成這這一任務(wù)務(wù)的解決決方案的的過(guò)程。這里所所說(shuō)的任任務(wù)，具具有廣義義的概念念，既可可以指機(jī)機(jī)器人要要完成的的某一具具體任務(wù)務(wù)，也可可以是機(jī)機(jī)器人的的某個(gè)動(dòng)動(dòng)作，比比如手部部或關(guān)節(jié)節(jié)的某個(gè)個(gè)規(guī)定的的運(yùn)動(dòng)等等。4.1機(jī)機(jī)器人人軌跡規(guī)規(guī)劃概述述一、機(jī)器器人規(guī)劃劃的概念念為說(shuō)明機(jī)機(jī)器人規(guī)規(guī)劃的概概念，我我們舉下下面的例例子：在一些老老齡化比比較嚴(yán)重重的國(guó)家家

2、，開發(fā)發(fā)了各種種各樣的的機(jī)器人人專門用用于伺候候老人，這些機(jī)機(jī)器人有有不少是是采用聲聲控的方方式比比如主人人用聲音音命令機(jī)機(jī)器人“給我倒倒一杯開開水”，我們先先不考慮慮機(jī)器人人是如何何識(shí)別人人的自然然語(yǔ)言，而是著著重分析析一下機(jī)機(jī)器人在在得到這這樣一個(gè)個(gè)命令后后，如何何來(lái)完成成主人交交給的任任務(wù)。一、機(jī)器器人規(guī)劃劃的概念念首先，機(jī)機(jī)器人應(yīng)應(yīng)該把任任務(wù)進(jìn)行行分解，把主人人交代的的任務(wù)分分解成為為“取一一個(gè)杯子子”、“找到水水壺”、“打開開瓶塞”、“把把水倒人人杯中”、“把把水送給給主人”等一系系列子任任務(wù)。這這一層次次的規(guī)劃劃稱為任務(wù)規(guī)劃劃(Task planning)，它完完成總體體任務(wù)的的分

3、解。然后再針針對(duì)每一一個(gè)子任任務(wù)進(jìn)行行進(jìn)一步步的規(guī)劃劃。以“把水倒倒入杯中中”這一一子任務(wù)務(wù)為例，可以進(jìn)進(jìn)一步分分解成為為一系列列動(dòng)作，這一層層次的規(guī)規(guī)劃稱為為動(dòng)作規(guī)劃劃，它把實(shí)實(shí)現(xiàn)每一一個(gè)子任任務(wù)的過(guò)過(guò)程分解解為一系系列具體體的動(dòng)作作。取一個(gè)杯子 找到水壺 打開水壺把水倒入杯中 把水送給主人給主人倒一杯水把水倒入杯中 提起水壺到杯口上方 把水壺傾斜 把水壺豎直 把水壺放回原處為了實(shí)現(xiàn)現(xiàn)每一個(gè)個(gè)動(dòng)作，需要對(duì)對(duì)手部的的運(yùn)動(dòng)軌軌跡進(jìn)行行必要的的規(guī)定，這是手手部軌跡規(guī)劃劃(Handtrajectory planning)。為了使手手部實(shí)現(xiàn)現(xiàn)預(yù)定的的運(yùn)動(dòng)，就要知知道各關(guān)關(guān)節(jié)的運(yùn)運(yùn)動(dòng)規(guī)律律，這是是關(guān)節(jié)軌

4、跡跡規(guī)劃(Jointtrajectory planning)。最后才是是關(guān)節(jié)的的運(yùn)動(dòng)控制制(Motioncontrol)。取一個(gè)杯子 找到水壺 打開水壺把水倒入杯中 把水送給主人給主人倒一杯水 提起水壺到杯口上方 把水壺傾斜 把水壺豎直 把水壺放回原處 手部從A點(diǎn)移到B點(diǎn) 關(guān)節(jié)從C點(diǎn)移到D點(diǎn)上述例子子可以看看出，機(jī)機(jī)器人的的規(guī)劃是是分層次次的，從從高層的的任務(wù)規(guī)劃劃，動(dòng)作規(guī)劃劃到手部軌跡規(guī)規(guī)劃和關(guān)節(jié)軌跡跡規(guī)劃。在上述述例子中中，我們們沒(méi)有討討論力的的問(wèn)題，實(shí)際上上，對(duì)有有些機(jī)器器人來(lái)說(shuō)說(shuō)，力的的大小也也是要控控制的，這時(shí)，除了手手部或關(guān)關(guān)節(jié)的軌軌跡規(guī)劃劃，還要要進(jìn)行手手部和關(guān)關(guān)節(jié)輸出出力的規(guī)劃

5、劃。智能化程程度越高高，規(guī)劃劃的層次次越多，操作就就越簡(jiǎn)單單。軌跡規(guī)劃劃的目的的是將操操作人員員輸入的的簡(jiǎn)單的的任務(wù)描述述變?yōu)樵敿?xì)細(xì)的運(yùn)動(dòng)軌跡跡描述。例如，對(duì)對(duì)一般的的工業(yè)機(jī)機(jī)器人來(lái)來(lái)說(shuō)，操操作員可可能只輸輸入機(jī)械械手末端端的目標(biāo)標(biāo)位置和和方位，而規(guī)劃劃的任務(wù)務(wù)便是要要確定出出達(dá)到目目標(biāo)的關(guān)關(guān)節(jié)軌跡跡的形狀狀、運(yùn)動(dòng)動(dòng)的時(shí)間間和速度度等。這這里所說(shuō)說(shuō)的軌跡跡是指隨隨時(shí)間變變化的位位置、速速度和加加速度。簡(jiǎn)言之，機(jī)器人人的工作作過(guò)程，就是通通過(guò)規(guī)劃劃，將要要求的任任務(wù)變?yōu)闉槠谕牡倪\(yùn)動(dòng)和和力，由由控制環(huán)環(huán)節(jié)根據(jù)據(jù)期望的的運(yùn)動(dòng)和和力的信信號(hào)，產(chǎn)產(chǎn)生相應(yīng)應(yīng)的控制制作用，以使機(jī)機(jī)器人輸輸出實(shí)際際的運(yùn)動(dòng)動(dòng)

6、和力，從而完完成期望望的任務(wù)務(wù)。如下下圖所示示。這里里，機(jī)器器人實(shí)際際運(yùn)動(dòng)的的情況通通常還要要反饋給給規(guī)劃級(jí)級(jí)和控制制級(jí)，以以便對(duì)規(guī)規(guī)劃和控控制的結(jié)結(jié)果做出出適當(dāng)?shù)牡男拚Ｈ藱C(jī)接口規(guī)劃控制機(jī)器人本體要求的任務(wù) 期望的運(yùn)動(dòng)和力 實(shí)際的運(yùn)動(dòng)和力控制作用要求的任任務(wù)由操操作人員員輸入給給機(jī)器人人，為了了使機(jī)器器人操作作方便、使用簡(jiǎn)簡(jiǎn)單，必必須允許許操作人人員給出出盡量簡(jiǎn)簡(jiǎn)單的描描述。期望的運(yùn)運(yùn)動(dòng)和力力是進(jìn)行行機(jī)器人人控制所所必需的的輸入量量，它們們是機(jī)械械手末端端在每一一個(gè)時(shí)刻刻的位姿姿和速度度，對(duì)于于絕大多多數(shù)情況況，還要要求給出出每一時(shí)時(shí)刻期望望的關(guān)節(jié)節(jié)位移和和速度，有些控控制方法法還要求求給出

7、期期望的加加速度等等。4.1機(jī)機(jī)器人人軌跡規(guī)規(guī)劃概述述軌跡規(guī)劃劃？機(jī)器人在在作業(yè)空空間要完完成給定定的任務(wù)務(wù)，其手手部運(yùn)動(dòng)動(dòng)必須按按一定的的軌跡(trajectory)進(jìn)行。軌跡的生生成一般般是先給給定軌跡跡上的若若干個(gè)點(diǎn)點(diǎn)，將其其經(jīng)運(yùn)動(dòng)動(dòng)學(xué)反解解映射到到關(guān)節(jié)空空間，對(duì)對(duì)關(guān)節(jié)空空間中的的相應(yīng)點(diǎn)點(diǎn)建立運(yùn)運(yùn)動(dòng)方程程，然后后按這些些運(yùn)動(dòng)方方程對(duì)關(guān)關(guān)節(jié)進(jìn)行行插值，從而實(shí)實(shí)現(xiàn)作業(yè)業(yè)空間的的運(yùn)動(dòng)要要求，這這一過(guò)程程通常稱稱為軌跡規(guī)劃劃。二、軌跡跡規(guī)劃的的一般性性問(wèn)題工業(yè)機(jī)器器人的作作業(yè)可以以描述成成工具坐坐標(biāo)系T相對(duì)于于工件坐坐標(biāo)系S的一系系列運(yùn)動(dòng)動(dòng)。圖4.1機(jī)器人將將銷插入入工件孔孔中的作作業(yè)描述述圖4

8、.1所示的的將銷插插入工件件孔中的的作業(yè)，可以借借助工具具坐標(biāo)系系的一系系列位姿姿Pi(i=1，2，n)來(lái)描述述。二、軌跡跡規(guī)劃的的一般性性問(wèn)題用工具坐坐標(biāo)系相相對(duì)于工工件坐標(biāo)標(biāo)系的運(yùn)運(yùn)動(dòng)來(lái)描描述作業(yè)業(yè)路徑是是一種通通用的作作業(yè)描述述方法。它把作業(yè)業(yè)路徑描描述與具具體的機(jī)機(jī)器人、手爪或或工具分分離開來(lái)來(lái)，形成成了模型型化的作作業(yè)描述述方法，從而使使這種描描述既適適用于不不同的機(jī)機(jī)器人，也適用用于在同同一機(jī)器器人上裝裝夾不同同規(guī)格的的工具。圖4.2機(jī)器人的的初始狀狀態(tài)和終終止?fàn)顟B(tài)態(tài)對(duì)點(diǎn)位作業(yè)業(yè)(pickandplaceoperation)的機(jī)機(jī)器人，需要描描述它的的起始狀狀態(tài)和目目標(biāo)狀態(tài)態(tài)，即工

9、工具坐標(biāo)標(biāo)系的起起始值T0，目標(biāo)標(biāo)值Tf。在此，用“點(diǎn)”這個(gè)詞詞表示工工具坐標(biāo)標(biāo)系的位位置和姿姿態(tài)(簡(jiǎn)簡(jiǎn)稱位姿姿)。對(duì)于另外外一些作作業(yè)，如如弧焊和和曲面加加工等，不僅要要規(guī)定操操作臂的的起始點(diǎn)點(diǎn)和終止止點(diǎn)，而而且要指指明兩點(diǎn)點(diǎn)之間的的若干中中間點(diǎn)(稱路徑點(diǎn))，必須須沿特定定的路徑徑運(yùn)動(dòng)(路徑約束束)。這類類稱為連連續(xù)路徑徑運(yùn)動(dòng)(continuousPath motion)或或輪廓運(yùn)運(yùn)動(dòng)(contour motion)。在規(guī)劃?rùn)C(jī)機(jī)器人的的運(yùn)動(dòng)時(shí)時(shí)還需需要弄清清楚在其其路徑上上是否存存在障礙礙物(障礙約束束)。主要討論論連續(xù)路路徑的無(wú)無(wú)障礙的的軌跡規(guī)規(guī)劃方法法。軌跡規(guī)劃劃器可形形象地看看成為一一

10、個(gè)黑箱箱，其輸輸入包括括路徑的的“設(shè)定定”和“約束”，輸出出的是操操作臂末末端手部部的“位位姿序列列”，表表示手部部在各離離散時(shí)刻刻的中間間形位。軌跡規(guī)劃劃既可在在關(guān)節(jié)空空間也可可在直角角空間中中進(jìn)行，但是所所規(guī)劃的的軌跡函函數(shù)都必必須連續(xù)續(xù)和平滑滑，使機(jī)機(jī)器人的的運(yùn)動(dòng)平平穩(wěn)，不不平穩(wěn)的的運(yùn)動(dòng)將將加劇機(jī)機(jī)械部件件的磨損損，并導(dǎo)導(dǎo)致機(jī)器器人的振振動(dòng)和沖沖擊。為為此，要要求所選選擇的運(yùn)運(yùn)動(dòng)軌跡跡描述函函數(shù)必須須連續(xù)，而且它它的一階階導(dǎo)數(shù)(速度)，有時(shí)時(shí)甚至二二階導(dǎo)數(shù)數(shù)(加速速度)也也應(yīng)該連連續(xù)。在關(guān)節(jié)空空間進(jìn)行行規(guī)劃時(shí)時(shí)是將關(guān)關(guān)節(jié)變量量表示成成時(shí)間的的函數(shù)，并規(guī)劃劃它的一一階和二二階時(shí)間間導(dǎo)數(shù)。在

11、直角空空間進(jìn)行行規(guī)劃是是指將手手部位姿姿、速度度和加速速度表示示為時(shí)間間的函數(shù)數(shù)。而相相應(yīng)的關(guān)關(guān)節(jié)位移移、速度度和加速速度由手手部的信信息導(dǎo)出出。通常常通過(guò)運(yùn)運(yùn)動(dòng)學(xué)反反解得出出關(guān)節(jié)位位移、用用逆稚可可比求出出關(guān)節(jié)速速度，用用逆雅可可比及其其導(dǎo)數(shù)求求解關(guān)節(jié)節(jié)加速度度。三、軌跡跡的生成成方式(1)示示教-再現(xiàn)運(yùn)運(yùn)動(dòng)。這種運(yùn)動(dòng)動(dòng)由人手手把手示示教機(jī)器器人，定定時(shí)記錄錄各關(guān)節(jié)節(jié)變量，得到沿沿路徑運(yùn)運(yùn)動(dòng)時(shí)各各關(guān)節(jié)的的位移時(shí)時(shí)間函數(shù)數(shù)q(t)；再再現(xiàn)時(shí)，按內(nèi)存存中記錄錄的各點(diǎn)點(diǎn)的值產(chǎn)產(chǎn)生序列列動(dòng)作。(2)關(guān)關(guān)節(jié)空空間運(yùn)動(dòng)動(dòng)。這種運(yùn)動(dòng)動(dòng)直接在在關(guān)節(jié)空空間里進(jìn)進(jìn)行。由由于動(dòng)力力學(xué)參數(shù)數(shù)及其極極限值直直接在關(guān)關(guān)

12、節(jié)空間間里描述述，所以以用這種種方式求求最短時(shí)時(shí)間運(yùn)動(dòng)動(dòng)很方便便。(3)空空間直直線運(yùn)動(dòng)動(dòng)。這是一種種直角空空間里的的運(yùn)動(dòng)，它便于于描述空空間操作作，計(jì)算算量小，適宜簡(jiǎn)簡(jiǎn)單的作作業(yè)。(4)空空間曲曲線運(yùn)動(dòng)動(dòng)。這是一一種在描描述空間間中用明明確的函函數(shù)表達(dá)達(dá)的運(yùn)動(dòng)動(dòng)，如圓圓周運(yùn)動(dòng)動(dòng)、螺旋旋運(yùn)動(dòng)等等4.2插插補(bǔ)方式式分類與與軌跡控控制一、插補(bǔ)補(bǔ)方式分分類點(diǎn)位作業(yè)業(yè)（PTP=point-to-pointmotion）連續(xù)路徑徑作業(yè)（CP=continuous-path motion）路徑控制不插補(bǔ)關(guān)節(jié)插補(bǔ)(平滑)空間插補(bǔ)點(diǎn)位控制PTP(1) 各軸獨(dú)立快速到達(dá)。(2) 各關(guān)節(jié)最大加速度限制(1) 各軸

13、協(xié)調(diào)運(yùn)動(dòng)定時(shí)插補(bǔ)。(2) 各關(guān)節(jié)最大加速度限制連續(xù)路徑控制CP(1) 在空間插補(bǔ)點(diǎn)間進(jìn)行關(guān)節(jié)定時(shí)插補(bǔ)。(2) 用關(guān)節(jié)的低階多項(xiàng)式擬合空間直線使各軸協(xié)調(diào)運(yùn)動(dòng)。(3) 各關(guān)節(jié)最大加速度限制(1) 直線、圓弧、曲線等距插補(bǔ)。(2) 起停線速度、線加速度給定，各關(guān)節(jié)速度、加速度限制二、機(jī)機(jī)器人軌軌跡控制制過(guò)程圖4.3機(jī)機(jī)器人軌軌跡控制制過(guò)程機(jī)器人的的基本操操作方式式是示教教-再現(xiàn)現(xiàn)，即首首先教機(jī)機(jī)器人如如何做，機(jī)器人人記住了了這個(gè)過(guò)過(guò)程，于于是它可可以根據(jù)據(jù)需要重重復(fù)這個(gè)個(gè)動(dòng)作。操作過(guò)過(guò)程中，不可能能把空間間軌跡的的所有點(diǎn)點(diǎn)都示教教一遍使使機(jī)器人人記住，這樣太太繁瑣，也浪費(fèi)費(fèi)很多計(jì)計(jì)算機(jī)內(nèi)內(nèi)存。實(shí)實(shí)際

14、上，對(duì)于有有規(guī)律的的軌跡，僅示教教幾個(gè)特特征點(diǎn)，計(jì)算機(jī)機(jī)就能利利用插補(bǔ)補(bǔ)算法獲獲得中間間點(diǎn)的坐坐標(biāo)，如如直線需需要示教教兩點(diǎn)，圓弧需需要示教教三點(diǎn)，通過(guò)機(jī)機(jī)器人逆逆向運(yùn)動(dòng)動(dòng)學(xué)算法法由這些些點(diǎn)的坐坐標(biāo)求出出機(jī)器人人各關(guān)節(jié)節(jié)的位置置和角度度(1,n)，然后后由后面面的角位位置閉環(huán)環(huán)控制系系統(tǒng)實(shí)現(xiàn)現(xiàn)要求的的軌跡上上的一點(diǎn)點(diǎn)。繼續(xù)續(xù)插補(bǔ)并并重復(fù)上上述過(guò)程程，從而而實(shí)現(xiàn)要要求的軌軌跡。4.3機(jī)機(jī)器器人軌跡跡插值計(jì)計(jì)算一、直直線插補(bǔ)補(bǔ)空間直線線插補(bǔ)是是在已知知該直線線始末兩兩點(diǎn)的位位置和姿姿態(tài)的條條件下，求各軌軌跡中間間點(diǎn)(插插補(bǔ)點(diǎn))的位置置和姿態(tài)態(tài)。已知直線線始末兩兩點(diǎn)的坐坐標(biāo)值P0(X0，Y0，Z0)

15、、Pe(Xe，Ye，Ze)及姿姿態(tài)，其其中P0、Pe是相對(duì)對(duì)于基坐坐標(biāo)系的的位置。這些已已知的位位置和姿姿態(tài)通常常是通過(guò)過(guò)示教方方式得到到的。設(shè)設(shè)v為要求的的沿直線線運(yùn)動(dòng)的的速度；ts為插補(bǔ)補(bǔ)時(shí)間間間隔。直線長(zhǎng)度度直線插補(bǔ)補(bǔ)和圓弧弧插補(bǔ)是是機(jī)器人人系統(tǒng)中中的基本本插補(bǔ)算算法。對(duì)對(duì)于非直直線和圓圓弧軌跡跡，可用用直線或或圓弧逼逼近，以以實(shí)現(xiàn)這這些軌跡跡。ts間隔內(nèi)內(nèi)行程d=vts；插補(bǔ)總步步數(shù)N為L(zhǎng)/d+1的整整數(shù)部分分；各軸增量量：各插補(bǔ)點(diǎn)點(diǎn)坐標(biāo)值值：式中：i=0，1，2，N?？梢?jiàn)，兩兩個(gè)插補(bǔ)補(bǔ)點(diǎn)之間間的距離離正比于于要求的的運(yùn)動(dòng)速速度，只只有插補(bǔ)補(bǔ)點(diǎn)之間間的距離離足夠小小，才能能滿足一一定的

16、軌軌跡控制制精度要要求。機(jī)器人控控制系統(tǒng)統(tǒng)易于實(shí)實(shí)現(xiàn)定時(shí)時(shí)插補(bǔ)，例如采采用定時(shí)時(shí)中斷方方式每隔隔ts中中斷一次次進(jìn)行一一次插補(bǔ)補(bǔ)，計(jì)算算一次逆逆向運(yùn)動(dòng)動(dòng)學(xué)，輸輸出一個(gè)個(gè)給定值值。由于于ts僅僅為幾毫毫秒，機(jī)機(jī)器人沿沿著要求求軌跡的的速度一一般不會(huì)會(huì)很高，且機(jī)器器人總的的運(yùn)動(dòng)精精度不高高，故大大多數(shù)工工業(yè)機(jī)器器人采用用定時(shí)插插補(bǔ)方式式。二、圓弧弧插補(bǔ)1.平面面圓弧插插補(bǔ)平面圓弧弧是指圓圓弧平面面與基坐坐標(biāo)系的的三大平平面之一一重合，以XOY平面圓弧弧為例。已知不不在一條條直線上上的三點(diǎn)點(diǎn)P1、P2、P3及這三三點(diǎn)對(duì)應(yīng)應(yīng)的機(jī)器器人手端端的姿態(tài)態(tài)，如圖圖4.5及圖4.6所所示。圖4.5由已知的的三點(diǎn)

17、P1、P2、P3決定的圓圓弧圖4.6圓弧插補(bǔ)補(bǔ)設(shè)v為沿圓弧弧運(yùn)動(dòng)速速度；ts為插補(bǔ)補(bǔ)時(shí)時(shí)間間隔。(1)由由P1、P2、P3決定的的圓弧半半徑R。(2)總總的圓圓心角=1+2，即(3)ts時(shí)間內(nèi)內(nèi)角位移移量=tsv/R(4)總總插補(bǔ)補(bǔ)步數(shù)(取整數(shù)數(shù))N=/+ 1對(duì)Pi+1點(diǎn)的坐標(biāo)標(biāo)，有式中：Xi=Rcosi；Yi=Rsini。由i+1=i+可判斷是是否到插插補(bǔ)終點(diǎn)點(diǎn)。若i+1，則繼續(xù)續(xù)插補(bǔ)下下去；當(dāng)當(dāng)i+1時(shí)，則修修正最后后一步的的步長(zhǎng),故平面圓圓弧位置置插補(bǔ)為為2.空間間圓弧插插補(bǔ)空間圓弧弧是指三三維空間間任一平平面內(nèi)的的圓弧?？臻g圓弧弧插補(bǔ)可可分三步步來(lái)處理理：(1)把把三維維問(wèn)題轉(zhuǎn)轉(zhuǎn)化成二

18、二維，找找出圓弧弧所在平平面。(2)利利用二二維平面面插補(bǔ)算算法求出出插補(bǔ)點(diǎn)點(diǎn)坐標(biāo)(Xi+1,Yi+1)。(3)把把該點(diǎn)點(diǎn)的坐標(biāo)標(biāo)值轉(zhuǎn)變變?yōu)榛A(chǔ)礎(chǔ)坐標(biāo)系系下的值值，如圖圖4.7所示。通過(guò)不在在同一直直線上的的三點(diǎn)P1、P2、P3可確定一一個(gè)圓及及三點(diǎn)間間的圓弧弧，其圓圓心為OR,半徑為為R，圓弧所所在平面面與基礎(chǔ)礎(chǔ)坐標(biāo)系系平面的的交線分分別為AB、BC、CA。建立圓弧弧平面插插補(bǔ)坐標(biāo)標(biāo)系，即即把ORXRYRZR坐標(biāo)系原原點(diǎn)與圓圓心OR重合，設(shè)設(shè)ORXRYR平面為圓圓弧所在在平面，且保持持ZR為外法線線方向。求解兩坐坐標(biāo)系的的轉(zhuǎn)換矩矩陣。令TR表示由圓圓弧坐標(biāo)標(biāo)ORXRYRZR至基礎(chǔ)坐坐標(biāo)系OX

19、0Y0Z0的轉(zhuǎn)換矩矩陣。若ZR軸與基礎(chǔ)礎(chǔ)坐標(biāo)系系Z0軸的夾夾角為，XR軸與基礎(chǔ)礎(chǔ)坐標(biāo)系系的夾角角為，則可完完成下述述步驟：將XRYRZR的原點(diǎn)OR放到基礎(chǔ)礎(chǔ)原點(diǎn)O上；繞ZR軸轉(zhuǎn)，使X0與XR平行；再繞XR軸轉(zhuǎn)角，使Z0與ZR平行。這三步完完成了XRYRZR向X0Y0Z0的轉(zhuǎn)換，故總轉(zhuǎn)轉(zhuǎn)換矩陣陣應(yīng)為欲將基礎(chǔ)礎(chǔ)坐標(biāo)系系的坐標(biāo)標(biāo)值表示示在ORXRYRZR坐標(biāo)系，則要用用到TR的逆矩陣陣三、關(guān)節(jié)節(jié)空間插插補(bǔ)路徑點(diǎn)(結(jié)點(diǎn))通常用用工具坐坐標(biāo)系相相對(duì)于工工件坐標(biāo)標(biāo)系位姿姿來(lái)表示示。為了了求得在在關(guān)節(jié)空空間形成成所要求求的軌跡跡，首先先用運(yùn)動(dòng)動(dòng)學(xué)反解解將路徑徑點(diǎn)轉(zhuǎn)換換成關(guān)節(jié)節(jié)矢量角角度值，然后對(duì)對(duì)每個(gè)關(guān)關(guān)節(jié)

20、擬合合一個(gè)光光滑函數(shù)數(shù)，使之之從起始始點(diǎn)開始始，依次次通過(guò)所所有路徑徑點(diǎn)，最最后到達(dá)達(dá)目標(biāo)點(diǎn)點(diǎn)。對(duì)于每一一段路徑徑，各個(gè)關(guān)節(jié)節(jié)運(yùn)動(dòng)時(shí)時(shí)間均相相同，這這樣保證證所有關(guān)關(guān)節(jié)同時(shí)時(shí)到達(dá)路路徑點(diǎn)和和終止點(diǎn)點(diǎn)，從而得到到工具坐坐標(biāo)系應(yīng)應(yīng)有的位位置和姿姿態(tài)。但但是，盡盡管每個(gè)個(gè)關(guān)節(jié)在在同一段段路徑中中的運(yùn)動(dòng)動(dòng)時(shí)間相相同，各各個(gè)關(guān)節(jié)節(jié)函數(shù)之之間卻是是相互獨(dú)獨(dú)立的。三、關(guān)節(jié)節(jié)空間插插補(bǔ)在關(guān)節(jié)空空間中進(jìn)進(jìn)行軌跡跡規(guī)劃，需要給給定機(jī)器器人在起起始點(diǎn)、終止點(diǎn)點(diǎn)手臂的的形位。對(duì)關(guān)節(jié)進(jìn)進(jìn)行插值值時(shí)，應(yīng)應(yīng)滿足一一系列約約束條件件，在滿滿足所要要求的約約束條件件下，可可以選取取不同類類型的關(guān)關(guān)節(jié)插值值函數(shù)生生成不同同的軌跡

21、跡。三、關(guān)節(jié)節(jié)空間插插補(bǔ)1.三次次多項(xiàng)式式插值在操作臂臂運(yùn)動(dòng)的的過(guò)程中中，由于于相應(yīng)于于起始點(diǎn)點(diǎn)的關(guān)節(jié)節(jié)角度0是已知知的而而終止點(diǎn)點(diǎn)的關(guān)節(jié)節(jié)角f可以通通過(guò)運(yùn)動(dòng)動(dòng)學(xué)反解解得到，因此，運(yùn)動(dòng)軌軌跡的描描述，可可用起始始點(diǎn)關(guān)節(jié)節(jié)角與終終止點(diǎn)關(guān)關(guān)節(jié)角度度的一個(gè)個(gè)平滑插插值函數(shù)數(shù)(t)來(lái)表示示。(t)在t0=0時(shí)時(shí)刻的值值是起始始關(guān)節(jié)角角度0，終端端時(shí)刻tf的值是是終止關(guān)關(guān)節(jié)角度度f(wàn)。為實(shí)現(xiàn)單單個(gè)關(guān)節(jié)節(jié)的平穩(wěn)穩(wěn)運(yùn)動(dòng)，軌跡函函數(shù)(t)至少需需要滿足足四個(gè)約約束條件件，即兩端點(diǎn)位位置約束束和兩端點(diǎn)速速度約束束。端點(diǎn)位置置約束是是指起始始位姿和和終止位位姿分別別所對(duì)應(yīng)應(yīng)的關(guān)節(jié)節(jié)角度。(t)在時(shí)刻刻t0=0時(shí)時(shí)的

22、值是是起始關(guān)關(guān)節(jié)角度度0，在終終端時(shí)刻刻tf時(shí)的值值是終止止關(guān)節(jié)角角度f(wàn)，即1.三次次多項(xiàng)式式插值為滿足關(guān)關(guān)節(jié)運(yùn)動(dòng)動(dòng)速度的的連續(xù)性性要求，兩外還還有兩個(gè)個(gè)約束條條件，即即在起始點(diǎn)和和終止點(diǎn)點(diǎn)的關(guān)節(jié)節(jié)速度要要求。為了滿足足關(guān)節(jié)運(yùn)運(yùn)動(dòng)速度度連續(xù)性性的要求求，起始始點(diǎn)和終終止點(diǎn)的的關(guān)節(jié)速速度可簡(jiǎn)簡(jiǎn)單地設(shè)設(shè)定為零零。1.三次次多項(xiàng)式式插值上面給出出的四個(gè)個(gè)約束條條件可以以惟一地地確定一一個(gè)三次次多項(xiàng)式式運(yùn)動(dòng)過(guò)程程中的關(guān)關(guān)節(jié)速度度和加速速度則為為為求得三三次多項(xiàng)項(xiàng)式的系系數(shù)a0，a1，a2和a3，代以以給定的的約束條條件，有有方程組組求解該方方程組，可得對(duì)于起始始速度及及終止速速度為零零的關(guān)節(jié)節(jié)運(yùn)動(dòng)，滿

23、足連連續(xù)平穩(wěn)穩(wěn)運(yùn)動(dòng)要要求的三三次多項(xiàng)項(xiàng)式插值值函數(shù)為為關(guān)節(jié)角速速度和角角加速度度的表達(dá)達(dá)式為這里再次次指出：這組解只只適用于于關(guān)節(jié)起起始、終終止速度度為零的的運(yùn)動(dòng)情情況。圖4.8三三次多項(xiàng)項(xiàng)式插值值的關(guān)節(jié)節(jié)運(yùn)動(dòng)軌軌跡三次多項(xiàng)項(xiàng)式插值值的關(guān)節(jié)節(jié)運(yùn)動(dòng)軌軌跡曲線線如圖4.8所所示。由由圖可知知，其速速度曲線線為拋物物線，相相應(yīng)的加加速度曲曲線為直直線。例：設(shè)有有一臺(tái)轉(zhuǎn)轉(zhuǎn)動(dòng)關(guān)節(jié)節(jié)的機(jī)器器人，其其在執(zhí)行行一項(xiàng)作作業(yè)時(shí)關(guān)關(guān)節(jié)運(yùn)動(dòng)動(dòng)歷時(shí)3s。根據(jù)需需要，其其上某一一關(guān)節(jié)必必須運(yùn)動(dòng)動(dòng)平穩(wěn)，并具有有如下作作業(yè)狀態(tài)態(tài)：初始始時(shí)，關(guān)關(guān)節(jié)靜止止不動(dòng)，位置0=15；運(yùn)動(dòng)動(dòng)結(jié)束時(shí)時(shí)f =75，此時(shí)關(guān)關(guān)節(jié)速度度為0。試根據(jù)

24、據(jù)上述要要求規(guī)劃劃該關(guān)節(jié)節(jié)的運(yùn)動(dòng)動(dòng)。2.過(guò)路路徑點(diǎn)的的三次多多項(xiàng)式插插值一般情況況下要要求規(guī)劃劃過(guò)路徑徑點(diǎn)的軌軌跡。如如圖4.9所示示，機(jī)器器人作業(yè)業(yè)除在A、B點(diǎn)點(diǎn)有位姿姿要求外外，在路路徑點(diǎn)C、D也也有位姿姿要求。對(duì)于這這種情況況，假如如末端執(zhí)執(zhí)行器在在路徑點(diǎn)點(diǎn)停留，即各路路徑點(diǎn)上上速度為為0，則則軌跡規(guī)規(guī)劃可連連續(xù)直接接使用前前面介紹紹的三次次多項(xiàng)式式插值方方法；但但若末端端執(zhí)行器器只是經(jīng)經(jīng)過(guò)，并并不停留留，就需需要將前前述方法法推廣。實(shí)際上，可以把把所有路路徑點(diǎn)看看作是“起始點(diǎn)點(diǎn)”或“終止點(diǎn)點(diǎn)”，求求解逆運(yùn)運(yùn)動(dòng)學(xué)，得到相相應(yīng)的關(guān)關(guān)節(jié)矢量量值。然然后確定定所要求求的三次次多項(xiàng)式式插值函函數(shù)

25、，把把路徑點(diǎn)點(diǎn)平滑地地連接起起來(lái)。但但是，在在這些“起始點(diǎn)點(diǎn)”和“終止點(diǎn)點(diǎn)“的關(guān)節(jié)運(yùn)動(dòng)動(dòng)速度不不再是零零。設(shè)路徑點(diǎn)點(diǎn)上的關(guān)關(guān)節(jié)速度度已知，在某段段路徑上上，起始始點(diǎn)為0和，終止點(diǎn)點(diǎn)為f和，這時(shí)，確定三三次多項(xiàng)項(xiàng)式系數(shù)數(shù)的方法法與前所所述完全全一致，只是速速度約束束條件變變?yōu)槔眉s束束條件確確定三次次多項(xiàng)式式系數(shù)，有下列列方程組組：求解方程程組，得得如何來(lái)確確定路徑徑點(diǎn)上的的關(guān)節(jié)速速度？由上式確確定的三三次多項(xiàng)項(xiàng)式描述述了起始始點(diǎn)和終終止點(diǎn)具具有任意意給定位位置和速速度約束束條件的的運(yùn)動(dòng)軌軌跡。如何確定定路徑點(diǎn)點(diǎn)上的關(guān)關(guān)節(jié)速度度？（1）根根據(jù)工具具坐標(biāo)系系在直角角坐標(biāo)空空間中的的瞬時(shí)線線速度和和

26、角速度度來(lái)確定定每個(gè)路路徑點(diǎn)的的關(guān)節(jié)速速度。該方法利利用操作作臂在此此路徑點(diǎn)點(diǎn)上的逆逆雅可比比，把該該點(diǎn)的直直角坐標(biāo)標(biāo)速度“映射”為所要要求的關(guān)關(guān)節(jié)速度度。當(dāng)然然，如果果操作臂臂的某個(gè)個(gè)路徑點(diǎn)點(diǎn)是奇異異點(diǎn)，這這時(shí)就不不能任意意設(shè)置速速度值。按照該方方法生成成的軌跡跡雖然能能滿足用用戶設(shè)置置速度的的需要，但是逐逐點(diǎn)設(shè)置置速度畢畢竟要耗耗費(fèi)很大大的工作作量。如何確定定路徑點(diǎn)點(diǎn)上的關(guān)關(guān)節(jié)速度度？(2)在直角坐坐標(biāo)空間間或關(guān)節(jié)節(jié)空間中中采用適適當(dāng)?shù)膯l(fā)式方方法，由由控制系系統(tǒng)自動(dòng)動(dòng)地選擇擇路徑點(diǎn)點(diǎn)的速度度。圖表示一一種啟發(fā)發(fā)式選擇擇路徑點(diǎn)點(diǎn)速度的的方式。圖中0為起始點(diǎn)點(diǎn)；D為終止點(diǎn)點(diǎn)，A，B和C是路

27、徑點(diǎn)點(diǎn)，用細(xì)細(xì)實(shí)線表表示過(guò)路路徑點(diǎn)時(shí)時(shí)的關(guān)節(jié)節(jié)運(yùn)動(dòng)速速度。這這里所用用的啟發(fā)發(fā)式信息息從概念念到計(jì)算算方法都都很簡(jiǎn)單單，即，假設(shè)用用直線段段把這些些路徑點(diǎn)點(diǎn)依次連連接起來(lái)來(lái)，如果果相鄰線線段的斜斜率在路路徑點(diǎn)處處改變將將號(hào)，則則把速度度選定為為零；如如果相鄰鄰線段不不改變符符號(hào)，則則選取路路徑點(diǎn)兩兩側(cè)的線線段斜率率的平均均值作為為該點(diǎn)的的速度。因此，根據(jù)規(guī)規(guī)定的路路徑點(diǎn)，系統(tǒng)就就能夠按按此規(guī)則則自動(dòng)生生成相應(yīng)應(yīng)的路徑徑點(diǎn)速度度。如何確定定路徑點(diǎn)點(diǎn)上的關(guān)關(guān)節(jié)速度度？(3)為為了保證證每個(gè)路路徑點(diǎn)上上的加速速度連續(xù)續(xù)，由控控制系統(tǒng)統(tǒng)按此要要求自動(dòng)動(dòng)地選擇擇路徑點(diǎn)點(diǎn)的速度度。該方法為為了保證證路徑點(diǎn)

28、點(diǎn)處的加加速度連連續(xù)，可可以設(shè)法法用兩條條三次曲曲線在路路徑點(diǎn)處處按一定定規(guī)則聯(lián)聯(lián)接起來(lái)來(lái)，拼湊湊成所要要求的軌軌跡。其約束條條件是：聯(lián)接處處不僅速速度連續(xù)續(xù)，而且且加速度度也連續(xù)續(xù)，下面面具體地地說(shuō)明這這種方法法。設(shè)所經(jīng)過(guò)過(guò)的路徑徑點(diǎn)處的的關(guān)節(jié)角角度為v，與該點(diǎn)點(diǎn)相鄰的的前后兩兩點(diǎn)的關(guān)關(guān)節(jié)角分分別為0和g。設(shè)其路路徑點(diǎn)處處的關(guān)節(jié)節(jié)加速度度連續(xù)。如果路路徑點(diǎn)用用三次多多項(xiàng)式連連接，試試確定多多項(xiàng)式的的所有系系數(shù)。該機(jī)器人人路徑可可分為0到v段及v到g段兩段，可通過(guò)過(guò)由兩個(gè)個(gè)三次多多項(xiàng)式組組成的樣樣條函數(shù)數(shù)連接。設(shè)從0到v的三次多多項(xiàng)式插插值函數(shù)數(shù)為而從v到g的三次多多項(xiàng)式插插值函數(shù)數(shù)為時(shí)間區(qū)間

29、間分別是是0,tf1、0,tf2；約束束是：對(duì)于第一一個(gè)方程程：對(duì)于第二二個(gè)方程程：二個(gè)方程程：第一條曲曲線在t=tf1時(shí)的速度度等于第第二條曲曲線在t=0的的速度；第一條曲曲線在t=tf1時(shí)的加速速度等于于第二條條曲線在在t=0時(shí)的加加速度兩個(gè)方程程共有8個(gè)未知知數(shù)：已知條件件有8個(gè)個(gè)：第一方程程：第二方程程：第一條曲曲線在t=tf1時(shí)的速度度等于第第二條曲曲線在t=0的的速度；第一條曲曲線在t=tf1時(shí)的加速速度等于于第二條條曲線在在t=0時(shí)的加速度當(dāng)tf1=tf2=tf時(shí)的解為為：3.高高階多項(xiàng)項(xiàng)式插值值如果對(duì)于于運(yùn)動(dòng)軌軌跡的要要求更為為嚴(yán)格，約束條條件增多多，那么么三次多多項(xiàng)式就就不能

30、滿滿足需要要，必須須用更高高階的多多項(xiàng)式對(duì)對(duì)運(yùn)動(dòng)軌軌跡的路路徑段進(jìn)進(jìn)行插值值。例如如，對(duì)對(duì)某段路路徑的超超始點(diǎn)和和終止點(diǎn)點(diǎn)都有規(guī)規(guī)定了關(guān)關(guān)節(jié)的位位置、速速度和加加速度要要求，則則要用一一個(gè)五次次多項(xiàng)式式進(jìn)行插插值。1）超始始點(diǎn)的位位置0、速度0、加速度度0；2）終止止點(diǎn)的位位置f、速度f(wàn)、加速度度f(wàn)；在給定六六個(gè)約束束條件下下，方程程的解為為：五次多項(xiàng)項(xiàng)式有六六個(gè)系數(shù)數(shù)，必須須滿足六六個(gè)約束束條件：六個(gè)約束束條件是是：4.用拋拋物線過(guò)過(guò)渡的線線性插值值在關(guān)節(jié)空空間軌跡跡規(guī)劃中中，對(duì)于于給定起起始點(diǎn)和和終止點(diǎn)點(diǎn)的情況況選擇線線性函數(shù)數(shù)插值較較為簡(jiǎn)單單，如圖圖4.10所示示。然而，單單純線性性插值

31、會(huì)會(huì)導(dǎo)致起起始點(diǎn)和和終止點(diǎn)點(diǎn)的關(guān)節(jié)節(jié)運(yùn)動(dòng)速速度不連連續(xù)，且且加速度度無(wú)窮大大，顯然然，在兩兩端點(diǎn)會(huì)會(huì)造成剛剛性沖擊擊。為此應(yīng)對(duì)對(duì)線性函函數(shù)插值值方案進(jìn)進(jìn)行修正正，在線線性插值值兩端點(diǎn)點(diǎn)的鄰域域內(nèi)設(shè)置置一段拋拋物線形形緩沖區(qū)區(qū)段。由由于拋物物線函數(shù)數(shù)對(duì)于時(shí)時(shí)間的二二階導(dǎo)數(shù)數(shù)為常數(shù)數(shù)，即相相應(yīng)區(qū)段段內(nèi)的加速度恒恒定，這樣保保證起始始點(diǎn)和終終止點(diǎn)的的速度平平滑過(guò)渡渡，從而而使整個(gè)個(gè)軌跡上上的位置置和速度度連續(xù)。線性函函數(shù)與兩兩段拋物物線函數(shù)數(shù)平滑地地銜接在在一起形形成的軌軌跡稱為為帶有拋物物線過(guò)渡渡域的線線性軌跡跡，如圖4.11所示。為了構(gòu)造造這段運(yùn)運(yùn)動(dòng)軌跡跡，假設(shè)設(shè)兩端的的拋物線線軌跡具具有相同同

32、的持續(xù)續(xù)時(shí)間，具有大大小相同同而符號(hào)號(hào)相反的的恒加速速度。對(duì)對(duì)于這種種路徑規(guī)規(guī)劃存在在有多個(gè)個(gè)解，其其軌跡不不惟一。但是，每條路路徑都對(duì)對(duì)稱于時(shí)時(shí)間中點(diǎn)點(diǎn)th和位置中中點(diǎn)h。要保證路路徑軌跡跡的連續(xù)續(xù)、光滑滑，即要要求拋物物線軌跡跡的終點(diǎn)點(diǎn)速度必必須等于于線性段段的速度度，故有有下列關(guān)關(guān)系設(shè)關(guān)節(jié)從從起始點(diǎn)點(diǎn)到終止止點(diǎn)的總總運(yùn)動(dòng)時(shí)時(shí)間為tf，則tf=2th，并注意意到 一般情況下，0、f、tf是已知條件，這樣，可以選擇相應(yīng)的和得到相應(yīng)的軌跡。 為保證有解，加速度值必須選得足夠大，即 通常的做法是先選定加速度的值，然后按下式求出相應(yīng)的 當(dāng)上式中中的等號(hào)號(hào)成立時(shí)時(shí)，軌跡跡線性段段的長(zhǎng)度度縮減為為零，整整個(gè)軌跡跡由兩個(gè)個(gè)過(guò)渡域域組成，這兩個(gè)個(gè)過(guò)渡域域在銜接接處的斜斜率(關(guān)關(guān)節(jié)速度度)相等等；加速速度的取取值愈大大，過(guò)渡渡域的長(zhǎng)長(zhǎng)度會(huì)變變得愈短短，若加加速度趨趨于無(wú)窮窮大，軌軌跡又復(fù)復(fù)歸到簡(jiǎn)簡(jiǎn)單的線線性插值值情況。例：已知知條件為為0=15，f=75，tf