機器人軌跡規(guī)劃-課件

第四章機器人軌跡規(guī)劃第四章機器人軌跡規(guī)劃本章主要內容4.1機器人軌跡規(guī)劃概述4.2插補方式分類與軌跡控制4.3機器人軌跡插補計算4.4軌跡的實時生成本章主要內容4.1機器人軌跡規(guī)劃概述

所謂機器人的規(guī)劃(P1anning)，指的是——機器人根據(jù)自身的任務，求得完成這一任務的解決方案的過程。這里所說的任務，具有廣義的概念，既可以指機器人要完成的某一具體任務，也可以是機器人的某個動作，比如手部或關節(jié)的某個規(guī)定的運動等。4.1機器人軌跡規(guī)劃概述一、機器人規(guī)劃的概念所謂機器人的規(guī)劃(P1anning)，指的是——機器

為說明機器人規(guī)劃的概念，我們舉下面的例子：在一些老齡化比較嚴重的國家，開發(fā)了各種各樣的機器人專門用于伺候老人，這些機器人有不少是采用聲控的方式．比如主人用聲音命令機器人“給我倒一杯開水”，我們先不考慮機器人是如何識別人的自然語言，而是著重分析一下機器人在得到這樣一個命令后，如何來完成主人交給的任務。一、機器人規(guī)劃的概念為說明機器人規(guī)劃的概念，我們舉下面的例子：一、機

首先，機器人應該把任務進行分解，把主人交代的任務分解成為“取一個杯子”、“找到水壺”、“打開瓶塞”、“把水倒人杯中”、“把水送給主人”等一系列子任務。這一層次的規(guī)劃稱為任務規(guī)劃(Taskplanning)，它完成總體任務的分解。

然后再針對每一個子任務進行進一步的規(guī)劃。以“把水倒入杯中”這一子任務為例，可以進一步分解成為一系列動作，這一層次的規(guī)劃稱為動作規(guī)劃，它把實現(xiàn)每一個子任務的過程分解為一系列具體的動作。取一個杯子

找到水壺

打開水壺把水倒入杯中

把水送給主人給主人倒一杯水把水倒入杯中

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處首先，機器人應該把任務進行分解，把主人交

為了實現(xiàn)每一個動作，需要對手部的運動軌跡進行必要的規(guī)定，這是手部軌跡規(guī)劃(Handtrajectoryplanning

)。為了使手部實現(xiàn)預定的運動，就要知道各關節(jié)的運動規(guī)律，這是關節(jié)軌跡規(guī)劃(Jointtrajectoryplanning)。最后才是關節(jié)的運動控制(Motioncontrol)。取一個杯子

找到水壺

打開水壺把水倒入杯中

把水送給主人給主人倒一杯水

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處

手部從A點移到B 點

關節(jié)從C點移到D點為了實現(xiàn)每一個動作，需要對手部的運動軌跡進行必要的規(guī)

上述例子可以看出，機器人的規(guī)劃是分層次的，從高層的任務規(guī)劃，動作規(guī)劃到手部軌跡規(guī)劃和關節(jié)軌跡規(guī)劃。在上述例子中，我們沒有討論力的問題，實際上，對有些機器人來說，力的大小也是要控制的，這時，除了手部或關節(jié)的軌跡規(guī)劃，還要進行手部和關節(jié)輸出力的規(guī)劃。智能化程度越高，規(guī)劃的層次越多，操作就越簡單。

軌跡規(guī)劃的目的是將操作人員輸入的簡單的任務描述變?yōu)樵敿毜倪\動軌跡描述。

例如，對一般的工業(yè)機器人來說，操作員可能只輸入機械手末端的目標位置和方位，而規(guī)劃的任務便是要確定出達到目標的關節(jié)軌跡的形狀、運動的時間和速度等。這里所說的軌跡是指隨時間變化的位置、速度和加速度。上述例子可以看出，機器人的規(guī)劃是分層次的，

簡言之，機器人的工作過程，就是通過規(guī)劃，將要求的任務變?yōu)槠谕倪\動和力，由控制環(huán)節(jié)根據(jù)期望的運動和力的信號，產(chǎn)生相應的控制作用，以使機器人輸出實際的運動和力，從而完成期望的任務。如下圖所示。這里，機器人實際運動的情況通常還要反饋給規(guī)劃級和控制級，以便對規(guī)劃和控制的結果做出適當?shù)男拚?。人機接口規(guī)劃控制機器人本體要求的任務

期望的運動和力

實際的運動和力控制作用

要求的任務由操作人員輸入給機器人，為了使機器人操作方便、使用簡單，必須允許操作人員給出盡量簡單的描述。期望的運動和力是進行機器人控制所必需的輸入量，它們是機械手末端在每一個時刻的位姿和速度，對于絕大多數(shù)情況，還要求給出每一時刻期望的關節(jié)位移和速度，有些控制方法還要求給出期望的加速度等。簡言之，機器人的工作過程，就是通過規(guī)劃，4.1機器人軌跡規(guī)劃概述軌跡規(guī)劃？機器人在作業(yè)空間要完成給定的任務，其手部運動必須按一定的軌跡(trajectory)進行。軌跡的生成一般是先給定軌跡上的若干個點，將其經(jīng)運動學反解映射到關節(jié)空間，對關節(jié)空間中的相應點建立運動方程，然后按這些運動方程對關節(jié)進行插值，從而實現(xiàn)作業(yè)空間的運動要求，這一過程通常稱為軌跡規(guī)劃。

4.1機器人軌跡規(guī)劃概述軌跡規(guī)劃？二、軌跡規(guī)劃的一般性問題

工業(yè)機器人的作業(yè)可以描述成工具坐標系{T}相對于工件坐標系{S}的一系列運動。圖4.1機器人將銷插入工件孔中的作業(yè)描述

圖4.1所示的將銷插入工件孔中的作業(yè)，可以借助工具坐標系的一系列位姿Pi(i=1，2，…，n)來描述。二、軌跡規(guī)劃的一般性問題工業(yè)機器人的作業(yè)可以描述成工二、軌跡規(guī)劃的一般性問題

