《機(jī)器人路徑規(guī)劃》由會員分享，可在線閱讀，更多相關(guān)《機(jī)器人路徑規(guī)劃（18頁珍藏版）》請在裝配圖網(wǎng)上搜索。

1、1 第7章 機(jī)器人路徑規(guī)劃在機(jī)器人完成指定任務(wù)時，需要規(guī)劃機(jī)器人在空間中的期望運(yùn)動軌跡或者路徑。 路徑和軌跡是兩個相似但含義不同的概念，機(jī)器人運(yùn)動的路徑描述機(jī)器人的位姿隨空間的變化，而機(jī)器人運(yùn)動的軌跡描述機(jī)器人的位姿隨時間的變化。 所謂軌跡是指機(jī)器人每個自由度的位置、速度和加速度的時間歷程。 本章將介紹移動機(jī)器人路徑規(guī)劃和機(jī)械臂的軌跡規(guī)劃問題。u移動機(jī)器人路徑規(guī)劃 移動機(jī)器人路徑規(guī)劃的任務(wù): 已知機(jī)器人初始位姿、給定機(jī)器人的目標(biāo)位姿，在存在障礙的環(huán)境中規(guī)劃一條無碰撞、時間（能量）最優(yōu)的路徑。 若已知環(huán)境地圖，即已知機(jī)器人模型和障礙模型，可采用基于模型的路徑規(guī)劃。 若機(jī)器人在未知或動態(tài)環(huán)境中移動

2、，機(jī)器人需要向目標(biāo)移動、同時需要使用傳感器探測障礙，稱為基于傳感器的路徑規(guī)劃。 為了簡化問題描述，假定機(jī)器人為兩個自由度，即只考慮機(jī)器人的位置，不考慮其姿態(tài)。 2 圖7-1障礙物擴(kuò)張法路徑規(guī)劃 任務(wù)是規(guī)劃一條路徑，使得機(jī)器人從起點達(dá)到目標(biāo)點（終點），同時不與環(huán)境中的障礙發(fā)生碰撞。 以平面全向移動機(jī)器人為例，假設(shè)機(jī)器人為半徑為r的圓形機(jī)構(gòu)。 首先，由于機(jī)器人可以全方向移動，所以可以忽略移動機(jī)器人的方向（姿態(tài)的自由度）。 其次，因為能用園表示機(jī)器人，所以可把障礙物沿徑向擴(kuò)張r的寬度，同時將機(jī)器人收縮成一個點（如圖7-1所示）。 因此，移動機(jī)器人路徑規(guī)劃可以簡化為在擴(kuò)張了障礙物的地圖上，點機(jī)器人的路

3、徑規(guī)劃問題。 3 u人工勢場方法 人工勢場的基本思想是構(gòu)造目標(biāo)位置引力場和障礙物周圍斥力場共同作用下的人工勢場。 搜索勢函數(shù)的下降方向來尋找無碰撞路徑。 下面給出各種勢場的定義 目標(biāo)引力場 212att goalE K -p p p 7-1 其中p是機(jī)器人位置，pgoal是目標(biāo)位置，K是引力常數(shù)。圖7-2機(jī)器人受力示意圖障礙物斥力場 2 001 12 0 obsrep obs dE d else - - - p pp p p 7-2其中pobs是障礙物位置，d0表示障礙物的影響范圍，h是斥力常數(shù)。根據(jù)（7-1）式，機(jī)器人受到的引力表示為 att att goalE K- -F p p p 7-

4、3 4 圖7-2機(jī)器人受力示意圖 根據(jù)（7-2）式，機(jī)器人在障礙物的影響范圍內(nèi)受到的斥力表示為 301 1 obsrep obs obsd - - - - p pF p p p p p 7-4 可得機(jī)器人所受合力為： Ftotal =Fatt + Fobs (7-5)這樣，我們就在環(huán)境地圖中定義了機(jī)器人的引力場 因此，機(jī)器人的路徑規(guī)劃問題被轉(zhuǎn)化為點在引力場中的運(yùn)動問題。 而點在引力場中的運(yùn)動問題在物理學(xué)和數(shù)學(xué)中已經(jīng)研究得非常清楚，可以比較方便地進(jìn)行求解。 算法優(yōu)點：簡單方便，可以實時規(guī)劃控制，并能考慮多個障礙，連續(xù)移動。規(guī)劃的路徑比較平滑安全。 5 算法缺點：規(guī)劃算法是局部最優(yōu)算法 復(fù)雜多障礙

