1、1,第五章 機(jī)器人的軌跡規(guī)劃,5.1 機(jī)器人規(guī)劃的定義和作用,5.1.1 概述,所謂機(jī)器人的規(guī)劃(P1anning)，指的是機(jī)器人根據(jù)自身的任務(wù)，求得完成這一任務(wù)的解決方案的過程。這里所說的任務(wù)，具有廣義的概念，既可以指機(jī)器人要完成的某一具體任務(wù)，也可以是機(jī)器人的某個(gè)動(dòng)作，比如手部或關(guān)節(jié)的某個(gè)規(guī)定的運(yùn)動(dòng)等。,2,為說明機(jī)器人規(guī)劃的概念，我們舉下面的例子： 在一些老齡化比較嚴(yán)重的國家，開發(fā)了各種各樣的機(jī)器人專門用于伺候老人，這些機(jī)器人有不少是采用聲控的方式比如主人用聲音命令機(jī)器人“給我倒一杯開水”，我們先不考慮機(jī)器人是如何識(shí)別人的自然語言，而是著重分析一下機(jī)器人在得到這樣一個(gè)命今后，如何來完成主

2、人交給的任務(wù)。,3,首先，機(jī)器人應(yīng)該把任務(wù)進(jìn)行分解，把主人交代的任務(wù)分解成為“取一個(gè)杯子”、“找到水壺”、“打開瓶塞”、“把水倒人杯中”、“把水送給主人”等一系列子任務(wù)。這一層次的規(guī)劃稱為任務(wù)規(guī)劃(Task planning)，它完成總體任務(wù)的分解。 然后再針對(duì)每一個(gè)子任務(wù)進(jìn)行進(jìn)一步的規(guī)劃。以“把水倒入杯中”這一子任務(wù)為例，可以進(jìn)一步分解成為“把水壺提到杯口上方”、“把水壺傾斜倒水入杯”、“把水壺豎直“、“把水壺放回原處”等一系列動(dòng)作，這一層次的規(guī)劃稱為動(dòng)作規(guī)劃(Motion P1anning)，它把實(shí)現(xiàn)每一個(gè)子任務(wù)的過程分解為一系列具體的動(dòng)作。 為了實(shí)現(xiàn)每一個(gè)動(dòng)作，需要對(duì)手部的運(yùn)動(dòng)軌跡進(jìn)行必

3、要的規(guī)定，這是手部軌跡規(guī)劃(Hand trajectory planning )。 為了使手部實(shí)現(xiàn)預(yù)定的運(yùn)動(dòng)，就要知道各關(guān)節(jié)的運(yùn)動(dòng)規(guī)律，這是關(guān)節(jié)軌跡規(guī)劃(Joint trajectory planning)。 最后才是關(guān)節(jié)的運(yùn)動(dòng)控制(Motion control)。,4,上述例子可以看出，機(jī)器人的規(guī)劃是分層次的，從高層的任務(wù)規(guī)劃，動(dòng)作規(guī)劃到手部軌跡規(guī)劃和關(guān)節(jié)軌跡規(guī)劃，最后才是底層的控制(見圖)。在上述例子中，我們沒有討論力的問題，實(shí)際上，對(duì)有些機(jī)器人來說，力的大小也是要控制的，這時(shí)，除了手部或關(guān)節(jié)的軌跡規(guī)劃，還要進(jìn)行手部和關(guān)節(jié)輸出力的規(guī)劃。 智能化程度越高，規(guī)劃的層次越多，操作就越簡單。 對(duì)工

