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

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

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

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

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

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

找到水壺

打開(kāi)水壺把水倒入杯中

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

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處首先，機(jī)器人應(yīng)該把任務(wù)進(jìn)行分解，把主人交

為了實(shí)現(xiàn)每一個(gè)動(dòng)作，需要對(duì)手部的運(yùn)動(dòng)軌跡進(jìn)行必要的規(guī)定，這是手部軌跡規(guī)劃(Handtrajectoryplanning

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

找到水壺

打開(kāi)水壺把水倒入杯中

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

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處

手部從A點(diǎn)移到B 點(diǎn)

關(guān)節(jié)從C點(diǎn)移到D點(diǎn)為了實(shí)現(xiàn)每一個(gè)動(dòng)作，需要對(duì)手部的運(yùn)動(dòng)軌跡進(jìn)行必要的規(guī)

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

軌跡規(guī)劃的目的是將操作人員輸入的簡(jiǎn)單的任務(wù)描述變?yōu)樵敿?xì)的運(yùn)動(dòng)軌跡描述。

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

簡(jiǎn)言之，機(jī)器人的工作過(guò)程，就是通過(guò)規(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ī)接口規(guī)劃控制機(jī)器人本體要求的任務(wù)

期望的運(yùn)動(dòng)和力

實(shí)際的運(yùn)動(dòng)和力控制作用

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

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

工業(yè)機(jī)器人的作業(yè)可以描述成工具坐標(biāo)系{T}相對(duì)于工件坐標(biāo)系{S}的一系列運(yùn)動(dòng)。圖4.1機(jī)器人將銷(xiāo)插入工件孔中的作業(yè)描述

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

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

對(duì)點(diǎn)位作業(yè)(pickandplaceoperation)的機(jī)器人，需要描述它的起始狀態(tài)和目標(biāo)狀態(tài)，即工具坐標(biāo)系的起始值{T0}，目標(biāo)值{Tf}。在此，用“點(diǎn)”這個(gè)詞表示工具坐標(biāo)系的位置和姿態(tài)(簡(jiǎn)稱(chēng)位姿)。對(duì)點(diǎn)位作業(yè)(pickandplaceopera

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

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

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

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

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

(3)空間直線(xiàn)運(yùn)動(dòng)。這是一種直角空間里的運(yùn)動(dòng)，它便于描述空間操作，計(jì)算量小，適宜簡(jiǎn)單的作業(yè)。

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

三、軌跡的生成方式(1)示教-再現(xiàn)運(yùn)動(dòng)。這種運(yùn)動(dòng)由4.2插補(bǔ)方式分類(lèi)與軌跡控制

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

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

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

已知直線(xiàn)始末兩點(diǎn)的坐標(biāo)值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對(duì)于基坐標(biāo)系的位置。這些已知的位置和姿態(tài)通常是通過(guò)示教方式得到的。設(shè)v為要求的沿直線(xiàn)運(yùn)動(dòng)的速度；ts為插補(bǔ)時(shí)間間隔。直線(xiàn)長(zhǎng)度

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

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

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

設(shè)v為沿圓弧運(yùn)動(dòng)速度；ts為插補(bǔ)時(shí)時(shí)間隔。(3)ts時(shí)間(4)總插補(bǔ)步數(shù)(取整數(shù)) N=

/θ+1對(duì)Pi+1點(diǎn)的坐標(biāo)，有式中：Xi=Rcosθi；Yi=Rsinθi。

由θi+1=θi+θ可判斷是否到插補(bǔ)終點(diǎn)。若θi+1，則繼續(xù)插補(bǔ)下去；當(dāng)θi+1>

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

至基礎(chǔ)坐標(biāo)系OX0Y0Z0的轉(zhuǎn)換矩陣。通過(guò)不在同一直線(xiàn)上的三點(diǎn)P1、P2、P3可確定一個(gè)圓及三點(diǎn)間若ZR軸與基礎(chǔ)坐標(biāo)系Z0軸的夾角為，XR軸與基礎(chǔ)坐標(biāo)系的夾角為θ，則可完成下述步驟：①將XRYRZR的原點(diǎn)OR放到基礎(chǔ)原點(diǎn)O上；②繞ZR軸轉(zhuǎn)θ，使X0與XR平行；③再繞XR軸轉(zhuǎn)角，使Z0與ZR平行。若ZR軸與基礎(chǔ)坐標(biāo)系Z0軸的夾角為，XR軸與基礎(chǔ)坐標(biāo)系的夾這三步完成了XRYRZR向X0Y0Z0的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)為這三步完成了XRYRZR向X0Y0Z0的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)

欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在ORXRYRZR坐標(biāo)系，則要用到TR的逆矩陣欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在ORXRYRZR

三、關(guān)節(jié)空間插補(bǔ)

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

三、關(guān)節(jié)空間插補(bǔ)

對(duì)于每一段路徑，各個(gè)關(guān)節(jié)運(yùn)動(dòng)時(shí)間均相同，這樣保證所有關(guān)節(jié)同時(shí)

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

三、關(guān)節(jié)空間插補(bǔ)

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

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

(t)來(lái)表示。

(t)在t0=0時(shí)刻的值是起始關(guān)節(jié)角度0，終端時(shí)刻tf的值是終止關(guān)節(jié)角度f(wàn)。1.三次多項(xiàng)式插值在操作臂運(yùn)動(dòng)的過(guò)程中，由

為實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)

(t)至少需要滿(mǎn)足四個(gè)約束條件，即兩端點(diǎn)位置約束和兩端點(diǎn)速度約束。端點(diǎn)位置約束是指起始位姿和終止位姿分別所對(duì)應(yīng)的關(guān)節(jié)角度。

(t)在時(shí)刻t0=0時(shí)的值是起始關(guān)節(jié)角度0，在終端時(shí)刻tf時(shí)的值是終止關(guān)節(jié)角度f(wàn)，即1.三次多項(xiàng)式插值

為實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)

(t)

為滿(mǎn)足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，兩外還有兩個(gè)約束條件，即在起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度要求。為了滿(mǎn)足關(guān)節(jié)運(yùn)動(dòng)速度連續(xù)性的要求，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度可簡(jiǎn)單地設(shè)定為零。1.三次多項(xiàng)式插值

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

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

三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡曲線(xiàn)如圖4.8所示。由圖可知，其速度曲線(xiàn)為拋物線(xiàn)，相應(yīng)的加速度曲線(xiàn)為直線(xiàn)。圖4.8三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡三次多項(xiàng)式插

例：設(shè)有一臺(tái)轉(zhuǎn)動(dòng)關(guān)節(jié)的機(jī)器人，其在執(zhí)行一項(xiàng)作業(yè)時(shí)關(guān)節(jié)運(yùn)動(dòng)歷時(shí)3

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

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

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

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

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

由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度約束條件的運(yùn)動(dòng)軌跡。求解方程組，得如何來(lái)確定路徑點(diǎn)上的關(guān)節(jié)速度？由上式確定的三如何確定路徑點(diǎn)上的關(guān)節(jié)速度？

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

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

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

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

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

、加速度θ’’0

；2）終止點(diǎn)的位置θf(wàn)、速度θ’f

、加速度θ’’f

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

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

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

為了構(gòu)造這段運(yùn)動(dòng)軌跡，假設(shè)兩端的拋物線(xiàn)軌跡具有相同的持續(xù)時(shí)間，具有大小相同而符號(hào)相反的恒加速度。對(duì)于這種路徑規(guī)劃存在有多個(gè)解，其軌跡不惟一。但是，每條路徑都對(duì)稱(chēng)于時(shí)間中點(diǎn)th和位置中點(diǎn)

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

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

