第六章機器人的軌跡規(guī)劃、生成與控制技術

1、 機器人的軌跡規(guī)劃與生成機器人的軌跡規(guī)劃與生成 機器人控制綜述機器人控制綜述 機器人控制系統的一般構成機器人控制系統的一般構成 機器人傳感器機器人傳感器4.1 4.1 機器人的軌跡規(guī)劃與生成機器人的軌跡規(guī)劃與生成 所謂機器人的規(guī)劃所謂機器人的規(guī)劃(P1anning)(P1anning)，指的是，指的是機器人根據自身的任務，求得完成這一任務機器人根據自身的任務，求得完成這一任務的解決方案的過程的解決方案的過程。這里所說的任務，具有廣。這里所說的任務，具有廣義的概念，既可以指機器人要完成的某一具體義的概念，既可以指機器人要完成的某一具體任務，也可以是機器人的某個動作，比如手部任務，也可以是機器人的

2、某個動作，比如手部或關節(jié)的某個規(guī)定的運動等。或關節(jié)的某個規(guī)定的運動等。 為說明機器人規(guī)劃的概念，我們舉下面的為說明機器人規(guī)劃的概念，我們舉下面的例子：例子： 在一些老齡化比較嚴重的國家，開發(fā)了各種各在一些老齡化比較嚴重的國家，開發(fā)了各種各樣的機器人專門用于伺候老人，這些機器人有不少是樣的機器人專門用于伺候老人，這些機器人有不少是采用聲控的方式比如主人用聲音命令機器人采用聲控的方式比如主人用聲音命令機器人“給我給我倒一杯開水倒一杯開水”，我們先不考慮機器人是如何識別人的，我們先不考慮機器人是如何識別人的自然語言，而是著重分析一下機器人在得到這樣一個自然語言，而是著重分析一下機器人在得到這樣一個命

3、今后，如何來完成主人交給的任務。命今后，如何來完成主人交給的任務。 首先，機器人應該把任務進行分解，把主人交代的任務分解成為“取一個杯子”、“找到水壺”、“打開瓶塞”、“把水倒人杯中”、“把水送給主人”等一系列子任務。這一層次的規(guī)劃稱為(Task planning)，它完成總體任務的分解。 然后再針對每一個子任務進行進一步的規(guī)劃。以“把水倒入杯中”這一子任務為例，可以進一步分解成為“把水壺提到杯口上方”、“把水壺傾斜倒水入杯”、“把水壺豎直“、“把水壺放回原處”等一系列動作，這一層次的規(guī)劃稱為(Motion P1anning)，它把實現每一個子任務的過程分解為一系列具體的動作。 為了實現每一個

4、動作，需要對手部的運動軌跡進行必要的規(guī)定，這是手部軌跡規(guī)劃軌跡規(guī)劃(Hand trajectory planning )。 為了使手部實現預定的運動，就要知道各關節(jié)的運動規(guī)律，這是關節(jié)軌跡規(guī)劃關節(jié)軌跡規(guī)劃(Joint trajectory planning)。 最后才是關節(jié)的運動控制運動控制(Motion control)。 上述例子可以看出，機器人的規(guī)劃是分層次的，上述例子可以看出，機器人的規(guī)劃是分層次的，從高層的從高層的任務規(guī)劃任務規(guī)劃，動作規(guī)劃動作規(guī)劃到到手手部軌跡規(guī)劃部軌跡規(guī)劃和和關節(jié)關節(jié)軌跡規(guī)劃軌跡規(guī)劃，最后才是，最后才是底層的控制底層的控制( (見圖見圖) )。在上述例子。在上述例