4、業(yè)機(jī)器人來說，高層的任務(wù)規(guī)劃和動(dòng)作規(guī)劃一般是依賴人來完成的。而且一般的工業(yè)機(jī)器人也不具備力的反饋，所以，工業(yè)機(jī)器人通常只具有軌跡規(guī)劃的和底層的控制功能。,5,取一個(gè)杯子,找到水壺,打開水壺,把水倒入杯中,把水送給主人,給主人倒一杯水,提起水壺到杯口上方,把水壺傾斜,把水壺豎直,把水壺放回原處,手部從A點(diǎn)移到B點(diǎn),關(guān)節(jié)從A點(diǎn)移到B點(diǎn),圖 智能機(jī)器人的規(guī)劃層次,6,許多規(guī)劃所包的步驟是含糊的，而且需要進(jìn)一步說明（子規(guī)劃）。大多數(shù)規(guī)劃具有很大的子規(guī)劃結(jié)構(gòu)，規(guī)劃中的每個(gè)目標(biāo)可以由達(dá)到此目標(biāo)的比較詳細(xì)的子規(guī)劃所代替。因此，最終得到的規(guī)劃是某個(gè)問題求解算符的線性或分部排序，但是由算符來實(shí)現(xiàn)的目標(biāo)常常具有分

5、層結(jié)構(gòu)。 把某些比較復(fù)雜的問題分解為一些比較小的問題的想法使我們應(yīng)用規(guī)劃方法求解問題在實(shí)際上成為可能。 有兩條能夠?qū)崿F(xiàn)這種分解的重要途徑：第一條是當(dāng)從一個(gè)問題狀態(tài)移動(dòng)到下一個(gè)狀態(tài)時(shí)，無需計(jì)算整個(gè)新的狀態(tài)，而只要考慮狀態(tài)中可能變化了的那些部分。第二條是把單一的困難問題分割為幾個(gè)有希望的、較為容易解決的子問題，這種分解能夠使困難問題的求解變得容易些。,7,機(jī)器人規(guī)劃分為高層規(guī)劃和低層規(guī)劃。自動(dòng)規(guī)劃在機(jī)器人規(guī)劃中稱為高層規(guī)劃。在無特別說明時(shí)，機(jī)器人規(guī)劃都是指自動(dòng)規(guī)劃。自動(dòng)規(guī)劃是一種重要的問題求解技術(shù)，它從某個(gè)特定的問題狀態(tài)出發(fā)，尋求一系列行為動(dòng)作，并建立一個(gè)操作序列，直到求得目標(biāo)狀態(tài)為止。與一般問題

6、求解相比，自動(dòng)規(guī)劃更注重于問題的求解過程，而不是求解結(jié)果。 我們?cè)陉U述機(jī)器人自動(dòng)規(guī)劃問題時(shí)，機(jī)器人一般配備有傳感器和一組能在某個(gè)易于理解的現(xiàn)場中完成的基本動(dòng)作。這些動(dòng)作可把該現(xiàn)場從一種狀態(tài)或布局變換為另一種狀態(tài)或布局。,8,機(jī)器人規(guī)劃是機(jī)器人學(xué)的一個(gè)重要研究領(lǐng)域，也是人工智能與機(jī)器人學(xué)一個(gè)令人感興趣的結(jié)合點(diǎn)。,機(jī)器人學(xué)中的一個(gè)基本問題是為解決某個(gè)預(yù)定的任務(wù)而規(guī)劃機(jī)器人的動(dòng)作，然后在機(jī)器人執(zhí)行完成那些動(dòng)作所需的命令時(shí)控制它。這里，規(guī)劃的意思就是機(jī)器人在行動(dòng)前確定一系列動(dòng)作(作決策)，這種動(dòng)作的確定可用問題求解系統(tǒng)來解決，給定初始情況后，該系統(tǒng)可達(dá)到某一規(guī)定的目標(biāo)。因此，規(guī)劃就是指機(jī)器人為達(dá)到目標(biāo)

