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

1、1& 第七章第七章 機(jī)器人的軌跡規(guī)劃機(jī)器人的軌跡規(guī)劃7.1 7.1 機(jī)器人規(guī)劃的定義和作用機(jī)器人規(guī)劃的定義和作用7.1.1 概述概述機(jī)器人學(xué)中的一個(gè)基本問題是為解決某個(gè)預(yù)定的任務(wù)而規(guī)劃機(jī)器人學(xué)中的一個(gè)基本問題是為解決某個(gè)預(yù)定的任務(wù)而規(guī)劃機(jī)器人的動(dòng)作，然后在機(jī)器人執(zhí)行完成那些動(dòng)作所需的命令時(shí)機(jī)器人的動(dòng)作，然后在機(jī)器人執(zhí)行完成那些動(dòng)作所需的命令時(shí)控制它。這里，規(guī)劃的意思就是機(jī)器人在行動(dòng)前確定一系列動(dòng)控制它。這里，規(guī)劃的意思就是機(jī)器人在行動(dòng)前確定一系列動(dòng)作作(作決策作決策)，這種動(dòng)作的確定可用問題求解系統(tǒng)來解決，給定，這種動(dòng)作的確定可用問題求解系統(tǒng)來解決，給定初始情況后，該系統(tǒng)可達(dá)到某一規(guī)定

2、的目標(biāo)。因此，初始情況后，該系統(tǒng)可達(dá)到某一規(guī)定的目標(biāo)。因此，規(guī)劃就是規(guī)劃就是指機(jī)器人為達(dá)到目標(biāo)而需要的行動(dòng)過程的描述。指機(jī)器人為達(dá)到目標(biāo)而需要的行動(dòng)過程的描述。 規(guī)劃內(nèi)容可能沒有次序，但是一般來說，規(guī)劃具有某個(gè)規(guī)劃規(guī)劃內(nèi)容可能沒有次序，但是一般來說，規(guī)劃具有某個(gè)規(guī)劃目標(biāo)的蘊(yùn)含排序。例如，早晨起床后的安排。目標(biāo)的蘊(yùn)含排序。例如，早晨起床后的安排。 缺乏規(guī)劃可能導(dǎo)致不是最佳的問題求解，甚至得不到問題的缺乏規(guī)劃可能導(dǎo)致不是最佳的問題求解，甚至得不到問題的求解。求解。2 許多規(guī)劃所包的步驟是含糊的，而且需要進(jìn)一步說明（子許多規(guī)劃所包的步驟是含糊的，而且需要進(jìn)一步說明（子規(guī)劃）。規(guī)劃）。大多數(shù)規(guī)劃具有很

3、大的子規(guī)劃結(jié)構(gòu)，規(guī)劃中的每個(gè)大多數(shù)規(guī)劃具有很大的子規(guī)劃結(jié)構(gòu)，規(guī)劃中的每個(gè)目標(biāo)可以由達(dá)到此目標(biāo)的比較詳細(xì)的子規(guī)劃所代替。因此，目標(biāo)可以由達(dá)到此目標(biāo)的比較詳細(xì)的子規(guī)劃所代替。因此，最終得到的規(guī)劃是某個(gè)問題求解算符的線性或分部排序，但最終得到的規(guī)劃是某個(gè)問題求解算符的線性或分部排序，但是由算符來實(shí)現(xiàn)的目標(biāo)常常具有分層結(jié)構(gòu)。是由算符來實(shí)現(xiàn)的目標(biāo)常常具有分層結(jié)構(gòu)。 把某些比較復(fù)雜的問題分解為一些比較小的問題的想法使把某些比較復(fù)雜的問題分解為一些比較小的問題的想法使我們應(yīng)用規(guī)劃方法求解問題在實(shí)際上成為可能。我們應(yīng)用規(guī)劃方法求解問題在實(shí)際上成為可能。 有兩條能夠?qū)崿F(xiàn)這種分解的重要途徑：第一條是當(dāng)從一個(gè)有兩條

4、能夠?qū)崿F(xiàn)這種分解的重要途徑：第一條是當(dāng)從一個(gè)問題狀態(tài)移動(dòng)到下一個(gè)狀態(tài)時(shí)，無需計(jì)算整個(gè)新的狀態(tài)，而問題狀態(tài)移動(dòng)到下一個(gè)狀態(tài)時(shí)，無需計(jì)算整個(gè)新的狀態(tài)，而只要考慮狀態(tài)中可能變化了的那些部分。第二條是把單一的只要考慮狀態(tài)中可能變化了的那些部分。第二條是把單一的困難問題分割為幾個(gè)有希望的、較為容易解決的子問題，這困難問題分割為幾個(gè)有希望的、較為容易解決的子問題，這種分解能夠使困難問題的求解變得容易些種分解能夠使困難問題的求解變得容易些。3 機(jī)器人規(guī)劃分為機(jī)器人規(guī)劃分為高層規(guī)劃高層規(guī)劃和和低層規(guī)劃低層規(guī)劃。自動(dòng)規(guī)劃自動(dòng)規(guī)劃在機(jī)器人在機(jī)器人規(guī)劃中稱為高層規(guī)劃。在無特別說明時(shí)，機(jī)器人規(guī)劃都是指規(guī)劃中稱為高層規(guī)

5、劃。在無特別說明時(shí)，機(jī)器人規(guī)劃都是指自動(dòng)規(guī)劃。自動(dòng)規(guī)劃是一種重要的問題求解技術(shù)，它從某個(gè)自動(dòng)規(guī)劃。自動(dòng)規(guī)劃是一種重要的問題求解技術(shù)，它從某個(gè)特定的問題狀態(tài)出發(fā)，尋求一系列行為動(dòng)作，并建立一個(gè)操特定的問題狀態(tài)出發(fā)，尋求一系列行為動(dòng)作，并建立一個(gè)操作序列，直到求得目標(biāo)狀態(tài)為止。與一般問題求解相比，自作序列，直到求得目標(biāo)狀態(tài)為止。與一般問題求解相比，自動(dòng)規(guī)劃更注重于問題的求解過程，而不是求解結(jié)果。動(dòng)規(guī)劃更注重于問題的求解過程，而不是求解結(jié)果。 我們在闡述機(jī)器人自動(dòng)規(guī)劃問題時(shí)，機(jī)器人一般配備有傳我們在闡述機(jī)器人自動(dòng)規(guī)劃問題時(shí)，機(jī)器人一般配備有傳感器和一組能在某個(gè)易于理解的現(xiàn)場中完成的基本動(dòng)作。這感器和

6、一組能在某個(gè)易于理解的現(xiàn)場中完成的基本動(dòng)作。這些動(dòng)作可把該現(xiàn)場從一種狀態(tài)或布局變換為另一種狀態(tài)或布些動(dòng)作可把該現(xiàn)場從一種狀態(tài)或布局變換為另一種狀態(tài)或布局。例如，局。例如， “積木世界積木世界” 。4 機(jī)器人能得到的一個(gè)解答是由下面的算符序列組成的：機(jī)器人能得到的一個(gè)解答是由下面的算符序列組成的：目標(biāo)狀態(tài)目標(biāo)狀態(tài) 機(jī)器人規(guī)劃是機(jī)器人學(xué)的一個(gè)重要研究領(lǐng)域，也是人工智能機(jī)器人規(guī)劃是機(jī)器人學(xué)的一個(gè)重要研究領(lǐng)域，也是人工智能與機(jī)器人學(xué)一個(gè)令人感興趣的結(jié)合點(diǎn)。與機(jī)器人學(xué)一個(gè)令人感興趣的結(jié)合點(diǎn)。5 機(jī)器人的規(guī)劃程序只需要給定任務(wù)機(jī)器人的規(guī)劃程序只需要給定任務(wù)初初始狀態(tài)和最終狀態(tài)的描始狀態(tài)和最終狀態(tài)的描述。這

7、些規(guī)劃系統(tǒng)一般都不說明為述。這些規(guī)劃系統(tǒng)一般都不說明為實(shí)實(shí)現(xiàn)現(xiàn)一一個(gè)算符所需的詳個(gè)算符所需的詳細(xì)細(xì)的的機(jī)器人運(yùn)動(dòng)。任務(wù)規(guī)劃程序機(jī)器人運(yùn)動(dòng)。任務(wù)規(guī)劃程序則則把任務(wù)級的說明變換成操作機(jī)級把任務(wù)級的說明變換成操作機(jī)級的說明。為了進(jìn)行這種變換，任務(wù)規(guī)劃程序必須的說明。為了進(jìn)行這種變換，任務(wù)規(guī)劃程序必須包含包含被操作物被操作物體、任務(wù)環(huán)境、機(jī)器人執(zhí)行的任務(wù)、環(huán)境的體、任務(wù)環(huán)境、機(jī)器人執(zhí)行的任務(wù)、環(huán)境的初初始狀態(tài)和所要求始狀態(tài)和所要求的最終的最終( (目標(biāo)目標(biāo)) )狀態(tài)等描述。任務(wù)規(guī)劃程序狀態(tài)等描述。任務(wù)規(guī)劃程序的的輸出就是一個(gè)機(jī)器輸出就是一個(gè)機(jī)器人程序，在指定的人程序，在指定的初初始狀態(tài)下執(zhí)行該程序后，