5、子中，我們沒有討論力的問題，實際上，對有些機器人中，我們沒有討論力的問題，實際上，對有些機器人來說，力的大小也是要控制的，這時，除了手部或關來說，力的大小也是要控制的，這時，除了手部或關節(jié)的軌跡規(guī)劃，還要進行手部和關節(jié)輸出節(jié)的軌跡規(guī)劃，還要進行手部和關節(jié)輸出力的規(guī)劃力的規(guī)劃。 智能化程度越高，規(guī)劃的層次越多，操作就越簡智能化程度越高，規(guī)劃的層次越多，操作就越簡單。單。 對工業(yè)機器人來說，高層的任務規(guī)劃和動作規(guī)劃對工業(yè)機器人來說，高層的任務規(guī)劃和動作規(guī)劃一般是依賴人來完成的。而且一般的工業(yè)機器人也不一般是依賴人來完成的。而且一般的工業(yè)機器人也不具備力的反饋，所以，工業(yè)機器人通常只具有軌跡規(guī)具備力

6、的反饋，所以，工業(yè)機器人通常只具有軌跡規(guī)劃的和底層的控制功能。劃的和底層的控制功能。取一個杯子取一個杯子 找到水壺找到水壺 打開水壺打開水壺把水倒入杯中把水倒入杯中 把水送給主人把水送給主人給主人倒一杯水給主人倒一杯水 提起水壺到杯口上方提起水壺到杯口上方 把水壺傾斜把水壺傾斜 把水壺豎直把水壺豎直 把水壺放回原處把水壺放回原處 手部從手部從A點移到點移到B 點點 關節(jié)從關節(jié)從A點移到點移到B點點圖圖 智能機器人的規(guī)劃層次智能機器人的規(guī)劃層次 軌跡規(guī)劃的目的是軌跡規(guī)劃的目的是將操作人員輸入的將操作人員輸入的簡單的簡單的任務描述任務描述變?yōu)樵敿毜淖優(yōu)樵敿毜倪\動軌跡描述運動軌跡描述。 例如，對一般

7、的工業(yè)機器人來說，操作員可能只例如，對一般的工業(yè)機器人來說，操作員可能只輸入機械手末端的目標位置和方位，而規(guī)劃的任務便輸入機械手末端的目標位置和方位，而規(guī)劃的任務便是要確定出達到目標的關節(jié)軌跡的形狀、運動的時間是要確定出達到目標的關節(jié)軌跡的形狀、運動的時間和速度等。這里所說的軌跡是指隨時間變化的位置、和速度等。這里所說的軌跡是指隨時間變化的位置、速度和加速度。速度和加速度。 簡言之，機器人的工作過程，就是通過規(guī)劃，將要求的任簡言之，機器人的工作過程，就是通過規(guī)劃，將要求的任務變?yōu)槠谕倪\動和力，由控制環(huán)節(jié)根據期望的運動和力的信務變?yōu)槠谕倪\動和力，由控制環(huán)節(jié)根據期望的運動和力的信號，產生相應的

8、控制作用，以使機器人輸出實際的運動和力，號，產生相應的控制作用，以使機器人輸出實際的運動和力，從而完成期望的任務。這一過程表述如下圖所示。這里，機器從而完成期望的任務。這一過程表述如下圖所示。這里，機器人實際運動的情況通常還要反饋給規(guī)劃級和控制級，以便對規(guī)人實際運動的情況通常還要反饋給規(guī)劃級和控制級，以便對規(guī)劃和控制的結果做出適當的修正。劃和控制的結果做出適當的修正。人人機機接接口口規(guī)規(guī)劃劃控控制制機機器器人人本本體體要求的任務要求的任務 期望的期望的運動和力運動和力 實際的實際的運動和力運動和力控制作用控制作用圖圖 機器人的工作原理示意圖機器人的工作原理示意圖 上圖中，要求的任務由操作人員輸

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

10、控制: : 通常只給出機械手末端的起點和終點，有時也通常只給出機械手末端的起點和終點，有時也給出一些中間經過點，所有這些點統稱為路徑點。給出一些中間經過點，所有這些點統稱為路徑點。應注意這里所說的應注意這里所說的“點點” ” 不僅包括機械手末端的不僅包括機械手末端的位置，而且包括方位，因此描述一個點通常需要位置，而且包括方位，因此描述一個點通常需要6 6個量。通常希望機械手末端的運動是光滑的，即它個量。通常希望機械手末端的運動是光滑的，即它具有連續(xù)的一階導數，有時甚至要求具有連續(xù)的二具有連續(xù)的一階導數，有時甚至要求具有連續(xù)的二階導數。不平滑的運動容易造成機構的磨損和破壞階導數。不平滑的運動容易