7、而需要的行動(dòng)過程的描述。 規(guī)劃內(nèi)容可能沒有次序，但是一般來說，規(guī)劃具有某個(gè)規(guī)劃目標(biāo)的蘊(yùn)含排序。例如，早晨起床后的安排。 缺乏規(guī)劃可能導(dǎo)致不是最佳的問題求解，甚至得不到問題的求解。,9,機(jī)器人的規(guī)劃程序只需要給定任務(wù)初始狀態(tài)和最終狀態(tài)的描述。這些規(guī)劃系統(tǒng)一般都不說明為實(shí)現(xiàn)一個(gè)算符所需的詳細(xì)的機(jī)器人運(yùn)動(dòng)。任務(wù)規(guī)劃程序則把任務(wù)級(jí)的說明變換成操作機(jī)級(jí)的說明。為了進(jìn)行這種變換，任務(wù)規(guī)劃程序必須包含被操作物體、任務(wù)環(huán)境、機(jī)器人執(zhí)行的任務(wù)、環(huán)境的初始狀態(tài)和所要求的最終(目標(biāo))狀態(tài)等描述。任務(wù)規(guī)劃程序的輸出就是一個(gè)機(jī)器人程序，在指定的初始狀態(tài)下執(zhí)行該程序后，就能達(dá)到所要求的最終狀態(tài)。 任務(wù)規(guī)劃有三個(gè)階段：建立

8、模型、任務(wù)說明和操作機(jī)程序綜合。任務(wù)的世界模型應(yīng)含有如下的信息:(1)任務(wù)環(huán)境中的所有物體和機(jī)器人的幾何描述；(2)所有物體的物理描述；(3)所有連接件的運(yùn)動(dòng)學(xué)描述，(4) 機(jī)器人和傳感器特性的描述。在世界模型中，任務(wù)狀態(tài)模型還必包括全部物體和連接件的布局。,5.1.2 機(jī)器人任務(wù)規(guī)劃的作用,10,軌跡規(guī)劃的目的是將操作人員輸入的簡單的任務(wù)描述變?yōu)樵敿?xì)的運(yùn)動(dòng)軌跡描述。 例如，對(duì)一般的工業(yè)機(jī)器人來說，操作員可能只輸入機(jī)械手末端的目標(biāo)位置和方位，而規(guī)劃的任務(wù)便是要確定出達(dá)到目標(biāo)的關(guān)節(jié)軌跡的形狀、運(yùn)動(dòng)的時(shí)間和速度等。這里所說的軌跡是指隨時(shí)間變化的位置、速度和加速度。,11,簡言之，機(jī)器人的工作過程，

9、就是通過規(guī)劃，將要求的任務(wù)變?yōu)槠谕倪\(yùn)動(dòng)和力，由控制環(huán)節(jié)根據(jù)期望的運(yùn)動(dòng)和力的信號(hào)，產(chǎn)生相應(yīng)的控制作用，以使機(jī)器人輸出實(shí)際的運(yùn)動(dòng)和力，從而完成期望的任務(wù)。這一過程表述如下圖所示。這里，機(jī)器人實(shí)際運(yùn)動(dòng)的情況通常還要反饋給規(guī)劃級(jí)和控制級(jí)，以便對(duì)規(guī)劃和控制的結(jié)果做出適當(dāng)?shù)男拚?圖 機(jī)器人的工作原理示意圖,12,上圖中，要求的任務(wù)由操作人員輸入給機(jī)器人，為了使機(jī)器人操作方便、使用簡單，必須允許操作人員給出盡量簡單的描述。 上圖中，期望的運(yùn)動(dòng)和力是進(jìn)行機(jī)器人控制所必需的輸入量，它們是機(jī)械手末端在每一個(gè)時(shí)刻的位姿和速度，對(duì)于絕大多數(shù)情況，還要求給出每一時(shí)刻期望的關(guān)節(jié)位移和速度，有些控制方法還要求給出期望的