用工具坐標系相對于工件坐標系的運動來描述作業(yè)路徑是一種通用的作業(yè)描述方法。它把作業(yè)路徑描述與具體的機器人、手爪或工具分離開來，形成了模型化的作業(yè)描述方法，從而使這種描述既適用于不同的機器人，也適用于在同一機器人上裝夾不同規(guī)格的工具。圖4.2機器人的初始狀態(tài)和終止狀態(tài)二、軌跡規(guī)劃的一般性問題用工具坐標系相對于工件坐標系

對點位作業(yè)(pickandplaceoperation)的機器人，需要描述它的起始狀態(tài)和目標狀態(tài)，即工具坐標系的起始值{T0}，目標值{Tf}。在此，用“點”這個詞表示工具坐標系的位置和姿態(tài)(簡稱位姿)。對點位作業(yè)(pickandplaceopera

對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操作臂的起始點和終止點，而且要指明兩點之間的若干中間點(稱路徑點)，必須沿特定的路徑運動(路徑約束)。這類稱為連續(xù)路徑運動(continuous—Pathmotion)或輪廓運動(contourmotion)。在規(guī)劃機器人的運動時．還需要弄清楚在其路徑上是否存在障礙物(障礙約束)。

主要討論連續(xù)路徑的無障礙的軌跡規(guī)劃方法。軌跡規(guī)劃器可形象地看成為一個黑箱，其輸入包括路徑的“設定”和“約束”，輸出的是操作臂末端手部的“位姿序列”，表示手部在各離散時刻的中間形位。對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操

軌跡規(guī)劃既可在關節(jié)空間也可在直角空間中進行，但是所規(guī)劃的軌跡函數(shù)都必須連續(xù)和平滑，使機器人的運動平穩(wěn)，不平穩(wěn)的運動將加劇機械部件的磨損，并導致機器人的振動和沖擊。為此，要求所選擇的運動軌跡描述函數(shù)必須連續(xù)，而且它的一階導數(shù)(速度)，有時甚至二階導數(shù)(加速度)也應該連續(xù)

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

(2)關節(jié)空間運動。這種運動直接在關節(jié)空間里進行。由于動力學參數(shù)及其極限值直接在關節(jié)空間里描述，所以用這種方式求最短時間運動很方便。

(3)空間直線運動。這是一種直角空間里的運動，它便于描述空間操作，計算量小，適宜簡單的作業(yè)。

(4)空間曲線運動。這是一種在描述空間中用明確的函數(shù)表達的運動，如圓周運動、螺旋運動等

三、軌跡的生成方式(1)示教-再現(xiàn)運動。這種運動由4.2插補方式分類與軌跡控制

一、插補方式分類點位作業(yè)（PTP=point-to-pointmotion）連續(xù)路徑作業(yè)（CP=continuous-pathmotion）4.2插補方式分類與軌跡控制一、插補方式分類二、機器人軌跡控制過程圖4.3機器人軌跡控制過程

機器人的基本操作方式是示教-再現(xiàn)，即首先教機器人如何做，機器人記住了這個過程，于是它可以根據(jù)需要重復這個動作。操作過程中，不可能把空間軌跡的所有點都示教一遍使機器人記住，這樣太繁瑣，也浪費很多計算機內存。實際上，對于有規(guī)律的軌跡，僅示教幾個特征點，計算機就能利用插補算法獲得中間點的坐標，如直線需要示教兩點，圓弧需要示教三點，通過機器人逆向運動學算法由這些點的坐標求出機器人各關節(jié)的位置和角度(1,…,n)，然后由后面的角位置閉環(huán)控制系統(tǒng)實現(xiàn)要求的軌跡上的一點。繼續(xù)插補并重復上述過程，從而實現(xiàn)要求的軌跡。二、機器人軌跡控制過程圖4.3機器人軌跡控制過程4.3機器人軌跡插值計算一、直線插補

空間直線插補是在已知該直線始末兩點的位置和姿態(tài)的條件下，求各軌跡中間點(插補點)的位置和姿態(tài)。

已知直線始末兩點的坐標值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對于基坐標系的位置。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設v為要求的沿直線運動的速度；ts為插補時間間隔。直線長度

直線插補和圓弧插補是機器人系統(tǒng)中的基本插補算法。對于非直線和圓弧軌跡，可用直線或圓弧逼近，以實現(xiàn)這些軌跡。4.3機器人軌跡插值計算一、直線插補空間直線插ts間隔內行程d=vts；插補總步數(shù)N為L/d+1的整數(shù)部分；各軸增量：各插補點坐標值：式中：i=0，1，2，…，N。

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

圖4.5由已知的三點P1、P2、P3決定的圓弧圖4.6圓弧插補二、圓弧插補1.平面圓弧插補圖4設v為沿圓弧運動速度；ts為插補時時間隔。(1)由P1、P2、P3決定的圓弧半徑R。(2)總的圓心角=1+2，即(3)ts時間內角位移量θ=tsv/R

設v為沿圓弧運動速度；ts為插補時時間隔。(3)ts時間(4)總插補步數(shù)(取整數(shù)) N=

/θ+1對Pi+1點的坐標，有式中：Xi=Rcosθi；Yi=Rsinθi。

由θi+1=θi+θ可判斷是否到插補終點。若θi+1，則繼續(xù)插補下去；當θi+1>

時，則修正最后一步的步長θ,故平面圓弧位置插補為(4)總插補步數(shù)(取整數(shù))由θi+1=θi+θ可判斷是2.空間圓弧插補空間圓弧是指三維空間任一平面內的圓弧?？臻g圓弧插補可分三步來處理：(1)把三維問題轉化成二維，找出圓弧所在平面。(2)利用二維平面插補算法求出插補點坐標(Xi+1,Yi+1)。(3)把該點的坐標值轉變?yōu)榛A坐標系下的值，如圖4.7所示。2.空間圓弧插補空間圓弧是指三維空間任一平面內的圓弧。通過不在同一直線上的三點P1、P2、P3可確定一個圓及三點間的圓弧，其圓心為OR,半徑為R，圓弧所在平面與基礎坐標系平面的交線分別為AB、BC、CA。建立圓弧平面插補坐標系，即把ORXRYRZR坐標系原點與圓心OR重合，設ORXRYR平面為圓弧所在平面，且保持ZR為外法線方向。求解兩坐標系的轉換矩陣。令TR表示由圓弧坐標ORXRYRZR