8、就能達(dá)到所要求始狀態(tài)下執(zhí)行該程序后，就能達(dá)到所要求的最終狀態(tài)。的最終狀態(tài)。 任務(wù)規(guī)劃有三個(gè)階段：建立模型、任務(wù)說明和操作機(jī)程序綜任務(wù)規(guī)劃有三個(gè)階段：建立模型、任務(wù)說明和操作機(jī)程序綜合。任務(wù)的世界模型應(yīng)含有如下的信息合。任務(wù)的世界模型應(yīng)含有如下的信息:(1):(1)任務(wù)環(huán)境中的所有任務(wù)環(huán)境中的所有物體和機(jī)器人的幾何描述；物體和機(jī)器人的幾何描述；(2)(2)所有物體的物理描述；所有物體的物理描述；(3)(3)所有所有連接件的運(yùn)動(dòng)學(xué)描述，連接件的運(yùn)動(dòng)學(xué)描述，(4) (4) 機(jī)器人和傳感器特性的描述。在世機(jī)器人和傳感器特性的描述。在世界模型中，任務(wù)狀態(tài)模型還必包括全部物體和連接件的布局。界模型中，任務(wù)

9、狀態(tài)模型還必包括全部物體和連接件的布局。7.1.2 機(jī)器人任務(wù)規(guī)劃的作用機(jī)器人任務(wù)規(guī)劃的作用6 機(jī)器人軌跡規(guī)劃機(jī)器人軌跡規(guī)劃屬于機(jī)器人低層規(guī)劃，基本上不涉及人工屬于機(jī)器人低層規(guī)劃，基本上不涉及人工智能問題，而是在機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的基礎(chǔ)上，討論機(jī)智能問題，而是在機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的基礎(chǔ)上，討論機(jī)器人運(yùn)動(dòng)的規(guī)劃及其方法。所謂器人運(yùn)動(dòng)的規(guī)劃及其方法。所謂軌跡軌跡，就是指機(jī)器人在運(yùn)動(dòng)，就是指機(jī)器人在運(yùn)動(dòng)過程中的位移、速度和加速度。過程中的位移、速度和加速度。 軌跡規(guī)劃問題通常是將軌跡規(guī)劃器看成軌跡規(guī)劃問題通常是將軌跡規(guī)劃器看成“黑箱黑箱”，接受表示，接受表示路徑約束的輸入變量，輸出為起點(diǎn)和終點(diǎn)之

10、間按時(shí)間排列的操路徑約束的輸入變量，輸出為起點(diǎn)和終點(diǎn)之間按時(shí)間排列的操作機(jī)中間形態(tài)（位姿作機(jī)中間形態(tài)（位姿, 速度和加速度）序列。速度和加速度）序列。路徑約束路徑約束動(dòng)力學(xué)約束動(dòng)力學(xué)約束路徑設(shè)定路徑設(shè)定軌跡規(guī)劃器軌跡規(guī)劃器7.2 7.2 機(jī)器人軌跡規(guī)劃的一般性問題機(jī)器人軌跡規(guī)劃的一般性問題由初始點(diǎn)運(yùn)動(dòng)到終止由初始點(diǎn)運(yùn)動(dòng)到終止點(diǎn)，所經(jīng)過的由中間點(diǎn)，所經(jīng)過的由中間形態(tài)序列構(gòu)成的空間形態(tài)序列構(gòu)成的空間曲線稱為曲線稱為路徑。路徑。這些這些形態(tài)序列即是曲線上形態(tài)序列即是曲線上的的“點(diǎn)點(diǎn)”。7規(guī)劃操作機(jī)的軌跡有兩種常用的方法：規(guī)劃操作機(jī)的軌跡有兩種常用的方法： 第一種方法：要求使用者在沿軌跡選定的位置點(diǎn)上

11、第一種方法：要求使用者在沿軌跡選定的位置點(diǎn)上(稱為結(jié)稱為結(jié)節(jié)或插值點(diǎn)節(jié)或插值點(diǎn))顯式地給定廣義坐標(biāo)位置、速度和加速度的一組顯式地給定廣義坐標(biāo)位置、速度和加速度的一組約束約束(例如，連續(xù)性和光滑程度等例如，連續(xù)性和光滑程度等)。然后，軌跡規(guī)劃器從插值。然后，軌跡規(guī)劃器從插值和滿足插值點(diǎn)約束的函數(shù)中選定參數(shù)化軌跡。顯然，在這種和滿足插值點(diǎn)約束的函數(shù)中選定參數(shù)化軌跡。顯然，在這種方法中，約束的給定和操作機(jī)軌跡規(guī)劃是在關(guān)節(jié)坐標(biāo)系中進(jìn)方法中，約束的給定和操作機(jī)軌跡規(guī)劃是在關(guān)節(jié)坐標(biāo)系中進(jìn)行的。行的。 第二種方法：使用者以解析函數(shù)顯式地給定操作機(jī)必經(jīng)之第二種方法：使用者以解析函數(shù)顯式地給定操作機(jī)必經(jīng)之路徑，

12、例如，笛卡爾坐標(biāo)中的直線路徑。然后，軌跡規(guī)劃器路徑，例如，笛卡爾坐標(biāo)中的直線路徑。然后，軌跡規(guī)劃器在關(guān)節(jié)坐標(biāo)或笛卡幾坐標(biāo)中確定一條與給定路徑近似的軌跡。在關(guān)節(jié)坐標(biāo)或笛卡幾坐標(biāo)中確定一條與給定路徑近似的軌跡。在這種方法中，路徑約束是在笛卡爾坐標(biāo)中給定的。在這種方法中，路徑約束是在笛卡爾坐標(biāo)中給定的。8 在第一種方法中，約束的給定和操作機(jī)軌跡規(guī)劃在關(guān)節(jié)坐標(biāo)在第一種方法中，約束的給定和操作機(jī)軌跡規(guī)劃在關(guān)節(jié)坐標(biāo)系中進(jìn)行。由于對操作機(jī)手部沒有約束，使用者難于跟蹤操作系中進(jìn)行。由于對操作機(jī)手部沒有約束，使用者難于跟蹤操作機(jī)手部運(yùn)行的路徑。因此，操作機(jī)手部可能在沒有事先警告的機(jī)手部運(yùn)行的路徑。因此，操作機(jī)手

13、部可能在沒有事先警告的情況下與障礙物相碰。情況下與障礙物相碰。 在第二種方法中，路徑約束在笛卡爾坐標(biāo)中給定，而關(guān)節(jié)驅(qū)在第二種方法中，路徑約束在笛卡爾坐標(biāo)中給定，而關(guān)節(jié)驅(qū)動(dòng)器是在關(guān)節(jié)坐標(biāo)中受控制的。因此，為了求得一條逼近給定動(dòng)器是在關(guān)節(jié)坐標(biāo)中受控制的。因此，為了求得一條逼近給定路徑的軌跡，必須用函數(shù)近似把笛卡爾坐標(biāo)中的路徑約束變換路徑的軌跡，必須用函數(shù)近似把笛卡爾坐標(biāo)中的路徑約束變換為關(guān)節(jié)坐標(biāo)中的路徑約束，再確定滿足關(guān)節(jié)坐標(biāo)路徑約束的參為關(guān)節(jié)坐標(biāo)中的路徑約束，再確定滿足關(guān)節(jié)坐標(biāo)路徑約束的參數(shù)化軌跡。數(shù)化軌跡。v 軌跡規(guī)劃既可在關(guān)節(jié)變量空間中進(jìn)行，也可在笛卡爾空間進(jìn)軌跡規(guī)劃既可在關(guān)節(jié)變量空間中進(jìn)行

14、，也可在笛卡爾空間進(jìn)行。對于關(guān)節(jié)變量空間的規(guī)劃，要規(guī)劃關(guān)節(jié)變量的時(shí)間函數(shù)及行。對于關(guān)節(jié)變量空間的規(guī)劃，要規(guī)劃關(guān)節(jié)變量的時(shí)間函數(shù)及其前二階時(shí)間導(dǎo)數(shù)，以便描述操作機(jī)的預(yù)定運(yùn)動(dòng)。在笛卡爾空其前二階時(shí)間導(dǎo)數(shù)，以便描述操作機(jī)的預(yù)定運(yùn)動(dòng)。在笛卡爾空間規(guī)劃中，要規(guī)劃操作機(jī)手部位置、速度和加速度的時(shí)間函數(shù)，間規(guī)劃中，要規(guī)劃操作機(jī)手部位置、速度和加速度的時(shí)間函數(shù)，而相應(yīng)的關(guān)節(jié)位置、速度和加速度可根據(jù)手部信息導(dǎo)出。而相應(yīng)的關(guān)節(jié)位置、速度和加速度可根據(jù)手部信息導(dǎo)出。9 面向笛卡爾空間方法的優(yōu)點(diǎn)是概念直觀，而且沿預(yù)定直線路面向笛卡爾空間方法的優(yōu)點(diǎn)是概念直觀，而且沿預(yù)定直線路徑可達(dá)到相當(dāng)?shù)臏?zhǔn)確性?？墒怯捎诂F(xiàn)代還沒有可用