10、加速度等。,13,對(duì)于PTP控制: 通常只給出機(jī)械手末端的起點(diǎn)和終點(diǎn)，有時(shí)也給出一些中間經(jīng)過點(diǎn)，所有這些點(diǎn)統(tǒng)稱為路徑點(diǎn)。應(yīng)注意這里所說的“點(diǎn)” 不僅包括機(jī)械手末端的位置，而且包括方位，因此描述一個(gè)點(diǎn)通常需要6個(gè)量。通常希望機(jī)械手末端的運(yùn)動(dòng)是光滑的，即它具有連續(xù)的一階導(dǎo)數(shù)，有時(shí)甚至要求具有連續(xù)的二階導(dǎo)數(shù)。不平滑的運(yùn)動(dòng)容易造成機(jī)構(gòu)的磨損和破壞，甚至可能激發(fā)機(jī)械手的振動(dòng)。因此規(guī)劃的任務(wù)便是要根據(jù)給定的路徑點(diǎn)規(guī)劃出通過這些點(diǎn)的光滑的運(yùn)動(dòng)軌跡。,14,對(duì)于CP控制: 機(jī)械手末端的運(yùn)動(dòng)軌跡是根據(jù)任務(wù)的需要給定的，但是它也必須按照一定的采樣間隔，通過逆運(yùn)動(dòng)學(xué)計(jì)算，將其變換到關(guān)節(jié)空間，然后在關(guān)節(jié)空間中尋找光滑

11、函數(shù)來擬合這些離散點(diǎn)最后，還有在機(jī)器人的計(jì)算機(jī)內(nèi)部如何表示軌跡，以及如何實(shí)時(shí)地生成軌跡的問題。,15,機(jī)器人軌跡規(guī)劃屬于機(jī)器人低層規(guī)劃，基本上不涉及人工智能問題，而是在機(jī)械手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的基礎(chǔ)上，討論機(jī)器人運(yùn)動(dòng)的規(guī)劃及其方法。所謂軌跡，就是指機(jī)器人在運(yùn)動(dòng)過程中的位移、速度和加速度。,軌跡規(guī)劃問題通常是將軌跡規(guī)劃器看成“黑箱”，接受表示路徑約束的輸入變量，輸出為起點(diǎn)和終點(diǎn)之間按時(shí)間排列的操作機(jī)中間形態(tài)（位姿, 速度和加速度）序列。,5.2 機(jī)器人軌跡規(guī)劃的一般性問題,由初始點(diǎn)運(yùn)動(dòng)到終止點(diǎn)，所經(jīng)過的由中間形態(tài)序列構(gòu)成的空間曲線稱為路徑。這些形態(tài)序列即是曲線上的“點(diǎn)”。,16,規(guī)劃操作機(jī)的軌跡有兩

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

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

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

15、節(jié)變量空間的規(guī)劃有三個(gè)優(yōu)點(diǎn)： 直接用運(yùn)動(dòng)時(shí)的受控變量規(guī)劃軌跡； 軌跡規(guī)劃可接近實(shí)時(shí)地進(jìn)行； 關(guān)節(jié)軌跡易于規(guī)劃。 伴隨的缺點(diǎn)是難于確定運(yùn)動(dòng)中各桿件和手的位置，但是，為了避開軌跡上的障礙常常又要求知道一些桿件和手位置。,20,由于面向笛卡爾空間的方法有前述鐘種缺點(diǎn)，使得面向關(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ì)算較快，而且易于處理操作機(jī)的動(dòng)力學(xué)約束。但當(dāng)取樣點(diǎn)落在擬合的光滑多項(xiàng)式曲線上時(shí)，面向關(guān)節(jié)空間的方法沿笛卡爾路徑的準(zhǔn)確性會(huì)有損失。,21,5.3 關(guān)節(jié)空間法 軌跡規(guī)劃問題又可以分為關(guān)節(jié)空間的軌跡規(guī)劃和直角空間的軌跡規(guī)劃。

