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

1、2 2 22-2-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25本章主要內(nèi)容本章主要內(nèi)容 4.1 機(jī)器人軌跡規(guī)劃概述機(jī)器人軌跡規(guī)劃概述 4.2 插補(bǔ)方式分類與軌跡控制插補(bǔ)方式分類與軌跡控制 4.3 機(jī)器人軌跡插補(bǔ)計算機(jī)器人軌跡插補(bǔ)計算 4.4 軌跡的實時生成軌跡的實時生成3 3 33-3-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 所謂機(jī)器人的規(guī)劃所謂機(jī)器人的規(guī)劃(p1anning)，指的是，指的是機(jī)器人機(jī)器人根據(jù)自身的任務(wù)，求得完成這一任務(wù)的解決方案的根據(jù)自身的任務(wù)，求得完成這一任務(wù)的解決方案的過程過程。這里所說的任務(wù)，具有廣義的概念，既可以。這里所說的任務(wù)，具有廣義的概

2、念，既可以指機(jī)器人要完成的某一具體任務(wù)，也可以是機(jī)器人指機(jī)器人要完成的某一具體任務(wù)，也可以是機(jī)器人的某個動作，比如手部或關(guān)節(jié)的某個規(guī)定的運(yùn)動等的某個動作，比如手部或關(guān)節(jié)的某個規(guī)定的運(yùn)動等。4.1 機(jī)器人軌跡規(guī)劃概述機(jī)器人軌跡規(guī)劃概述一、機(jī)器人規(guī)劃的概念一、機(jī)器人規(guī)劃的概念4 4 44-4-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為說明機(jī)器人規(guī)劃的概念，我們舉下面的例子：為說明機(jī)器人規(guī)劃的概念，我們舉下面的例子： 在一些老齡化比較嚴(yán)重的國家，開發(fā)了各種各樣的在一些老齡化比較嚴(yán)重的國家，開發(fā)了各種各樣的機(jī)器人專門用于伺候老人，這些機(jī)器人有不少是采用機(jī)器人專門用于伺候老人，這些機(jī)器人

3、有不少是采用聲控的方式比如主人用聲音命令機(jī)器人聲控的方式比如主人用聲音命令機(jī)器人“給我倒一給我倒一杯開水杯開水”，我們先不考慮機(jī)器人是如何識別人的自然，我們先不考慮機(jī)器人是如何識別人的自然語言，而是著重分析一下機(jī)器人在得到這樣一個命令語言，而是著重分析一下機(jī)器人在得到這樣一個命令后，如何來完成主人交給的任務(wù)。后，如何來完成主人交給的任務(wù)。一、機(jī)器人規(guī)劃的概念一、機(jī)器人規(guī)劃的概念5 5 55-5-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 首先，機(jī)器人應(yīng)該把任務(wù)進(jìn)行分解，把主人交代的任務(wù)分解成為首先，機(jī)器人應(yīng)該把任務(wù)進(jìn)行分解，把主人交代的任務(wù)分解成為“取一個杯子取一個杯子”、“找到水

4、壺找到水壺”、“打開瓶塞打開瓶塞”、“把水倒人杯中把水倒人杯中”、“把水送給主人把水送給主人”等一系列子任務(wù)。這一層次的規(guī)劃稱為等一系列子任務(wù)。這一層次的規(guī)劃稱為(task planning)，它完成總體任務(wù)的分解。，它完成總體任務(wù)的分解。 然后再針對每一個子任務(wù)進(jìn)行進(jìn)一步的規(guī)劃。以然后再針對每一個子任務(wù)進(jìn)行進(jìn)一步的規(guī)劃。以“把水倒入杯中把水倒入杯中”這這一子任務(wù)為例，可以進(jìn)一步分解成為一系列動作，這一層次的規(guī)劃稱為一子任務(wù)為例，可以進(jìn)一步分解成為一系列動作，這一層次的規(guī)劃稱為，它把實現(xiàn)每一個子任務(wù)的過程分解為一系列具體的動作。，它把實現(xiàn)每一個子任務(wù)的過程分解為一系列具體的動作。取一個杯子取一

5、個杯子 找到水壺找到水壺 打開水壺打開水壺把水倒入杯中把水倒入杯中 把水送給主人把水送給主人給主人倒一杯水給主人倒一杯水把水倒入杯中把水倒入杯中 提起水壺到杯口上方提起水壺到杯口上方 把水壺傾斜把水壺傾斜 把水壺豎直把水壺豎直 把水壺放回原處把水壺放回原處6 6 66-6-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為了實現(xiàn)每一個動作，需要對手部的運(yùn)動軌跡進(jìn)行必要的規(guī)定，這為了實現(xiàn)每一個動作，需要對手部的運(yùn)動軌跡進(jìn)行必要的規(guī)定，這是手部是手部軌跡規(guī)劃軌跡規(guī)劃(hand trajectory planning )。 為了使手部實現(xiàn)預(yù)定的運(yùn)動，就要知道各關(guān)節(jié)的運(yùn)動規(guī)律，這是為了使手部實

6、現(xiàn)預(yù)定的運(yùn)動，就要知道各關(guān)節(jié)的運(yùn)動規(guī)律，這是關(guān)關(guān)節(jié)軌跡規(guī)劃節(jié)軌跡規(guī)劃(joint trajectory planning)。 最后才是關(guān)節(jié)的最后才是關(guān)節(jié)的運(yùn)動控制運(yùn)動控制(motion control)。取一個杯子取一個杯子 找到水壺找到水壺 打開水壺打開水壺把水倒入杯中把水倒入杯中 把水送給主人把水送給主人給主人倒一杯水給主人倒一杯水 提起水壺到杯口上方提起水壺到杯口上方 把水壺傾斜把水壺傾斜 把水壺豎直把水壺豎直 把水壺放回原處把水壺放回原處 手部從手部從a點(diǎn)移到點(diǎn)移到b 點(diǎn)點(diǎn) 關(guān)節(jié)從關(guān)節(jié)從c點(diǎn)移到點(diǎn)移到d點(diǎn)點(diǎn)7 7 77-7-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 上述例

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

8、人員輸入的簡單的軌跡規(guī)劃的目的是將操作人員輸入的簡單的任務(wù)描述任務(wù)描述變變?yōu)樵敿?xì)的為詳細(xì)的運(yùn)動軌跡描述運(yùn)動軌跡描述。 例如，對一般的工業(yè)機(jī)器人來說，操作員可能只輸入機(jī)例如，對一般的工業(yè)機(jī)器人來說，操作員可能只輸入機(jī)械手末端的目標(biāo)位置和方位，而規(guī)劃的任務(wù)便是要確定出達(dá)械手末端的目標(biāo)位置和方位，而規(guī)劃的任務(wù)便是要確定出達(dá)到目標(biāo)的關(guān)節(jié)軌跡的形狀、運(yùn)動的時間和速度等。這里所說到目標(biāo)的關(guān)節(jié)軌跡的形狀、運(yùn)動的時間和速度等。這里所說的軌跡是指隨時間變化的位置、速度和加速度。的軌跡是指隨時間變化的位置、速度和加速度。8 8 88-8-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 簡言之，機(jī)器人的工作

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