15、笛卡爾坐標(biāo)徑可達(dá)到相當(dāng)?shù)臏?zhǔn)確性。可是由于現(xiàn)代還沒有可用笛卡爾坐標(biāo)測量操作機(jī)手部位置的傳感器，所有可用的控制算法都是建立測量操作機(jī)手部位置的傳感器，所有可用的控制算法都是建立在關(guān)節(jié)坐標(biāo)基礎(chǔ)上的。因此，笛卡爾空間路徑規(guī)劃就需要在笛在關(guān)節(jié)坐標(biāo)基礎(chǔ)上的。因此，笛卡爾空間路徑規(guī)劃就需要在笛卡爾坐標(biāo)和關(guān)節(jié)之間進(jìn)行實(shí)時(shí)變換，這是一個(gè)計(jì)算量很大的任卡爾坐標(biāo)和關(guān)節(jié)之間進(jìn)行實(shí)時(shí)變換，這是一個(gè)計(jì)算量很大的任務(wù)，常常導(dǎo)致較長的控制間隔。務(wù)，常常導(dǎo)致較長的控制間隔。 由笛卡爾坐標(biāo)向關(guān)節(jié)坐標(biāo)的變換是病態(tài)的，因而它不是一一由笛卡爾坐標(biāo)向關(guān)節(jié)坐標(biāo)的變換是病態(tài)的，因而它不是一一對應(yīng)的映射。對應(yīng)的映射。 如果在軌跡規(guī)劃階段要考慮

16、操作機(jī)的動(dòng)力學(xué)特性，就要以笛如果在軌跡規(guī)劃階段要考慮操作機(jī)的動(dòng)力學(xué)特性，就要以笛卡爾坐標(biāo)給定路徑約束，同時(shí)以關(guān)節(jié)坐標(biāo)給定物理約束卡爾坐標(biāo)給定路徑約束，同時(shí)以關(guān)節(jié)坐標(biāo)給定物理約束(例如，例如，每個(gè)關(guān)節(jié)電機(jī)的力和力矩、速度和加速度權(quán)限每個(gè)關(guān)節(jié)電機(jī)的力和力矩、速度和加速度權(quán)限)。這就會(huì)使最。這就會(huì)使最后的優(yōu)化問題具有在兩個(gè)不同坐標(biāo)系中的混合約束。后的優(yōu)化問題具有在兩個(gè)不同坐標(biāo)系中的混合約束。在笛卡爾空間進(jìn)行軌跡規(guī)劃的特點(diǎn)：在笛卡爾空間進(jìn)行軌跡規(guī)劃的特點(diǎn)：10 在關(guān)節(jié)變量空間的規(guī)劃有三個(gè)優(yōu)點(diǎn)：在關(guān)節(jié)變量空間的規(guī)劃有三個(gè)優(yōu)點(diǎn)：(1) 直接用運(yùn)動(dòng)時(shí)的受控變量規(guī)劃軌跡；直接用運(yùn)動(dòng)時(shí)的受控變量規(guī)劃軌跡；(2)

17、 軌跡規(guī)劃可接近實(shí)時(shí)地進(jìn)行；軌跡規(guī)劃可接近實(shí)時(shí)地進(jìn)行；(3) 關(guān)節(jié)軌跡易于規(guī)劃。關(guān)節(jié)軌跡易于規(guī)劃。 伴隨的缺點(diǎn)是難于確定運(yùn)動(dòng)中各桿件和手的位置，但是，為伴隨的缺點(diǎn)是難于確定運(yùn)動(dòng)中各桿件和手的位置，但是，為了避開軌跡上的障礙常常又要求知道一些桿件和手位置。了避開軌跡上的障礙常常又要求知道一些桿件和手位置。 由于面向笛卡爾空間的方法有前述鐘種缺點(diǎn)，使得面向關(guān)節(jié)由于面向笛卡爾空間的方法有前述鐘種缺點(diǎn)，使得面向關(guān)節(jié)空間的方法被廣泛采用。它把笛卡爾結(jié)點(diǎn)變換為相應(yīng)的關(guān)節(jié)坐空間的方法被廣泛采用。它把笛卡爾結(jié)點(diǎn)變換為相應(yīng)的關(guān)節(jié)坐標(biāo)，并用低次多項(xiàng)式內(nèi)插這些關(guān)節(jié)結(jié)點(diǎn)。這種方法的優(yōu)點(diǎn)是計(jì)標(biāo)，并用低次多項(xiàng)式內(nèi)插這些關(guān)

18、節(jié)結(jié)點(diǎn)。這種方法的優(yōu)點(diǎn)是計(jì)算較快，而且易于處理操作機(jī)的動(dòng)力學(xué)約束。但當(dāng)取樣點(diǎn)落在算較快，而且易于處理操作機(jī)的動(dòng)力學(xué)約束。但當(dāng)取樣點(diǎn)落在擬合的光滑多項(xiàng)式曲線上時(shí)，面向關(guān)節(jié)空間的方法沿笛卡爾路擬合的光滑多項(xiàng)式曲線上時(shí)，面向關(guān)節(jié)空間的方法沿笛卡爾路徑的準(zhǔn)確性會(huì)有損失。徑的準(zhǔn)確性會(huì)有損失。11一般說來，生成關(guān)節(jié)軌跡設(shè)定點(diǎn)的基本算法是很簡單的：一般說來，生成關(guān)節(jié)軌跡設(shè)定點(diǎn)的基本算法是很簡單的：0tt循環(huán)：等待下一個(gè)控制間隔；循環(huán)：等待下一個(gè)控制間隔； 時(shí)刻操作機(jī)關(guān)節(jié)所處的位置時(shí)刻操作機(jī)關(guān)節(jié)所處的位置； 若若 ，則退出循環(huán)；，則退出循環(huán)； 轉(zhuǎn)向循環(huán)。轉(zhuǎn)向循環(huán)。ttt ( )h ttftt 從上述算法可以看

19、出，要計(jì)算的是在每個(gè)控制間隔中必須更從上述算法可以看出，要計(jì)算的是在每個(gè)控制間隔中必須更新的軌跡函數(shù)新的軌跡函數(shù)(或軌跡規(guī)劃器或軌跡規(guī)劃器)h(t)。因此，對規(guī)劃的軌跡要提出。因此，對規(guī)劃的軌跡要提出四個(gè)限制。第一，必須便于用迭代方式計(jì)算軌跡設(shè)定點(diǎn)；第二，四個(gè)限制。第一，必須便于用迭代方式計(jì)算軌跡設(shè)定點(diǎn)；第二，必須求出并明確給定中間位置；第三，必須保證關(guān)節(jié)變量及其必須求出并明確給定中間位置；第三，必須保證關(guān)節(jié)變量及其前二階時(shí)間導(dǎo)數(shù)的連續(xù)性，使得規(guī)劃的關(guān)節(jié)軌跡是光滑的；最前二階時(shí)間導(dǎo)數(shù)的連續(xù)性，使得規(guī)劃的關(guān)節(jié)軌跡是光滑的；最后，必須減少額外的運(yùn)動(dòng)后，必須減少額外的運(yùn)動(dòng) (例如，例如，“游移游移”

20、 )。7.3 7.3 關(guān)節(jié)插值軌跡關(guān)節(jié)插值軌跡12 若某關(guān)節(jié)若某關(guān)節(jié)(例如關(guān)節(jié)例如關(guān)節(jié) i )的關(guān)節(jié)軌跡使用的關(guān)節(jié)軌跡使用 p 個(gè)多項(xiàng)式，則要滿個(gè)多項(xiàng)式，則要滿足初始和終止條件足初始和終止條件(關(guān)節(jié)位置、速度和加速度關(guān)節(jié)位置、速度和加速度)，并保證這些變，并保證這些變量在多項(xiàng)式銜接處的連續(xù)性，因而需要確定量在多項(xiàng)式銜接處的連續(xù)性，因而需要確定 3( p十十1)個(gè)系數(shù)。個(gè)系數(shù)。 若給定附加的中間條件若給定附加的中間條件(例如位置例如位置)，則對每個(gè)中間條件需要，則對每個(gè)中間條件需要增加一系數(shù)。通常，可以給定兩個(gè)中間位置，一個(gè)靠近初始位增加一系數(shù)。通常，可以給定兩個(gè)中間位置，一個(gè)靠近初始位置；另一