16、 關(guān)節(jié)空間法首先將在工具工作空間中期望的路徑點(diǎn)，通過逆運(yùn)動(dòng)學(xué)計(jì)算，得到期望的關(guān)節(jié)位置，然后在關(guān)節(jié)空間內(nèi)，給每個(gè)關(guān)節(jié)找到一個(gè)經(jīng)過中間點(diǎn)到達(dá)目的終點(diǎn)的光滑函數(shù)，同時(shí)使得每個(gè)關(guān)節(jié)到達(dá)中間點(diǎn)和終點(diǎn)的時(shí)間相同，這樣便可保證機(jī)械手工具能夠到達(dá)期望的直角坐標(biāo)位置。這里只要求各個(gè)關(guān)節(jié)在路徑點(diǎn)之間的時(shí)間相同，而各個(gè)關(guān)節(jié)的光滑函數(shù)的確定則是互相獨(dú)立的。,22,一般說來，生成關(guān)節(jié)軌跡設(shè)定點(diǎn)的基本算法是很簡單的：,循環(huán)：等待下一個(gè)控制間隔； 時(shí)刻操作機(jī)關(guān)節(jié)所處的位置； 若 ，則退出循環(huán)； 轉(zhuǎn)向循環(huán)。,從上述算法可以看出，要計(jì)算的是在每個(gè)控制間隔中必須更新的軌跡函數(shù)(或軌跡規(guī)劃器)h(t)。因此，對(duì)規(guī)劃的軌跡要提出四個(gè)

17、限制。第一，必須便于用迭代方式計(jì)算軌跡設(shè)定點(diǎn)；第二，必須求出并明確給定中間位置；第三，必須保證關(guān)節(jié)變量及其前二階時(shí)間導(dǎo)數(shù)的連續(xù)性，使得規(guī)劃的關(guān)節(jié)軌跡是光滑的；最后，必須減少額外的運(yùn)動(dòng) (例如，“游移” )。,23,若某關(guān)節(jié)(例如關(guān)節(jié) i )的關(guān)節(jié)軌跡使用 p 個(gè)多項(xiàng)式，則要滿足初始和終止條件(關(guān)節(jié)位置、速度和加速度)，并保證這些變量在多項(xiàng)式銜接處的連續(xù)性，因而需要確定 3( p十1)個(gè)系數(shù)。 若給定附加的中間條件(例如位置)，則對(duì)每個(gè)中間條件需要增加一系數(shù)。通常，可以給定兩個(gè)中間位置，一個(gè)靠近初始位置；另一個(gè)靠近終止位置。這樣做，除了可以較好地控制運(yùn)動(dòng)外，還能保證操作機(jī)末端以適當(dāng)?shù)姆较螂x開起點(diǎn)

18、和接近終點(diǎn)。 因此，對(duì)于連接初始位置和終止位置的每個(gè)關(guān)節(jié)變量，一個(gè)七次多項(xiàng)式就足夠了，或者用兩段四次軌跡加一段三次軌跡 （434），也可以用兩段三次軌跡加一段五次軌跡（353），或五段三次軌跡（33333）。,由約束條件數(shù)所對(duì)應(yīng)的多項(xiàng)式系數(shù)的個(gè)數(shù)確定多項(xiàng)式的次數(shù),24,為了控制操作機(jī)，在規(guī)劃運(yùn)動(dòng)軌跡之前，需要給定機(jī)器人在初始點(diǎn)和終止點(diǎn)的手臂形態(tài)。在規(guī)劃機(jī)器人關(guān)節(jié)插值運(yùn)動(dòng)軌跡時(shí)，需要注意下述幾點(diǎn)： 1、抓住一個(gè)物體時(shí)，手的運(yùn)動(dòng)方向應(yīng)該指向離開物體支承表面的方向。否則，手可能與支承面相碰。 2、若沿支承面的法線方向從初始點(diǎn)向外給定一個(gè)離開位置(提升點(diǎn))，并要求手(即手部坐標(biāo)系的原點(diǎn))經(jīng)過此位置，這