5、環(huán)境中可能出現(xiàn)局部極值點，即在非目標(biāo)點達(dá)到平衡狀態(tài)而停滯。因而不能規(guī)劃出達(dá)到目標(biāo)點的路徑。 前面只介紹了基本的人工勢場方法。近年來，針對基本人工勢場方法的的不足，人們提出了許多改進(jìn)的人工勢場方法。 圖7-3柵格法路徑規(guī)劃示意圖u柵格法 柵格法的基本思想： 將機(jī)器人工作空間劃分為多個簡單區(qū)域，稱為柵格。 若柵格內(nèi)沒有障礙物稱為自由柵格，否則稱障礙柵格。 將柵格編號，機(jī)器人路徑規(guī)劃就是搜索由起點到目標(biāo)點的自由柵格組成的連通域。 可以用柵格序號表示，再將柵格序號轉(zhuǎn)換成機(jī)器人空間的實際坐標(biāo)，令機(jī)器人按此路徑運(yùn)動。 圖7-3給出了柵格法路徑規(guī)劃的示意圖。 6 柵格法路徑規(guī)劃步驟：1.建立柵格。將機(jī)器人和

6、目標(biāo)點間區(qū)域劃分柵格，大小與機(jī)器人相關(guān)。 2.障礙地圖生成。標(biāo)注障礙柵格和自由柵格。 3.搜索無障礙最優(yōu)路徑，AA*搜索算法，遺傳算法，人工勢場，蟻群算法等。 優(yōu)點：若存在最優(yōu)路徑，算法得當(dāng)一定可以得到問題最優(yōu)解。有成熟的路徑搜索算法使用。 缺點：柵格粒度影響較大。劃分細(xì)時，存貯大和搜索時間長。得到的是折線，需要光滑處理。 7 u機(jī)械臂路徑規(guī)劃 在實際問題當(dāng)中，一般用工具坐標(biāo)系T相對工作臺坐標(biāo)系S的運(yùn)動來描述機(jī)械臂的運(yùn)動。 當(dāng)用工具坐標(biāo)系T相對工作臺坐標(biāo)系S的運(yùn)動來描述機(jī)械臂的路徑時，使得路徑規(guī)劃與具體的機(jī)械臂、末端執(zhí)行器和工件相分離。 這種規(guī)劃方法具有通用性，適合不同的機(jī)械臂和工具，同時也適

7、用于運(yùn)動的工作臺（如傳送帶）。 在進(jìn)行機(jī)械臂路徑規(guī)劃時，經(jīng)常需要規(guī)劃運(yùn)動的細(xì)節(jié)，而不是簡單地指定期望的終端位姿。 例如，一個完整的操作由若干步組成，每一步的都有期望的位姿，或者在機(jī)械臂運(yùn)動過程中需要規(guī)避障礙等。 解決該問題的方法是在規(guī)劃的路徑中增加一系列的中間點。 為了完成整個運(yùn)動，工具坐標(biāo)系必須通過中間點所描述的一系列過渡位姿。 稱路徑的起點、中間點和終點為路徑點。 通常都期望機(jī)械臂的運(yùn)動過程是平滑的，因此一般要求規(guī)劃的路徑是光滑的，至少具有連續(xù)的一階導(dǎo)數(shù)，甚至要求二階導(dǎo)數(shù)也是連續(xù)的。 一階導(dǎo)數(shù)對應(yīng)機(jī)械臂的運(yùn)動速度，二階導(dǎo)數(shù)對應(yīng)加速度。 光滑性要求就是要使機(jī)械臂的運(yùn)動更加平穩(wěn)，避免突然的劇烈