10、要求的任務(wù)要求的任務(wù) 期望的期望的運(yùn)動和力運(yùn)動和力 實際的實際的運(yùn)動和力運(yùn)動和力控制作用控制作用 要求的任務(wù)由操作人員輸入給機(jī)器人，為了使機(jī)器人操作方便、使要求的任務(wù)由操作人員輸入給機(jī)器人，為了使機(jī)器人操作方便、使用簡單，必須允許操作人員給出盡量簡單的描述。用簡單，必須允許操作人員給出盡量簡單的描述。 期望的運(yùn)動和力是進(jìn)行機(jī)器人控制所必需的輸入量，它們是機(jī)械手期望的運(yùn)動和力是進(jìn)行機(jī)器人控制所必需的輸入量，它們是機(jī)械手末端在每一個時刻的位姿和速度，對于絕大多數(shù)情況，還要求給出每一末端在每一個時刻的位姿和速度，對于絕大多數(shù)情況，還要求給出每一時刻期望的關(guān)節(jié)位移和速度，有些控制方法還要求給出期望的加

11、速度等時刻期望的關(guān)節(jié)位移和速度，有些控制方法還要求給出期望的加速度等。9 9 99-9-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/254.1 機(jī)器人軌跡規(guī)劃概述機(jī)器人軌跡規(guī)劃概述軌跡規(guī)劃？軌跡規(guī)劃？ 機(jī)器人在作業(yè)空間要完成給定的任務(wù)，其手機(jī)器人在作業(yè)空間要完成給定的任務(wù)，其手部運(yùn)動必須按一定的軌跡部運(yùn)動必須按一定的軌跡(trajectory)進(jìn)行。進(jìn)行。 軌跡的生成一般是先給定軌跡上的若干個點(diǎn)，軌跡的生成一般是先給定軌跡上的若干個點(diǎn)，將其經(jīng)運(yùn)動學(xué)反解映射到關(guān)節(jié)空間，對關(guān)節(jié)空間將其經(jīng)運(yùn)動學(xué)反解映射到關(guān)節(jié)空間，對關(guān)節(jié)空間中的相應(yīng)點(diǎn)建立運(yùn)動方程，然后按這些運(yùn)動方程中的相應(yīng)點(diǎn)建立運(yùn)動方程，然后

12、按這些運(yùn)動方程對關(guān)節(jié)進(jìn)行插值，從而實現(xiàn)作業(yè)空間的運(yùn)動要求，對關(guān)節(jié)進(jìn)行插值，從而實現(xiàn)作業(yè)空間的運(yùn)動要求，這一過程通常稱為這一過程通常稱為軌跡規(guī)劃軌跡規(guī)劃。 10101010-10-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25二、軌跡規(guī)劃的一般性問題二、軌跡規(guī)劃的一般性問題 工業(yè)機(jī)器人的作業(yè)可以描述成工具坐標(biāo)系工業(yè)機(jī)器人的作業(yè)可以描述成工具坐標(biāo)系t相相對于工件坐標(biāo)系對于工件坐標(biāo)系s的一系列運(yùn)動。的一系列運(yùn)動。 圖圖4.1 機(jī)器人將銷插入工件孔中的作業(yè)描述機(jī)器人將銷插入工件孔中的作業(yè)描述 圖圖4.1所示的將銷插入所示的將銷插入工件孔中的作業(yè)，可以工件孔中的作業(yè)，可以借助工具坐標(biāo)系的一系借助

13、工具坐標(biāo)系的一系列位姿列位姿pi (i=1，2，n)來描述。來描述。 11111111-11-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25二、軌跡規(guī)劃的一般性問題二、軌跡規(guī)劃的一般性問題 用工具坐標(biāo)系相對于工用工具坐標(biāo)系相對于工件坐標(biāo)系的運(yùn)動來描述作件坐標(biāo)系的運(yùn)動來描述作業(yè)路徑是一種通用的作業(yè)業(yè)路徑是一種通用的作業(yè)描述方法。描述方法。 它把作業(yè)路徑描述與具它把作業(yè)路徑描述與具體的機(jī)器人、手爪或工具體的機(jī)器人、手爪或工具分離開來，形成了模型化分離開來，形成了模型化的作業(yè)描述方法，從而使的作業(yè)描述方法，從而使這種描述既適用于不同的這種描述既適用于不同的機(jī)器人，也適用于在同一機(jī)器人，也適用

14、于在同一機(jī)器人上裝夾不同規(guī)格的機(jī)器人上裝夾不同規(guī)格的工具。工具。 圖圖4.2 機(jī)器人的初始狀態(tài)和終止?fàn)顟B(tài)機(jī)器人的初始狀態(tài)和終止?fàn)顟B(tài)12121212-12-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 對對點(diǎn)位作業(yè)點(diǎn)位作業(yè)(pick and place operation)的機(jī)器的機(jī)器人，需要描述它的起始狀態(tài)和目標(biāo)狀態(tài)，即工人，需要描述它的起始狀態(tài)和目標(biāo)狀態(tài)，即工具坐標(biāo)系的起始值具坐標(biāo)系的起始值t0，目標(biāo)值，目標(biāo)值tf。在此，。在此，用用“點(diǎn)點(diǎn)”這個詞表示工具坐標(biāo)系的位置和姿態(tài)這個詞表示工具坐標(biāo)系的位置和姿態(tài)(簡稱位姿簡稱位姿) 。 13131313-13-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人

15、技術(shù)基礎(chǔ)2016/9/25 對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定對于另外一些作業(yè)，如弧焊和曲面加工等，不僅要規(guī)定操作臂的起始點(diǎn)和終止點(diǎn)，而且要指明兩點(diǎn)之間的若干中操作臂的起始點(diǎn)和終止點(diǎn)，而且要指明兩點(diǎn)之間的若干中間點(diǎn)間點(diǎn)(稱稱路徑點(diǎn)路徑點(diǎn))，必須沿特定的路徑運(yùn)動，必須沿特定的路徑運(yùn)動(路徑約束路徑約束)。這。這類稱為連續(xù)路徑運(yùn)動類稱為連續(xù)路徑運(yùn)動(continuouspath motion)或輪廓運(yùn)或輪廓運(yùn)動動(contour motion) 。 在規(guī)劃機(jī)器人的運(yùn)動時還需要弄清楚在其路徑上是否在規(guī)劃機(jī)器人的運(yùn)動時還需要弄清楚在其路徑上是否存在障礙物存在障礙物(障礙約束障礙約束)。

16、主要討論連續(xù)路徑的無障礙的軌跡規(guī)劃方法。主要討論連續(xù)路徑的無障礙的軌跡規(guī)劃方法。 軌跡規(guī)劃器可形象地看成為一個黑箱，其輸入包括路軌跡規(guī)劃器可形象地看成為一個黑箱，其輸入包括路徑的徑的“設(shè)定設(shè)定”和和“約束約束”，輸出的是操作臂末端手部的，輸出的是操作臂末端手部的“位姿序列位姿序列”，表示手部在各離散時刻的中間形位。，表示手部在各離散時刻的中間形位。 14141414-14-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 軌跡規(guī)劃既可在關(guān)節(jié)空間也可在直角空間中進(jìn)行，軌跡規(guī)劃既可在關(guān)節(jié)空間也可在直角空間中進(jìn)行，但是所規(guī)劃的軌跡函數(shù)都必須連續(xù)和平滑，使機(jī)器人但是所規(guī)劃的軌跡函數(shù)都必須連續(xù)和平

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