19、種離開運(yùn)動(dòng)就是允許的。如果還給定由初始點(diǎn)運(yùn)動(dòng)到離開位置的時(shí)間，我們就可以控制提起物體運(yùn)動(dòng)的速度。 3、對(duì)于手臂運(yùn)動(dòng)提升點(diǎn)的要求同樣也適用于終止位置運(yùn)動(dòng)的下放點(diǎn)(即必須先運(yùn)動(dòng)到支承表面外法線方向上的某點(diǎn)，再慢慢下移至終止點(diǎn))。這樣，可獲得和控制正確的接近方向。 4、對(duì)手臂的每一次運(yùn)動(dòng)，都設(shè)定上述四個(gè)點(diǎn)：初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn)。,25,5、位置約束 (a) 初始點(diǎn)：給定速度和加速度(一般為零)； (b) 提升點(diǎn)：中間點(diǎn)運(yùn)動(dòng)的連續(xù)； (c) 下放點(diǎn)：同提升點(diǎn)； (d) 終止點(diǎn)：給定速度和加速度(一般為零)。,26,6、除上述約束外，所有關(guān)節(jié)軌跡的極值不能超出每個(gè)關(guān)節(jié)變量的物理和幾何極限。 7

20、、時(shí)間的考慮 (a) 軌跡的初始段和終止段：時(shí)間由手接近和離開支承表面的速率決定；也是由關(guān)節(jié)電機(jī)特性決定的某個(gè)常數(shù)。 (b) 軌跡的中間點(diǎn)或中間段：時(shí)間由各關(guān)節(jié)的最大速度和加速度決定，將使用這些時(shí)間中的一個(gè)最長時(shí)間(即用最低速關(guān)節(jié)確定的最長時(shí)間來歸一化)。,在關(guān)節(jié)軌跡的典型約束條件之下，我們所要研究的是選擇一種 n 次(或小于 n 次)的多項(xiàng)式函數(shù)，使得在各結(jié)點(diǎn)(初始點(diǎn)，提升點(diǎn)，下放點(diǎn)和終止點(diǎn))上滿足對(duì)位置、速度和加速度的要求，并使關(guān)節(jié)位置、速度和加速度在整個(gè)時(shí)間間隔 t0, tf 中保持連續(xù)。,27,規(guī)劃關(guān)節(jié)插值軌跡的約束條件：,28,一種方法是為每個(gè)關(guān)節(jié)規(guī)定一個(gè)七次多項(xiàng)式函數(shù),式中，未知系

21、數(shù) aj 可由已知的位置和連續(xù)條件確定。但用這種高次多項(xiàng)式內(nèi)插給定的結(jié)點(diǎn)也許不能令人滿意，因?yàn)樗臉O值難求，而且容易產(chǎn)生額外的運(yùn)動(dòng)。 另一種辦法是將整個(gè)關(guān)節(jié)空間軌跡分割成幾段，在每段軌跡中用不同的低次多項(xiàng)式來插值。有幾種分割軌跡的方法，每種方法的特性各不相同。,434 軌跡 每個(gè)關(guān)節(jié)有下面三段軌跡：第一段由初始點(diǎn)到提升點(diǎn)的軌跡用四次多項(xiàng)式表示。第二段(或中間段）由提升點(diǎn)到下放點(diǎn)的軌跡用三次多項(xiàng)式表示。最后一段由下放點(diǎn)到終止點(diǎn)的軌跡由四次多項(xiàng)式表示。,29,353 軌跡 與434軌跡相同，但每段所用多項(xiàng)式次數(shù)與前種不同。第一段用三次多項(xiàng)式，第二段用五次多項(xiàng)式，最后一段用三次多項(xiàng)式。 (33333

22、) 軌跡 對(duì)五段軌跡都使用三次多項(xiàng)式樣條函數(shù)。 注意，上述討論對(duì)每個(gè)關(guān)節(jié)軌跡都是有效的，即每個(gè)關(guān)節(jié)軌跡可分割成三段或五段。,30,下面具體介紹在關(guān)節(jié)空間內(nèi)常用的規(guī)劃方法 5.3.1 三次多項(xiàng)式函數(shù)插值 考慮機(jī)械手末端在一定時(shí)間內(nèi)從初始位置和方位移動(dòng)到目標(biāo)位置和方位的問題。利用逆運(yùn)動(dòng)學(xué)計(jì)算，可以首先求出一組起始和終了的關(guān)節(jié)位置現(xiàn)在的問題是求出一組通過起點(diǎn)和終點(diǎn)的光滑函數(shù)。滿足這個(gè)條件的光滑函數(shù)可以有許多條，如下圖所示：,31,顯然，這些光滑函數(shù)必須滿足以下條件：,同時(shí)若要求在起點(diǎn)和終點(diǎn)的速度為零，即：,那么可以選擇如下的三次多項(xiàng)式：,滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)角度約束,滿足起點(diǎn)和終點(diǎn)的關(guān)節(jié)速度約束（滿