11、造成機構的磨損和破壞，甚至可能激發(fā)機械手的振動。因此規(guī)劃的任務便，甚至可能激發(fā)機械手的振動。因此規(guī)劃的任務便是要根據給定的路徑點規(guī)劃出通過這些點的光滑的是要根據給定的路徑點規(guī)劃出通過這些點的光滑的運動軌跡。運動軌跡。對于對于CPCP控制控制: : 機械手末端的運動軌跡是根據任務的需要給定機械手末端的運動軌跡是根據任務的需要給定的，但是它也必須按照一定的采樣間隔，通過逆運的，但是它也必須按照一定的采樣間隔，通過逆運動學計算，將其變換到關節(jié)空間，然后在關節(jié)空間動學計算，將其變換到關節(jié)空間，然后在關節(jié)空間中尋找光滑函數來擬合這些離散點最后，還有在中尋找光滑函數來擬合這些離散點最后，還有在機器人的計算

12、機內部如何表示軌跡，以及如何實時機器人的計算機內部如何表示軌跡，以及如何實時地生成軌跡的問題。地生成軌跡的問題。 軌跡規(guī)劃問題又可以分為軌跡規(guī)劃問題又可以分為關節(jié)空間的軌跡規(guī)劃關節(jié)空間的軌跡規(guī)劃和和直角空間的軌跡規(guī)劃直角空間的軌跡規(guī)劃。關節(jié)空間法關節(jié)空間法 關節(jié)空間法首先將在工具空間中期望的路徑點，關節(jié)空間法首先將在工具空間中期望的路徑點，通過逆運動學計算，得到期望的通過逆運動學計算，得到期望的關節(jié)位置關節(jié)位置，然后在關，然后在關節(jié)空間內，給每個關節(jié)找到一個經過中間點到達目的節(jié)空間內，給每個關節(jié)找到一個經過中間點到達目的終點的終點的光滑函數光滑函數，同時使得每個關節(jié)到達中間點和終，同時使得每個

13、關節(jié)到達中間點和終點的點的時間相同時間相同，這樣便可保證機械手工具能夠到達期，這樣便可保證機械手工具能夠到達期望的直角坐標位置。這里只要求各個關節(jié)在路徑點之望的直角坐標位置。這里只要求各個關節(jié)在路徑點之間的間的時間相同時間相同，而，而各個關節(jié)的光滑函數的確定則是互各個關節(jié)的光滑函數的確定則是互相獨立的相獨立的。下面具體介紹在關節(jié)空間內常用的兩種規(guī)劃方法下面具體介紹在關節(jié)空間內常用的兩種規(guī)劃方法1 1） 三次多項式函數插值三次多項式函數插值 考慮機械手末端在一定時間內從初始位置和方位移動考慮機械手末端在一定時間內從初始位置和方位移動到目標位置和方位的問題。利用逆運動學計算，可以首先到目標位置和方

14、位的問題。利用逆運動學計算，可以首先求出一組起始和終了的關節(jié)位置現在的問題是求出一組求出一組起始和終了的關節(jié)位置現在的問題是求出一組通過起點和終點的通過起點和終點的。滿足這個條件的光滑函數可。滿足這個條件的光滑函數可以有許多條，如下圖所示：以有許多條，如下圖所示： 00q 0fq t 00qq ffq tq顯然，這些光滑函數必須滿足以下條件顯然，這些光滑函數必須滿足以下條件：同時若要求在起點和終點的速度為零，即：同時若要求在起點和終點的速度為零，即：那么可以選擇如下的三次多項式：那么可以選擇如下的三次多項式： 230123q taata ta t 作為所要求的光滑函數。式作為所要求的光滑函數。