8、加速或者減速產(chǎn)生沖擊作用而影響機(jī)械臂的運(yùn)動精度并加劇機(jī)構(gòu)的磨損。 8 u關(guān)節(jié)空間規(guī)劃方法 前面介紹了機(jī)械臂的期望運(yùn)動一般由指定的路徑點來描述，其中的每個點都代表工具坐標(biāo)系T相對工作臺坐標(biāo)系S的位姿。 我們可以采用第4章介紹的逆運(yùn)動學(xué)方法獲得這些路徑點對應(yīng)的關(guān)節(jié)角度。 規(guī)定機(jī)械臂的關(guān)節(jié)同步運(yùn)動，即每個關(guān)節(jié)角都同時達(dá)到路徑點期望的角度。 上述規(guī)定就是在相鄰路徑點之間每個關(guān)節(jié)的運(yùn)行時間都是相等的。 這樣，我們可以獨立規(guī)劃每個關(guān)節(jié)的軌跡、關(guān)節(jié)之間沒有影響。因此，機(jī)械臂軌跡規(guī)劃問題可以分解為n個獨立的單關(guān)節(jié)軌跡規(guī)劃問題。 單區(qū)間三次多項式插值 任務(wù)就是確定函數(shù)q (t)，使其在t =0時刻的值為關(guān)節(jié)角的

9、初始位置，在t =t f時刻的值為關(guān)節(jié)角的目標(biāo)位置。 另外，一般要求在初始時刻和終止時刻關(guān)節(jié)的速度均為零。 因此，關(guān)節(jié)軌跡規(guī)劃在數(shù)學(xué)上就是滿足4個約束條件的函數(shù)插值問題。 多項式插值比較簡單，同時便于計算，因此常用來解決函數(shù)插值問題。滿足4個約束條件的多項式函數(shù)插值函數(shù)是三次多項式。 9 位置約束：0(0)( )f ftq qq q 7-8 速度約束：(0) 0( ) 0ftqq 7-9 關(guān)節(jié)角軌跡可以用三次多項式表示為：2 30 1 2 3( )t a a t a t a tq 7-10 關(guān)節(jié)角速度和加速度軌跡可以表示為： 21 2 32 3( ) 2 3( ) 2 6t a a t a t

10、t a a tqq 7-11 把四個約束條件式7-8和式7-9代入到式7-10和式7-11得：0 0 2 3 0 1 2 31 21 2 300 2 3f f f ff faa a t a t a taa a t a tqq 7-12 其解為： 0 01 0aa q 2 023 033 ( )2 ( )ff ffa ta t q qq q - -滿足約束條件的三次多項式：2 30 0 02 33 2( ) ( ) ( )f ff ft t tt tq q q q q q - - -例7-1 假設(shè)一個具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，處于靜止?fàn)顟B(tài)時，關(guān)節(jié)角q 15o。期望在2秒內(nèi)平滑移動到關(guān)節(jié)角q

11、75o的目標(biāo)位置，并在目標(biāo)位置處于靜止?fàn)顟B(tài)。求滿足約束條件的三次多項式，并畫出關(guān)節(jié)角位置、速度和加速度隨時間變化的曲線。 10 解：tf =2，位置和速度約束分別為：0(0) 15( ) 75f ftq qq q (0) 0( ) 0ftqq 代入到 7-14式得關(guān)節(jié)角軌跡：2 3( ) 15 45 15t t tq - 2 30 0 02 33 2( ) ( ) ( )f ff ft t tt tq q q q q q - - -關(guān)節(jié)角速度和加速度軌跡為：2( ) 90 45( ) 90 90t t tt tqq - - 圖7-4關(guān)節(jié)角位置、速度和加速度隨時間變化的曲線可以看出，起點和終點的

