機器人技術(shù)第七章機器人的軌跡規(guī)劃.ppt

1、1,第七章 機器人的軌跡規(guī)劃,7.1 機器人規(guī)劃的定義和作用,7.1.1 概述,機器人學(xué)中的一個基本問題是為解決某個預(yù)定的任務(wù)而規(guī)劃機器人的動作，然后在機器人執(zhí)行完成那些動作所需的命令時控制它。這里，規(guī)劃的意思就是機器人在行動前確定一系列動作(作決策)，這種動作的確定可用問題求解系統(tǒng)來解決，給定初始情況后，該系統(tǒng)可達到某一規(guī)定的目標。因此，規(guī)劃就是指機器人為達到目標而需要的行動過程的描述。 規(guī)劃內(nèi)容可能沒有次序，但是一般來說，規(guī)劃具有某個規(guī)劃目標的蘊含排序。例如，早晨起床后的安排。 缺乏規(guī)劃可能導(dǎo)致不是最佳的問題求解，甚至得不到問題的求解。,2,許多規(guī)劃所包的步驟是含糊的，而且需要進一步說明（

2、子規(guī)劃）。大多數(shù)規(guī)劃具有很大的子規(guī)劃結(jié)構(gòu)，規(guī)劃中的每個目標可以由達到此目標的比較詳細的子規(guī)劃所代替。因此，最終得到的規(guī)劃是某個問題求解算符的線性或分部排序，但是由算符來實現(xiàn)的目標常常具有分層結(jié)構(gòu)。 把某些比較復(fù)雜的問題分解為一些比較小的問題的想法使我們應(yīng)用規(guī)劃方法求解問題在實際上成為可能。 有兩條能夠?qū)崿F(xiàn)這種分解的重要途徑：第一條是當從一個問題狀態(tài)移動到下一個狀態(tài)時，無需計算整個新的狀態(tài)，而只要考慮狀態(tài)中可能變化了的那些部分。第二條是把單一的困難問題分割為幾個有希望的、較為容易解決的子問題，這種分解能夠使困難問題的求解變得容易些。,3,機器人規(guī)劃分為高層規(guī)劃和低層規(guī)劃。自動規(guī)劃在機器人規(guī)劃中稱

3、為高層規(guī)劃。在無特別說明時，機器人規(guī)劃都是指自動規(guī)劃。自動規(guī)劃是一種重要的問題求解技術(shù)，它從某個特定的問題狀態(tài)出發(fā)，尋求一系列行為動作，并建立一個操作序列，直到求得目標狀態(tài)為止。與一般問題求解相比，自動規(guī)劃更注重于問題的求解過程，而不是求解結(jié)果。 我們在闡述機器人自動規(guī)劃問題時，機器人一般配備有傳感器和一組能在某個易于理解的現(xiàn)場中完成的基本動作。這些動作可把該現(xiàn)場從一種狀態(tài)或布局變換為另一種狀態(tài)或布局。例如， “積木世界” 。,4,機器人能得到的一個解答是由下面的算符序列組成的：,機器人規(guī)劃是機器人學(xué)的一個重要研究領(lǐng)域，也是人工智能與機器人學(xué)一個令人感興趣的結(jié)合點。,5,機器人的規(guī)劃程序只需要

4、給定任務(wù)初始狀態(tài)和最終狀態(tài)的描述。這些規(guī)劃系統(tǒng)一般都不說明為實現(xiàn)一個算符所需的詳細的機器人運動。任務(wù)規(guī)劃程序則把任務(wù)級的說明變換成操作機級的說明。為了進行這種變換，任務(wù)規(guī)劃程序必須包含被操作物體、任務(wù)環(huán)境、機器人執(zhí)行的任務(wù)、環(huán)境的初始狀態(tài)和所要求的最終(目標)狀態(tài)等描述。任務(wù)規(guī)劃程序的輸出就是一個機器人程序，在指定的初始狀態(tài)下執(zhí)行該程序后，就能達到所要求的最終狀態(tài)。 任務(wù)規(guī)劃有三個階段：建立模型、任務(wù)說明和操作機程序綜合。任務(wù)的世界模型應(yīng)含有如下的信息:(1)任務(wù)環(huán)境中的所有物體和機器人的幾何描述；(2)所有物體的物理描述；(3)所有連接件的運動學(xué)描述，(4) 機器人和傳感器特性的描述。在世界