15、式4-3中有中有4個待定系數，而個待定系數，而該式需滿足式該式需滿足式4-1和和4-2的的4個約束條件，因此可以唯一地解個約束條件，因此可以唯一地解出這些系數：出這些系數：（4-3）（4-2）（4-1）（4-4）001202303032ffffaqaaqqtaqqt 例：例： 設機械手的某個關節(jié)的起始關節(jié)角設機械手的某個關節(jié)的起始關節(jié)角0 015150 0，并且機械手，并且機械手原來是靜止的。要求在原來是靜止的。要求在3 3秒鐘內平滑地運動到秒鐘內平滑地運動到f f=75=750 0時停下來時停下來( (即要求在終端時速度為零即要求在終端時速度為零) )。規(guī)劃出滿足上述條件的平滑運動。規(guī)劃出滿

16、足上述條件的平滑運動的軌跡，并畫出關節(jié)角位置、角速度及角加速度隨時間變化的的軌跡，并畫出關節(jié)角位置、角速度及角加速度隨時間變化的曲線。曲線。解：解： 根據所給約束條件，直接代入式根據所給約束條件，直接代入式(4-4)(4-4)，可得：，可得： a0=15， a1=0， a2=20， a3=-4.44 所求關節(jié)角的位置函數為：所求關節(jié)角的位置函數為： 2315204.44ttt對上式求導，可以得到角速度和角加速度對上式求導，可以得到角速度和角加速度 24013.334026.66ttttt（4-5）（4-6）（4-7） 根據式根據式(4-5)(4-5)(4-7)(4-7)可畫出它們隨時間的變可畫

17、出它們隨時間的變化曲線如下圖所示。由圖看出，速度曲線為一拋化曲線如下圖所示。由圖看出，速度曲線為一拋物線，加速度則為一直線。物線，加速度則為一直線。利用三次多項式規(guī)劃出的關節(jié)角的運動軌跡利用三次多項式規(guī)劃出的關節(jié)角的運動軌跡2 2）拋物線連接的線性函數插值）拋物線連接的線性函數插值 前面介紹了利用三次多項式函數插值的規(guī)劃方前面介紹了利用三次多項式函數插值的規(guī)劃方法。另外一種常用方法是線性函數插值法，即用一法。另外一種常用方法是線性函數插值法，即用一條直線將起點與終點連接起來。但是，簡單的線性條直線將起點與終點連接起來。但是，簡單的線性函數插值將使得關節(jié)的運動速度在起點和終點處不函數插值將使得關

18、節(jié)的運動速度在起點和終點處不連續(xù)，它也意味著需要產生無窮大的加速度，這顯連續(xù)，它也意味著需要產生無窮大的加速度，這顯然是不希望的。因此可以考慮在起點和終點處，用然是不希望的。因此可以考慮在起點和終點處，用拋物線與直線連接起來，在拋物線段內，使用恒定拋物線與直線連接起來，在拋物線段內，使用恒定的加速度來平滑地改變速度，從而使得整個運動軌的加速度來平滑地改變速度，從而使得整個運動軌跡的位置和速度是連續(xù)的。跡的位置和速度是連續(xù)的。線性函數插值圖線性函數插值圖利用拋物線過渡的線性函利用拋物線過渡的線性函數插值圖數插值圖空間法空間法 前面介紹的在關節(jié)空間內的規(guī)劃，可以保證運動前面介紹的在關節(jié)空間內的規(guī)劃

19、，可以保證運動軌跡經過給定的路徑點。但是在直角坐標空間，路徑軌跡經過給定的路徑點。但是在直角坐標空間，路徑點之間的軌跡形狀往往是十分復雜的，它取決于機械點之間的軌跡形狀往往是十分復雜的，它取決于機械手的運動學機構特性。在有些情況下，對機械手末端手的運動學機構特性。在有些情況下，對機械手末端的軌跡形狀也有一定要求，如要求它在兩點之間走一的軌跡形狀也有一定要求，如要求它在兩點之間走一條直線，或者沿著一個圓弧運動以繞過障礙物等。這條直線，或者沿著一個圓弧運動以繞過障礙物等。這時便需要在直角坐標空間內規(guī)劃機械手的運動軌跡時便需要在直角坐標空間內規(guī)劃機械手的運動軌跡 直角坐標空間的路徑點，指的是機械手末