18、姿、速度和加速度表示為時間的函數(shù)。而相應(yīng)的關(guān)節(jié)位移、速度和加度表示為時間的函數(shù)。而相應(yīng)的關(guān)節(jié)位移、速度和加速度由手部的信息導(dǎo)出。通常通過運(yùn)動學(xué)反解得出關(guān)速度由手部的信息導(dǎo)出。通常通過運(yùn)動學(xué)反解得出關(guān)節(jié)位移、用逆稚可比求出關(guān)節(jié)速度，用逆雅可比及其節(jié)位移、用逆稚可比求出關(guān)節(jié)速度，用逆雅可比及其導(dǎo)數(shù)求解關(guān)節(jié)加速度。導(dǎo)數(shù)求解關(guān)節(jié)加速度。15151515-15-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25三、軌跡的生成方式三、軌跡的生成方式 (1) 示教示教-再現(xiàn)運(yùn)動。再現(xiàn)運(yùn)動。這種運(yùn)動由人手把手示教機(jī)器人，這種運(yùn)動由人手把手示教機(jī)器人，定時記錄各關(guān)節(jié)變量，得到沿路徑運(yùn)動時各關(guān)節(jié)的位移定時記錄

19、各關(guān)節(jié)變量，得到沿路徑運(yùn)動時各關(guān)節(jié)的位移時間函數(shù)時間函數(shù)q(t)；再現(xiàn)時，按內(nèi)存中記錄的各點(diǎn)的值產(chǎn)生；再現(xiàn)時，按內(nèi)存中記錄的各點(diǎn)的值產(chǎn)生序列動作。序列動作。 (2) 關(guān)節(jié)空間運(yùn)動。關(guān)節(jié)空間運(yùn)動。這種運(yùn)動直接在關(guān)節(jié)空間里進(jìn)行。這種運(yùn)動直接在關(guān)節(jié)空間里進(jìn)行。由于動力學(xué)參數(shù)及其極限值直接在關(guān)節(jié)空間里描述，所由于動力學(xué)參數(shù)及其極限值直接在關(guān)節(jié)空間里描述，所以用這種方式求最短時間運(yùn)動很方便。以用這種方式求最短時間運(yùn)動很方便。 (3) 空間直線運(yùn)動?？臻g直線運(yùn)動。這是一種直角空間里的運(yùn)動，它便這是一種直角空間里的運(yùn)動，它便于描述空間操作，計算量小，適宜簡單的作業(yè)。于描述空間操作，計算量小，適宜簡單的作業(yè)。

20、 (4) 空間曲線運(yùn)動空間曲線運(yùn)動。這是一種在描述空間中用明確的函。這是一種在描述空間中用明確的函數(shù)表達(dá)的運(yùn)動，如圓周運(yùn)動、螺旋運(yùn)動等數(shù)表達(dá)的運(yùn)動，如圓周運(yùn)動、螺旋運(yùn)動等 16161616-16-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/254.2插補(bǔ)方式分類與軌跡控制插補(bǔ)方式分類與軌跡控制 一、插補(bǔ)方式分類一、插補(bǔ)方式分類點(diǎn)位作業(yè)（點(diǎn)位作業(yè)（ptp=point-to-point motion）連續(xù)路徑作業(yè)（連續(xù)路徑作業(yè)（cp=continuous-path motion）路徑控路徑控制制不插補(bǔ)不插補(bǔ)關(guān)節(jié)插補(bǔ)關(guān)節(jié)插補(bǔ)(平滑平滑)空間插補(bǔ)空間插補(bǔ)點(diǎn)位控點(diǎn)位控制制ptp(1) 各軸獨(dú)立各軸

21、獨(dú)立快速到達(dá)?？焖俚竭_(dá)。(2) 各關(guān)節(jié)最各關(guān)節(jié)最大加速度限制大加速度限制(1) 各軸協(xié)調(diào)運(yùn)動定時插補(bǔ)。各軸協(xié)調(diào)運(yùn)動定時插補(bǔ)。(2) 各關(guān)節(jié)最大加速度限制各關(guān)節(jié)最大加速度限制連續(xù)路連續(xù)路徑徑控制控制cp(1) 在空間插補(bǔ)點(diǎn)間進(jìn)行關(guān)節(jié)定在空間插補(bǔ)點(diǎn)間進(jìn)行關(guān)節(jié)定時插補(bǔ)。時插補(bǔ)。(2) 用關(guān)節(jié)的低階多項式擬合空用關(guān)節(jié)的低階多項式擬合空間直線使各軸協(xié)調(diào)運(yùn)動。間直線使各軸協(xié)調(diào)運(yùn)動。(3) 各關(guān)節(jié)最大加速度限制各關(guān)節(jié)最大加速度限制(1) 直線、圓弧、曲直線、圓弧、曲線等距插補(bǔ)。線等距插補(bǔ)。(2) 起停線速度、線起停線速度、線加速度給定，各關(guān)節(jié)加速度給定，各關(guān)節(jié)速度、加速度限制速度、加速度限制17171717

22、-17-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25二、二、 機(jī)器人軌跡控制過程機(jī)器人軌跡控制過程圖圖4.3 機(jī)器人軌跡控制過程機(jī)器人軌跡控制過程 機(jī)器人的基本操作方式是示教機(jī)器人的基本操作方式是示教-再現(xiàn)，即首先教機(jī)器人如何做，機(jī)器再現(xiàn)，即首先教機(jī)器人如何做，機(jī)器人記住了這個過程，于是它可以根據(jù)需要重復(fù)這個動作。操作過程中，人記住了這個過程，于是它可以根據(jù)需要重復(fù)這個動作。操作過程中，不可能把空間軌跡的所有點(diǎn)都示教一遍使機(jī)器人記住，這樣太繁瑣，也不可能把空間軌跡的所有點(diǎn)都示教一遍使機(jī)器人記住，這樣太繁瑣，也浪費(fèi)很多計算機(jī)內(nèi)存。實際上，對于有規(guī)律的軌跡，僅示教幾個特征點(diǎn)，浪費(fèi)很多計算

23、機(jī)內(nèi)存。實際上，對于有規(guī)律的軌跡，僅示教幾個特征點(diǎn)，計算機(jī)就能利用插補(bǔ)算法獲得中間點(diǎn)的坐標(biāo)，如直線需要示教兩點(diǎn)，圓計算機(jī)就能利用插補(bǔ)算法獲得中間點(diǎn)的坐標(biāo)，如直線需要示教兩點(diǎn)，圓弧需要示教三點(diǎn)，通過機(jī)器人逆向運(yùn)動學(xué)算法由這些點(diǎn)的坐標(biāo)求出機(jī)器弧需要示教三點(diǎn)，通過機(jī)器人逆向運(yùn)動學(xué)算法由這些點(diǎn)的坐標(biāo)求出機(jī)器人各關(guān)節(jié)的位置和角度人各關(guān)節(jié)的位置和角度( 1, , n)，然后由后面的角位置閉環(huán)控制系統(tǒng)，然后由后面的角位置閉環(huán)控制系統(tǒng)實現(xiàn)要求的軌跡上的一點(diǎn)。繼續(xù)插補(bǔ)并重復(fù)上述過程，從而實現(xiàn)要求的實現(xiàn)要求的軌跡上的一點(diǎn)。繼續(xù)插補(bǔ)并重復(fù)上述過程，從而實現(xiàn)要求的軌跡。軌跡。18181818-18-工業(yè)機(jī)器人技術(shù)基礎(chǔ)