至基礎坐標系OX0Y0Z0的轉換矩陣。通過不在同一直線上的三點P1、P2、P3可確定一個圓及三點間若ZR軸與基礎坐標系Z0軸的夾角為，XR軸與基礎坐標系的夾角為θ，則可完成下述步驟：①將XRYRZR的原點OR放到基礎原點O上；②繞ZR軸轉θ，使X0與XR平行；③再繞XR軸轉角，使Z0與ZR平行。若ZR軸與基礎坐標系Z0軸的夾角為，XR軸與基礎坐標系的夾這三步完成了XRYRZR向X0Y0Z0的轉換，故總轉換矩陣應為這三步完成了XRYRZR向X0Y0Z0的轉換，故總轉換矩陣應

欲將基礎坐標系的坐標值表示在ORXRYRZR坐標系，則要用到TR的逆矩陣欲將基礎坐標系的坐標值表示在ORXRYRZR

三、關節(jié)空間插補

路徑點(結點)通常用工具坐標系相對于工件坐標系位姿來表示。為了求得在關節(jié)空間形成所要求的軌跡，首先用運動學反解將路徑點轉換成關節(jié)矢量角度值，然后對每個關節(jié)擬合一個光滑函數(shù)，使之從起始點開始，依次通過所有路徑點，最后到達目標點。三、關節(jié)空間插補路徑點(結點)通常用工具坐標系對于每一段路徑，各個關節(jié)運動時間均相同，這樣保證所有關節(jié)同時到達路徑點和終止點，從而得到工具坐標系應有的位置和姿態(tài)。但是，盡管每個關節(jié)在同一段路徑中的運動時間相同，各個關節(jié)函數(shù)之間卻是相互獨立的。

三、關節(jié)空間插補

對于每一段路徑，各個關節(jié)運動時間均相同，這樣保證所有關節(jié)同時

在關節(jié)空間中進行軌跡規(guī)劃，需要給定機器人在起始點、終止點手臂的形位。對關節(jié)進行插值時，應滿足一系列約束條件，在滿足所要求的約束條件下，可以選取不同類型的關節(jié)插值函數(shù)生成不同的軌跡。

三、關節(jié)空間插補

在關節(jié)空間中進行軌跡規(guī)劃，需要給定機器人在起始點、1.三次多項式插值

在操作臂運動的過程中，由于相應于起始點的關節(jié)角度0是已知的．而終止點的關節(jié)角f可以通過運動學反解得到，因此，運動軌跡的描述，可用起始點關節(jié)角與終止點關節(jié)角度的一個平滑插值函數(shù)

(t)來表示。

(t)在t0=0時刻的值是起始關節(jié)角度0，終端時刻tf的值是終止關節(jié)角度f。1.三次多項式插值在操作臂運動的過程中，由

為實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)

(t)至少需要滿足四個約束條件，即兩端點位置約束和兩端點速度約束。端點位置約束是指起始位姿和終止位姿分別所對應的關節(jié)角度。

(t)在時刻t0=0時的值是起始關節(jié)角度0，在終端時刻tf時的值是終止關節(jié)角度f，即1.三次多項式插值

為實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)

(t)

為滿足關節(jié)運動速度的連續(xù)性要求，兩外還有兩個約束條件，即在起始點和終止點的關節(jié)速度要求。為了滿足關節(jié)運動速度連續(xù)性的要求，起始點和終止點的關節(jié)速度可簡單地設定為零。1.三次多項式插值

為滿足關節(jié)運動速度的連續(xù)性要求，兩外還有兩個約束條上面給出的四個約束條件可以惟一地確定一個三次多項式運動過程中的關節(jié)速度和加速度則為上面給出的四個約束條件可以惟一地確定一個三次多項式運動過程為求得三次多項式的系數(shù)a0，a1，a2和a3，代以給定的約束條件，有方程組求解該方程組，可得為求得三次多項式的系數(shù)a0，a1，a2和a3，代以給定的約束

對于起始速度及終止速度為零的關節(jié)運動，滿足連續(xù)平穩(wěn)運動要求的三次多項式插值函數(shù)為關節(jié)角速度和角加速度的表達式為這里再次指出：這組解只適用于關節(jié)起始、終止速度為零的運動情況。對于起始速度及終止速度為零的關節(jié)運動，滿足連圖4.8三次多項式插值的關節(jié)運動軌跡

三次多項式插值的關節(jié)運動軌跡曲線如圖4.8所示。由圖可知，其速度曲線為拋物線，相應的加速度曲線為直線。圖4.8三次多項式插值的關節(jié)運動軌跡三次多項式插

例：設有一臺轉動關節(jié)的機器人，其在執(zhí)行一項作業(yè)時關節(jié)運動歷時3

s。根據(jù)需要，其上某一關節(jié)必須運動平穩(wěn)，并具有如下作業(yè)狀態(tài)：初始時，關節(jié)靜止不動，位置θ0=15°；運動結束時θf=75°，此時關節(jié)速度為0。試根據(jù)上述要求規(guī)劃該關節(jié)的運動。例：設有一臺轉動關節(jié)的機器人，其在執(zhí)行一項作2.過路徑點的三次多項式插值

一般情況下．要求規(guī)劃過路徑點的軌跡。如圖4.9所示，機器人作業(yè)除在A、B點有位姿要求外，在路徑點C、D也有位姿要求。對于這種情況，假如末端執(zhí)行器在路徑點停留，即各路徑點上速度為0，則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次多項式插值方法；但若末端執(zhí)行器只是經(jīng)過，并不停留，就需要將前述方法推廣。2.過路徑點的三次多項式插值一般情況下．要

實際上，可以把所有路徑點看作是“起始點”或“終止點”，求解逆運動學，得到相應的關節(jié)矢量值。然后確定所要求的三次多項式插值函數(shù)，把路徑點平滑地連接起來。但是，在這些“起始點”和“終止點“的關節(jié)運動速度不再是零。實際上，可以把所有路徑點看作是“起始點”或“終止點

設路徑點上的關節(jié)速度已知，在某段路徑上，起始點為θ0和，終止點為θf和，這時，確定三次多項式系數(shù)的方法與前所述完全一致，只是速度約束條件變?yōu)?/p>

利用約束條件確定三次多項式系數(shù)，有下列方程組：設路徑點上的關節(jié)速度已知，在某段路徑上，求解方程組，得如何來確定路徑點上的關節(jié)速度？