5、模型中，任務(wù)狀態(tài)模型還必包括全部物體和連接件的布局。,7.1.2 機器人任務(wù)規(guī)劃的作用,6,機器人軌跡規(guī)劃屬于機器人低層規(guī)劃，基本上不涉及人工智能問題，而是在機械手運動學(xué)和動力學(xué)的基礎(chǔ)上，討論機器人運動的規(guī)劃及其方法。所謂軌跡，就是指機器人在運動過程中的位移、速度和加速度。,軌跡規(guī)劃問題通常是將軌跡規(guī)劃器看成“黑箱”，接受表示路徑約束的輸入變量，輸出為起點和終點之間按時間排列的操作機中間形態(tài)（位姿, 速度和加速度）序列。,7.2 機器人軌跡規(guī)劃的一般性問題,由初始點運動到終止點，所經(jīng)過的由中間形態(tài)序列構(gòu)成的空間曲線稱為路徑。這些形態(tài)序列即是曲線上的“點”。,7,規(guī)劃操作機的軌跡有兩種常用的方法

6、： 第一種方法：要求使用者在沿軌跡選定的位置點上(稱為結(jié)節(jié)或插值點)顯式地給定廣義坐標位置、速度和加速度的一組約束(例如，連續(xù)性和光滑程度等)。然后，軌跡規(guī)劃器從插值和滿足插值點約束的函數(shù)中選定參數(shù)化軌跡。顯然，在這種方法中，約束的給定和操作機軌跡規(guī)劃是在關(guān)節(jié)坐標系中進行的。 第二種方法：使用者以解析函數(shù)顯式地給定操作機必經(jīng)之路徑，例如，笛卡爾坐標中的直線路徑。然后，軌跡規(guī)劃器在關(guān)節(jié)坐標或笛卡幾坐標中確定一條與給定路徑近似的軌跡。在這種方法中，路徑約束是在笛卡爾坐標中給定的。,8,在第一種方法中，約束的給定和操作機軌跡規(guī)劃在關(guān)節(jié)坐標系中進行。由于對操作機手部沒有約束，使用者難于跟蹤操作機手部運

7、行的路徑。因此，操作機手部可能在沒有事先警告的情況下與障礙物相碰。 在第二種方法中，路徑約束在笛卡爾坐標中給定，而關(guān)節(jié)驅(qū)動器是在關(guān)節(jié)坐標中受控制的。因此，為了求得一條逼近給定路徑的軌跡，必須用函數(shù)近似把笛卡爾坐標中的路徑約束變換為關(guān)節(jié)坐標中的路徑約束，再確定滿足關(guān)節(jié)坐標路徑約束的參數(shù)化軌跡。 軌跡規(guī)劃既可在關(guān)節(jié)變量空間中進行，也可在笛卡爾空間進行。對于關(guān)節(jié)變量空間的規(guī)劃，要規(guī)劃關(guān)節(jié)變量的時間函數(shù)及其前二階時間導(dǎo)數(shù)，以便描述操作機的預(yù)定運動。在笛卡爾空間規(guī)劃中，要規(guī)劃操作機手部位置、速度和加速度的時間函數(shù)，而相應(yīng)的關(guān)節(jié)位置、速度和加速度可根據(jù)手部信息導(dǎo)出。,9,面向笛卡爾空間方法的優(yōu)點是概念直觀

8、，而且沿預(yù)定直線路徑可達到相當?shù)臏蚀_性?？墒怯捎诂F(xiàn)代還沒有可用笛卡爾坐標測量操作機手部位置的傳感器，所有可用的控制算法都是建立在關(guān)節(jié)坐標基礎(chǔ)上的。因此，笛卡爾空間路徑規(guī)劃就需要在笛卡爾坐標和關(guān)節(jié)之間進行實時變換，這是一個計算量很大的任務(wù)，常常導(dǎo)致較長的控制間隔。 由笛卡爾坐標向關(guān)節(jié)坐標的變換是病態(tài)的，因而它不是一一對應(yīng)的映射。 如果在軌跡規(guī)劃階段要考慮操作機的動力學(xué)特性，就要以笛卡爾坐標給定路徑約束，同時以關(guān)節(jié)坐標給定物理約束(例如，每個關(guān)節(jié)電機的力和力矩、速度和加速度權(quán)限)。這就會使最后的優(yōu)化問題具有在兩個不同坐標系中的混合約束。,在笛卡爾空間進行軌跡規(guī)劃的特點：,10,在關(guān)節(jié)變量空間的規(guī)劃