24、工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/254.3 機(jī)器人軌跡插值計算機(jī)器人軌跡插值計算一、一、 直線插補(bǔ)直線插補(bǔ) 空間直線插補(bǔ)是在已知該直線始末兩點(diǎn)的位置和姿態(tài)的空間直線插補(bǔ)是在已知該直線始末兩點(diǎn)的位置和姿態(tài)的條件下，求各軌跡中間點(diǎn)條件下，求各軌跡中間點(diǎn)(插補(bǔ)點(diǎn)插補(bǔ)點(diǎn))的位置和姿態(tài)。的位置和姿態(tài)。 已知直線始末兩點(diǎn)的坐標(biāo)值已知直線始末兩點(diǎn)的坐標(biāo)值p0(x0，y0，z0)、pe(xe，ye，ze)及及姿態(tài)，其中姿態(tài)，其中p0、pe是相對于基坐標(biāo)系的位置。這些已知的位置和是相對于基坐標(biāo)系的位置。這些已知的位置和姿態(tài)通常是通過示教方式得到的。設(shè)姿態(tài)通常是通過示教方式得到的。設(shè)v為要求的沿直線運(yùn)動的速度；為

25、要求的沿直線運(yùn)動的速度；ts為插補(bǔ)時間間隔。為插補(bǔ)時間間隔。直線長度直線長度 222e0e0e0lxxyyzz 直線插補(bǔ)和圓弧插補(bǔ)是機(jī)器人系統(tǒng)中的基本插補(bǔ)算法。對于直線插補(bǔ)和圓弧插補(bǔ)是機(jī)器人系統(tǒng)中的基本插補(bǔ)算法。對于非直線和圓弧軌跡，可用直線或圓弧逼近，以實現(xiàn)這些軌跡。非直線和圓弧軌跡，可用直線或圓弧逼近，以實現(xiàn)這些軌跡。 19191919-19-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25ts間隔內(nèi)行程間隔內(nèi)行程d = vts；插補(bǔ)總步數(shù)插補(bǔ)總步數(shù)n為為l/d +1的整數(shù)部分；的整數(shù)部分；各軸增量：各軸增量：e0e0e0/xxxnyyynzzzn各插補(bǔ)點(diǎn)坐標(biāo)值：各插補(bǔ)點(diǎn)坐標(biāo)值：11

26、1iiiiiixxi xyyi yzzi z 式中：式中：i=0，1，2，n。 可見，兩個插補(bǔ)點(diǎn)之間的距離正比于要求的運(yùn)動速度，只有插補(bǔ)點(diǎn)之間可見，兩個插補(bǔ)點(diǎn)之間的距離正比于要求的運(yùn)動速度，只有插補(bǔ)點(diǎn)之間的距離足夠小，才能滿足一定的軌跡控制精度要求。的距離足夠小，才能滿足一定的軌跡控制精度要求。 機(jī)器人控制系統(tǒng)易于實現(xiàn)定時插補(bǔ)，例如采用定時中斷方式每隔機(jī)器人控制系統(tǒng)易于實現(xiàn)定時插補(bǔ)，例如采用定時中斷方式每隔ts中斷中斷一次進(jìn)行一次插補(bǔ)，計算一次逆向運(yùn)動學(xué)，輸出一個給定值。由于一次進(jìn)行一次插補(bǔ)，計算一次逆向運(yùn)動學(xué)，輸出一個給定值。由于ts僅為僅為幾毫秒，機(jī)器人沿著要求軌跡的速度一般不會很高，且機(jī)

27、器人總的運(yùn)動精幾毫秒，機(jī)器人沿著要求軌跡的速度一般不會很高，且機(jī)器人總的運(yùn)動精度不高，故大多數(shù)工業(yè)機(jī)器人采用定時插補(bǔ)方式。度不高，故大多數(shù)工業(yè)機(jī)器人采用定時插補(bǔ)方式。20202020-20-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25二、圓弧插補(bǔ)二、圓弧插補(bǔ) 1.平面圓弧插補(bǔ)平面圓弧插補(bǔ) 平面圓弧是指圓弧平面與基坐標(biāo)系的三大平面之一重平面圓弧是指圓弧平面與基坐標(biāo)系的三大平面之一重合，以合，以xoy平面圓弧為例。已知不在一條直線上的三平面圓弧為例。已知不在一條直線上的三點(diǎn)點(diǎn)p1、p2、p3及這三點(diǎn)對應(yīng)的機(jī)器人手端的姿態(tài)，如及這三點(diǎn)對應(yīng)的機(jī)器人手端的姿態(tài)，如圖圖4.5及圖及圖4.6所示。

28、所示。 圖圖4.5 由已知的三點(diǎn)由已知的三點(diǎn)p1、p2、p3決定的圓弧決定的圓弧 圖圖4.6 圓弧插補(bǔ)圓弧插補(bǔ) 21212121-21-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 設(shè)設(shè)v為沿圓弧運(yùn)動速度；為沿圓弧運(yùn)動速度；ts為插補(bǔ)時時間隔。為插補(bǔ)時時間隔。 (1) 由由p1、p2、p3決定的圓弧半徑?jīng)Q定的圓弧半徑r。 (2) 總的圓心角總的圓心角 = 1+ 2，即，即222212121222223232arccos()2/2arccos()2/2xxyyrrxxyyrr(3) ts時間內(nèi)角位移量時間內(nèi)角位移量 =tsv/r 22222222-22-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)

29、基礎(chǔ)2016/9/25 (4) 總插補(bǔ)步數(shù)總插補(bǔ)步數(shù)(取整數(shù)取整數(shù))n = / + 1 對對pi+1 點(diǎn)的坐標(biāo)，有點(diǎn)的坐標(biāo)，有 式中：式中：xi=r cosi；yi=rsini。 1cos()coscossinsincossiniiiiiixrrrxy 1sin()sincoscossincossiniiiiiiyrrryx 由由i+1=i + 可判斷是否到插補(bǔ)終點(diǎn)。若可判斷是否到插補(bǔ)終點(diǎn)。若i+1 ，則繼，則繼續(xù)插補(bǔ)下去；當(dāng)續(xù)插補(bǔ)下去；當(dāng) i+1 時，則修正最后一步的步長時，則修正最后一步的步長 ,故平面圓弧位置插補(bǔ)為故平面圓弧位置插補(bǔ)為111cossincossiniiiiiiiixxyy

30、yx 23232323-23-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/252.空間圓弧插補(bǔ)空間圓弧插補(bǔ) 空間圓弧是指三維空間任一平面內(nèi)的圓弧?？臻g圓弧是指三維空間任一平面內(nèi)的圓弧。 空間圓弧插補(bǔ)可分三步來處理：空間圓弧插補(bǔ)可分三步來處理： (1) 把三維問題轉(zhuǎn)化成二維，找出圓弧所在平面。把三維問題轉(zhuǎn)化成二維，找出圓弧所在平面。 (2) 利用二維平面插補(bǔ)算法求出插補(bǔ)點(diǎn)坐標(biāo)利用二維平面插補(bǔ)算法求出插補(bǔ)點(diǎn)坐標(biāo)(xi+1, yi+1)。 (3) 把該點(diǎn)的坐標(biāo)值轉(zhuǎn)變?yōu)榛A(chǔ)坐標(biāo)系下的值，如圖把該點(diǎn)的坐標(biāo)值轉(zhuǎn)變?yōu)榛A(chǔ)坐標(biāo)系下的值，如圖4.7所所示。示。24242424-24-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)