12、角度值等于指定角度，且速度為零，加速度線性變化。 11 u具有中間點的三次多項式插值 一般情況下，機(jī)械臂需要連續(xù)經(jīng)過若干中間點，因此需要建立滿足這些約束的插值函數(shù)。若仍然采用三次多項式插值函數(shù)，則必須采用分段插值方法。 即在相鄰路徑點組成的每個區(qū)間內(nèi)進(jìn)行三次多項式插值，同時要求在兩段曲線的連接處滿足一定的光滑條件。比較簡單的做法是，指定中間點關(guān)節(jié)角的位置和速度，這樣每個區(qū)間可以獨立進(jìn)行插值計算。 這樣與單區(qū)間三次多項式插值基本類似，只是7-9式的速度約束一般不為零，而是指定的速度： 0(0) ( )f ftq q q q 滿足約束條件的三次多項式系數(shù)的解 0 0 1 0a aq q 2 0 0

13、23 0 03 23 2 1( )2 1( ) ( )f ff f ff ff fa t t ta t tq q q qq q q q - - - - 例7-2假設(shè)一個具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，起始點和終止點速度為零，且位置滿足 q0 =15o，qf =45o。設(shè)置一個中間點，位置和速度分別為 qm =75o和 。 10deg/smq -假設(shè)兩段區(qū)間的長度均為2秒。求滿足約束條件的分段三次多項式，并畫出關(guān)節(jié)角位置、速度和加速度隨時間變化的曲線。 12 解：先計算起始點到中間點的三次多項式。其中tf =2，位置和速度約束分別為：0(0) 15( ) 75f ftq qq q (0) 0( )

14、 10ftqq -關(guān)節(jié)角軌跡：2 3( ) 15 50 17.5t t tq -關(guān)節(jié)角速度和加速度軌跡為：2( ) 100 52.5 ( ) 100 105t t t t tq q - - 中間點到終止點的關(guān)節(jié)角軌跡： 2 3( ) 75 10 12.5 5t t t tq - - 關(guān)節(jié)角速度和加速度軌跡為：2( ) 10 25 15 ( ) 25 30t t t t tq q- - - 13 u具有拋物線擬合的線性插值 連接相鄰兩個路徑點的最簡單曲線是直線，因此希望采用線性插值，但線性插值在連接點處速度不連續(xù)。 獲得速度連續(xù)光滑曲線的方法是在直線段兩端采用拋物線擬合段。因為拋物線是二次函數(shù)，

15、所以在擬合段內(nèi)加速度為常數(shù)。 假設(shè)兩端拋物線擬合段的加速度數(shù)值相等（符號相反），如圖所示，滿足條件的解不唯一，但每個解都是關(guān)于時間中點tm和位置中點qm對稱的。 拋物線和直線的連接點處的速度相同，而整個直線段內(nèi)速度是常值，所以 m bb m bt t tq qq q - - 其中qb是tb時刻的角度值， 是擬合段加速度值， 是直線段速度值。 q q 20 12b btq q q 7-187-19 將7-19式代入到7-18式，并且注意到tm=t/2，qm=(q0+qf)/2可以得到以下關(guān)系式2 0 0b b ft ttq q q q- - t是期望運(yùn)行時間。 04( )/f tq q q -

16、通常先選擇加速度，在計算時間 tb 14 u具有中間點的三次樣條插值 那么能否得到不需要指定中間點的速度，同時在整個運(yùn)行時間內(nèi)位置、速度和加速度都是連續(xù)的插值函數(shù)？ 前面介紹的具有中間點的三次多項式插值方法需要指定中間點的速度，同時在中間點處的加速度不連續(xù)。答案是肯定的，方法就是采用樣條插值技術(shù)。 下面針對關(guān)節(jié)角軌跡規(guī)劃問題，給出常用的三次樣條插值函數(shù)的定義。在機(jī)械臂運(yùn)行區(qū)間0, tf上取n+1個時間節(jié)點0=t0 t1 t2 tn-1 tn=tf 給出這些點處關(guān)節(jié)角位置函數(shù)的n+1個值（路徑點）q i，i=0,1,2,n。要求構(gòu)造一個三次樣條插值函數(shù)q(t)，滿足以下條件：1）q(ti)=qi