9、有三個優(yōu)點： 直接用運動時的受控變量規(guī)劃軌跡； 軌跡規(guī)劃可接近實時地進行； 關(guān)節(jié)軌跡易于規(guī)劃。 伴隨的缺點是難于確定運動中各桿件和手的位置，但是，為 了避開軌跡上的障礙常常又要求知道一些桿件和手位置。,由于面向笛卡爾空間的方法有前述鐘種缺點，使得面向關(guān)節(jié)空間的方法被廣泛采用。它把笛卡爾結(jié)點變換為相應(yīng)的關(guān)節(jié)坐標，并用低次多項式內(nèi)插這些關(guān)節(jié)結(jié)點。這種方法的優(yōu)點是計算較快，而且易于處理操作機的動力學(xué)約束。但當取樣點落在擬合的光滑多項式曲線上時，面向關(guān)節(jié)空間的方法沿笛卡爾路徑的準確性會有損失。,11,一般說來，生成關(guān)節(jié)軌跡設(shè)定點的基本算法是很簡單的：,循環(huán)：等待下一個控制間隔； 時刻操作機關(guān)節(jié)所處的位

10、置； 若 ，則退出循環(huán)； 轉(zhuǎn)向循環(huán)。,從上述算法可以看出，要計算的是在每個控制間隔中必須更新的軌跡函數(shù)(或軌跡規(guī)劃器)h(t)。因此，對規(guī)劃的軌跡要提出四個限制。第一，必須便于用迭代方式計算軌跡設(shè)定點；第二，必須求出并明確給定中間位置；第三，必須保證關(guān)節(jié)變量及其前二階時間導(dǎo)數(shù)的連續(xù)性，使得規(guī)劃的關(guān)節(jié)軌跡是光滑的；最后，必須減少額外的運動 (例如，“游移” )。,7.3 關(guān)節(jié)插值軌跡,12,若某關(guān)節(jié)(例如關(guān)節(jié) i )的關(guān)節(jié)軌跡使用 p 個多項式，則要滿足初始和終止條件(關(guān)節(jié)位置、速度和加速度)，并保證這些變量在多項式銜接處的連續(xù)性，因而需要確定 3( p十1)個系數(shù)。 若給定附加的中間條件(例如

11、位置)，則對每個中間條件需要增加一系數(shù)。通常，可以給定兩個中間位置，一個靠近初始位置；另一個靠近終止位置。這樣做，除了可以較好地控制運動外，還能保證操作機末端以適當?shù)姆较螂x開起點和接近終點。 因此，對于連接初始位置和終止位置的每個關(guān)節(jié)變量，一個七次多項式就足夠了，或者用兩段四次軌跡加一段三次軌跡 （434），也可以用兩段三次軌跡加一段五次軌跡（353），或五段三次軌跡（33333）。,由約束條件數(shù)所對應(yīng)的多項式系數(shù)的個數(shù)確定多項式的次數(shù),13,為了控制操作機，在規(guī)劃運動軌跡之前，需要給定機器人在初始點和終止點的手臂形態(tài)。在規(guī)劃機器人關(guān)節(jié)插值運動軌跡時，需要注意下述幾點： 1、抓住一個物體時，手

12、的運動方向應(yīng)該指向離開物體支承表面的方向。否則，手可能與支承面相碰。 2、若沿支承面的法線方向從初始點向外給定一個離開位置(提升點)，并要求手(即手部坐標系的原點)經(jīng)過此位置，這種離開運動就是允許的。如果還給定由初始點運動到離開位置的時間，我們就可以控制提起物體運動的速度。 3、對于手臂運動提升點的要求同樣也適用于終止位置運動的下放點(即必須先運動到支承表面外法線方向上的某點，再慢慢下移至終止點)。這樣，可獲得和控制正確的接近方向。 4、對手臂的每一次運動，都設(shè)定上述四個點：初始點，提升點，下放點和終止點。,14,5、位置約束 (a) 初始點：給定速度和加速度(一般為零)； (b) 提升點：中