31、機(jī)器人技術(shù)基礎(chǔ)2016/9/25 通過不在同一直線上的三點(diǎn)通過不在同一直線上的三點(diǎn)p1、p2、p3可確定一個圓及三可確定一個圓及三點(diǎn)間的圓弧，其圓心為點(diǎn)間的圓弧，其圓心為or,半徑為半徑為r，圓弧所在平面與基，圓弧所在平面與基礎(chǔ)坐標(biāo)系平面的交線分別為礎(chǔ)坐標(biāo)系平面的交線分別為ab、bc、ca。 建立圓弧平面插補(bǔ)坐標(biāo)系，即把建立圓弧平面插補(bǔ)坐標(biāo)系，即把orxryrzr坐標(biāo)系原點(diǎn)與坐標(biāo)系原點(diǎn)與圓心圓心or重合，設(shè)重合，設(shè)orxryr平面為圓弧所在平面，且保持平面為圓弧所在平面，且保持zr為外法線方向。為外法線方向。 求解兩坐標(biāo)系的轉(zhuǎn)換矩陣。求解兩坐標(biāo)系的轉(zhuǎn)換矩陣。 令令tr表示由圓弧坐標(biāo)表示由圓弧坐

32、標(biāo)orxryrzr 至基礎(chǔ)坐標(biāo)系至基礎(chǔ)坐標(biāo)系ox0y0z0的轉(zhuǎn)換矩陣。的轉(zhuǎn)換矩陣。 25252525-25-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 若若zr軸與基礎(chǔ)坐標(biāo)系軸與基礎(chǔ)坐標(biāo)系z0軸的夾角為軸的夾角為 ，xr軸與基礎(chǔ)坐標(biāo)系的夾角為軸與基礎(chǔ)坐標(biāo)系的夾角為，則可完成，則可完成下述步驟：下述步驟： 將將xryrzr的原點(diǎn)的原點(diǎn)or放到基礎(chǔ)原點(diǎn)放到基礎(chǔ)原點(diǎn)o上；上； 繞繞zr軸轉(zhuǎn)軸轉(zhuǎn)，使使x0與與xr平行；平行； 再繞再繞xr軸轉(zhuǎn)軸轉(zhuǎn) 角，使角，使z0與與zr平行。平行。26262626-26-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 這三步完成了這三步完成了xr

33、yrzr向向x0y0z0的轉(zhuǎn)換，故的轉(zhuǎn)換，故總轉(zhuǎn)換矩陣應(yīng)為總轉(zhuǎn)換矩陣應(yīng)為(,) ( , ) (, )cossincossincossincos coscos sin0sincos0001rrrrrrrooooooxyzzxxyzttrr27272727-27-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在欲將基礎(chǔ)坐標(biāo)系的坐標(biāo)值表示在orxryrzr坐標(biāo)系，則要用到坐標(biāo)系，則要用到tr的逆矩陣的逆矩陣 1cossin0cossinsin coscos cossinsin coscos cossinsin sincos sincossin sincos sin

34、cos0001rrrrrrrroooooroooxyxyzxyzt28282828-28-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 三、關(guān)節(jié)空間插補(bǔ)三、關(guān)節(jié)空間插補(bǔ) 路徑點(diǎn)路徑點(diǎn)(結(jié)點(diǎn)結(jié)點(diǎn))通常用工具坐標(biāo)系相對通常用工具坐標(biāo)系相對于工件坐標(biāo)系位姿來表示。為了求得在于工件坐標(biāo)系位姿來表示。為了求得在關(guān)節(jié)空間形成所要求的軌跡，首先用運(yùn)關(guān)節(jié)空間形成所要求的軌跡，首先用運(yùn)動學(xué)反解將路徑點(diǎn)轉(zhuǎn)換成關(guān)節(jié)矢量角度動學(xué)反解將路徑點(diǎn)轉(zhuǎn)換成關(guān)節(jié)矢量角度值，然后對每個關(guān)節(jié)擬合一個光滑函數(shù)，值，然后對每個關(guān)節(jié)擬合一個光滑函數(shù)，使之從起始點(diǎn)開始，依次通過所有路徑使之從起始點(diǎn)開始，依次通過所有路徑點(diǎn)，最后到達(dá)

35、目標(biāo)點(diǎn)。點(diǎn)，最后到達(dá)目標(biāo)點(diǎn)。29292929-29-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 對于每一段路徑，對于每一段路徑，各個關(guān)節(jié)運(yùn)動時間均各個關(guān)節(jié)運(yùn)動時間均相同，這樣保證所有關(guān)節(jié)同時到達(dá)路徑相同，這樣保證所有關(guān)節(jié)同時到達(dá)路徑點(diǎn)和終止點(diǎn)，點(diǎn)和終止點(diǎn)，從而得到工具坐標(biāo)系應(yīng)有從而得到工具坐標(biāo)系應(yīng)有的位置和姿態(tài)。但是，盡管每個關(guān)節(jié)在的位置和姿態(tài)。但是，盡管每個關(guān)節(jié)在同一段路徑中的運(yùn)動時間相同，各個關(guān)同一段路徑中的運(yùn)動時間相同，各個關(guān)節(jié)函數(shù)之間卻是相互獨(dú)立的。節(jié)函數(shù)之間卻是相互獨(dú)立的。 三、關(guān)節(jié)空間插補(bǔ)三、關(guān)節(jié)空間插補(bǔ) 30303030-30-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)20

36、16/9/25 在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，需要給在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，需要給定機(jī)器人在起始點(diǎn)、終止點(diǎn)手臂的形位。定機(jī)器人在起始點(diǎn)、終止點(diǎn)手臂的形位。 對關(guān)節(jié)進(jìn)行插值時，應(yīng)滿足一系列約對關(guān)節(jié)進(jìn)行插值時，應(yīng)滿足一系列約束條件，在滿足所要求的約束條件下，束條件，在滿足所要求的約束條件下，可以選取不同類型的關(guān)節(jié)插值函數(shù)生成可以選取不同類型的關(guān)節(jié)插值函數(shù)生成不同的軌跡。不同的軌跡。 三、關(guān)節(jié)空間插補(bǔ)三、關(guān)節(jié)空間插補(bǔ) 31313131-31-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/251.三次多項式插值三次多項式插值 在操作臂運(yùn)動的過程中，由于相應(yīng)于起始在操作臂運(yùn)動的過程中，由于相應(yīng)于起始點(diǎn)

37、的關(guān)節(jié)角度點(diǎn)的關(guān)節(jié)角度 0是已知的而終止點(diǎn)的關(guān)節(jié)角是已知的而終止點(diǎn)的關(guān)節(jié)角 f可以通過運(yùn)動學(xué)反解得到，因此，運(yùn)動軌跡可以通過運(yùn)動學(xué)反解得到，因此，運(yùn)動軌跡的描述，可用起始點(diǎn)關(guān)節(jié)角與終止點(diǎn)關(guān)節(jié)角度的描述，可用起始點(diǎn)關(guān)節(jié)角與終止點(diǎn)關(guān)節(jié)角度的一個平滑插值函數(shù)的一個平滑插值函數(shù) (t)來表示。來表示。 (t)在在t0=0時刻的值是起始關(guān)節(jié)角度時刻的值是起始關(guān)節(jié)角度 0，終端時刻，終端時刻tf的值的值是終止關(guān)節(jié)角度是終止關(guān)節(jié)角度 f。 32323232-32-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為實現(xiàn)單個關(guān)節(jié)的平穩(wěn)運(yùn)動，軌跡為實現(xiàn)單個關(guān)節(jié)的平穩(wěn)運(yùn)動，軌跡函數(shù)函數(shù) (t)至少需要滿足四