由上式確定的三次多項式描述了起始點和終止點具有任意給定位置和速度約束條件的運動軌跡。求解方程組，得如何來確定路徑點上的關節(jié)速度？由上式確定的三如何確定路徑點上的關節(jié)速度？

（1）根據(jù)工具坐標系在直角坐標空間中的瞬時線速度和角速度來確定每個路徑點的關節(jié)速度。該方法利用操作臂在此路徑點上的逆雅可比，把該點的直角坐標速度“映射”為所要求的關節(jié)速度。當然，如果操作臂的某個路徑點是奇異點，這時就不能任意設置速度值。按照該方法生成的軌跡雖然能滿足用戶設置速度的需要，但是逐點設置速度畢竟要耗費很大的工作量。如何確定路徑點上的關節(jié)速度？（1）根據(jù)工具坐標系在直角如何確定路徑點上的關節(jié)速度？(2)在直角坐標空間或關節(jié)空間中采用適當?shù)膯l(fā)式方法，由控制系統(tǒng)自動地選擇路徑點的速度。

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

該方法為了保證路徑點處的加速度連續(xù)，可以設法用兩條三次曲線在路徑點處按一定規(guī)則聯(lián)接起來，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也連續(xù)，下面具體地說明這種方法。如何確定路徑點上的關節(jié)速度？(3)為了保證每個路徑點上

設所經(jīng)過的路徑點處的關節(jié)角度為θv，與該點相鄰的前后兩點的關節(jié)角分別為θ0和θg。設其路徑點處的關節(jié)加速度連續(xù)。如果路徑點用三次多項式連接，試確定多項式的所有系數(shù)。該機器人路徑可分為0到v段及v到g段兩段，可通過由兩個三次多項式組成的樣條函數(shù)連接。設從0到v的三次多項式插值函數(shù)為而從v到g的三次多項式插值函數(shù)為設所經(jīng)過的路徑點處的關節(jié)角度為θv，與該點相時間區(qū)間分別是[0,tf1]、[0,tf2]；約束是：對于第一個方程：對于第二個方程：時間區(qū)間分別是[0,tf1]、[0,tf2]；約束是：對于第二個方程：第一條曲線在t=tf1時的速度等于第二條曲線在t=0的速度；第一條曲線在t=tf1時的加速度等于第二條曲線在t=0時的加速度二個方程：兩個方程共有8個未知數(shù)：已知條件有8個：第一方程：第二方程：

第一條曲線在t=tf1時的速度等于第二條曲線在t=0的速度；第一條曲線在t=tf1時的加速度等于第二條曲線在t=0時的加速度兩個方程共有8個未知數(shù)：已知條件有8個：第一方程：第二方程：當tf1=tf2=tf時的解為：當tf1=tf2=tf時的解為：3.高階多項式插值如果對于運動軌跡的要求更為嚴格，約束條件增多，那么三次多項式就不能滿足需要，必須用更高階的多項式對運動軌跡的路徑段進行插值。例如，對某段路徑的超始點和終止點都有規(guī)定了關節(jié)的位置、速度和加速度要求，則要用一個五次多項式進行插值。3.高階多項式插值1）超始點的位置θ0、速度θ’0

、加速度θ’’0

；2）終止點的位置θf、速度θ’f

、加速度θ’’f

；在給定六個約束條件下，方程的解為：五次多項式有六個系數(shù)，必須滿足六個約束條件：六個約束條件是：1）超始點的位置θ0、速度θ’0、加速度θ’’0；五次多4.用拋物線過渡的線性插值

在關節(jié)空間軌跡規(guī)劃中，對于給定起始點和終止點的情況選擇線性函數(shù)插值較為簡單，如圖4.10所示。然而，單純線性插值會導致起始點和終止點的關節(jié)運動速度不連續(xù)，且加速度無窮大，顯然，在兩端點會造成剛性沖擊。4.用拋物線過渡的線性插值在關節(jié)空間軌跡規(guī)劃中

為此應對線性函數(shù)插值方案進行修正，在線性插值兩端點的鄰域內設置一段拋物線形緩沖區(qū)段。由于拋物線函數(shù)對于時間的二階導數(shù)為常數(shù)，即相應區(qū)段內的加速度恒定，這樣保證起始點和終止點的速度平滑過渡，從而使整個軌跡上的位置和速度連續(xù)。線性函數(shù)與兩段拋物線函數(shù)平滑地銜接在一起形成的軌跡稱為帶有拋物線過渡域的線性軌跡，如圖4.11所示。為此應對線性函數(shù)插值方案進行修正，在線性插值兩端點

為了構造這段運動軌跡，假設兩端的拋物線軌跡具有相同的持續(xù)時間，具有大小相同而符號相反的恒加速度。對于這種路徑規(guī)劃存在有多個解，其軌跡不惟一。但是，每條路徑都對稱于時間中點th和位置中點

h。要保證路徑軌跡的連續(xù)、光滑，即要求拋物線軌跡的終點速度必須等于線性段的速度，故有下列關系為了構造這段運動軌跡，假設兩端的拋物線軌跡具有相同設關節(jié)從起始點到終止點的總運動時間為tf，則tf=2th，并注意到

一般情況下，0、f、tf是已知條件，這樣，可以選擇相應的和得到相應的軌跡。設關節(jié)從起始點到終止點的總運動時間為tf，則tf=2th，為保證有解，加速度值必須選得足夠大，即通常的做法是先選定加速度的值，然后按下式求出相應的

當上式中的等號成立時，軌跡線性段的長度縮減為零，整個軌跡由兩個過渡域組成，這兩個過渡域在銜接處的斜率(關節(jié)速度)相等；加速度的取值愈大，過渡域的長度會變得愈短，若加速度趨于無窮大，軌跡又復歸到簡單的線性插值情況。為保證有解，加速度值必須選得足夠大，即通常的做法是先選定加

例：已知條件為θ0=15°，θf=75°，tf=3

s，試設計兩條帶有拋物線過渡的線性軌跡。例：已知條件為θ0=15°，θf=75°，tf解根據(jù)題意，求出加速度的取值范圍，為此，將已知條件代入下式中，有

≥26.67°/s2

1)設計第一條軌跡對于第一條軌跡，如果選=42°/s2，算出過渡時間，則