21、個(gè)靠近終止位置。這樣做，除了可以較好地控制運(yùn)動(dòng)置；另一個(gè)靠近終止位置。這樣做，除了可以較好地控制運(yùn)動(dòng)外，還能保證操作機(jī)末端以適當(dāng)?shù)姆较螂x開起點(diǎn)和接近終點(diǎn)。外，還能保證操作機(jī)末端以適當(dāng)?shù)姆较螂x開起點(diǎn)和接近終點(diǎn)。 因此，對于連接初始位置和終止位置的每個(gè)關(guān)節(jié)變量，一因此，對于連接初始位置和終止位置的每個(gè)關(guān)節(jié)變量，一個(gè)七次多項(xiàng)式就足夠了，或者用兩段四次軌跡加一段三次軌跡個(gè)七次多項(xiàng)式就足夠了，或者用兩段四次軌跡加一段三次軌跡 （434），也可以用兩段三次軌跡加一段五次軌跡（），也可以用兩段三次軌跡加一段五次軌跡（353），或五段三次軌跡（），或五段三次軌跡（33333）。）。p +1個(gè)點(diǎn)個(gè)點(diǎn)由約束條件數(shù)

22、所對應(yīng)的多項(xiàng)式系數(shù)的個(gè)數(shù)確定多項(xiàng)式的次數(shù)由約束條件數(shù)所對應(yīng)的多項(xiàng)式系數(shù)的個(gè)數(shù)確定多項(xiàng)式的次數(shù)13 為了控制操作機(jī)，在規(guī)劃運(yùn)動(dòng)軌跡之前，需要給定機(jī)器人為了控制操作機(jī)，在規(guī)劃運(yùn)動(dòng)軌跡之前，需要給定機(jī)器人在初始點(diǎn)和終止點(diǎn)的手臂形態(tài)。在規(guī)劃機(jī)器人關(guān)節(jié)插值運(yùn)動(dòng)在初始點(diǎn)和終止點(diǎn)的手臂形態(tài)。在規(guī)劃機(jī)器人關(guān)節(jié)插值運(yùn)動(dòng)軌跡時(shí)，需要注意下述幾點(diǎn)：軌跡時(shí)，需要注意下述幾點(diǎn)：1、抓住一個(gè)物體時(shí)，手的運(yùn)動(dòng)方向應(yīng)該指向離開物體支承表、抓住一個(gè)物體時(shí)，手的運(yùn)動(dòng)方向應(yīng)該指向離開物體支承表面的方向。否則，手可能與支承面相碰。面的方向。否則，手可能與支承面相碰。2、若沿支承面的法線方向從初始點(diǎn)向外給定一個(gè)離開位置、若沿支承面的法

23、線方向從初始點(diǎn)向外給定一個(gè)離開位置(提升點(diǎn)提升點(diǎn))，并要求手，并要求手(即手部坐標(biāo)系的原點(diǎn)即手部坐標(biāo)系的原點(diǎn))經(jīng)過此位置，這種經(jīng)過此位置，這種離開運(yùn)動(dòng)就是允許的。如果還給定由初始點(diǎn)運(yùn)動(dòng)到離開位置離開運(yùn)動(dòng)就是允許的。如果還給定由初始點(diǎn)運(yùn)動(dòng)到離開位置的時(shí)間，我們就可以控制提起物體運(yùn)動(dòng)的速度。的時(shí)間，我們就可以控制提起物體運(yùn)動(dòng)的速度。3、對于手臂運(yùn)動(dòng)提升點(diǎn)的要求同樣也適用于終止位置運(yùn)動(dòng)的、對于手臂運(yùn)動(dòng)提升點(diǎn)的要求同樣也適用于終止位置運(yùn)動(dòng)的下放點(diǎn)下放點(diǎn)(即必須先運(yùn)動(dòng)到支承表面外法線方向上的某點(diǎn)，再慢即必須先運(yùn)動(dòng)到支承表面外法線方向上的某點(diǎn)，再慢慢下移至終止點(diǎn)慢下移至終止點(diǎn))。這樣，可獲得和控制正確的接

24、近方向。這樣，可獲得和控制正確的接近方向。4、對手臂的每一次運(yùn)動(dòng)，都設(shè)定上述四個(gè)點(diǎn)：初始點(diǎn)，提升、對手臂的每一次運(yùn)動(dòng)，都設(shè)定上述四個(gè)點(diǎn)：初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn)。點(diǎn)，下放點(diǎn)和終止點(diǎn)。145、位置約束、位置約束 (a) 初始點(diǎn)：給定速度和加速度初始點(diǎn)：給定速度和加速度(一般為零一般為零)； (b) 提升點(diǎn)：中間點(diǎn)運(yùn)動(dòng)的連續(xù)；提升點(diǎn)：中間點(diǎn)運(yùn)動(dòng)的連續(xù)； (c) 下放點(diǎn)：同提升點(diǎn)；下放點(diǎn)：同提升點(diǎn)； (d) 終止點(diǎn)：給定速度和加速度終止點(diǎn)：給定速度和加速度(一般為零一般為零)。156、除上述約束外，所有關(guān)節(jié)軌跡的極值不能超出每個(gè)關(guān)節(jié)、除上述約束外，所有關(guān)節(jié)軌跡的極值不能超出每個(gè)關(guān)節(jié)變量的物理和

25、幾何極限。變量的物理和幾何極限。7、時(shí)間的考慮、時(shí)間的考慮 (a) 軌跡的初始段和終止段：時(shí)間由手接近和離開支承表面軌跡的初始段和終止段：時(shí)間由手接近和離開支承表面的速率決定；也是由關(guān)節(jié)電機(jī)特性決定的某個(gè)常數(shù)。的速率決定；也是由關(guān)節(jié)電機(jī)特性決定的某個(gè)常數(shù)。 (b) 軌跡的中間點(diǎn)或中間段：時(shí)間由各關(guān)節(jié)的最大速度和加軌跡的中間點(diǎn)或中間段：時(shí)間由各關(guān)節(jié)的最大速度和加速度決定，將使用這些時(shí)間中的一個(gè)最長時(shí)間速度決定，將使用這些時(shí)間中的一個(gè)最長時(shí)間(即用最低速即用最低速關(guān)節(jié)確定的最長時(shí)間來歸一化關(guān)節(jié)確定的最長時(shí)間來歸一化)。 在關(guān)節(jié)軌跡的典型約束條件之下，我們所要研究的是選擇在關(guān)節(jié)軌跡的典型約束條件之下

26、，我們所要研究的是選擇一種一種 n 次次(或小于或小于 n 次次)的多項(xiàng)式函數(shù)，使得在各結(jié)點(diǎn)的多項(xiàng)式函數(shù)，使得在各結(jié)點(diǎn)(初始點(diǎn)，初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn)提升點(diǎn)，下放點(diǎn)和終止點(diǎn))上滿足對位置、速度和加速度的要上滿足對位置、速度和加速度的要求，并使關(guān)節(jié)位置、速度和加速度在整個(gè)時(shí)間間隔求，并使關(guān)節(jié)位置、速度和加速度在整個(gè)時(shí)間間隔 t0, tf 中中保持連續(xù)。保持連續(xù)。16 1. 位置位置(給定給定) 2速度速度(給定，通常為零給定，通常為零) 3. 加速度加速度(給定，通常為零給定，通常為零) 4. 提升點(diǎn)位置提升點(diǎn)位置(給定給定) 5提升點(diǎn)位置（與前一段軌跡連續(xù)）提升點(diǎn)位置（與前一段軌跡連續(xù)

27、） 6. 速度（與前一段軌跡連續(xù)）速度（與前一段軌跡連續(xù)） 7加速度（與前一段軌跡連續(xù)）加速度（與前一段軌跡連續(xù)） 8. 下放點(diǎn)位置下放點(diǎn)位置(給定給定) 9下放點(diǎn)位置（與前一段軌跡連續(xù)）下放點(diǎn)位置（與前一段軌跡連續(xù)） 10. 速度（與前一段軌跡連續(xù)）速度（與前一段軌跡連續(xù)） 11. 加速度（與前一段軌跡連續(xù)）加速度（與前一段軌跡連續(xù)） 12. 位置位置(給定給定) 13. 速度速度(給定，通常為零給定，通常為零) 14. 加速度加速度(給定，通常為零給定，通常為零)終止位置終止位置中間位置中間位置初始位置初始位置 規(guī)劃關(guān)節(jié)插值軌跡的約束條件：規(guī)劃關(guān)節(jié)插值軌跡的約束條件：17一種方法是為每個(gè)關(guān)

28、節(jié)規(guī)定一個(gè)七次多項(xiàng)式函數(shù)一種方法是為每個(gè)關(guān)節(jié)規(guī)定一個(gè)七次多項(xiàng)式函數(shù)式中，未知系數(shù)式中，未知系數(shù) aj 可由已知的位置和連續(xù)條件確定。但用這種可由已知的位置和連續(xù)條件確定。但用這種高次多項(xiàng)式內(nèi)插給定的結(jié)點(diǎn)也許不能令人滿意，因?yàn)樗臉O值高次多項(xiàng)式內(nèi)插給定的結(jié)點(diǎn)也許不能令人滿意，因?yàn)樗臉O值難求，而且容易產(chǎn)生額外的運(yùn)動(dòng)。難求，而且容易產(chǎn)生額外的運(yùn)動(dòng)。 另一種辦法是將整個(gè)關(guān)節(jié)空間軌跡分割成幾段，在每段軌跡另一種辦法是將整個(gè)關(guān)節(jié)空間軌跡分割成幾段，在每段軌跡中用不同的低次多項(xiàng)式來插值。有幾種分割軌跡的方法，每種中用不同的低次多項(xiàng)式來插值。有幾種分割軌跡的方法，每種方法的特性各不相同。方法的特性各不相同。