17、，i=0,1,2,n。2）在每個子區(qū)間ti, ti+1上，q(ti)是三次多項式。 3）q(t)在整個運(yùn)行區(qū)間0, tf上具有二階連續(xù)導(dǎo)數(shù)。 從上面的定義可以發(fā)現(xiàn)，樣條插值函數(shù)q(t)是分段三次多項式，即在每個子區(qū)間上都是一個三次多項式。 確定樣條插值函數(shù)q(t)只需要n+1個路徑點qi值，而不需要其導(dǎo)數(shù)(速度)值，且在整個機(jī)械臂運(yùn)行時間內(nèi)樣條插值函數(shù)q(t)的二階導(dǎo)數(shù)都是連續(xù)的。 15 u三次樣條插值函數(shù)q(t)的存在性 因為每個區(qū)間都是一個三次多項式，因此有4個未知量，有n個子區(qū)間，所以未知量個數(shù)共有4n 個。 每個區(qū)間兩個端點的函數(shù)值是事先指定的，所以有2n 個約束， n-1個中間點處的

18、一階和二階導(dǎo)數(shù)（速度和加速度）連續(xù)，所以有2(n-1) 個約束。 約束的個數(shù)為2n+2(n-1)=4n -2因此，單從三次樣條插值函數(shù)q(t)的定義不能唯一確定函數(shù)本身。 對于機(jī)械臂軌跡規(guī)劃問題，一般要求初始點和終止點的速度（一階導(dǎo)數(shù)）為零，恰好補(bǔ)充兩個約束條件。因此，三次樣條插值函數(shù)q(t)可以唯一確定。 2 310 11 12 13( )t a a t a t a tq 2 320 21 22 23( )t a a t a t a tq 下面以具有一個中間點的關(guān)節(jié)角軌跡規(guī)劃問題為例驗證三次樣條插值方法，假定兩段區(qū)間的長度均為tf，指定中間點關(guān)節(jié)角的位置。兩段函數(shù)假設(shè)為解為： 10 011

19、212 0 313 00(12 3 9 )/(4 )( 8 3 5 )/(4 )m f fm f faaa ta tq q q qq q q - - - 2021 0 222 0 323 0(3 3 )/(4 )( 12 6 6 )/(4 )(8 5 3 )/(4 )m f fm f fm f faa ta ta tqq qq q qq q q - - - - 16 例7-3假設(shè)一個具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，起始點和終止點速度為零，且位置滿足 q0 =15o，qf =45o。設(shè)置一個中間點，位置為qm =75o。假設(shè)兩段區(qū)間的長度均為2秒。求滿足約束條件的三次樣條插值多項式，并畫出關(guān)節(jié)角位置、速度和加速度隨時間變化的曲線。 17 u笛卡爾空間規(guī)劃方法 前面介紹的關(guān)節(jié)空間規(guī)劃方法可以保證機(jī)械臂能夠達(dá)到中間點和目標(biāo)點。但是連接這些點的中間路徑在笛卡爾空間可能是非常復(fù)雜的，其復(fù)雜程度取決于機(jī)械臂的運(yùn)動學(xué)結(jié)構(gòu)。 如果我們關(guān)心機(jī)械臂在笛卡爾空間的整個路徑，而不僅僅是關(guān)鍵點，如期望工具直線運(yùn)動、畫圓等，就需要采用笛卡爾空間規(guī)劃方法。 在笛卡爾空間規(guī)劃路徑要比在關(guān)節(jié)空間規(guī)劃路徑困難，所以除非必要，一般均采用關(guān)節(jié)空間路徑規(guī)劃方法。由于機(jī)械臂驅(qū)動能力總是有限的，所以對關(guān)節(jié)的速度和加速度需要進(jìn)行限制。若規(guī)劃的軌跡不滿足要求，可以通過增加運(yùn)行時間的方法來減小關(guān)節(jié)的速度和加速度。 18