解根據(jù)題意，求出加速度的取值范圍，為此，將已知條件代入下計算過渡域終了時的關節(jié)位置和關節(jié)速度，得計算過渡域終了時的關節(jié)位置和關節(jié)速度，得2)設計第二條軌跡對于第二條軌跡，若選擇加速度為27°/s2，可求出2)設計第二條軌跡(a)加速度較小時的位移、速度、加速度曲線(b)加速度較大時的位移、速度、加速度曲線(a)加速度較小時的位移、速度、加速度曲線(b)加速度較五、過路徑點的用拋物線過渡的線性插值

某個關節(jié)在運動中有n個路徑點，其中三個相鄰的路徑點表示為j,k,l，每兩個相鄰的路徑點之間都以線性函數(shù)相連，而所有路徑點附近則由拋物線過渡。五、過路徑點的用拋物線過渡的線性插值某個關節(jié)對于內部路徑段對于第一路徑段，可用以下公式求出對于內部路徑段對于第一路徑段，可用以下公式求出值得注意的是，多段用拋物線過渡的直線樣條函數(shù)一般并不經(jīng)過那些路徑點，除非在這些路徑點處停止。若選取的加速度充分大，則實際路徑將與理想路徑點十分靠近。值得注意的是，多段用拋物線過渡的直線樣條函數(shù)一般并不經(jīng)過那些如果要求機器人途經(jīng)某個結點，同時速度不為零，可以在此結點兩端規(guī)定兩價目“偽結點”，令該結點在兩個“偽結點”的連線上，并位于兩噎渡域之間的連線上。如果要求機器人途經(jīng)某個結點，同時速度不為零，可以在此結點兩端★軌跡的實時生成前面所述的計算構成了機器人的軌跡規(guī)劃。運行中的軌跡實時生成是指由這些數(shù)據(jù)，以軌跡更新的速率不斷產(chǎn)生θ、θ’、θ’’所表示的軌跡，并將此信息送至機器人的控制系統(tǒng)?！镪P節(jié)空間軌跡的生成前面已經(jīng)介紹了幾種關節(jié)空間軌跡規(guī)劃的方法，按照這些方法，其計算結果都是有關各個路徑段的一組數(shù)據(jù)，系統(tǒng)的軌跡生成器利用這些數(shù)據(jù)以軌跡更新速率具體計算出θ、θ’、θ’’?！镘壽E的實時生成對于三次樣條，軌跡生成器只需隨時間的變化不斷按下式計算θ、θ’、θ’’。當?shù)竭_路徑段的終點時，調用新路徑段的三次樣條系數(shù)，重新賦t為零，繼續(xù)生成軌跡。對于三次樣條，軌跡生成器只需隨時間的變化不斷按下式計算θ、對于帶拋物線過渡的直線樣條插值，每次更新軌跡時，應首先檢測時間t的值以判斷當前處于路徑段的線性域和過渡域。處于線性域時，各關節(jié)的軌跡按下式計算：其中t是從第j個路徑點算起的時間；第二式右邊θ’jk的值在軌跡規(guī)劃時由下式算出。對于帶拋物線過渡的直線樣條插值，每次更新軌跡時，應首先檢測時處于守渡域時各關節(jié)軌跡按下式計算其中θ’jk

，θ’’k

，和tjk在軌跡規(guī)劃時已經(jīng)算出。處于守渡域時各關節(jié)軌跡按下式計算其中θ’jk，θ’’kClassisover.

Bye-bye!Classisover.

Bye-bye!第四章機器人軌跡規(guī)劃第四章機器人軌跡規(guī)劃本章主要內容4.1機器人軌跡規(guī)劃概述4.2插補方式分類與軌跡控制4.3機器人軌跡插補計算4.4軌跡的實時生成本章主要內容4.1機器人軌跡規(guī)劃概述

所謂機器人的規(guī)劃(P1anning)，指的是——機器人根據(jù)自身的任務，求得完成這一任務的解決方案的過程。這里所說的任務，具有廣義的概念，既可以指機器人要完成的某一具體任務，也可以是機器人的某個動作，比如手部或關節(jié)的某個規(guī)定的運動等。4.1機器人軌跡規(guī)劃概述一、機器人規(guī)劃的概念所謂機器人的規(guī)劃(P1anning)，指的是——機器

為說明機器人規(guī)劃的概念，我們舉下面的例子：在一些老齡化比較嚴重的國家，開發(fā)了各種各樣的機器人專門用于伺候老人，這些機器人有不少是采用聲控的方式．比如主人用聲音命令機器人“給我倒一杯開水”，我們先不考慮機器人是如何識別人的自然語言，而是著重分析一下機器人在得到這樣一個命令后，如何來完成主人交給的任務。一、機器人規(guī)劃的概念為說明機器人規(guī)劃的概念，我們舉下面的例子：一、機

首先，機器人應該把任務進行分解，把主人交代的任務分解成為“取一個杯子”、“找到水壺”、“打開瓶塞”、“把水倒人杯中”、“把水送給主人”等一系列子任務。這一層次的規(guī)劃稱為任務規(guī)劃(Taskplanning)，它完成總體任務的分解。

然后再針對每一個子任務進行進一步的規(guī)劃。以“把水倒入杯中”這一子任務為例，可以進一步分解成為一系列動作，這一層次的規(guī)劃稱為動作規(guī)劃，它把實現(xiàn)每一個子任務的過程分解為一系列具體的動作。取一個杯子

找到水壺

打開水壺把水倒入杯中

把水送給主人給主人倒一杯水把水倒入杯中

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處首先，機器人應該把任務進行分解，把主人交

為了實現(xiàn)每一個動作，需要對手部的運動軌跡進行必要的規(guī)定，這是手部軌跡規(guī)劃(Handtrajectoryplanning

)。為了使手部實現(xiàn)預定的運動，就要知道各關節(jié)的運動規(guī)律，這是關節(jié)軌跡規(guī)劃(Jointtrajectoryplanning)。最后才是關節(jié)的運動控制(Motioncontrol)。取一個杯子

找到水壺

打開水壺把水倒入杯中

把水送給主人給主人倒一杯水

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處

手部從A點移到B 點

關節(jié)從C點移到D點為了實現(xiàn)每一個動作，需要對手部的運動軌跡進行必要的規(guī)