29、 434 軌跡軌跡 每個(gè)關(guān)節(jié)有下面三段軌跡：第一段由初始點(diǎn)到提升點(diǎn)的軌每個(gè)關(guān)節(jié)有下面三段軌跡：第一段由初始點(diǎn)到提升點(diǎn)的軌跡用四次多項(xiàng)式表示。第二段跡用四次多項(xiàng)式表示。第二段(或中間段）由提升點(diǎn)到下放或中間段）由提升點(diǎn)到下放點(diǎn)的軌跡用三次多項(xiàng)式表示。最后一段由下放點(diǎn)到終止點(diǎn)的點(diǎn)的軌跡用三次多項(xiàng)式表示。最后一段由下放點(diǎn)到終止點(diǎn)的軌跡由四次多項(xiàng)式表示。軌跡由四次多項(xiàng)式表示。18 353 軌跡軌跡 與與434軌跡相同，但每段所用多項(xiàng)式次數(shù)與前種不同。軌跡相同，但每段所用多項(xiàng)式次數(shù)與前種不同。第一段用三次多項(xiàng)式，第二段用五次多項(xiàng)式，最后一段用三第一段用三次多項(xiàng)式，第二段用五次多項(xiàng)式，最后一段用三次多項(xiàng)式

30、。次多項(xiàng)式。 (33333) 軌跡軌跡 對五段軌跡都使用三次多項(xiàng)式樣條函數(shù)。對五段軌跡都使用三次多項(xiàng)式樣條函數(shù)。 注意，上述討論對每個(gè)關(guān)節(jié)軌跡都是有效的，即每個(gè)關(guān)節(jié)注意，上述討論對每個(gè)關(guān)節(jié)軌跡都是有效的，即每個(gè)關(guān)節(jié)軌跡可分割成三段或五段。軌跡可分割成三段或五段。7.3.1 434 關(guān)節(jié)軌跡的計(jì)算關(guān)節(jié)軌跡的計(jì)算 對于對于N個(gè)關(guān)節(jié)個(gè)關(guān)節(jié), 在每段軌跡規(guī)劃中就要確定在每段軌跡規(guī)劃中就要確定 N 條關(guān)節(jié)軌跡，條關(guān)節(jié)軌跡，引用歸一化時(shí)間變量引用歸一化時(shí)間變量 是方便的，它使我們能用同樣的方是方便的，它使我們能用同樣的方法處理每個(gè)關(guān)節(jié)每段軌跡的方程。時(shí)間變化范圍均由法處理每個(gè)關(guān)節(jié)每段軌跡的方程。時(shí)間變化范

31、圍均由 (各各段軌跡的初始時(shí)間段軌跡的初始時(shí)間)變到變到 (各段軌跡的終止時(shí)間各段軌跡的終止時(shí)間)。0,1t0t 1t 19定義下列變量：定義下列變量： 軌跡是由多項(xiàng)式序列軌跡是由多項(xiàng)式序列hi(t)構(gòu)成的，這些多項(xiàng)式合起來形成關(guān)構(gòu)成的，這些多項(xiàng)式合起來形成關(guān)節(jié)節(jié) j 的軌跡。在每段軌跡中的軌跡。在每段軌跡中關(guān)節(jié)變量關(guān)節(jié)變量的多項(xiàng)式用歸一化時(shí)間表的多項(xiàng)式用歸一化時(shí)間表示為示為:20這些關(guān)節(jié)軌跡分段多項(xiàng)式所應(yīng)滿足的邊界條件為這些關(guān)節(jié)軌跡分段多項(xiàng)式所應(yīng)滿足的邊界條件為:21434關(guān)節(jié)軌跡的邊界條件如圖示關(guān)節(jié)軌跡的邊界條件如圖示。22這些多項(xiàng)式對實(shí)際時(shí)間這些多項(xiàng)式對實(shí)際時(shí)間 t 的一階和二階導(dǎo)數(shù)。可

32、寫成的一階和二階導(dǎo)數(shù)?？蓪懗傻谝欢诬壽E的基本多項(xiàng)式是四次的第一段軌跡的基本多項(xiàng)式是四次的23對于對于t =0(這段軌跡的初始位置這段軌跡的初始位置)，要滿足此位置的邊界條件，則，要滿足此位置的邊界條件，則24用求出的這些未知量得到用求出的這些未知量得到對于對于t=1(這段軌跡的終點(diǎn)這段軌跡的終點(diǎn))，在此位置，我們放寬插值多項(xiàng)式，在此位置，我們放寬插值多項(xiàng)式必須準(zhǔn)確通過該點(diǎn)的要求，僅要求在此位置的速度和加速度必必須準(zhǔn)確通過該點(diǎn)的要求，僅要求在此位置的速度和加速度必須和下一段軌跡起點(diǎn)的速度和加速度連續(xù)。此處的速度和加速須和下一段軌跡起點(diǎn)的速度和加速度連續(xù)。此處的速度和加速度為度為第二段軌跡的基本多

33、項(xiàng)式是三次的第二段軌跡的基本多項(xiàng)式是三次的25對于對于t=0(提升點(diǎn)提升點(diǎn))，此點(diǎn)的速度和加速度分別為，此點(diǎn)的速度和加速度分別為由于此點(diǎn)的速度相加速度必須分別和前一段軌跡終點(diǎn)的速度和由于此點(diǎn)的速度相加速度必須分別和前一段軌跡終點(diǎn)的速度和加速度連續(xù)，故可得加速度連續(xù)，故可得26或或或或 對于對于t=1(下放點(diǎn)下放點(diǎn))，該點(diǎn)的速度相加速度必須與下一段軌跡，該點(diǎn)的速度相加速度必須與下一段軌跡起點(diǎn)處的速度和加速度連續(xù)?？煞謩e求得此位置的速度和加速起點(diǎn)處的速度和加速度連續(xù)?？煞謩e求得此位置的速度和加速度為度為27末段軌跡的基本多項(xiàng)式是四次的末段軌跡的基本多項(xiàng)式是四次的如果在上式中以如果在上式中以 代替代

34、替 t ，我們就把歸一化時(shí)間，我們就把歸一化時(shí)間 t 由由 移至移至 。1tt 0,1t1,0t 28可得可得對于對于 (這段軌跡的終點(diǎn)這段軌跡的終點(diǎn))，要滿足軌跡終點(diǎn)的邊界條件，即，要滿足軌跡終點(diǎn)的邊界條件，即0t 對于對于 (這段軌跡的起點(diǎn)這段軌跡的起點(diǎn))，要滿足軌跡起點(diǎn)的邊界條件，即，要滿足軌跡起點(diǎn)的邊界條件，即1t 29在此下放點(diǎn)的速度和加速度連續(xù)性條件是：在此下放點(diǎn)的速度和加速度連續(xù)性條件是：或或30可以求出相連軌跡段間關(guān)節(jié)角之差為可以求出相連軌跡段間關(guān)節(jié)角之差為 軌跡多項(xiàng)式其余七個(gè)未知系數(shù)可由以上的速度、加速度連續(xù)軌跡多項(xiàng)式其余七個(gè)未知系數(shù)可由以上的速度、加速度連續(xù)約束條件的聯(lián)立方

35、程解出，用矩陣矢量符號改寫這些方程，可約束條件的聯(lián)立方程解出，用矩陣矢量符號改寫這些方程，可得得其中其中31或或C 矩陣的結(jié)構(gòu)便于計(jì)算未知系數(shù)，若時(shí)間間隔矩陣的結(jié)構(gòu)便于計(jì)算未知系數(shù)，若時(shí)間間隔 ti 為正值，為正值， C 的逆矩陣總是存在的。則關(guān)節(jié)各段關(guān)節(jié)軌跡多項(xiàng)的逆矩陣總是存在的。則關(guān)節(jié)各段關(guān)節(jié)軌跡多項(xiàng)式系數(shù)即可全部求得。式系數(shù)即可全部求得。(1,2, )in32同樣，我們可用此方法可計(jì)算同樣，我們可用此方法可計(jì)算353 關(guān)節(jié)軌跡。關(guān)節(jié)軌跡。 對于末段軌跡，把歸一化時(shí)間對于末段軌跡，把歸一化時(shí)間 t 由由-1，0重新變回重新變回0, 1，可求出可求出 hn(t)334-3-4 關(guān)節(jié)軌跡多項(xiàng)式

36、關(guān)節(jié)軌跡多項(xiàng)式34353-5-3 關(guān)節(jié)軌跡多項(xiàng)式關(guān)節(jié)軌跡多項(xiàng)式367.3.2 三三次樣條軌跡（五段三次多項(xiàng)式）次樣條軌跡（五段三次多項(xiàng)式） 用一組三次多項(xiàng)式插補(bǔ)給定的函數(shù)并保證插補(bǔ)點(diǎn)處一階和二用一組三次多項(xiàng)式插補(bǔ)給定的函數(shù)并保證插補(bǔ)點(diǎn)處一階和二階導(dǎo)數(shù)連續(xù)的方法，稱為三次樣條函數(shù)法。所能達(dá)到的近似程階導(dǎo)數(shù)連續(xù)的方法，稱為三次樣條函數(shù)法。所能達(dá)到的近似程度和光滑程度是相當(dāng)好的。度和光滑程度是相當(dāng)好的。 一般來說，樣條曲線是在插值點(diǎn)具有一般來說，樣條曲線是在插值點(diǎn)具有 k-1 階導(dǎo)數(shù)連續(xù)性的階導(dǎo)數(shù)連續(xù)性的 k次多項(xiàng)式。對于三次樣條函數(shù)，一階導(dǎo)數(shù)代表速度的連續(xù)性，次多項(xiàng)式。對于三次樣條函數(shù)，一階導(dǎo)數(shù)代

37、表速度的連續(xù)性，二階導(dǎo)數(shù)代表加速度的連續(xù)性。二階導(dǎo)數(shù)代表加速度的連續(xù)性。 三次樣條函數(shù)有某些優(yōu)點(diǎn)。首先，它是使速度和加速度連續(xù)三次樣條函數(shù)有某些優(yōu)點(diǎn)。首先，它是使速度和加速度連續(xù)的最低次多項(xiàng)式函數(shù)。其次，低次多項(xiàng)式減少了計(jì)算量和數(shù)值的最低次多項(xiàng)式函數(shù)。其次，低次多項(xiàng)式減少了計(jì)算量和數(shù)值不穩(wěn)定的可能性。不穩(wěn)定的可能性。每段關(guān)節(jié)軌跡的五段三次多項(xiàng)式的通式為每段關(guān)節(jié)軌跡的五段三次多項(xiàng)式的通式為其中其中37 在在應(yīng)用五段三次多項(xiàng)式插值時(shí)，需要有五段軌跡和六個(gè)插值應(yīng)用五段三次多項(xiàng)式插值時(shí)，需要有五段軌跡和六個(gè)插值點(diǎn)。點(diǎn)。但是，在前面的討論中只有四個(gè)插值點(diǎn)，即初始點(diǎn)、提升但是，在前面的討論中只有四個(gè)插值點(diǎn)

38、，即初始點(diǎn)、提升點(diǎn)、下放點(diǎn)和終止點(diǎn)。所以，必須選擇另外兩個(gè)插值點(diǎn)，以便點(diǎn)、下放點(diǎn)和終止點(diǎn)。所以，必須選擇另外兩個(gè)插值點(diǎn)，以便有足夠多的邊界條件求解各多項(xiàng)式系數(shù)?？梢栽谔嵘c(diǎn)和下放有足夠多的邊界條件求解各多項(xiàng)式系數(shù)。可以在提升點(diǎn)和下放點(diǎn)之間選取這兩個(gè)額外的結(jié)點(diǎn)。點(diǎn)之間選取這兩個(gè)額外的結(jié)點(diǎn)。 沒有必要知道這兩個(gè)點(diǎn)的確切位置，只要知道時(shí)間間隔，以沒有必要知道這兩個(gè)點(diǎn)的確切位置，只要知道時(shí)間間隔，以及必須滿足這兩點(diǎn)速度和加速度的連續(xù)性條件。因此，這組關(guān)及必須滿足這兩點(diǎn)速度和加速度的連續(xù)性條件。因此，這組關(guān)節(jié)軌跡分段多項(xiàng)式必須滿足的邊界條件是：節(jié)軌跡分段多項(xiàng)式必須滿足的邊界條件是： (1) 在初始點(diǎn)、提

39、升點(diǎn)、下放點(diǎn)和終止點(diǎn)的位置約束；在初始點(diǎn)、提升點(diǎn)、下放點(diǎn)和終止點(diǎn)的位置約束； (2) 在所有插值點(diǎn)的速度和加速度的連續(xù)性。在所有插值點(diǎn)的速度和加速度的連續(xù)性。38 五段三次關(guān)節(jié)軌跡的邊界條件示于中。其中有下劃線的變量五段三次關(guān)節(jié)軌跡的邊界條件示于中。其中有下劃線的變量是在計(jì)算五段三次多項(xiàng)式之前的已知量。是在計(jì)算五段三次多項(xiàng)式之前的已知量。39這些多項(xiàng)式對實(shí)際時(shí)間的一階和二階導(dǎo)數(shù)為：這些多項(xiàng)式對實(shí)際時(shí)間的一階和二階導(dǎo)數(shù)為：式中，式中，tj 是通過第是通過第 j 段軌跡所需的實(shí)際時(shí)間。給定了初始點(diǎn)和段軌跡所需的實(shí)際時(shí)間。給定了初始點(diǎn)和終止點(diǎn)的位置、速度和加速度，第一段軌跡和末段軌跡的多項(xiàng)終止點(diǎn)的位

40、置、速度和加速度，第一段軌跡和末段軌跡的多項(xiàng)式式 h1 (t) 和和hn (t) 就完全確定了。就完全確定了。旦算出這兩個(gè)多項(xiàng)式，就可旦算出這兩個(gè)多項(xiàng)式，就可用位置約束和連續(xù)條件求出用位置約束和連續(xù)條件求出h2 (t)、 h3 (t) 和和 h4 (t) 。第一段軌跡，基本多項(xiàng)式為第一段軌跡，基本多項(xiàng)式為當(dāng)當(dāng) t=0 時(shí)，由該位置的邊界條件，可得時(shí)，由該位置的邊界條件，可得(給定給定)40由此可得：由此可得：當(dāng)當(dāng) t=1 時(shí)，由此位置的邊界條件，可得：時(shí)，由此位置的邊界條件，可得：可得可得其中，其中，i= i i-1 。第一段軌跡的多項(xiàng)式就完全確定了。第一段軌跡的多項(xiàng)式就完全確定了。(給定給定

41、)41由上式可求出由上式可求出 t1時(shí)的速度和加速度：時(shí)的速度和加速度：此速度相加速度必須與下一段軌跡起點(diǎn)的速度和加速度連續(xù)。此速度相加速度必須與下一段軌跡起點(diǎn)的速度和加速度連續(xù)。末段軌跡的多項(xiàng)式為（仍做類似代換末段軌跡的多項(xiàng)式為（仍做類似代換 ）(給定給定)(給定給定)1tt 當(dāng)當(dāng)t0 和和t1時(shí)（對應(yīng)時(shí)（對應(yīng) 和和 ），由邊界條件，有），由邊界條件，有1t 0t 42由上述方程解出未知系數(shù)，最后得到：由上述方程解出未知系數(shù)，最后得到： 給定初始點(diǎn)、提升點(diǎn)、下放點(diǎn)和終止點(diǎn)的位置以及通過每段給定初始點(diǎn)、提升點(diǎn)、下放點(diǎn)和終止點(diǎn)的位置以及通過每段軌跡所需要的時(shí)間軌跡所需要的時(shí)間 (tj ) 后，五

42、段三次多項(xiàng)式即可后，五段三次多項(xiàng)式即可唯一地唯一地確定，確定，滿足所有位置約束和連續(xù)條件。滿足所有位置約束和連續(xù)條件。算出以上個(gè)多項(xiàng)式，就可用位置約束和連續(xù)條件求出算出以上個(gè)多項(xiàng)式，就可用位置約束和連續(xù)條件求出h2 (t)、 h3 (t) 和和 h4 (t) 。43 對于較復(fù)雜的機(jī)器人系統(tǒng)，為了控制操作機(jī)完成作業(yè)而設(shè)對于較復(fù)雜的機(jī)器人系統(tǒng)，為了控制操作機(jī)完成作業(yè)而設(shè)計(jì)了編程語言。在這種系統(tǒng)中，作業(yè)通常是用操作機(jī)手部計(jì)了編程語言。在這種系統(tǒng)中，作業(yè)通常是用操作機(jī)手部(或或末端執(zhí)行器末端執(zhí)行器)必須通過的笛卡爾結(jié)點(diǎn)序列給定的。因此，描述必須通過的笛卡爾結(jié)點(diǎn)序列給定的。因此，描述操作機(jī)在作業(yè)中的運(yùn)動(dòng)

43、時(shí)，我們就更關(guān)心描述操作機(jī)手部要達(dá)操作機(jī)在作業(yè)中的運(yùn)動(dòng)時(shí)，我們就更關(guān)心描述操作機(jī)手部要達(dá)到的目標(biāo)位置及通過的笛卡爾空間曲線到的目標(biāo)位置及通過的笛卡爾空間曲線(或路徑或路徑)的形式。的形式。7.4 7.4 笛卡爾路徑軌跡規(guī)劃笛卡爾路徑軌跡規(guī)劃 一般，實(shí)現(xiàn)笛卡爾路徑規(guī)劃可采用下述兩個(gè)相連的步驟：一般，實(shí)現(xiàn)笛卡爾路徑規(guī)劃可采用下述兩個(gè)相連的步驟：(1)沿笛卡爾路徑，按照某種規(guī)則以笛卡爾坐標(biāo)生成或選擇沿笛卡爾路徑，按照某種規(guī)則以笛卡爾坐標(biāo)生成或選擇一組結(jié)點(diǎn)或插值點(diǎn)；一組結(jié)點(diǎn)或插值點(diǎn)；(2)規(guī)定一種函數(shù)，按某些準(zhǔn)則連接這規(guī)定一種函數(shù)，按某些準(zhǔn)則連接這些結(jié)點(diǎn)些結(jié)點(diǎn)(或逼近分段的路徑或逼近分段的路徑)。 對

44、于第二步，所選用的準(zhǔn)則可以采用兩種控制算法，以保對于第二步，所選用的準(zhǔn)則可以采用兩種控制算法，以保證跟蹤給定的路徑。證跟蹤給定的路徑。44 (1) 面向笛卡爾空間的方法。在此方法中，大部分計(jì)算和面向笛卡爾空間的方法。在此方法中，大部分計(jì)算和優(yōu)化是以笛卡爾坐標(biāo)完成的，然后，在手部這一級上進(jìn)行控優(yōu)化是以笛卡爾坐標(biāo)完成的，然后，在手部這一級上進(jìn)行控制。按固定的取樣間隔在預(yù)定路徑上選擇伺服取樣點(diǎn)，在控制。按固定的取樣間隔在預(yù)定路徑上選擇伺服取樣點(diǎn)，在控制操作機(jī)時(shí)實(shí)時(shí)地把它們轉(zhuǎn)換為與之相應(yīng)的關(guān)節(jié)變量。所得制操作機(jī)時(shí)實(shí)時(shí)地把它們轉(zhuǎn)換為與之相應(yīng)的關(guān)節(jié)變量。所得到的軌跡是分段直線。到的軌跡是分段直線。 (2)

45、 面向關(guān)節(jié)空間的方法。這種方法用關(guān)節(jié)變量空間中的面向關(guān)節(jié)空間的方法。這種方法用關(guān)節(jié)變量空間中的低次多項(xiàng)式函數(shù)迫近直線路徑上的兩相鄰結(jié)點(diǎn)間的一段路徑，低次多項(xiàng)式函數(shù)迫近直線路徑上的兩相鄰結(jié)點(diǎn)間的一段路徑，而控制是在關(guān)節(jié)這一級上進(jìn)行的。所得到的笛卡爾路徑是不而控制是在關(guān)節(jié)這一級上進(jìn)行的。所得到的笛卡爾路徑是不分段的直線。分段的直線。 Paul 敘述了用一系列直線段構(gòu)成操作機(jī)手部笛卡爾路徑敘述了用一系列直線段構(gòu)成操作機(jī)手部笛卡爾路徑的設(shè)計(jì)方法。把手部在這些直線段中的速度和加速度轉(zhuǎn)換的設(shè)計(jì)方法。把手部在這些直線段中的速度和加速度轉(zhuǎn)換到關(guān)節(jié)坐標(biāo)，并用二次插值進(jìn)行平滑連接，從而實(shí)現(xiàn)了手到關(guān)節(jié)坐標(biāo)，并用二次

46、插值進(jìn)行平滑連接，從而實(shí)現(xiàn)了手部的規(guī)劃運(yùn)動(dòng)控制。我們將學(xué)習(xí)如何用該方法設(shè)計(jì)笛卡爾部的規(guī)劃運(yùn)動(dòng)控制。我們將學(xué)習(xí)如何用該方法設(shè)計(jì)笛卡爾直線路徑。直線路徑。45對于笛卡爾路徑控制，軌跡設(shè)定點(diǎn)算法為：對于笛卡爾路徑控制，軌跡設(shè)定點(diǎn)算法為：0tt循環(huán)：等待下一個(gè)控制間隔；循環(huán)：等待下一個(gè)控制間隔； 時(shí)刻操作機(jī)末端所處的位置時(shí)刻操作機(jī)末端所處的位置； 相應(yīng)于相應(yīng)于 的關(guān)節(jié)變量解的關(guān)節(jié)變量解； 若若 ，則退出循環(huán)；，則退出循環(huán)； 轉(zhuǎn)向循環(huán)。轉(zhuǎn)向循環(huán)。ttt ( )H ttftt( )Q H t( )H t 這里，除了需要計(jì)算每一控制間隔中操作機(jī)手部軌跡函數(shù)這里，除了需要計(jì)算每一控制間隔中操作機(jī)手部軌跡函數(shù)

47、外，還需把笛卡爾位置變換為相應(yīng)的關(guān)節(jié)變量外，還需把笛卡爾位置變換為相應(yīng)的關(guān)節(jié)變量 。矩陣函數(shù)矩陣函數(shù) 表示操作機(jī)手部在表示操作機(jī)手部在 時(shí)刻的預(yù)定位置?？捎脮r(shí)刻的預(yù)定位置?？捎?4齊次變換矩陣表示。齊次變換矩陣表示。( )H t( )Q H t( )H tt46 在可編程機(jī)器人系統(tǒng)中，預(yù)定的運(yùn)動(dòng)可用笛卡爾結(jié)點(diǎn)序列給在可編程機(jī)器人系統(tǒng)中，預(yù)定的運(yùn)動(dòng)可用笛卡爾結(jié)點(diǎn)序列給定，每個(gè)結(jié)點(diǎn)可用聯(lián)系操作機(jī)手部坐標(biāo)系和工作空間坐標(biāo)系的定，每個(gè)結(jié)點(diǎn)可用聯(lián)系操作機(jī)手部坐標(biāo)系和工作空間坐標(biāo)系的齊次變換來描述。相應(yīng)于這些笛卡爾結(jié)點(diǎn)的關(guān)節(jié)坐標(biāo)可用運(yùn)動(dòng)齊次變換來描述。相應(yīng)于這些笛卡爾結(jié)點(diǎn)的關(guān)節(jié)坐標(biāo)可用運(yùn)動(dòng)學(xué)逆問題求解程序

48、算出，并用二次多項(xiàng)式平滑關(guān)節(jié)坐標(biāo)中兩個(gè)學(xué)逆問題求解程序算出，并用二次多項(xiàng)式平滑關(guān)節(jié)坐標(biāo)中兩個(gè)相鄰的關(guān)節(jié)結(jié)點(diǎn)，以利于控制，從而，控制操作機(jī)手部沿連接相鄰的關(guān)節(jié)結(jié)點(diǎn)，以利于控制，從而，控制操作機(jī)手部沿連接這些結(jié)點(diǎn)的直線運(yùn)動(dòng)。這種方法的優(yōu)點(diǎn)是，它使我們能控制操這些結(jié)點(diǎn)的直線運(yùn)動(dòng)。這種方法的優(yōu)點(diǎn)是，它使我們能控制操作機(jī)手部跟蹤運(yùn)動(dòng)的物體。雖然目標(biāo)位置是用變換描述的，但作機(jī)手部跟蹤運(yùn)動(dòng)的物體。雖然目標(biāo)位置是用變換描述的，但它們并不規(guī)定操作機(jī)手部怎樣由一個(gè)變換運(yùn)動(dòng)到另一個(gè)變換。它們并不規(guī)定操作機(jī)手部怎樣由一個(gè)變換運(yùn)動(dòng)到另一個(gè)變換。 Paul 用一個(gè)用一個(gè)直線平移直線平移和和兩個(gè)轉(zhuǎn)動(dòng)兩個(gè)轉(zhuǎn)動(dòng)來完成兩相鄰笛卡

49、爾結(jié)點(diǎn)來完成兩相鄰笛卡爾結(jié)點(diǎn)間的運(yùn)動(dòng)。第一個(gè)轉(zhuǎn)動(dòng)是繞單位矢量間的運(yùn)動(dòng)。第一個(gè)轉(zhuǎn)動(dòng)是繞單位矢量 k 進(jìn)行的，把工具或末端進(jìn)行的，把工具或末端執(zhí)行器的軸線調(diào)整到預(yù)定的接近方向；而第二個(gè)轉(zhuǎn)動(dòng)則繞工具執(zhí)行器的軸線調(diào)整到預(yù)定的接近方向；而第二個(gè)轉(zhuǎn)動(dòng)則繞工具軸調(diào)整工具的方向。軸調(diào)整工具的方向。7.4.1 齊次變換矩陣法齊次變換矩陣法47 一般來說，操作機(jī)的目標(biāo)位置可用如下基本矩陣方程表示：一般來說，操作機(jī)的目標(biāo)位置可用如下基本矩陣方程表示：060base6toolbaseobjTT= C(t)P其中，其中，0T6 為描述操作機(jī)手部相對于機(jī)座為描述操作機(jī)手部相對于機(jī)座坐標(biāo)系位姿的齊次變換矩陣；坐標(biāo)系位姿的齊

50、次變換矩陣； 6Ttool 為描為描述工具相對于手部坐標(biāo)系位姿的齊次變述工具相對于手部坐標(biāo)系位姿的齊次變換矩陣，它描述了受控制的工具端點(diǎn)；換矩陣，它描述了受控制的工具端點(diǎn)； 0Cbase(t) 為描述物體位姿的工作坐標(biāo)系為描述物體位姿的工作坐標(biāo)系相對于機(jī)座坐標(biāo)系的齊次變換矩陣；相對于機(jī)座坐標(biāo)系的齊次變換矩陣； basePobj 為描述末端執(zhí)行器相對于工作坐為描述末端執(zhí)行器相對于工作坐標(biāo)系預(yù)定抓取物體的位姿的齊次變換矩標(biāo)系預(yù)定抓取物體的位姿的齊次變換矩陣。陣。 左邊的矩陣描述操作機(jī)抓取位姿，而右邊的矩陣描述了我左邊的矩陣描述操作機(jī)抓取位姿，而右邊的矩陣描述了我們要讓操作機(jī)工具抓握物體特征部位的位

51、姿。們要讓操作機(jī)工具抓握物體特征部位的位姿。0Cbase(t)B060base6toolbaseobjTT= C(t)P060base6toolbaseobjTT= C(t)T060base6toolbaseobjTT= C(t)Tobjtool48 我們可以解出我們可以解出0T6 ，它描述了以正確和預(yù)定的方式抓取物體，它描述了以正確和預(yù)定的方式抓取物體時(shí)操作機(jī)應(yīng)有的形態(tài)。時(shí)操作機(jī)應(yīng)有的形態(tài)。100base66baseobjtoolT = C(t)PT 如果計(jì)算如果計(jì)算0T6 的速度足夠高，并轉(zhuǎn)換成相應(yīng)的關(guān)節(jié)角，那么的速度足夠高，并轉(zhuǎn)換成相應(yīng)的關(guān)節(jié)角，那么就可控制操作機(jī)沿預(yù)定的軌跡運(yùn)動(dòng)就可控制

52、操作機(jī)沿預(yù)定的軌跡運(yùn)動(dòng) 利用前式，確定一項(xiàng)作業(yè)的利用前式，確定一項(xiàng)作業(yè)的 N個(gè)個(gè) 目標(biāo)位置的序列可表示為目標(biāo)位置的序列可表示為49由由Ci(t) Pi確定的位置，可求得相鄰點(diǎn)之間的距離。若進(jìn)一步確定的位置，可求得相鄰點(diǎn)之間的距離。若進(jìn)一步給定線速度和角速度，即可求得由位置給定線速度和角速度，即可求得由位置i 到到 i+1所需的時(shí)間。所需的時(shí)間。 以上位置的工具和運(yùn)動(dòng)坐標(biāo)系都是相對于機(jī)座坐標(biāo)系給定以上位置的工具和運(yùn)動(dòng)坐標(biāo)系都是相對于機(jī)座坐標(biāo)系給定的。從一個(gè)位置移向下一個(gè)位置最好是相對于目標(biāo)坐標(biāo)系給的。從一個(gè)位置移向下一個(gè)位置最好是相對于目標(biāo)坐標(biāo)系給定位置和工具。這樣，從運(yùn)動(dòng)定位置和工具。這樣，從

53、運(yùn)動(dòng)坐標(biāo)系觀察坐標(biāo)系觀察，工具是靜止的。，工具是靜止的。為了做到這一點(diǎn)，需要相對于后續(xù)的坐標(biāo)系重新定義當(dāng)前的為了做到這一點(diǎn)，需要相對于后續(xù)的坐標(biāo)系重新定義當(dāng)前的位置位置和工具。利用雙下標(biāo)符號和工具。利用雙下標(biāo)符號 Pij 重新定義重新定義Pi變換，就不難做變換，就不難做到這一點(diǎn)。到這一點(diǎn)。 Pij是在第是在第 j 個(gè)工作坐標(biāo)系個(gè)工作坐標(biāo)系中表示的位置中表示的位置Pi。則在位置。則在位置1，將它在目標(biāo)坐標(biāo)系將它在目標(biāo)坐標(biāo)系中表示，可中表示，可得得1111tool6TT = C (t)P2212tool6TT = C (t)P將它用位置將它用位置2時(shí)的坐標(biāo)系表示，則有時(shí)的坐標(biāo)系表示，則有50由以上

54、兩個(gè)方程即可求得由以上兩個(gè)方程即可求得1112211112()tooltoolP= C(t)C (t)PTT 上述方程的目的是耍在已知上述方程的目的是耍在已知 P11時(shí)時(shí) 求得求得P12 。因此，在任。因此，在任意二相鄰位置意二相鄰位置 I 和和 i+1 之間的運(yùn)動(dòng)有之間的運(yùn)動(dòng)有111,11,11( )( )()tooltooliiiii iiiPCt C t PTT其中其中Pi,i+11 和和Pi+1,i+1 為上面所討論過的變換。為上面所討論過的變換。 Paul 使用了一種簡單的方法，以控制操作機(jī)手部從一個(gè)使用了一種簡單的方法，以控制操作機(jī)手部從一個(gè)變換運(yùn)動(dòng)到另一個(gè)變換。此法包括一個(gè)平移運(yùn)

55、動(dòng)和一個(gè)繞空變換運(yùn)動(dòng)到另一個(gè)變換。此法包括一個(gè)平移運(yùn)動(dòng)和一個(gè)繞空間某定軸的轉(zhuǎn)動(dòng)以及另一個(gè)繞工具軸的轉(zhuǎn)動(dòng)，以便形成操作間某定軸的轉(zhuǎn)動(dòng)以及另一個(gè)繞工具軸的轉(zhuǎn)動(dòng)，以便形成操作機(jī)受控的線速度和角速度運(yùn)動(dòng)。第一個(gè)轉(zhuǎn)動(dòng)使工具軸與預(yù)定機(jī)受控的線速度和角速度運(yùn)動(dòng)。第一個(gè)轉(zhuǎn)動(dòng)使工具軸與預(yù)定的接近方向?qū)?zhǔn)，第二個(gè)轉(zhuǎn)動(dòng)使工具軸與方向矢量對準(zhǔn)。的接近方向?qū)?zhǔn)，第二個(gè)轉(zhuǎn)動(dòng)使工具軸與方向矢量對準(zhǔn)。51 由位置由位置 i 到到 i+1的運(yùn)動(dòng)可用的運(yùn)動(dòng)可用“驅(qū)動(dòng)驅(qū)動(dòng)”變換變換D()表示，它是歸表示，它是歸一化時(shí)間一化時(shí)間 的函數(shù)的函數(shù)161,11( )( )( )()toolii iiTCPDT其中，其中，1,1,1(1)i