13、間點運動的連續(xù)； (c) 下放點：同提升點； (d) 終止點：給定速度和加速度(一般為零)。,15,6、除上述約束外，所有關(guān)節(jié)軌跡的極值不能超出每個關(guān)節(jié)變量的物理和幾何極限。 7、時間的考慮 (a) 軌跡的初始段和終止段：時間由手接近和離開支承表面的速率決定；也是由關(guān)節(jié)電機特性決定的某個常數(shù)。 (b) 軌跡的中間點或中間段：時間由各關(guān)節(jié)的最大速度和加速度決定，將使用這些時間中的一個最長時間(即用最低速關(guān)節(jié)確定的最長時間來歸一化)。,在關(guān)節(jié)軌跡的典型約束條件之下，我們所要研究的是選擇一種 n 次(或小于 n 次)的多項式函數(shù)，使得在各結(jié)點(初始點，提升點，下放點和終止點)上滿足對位置、速度和加速

14、度的要求，并使關(guān)節(jié)位置、速度和加速度在整個時間間隔 t0, tf 中保持連續(xù)。,16,規(guī)劃關(guān)節(jié)插值軌跡的約束條件：,17,一種方法是為每個關(guān)節(jié)規(guī)定一個七次多項式函數(shù),式中，未知系數(shù) aj 可由已知的位置和連續(xù)條件確定。但用這種高次多項式內(nèi)插給定的結(jié)點也許不能令人滿意，因為它的極值難求，而且容易產(chǎn)生額外的運動。 另一種辦法是將整個關(guān)節(jié)空間軌跡分割成幾段，在每段軌跡中用不同的低次多項式來插值。有幾種分割軌跡的方法，每種方法的特性各不相同。,434 軌跡 每個關(guān)節(jié)有下面三段軌跡：第一段由初始點到提升點的軌跡用四次多項式表示。第二段(或中間段）由提升點到下放點的軌跡用三次多項式表示。最后一段由下放點到

15、終止點的軌跡由四次多項式表示。,18,353 軌跡 與434軌跡相同，但每段所用多項式次數(shù)與前種不同。第一段用三次多項式，第二段用五次多項式，最后一段用三次多項式。 (33333) 軌跡 對五段軌跡都使用三次多項式樣條函數(shù)。 注意，上述討論對每個關(guān)節(jié)軌跡都是有效的，即每個關(guān)節(jié)軌跡可分割成三段或五段。,7.3.1 434 關(guān)節(jié)軌跡的計算,對于N個關(guān)節(jié), 在每段軌跡規(guī)劃中就要確定 N 條關(guān)節(jié)軌跡，引用歸一化時間變量 是方便的，它使我們能用同樣的方法處理每個關(guān)節(jié)每段軌跡的方程。時間變化范圍均由 (各段軌跡的初始時間)變到 (各段軌跡的終止時間)。,19,定義下列變量：,軌跡是由多項式序列hi(t)構(gòu)

16、成的，這些多項式合起來形成關(guān)節(jié) j 的軌跡。在每段軌跡中關(guān)節(jié)變量的多項式用歸一化時間表示為:,20,這些關(guān)節(jié)軌跡分段多項式所應(yīng)滿足的邊界條件為:,21,434關(guān)節(jié)軌跡的邊界條件如圖示。,22,這些多項式對實際時間 t 的一階和二階導(dǎo)數(shù)?？蓪懗?第一段軌跡的基本多項式是四次的,23,對于t =0(這段軌跡的初始位置)，要滿足此位置的邊界條件，則,24,用求出的這些未知量得到,對于t=1(這段軌跡的終點)，在此位置，我們放寬插值多項式必須準確通過該點的要求，僅要求在此位置的速度和加速度必須和下一段軌跡起點的速度和加速度連續(xù)。此處的速度和加速度為,第二段軌跡的基本多項式是三次的,25,對于t=0(提

17、升點)，此點的速度和加速度分別為,由于此點的速度相加速度必須分別和前一段軌跡終點的速度和加速度連續(xù)，故可得,26,或,或,對于t=1(下放點)，該點的速度相加速度必須與下一段軌跡起點處的速度和加速度連續(xù)?？煞謩e求得此位置的速度和加速度為,27,末段軌跡的基本多項式是四次的,如果在上式中以 代替 t ，我們就把歸一化時間 t 由 移至 。,28,可得,對于 (這段軌跡的終點)，要滿足軌跡終點的邊界條件，即,對于 (這段軌跡的起點)，要滿足軌跡起點的邊界條件，即,29,在此下放點的速度和加速度連續(xù)性條件是：,或,30,可以求出相連軌跡段間關(guān)節(jié)角之差為,軌跡多項式其余七個未知系數(shù)可由以上的速度、加速

18、度連續(xù)約束條件的聯(lián)立方程解出，用矩陣矢量符號改寫這些方程，可得,其中,31,或,C 矩陣的結(jié)構(gòu)便于計算未知系數(shù)，若時間間隔 ti 為正值， C 的逆矩陣總是存在的。則關(guān)節(jié)各段關(guān)節(jié)軌跡多項式系數(shù)即可全部求得。,32,同樣，我們可用此方法可計算353 關(guān)節(jié)軌跡。,對于末段軌跡，把歸一化時間 t 由-1，0重新變回0, 1，可求出 hn(t),33,4-3-4 關(guān)節(jié)軌跡多項式,34,35,3-5-3 關(guān)節(jié)軌跡多項式,36,7.3.2 三次樣條軌跡（五段三次多項式）,用一組三次多項式插補給定的函數(shù)并保證插補點處一階和二階導(dǎo)數(shù)連續(xù)的方法，稱為三次樣條函數(shù)法。所能達到的近似程度和光滑程度是相當好的。 一般

19、來說，樣條曲線是在插值點具有 k-1 階導(dǎo)數(shù)連續(xù)性的 k次多項式。對于三次樣條函數(shù)，一階導(dǎo)數(shù)代表速度的連續(xù)性，二階導(dǎo)數(shù)代表加速度的連續(xù)性。 三次樣條函數(shù)有某些優(yōu)點。首先，它是使速度和加速度連續(xù)的最低次多項式函數(shù)。其次，低次多項式減少了計算量和數(shù)值不穩(wěn)定的可能性。,每段關(guān)節(jié)軌跡的五段三次多項式的通式為,其中,37,在應(yīng)用五段三次多項式插值時，需要有五段軌跡和六個插值點。但是，在前面的討論中只有四個插值點，即初始點、提升點、下放點和終止點。所以，必須選擇另外兩個插值點，以便有足夠多的邊界條件求解各多項式系數(shù)?？梢栽谔嵘c和下放點之間選取這兩個額外的結(jié)點。 沒有必要知道這兩個點的確切位置，只要知道時

20、間間隔，以及必須滿足這兩點速度和加速度的連續(xù)性條件。因此，這組關(guān)節(jié)軌跡分段多項式必須滿足的邊界條件是： (1) 在初始點、提升點、下放點和終止點的位置約束； (2) 在所有插值點的速度和加速度的連續(xù)性。,38,五段三次關(guān)節(jié)軌跡的邊界條件示于中。其中有下劃線的變量是在計算五段三次多項式之前的已知量。,39,這些多項式對實際時間的一階和二階導(dǎo)數(shù)為：,式中，tj 是通過第 j 段軌跡所需的實際時間。給定了初始點和終止點的位置、速度和加速度，第一段軌跡和末段軌跡的多項式 h1 (t) 和hn (t) 就完全確定了。旦算出這兩個多項式，就可用位置約束和連續(xù)條件求出h2 (t)、 h3 (t) 和 h4

21、(t) 。,第一段軌跡，基本多項式為,當 t=0 時，由該位置的邊界條件，可得,(給定),40,由此可得：,當 t=1 時，由此位置的邊界條件，可得：,可得,其中，i= i i-1 。第一段軌跡的多項式就完全確定了。,(給定),41,由上式可求出 t1時的速度和加速度：,此速度相加速度必須與下一段軌跡起點的速度和加速度連續(xù)。,末段軌跡的多項式為（仍做類似代換 ）,當t0 和t1時（對應(yīng) 和 ），由邊界條件，有,42,由上述方程解出未知系數(shù)，最后得到：,給定初始點、提升點、下放點和終止點的位置以及通過每段軌跡所需要的時間 (tj ) 后，五段三次多項式即可唯一地確定，滿足所有位置約束和連續(xù)條件。

22、,算出以上個多項式，就可用位置約束和連續(xù)條件求出h2 (t)、 h3 (t) 和 h4 (t) 。,43,對于較復(fù)雜的機器人系統(tǒng)，為了控制操作機完成作業(yè)而設(shè)計了編程語言。在這種系統(tǒng)中，作業(yè)通常是用操作機手部(或末端執(zhí)行器)必須通過的笛卡爾結(jié)點序列給定的。因此，描述操作機在作業(yè)中的運動時，我們就更關(guān)心描述操作機手部要達到的目標位置及通過的笛卡爾空間曲線(或路徑)的形式。,7.4 笛卡爾路徑軌跡規(guī)劃,一般，實現(xiàn)笛卡爾路徑規(guī)劃可采用下述兩個相連的步驟：(1)沿笛卡爾路徑，按照某種規(guī)則以笛卡爾坐標生成或選擇一組結(jié)點或插值點；(2)規(guī)定一種函數(shù)，按某些準則連接這些結(jié)點(或逼近分段的路徑)。 對于第二步，

23、所選用的準則可以采用兩種控制算法，以保證跟蹤給定的路徑。,44,(1) 面向笛卡爾空間的方法。在此方法中，大部分計算和優(yōu)化是以笛卡爾坐標完成的，然后，在手部這一級上進行控制。按固定的取樣間隔在預(yù)定路徑上選擇伺服取樣點，在控制操作機時實時地把它們轉(zhuǎn)換為與之相應(yīng)的關(guān)節(jié)變量。所得到的軌跡是分段直線。 (2) 面向關(guān)節(jié)空間的方法。這種方法用關(guān)節(jié)變量空間中的低次多項式函數(shù)迫近直線路徑上的兩相鄰結(jié)點間的一段路徑，而控制是在關(guān)節(jié)這一級上進行的。所得到的笛卡爾路徑是不分段的直線。,Paul 敘述了用一系列直線段構(gòu)成操作機手部笛卡爾路徑的設(shè)計方法。把手部在這些直線段中的速度和加速度轉(zhuǎn)換到關(guān)節(jié)坐標，并用二次插值進

24、行平滑連接，從而實現(xiàn)了手部的規(guī)劃運動控制。我們將學(xué)習(xí)如何用該方法設(shè)計笛卡爾直線路徑。,45,對于笛卡爾路徑控制，軌跡設(shè)定點算法為：,循環(huán)：等待下一個控制間隔； 時刻操作機末端所處的位置； 相應(yīng)于 的關(guān)節(jié)變量解； 若 ，則退出循環(huán)； 轉(zhuǎn)向循環(huán)。,這里，除了需要計算每一控制間隔中操作機手部軌跡函數(shù) 外，還需把笛卡爾位置變換為相應(yīng)的關(guān)節(jié)變量 。矩陣函數(shù) 表示操作機手部在 時刻的預(yù)定位置。可用44齊次變換矩陣表示。,46,在可編程機器人系統(tǒng)中，預(yù)定的運動可用笛卡爾結(jié)點序列給定，每個結(jié)點可用聯(lián)系操作機手部坐標系和工作空間坐標系的齊次變換來描述。相應(yīng)于這些笛卡爾結(jié)點的關(guān)節(jié)坐標可用運動學(xué)逆問題求解程序算出，

25、并用二次多項式平滑關(guān)節(jié)坐標中兩個相鄰的關(guān)節(jié)結(jié)點，以利于控制，從而，控制操作機手部沿連接這些結(jié)點的直線運動。這種方法的優(yōu)點是，它使我們能控制操作機手部跟蹤運動的物體。雖然目標位置是用變換描述的，但它們并不規(guī)定操作機手部怎樣由一個變換運動到另一個變換。 Paul 用一個直線平移和兩個轉(zhuǎn)動來完成兩相鄰笛卡爾結(jié)點間的運動。第一個轉(zhuǎn)動是繞單位矢量 k 進行的，把工具或末端執(zhí)行器的軸線調(diào)整到預(yù)定的接近方向；而第二個轉(zhuǎn)動則繞工具軸調(diào)整工具的方向。,7.4.1 齊次變換矩陣法,47,一般來說，操作機的目標位置可用如下基本矩陣方程表示：,其中，0T6 為描述操作機手部相對于機座坐標系位姿的齊次變換矩陣； 6Tt

26、ool 為描述工具相對于手部坐標系位姿的齊次變換矩陣，它描述了受控制的工具端點； 0Cbase(t) 為描述物體位姿的工作坐標系相對于機座坐標系的齊次變換矩陣； basePobj 為描述末端執(zhí)行器相對于工作坐標系預(yù)定抓取物體的位姿的齊次變換矩陣。,左邊的矩陣描述操作機抓取位姿，而右邊的矩陣描述了我們要讓操作機工具抓握物體特征部位的位姿。,48,我們可以解出0T6 ，它描述了以正確和預(yù)定的方式抓取物體時操作機應(yīng)有的形態(tài)。,如果計算0T6 的速度足夠高，并轉(zhuǎn)換成相應(yīng)的關(guān)節(jié)角，那么就可控制操作機沿預(yù)定的軌跡運動 利用前式，確定一項作業(yè)的 N個 目標位置的序列可表示為,49,由Ci(t) Pi確定的位

27、置，可求得相鄰點之間的距離。若進一步給定線速度和角速度，即可求得由位置i 到 i+1所需的時間。,以上位置的工具和運動坐標系都是相對于機座坐標系給定的。從一個位置移向下一個位置最好是相對于目標坐標系給定位置和工具。這樣，從運動坐標系觀察，工具是靜止的。為了做到這一點，需要相對于后續(xù)的坐標系重新定義當前的位置和工具。利用雙下標符號 Pij 重新定義Pi變換，就不難做到這一點。 Pij是在第 j 個工作坐標系中表示的位置Pi。則在位置1，將它在目標坐標系中表示，可得,將它用位置2時的坐標系表示，則有,50,由以上兩個方程即可求得,上述方程的目的是耍在已知 P11時 求得P12 。因此，在任意二相鄰

28、位置 I 和 i+1 之間的運動有,其中Pi,i+11 和Pi+1,i+1 為上面所討論過的變換。 Paul 使用了一種簡單的方法，以控制操作機手部從一個變換運動到另一個變換。此法包括一個平移運動和一個繞空間某定軸的轉(zhuǎn)動以及另一個繞工具軸的轉(zhuǎn)動，以便形成操作機受控的線速度和角速度運動。第一個轉(zhuǎn)動使工具軸與預(yù)定的接近方向?qū)?，第二個轉(zhuǎn)動使工具軸與方向矢量對準。,51,由位置 i 到 i+1的運動可用“驅(qū)動”變換D()表示，它是歸一化時間 的函數(shù),其中，,可得,于是有,52,用位置 i 和 i+1各自的齊次變換矩陣表示它們，可得,利用前式，求 Pi,i+1的逆并乘以Pi+1,i+1 ，得到,53,

29、若驅(qū)動函數(shù)由一個平移運動和兩個旋轉(zhuǎn)運動構(gòu)成，那么這些平移和轉(zhuǎn)動量將直接同成正比。若隨時間線性地變化，則 D() 所代表的合成運動將相應(yīng)于一個不變的線速度和兩個角速度。 平移運動可由齊次變換矩陣L()表示；第一個轉(zhuǎn)動可用齊次變換矩陣RA()表示，它的作用是把Pi位置的接近矢量轉(zhuǎn)向Pi+1位置的接近矢量；第二個轉(zhuǎn)動可用RB()表示，它的作用是把Pi 位置的方向矢量繞工具軸轉(zhuǎn)向Pi+1位置的方向矢量。因此，驅(qū)動函數(shù)可表示為,54,其中，,旋轉(zhuǎn)矩陣RA() 表示繞 Pi 的接近矢量（zi 軸）轉(zhuǎn)動了角，又繞方向矢量（yi 軸）轉(zhuǎn)動了角； RB() 表示繞工具的接近矢量轉(zhuǎn)動 角的旋轉(zhuǎn)。,55,其中，,56,操作機經(jīng)常要沿相連的直線段軌跡運動，以滿足作業(yè)運動的要求