上述例子可以看出，機器人的規(guī)劃是分層次的，從高層的任務規(guī)劃，動作規(guī)劃到手部軌跡規(guī)劃和關節(jié)軌跡規(guī)劃。在上述例子中，我們沒有討論力的問題，實際上，對有些機器人來說，力的大小也是要控制的，這時，除了手部或關節(jié)的軌跡規(guī)劃，還要進行手部和關節(jié)輸出力的規(guī)劃。智能化程度越高，規(guī)劃的層次越多，操作就越簡單。

軌跡規(guī)劃的目的是將操作人員輸入的簡單的任務描述變?yōu)樵敿毜倪\動軌跡描述。

例如，對一般的工業(yè)機器人來說，操作員可能只輸入機械手末端的目標位置和方位，而規(guī)劃的任務便是要確定出達到目標的關節(jié)軌跡的形狀、運動的時間和速度等。這里所說的軌跡是指隨時間變化的位置、速度和加速度。上述例子可以看出，機器人的規(guī)劃是分層次的，

簡言之，機器人的工作過程，就是通過規(guī)劃，將要求的任務變?yōu)槠谕倪\動和力，由控制環(huán)節(jié)根據(jù)期望的運動和力的信號，產(chǎn)生相應的控制作用，以使機器人輸出實際的運動和力，從而完成期望的任務。如下圖所示。這里，機器人實際運動的情況通常還要反饋給規(guī)劃級和控制級，以便對規(guī)劃和控制的結果做出適當?shù)男拚?。人機接口規(guī)劃控制機器人本體要求的任務

期望的運動和力

實際的運動和力控制作用

要求的任務由操作人員輸入給機器人，為了使機器人操作方便、使用簡單，必須允許操作人員給出盡量簡單的描述。期望的運動和力是進行機器人控制所必需的輸入量，它們是機械手末端在每一個時刻的位姿和速度，對于絕大多數(shù)情況，還要求給出每一時刻期望的關節(jié)位移和速度，有些控制方法還要求給出期望的加速度等。簡言之，機器人的工作過程，就是通過規(guī)劃，4.1機器人軌跡規(guī)劃概述軌跡規(guī)劃？機器人在作業(yè)空間要完成給定的任務，其手部運動必須按一定的軌跡(trajectory)進行。軌跡的生成一般是先給定軌跡上的若干個點，將其經(jīng)運動學反解映射到關節(jié)空間，對關節(jié)空間中的相應點建立運動方程，然后按這些運動方程對關節(jié)進行插值，從而實現(xiàn)作業(yè)空間的運動要求，這一過程通常稱為軌跡規(guī)劃。

4.1機器人軌跡規(guī)劃概述軌跡規(guī)劃？二、軌跡規(guī)劃的一般性問題

工業(yè)機器人的作業(yè)可以描述成工具坐標系{T}相對于工件坐標系{S}的一系列運動。圖4.1機器人將銷插入工件孔中的作業(yè)描述

圖4.1所示的將銷插入工件孔中的作業(yè)，可以借助工具坐標系的一系列位姿Pi(i=1，2，…，n)來描述。二、軌跡規(guī)劃的一般性問題工業(yè)機器人的作業(yè)可以描述成工二、軌跡規(guī)劃的一般性問題

用工具坐標系相對于工件坐標系的運動來描述作業(yè)路徑是一種通用的作業(yè)描述方法。它把作業(yè)路徑描述與具體的機器人、手爪或工具分離開來，形成了模型化的作業(yè)描述方法，從而使這種描述既適用于不同的機器人，也適用于在同一機器人上裝夾不同規(guī)格的工具。圖4.2機器人的初始狀態(tài)和終止狀態(tài)二、軌跡規(guī)劃的一般性問題用工具坐標系相對于工件坐標系