56、ii iPPD1,11,1(1)()i iiiDPP 可得可得 于是有于是有52用位置用位置 i 和和 i+1各自的齊次變換矩陣表示它們，可得各自的齊次變換矩陣表示它們，可得利用前式，求利用前式，求 Pi,i+1的逆并乘以的逆并乘以Pi+1,i+1 ，得到，得到53 若驅(qū)動(dòng)函數(shù)由一個(gè)平移運(yùn)動(dòng)和兩個(gè)旋轉(zhuǎn)運(yùn)動(dòng)構(gòu)成，那么這些若驅(qū)動(dòng)函數(shù)由一個(gè)平移運(yùn)動(dòng)和兩個(gè)旋轉(zhuǎn)運(yùn)動(dòng)構(gòu)成，那么這些平移和轉(zhuǎn)動(dòng)量將直接同平移和轉(zhuǎn)動(dòng)量將直接同成正比。若成正比。若隨時(shí)間線性地變化，則隨時(shí)間線性地變化，則 D() 所代表的合成運(yùn)動(dòng)將相應(yīng)于一個(gè)不變的線速度和兩個(gè)角速所代表的合成運(yùn)動(dòng)將相應(yīng)于一個(gè)不變的線速度和兩個(gè)角速度。度。 平移運(yùn)動(dòng)可由齊次變換矩陣平移運(yùn)動(dòng)可由齊次變換矩陣L()表示；第一個(gè)轉(zhuǎn)動(dòng)可用齊次表示；第一個(gè)轉(zhuǎn)動(dòng)可用齊次變換矩陣變換矩陣RA()表示，它的作用是把表示，它的作用是把Pi位置的接近矢量轉(zhuǎn)向位置的接近矢量轉(zhuǎn)向Pi+1位置的接近矢量；第二個(gè)轉(zhuǎn)動(dòng)可用位置的接近矢量；第二個(gè)轉(zhuǎn)動(dòng)可用RB()表示，它的作用是把表示，它的作用是把Pi 位置的方向矢量繞工具軸轉(zhuǎn)向位置的方向矢量繞工具軸轉(zhuǎn)向Pi+1位置的方向矢量。因此，驅(qū)位置的方向矢量。因此，驅(qū)動(dòng)函數(shù)可表示為動(dòng)函數(shù)可表示為 54其中，其中， 旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣RA() 表示繞表示繞 Pi 的接近矢量（的接近矢量（zi 軸軸）轉(zhuǎn)動(dòng)了）轉(zhuǎn)