20、端的工直角坐標空間的路徑點，指的是機械手末端的工具坐標相對于基坐標的位置和姿態(tài)每一個點由具坐標相對于基坐標的位置和姿態(tài)每一個點由6 6個個量組成，其中量組成，其中3 3個量描述位置，另外個量描述位置，另外3 3個量描述姿態(tài)。個量描述姿態(tài)。 在直角坐標空間內規(guī)劃的方法主要有：在直角坐標空間內規(guī)劃的方法主要有：線性函數線性函數插值法插值法和和圓弧插值法圓弧插值法。 前面軌跡規(guī)劃的任務，是根據給定的路徑點規(guī)前面軌跡規(guī)劃的任務，是根據給定的路徑點規(guī)劃出運動軌跡的所有參數。劃出運動軌跡的所有參數。 例如，在用三次多項式函數插值時，便是產生例如，在用三次多項式函數插值時，便是產生出多項式系數出多項式系數a

21、 a0 0,a,a1 1,a,a2 2,a,a3 3從而得到整個軌跡的運動從而得到整個軌跡的運動方程：方程： 230123iiiiq taa ta ta t 2123232326iiiiiq taa ta tq taa t對上式求導，可以得到速度和加速度對上式求導，可以得到速度和加速度 前面討論了在給定路徑點的情況下如何規(guī)劃出前面討論了在給定路徑點的情況下如何規(guī)劃出運動軌跡的問題。但是還有一個如何描述路徑點并運動軌跡的問題。但是還有一個如何描述路徑點并以合適的方式輸入給機器人的問題。最常用的方法以合適的方式輸入給機器人的問題。最常用的方法便是利用機器人語言。便是利用機器人語言。 用戶將要求實現

22、的動作編成相應的應用程序，用戶將要求實現的動作編成相應的應用程序，其中有相應的語句用來描述軌跡規(guī)劃，并通過相應其中有相應的語句用來描述軌跡規(guī)劃，并通過相應的控制作用來實現期望的運動。的控制作用來實現期望的運動。 根據不同的分類方法，機器人控制方式可以有根據不同的分類方法，機器人控制方式可以有不同的分類。下圖是一種常用的分類方法。從總體不同的分類。下圖是一種常用的分類方法。從總體上，機器人的控制方式可以分為上，機器人的控制方式可以分為動作控制方式動作控制方式和和示示教控制方式教控制方式。 按照被控對象來分，可以分為按照被控對象來分，可以分為位置控制、速度位置控制、速度控制、加速度控制、力控制、力

23、矩控制、力和位置控制、加速度控制、力控制、力矩控制、力和位置混合控制混合控制等等。等等。 無論是位置控制或速度控制，從伺服反饋信號無論是位置控制或速度控制，從伺服反饋信號的形式來看，又可以分為的形式來看，又可以分為基于關節(jié)空間的伺服控制基于關節(jié)空間的伺服控制和和基于作業(yè)空間基于作業(yè)空間( (手部坐標手部坐標) )的伺服控制的伺服控制。舉例：舉例：關節(jié)伺服控制關節(jié)伺服控制 這是以關節(jié)位置或關節(jié)軌跡為目標值的控制形式。下圖這是以關節(jié)位置或關節(jié)軌跡為目標值的控制形式。下圖給出了關節(jié)伺服系統的構成。若目標值以關節(jié)位移給出，則給出了關節(jié)伺服系統的構成。若目標值以關節(jié)位移給出，則如下圖所示，各關節(jié)可以獨立