23、足關(guān)節(jié)速度的連續(xù)性要求）,32,作為所要求的光滑函數(shù)。式4-3中有4個(gè)待定系數(shù)，而該式需滿足式4-1和4-2的4個(gè)約束條件，因此可以唯一地解出這些系數(shù)：,注意：這組解只適用于關(guān)節(jié)起點(diǎn)、終點(diǎn)速度為零的運(yùn)動(dòng)情況。,33,例： 設(shè)機(jī)械手的某個(gè)關(guān)節(jié)的起始關(guān)節(jié)角0150，并且機(jī)械手原來是靜止的。要求在3秒鐘內(nèi)平滑地運(yùn)動(dòng)到f=750時(shí)停下來(即要求在終端時(shí)速度為零)。規(guī)劃出滿足上述條件的平滑運(yùn)動(dòng)的軌跡，并畫出關(guān)節(jié)角位置、角速度及角加速度隨時(shí)間變化的曲線。 解： 根據(jù)所給約束條件，直接代入式(4-4)，可得： a0=15， a1=0， a2=20， a3=-4.44 則關(guān)節(jié)角的位置函數(shù)可求，對(duì)位置求導(dǎo)，可以

24、得到角速度和角加速度,34,畫出它們隨時(shí)間的變化曲線如下圖所示。由圖看出，速度曲線為一拋物線，加速度則為一直線。,利用三次多項(xiàng)式規(guī)劃出的關(guān)節(jié)角的運(yùn)動(dòng)軌跡,35,5.3. 2 過路徑點(diǎn)的三次多項(xiàng)式插值,方法是：把所有路徑點(diǎn)都看成是“起點(diǎn)”或“終點(diǎn)”，求解逆運(yùn)動(dòng)學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項(xiàng)式插值函數(shù)，把路徑點(diǎn)平滑的連接起來。不同的是，這些“起點(diǎn)”和“終點(diǎn)”的關(guān)節(jié)速度不再是零（即不在路徑點(diǎn)停留）。,36,由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度的運(yùn)動(dòng)軌跡。剩下的問題就是如何確定路徑點(diǎn)上的關(guān)節(jié)速度。,同理可以求得此時(shí)的三次多項(xiàng)式系數(shù)：,37,（1） 根據(jù)工

25、具坐標(biāo)系在直角坐標(biāo)空間中的瞬時(shí)線速度和角速度來確定每個(gè)路徑點(diǎn)的關(guān)節(jié)速度 ；該方法工作量大。,（2）為了保證每個(gè)路徑點(diǎn)上的加速度連續(xù)，由控制系統(tǒng)按照此要求自動(dòng)地選擇路徑點(diǎn)的速度。,（3）在直角坐標(biāo)空間或關(guān)節(jié)空間中采用某種適當(dāng)?shù)膯l(fā)式方法，由控制系統(tǒng)自動(dòng)地選擇路徑點(diǎn)的速度；,38,對(duì)于方法（2），為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法用兩條三次曲線在路徑點(diǎn)處按照一定的規(guī)則聯(lián)系起來，拼湊成所要求的軌跡。其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也要連續(xù)。,39,對(duì)于方法（3）， 這里所說的啟發(fā)式方法很簡單，即假設(shè)用直線段把這些路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑點(diǎn)處改變符號(hào)，則把速度選定為