38、個約束條件，至少需要滿足四個約束條件，即即兩端點(diǎn)位置約束兩端點(diǎn)位置約束和和兩端點(diǎn)速度約束兩端點(diǎn)速度約束。 端點(diǎn)位置約束是指起始位姿和終止端點(diǎn)位置約束是指起始位姿和終止位姿分別所對應(yīng)的關(guān)節(jié)角度。位姿分別所對應(yīng)的關(guān)節(jié)角度。 (t)在時在時刻刻t0=0時的值是起始關(guān)節(jié)角度時的值是起始關(guān)節(jié)角度 0，在終，在終端時刻端時刻tf時的值是終止關(guān)節(jié)角度時的值是終止關(guān)節(jié)角度 f，即，即1.三次多項式插值三次多項式插值 0ff0t33333333-33-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為滿足關(guān)節(jié)運(yùn)動速度的連續(xù)性要求，兩外為滿足關(guān)節(jié)運(yùn)動速度的連續(xù)性要求，兩外還有兩個約束條件，即在還有兩個約束

39、條件，即在起始點(diǎn)和終止點(diǎn)的關(guān)起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度要求。節(jié)速度要求。為了滿足關(guān)節(jié)運(yùn)動速度連續(xù)性的為了滿足關(guān)節(jié)運(yùn)動速度連續(xù)性的要求，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度可簡單地設(shè)要求，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度可簡單地設(shè)定為零。定為零。 f000t1.三次多項式插值三次多項式插值 34343434-34-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 上面給出的四個約束條件可以惟一地確上面給出的四個約束條件可以惟一地確定一個三次多項式定一個三次多項式 230123taa ta ta t運(yùn)動過程中的關(guān)節(jié)速度和加速度則為運(yùn)動過程中的關(guān)節(jié)速度和加速度則為 2123232326taa ta ttaa t35

40、353535-35-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25為求得三次多項式的系數(shù)為求得三次多項式的系數(shù)a0，a1，a2和和a3，代以，代以給定的約束條件，有方程組給定的約束條件，有方程組 0023f01 f2 f3 f1212 f3 f0023aaa ta ta taaa ta t求解該方程組，可得求解該方程組，可得 0012f02f3f03f032aaatat 36363636-36-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 對于起始速度及終止速度為零的關(guān)節(jié)運(yùn)動對于起始速度及終止速度為零的關(guān)節(jié)運(yùn)動，滿足連續(xù)，滿足連續(xù)平穩(wěn)運(yùn)動要求的三次多項式插值函數(shù)為平穩(wěn)運(yùn)動要求

41、的三次多項式插值函數(shù)為230f0f023ff32( )()()ttttt關(guān)節(jié)角速度和角加速度的表達(dá)式為關(guān)節(jié)角速度和角加速度的表達(dá)式為 2f0f023fff0f023ff66( )()()612( )()()ttttttttt這里再次指出：這里再次指出：這組解只適用于關(guān)節(jié)起始、終止速度為零這組解只適用于關(guān)節(jié)起始、終止速度為零的運(yùn)動情況。的運(yùn)動情況。37373737-37-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25圖圖4.8 三次多項式插值的關(guān)節(jié)運(yùn)動軌跡三次多項式插值的關(guān)節(jié)運(yùn)動軌跡 三次多項式插值的關(guān)節(jié)運(yùn)動軌跡曲線如圖三次多項式插值的關(guān)節(jié)運(yùn)動軌跡曲線如圖4.8所所示。由圖可知，其速度曲

42、線為拋物線，相應(yīng)的加速示。由圖可知，其速度曲線為拋物線，相應(yīng)的加速度曲線為直線。度曲線為直線。38383838-38-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 例：設(shè)有一臺轉(zhuǎn)動關(guān)節(jié)的機(jī)器人，其在執(zhí)行例：設(shè)有一臺轉(zhuǎn)動關(guān)節(jié)的機(jī)器人，其在執(zhí)行一項作業(yè)時關(guān)節(jié)運(yùn)動歷時一項作業(yè)時關(guān)節(jié)運(yùn)動歷時3 s。根據(jù)需要，其上某。根據(jù)需要，其上某一關(guān)節(jié)必須運(yùn)動平穩(wěn)，并具有如下作業(yè)狀態(tài)：初一關(guān)節(jié)必須運(yùn)動平穩(wěn)，并具有如下作業(yè)狀態(tài)：初始時，關(guān)節(jié)靜止不動，位置始時，關(guān)節(jié)靜止不動，位置0=15；運(yùn)動結(jié)束時；運(yùn)動結(jié)束時f =75，此時關(guān)節(jié)速度為，此時關(guān)節(jié)速度為0。試根據(jù)上述要求規(guī)。試根據(jù)上述要求規(guī)劃該關(guān)節(jié)的運(yùn)動。劃該

43、關(guān)節(jié)的運(yùn)動。 39393939-39-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/252.過路徑點(diǎn)的三次多項式插值過路徑點(diǎn)的三次多項式插值 一般情況下要求規(guī)劃過路徑點(diǎn)的軌跡。如圖一般情況下要求規(guī)劃過路徑點(diǎn)的軌跡。如圖4.9所示，所示，機(jī)器人作業(yè)除在機(jī)器人作業(yè)除在a、b點(diǎn)有位姿要求外，在路徑點(diǎn)點(diǎn)有位姿要求外，在路徑點(diǎn)c、d也有也有位姿要求。對于這種情況，假如末端執(zhí)行器在路徑點(diǎn)停留，位姿要求。對于這種情況，假如末端執(zhí)行器在路徑點(diǎn)停留，即各路徑點(diǎn)上速度為即各路徑點(diǎn)上速度為0，則軌跡規(guī)劃可連續(xù)直接使用前面介，則軌跡規(guī)劃可連續(xù)直接使用前面介紹的三次多項式插值方法；但若末端執(zhí)行器只是經(jīng)過，并不紹的三

44、次多項式插值方法；但若末端執(zhí)行器只是經(jīng)過，并不停留，就需要將前述方法推廣。停留，就需要將前述方法推廣。 40404040-40-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 實際上，可以把所有路徑點(diǎn)看作是實際上，可以把所有路徑點(diǎn)看作是“起始點(diǎn)起始點(diǎn)”或或“終止點(diǎn)終止點(diǎn)”，求解逆運(yùn)動，求解逆運(yùn)動學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定學(xué)，得到相應(yīng)的關(guān)節(jié)矢量值。然后確定所要求的三次多項式插值函數(shù)，把路徑所要求的三次多項式插值函數(shù)，把路徑點(diǎn)平滑地連接起來。但是，在這些點(diǎn)平滑地連接起來。但是，在這些“起起始點(diǎn)始點(diǎn)”和和“終止點(diǎn)終止點(diǎn)“的的關(guān)節(jié)運(yùn)動速度不關(guān)節(jié)運(yùn)動速度不再是零再是零。41414141-