當(dāng)上式中的等號(hào)成立時(shí)，軌跡線(xiàn)性段的長(zhǎng)度縮減為零，整個(gè)軌跡由兩個(gè)過(guò)渡域組成，這兩個(gè)過(guò)渡域在銜接處的斜率(關(guān)節(jié)速度)相等；加速度的取值愈大，過(guò)渡域的長(zhǎng)度會(huì)變得愈短，若加速度趨于無(wú)窮大，軌跡又復(fù)歸到簡(jiǎn)單的線(xiàn)性插值情況。為保證有解，加速度值必須選得足夠大，即通常的做法是先選定加

例：已知條件為θ0=15°，θf(wàn)=75°，tf=3

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

≥26.67°/s2

1)設(shè)計(jì)第一條軌跡對(duì)于第一條軌跡，如果選=42°/s2，算出過(guò)渡時(shí)間，則

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

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

，θ’’k

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

Bye-bye!Classisover.

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

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

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

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

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

找到水壺

打開(kāi)水壺把水倒入杯中

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

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處首先，機(jī)器人應(yīng)該把任務(wù)進(jìn)行分解，把主人交

為了實(shí)現(xiàn)每一個(gè)動(dòng)作，需要對(duì)手部的運(yùn)動(dòng)軌跡進(jìn)行必要的規(guī)定，這是手部軌跡規(guī)劃(Handtrajectoryplanning

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

找到水壺

打開(kāi)水壺把水倒入杯中

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

提起水壺到杯口上方

把水壺傾斜

把水壺豎直

把水壺放回原處

手部從A點(diǎn)移到B 點(diǎn)

關(guān)節(jié)從C點(diǎn)移到D點(diǎn)為了實(shí)現(xiàn)每一個(gè)動(dòng)作，需要對(duì)手部的運(yùn)動(dòng)軌跡進(jìn)行必要的規(guī)

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

軌跡規(guī)劃的目的是將操作人員輸入的簡(jiǎn)單的任務(wù)描述變?yōu)樵敿?xì)的運(yùn)動(dòng)軌跡描述。

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

簡(jiǎn)言之，機(jī)器人的工作過(guò)程，就是通過(guò)規(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ù)男拚Ｈ藱C(jī)接口規(guī)劃控制機(jī)器人本體要求的任務(wù)

期望的運(yùn)動(dòng)和力

實(shí)際的運(yùn)動(dòng)和力控制作用

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

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

工業(yè)機(jī)器人的作業(yè)可以描述成工具坐標(biāo)系{T}相對(duì)于工件坐標(biāo)系{S}的一系列運(yùn)動(dòng)。圖4.1機(jī)器人將銷(xiāo)插入工件孔中的作業(yè)描述

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

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

對(duì)點(diǎn)位作業(yè)(pickandplaceoperation)的機(jī)器人，需要描述它的起始狀態(tài)和目標(biāo)狀態(tài)，即工具坐標(biāo)系的起始值{T0}，目標(biāo)值{Tf}。在此，用“點(diǎn)”這個(gè)詞表示工具坐標(biāo)系的位置和姿態(tài)(簡(jiǎn)稱(chēng)位姿)。對(duì)點(diǎn)位作業(yè)(pickandplaceopera

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

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

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

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

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

(3)空間直線(xiàn)運(yùn)動(dòng)。這是一種直角空間里的運(yùn)動(dòng)，它便于描述空間操作，計(jì)算量小，適宜簡(jiǎn)單的作業(yè)。

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

三、軌跡的生成方式(1)示教-再現(xiàn)運(yùn)動(dòng)。這種運(yùn)動(dòng)由4.2插補(bǔ)方式分類(lèi)與軌跡控制

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

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

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

已知直線(xiàn)始末兩點(diǎn)的坐標(biāo)值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態(tài)，其中P0、Pe是相對(duì)于基坐標(biāo)系的位置。這些已知的位置和姿態(tài)通常是通過(guò)示教方式得到的。設(shè)v為要求的沿直線(xiàn)運(yùn)動(dòng)的速度；ts為插補(bǔ)時(shí)間間隔。直線(xiàn)長(zhǎng)度

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

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

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

設(shè)v為沿圓弧運(yùn)動(dòng)速度；ts為插補(bǔ)時(shí)時(shí)間隔。(3)ts時(shí)間(4)總插補(bǔ)步數(shù)(取整數(shù)) N=

/θ+1對(duì)Pi+1點(diǎn)的坐標(biāo)，有式中：Xi=Rcosθi；Yi=Rsinθi。

由θi+1=θi+θ可判斷是否到插補(bǔ)終點(diǎn)。若θi+1，則繼續(xù)插補(bǔ)下去；當(dāng)θi+1>

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

至基礎(chǔ)坐標(biāo)系OX0Y0Z0的轉(zhuǎn)換矩陣。通過(guò)不在同一直線(xiàn)上的三點(diǎn)P1、P2、P3可確定一個(gè)圓及三點(diǎn)間若ZR軸與基礎(chǔ)坐標(biāo)系Z0軸的夾角為，XR軸與基礎(chǔ)坐標(biāo)系的夾角為θ，則可完成下述步驟：①將XRYRZR的原點(diǎn)OR放到基礎(chǔ)原點(diǎn)O上；②繞ZR軸轉(zhuǎn)θ，使X0與XR平行；③再繞XR軸轉(zhuǎn)角，使Z0與ZR平行。若ZR軸與基礎(chǔ)坐標(biāo)系Z0軸的夾角為，XR軸與基礎(chǔ)坐標(biāo)系的夾這三步完成了XRYRZR向X0Y0Z0的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)為這三步完成了XRYRZR向X0Y0Z0的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)

欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在ORXRYRZR坐標(biāo)系，則要用到TR的逆矩陣欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在ORXRYRZR

三、關(guān)節(jié)空間插補(bǔ)

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

三、關(guān)節(jié)空間插補(bǔ)

對(duì)于每一段路徑，各個(gè)關(guān)節(jié)運(yùn)動(dòng)時(shí)間均相同，這樣保證所有關(guān)節(jié)同時(shí)

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

三、關(guān)節(jié)空間插補(bǔ)

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

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

(t)來(lái)表示。

(t)在t0=0時(shí)刻的值是起始關(guān)節(jié)角度0，終端時(shí)刻tf的值是終止關(guān)節(jié)角度f(wàn)。1.三次多項(xiàng)式插值在操作臂運(yùn)動(dòng)的過(guò)程中，由

為實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)

(t)至少需要滿(mǎn)足四個(gè)約束條件，即兩端點(diǎn)位置約束和兩端點(diǎn)速度約束。端點(diǎn)位置約束是指起始位姿和終止位姿分別所對(duì)應(yīng)的關(guān)節(jié)角度。

(t)在時(shí)刻t0=0時(shí)的值是起始關(guān)節(jié)角度0，在終端時(shí)刻tf時(shí)的值是終止關(guān)節(jié)角度f(wàn)，即1.三次多項(xiàng)式插值

為實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)

(t)

為滿(mǎn)足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，兩外還有兩個(gè)約束條件，即在起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度要求。為了滿(mǎn)足關(guān)節(jié)運(yùn)動(dòng)速度連續(xù)性的要求，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度可簡(jiǎn)單地設(shè)定為零。1.三次多項(xiàng)式插值

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

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

三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡曲線(xiàn)如圖4.8所示。由圖可知，其速度曲線(xiàn)為拋物線(xiàn)，相應(yīng)的加速度曲線(xiàn)為直線(xiàn)。圖4.8三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡三次多項(xiàng)式插

例：設(shè)有一臺(tái)轉(zhuǎn)動(dòng)關(guān)節(jié)的機(jī)器人，其在執(zhí)行一項(xiàng)作業(yè)時(shí)關(guān)節(jié)運(yùn)動(dòng)歷時(shí)3

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

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

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

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

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

由上式確定的三次多項(xiàng)式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度約束條件的運(yùn)動(dòng)軌跡。求解方程組，得如何來(lái)確定路徑點(diǎn)上的關(guān)節(jié)速度？由上式確定的三如何確定路徑點(diǎn)上的關(guān)節(jié)速度？

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

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

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

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

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