對點位作業(yè)(pickandplaceoperation)的機器人，需要描述它的起始狀態(tài)和目標狀態(tài)，即工具坐標系的起始值{T0}，目標值{Tf}。在此，用“點”這個詞表示工具坐標系的位置和姿態(tài)(簡稱位姿)。對點位作業(yè)(pickandplaceopera

對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操作臂的起始點和終止點，而且要指明兩點之間的若干中間點(稱路徑點)，必須沿特定的路徑運動(路徑約束)。這類稱為連續(xù)路徑運動(continuous—Pathmotion)或輪廓運動(contourmotion)。在規(guī)劃機器人的運動時．還需要弄清楚在其路徑上是否存在障礙物(障礙約束)。

主要討論連續(xù)路徑的無障礙的軌跡規(guī)劃方法。軌跡規(guī)劃器可形象地看成為一個黑箱，其輸入包括路徑的“設定”和“約束”，輸出的是操作臂末端手部的“位姿序列”，表示手部在各離散時刻的中間形位。對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操

軌跡規(guī)劃既可在關節(jié)空間也可在直角空間中進行，但是所規(guī)劃的軌跡函數(shù)都必須連續(xù)和平滑，使機器人的運動平穩(wěn)，不平穩(wěn)的運動將加劇機械部件的磨損，并導致機器人的振動和沖擊。為此，要求所選擇的運動軌跡描述函數(shù)必須連續(xù)，而且它的一階導數(shù)(速度)，有時甚至二階導數(shù)(加速度)也應該連續(xù)

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

(2)關節(jié)空間運動。這種運動直接在關節(jié)空間里進行。由于動力學參數(shù)及其極限值直接在關節(jié)空間里描述，所以用這種方式求最短時間運動很方便。

(3)空間直線運動。這是一種直角空間里的運動，它便于描述空間操作，計算量小，適宜簡單的作業(yè)。

(4)空間曲線運動。這是一種在描述空間中用明確的函數(shù)表達的運動，如圓周運動、螺旋運動等

三、軌跡的生成方式(1)示教-再現(xiàn)運動。這種運動由4.2插補方式分類與軌跡控制

一、插補方式分類點位作業(yè)（PTP=point-to-pointmotion）連續(xù)路徑作業(yè)（CP=continuous-pathmotion）4.2插補方式分類與軌跡控制一、插補方式分類二、機器人軌跡控制過程圖4.3機器人軌跡控制過程

機器人的基本操作方式是示教-再現(xiàn)，即首先教機器人如何做，機器人記住了這個過程，于是它可以根據(jù)需要重復這個動作。操作過程中，不可能把空間軌跡的所有點都示教一遍使機器人記住，這樣太繁瑣，也浪費很多計算機內存。實際上，對于有規(guī)律的軌跡，僅示教幾個特征點，計算機就能利用插補算法獲得中間點的坐標，如直線需要示教兩點，圓弧需要示教三點，通過機器人逆向運動學算法由這些點的坐標求出機器人各關節(jié)的位置和角度(1,…,n)，然后由后面的角位置閉環(huán)控制系統(tǒng)實現(xiàn)要求的軌跡上的一點。繼續(xù)插補并重復上述過程，從而實現(xiàn)要求的軌跡。二、機器人軌跡控制過程圖4.3機器人軌跡控制過程4.3機器人軌跡插值計算一、直線插補

空間直線插補是在已知該直線始末兩點的位置和姿態(tài)的條件下，求各軌跡中間點(插補點)的位置和姿態(tài)。

已知直線始末兩點的坐標值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對于基坐標系的位置。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設v為要求的沿直線運動的速度；ts為插補時間間隔。直線長度

直線插補和圓弧插補是機器人系統(tǒng)中的基本插補算法。對于非直線和圓弧軌跡，可用直線或圓弧逼近，以實現(xiàn)這些軌跡。4.3機器人軌跡插值計算一、直線插補空間直線插ts間隔內行程d=vts；插補總步數(shù)N為L/d+1的整數(shù)部分；各軸增量：各插補點坐標值：式中：i=0，1，2，…，N。

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

圖4.5由已知的三點P1、P2、P3決定的圓弧圖4.6圓弧插補二、圓弧插補1.平面圓弧插補圖4設v為沿圓弧運動速度；ts為插補時時間隔。(1)由P1、P2、P3決定的圓弧半徑R。(2)總的圓心角=1+2，即(3)ts時間內角位移量θ=tsv/R

設v為沿圓弧運動速度；ts為插補時時間隔。(3)ts時間(4)總插補步數(shù)(取整數(shù)) N=

/θ+1對Pi+1點的坐標，有式中：Xi=Rcosθi；Yi=Rsinθi。

由θi+1=θi+θ可判斷是否到插補終點。若θi+1，則繼續(xù)插補下去；當θi+1>

時，則修正最后一步的步長θ,故平面圓弧位置插補為(4)總插補步數(shù)(取整數(shù))由θi+1=θi+θ可判斷是2.空間圓弧插補空間圓弧是指三維空間任一平面內的圓弧。空間圓弧插補可分三步來處理：(1)把三維問題轉化成二維，找出圓弧所在平面。(2)利用二維平面插補算法求出插補點坐標(Xi+1,Yi+1)。(3)把該點的坐標值轉變?yōu)榛A坐標系下的值，如圖4.7所示。2.空間圓弧插補空間圓弧是指三維空間任一平面內的圓弧。通過不在同一直線上的三點P1、P2、P3可確定一個圓及三點間的圓弧，其圓心為OR,半徑為R，圓弧所在平面與基礎坐標系平面的交線分別為AB、BC、CA。建立圓弧平面插補坐標系，即把ORXRYRZR坐標系原點與圓心OR重合，設ORXRYR平面為圓弧所在平面，且保持ZR為外法線方向。求解兩坐標系的轉換矩陣。令TR表示由圓弧坐標ORXRYRZR

至基礎坐標系OX0Y0Z0的轉換矩陣。通過不在同一直線上的三點P1、P2、P3可確定一個圓及三點間若ZR軸與基礎坐標系Z0軸的夾角為，XR軸與基礎坐標系的夾角為θ，則可完成下述步驟：①將XRYRZR的原點OR放到基礎原點O上；②繞ZR軸轉θ，使X0與XR平行；③再繞XR軸轉角，使Z0與ZR平行。若ZR軸與基礎坐標系Z0軸的夾角為，XR軸與基礎坐標系的夾這三步完成了XRYRZR向X0Y0Z0的轉換，故總轉換矩陣應為這三步完成了XRYRZR向X0Y0Z0的轉換，故總轉換矩陣應

欲將基礎坐標系的坐標值表示在ORXRYRZR坐標系，則要用到TR的逆矩陣欲將基礎坐標系的坐標值表示在ORXRYRZR

三、關節(jié)空間插補

路徑點(結點)通常用工具坐標系相對于工件坐標系位姿來表示。為了求得在關節(jié)空間形成所要求的軌跡，首先用運動學反解將路徑點轉換成關節(jié)矢量角度值，然后對每個關節(jié)擬合一個光滑函數(shù)，使之從起始點開始，依次通過所有路徑點，最后到達目標點。三、關節(jié)空間插補路徑點(結點)通常用工具坐標系對于每一段路徑，各個關節(jié)運動時間均相同，這樣保證所有關節(jié)同時到達路徑點和終止點，從而得到工具坐標系應有的位置和姿態(tài)。但是，盡管每個關節(jié)在同一段路徑中的運動時間相同，各個關節(jié)函數(shù)之間卻是相互獨立的。

三、關節(jié)空間插補

對于每一段路徑，各個關節(jié)運動時間均相同，這樣保證所有關節(jié)同時

在關節(jié)空間中進行軌跡規(guī)劃，需要給定機器人在起始點、終止點手臂的形位。對關節(jié)進行插值時，應滿足一系列約束條件，在滿足所要求的約束條件下，可以選取不同類型的關節(jié)插值函數(shù)生成不同的軌跡。

三、關節(jié)空間插補

在關節(jié)空間中進行軌跡規(guī)劃，需要給定機器人在起始點、1.三次多項式插值

在操作臂運動的過程中，由于相應于起始點的關節(jié)角度0是已知的．而終止點的關節(jié)角f可以通過運動學反解得到，因此，運動軌跡的描述，可用起始點關節(jié)角與終止點關節(jié)角度的一個平滑插值函數(shù)

(t)來表示。

(t)在t0=0時刻的值是起始關節(jié)角度0，終端時刻tf的值是終止關節(jié)角度f。1.三次多項式插值在操作臂運動的過程中，由

為實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)

(t)至少需要滿足四個約束條件，即兩端點位置約束和兩端點速度約束。端點位置約束是指起始位姿和終止位姿分別所對應的關節(jié)角度。

(t)在時刻t0=0時的值是起始關節(jié)角度0，在終端時刻tf時的值是終止關節(jié)角度f，即1.三次多項式插值

為實現(xiàn)單個關節(jié)的平穩(wěn)運動，軌跡函數(shù)

(t)

