機器人軌跡規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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