45、41-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 設(shè)路徑點(diǎn)上的關(guān)節(jié)速度已知，在某段路徑設(shè)路徑點(diǎn)上的關(guān)節(jié)速度已知，在某段路徑上，起始點(diǎn)為上，起始點(diǎn)為0和和 ，終止點(diǎn)為，終止點(diǎn)為f和和 ，這，這時，確定三次多項式系數(shù)的方法與前所述完全時，確定三次多項式系數(shù)的方法與前所述完全一致，只是速度約束條件變?yōu)橐恢拢皇撬俣燃s束條件變?yōu)?0ff0t0f 利用約束條件確定三次多項式系數(shù)，有下利用約束條件確定三次多項式系數(shù)，有下列方程組：列方程組：0023f01 f2 f3 f12f12 f3 f23aaa ta ta taaa ta t42424242-42-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)20

46、16/9/25 求解方程組，得求解方程組，得00102f00f2fff3f00f32ff32121aaatttatt 如何來確定路徑點(diǎn)上的關(guān)節(jié)速度？如何來確定路徑點(diǎn)上的關(guān)節(jié)速度？ 由上式確定的三次多項式描述了起始點(diǎn)和終止點(diǎn)具有任由上式確定的三次多項式描述了起始點(diǎn)和終止點(diǎn)具有任意給定位置和速度約束條件的運(yùn)動軌跡。意給定位置和速度約束條件的運(yùn)動軌跡。43434343-43-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25如何確定路徑點(diǎn)上的關(guān)節(jié)速度？如何確定路徑點(diǎn)上的關(guān)節(jié)速度？ （1）根據(jù)工具坐標(biāo)系在直角坐標(biāo)空間中的）根據(jù)工具坐標(biāo)系在直角坐標(biāo)空間中的瞬時線速度和角速度來確定每個路徑點(diǎn)的關(guān)節(jié)瞬時

47、線速度和角速度來確定每個路徑點(diǎn)的關(guān)節(jié)速度。速度。 該方法利用操作臂在此路徑點(diǎn)上的逆雅可比，該方法利用操作臂在此路徑點(diǎn)上的逆雅可比，把該點(diǎn)的直角坐標(biāo)速度把該點(diǎn)的直角坐標(biāo)速度“映射映射”為所要求的關(guān)為所要求的關(guān)節(jié)速度。當(dāng)然，如果操作臂的某個路徑點(diǎn)是奇節(jié)速度。當(dāng)然，如果操作臂的某個路徑點(diǎn)是奇異點(diǎn)，這時就不能任意設(shè)置速度值。異點(diǎn)，這時就不能任意設(shè)置速度值。 按照該方法生成的軌跡雖然能滿足用戶設(shè)置按照該方法生成的軌跡雖然能滿足用戶設(shè)置速度的需要，但是逐點(diǎn)設(shè)置速度畢竟要耗費(fèi)很速度的需要，但是逐點(diǎn)設(shè)置速度畢竟要耗費(fèi)很大的工作量。大的工作量。44444444-44-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016

48、/9/25如何確定路徑點(diǎn)上的關(guān)節(jié)速度？如何確定路徑點(diǎn)上的關(guān)節(jié)速度？ (2) 在直角坐標(biāo)空間或關(guān)節(jié)空間中采用適當(dāng)?shù)膯l(fā)式方法，在直角坐標(biāo)空間或關(guān)節(jié)空間中采用適當(dāng)?shù)膯l(fā)式方法，由控制系統(tǒng)自動地選擇路徑點(diǎn)的速度。由控制系統(tǒng)自動地選擇路徑點(diǎn)的速度。 圖表示一種啟發(fā)式選擇路徑點(diǎn)速度的方式。圖中圖表示一種啟發(fā)式選擇路徑點(diǎn)速度的方式。圖中0為起始點(diǎn)；為起始點(diǎn)；d為為終止點(diǎn)，終止點(diǎn)，a，b和和c是路徑點(diǎn)，用細(xì)實線表示過路徑點(diǎn)時的關(guān)節(jié)運(yùn)動是路徑點(diǎn)，用細(xì)實線表示過路徑點(diǎn)時的關(guān)節(jié)運(yùn)動速度。這里所用的啟發(fā)式信息從概念到計算方法都很簡單，即，假設(shè)速度。這里所用的啟發(fā)式信息從概念到計算方法都很簡單，即，假設(shè)用直線段把這些

49、路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑點(diǎn)用直線段把這些路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑點(diǎn)處改變將號，則把速度選定為零；如果相鄰線段不改變符號，則選取處改變將號，則把速度選定為零；如果相鄰線段不改變符號，則選取路徑點(diǎn)兩側(cè)的線段斜率的平均值作為該點(diǎn)的速度。因此，根據(jù)規(guī)定的路徑點(diǎn)兩側(cè)的線段斜率的平均值作為該點(diǎn)的速度。因此，根據(jù)規(guī)定的路徑點(diǎn)，系統(tǒng)就能夠按此規(guī)則自動生成相應(yīng)的路徑點(diǎn)速度。路徑點(diǎn)，系統(tǒng)就能夠按此規(guī)則自動生成相應(yīng)的路徑點(diǎn)速度。45454545-45-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25如何確定路徑點(diǎn)上的關(guān)節(jié)速度？如何確定路徑點(diǎn)上的關(guān)節(jié)速度？ (3)為了保證

50、每個路徑點(diǎn)上的加速度連續(xù)，由為了保證每個路徑點(diǎn)上的加速度連續(xù)，由控制系統(tǒng)按此要求自動地選擇路徑點(diǎn)的速度?？刂葡到y(tǒng)按此要求自動地選擇路徑點(diǎn)的速度。 該方法為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法用該方法為了保證路徑點(diǎn)處的加速度連續(xù)，可以設(shè)法用兩條三次曲線在路徑點(diǎn)處按一定規(guī)則聯(lián)接起來，拼湊成兩條三次曲線在路徑點(diǎn)處按一定規(guī)則聯(lián)接起來，拼湊成所要求的軌跡。所要求的軌跡。 其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也其約束條件是：聯(lián)接處不僅速度連續(xù)，而且加速度也連續(xù)，下面具體地說明這種方法。連續(xù)，下面具體地說明這種方法。46464646-46-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 設(shè)所

51、經(jīng)過的路徑點(diǎn)處的關(guān)節(jié)角度為設(shè)所經(jīng)過的路徑點(diǎn)處的關(guān)節(jié)角度為v，與該點(diǎn)，與該點(diǎn)相鄰的前后兩點(diǎn)的關(guān)節(jié)角分別為相鄰的前后兩點(diǎn)的關(guān)節(jié)角分別為0和和g。設(shè)其路徑。設(shè)其路徑點(diǎn)處的關(guān)節(jié)加速度連續(xù)。如果路徑點(diǎn)用三次多項點(diǎn)處的關(guān)節(jié)加速度連續(xù)。如果路徑點(diǎn)用三次多項式連接，試確定多項式的所有系數(shù)。式連接，試確定多項式的所有系數(shù)。 該機(jī)器人路徑可分為該機(jī)器人路徑可分為 0到到 v段及段及 v到到 g段兩段，段兩段，可通過由兩個三次多項式組成的樣條函數(shù)連接。可通過由兩個三次多項式組成的樣條函數(shù)連接。設(shè)從設(shè)從 0到到 v的三次多項式插值函數(shù)為的三次多項式插值函數(shù)為 23110111213taa ta ta t而從而從 v到