24、構成伺服系統，十分簡單。如下圖所示，各關節(jié)可以獨立構成伺服系統，十分簡單。diqdiq4.3 4.3 機器人控制系統的一般構成機器人控制系統的一般構成 從前面關于服務機器人的規(guī)劃過程的介紹中可知，對于從前面關于服務機器人的規(guī)劃過程的介紹中可知，對于一個具有高度智能的機器人，它的控制實際上包含了一個具有高度智能的機器人，它的控制實際上包含了“任務任務規(guī)劃規(guī)劃”、“動作規(guī)劃動作規(guī)劃”、“軌跡規(guī)劃軌跡規(guī)劃”和基于規(guī)模的和基于規(guī)模的“伺服伺服控制控制”等多個層次，如下圖所示等多個層次，如下圖所示, ,機器人首先要機器人首先要通過人機接通過人機接口獲取操作者的指令口獲取操作者的指令，指令的形式可以是人的

25、自然語言，或，指令的形式可以是人的自然語言，或者是由人發(fā)出的專用的指令語言者是由人發(fā)出的專用的指令語言( (用在大部分服務機器人上用在大部分服務機器人上) )，也可以是通過示教工具輸入的示教指令，也可以是通過示教工具輸入的示教指令( (如一般的示教控如一般的示教控制機器人制機器人) )，或者鍵盤輸人的機器人指令語言以及計算機程，或者鍵盤輸人的機器人指令語言以及計算機程序指令序指令( (加大部分工業(yè)機器人加大部分工業(yè)機器人) )，然后機器人對控制命令進行，然后機器人對控制命令進行解釋理解解釋理解，把操作者的命令分解為機器人可以實現的，把操作者的命令分解為機器人可以實現的“任務任務”，這是，這是任

26、務規(guī)劃任務規(guī)劃。機器人針對各個任務進行動作分解，這。機器人針對各個任務進行動作分解，這是是動作規(guī)劃動作規(guī)劃。為了實現機器人的一系列動作，應該對機器人。為了實現機器人的一系列動作，應該對機器人每個關節(jié)的運動進行設計，這是機器人的每個關節(jié)的運動進行設計，這是機器人的軌跡規(guī)劃軌跡規(guī)劃。最底層。最底層是是關節(jié)運動的伺服控制關節(jié)運動的伺服控制。機器人控制系統的硬件構成機器人控制系統的硬件構成(1)機器人控制系統的硬件構成機器人控制系統的硬件構成(2)PUMA560機器人控制系統機器人控制系統 基本功能構成基本功能構成 記憶功能：記憶功能：存儲作業(yè)順序、運動路徑、運動方式、運動速度和存儲作業(yè)順序、運動路徑

27、、運動方式、運動速度和 與生產工藝有關的信息。與生產工藝有關的信息。 示教功能：示教功能：離線編程，在線示教，間接示教。離線編程，在線示教，間接示教。 在線示教包括示教盒和導引示教兩種。在線示教包括示教盒和導引示教兩種。 與外圍設備聯系功能：與外圍設備聯系功能：輸入和輸出接口、通信接口、網絡接口、同步接口。輸入和輸出接口、通信接口、網絡接口、同步接口。 坐標設置功能：坐標設置功能：有關節(jié)、絕對、工具、用戶自定義四種坐標系。有關節(jié)、絕對、工具、用戶自定義四種坐標系。 人機接口：人機接口：示教盒、操作面板、顯示屏。示教盒、操作面板、顯示屏。 傳感器接口：傳感器接口：位置檢測、視覺、觸覺、力覺等位置檢測、視覺、觸覺、力覺等 位置伺服功能：位置伺服功能：機器人多軸聯動、運動控制、速度和加速度控制、動態(tài)補償等。機器人多軸聯動、運動控制、速度和加速度控制、動態(tài)補償等。 故障診斷安全保護功能：故障診斷安全保護功能：運行時系統狀態(tài)監(jiān)視、故障狀態(tài)下的安全保護和運行時系統狀態(tài)監(jiān)視、故障狀態(tài)下的安全保護和 故障自診斷。故障自診斷。 （1）控制計算機控制計算機 控制系統