1 第7章 機(jī)器人路徑規(guī)劃在機(jī)器人完成指定任務(wù)時(shí)，需要規(guī)劃機(jī)器人在空間中的期望運(yùn)動(dòng)軌跡或者路徑。 路徑和軌跡是兩個(gè)相似但含義不同的概念，機(jī)器人運(yùn)動(dòng)的路徑描述機(jī)器人的位姿隨空間的變化，而機(jī)器人運(yùn)動(dòng)的軌跡描述機(jī)器人的位姿隨時(shí)間的變化。 所謂軌跡是指機(jī)器人每個(gè)自由度的位置、速度和加速度的時(shí)間歷程。 本章將介紹移動(dòng)機(jī)器人路徑規(guī)劃和機(jī)械臂的軌跡規(guī)劃問題。u移動(dòng)機(jī)器人路徑規(guī)劃 移動(dòng)機(jī)器人路徑規(guī)劃的任務(wù): 已知機(jī)器人初始位姿、給定機(jī)器人的目標(biāo)位姿，在存在障礙的環(huán)境中規(guī)劃一條無碰撞、時(shí)間（能量）最優(yōu)的路徑。 若已知環(huán)境地圖，即已知機(jī)器人模型和障礙模型，可采用基于模型的路徑規(guī)劃。 若機(jī)器人在未知或動(dòng)態(tài)環(huán)境中移動(dòng)，機(jī)器人需要向目標(biāo)移動(dòng)、同時(shí)需要使用傳感器探測(cè)障礙，稱為基于傳感器的路徑規(guī)劃。 為了簡化問題描述，假定機(jī)器人為兩個(gè)自由度，即只考慮機(jī)器人的位置，不考慮其姿態(tài)。 2 圖7-1障礙物擴(kuò)張法路徑規(guī)劃 任務(wù)是規(guī)劃一條路徑，使得機(jī)器人從起點(diǎn)達(dá)到目標(biāo)點(diǎn)（終點(diǎn)），同時(shí)不與環(huán)境中的障礙發(fā)生碰撞。 以平面全向移動(dòng)機(jī)器人為例，假設(shè)機(jī)器人為半徑為r的圓形機(jī)構(gòu)。 首先，由于機(jī)器人可以全方向移動(dòng)，所以可以忽略移動(dòng)機(jī)器人的方向（姿態(tài)的自由度）。 其次，因?yàn)槟苡脠@表示機(jī)器人，所以可把障礙物沿徑向擴(kuò)張r的寬度，同時(shí)將機(jī)器人收縮成一個(gè)點(diǎn)（如圖7-1所示）。 因此，移動(dòng)機(jī)器人路徑規(guī)劃可以簡化為在擴(kuò)張了障礙物的地圖上，點(diǎn)機(jī)器人的路徑規(guī)劃問題。 3 u人工勢(shì)場(chǎng)方法 人工勢(shì)場(chǎng)的基本思想是構(gòu)造目標(biāo)位置引力場(chǎng)和障礙物周圍斥力場(chǎng)共同作用下的人工勢(shì)場(chǎng)。 搜索勢(shì)函數(shù)的下降方向來尋找無碰撞路徑。 下面給出各種勢(shì)場(chǎng)的定義 目標(biāo)引力場(chǎng) 212att goalE K -p p p 7-1 其中p是機(jī)器人位置，pgoal是目標(biāo)位置，K是引力常數(shù)。圖7-2機(jī)器人受力示意圖障礙物斥力場(chǎng) 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-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ī)器人的引力場(chǎng) 因此，機(jī)器人的路徑規(guī)劃問題被轉(zhuǎn)化為點(diǎn)在引力場(chǎng)中的運(yùn)動(dòng)問題。 而點(diǎn)在引力場(chǎng)中的運(yùn)動(dòng)問題在物理學(xué)和數(shù)學(xué)中已經(jīng)研究得非常清楚，可以比較方便地進(jìn)行求解。 算法優(yōu)點(diǎn)：簡單方便，可以實(shí)時(shí)規(guī)劃控制，并能考慮多個(gè)障礙，連續(xù)移動(dòng)。規(guī)劃的路徑比較平滑安全。 5 算法缺點(diǎn)：規(guī)劃算法是局部最優(yōu)算法 復(fù)雜多障礙環(huán)境中可能出現(xiàn)局部極值點(diǎn)，即在非目標(biāo)點(diǎn)達(dá)到平衡狀態(tài)而停滯。因而不能規(guī)劃出達(dá)到目標(biāo)點(diǎn)的路徑。 前面只介紹了基本的人工勢(shì)場(chǎng)方法。近年來，針對(duì)基本人工勢(shì)場(chǎng)方法的的不足，人們提出了許多改進(jìn)的人工勢(shì)場(chǎng)方法。 圖7-3柵格法路徑規(guī)劃示意圖u柵格法 柵格法的基本思想： 將機(jī)器人工作空間劃分為多個(gè)簡單區(qū)域，稱為柵格。 若柵格內(nèi)沒有障礙物稱為自由柵格，否則稱障礙柵格。 將柵格編號(hào)，機(jī)器人路徑規(guī)劃就是搜索由起點(diǎn)到目標(biāo)點(diǎn)的自由柵格組成的連通域。 可以用柵格序號(hào)表示，再將柵格序號(hào)轉(zhuǎn)換成機(jī)器人空間的實(shí)際坐標(biāo)，令機(jī)器人按此路徑運(yùn)動(dòng)。 圖7-3給出了柵格法路徑規(guī)劃的示意圖。 6 柵格法路徑規(guī)劃步驟：1.建立柵格。將機(jī)器人和目標(biāo)點(diǎn)間區(qū)域劃分柵格，大小與機(jī)器人相關(guān)。 2.障礙地圖生成。標(biāo)注障礙柵格和自由柵格。 3.搜索無障礙最優(yōu)路徑，AA*搜索算法，遺傳算法，人工勢(shì)場(chǎng)，蟻群算法等。 優(yōu)點(diǎn)：若存在最優(yōu)路徑，算法得當(dāng)一定可以得到問題最優(yōu)解。有成熟的路徑搜索算法使用。 缺點(diǎn)：柵格粒度影響較大。劃分細(xì)時(shí)，存貯大和搜索時(shí)間長。得到的是折線，需要光滑處理。 7 u機(jī)械臂路徑規(guī)劃 在實(shí)際問題當(dāng)中，一般用工具坐標(biāo)系T相對(duì)工作臺(tái)坐標(biāo)系S的運(yùn)動(dòng)來描述機(jī)械臂的運(yùn)動(dòng)。 當(dāng)用工具坐標(biāo)系T相對(duì)工作臺(tái)坐標(biāo)系S的運(yùn)動(dòng)來描述機(jī)械臂的路徑時(shí)，使得路徑規(guī)劃與具體的機(jī)械臂、末端執(zhí)行器和工件相分離。 這種規(guī)劃方法具有通用性，適合不同的機(jī)械臂和工具，同時(shí)也適用于運(yùn)動(dòng)的工作臺(tái)（如傳送帶）。 在進(jìn)行機(jī)械臂路徑規(guī)劃時(shí)，經(jīng)常需要規(guī)劃運(yùn)動(dòng)的細(xì)節(jié)，而不是簡單地指定期望的終端位姿。 例如，一個(gè)完整的操作由若干步組成，每一步的都有期望的位姿，或者在機(jī)械臂運(yùn)動(dòng)過程中需要規(guī)避障礙等。 解決該問題的方法是在規(guī)劃的路徑中增加一系列的中間點(diǎn)。 為了完成整個(gè)運(yùn)動(dòng)，工具坐標(biāo)系必須通過中間點(diǎn)所描述的一系列過渡位姿。 稱路徑的起點(diǎn)、中間點(diǎn)和終點(diǎn)為路徑點(diǎn)。 通常都期望機(jī)械臂的運(yùn)動(dòng)過程是平滑的，因此一般要求規(guī)劃的路徑是光滑的，至少具有連續(xù)的一階導(dǎo)數(shù)，甚至要求二階導(dǎo)數(shù)也是連續(xù)的。 一階導(dǎo)數(shù)對(duì)應(yīng)機(jī)械臂的運(yùn)動(dòng)速度，二階導(dǎo)數(shù)對(duì)應(yīng)加速度。 光滑性要求就是要使機(jī)械臂的運(yùn)動(dòng)更加平穩(wěn)，避免突然的劇烈加速或者減速產(chǎn)生沖擊作用而影響機(jī)械臂的運(yùn)動(dòng)精度并加劇機(jī)構(gòu)的磨損。 8 u關(guān)節(jié)空間規(guī)劃方法 前面介紹了機(jī)械臂的期望運(yùn)動(dòng)一般由指定的路徑點(diǎn)來描述，其中的每個(gè)點(diǎn)都代表工具坐標(biāo)系T相對(duì)工作臺(tái)坐標(biāo)系S的位姿。 我們可以采用第4章介紹的逆運(yùn)動(dòng)學(xué)方法獲得這些路徑點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度。 規(guī)定機(jī)械臂的關(guān)節(jié)同步運(yùn)動(dòng)，即每個(gè)關(guān)節(jié)角都同時(shí)達(dá)到路徑點(diǎn)期望的角度。 上述規(guī)定就是在相鄰路徑點(diǎn)之間每個(gè)關(guān)節(jié)的運(yùn)行時(shí)間都是相等的。 這樣，我們可以獨(dú)立規(guī)劃每個(gè)關(guān)節(jié)的軌跡、關(guān)節(jié)之間沒有影響。因此，機(jī)械臂軌跡規(guī)劃問題可以分解為n個(gè)獨(dú)立的單關(guān)節(jié)軌跡規(guī)劃問題。 單區(qū)間三次多項(xiàng)式插值 任務(wù)就是確定函數(shù)q (t)，使其在t =0時(shí)刻的值為關(guān)節(jié)角的初始位置，在t =t f時(shí)刻的值為關(guān)節(jié)角的目標(biāo)位置。 另外，一般要求在初始時(shí)刻和終止時(shí)刻關(guān)節(jié)的速度均為零。 因此，關(guān)節(jié)軌跡規(guī)劃在數(shù)學(xué)上就是滿足4個(gè)約束條件的函數(shù)插值問題。 多項(xiàng)式插值比較簡單，同時(shí)便于計(jì)算，因此常用來解決函數(shù)插值問題。滿足4個(gè)約束條件的多項(xiàng)式函數(shù)插值函數(shù)是三次多項(xiàng)式。 9 位置約束：0(0)( )f ftq qq q 7-8 速度約束：(0) 0( ) 0ftqq 7-9 關(guān)節(jié)角軌跡可以用三次多項(xiàng)式表示為：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 tt a a tqq 7-11 把四個(gè)約束條件式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 - -滿足約束條件的三次多項(xiàng)式：2 30 0 02 33 2( ) ( ) ( )f ff ft t tt tq q q q q q - - -例7-1 假設(shè)一個(gè)具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，處于靜止?fàn)顟B(tài)時(shí)，關(guān)節(jié)角q 15o。期望在2秒內(nèi)平滑移動(dòng)到關(guān)節(jié)角q 75o的目標(biāo)位置，并在目標(biāo)位置處于靜止?fàn)顟B(tài)。求滿足約束條件的三次多項(xiàng)式，并畫出關(guān)節(jié)角位置、速度和加速度隨時(shí)間變化的曲線。 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é)角位置、速度和加速度隨時(shí)間變化的曲線可以看出，起點(diǎn)和終點(diǎn)的角度值等于指定角度，且速度為零，加速度線性變化。 11 u具有中間點(diǎn)的三次多項(xiàng)式插值 一般情況下，機(jī)械臂需要連續(xù)經(jīng)過若干中間點(diǎn)，因此需要建立滿足這些約束的插值函數(shù)。若仍然采用三次多項(xiàng)式插值函數(shù)，則必須采用分段插值方法。 即在相鄰路徑點(diǎn)組成的每個(gè)區(qū)間內(nèi)進(jìn)行三次多項(xiàng)式插值，同時(shí)要求在兩段曲線的連接處滿足一定的光滑條件。比較簡單的做法是，指定中間點(diǎn)關(guān)節(jié)角的位置和速度，這樣每個(gè)區(qū)間可以獨(dú)立進(jìn)行插值計(jì)算。 這樣與單區(qū)間三次多項(xiàng)式插值基本類似，只是7-9式的速度約束一般不為零，而是指定的速度： 0(0) ( )f ftq q q q 滿足約束條件的三次多項(xiàng)式系數(shù)的解 0 0 1 0a aq q 2 0 023 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è)一個(gè)具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，起始點(diǎn)和終止點(diǎn)速度為零，且位置滿足 q0 =15o，qf =45o。設(shè)置一個(gè)中間點(diǎn)，位置和速度分別為 qm =75o和 。 10deg/smq -假設(shè)兩段區(qū)間的長度均為2秒。求滿足約束條件的分段三次多項(xiàng)式，并畫出關(guān)節(jié)角位置、速度和加速度隨時(shí)間變化的曲線。 12 解：先計(jì)算起始點(diǎn)到中間點(diǎn)的三次多項(xiàng)式。其中tf =2，位置和速度約束分別為：0(0) 15( ) 75f ftq qq q (0) 0( ) 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 - - 中間點(diǎn)到終止點(diǎn)的關(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具有拋物線擬合的線性插值 連接相鄰兩個(gè)路徑點(diǎn)的最簡單曲線是直線，因此希望采用線性插值，但線性插值在連接點(diǎn)處速度不連續(xù)。 獲得速度連續(xù)光滑曲線的方法是在直線段兩端采用拋物線擬合段。因?yàn)閽佄锞€是二次函數(shù)，所以在擬合段內(nèi)加速度為常數(shù)。 假設(shè)兩端拋物線擬合段的加速度數(shù)值相等（符號(hào)相反），如圖所示，滿足條件的解不唯一，但每個(gè)解都是關(guān)于時(shí)間中點(diǎn)tm和位置中點(diǎn)qm對(duì)稱的。 拋物線和直線的連接點(diǎn)處的速度相同，而整個(gè)直線段內(nèi)速度是常值，所以 m bb m bt t tq qq q - - 其中qb是tb時(shí)刻的角度值， 是擬合段加速度值， 是直線段速度值。 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)行時(shí)間。 04( )/f tq q q - 通常先選擇加速度，在計(jì)算時(shí)間 tb 14 u具有中間點(diǎn)的三次樣條插值 那么能否得到不需要指定中間點(diǎn)的速度，同時(shí)在整個(gè)運(yùn)行時(shí)間內(nèi)位置、速度和加速度都是連續(xù)的插值函數(shù)？ 前面介紹的具有中間點(diǎn)的三次多項(xiàng)式插值方法需要指定中間點(diǎn)的速度，同時(shí)在中間點(diǎn)處的加速度不連續(xù)。答案是肯定的，方法就是采用樣條插值技術(shù)。 下面針對(duì)關(guān)節(jié)角軌跡規(guī)劃問題，給出常用的三次樣條插值函數(shù)的定義。在機(jī)械臂運(yùn)行區(qū)間0, tf上取n+1個(gè)時(shí)間節(jié)點(diǎn)0=t0 t1 t2 tn-1 tn=tf 給出這些點(diǎn)處關(guān)節(jié)角位置函數(shù)的n+1個(gè)值（路徑點(diǎn)）q i，i=0,1,2,n。要求構(gòu)造一個(gè)三次樣條插值函數(shù)q(t)，滿足以下條件：1）q(ti)=qi，i=0,1,2,n。2）在每個(gè)子區(qū)間ti, ti+1上，q(ti)是三次多項(xiàng)式。 3）q(t)在整個(gè)運(yùn)行區(qū)間0, tf上具有二階連續(xù)導(dǎo)數(shù)。 從上面的定義可以發(fā)現(xiàn)，樣條插值函數(shù)q(t)是分段三次多項(xiàng)式，即在每個(gè)子區(qū)間上都是一個(gè)三次多項(xiàng)式。 確定樣條插值函數(shù)q(t)只需要n+1個(gè)路徑點(diǎn)qi值，而不需要其導(dǎo)數(shù)(速度)值，且在整個(gè)機(jī)械臂運(yùn)行時(shí)間內(nèi)樣條插值函數(shù)q(t)的二階導(dǎo)數(shù)都是連續(xù)的。 15 u三次樣條插值函數(shù)q(t)的存在性 因?yàn)槊總€(gè)區(qū)間都是一個(gè)三次多項(xiàng)式，因此有4個(gè)未知量，有n個(gè)子區(qū)間，所以未知量個(gè)數(shù)共有4n 個(gè)。 每個(gè)區(qū)間兩個(gè)端點(diǎn)的函數(shù)值是事先指定的，所以有2n 個(gè)約束， n-1個(gè)中間點(diǎn)處的一階和二階導(dǎo)數(shù)（速度和加速度）連續(xù)，所以有2(n-1) 個(gè)約束。 約束的個(gè)數(shù)為2n+2(n-1)=4n -2因此，單從三次樣條插值函數(shù)q(t)的定義不能唯一確定函數(shù)本身。 對(duì)于機(jī)械臂軌跡規(guī)劃問題，一般要求初始點(diǎn)和終止點(diǎn)的速度（一階導(dǎo)數(shù)）為零，恰好補(bǔ)充兩個(gè)約束條件。因此，三次樣條插值函數(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 下面以具有一個(gè)中間點(diǎn)的關(guān)節(jié)角軌跡規(guī)劃問題為例驗(yàn)證三次樣條插值方法，假定兩段區(qū)間的長度均為tf，指定中間點(diǎn)關(guān)節(jié)角的位置。兩段函數(shù)假設(shè)為解為： 10 011 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è)一個(gè)具有單旋轉(zhuǎn)關(guān)節(jié)單自由度機(jī)器人，起始點(diǎn)和終止點(diǎn)速度為零，且位置滿足 q0 =15o，qf =45o。設(shè)置一個(gè)中間點(diǎn)，位置為qm =75o。假設(shè)兩段區(qū)間的長度均為2秒。求滿足約束條件的三次樣條插值多項(xiàng)式，并畫出關(guān)節(jié)角位置、速度和加速度隨時(shí)間變化的曲線。 17 u笛卡爾空間規(guī)劃方法 前面介紹的關(guān)節(jié)空間規(guī)劃方法可以保證機(jī)械臂能夠達(dá)到中間點(diǎn)和目標(biāo)點(diǎn)。但是連接這些點(diǎn)的中間路徑在笛卡爾空間可能是非常復(fù)雜的，其復(fù)雜程度取決于機(jī)械臂的運(yùn)動(dòng)學(xué)結(jié)構(gòu)。 如果我們關(guān)心機(jī)械臂在笛卡爾空間的整個(gè)路徑，而不僅僅是關(guān)鍵點(diǎn)，如期望工具直線運(yùn)動(dòng)、畫圓等，就需要采用笛卡爾空間規(guī)劃方法。 在笛卡爾空間規(guī)劃路徑要比在關(guān)節(jié)空間規(guī)劃路徑困難，所以除非必要，一般均采用關(guān)節(jié)空間路徑規(guī)劃方法。由于機(jī)械臂驅(qū)動(dòng)能力總是有限的，所以對(duì)關(guān)節(jié)的速度和加速度需要進(jìn)行限制。若規(guī)劃的軌跡不滿足要求，可以通過增加運(yùn)行時(shí)間的方法來減小關(guān)節(jié)的速度和加速度。 18