26、零；如果相鄰線段不改變符號(hào)，則選擇路徑點(diǎn)兩側(cè)的線段斜率的平均值作為該點(diǎn)的速度。,40,如果對(duì)于運(yùn)動(dòng)軌跡的要求更為嚴(yán)格，約束條件增多，那么三次多項(xiàng)式就不能滿足需要，必須用更高階的多項(xiàng)式對(duì)運(yùn)動(dòng)軌跡的路徑段進(jìn)行插值。例如，對(duì)某段路徑的起點(diǎn)和終點(diǎn)都規(guī)定了關(guān)節(jié)的位置、速度和加速度（有六個(gè)未知的系數(shù)），則要用一個(gè)五次多項(xiàng)式進(jìn)行插值。,41,5.3.3 拋物線連接的線性函數(shù)插值 前面介紹了利用三次多項(xiàng)式函數(shù)插值的規(guī)劃方法。另外一種常用方法是線性函數(shù)插值法，即用一條直線將起點(diǎn)與終點(diǎn)連接起來。但是，簡單的線性函數(shù)插值將使得關(guān)節(jié)的運(yùn)動(dòng)速度在起點(diǎn)和終點(diǎn)處不連續(xù)，它也意味著需要產(chǎn)生無窮大的加速度，這顯然是不希望的。因

27、此可以考慮在起點(diǎn)和終點(diǎn)處，用拋物線與直線連接起來，在拋物線段內(nèi)，使用恒定的加速度來平滑地改變速度，從而使得整個(gè)運(yùn)動(dòng)軌跡的位置和速度是連續(xù)的。,42,線性函數(shù)插值圖,利用拋物線過渡的線性函數(shù)插值圖,43,單純線性插值將導(dǎo)致在結(jié)點(diǎn)處關(guān)節(jié)運(yùn)動(dòng)速度不連續(xù)，加速度無限大。,44,解決辦法：在使用線性插值時(shí)，把每個(gè)結(jié)點(diǎn)的鄰域內(nèi)增加一段拋物線的“緩沖區(qū)段”，從而使整個(gè)軌跡上的位移和速度都連續(xù)。,45,對(duì)于多解情況，如下圖所示。加速度的值越大，過渡長度越短。,46,5.3.4 過路徑點(diǎn)的用拋物線過渡的線性插值,如圖所示，某個(gè)關(guān)節(jié)在運(yùn)動(dòng)中設(shè)有n個(gè)路徑點(diǎn)，其中三個(gè)相鄰的路徑點(diǎn)表示為j，k和l，每兩個(gè)相鄰的路徑點(diǎn)之

28、間都以線性函數(shù)相連，而所有的路徑點(diǎn)附近則有拋物線過渡。（同樣存在多解）,47,5.4.1 直角坐標(biāo)空間法,前面介紹的在關(guān)節(jié)空間內(nèi)的規(guī)劃，可以保證運(yùn)動(dòng)軌跡經(jīng)過給定的路徑點(diǎn)。但是在直角坐標(biāo)空間，路徑點(diǎn)之間的軌跡形狀往往是十分復(fù)雜的，它取決于機(jī)械手的運(yùn)動(dòng)學(xué)機(jī)構(gòu)特性。在有些情況下，對(duì)機(jī)械手末端的軌跡形狀也有一定要求，如要求它在兩點(diǎn)之間走一條直線，或者沿著一個(gè)圓弧運(yùn)動(dòng)以繞過障礙物等。這時(shí)便需要在直角坐標(biāo)空間內(nèi)規(guī)劃機(jī)械手的運(yùn)動(dòng)軌跡 直角坐標(biāo)空間的路徑點(diǎn)，指的是機(jī)械手末端的工具坐標(biāo)相對(duì)于基坐標(biāo)的位置和姿態(tài)每一個(gè)點(diǎn)由6個(gè)量組成，其中3個(gè)量描述位置，另外3個(gè)量描述姿態(tài)。 在直角坐標(biāo)空間內(nèi)規(guī)劃的方法主要有：線性函數(shù)插值法和圓弧插值法。,48,5.5 路