52、到 g的三次多項式插值函數(shù)為的三次多項式插值函數(shù)為 23220212223taa ta ta t47474747-47-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25時間區(qū)間分別是時間區(qū)間分別是0,tf1、0,tf2；約束是：；約束是：對于第一個方程：對于第一個方程：對于第二個方程：對于第二個方程：48484848-48-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25二個方程：二個方程： 第一條曲線在第一條曲線在t=tf1時的速度等于第二時的速度等于第二條曲線在條曲線在t=0的速度；的速度；第一條曲線在第一條曲線在t=tf1時的加速度等于第二條時的加速度等于第二條曲線在曲線在t

53、=0時的加速度時的加速度49494949-49-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25兩個方程共有兩個方程共有8個未知數(shù)：個未知數(shù)：已知條件有已知條件有8個：個：第一方程：第一方程：第二方程：第二方程： 第一條曲線在第一條曲線在t=tf1時的速度等于第二條曲線在時的速度等于第二條曲線在t=0的速度；的速度；第一條曲線在第一條曲線在t=tf1時的加速度等于第二條曲線在時的加速度等于第二條曲線在t=0時的時的加速度加速度50505050-50-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25當(dāng)當(dāng)tf1=tf2=tf時的解為：時的解為：51515151-51-工業(yè)機(jī)器人技術(shù)基礎(chǔ)

54、工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/253. 高階多項式插值高階多項式插值 如果對于運(yùn)動軌跡的要求更為嚴(yán)格，約束如果對于運(yùn)動軌跡的要求更為嚴(yán)格，約束條件增多，那么三次多項式就不能滿足需要，條件增多，那么三次多項式就不能滿足需要，必須用更高階的多項式對運(yùn)動軌跡的路徑段進(jìn)必須用更高階的多項式對運(yùn)動軌跡的路徑段進(jìn)行插值。例如，行插值。例如， 對某段路徑的超始點(diǎn)和終止點(diǎn)對某段路徑的超始點(diǎn)和終止點(diǎn)都有規(guī)定了關(guān)節(jié)的位置、速度和加速度要求，都有規(guī)定了關(guān)節(jié)的位置、速度和加速度要求，則要用一個五次多項式進(jìn)行插值。則要用一個五次多項式進(jìn)行插值。52525252-52-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/

55、251）超始點(diǎn)的位置）超始點(diǎn)的位置0、速度、速度0 、加速度、加速度0 ；2）終止點(diǎn)的位置）終止點(diǎn)的位置f、速度、速度f 、加速度、加速度f ； 在給定六個約束條件下，方程的解為：在給定六個約束條件下，方程的解為：五次多項式有六個系數(shù)，必須滿足六個約束條件：五次多項式有六個系數(shù)，必須滿足六個約束條件：六個約束條件是：六個約束條件是：002345f01 f2 f3 f4 f5 f01234f12 f3 f4 f5 f0223f23 f4 f5 f23452261220aaa ta ta ta ta taaa ta ta ta taaa ta ta t53535353-53-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)

56、機(jī)器人技術(shù)基礎(chǔ)2016/9/254.用拋物線過渡的線性插值用拋物線過渡的線性插值 在關(guān)節(jié)空間軌跡規(guī)劃中，對于給定起始點(diǎn)和在關(guān)節(jié)空間軌跡規(guī)劃中，對于給定起始點(diǎn)和終止點(diǎn)的情況選擇線性函數(shù)插值較為簡單，如圖終止點(diǎn)的情況選擇線性函數(shù)插值較為簡單，如圖4.10所示。所示。 然而，單純線性插值會導(dǎo)致起始點(diǎn)和終止點(diǎn)的然而，單純線性插值會導(dǎo)致起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)運(yùn)動速度不連續(xù)，且加速度無窮大，顯然，關(guān)節(jié)運(yùn)動速度不連續(xù)，且加速度無窮大，顯然，在兩端點(diǎn)會造成剛性沖擊。在兩端點(diǎn)會造成剛性沖擊。54545454-54-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為此應(yīng)對線性函數(shù)插值方案進(jìn)為此應(yīng)對線性函數(shù)插

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

58、5555-55-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/25 為了構(gòu)造這段運(yùn)動軌跡，假設(shè)為了構(gòu)造這段運(yùn)動軌跡，假設(shè)兩端的拋物線軌跡具有相同的持兩端的拋物線軌跡具有相同的持續(xù)時間，具有大小相同而符號相續(xù)時間，具有大小相同而符號相反的恒加速度。對于這種路徑規(guī)反的恒加速度。對于這種路徑規(guī)劃存在有多個解，其軌跡不惟一。劃存在有多個解，其軌跡不惟一。但是，每條路徑都對稱于時間中但是，每條路徑都對稱于時間中點(diǎn)點(diǎn)th和位置中點(diǎn)和位置中點(diǎn) h。 要保證路徑軌跡的連續(xù)、光滑，要保證路徑軌跡的連續(xù)、光滑，即要求拋物線軌跡的終點(diǎn)速度必即要求拋物線軌跡的終點(diǎn)速度必須等于線性段的速度，故有下列須等于線性段的速

59、度，故有下列關(guān)系關(guān)系haahattt56565656-56-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/252a0a1 2t設(shè)關(guān)節(jié)從起始點(diǎn)到終止點(diǎn)的總運(yùn)動時間為設(shè)關(guān)節(jié)從起始點(diǎn)到終止點(diǎn)的總運(yùn)動時間為tf ，則，則tf=2th，并注意到，并注意到h0f1()22afaf00tt tat 一般情況下，一般情況下， 0、 f、tf是已知條是已知條件，這樣，可以選擇相應(yīng)的件，這樣，可以選擇相應(yīng)的和和得到相應(yīng)的軌跡。得到相應(yīng)的軌跡。 57575757-57-工業(yè)機(jī)器人技術(shù)基礎(chǔ)工業(yè)機(jī)器人技術(shù)基礎(chǔ)2016/9/2522ff0fa422ttt at為保證為保證有解，加速度值有解，加速度值必須選得足夠大，即

60、必須選得足夠大，即 f02f4tat通常的做法是先選定加速度通常的做法是先選定加速度的值，然后按下式求出相應(yīng)的的值，然后按下式求出相應(yīng)的 當(dāng)上式中的等號成立時，軌跡線性段的長度縮減為當(dāng)上式中的等號成立時，軌跡線性段的長度縮減為零，整個軌跡由兩個過渡域組成，這兩個過渡域在銜零，整個軌跡由兩個過渡域組成，這兩個過渡域在銜接處的斜率接處的斜率(關(guān)節(jié)速度關(guān)節(jié)速度)相等；加速度的取值愈大，過相等；加速度的取值愈大，過渡域的長度會變得愈短，若加速度趨于無窮大，軌跡渡域的長度會變得愈短，若加速度趨于無窮大，軌跡又復(fù)歸到簡單的線性插值情況。又復(fù)歸到簡單的線性插值情況。58585858-58-工業(yè)機(jī)器人技術(shù)基礎(chǔ)