為滿足關節(jié)運動速度的連續(xù)性要求，兩外還有兩個約束條件，即在起始點和終止點的關節(jié)速度要求。為了滿足關節(jié)運動速度連續(xù)性的要求，起始點和終止點的關節(jié)速度可簡單地設定為零。1.三次多項式插值

為滿足關節(jié)運動速度的連續(xù)性要求，兩外還有兩個約束條上面給出的四個約束條件可以惟一地確定一個三次多項式運動過程中的關節(jié)速度和加速度則為上面給出的四個約束條件可以惟一地確定一個三次多項式運動過程為求得三次多項式的系數(shù)a0，a1，a2和a3，代以給定的約束條件，有方程組求解該方程組，可得為求得三次多項式的系數(shù)a0，a1，a2和a3，代以給定的約束

對于起始速度及終止速度為零的關節(jié)運動，滿足連續(xù)平穩(wěn)運動要求的三次多項式插值函數(shù)為關節(jié)角速度和角加速度的表達式為這里再次指出：這組解只適用于關節(jié)起始、終止速度為零的運動情況。對于起始速度及終止速度為零的關節(jié)運動，滿足連圖4.8三次多項式插值的關節(jié)運動軌跡

三次多項式插值的關節(jié)運動軌跡曲線如圖4.8所示。由圖可知，其速度曲線為拋物線，相應的加速度曲線為直線。圖4.8三次多項式插值的關節(jié)運動軌跡三次多項式插

例：設有一臺轉動關節(jié)的機器人，其在執(zhí)行一項作業(yè)時關節(jié)運動歷時3

s。根據(jù)需要，其上某一關節(jié)必須運動平穩(wěn)，并具有如下作業(yè)狀態(tài)：初始時，關節(jié)靜止不動，位置θ0=15°；運動結束時θf=75°，此時關節(jié)速度為0。試根據(jù)上述要求規(guī)劃該關節(jié)的運動。例：設有一臺轉動關節(jié)的機器人，其在執(zhí)行一項作2.過路徑點的三次多項式插值

一般情況下．要求規(guī)劃過路徑點的軌跡。如圖4.9所示，機器人作業(yè)除在A、B點有位姿要求外，在路徑點C、D也有位姿要求。對于這種情況，假如末端執(zhí)行器在路徑點停留，即各路徑點上速度為0，則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次多項式插值方法；但若末端執(zhí)行器只是經(jīng)過，并不停留，就需要將前述方法推廣。2.過路徑點的三次多項式插值一般情況下．要

實際上，可以把所有路徑點看作是“起始點”或“終止點”，求解逆運動學，得到相應的關節(jié)矢量值。然后確定所要求的三次多項式插值函數(shù)，把路徑點平滑地連接起來。但是，在這些“起始點”和“終止點“的關節(jié)運動速度不再是零。實際上，可以把所有路徑點看作是“起始點”或“終止點

設路徑點上的關節(jié)速度已知，在某段路徑上，起始點為θ0和，終止點為θf和，這時，確定三次多項式系數(shù)的方法與前所述完全一致，只是速度約束條件變?yōu)?/p>

利用約束條件確定三次多項式系數(shù)，有下列方程組：設路徑點上的關節(jié)速度已知，在某段路徑上，求解方程組，得如何來確定路徑點上的關節(jié)速度？

由上式確定的三次多項式描述了起始點和終止點具有任意給定位置和速度約束條件的運動軌跡。求解方程組，得如何來確定路徑點上的關節(jié)速度？由上式確定的三如何確定路徑點上的關節(jié)速度？

（1）根據(jù)工具坐標系在直角坐標空間中的瞬時線速度和角速度來確定每個路徑點的關節(jié)速度。該方法利用操作臂在此路徑點上的逆雅可比，把該點的直角坐標速度“映射”為所要求的關節(jié)速度。當然，如果操作臂的某個路徑點是奇異點，這時就不能任意設置速度值。按照該方法生成的軌跡雖然能滿足用戶設置速度的需要，但是逐點設置速度畢竟要耗費很大的工作量。如何確定路徑點上的關節(jié)速度？（1）根據(jù)工具坐標系在直角如何確定路徑點上的關節(jié)速度？(2)在直角坐標空間或關節(jié)空間中采用適當?shù)膯l(fā)式方法，由控制系統(tǒng)自動地選擇路徑點的速度。

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

該方法為了保證路徑點處的加速度連續(xù)，可以設法用兩條三次曲線在路徑點處按一定規(guī)則聯(lián)接起來，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也連續(xù)，下面具體地說明這種方法。如何確定路徑點上的關節(jié)速度？(3)為了保證每個路徑點上

設所經(jīng)過的路徑點處的關節(jié)角度為θv，與該點相鄰的前后兩點的關節(jié)角分別為θ0和θg。設其路徑點處的關節(jié)加速度連續(xù)。如果路徑點用三次多項式連接，試確定多項式的所有系數(shù)。該機器人路徑可分為0到v段及v到g段兩段，可通過由兩個三次多項式組成的樣條函數(shù)連接。設從0到v的三次多項式插值函數(shù)為而從v到g的三次多項式插值函數(shù)為設所經(jīng)過的路徑點處的關節(jié)角度為θv，與該點相時間區(qū)間分別是[0,tf1]、[0,tf2]；約束是：對于第一個方程：對于第二個方程：時間區(qū)間分別是[0,tf1]、[0,tf2]；約束是：對于第二個方程：第一條曲線在t=tf1時的速度等于第二條曲線在t=0的速度；第一條曲線在t=tf1時的加速度等于第二條曲線在t=0時的加速度二個方程：兩個方程共有8個未知數(shù)：已知條件有8個：第一方程：第二方程：

第一條曲線在t=tf1時的速度等于第二條曲線在t=0的速度；第一條曲線在t=tf1時的加速度等于第二條曲線在t=0時的加速度兩個方程共有8個未知數(shù)：已知條件有8個：第一方程：第二方程：當tf1=tf2=tf時的解為：當tf1=tf2=tf時的解為：3.高階多項式插值如果對于運動軌跡的要求更為嚴格，約束條件增多，那么三次多項式就不能滿足需要，必須用更高階的多項式對運動軌跡的路徑段進行插值。例如，對某段路徑的超始點和終止點都有規(guī)定了關節(jié)的位置、速度和加速度要求，則要用一個五次多項式進行插值。3.高階多項式插值