物流工程 73 路徑規(guī)劃課件

7.3路徑規(guī)劃7.3路徑規(guī)劃AGV智能化的新發(fā)展在于自主回避障礙物并達(dá)到目的地的路徑規(guī)劃。首先，影響路徑規(guī)劃的是AGV的自由度數(shù)和地圖的有無。為了便于理解，以2自由度AGV為例，分別考慮有地圖時(shí)(環(huán)境已知時(shí))和沒有地圖時(shí)(環(huán)境未知時(shí))的路徑·動(dòng)作規(guī)劃。這里，假定AGV只考慮兩個(gè)位置自由度(X、Y軸上的位置)，不考慮姿態(tài)方面的一個(gè)自由度(繞中心的回轉(zhuǎn))7.3路徑規(guī)劃7.3路徑規(guī)劃17.3路徑規(guī)劃

由于地圖是由AGV和障礙物的模型，所以有地圖時(shí)的路徑規(guī)劃稱為基于模型的路徑規(guī)劃(Model-basedPath-Planning)。基于模型的路徑規(guī)劃稱為離線路徑規(guī)劃。

沒有地圖時(shí)的路徑規(guī)劃，AGV用外部傳感器(視覺、超聲波傳感器、光傳感器等)得到一面回避障礙一面到達(dá)目的地的路徑，由此稱為基于傳感器的路徑規(guī)劃(Sensor-basedPath-Planning)?；趥鞲衅鞯穆窂揭?guī)劃又稱為在線路徑規(guī)劃。7.3路徑規(guī)劃27.3路徑規(guī)劃基于模型的路徑規(guī)劃首先說明為了快速選擇最佳(最短)路徑，應(yīng)采用怎樣的數(shù)據(jù)結(jié)構(gòu)來表現(xiàn)地圖。最佳(最短)路徑由于接近障礙物，如果有位置誤差，AGV與障礙物碰撞的可能性很高。下面要說明的是，為防止碰撞，除了最佳性以外更重視安全性的方法，即為了選擇離障礙物足夠遠(yuǎn)的安全路徑，應(yīng)采用怎樣的數(shù)據(jù)結(jié)構(gòu)來表現(xiàn)地圖。這里由于采用一種OR表。7.3路徑規(guī)劃37.3路徑規(guī)劃基于傳感器的路徑規(guī)劃AGV用傳感器一面檢測(cè)障礙物一面進(jìn)行回避并最終到達(dá)目的地的路徑規(guī)劃算法，即使存在位置誤差，AGV也不會(huì)迷失確定的路徑，而最終到達(dá)目的地附近。7.3路徑規(guī)劃47.3路徑規(guī)劃7.3.1

路徑規(guī)劃的模型1．幾何模型AGV有2個(gè)位置自由度(X、Y軸上的位置)和1個(gè)姿態(tài)自由度(繞中心的旋轉(zhuǎn))。這里，取可全方位移動(dòng)的AGV為例加以說明。由于AGV能全方位移動(dòng)，所以可忽略AGV的方向(姿態(tài)的自由度)。這樣，就能用以最大回轉(zhuǎn)長(zhǎng)度為半徑的圓表示AGV。在此基礎(chǔ)上，可以把障礙物的幾何尺寸徑向擴(kuò)張一個(gè)AGV圓的半徑，同時(shí)把AGV縮成一個(gè)點(diǎn)(圖7-16)。由此，在存在擴(kuò)張了的障礙物的地圖(XY平面)上，可以規(guī)劃成為幾何點(diǎn)的AGV的路徑。7.3路徑規(guī)劃7.3.1路徑規(guī)劃的模型57.3路徑規(guī)劃7.3路徑規(guī)劃67.3路徑規(guī)劃2．?dāng)?shù)學(xué)模型

首先，說明基于模型的路徑規(guī)劃。為了快速選取路徑，用所謂圖的數(shù)據(jù)結(jié)構(gòu)表示規(guī)劃的數(shù)學(xué)模型(俗稱“地圖”)。所謂圖就是用弧連接節(jié)點(diǎn)的數(shù)據(jù)結(jié)構(gòu)，節(jié)點(diǎn)意味著AGV的位置，弧意味著兩個(gè)位置間的移動(dòng)(圖7-17)。將移動(dòng)的幾何距離、工作量或時(shí)間加權(quán)折算得出兩個(gè)位置間移動(dòng)的模型費(fèi)用，把模型費(fèi)用希望值作為費(fèi)用賦于該兩個(gè)位置間的弧上。7.3路徑規(guī)劃2．?dāng)?shù)學(xué)模型

首先，說明基于模型的路徑規(guī)劃。77.3路徑規(guī)劃ABEDCF9圖7-17

圖(節(jié)點(diǎn)和弧)7.3路徑規(guī)劃ABEDCF9圖7-17圖(節(jié)點(diǎn)和弧)87.3路徑規(guī)劃當(dāng)所有節(jié)點(diǎn)間移動(dòng)的工作量不變時(shí)，弧上賦于的費(fèi)用可以直接用幾何距離標(biāo)記?；∮洃涍M(jìn)入節(jié)點(diǎn)和輸出節(jié)點(diǎn)，總是回到原來的地方(程序上稱為“指針返回”)。而且，如果能在兩個(gè)方向移動(dòng)則用無向弧，只能單方向移動(dòng)的用有向弧。7.3路徑規(guī)劃97.3路徑規(guī)劃7.3.2

基于模型的路徑規(guī)劃這里介紹兩個(gè)典型的圖。一個(gè)是管理從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最短路徑的切線圖(TangentialGraph)，另一個(gè)是連接這些節(jié)點(diǎn)的安全路徑，即管理盡量離開障礙物路徑的Voronoi圖(VoronoiGraph)。無論哪一種圖都是由節(jié)點(diǎn)和弧構(gòu)成的，用節(jié)點(diǎn)表示起始點(diǎn)、經(jīng)過點(diǎn)、目標(biāo)點(diǎn)；用無向弧表示其間的路徑，其上附加有作為費(fèi)用的歐幾里得距離。最后，無論哪個(gè)圖，都是用算法A選出任意路徑，用算法A*選出最佳(滿足)路徑。7.3路徑規(guī)劃7.3.2基于模型的路徑規(guī)劃107.3路徑規(guī)劃1．切線圖切線圖用障礙物的切線表示弧。由此可選擇從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳(最短)路徑。即切線圖是把障礙物邊界切線化得到的。從起始節(jié)點(diǎn)ns開始，過兩相鄰節(jié)點(diǎn)向障礙物邊界作切線，每?jī)蓷l切線的交點(diǎn)形成輔助節(jié)點(diǎn)。由主節(jié)點(diǎn)(起始點(diǎn)、經(jīng)過點(diǎn)、目標(biāo)點(diǎn))、輔助節(jié)點(diǎn)和節(jié)點(diǎn)間連線組成了切線圖。首先把對(duì)應(yīng)起始點(diǎn)S和目標(biāo)點(diǎn)G的兩個(gè)節(jié)點(diǎn)ns和ng標(biāo)注在新的切線圖上，然后用算法A*選出最佳(最短)路徑P，7.3路徑規(guī)劃1．切線圖117.3路徑規(guī)劃

最后，使點(diǎn)AGV沿著路徑P進(jìn)行PTP(Point-To-Point)控制和CP(ContinuousPath)控制，把AGV引導(dǎo)到目的地。

如果在這種控制過程中產(chǎn)生位置誤差，機(jī)器人碰撞障礙物的可能性會(huì)較高，因?yàn)锳GV幾乎接近障礙物行走

7.3路徑規(guī)劃如果在這種控制過程中產(chǎn)生位置誤差，機(jī)器人127.3路徑規(guī)劃2．Voronoi圖Voronoi圖可用弧表示距兩個(gè)以上障礙物和墻壁表面等距離的點(diǎn)陣，用節(jié)點(diǎn)表示它們的交叉位置。首先把對(duì)應(yīng)起始點(diǎn)S和目標(biāo)點(diǎn)G的起始節(jié)點(diǎn)ns和目標(biāo)節(jié)點(diǎn)ng標(biāo)注在圖上，然后用搜索算法A*選出安全路徑P，最后，使點(diǎn)AGV沿著路徑P進(jìn)行PTP控制和CP控制，把AGV引導(dǎo)到目的地。由于選擇的是安全路徑，所以，即使產(chǎn)生位置誤差，AGV也能夠在離障礙物足夠遠(yuǎn)的路徑上走行。7.3路徑規(guī)劃2．Voronoi圖137.3路徑規(guī)劃7.3路徑規(guī)劃147.3路徑規(guī)劃3.搜索算法A*(A)這里要介紹的是，把前面所說的切線圖和Voronoi圖作為搜索圖G，選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳(或滿足)路徑的算法A*(或選出任意路徑的算法A)。算法A*(或A)一面計(jì)算節(jié)點(diǎn)n的費(fèi)用f(n)，一面搜索圖G。費(fèi)用f(n)是從起始節(jié)點(diǎn)ns經(jīng)由當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)ng的最小費(fèi)用(最短距離)的估價(jià)函數(shù)。7.3路徑規(guī)劃3.搜索算法A*(A)157.3路徑規(guī)劃可用下式計(jì)算：f（n）=g(n)+h(n)式中，g(n)是起始節(jié)點(diǎn)ns和當(dāng)前節(jié)點(diǎn)n之間的現(xiàn)時(shí)點(diǎn)上的最小費(fèi)用(最短距離)；而h(n)是當(dāng)前節(jié)點(diǎn)n和目標(biāo)節(jié)點(diǎn)ng之間的最小費(fèi)用h*(n)的估計(jì)值，稱為啟發(fā)式函值。OPEN是管理以后擴(kuò)展節(jié)點(diǎn)的明細(xì)表，所有節(jié)點(diǎn)按費(fèi)用f（n）遞增順序排列，CLOSED是管理已擴(kuò)展節(jié)點(diǎn)的明細(xì)表。7.3路徑規(guī)劃可用下式計(jì)算：167.3路徑規(guī)劃通常A*(或A)等搜索算法，從節(jié)點(diǎn)n擴(kuò)展的所有節(jié)點(diǎn)n’中，把必要的節(jié)點(diǎn)同費(fèi)用f(n’)都標(biāo)注OPEN上(參看算法的第⑤步)，這個(gè)操作稱為“擴(kuò)展節(jié)點(diǎn)n”。算法A*(A)的程序流程說明：①把起始節(jié)點(diǎn)ns代入OPEN。②如果OPEN是空表，由于路徑不存在，所以算法終止。③從OPEN取出先頭(費(fèi)用f最小)的節(jié)點(diǎn)n，并把它移到CLOSED。7.3路徑規(guī)劃通常A*(或A)等搜索算法，從節(jié)點(diǎn)n擴(kuò)展的所177.3路徑規(guī)劃④如果節(jié)點(diǎn)n是目標(biāo)節(jié)點(diǎn)ng，則順次返回到來自的節(jié)點(diǎn)上(程序上是追尋(返回)指針)。然后，若是到達(dá)起始節(jié)點(diǎn)ns，則終止算法，得到一個(gè)路徑。⑤

如果不是這樣，擴(kuò)展節(jié)點(diǎn)n，把指針從其子孫節(jié)點(diǎn)n’返回到節(jié)點(diǎn)n(記住從哪來的)。然后，對(duì)所有的子孫節(jié)點(diǎn)n’做以下工作:7.3路徑規(guī)劃④如果節(jié)點(diǎn)n是目標(biāo)節(jié)點(diǎn)ng，則順次返回到來自187.3路徑規(guī)劃節(jié)點(diǎn)n’如果不在OPEN或CLOSED表中，則它就是新的搜索節(jié)點(diǎn)。因此，首先計(jì)算估計(jì)值h(n’)(從節(jié)點(diǎn)n’到節(jié)點(diǎn)ng的最短距離的估計(jì)值).其次,計(jì)算評(píng)價(jià)值f(n’)=g(n’)+h(n’)(這里g(n’)=g(n)+c(n、n’),g(ns)=0,c(n、n’)是連接節(jié)點(diǎn)n和n’弧的費(fèi)用)。然后，把節(jié)點(diǎn)n’同估計(jì)值f（n’）都代入OPEN。若節(jié)點(diǎn)n’存在于OPEN或CLOSED表中，則它就是已被搜索的節(jié)點(diǎn)。于是，把指針換到帶來最小值g(n’)的路徑上(變更來自的地方)。然后，在這個(gè)指針發(fā)生替換時(shí)，若節(jié)點(diǎn)n’存在于CLOSED中，則把它返回到OPEN后，再計(jì)算值f(n’)=g(n’)+h(n’)。7.3路徑規(guī)劃節(jié)點(diǎn)n’如果不在OPEN或CLOSED表中，197.3路徑規(guī)劃返回到②

算法A*(A)的程序流程7.3路徑規(guī)劃返回到②207.3路徑規(guī)劃估計(jì)值h比真值h*小或相等時(shí)，上述的算法變?yōu)锳*，可選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳路徑(總計(jì)費(fèi)用最小的路徑)。

若估計(jì)值h比真值大，

h*算法則變?yōu)锳，可選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的滿足要求的路徑(總計(jì)費(fèi)用不是最小的路徑)。

因此，機(jī)器人的路徑規(guī)劃多用從當(dāng)前地點(diǎn)(,,)到目的地(,)的平方范數(shù)7.3路徑規(guī)劃估計(jì)值h比真值h*小或相等時(shí)，上述的算法變?yōu)?17.3路徑規(guī)劃

定義估計(jì)值。這個(gè)估計(jì)值h常常比h*小，成為算法A*，用它可選擇最佳路徑。

圖7-20的搜索圖G存在估計(jì)值h(在節(jié)點(diǎn)上用括號(hào)給出)比真值h*大的節(jié)點(diǎn)。例如，節(jié)點(diǎn)A和H的估計(jì)值h是8和4，但到達(dá)目的地的最小真值是7和2。由于這個(gè)費(fèi)用評(píng)價(jià)過大，存在于最短路徑上的節(jié)點(diǎn)H等可以忽略，算法錯(cuò)過了費(fèi)用8的最佳路徑，最終得到費(fèi)用9的滿足要求的路徑。7.3路徑規(guī)劃定義估計(jì)值。這個(gè)估計(jì)值h常常比h*小，227.3路徑規(guī)劃EDFGHIBCAS(7)32(5)(5)(5)(3)(8)(1)(4)(3)221332322441圖7-20

搜索圖G(有的地方估計(jì)值比真值大)7.3路徑規(guī)劃EDFGHIBCAS(7)32(5)(5)(237.3路徑規(guī)劃7.3路徑規(guī)劃24A、起始節(jié)點(diǎn)S被代入OPEN[圖7-21(a)]，子節(jié)點(diǎn)A和B被代人OPEN[圖7-21(b)]。起始節(jié)點(diǎn)S擴(kuò)展后移到CLOSED，B、由于節(jié)點(diǎn)A、B的評(píng)價(jià)值分別為10、8，所以選中擴(kuò)展節(jié)點(diǎn)B。B的子節(jié)點(diǎn)D、E、F，評(píng)價(jià)值分別為9、8、10，全都代人OPEN，擴(kuò)展后節(jié)點(diǎn)B被移到CLOSED[圖7-21(c)]。C、節(jié)點(diǎn)E評(píng)價(jià)值最小，選中擴(kuò)展節(jié)點(diǎn)E。E的子節(jié)點(diǎn)H同評(píng)價(jià)值10都代入OPEN，擴(kuò)展后節(jié)點(diǎn)E被移到CLOSED。7.3路徑規(guī)劃A、起始節(jié)點(diǎn)S被代入OPEN[圖7-21(a)]，子節(jié)點(diǎn)A和25D、OPEN中當(dāng)前評(píng)價(jià)值最小的子節(jié)點(diǎn)是D，所以選中節(jié)點(diǎn)D。D的子節(jié)點(diǎn)H被再次搜索

，節(jié)點(diǎn)D被移到CLOSED。注意到節(jié)點(diǎn)H的值g，由于過去的費(fèi)用6(經(jīng)由節(jié)點(diǎn)E、B返回到節(jié)點(diǎn)S)比新的費(fèi)用7(經(jīng)由節(jié)點(diǎn)D、B返回到S)小，所以不更換指針[圖7-21(d)]。指針仍在E。由于當(dāng)前OPEN上存在評(píng)價(jià)值f都為10的三個(gè)節(jié)點(diǎn)A、H、F，此時(shí)須對(duì)這三個(gè)節(jié)點(diǎn)A、H、F都分別向下擴(kuò)展。7.3路徑規(guī)劃7.3路徑規(guī)劃26E、用中斷連接節(jié)點(diǎn)A，擴(kuò)展節(jié)點(diǎn)C同評(píng)價(jià)值8都代人OPEN，節(jié)點(diǎn)A被移到CLOSED。然后，C的兩個(gè)擴(kuò)展節(jié)點(diǎn)I、H中，選中值f最小的節(jié)點(diǎn)I（實(shí)際上，ACH路徑在先已經(jīng)被否定），節(jié)點(diǎn)I同評(píng)價(jià)值10都代人OPEN，節(jié)點(diǎn)C同評(píng)價(jià)值8都被移到CLOSED。節(jié)點(diǎn)I的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，

OPEN保持F、節(jié)點(diǎn)H的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持7.3路徑規(guī)劃E、用中斷連接節(jié)點(diǎn)A，擴(kuò)展節(jié)點(diǎn)C同評(píng)價(jià)值8都代人OPEN，節(jié)27G、節(jié)點(diǎn)F的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持H、當(dāng)前OPEN上三個(gè)節(jié)點(diǎn)I、H、F的評(píng)價(jià)值都為10,其后的擴(kuò)展節(jié)點(diǎn)都是目標(biāo)G。計(jì)算分別經(jīng)三個(gè)節(jié)點(diǎn)F、H、I到目標(biāo)G的評(píng)價(jià)值,經(jīng)節(jié)點(diǎn)F到目標(biāo)G的評(píng)價(jià)值最小。所以用中斷連接擴(kuò)展節(jié)點(diǎn)F，節(jié)點(diǎn)G連同評(píng)價(jià)值9都代人OPEN，節(jié)點(diǎn)F移到CLOSED[圖7-21(e)]。7.3路徑規(guī)劃G、節(jié)點(diǎn)F的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持7.3路徑規(guī)劃28在圖7-22的搜索圖G上，所有節(jié)點(diǎn)的估計(jì)值h(在節(jié)點(diǎn)上用括號(hào)給出)常常比真值h*小或相等。

7.3路徑規(guī)劃在圖7-22的搜索圖G上，所有節(jié)點(diǎn)的估計(jì)值h(在297.3路徑規(guī)劃7.3路徑規(guī)劃30A、起始節(jié)點(diǎn)S被代入OPEN，子節(jié)點(diǎn)A、B連同評(píng)價(jià)值f6、8被代人OPEN，起始節(jié)點(diǎn)S擴(kuò)展后移到CLOSED。B、由于節(jié)點(diǎn)A、B的評(píng)價(jià)值f分別為6、8，所以節(jié)點(diǎn)A擴(kuò)展子節(jié)點(diǎn)C、D后移到CLOSED，節(jié)點(diǎn)B、C、D連同各自的評(píng)價(jià)值8、8、9都代入OPEN。7.3路徑規(guī)劃A、起始節(jié)點(diǎn)S被代入OPEN，子節(jié)點(diǎn)A、B連同評(píng)價(jià)值f6、831C、用中斷連接擴(kuò)展節(jié)點(diǎn)B，節(jié)點(diǎn)B擴(kuò)展子節(jié)點(diǎn)E、F。子節(jié)點(diǎn)E、F連同評(píng)價(jià)值8、9都代入OPEN，節(jié)點(diǎn)B擴(kuò)展后代入CLOSED。再次搜索節(jié)點(diǎn)D。這時(shí)，如果注意到節(jié)點(diǎn)D的值g，由于新的費(fèi)用值4(經(jīng)由節(jié)點(diǎn)B返回到節(jié)點(diǎn)S)比過去的費(fèi)用5(經(jīng)由節(jié)點(diǎn)A返回到節(jié)點(diǎn)S)小，所以要更換指針，重新計(jì)算的評(píng)價(jià)值f變?yōu)?[圖7-23(a)]。D、仍然用中斷連接擴(kuò)展節(jié)點(diǎn)C，節(jié)點(diǎn)C擴(kuò)展子節(jié)點(diǎn)I、H。子節(jié)點(diǎn)I、H連同評(píng)價(jià)值10、9都代入OPEN，節(jié)點(diǎn)C移到CLOSED[圖7-23(b)]，7.3路徑規(guī)劃C、用中斷連接擴(kuò)展節(jié)點(diǎn)B，節(jié)點(diǎn)B擴(kuò)展子節(jié)點(diǎn)E、F。子節(jié)點(diǎn)E、32E、當(dāng)前OPEN上五個(gè)節(jié)點(diǎn)I、H、

D、

E

、F。所以選中評(píng)價(jià)值f最小的擴(kuò)展節(jié)點(diǎn)E。然后，把節(jié)點(diǎn)E代入CLOSED，再次搜索節(jié)點(diǎn)H。F、這時(shí)，注意到節(jié)點(diǎn)H的值g，比起過去的費(fèi)用7(經(jīng)由節(jié)點(diǎn)C，A返回節(jié)點(diǎn)S)來新的費(fèi)用6(經(jīng)由節(jié)點(diǎn)E、B返回到節(jié)點(diǎn)S)要小，所以更換指針，重新計(jì)算H的評(píng)價(jià)值f為8[圖7-23(c)]7.3路徑規(guī)劃E、當(dāng)前OPEN上五個(gè)節(jié)點(diǎn)I、H、D、E、F。所以選33G、當(dāng)前OPEN上擴(kuò)展值f最小的節(jié)點(diǎn)H，其后的擴(kuò)展節(jié)點(diǎn)是目標(biāo)G，節(jié)點(diǎn)G以評(píng)價(jià)值8代人OPEN，節(jié)點(diǎn)H代入CLOSEDH、最后，節(jié)點(diǎn)G如果選擇作為值f最小的節(jié)點(diǎn)，則把指針返回到節(jié)點(diǎn)H、E、B、S，最終得到費(fèi)用8的最短路徑。這里，由于節(jié)點(diǎn)H的估計(jì)值h與真值h*相比過小，所以這個(gè)最佳路徑上的節(jié)點(diǎn)必須調(diào)整。7.3路徑規(guī)劃G、當(dāng)前OPEN上擴(kuò)展值f最小的節(jié)點(diǎn)H，其后的擴(kuò)展節(jié)點(diǎn)是目標(biāo)344．AGV的路徑規(guī)劃實(shí)例Dijkstra最短路徑確定法。圖7-24中的節(jié)點(diǎn)1～5是AGV運(yùn)行路線上的岔口，6～18是貨物裝卸點(diǎn)，6是系統(tǒng)的入口，9、15、16是系統(tǒng)的出口，19是AGV停車處。在圖7-24(b)中，將AGV可能的運(yùn)行方向用箭頭進(jìn)行了表示。7.3路徑規(guī)劃4．AGV的路徑規(guī)劃實(shí)例7.3路徑規(guī)劃357.3路徑規(guī)劃7.3路徑規(guī)劃36(1)因?yàn)楣?jié)點(diǎn)1～5是找出6～18貨物裝卸點(diǎn)間最短距離的關(guān)鍵點(diǎn)，故先只考慮系統(tǒng)中的各岔口即節(jié)點(diǎn)l～5間的最短路徑。為此，將相應(yīng)箭頭所表示的距離填人下面的距離矩陣。因節(jié)點(diǎn)1、3之間無直接連接的箭頭，但兩者之間可通過16、18連在一起，所以C1,3=C1,16+C16,18+Cl8,3=84。C3,4=∞表示所連兩點(diǎn)間無直接聯(lián)系，之間的聯(lián)系要通過岔口節(jié)點(diǎn)。由此得到系統(tǒng)的距離矩陣(表7-2)。7.3路徑規(guī)劃(1)因?yàn)楣?jié)點(diǎn)1～5是找出6～18貨物裝卸點(diǎn)間最短距離的37表7-2距離矩陣7.3路徑規(guī)劃表7-2距離矩陣7.3路徑規(guī)劃38(2)搜索過程A、最短路徑法首先要求找到搜索的起始點(diǎn)i，起始點(diǎn)i必須是通過一個(gè)單箭頭就能和其余節(jié)點(diǎn)相連最多的節(jié)點(diǎn)。很顯然，圖7-24(b)中的節(jié)點(diǎn)1符合這個(gè)要求，將此節(jié)點(diǎn)作為第一層。B、在找出由該i點(diǎn)出發(fā)的最短距離Ci，k后，令di，k=Ci，k再以k節(jié)點(diǎn)作為第2次搜索的起點(diǎn)。7.3路徑規(guī)劃(2)搜索過程7.3路徑規(guī)劃39C、根據(jù)距離矩陣中與k節(jié)點(diǎn)對(duì)應(yīng)的行，計(jì)算由起始點(diǎn)出發(fā)，經(jīng)此k點(diǎn)到其余節(jié)點(diǎn)的最短距離，若到節(jié)點(diǎn)j的距離最短，則計(jì)算di，k+Ck，j

D、若di，k+Ck，j>Ci，j

再取ij路徑的j點(diǎn)作為第3次搜索的起點(diǎn)。

重復(fù)上面搜索過程（將j替換k）E、若di，k+Ck，j<Ci，j令di，j=di，k+Ck，j

，再取ikj路徑的j點(diǎn)作為第3次搜索的起點(diǎn)。如此往返進(jìn)行，直到所有節(jié)點(diǎn)均被選上為止。

最短路徑法的整個(gè)搜索過程見表7-3，最短路徑計(jì)算結(jié)果見圖7-25，細(xì)節(jié)見表7-4。7.3路徑規(guī)劃C、根據(jù)距離矩陣中與k節(jié)點(diǎn)對(duì)應(yīng)的行，計(jì)算由起始點(diǎn)出發(fā)，經(jīng)此k40表7-3最短路徑法的搜索步驟7.3路徑規(guī)劃表7-3最短路徑法的搜索步驟7.3路徑規(guī)劃41表7-4最短路徑一覽表

7.3路徑規(guī)劃表7-4最短路徑一覽表7.3路徑規(guī)劃427.3.3基于傳感器的路徑規(guī)劃(1)基于傳感器的路徑規(guī)劃的指導(dǎo)思想:離障礙物一定距離時(shí)，實(shí)施變向避障,同時(shí)單調(diào)減少到達(dá)目的地的距離，從而保證AGV到達(dá)目的地A、首先AGV向目的地直進(jìn)，若是AGV受到障礙物阻礙，記錄碰撞點(diǎn)Hi,同時(shí)AGV順時(shí)針或反時(shí)針轉(zhuǎn)過障礙物。7.3路徑規(guī)劃7.3.3基于傳感器的路徑規(guī)劃7.3路徑規(guī)劃43B、如果目的地的方向有空位(脫離點(diǎn))，記錄該脫離點(diǎn)Li,若是脫離點(diǎn)Li比碰撞點(diǎn)Hi更接近目的地，AGV就離開障礙物向目的地直進(jìn)(圖7-26)。C、若是脫離點(diǎn)Li比碰撞點(diǎn)Hi更遠(yuǎn)離目的地，AGV就返回碰撞點(diǎn)Hi,反向搜索比碰撞點(diǎn)Hi更接近目的地的脫離點(diǎn)D、按照這個(gè)程序，脫離點(diǎn)Li和碰撞點(diǎn)Hi單調(diào)接近目的地GE、AGV可看成點(diǎn)機(jī)器人。點(diǎn)機(jī)器人可能與障礙物碰撞的領(lǐng)域Ri單調(diào)減少(領(lǐng)域R1→R2→R3→R4)。由此，AGV最終到達(dá)目的地G。7.3路徑規(guī)劃B、如果目的地的方向有空位(脫離點(diǎn))，記錄該脫離點(diǎn)Li,若是44算法Class2的程序：①

初始設(shè)定L0=S=R，i=0。②點(diǎn)機(jī)器人R向目的地直進(jìn)。如果點(diǎn)機(jī)器人到達(dá)目的地，由于得到了解，算法終止；如果向量RG被障礙物妨礙，取i=i+1，把當(dāng)前地點(diǎn)作為碰撞點(diǎn)Hi，執(zhí)行③。7.3路徑規(guī)劃算法Class2的程序：7.3路徑規(guī)劃45③點(diǎn)機(jī)器人R順時(shí)針或反時(shí)針方向繞過障礙物進(jìn)行追尋?！と绻c(diǎn)機(jī)器人到達(dá)目的地G，由于得到了解，算法終止；·如果歐幾里得距離|RG|比|HiG|小，并且向量RG不受障礙物妨礙，把當(dāng)前地點(diǎn)作為脫離點(diǎn)Li,,回到②；·若點(diǎn)機(jī)器人R返回到碰撞點(diǎn)Hi，由于不存在解，所以算法終止。7.3路徑規(guī)劃③點(diǎn)機(jī)器人R順時(shí)針或反時(shí)針方向繞過障礙物進(jìn)行追尋。7.3路467.3路徑規(guī)劃7.3路徑規(guī)劃477.3路徑規(guī)劃7.3路徑規(guī)劃AGV智能化的新發(fā)展在于自主回避障礙物并達(dá)到目的地的路徑規(guī)劃。首先，影響路徑規(guī)劃的是AGV的自由度數(shù)和地圖的有無。為了便于理解，以2自由度AGV為例，分別考慮有地圖時(shí)(環(huán)境已知時(shí))和沒有地圖時(shí)(環(huán)境未知時(shí))的路徑·動(dòng)作規(guī)劃。這里，假定AGV只考慮兩個(gè)位置自由度(X、Y軸上的位置)，不考慮姿態(tài)方面的一個(gè)自由度(繞中心的回轉(zhuǎn))7.3路徑規(guī)劃7.3路徑規(guī)劃487.3路徑規(guī)劃

由于地圖是由AGV和障礙物的模型，所以有地圖時(shí)的路徑規(guī)劃稱為基于模型的路徑規(guī)劃(Model-basedPath-Planning)?；谀Ｐ偷穆窂揭?guī)劃稱為離線路徑規(guī)劃。

沒有地圖時(shí)的路徑規(guī)劃，AGV用外部傳感器(視覺、超聲波傳感器、光傳感器等)得到一面回避障礙一面到達(dá)目的地的路徑，由此稱為基于傳感器的路徑規(guī)劃(Sensor-basedPath-Planning)。基于傳感器的路徑規(guī)劃又稱為在線路徑規(guī)劃。7.3路徑規(guī)劃497.3路徑規(guī)劃基于模型的路徑規(guī)劃首先說明為了快速選擇最佳(最短)路徑，應(yīng)采用怎樣的數(shù)據(jù)結(jié)構(gòu)來表現(xiàn)地圖。最佳(最短)路徑由于接近障礙物，如果有位置誤差，AGV與障礙物碰撞的可能性很高。下面要說明的是，為防止碰撞，除了最佳性以外更重視安全性的方法，即為了選擇離障礙物足夠遠(yuǎn)的安全路徑，應(yīng)采用怎樣的數(shù)據(jù)結(jié)構(gòu)來表現(xiàn)地圖。這里由于采用一種OR表。7.3路徑規(guī)劃507.3路徑規(guī)劃基于傳感器的路徑規(guī)劃AGV用傳感器一面檢測(cè)障礙物一面進(jìn)行回避并最終到達(dá)目的地的路徑規(guī)劃算法，即使存在位置誤差，AGV也不會(huì)迷失確定的路徑，而最終到達(dá)目的地附近。7.3路徑規(guī)劃517.3路徑規(guī)劃7.3.1

路徑規(guī)劃的模型1．幾何模型AGV有2個(gè)位置自由度(X、Y軸上的位置)和1個(gè)姿態(tài)自由度(繞中心的旋轉(zhuǎn))。這里，取可全方位移動(dòng)的AGV為例加以說明。由于AGV能全方位移動(dòng)，所以可忽略AGV的方向(姿態(tài)的自由度)。這樣，就能用以最大回轉(zhuǎn)長(zhǎng)度為半徑的圓表示AGV。在此基礎(chǔ)上，可以把障礙物的幾何尺寸徑向擴(kuò)張一個(gè)AGV圓的半徑，同時(shí)把AGV縮成一個(gè)點(diǎn)(圖7-16)。由此，在存在擴(kuò)張了的障礙物的地圖(XY平面)上，可以規(guī)劃成為幾何點(diǎn)的AGV的路徑。7.3路徑規(guī)劃7.3.1路徑規(guī)劃的模型527.3路徑規(guī)劃7.3路徑規(guī)劃537.3路徑規(guī)劃2．?dāng)?shù)學(xué)模型

首先，說明基于模型的路徑規(guī)劃。為了快速選取路徑，用所謂圖的數(shù)據(jù)結(jié)構(gòu)表示規(guī)劃的數(shù)學(xué)模型(俗稱“地圖”)。所謂圖就是用弧連接節(jié)點(diǎn)的數(shù)據(jù)結(jié)構(gòu)，節(jié)點(diǎn)意味著AGV的位置，弧意味著兩個(gè)位置間的移動(dòng)(圖7-17)。將移動(dòng)的幾何距離、工作量或時(shí)間加權(quán)折算得出兩個(gè)位置間移動(dòng)的模型費(fèi)用，把模型費(fèi)用希望值作為費(fèi)用賦于該兩個(gè)位置間的弧上。7.3路徑規(guī)劃2．?dāng)?shù)學(xué)模型

首先，說明基于模型的路徑規(guī)劃。547.3路徑規(guī)劃ABEDCF9圖7-17

圖(節(jié)點(diǎn)和弧)7.3路徑規(guī)劃ABEDCF9圖7-17圖(節(jié)點(diǎn)和弧)557.3路徑規(guī)劃當(dāng)所有節(jié)點(diǎn)間移動(dòng)的工作量不變時(shí)，弧上賦于的費(fèi)用可以直接用幾何距離標(biāo)記?；∮洃涍M(jìn)入節(jié)點(diǎn)和輸出節(jié)點(diǎn)，總是回到原來的地方(程序上稱為“指針返回”)。而且，如果能在兩個(gè)方向移動(dòng)則用無向弧，只能單方向移動(dòng)的用有向弧。7.3路徑規(guī)劃567.3路徑規(guī)劃7.3.2

基于模型的路徑規(guī)劃這里介紹兩個(gè)典型的圖。一個(gè)是管理從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最短路徑的切線圖(TangentialGraph)，另一個(gè)是連接這些節(jié)點(diǎn)的安全路徑，即管理盡量離開障礙物路徑的Voronoi圖(VoronoiGraph)。無論哪一種圖都是由節(jié)點(diǎn)和弧構(gòu)成的，用節(jié)點(diǎn)表示起始點(diǎn)、經(jīng)過點(diǎn)、目標(biāo)點(diǎn)；用無向弧表示其間的路徑，其上附加有作為費(fèi)用的歐幾里得距離。最后，無論哪個(gè)圖，都是用算法A選出任意路徑，用算法A*選出最佳(滿足)路徑。7.3路徑規(guī)劃7.3.2基于模型的路徑規(guī)劃577.3路徑規(guī)劃1．切線圖切線圖用障礙物的切線表示弧。由此可選擇從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳(最短)路徑。即切線圖是把障礙物邊界切線化得到的。從起始節(jié)點(diǎn)ns開始，過兩相鄰節(jié)點(diǎn)向障礙物邊界作切線，每?jī)蓷l切線的交點(diǎn)形成輔助節(jié)點(diǎn)。由主節(jié)點(diǎn)(起始點(diǎn)、經(jīng)過點(diǎn)、目標(biāo)點(diǎn))、輔助節(jié)點(diǎn)和節(jié)點(diǎn)間連線組成了切線圖。首先把對(duì)應(yīng)起始點(diǎn)S和目標(biāo)點(diǎn)G的兩個(gè)節(jié)點(diǎn)ns和ng標(biāo)注在新的切線圖上，然后用算法A*選出最佳(最短)路徑P，7.3路徑規(guī)劃1．切線圖587.3路徑規(guī)劃

最后，使點(diǎn)AGV沿著路徑P進(jìn)行PTP(Point-To-Point)控制和CP(ContinuousPath)控制，把AGV引導(dǎo)到目的地。

如果在這種控制過程中產(chǎn)生位置誤差，機(jī)器人碰撞障礙物的可能性會(huì)較高，因?yàn)锳GV幾乎接近障礙物行走

7.3路徑規(guī)劃如果在這種控制過程中產(chǎn)生位置誤差，機(jī)器人597.3路徑規(guī)劃2．Voronoi圖Voronoi圖可用弧表示距兩個(gè)以上障礙物和墻壁表面等距離的點(diǎn)陣，用節(jié)點(diǎn)表示它們的交叉位置。首先把對(duì)應(yīng)起始點(diǎn)S和目標(biāo)點(diǎn)G的起始節(jié)點(diǎn)ns和目標(biāo)節(jié)點(diǎn)ng標(biāo)注在圖上，然后用搜索算法A*選出安全路徑P，最后，使點(diǎn)AGV沿著路徑P進(jìn)行PTP控制和CP控制，把AGV引導(dǎo)到目的地。由于選擇的是安全路徑，所以，即使產(chǎn)生位置誤差，AGV也能夠在離障礙物足夠遠(yuǎn)的路徑上走行。7.3路徑規(guī)劃2．Voronoi圖607.3路徑規(guī)劃7.3路徑規(guī)劃617.3路徑規(guī)劃3.搜索算法A*(A)這里要介紹的是，把前面所說的切線圖和Voronoi圖作為搜索圖G，選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳(或滿足)路徑的算法A*(或選出任意路徑的算法A)。算法A*(或A)一面計(jì)算節(jié)點(diǎn)n的費(fèi)用f(n)，一面搜索圖G。費(fèi)用f(n)是從起始節(jié)點(diǎn)ns經(jīng)由當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)ng的最小費(fèi)用(最短距離)的估價(jià)函數(shù)。7.3路徑規(guī)劃3.搜索算法A*(A)627.3路徑規(guī)劃可用下式計(jì)算：f（n）=g(n)+h(n)式中，g(n)是起始節(jié)點(diǎn)ns和當(dāng)前節(jié)點(diǎn)n之間的現(xiàn)時(shí)點(diǎn)上的最小費(fèi)用(最短距離)；而h(n)是當(dāng)前節(jié)點(diǎn)n和目標(biāo)節(jié)點(diǎn)ng之間的最小費(fèi)用h*(n)的估計(jì)值，稱為啟發(fā)式函值。OPEN是管理以后擴(kuò)展節(jié)點(diǎn)的明細(xì)表，所有節(jié)點(diǎn)按費(fèi)用f（n）遞增順序排列，CLOSED是管理已擴(kuò)展節(jié)點(diǎn)的明細(xì)表。7.3路徑規(guī)劃可用下式計(jì)算：637.3路徑規(guī)劃通常A*(或A)等搜索算法，從節(jié)點(diǎn)n擴(kuò)展的所有節(jié)點(diǎn)n’中，把必要的節(jié)點(diǎn)同費(fèi)用f(n’)都標(biāo)注OPEN上(參看算法的第⑤步)，這個(gè)操作稱為“擴(kuò)展節(jié)點(diǎn)n”。算法A*(A)的程序流程說明：①把起始節(jié)點(diǎn)ns代入OPEN。②如果OPEN是空表，由于路徑不存在，所以算法終止。③從OPEN取出先頭(費(fèi)用f最小)的節(jié)點(diǎn)n，并把它移到CLOSED。7.3路徑規(guī)劃通常A*(或A)等搜索算法，從節(jié)點(diǎn)n擴(kuò)展的所647.3路徑規(guī)劃④如果節(jié)點(diǎn)n是目標(biāo)節(jié)點(diǎn)ng，則順次返回到來自的節(jié)點(diǎn)上(程序上是追尋(返回)指針)。然后，若是到達(dá)起始節(jié)點(diǎn)ns，則終止算法，得到一個(gè)路徑。⑤

如果不是這樣，擴(kuò)展節(jié)點(diǎn)n，把指針從其子孫節(jié)點(diǎn)n’返回到節(jié)點(diǎn)n(記住從哪來的)。然后，對(duì)所有的子孫節(jié)點(diǎn)n’做以下工作:7.3路徑規(guī)劃④如果節(jié)點(diǎn)n是目標(biāo)節(jié)點(diǎn)ng，則順次返回到來自657.3路徑規(guī)劃節(jié)點(diǎn)n’如果不在OPEN或CLOSED表中，則它就是新的搜索節(jié)點(diǎn)。因此，首先計(jì)算估計(jì)值h(n’)(從節(jié)點(diǎn)n’到節(jié)點(diǎn)ng的最短距離的估計(jì)值).其次,計(jì)算評(píng)價(jià)值f(n’)=g(n’)+h(n’)(這里g(n’)=g(n)+c(n、n’),g(ns)=0,c(n、n’)是連接節(jié)點(diǎn)n和n’弧的費(fèi)用)。然后，把節(jié)點(diǎn)n’同估計(jì)值f（n’）都代入OPEN。若節(jié)點(diǎn)n’存在于OPEN或CLOSED表中，則它就是已被搜索的節(jié)點(diǎn)。于是，把指針換到帶來最小值g(n’)的路徑上(變更來自的地方)。然后，在這個(gè)指針發(fā)生替換時(shí)，若節(jié)點(diǎn)n’存在于CLOSED中，則把它返回到OPEN后，再計(jì)算值f(n’)=g(n’)+h(n’)。7.3路徑規(guī)劃節(jié)點(diǎn)n’如果不在OPEN或CLOSED表中，667.3路徑規(guī)劃返回到②

算法A*(A)的程序流程7.3路徑規(guī)劃返回到②677.3路徑規(guī)劃估計(jì)值h比真值h*小或相等時(shí)，上述的算法變?yōu)锳*，可選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的最佳路徑(總計(jì)費(fèi)用最小的路徑)。

若估計(jì)值h比真值大，

h*算法則變?yōu)锳，可選出從起始節(jié)點(diǎn)ns到目標(biāo)節(jié)點(diǎn)ng的滿足要求的路徑(總計(jì)費(fèi)用不是最小的路徑)。

因此，機(jī)器人的路徑規(guī)劃多用從當(dāng)前地點(diǎn)(,,)到目的地(,)的平方范數(shù)7.3路徑規(guī)劃估計(jì)值h比真值h*小或相等時(shí)，上述的算法變?yōu)?87.3路徑規(guī)劃

定義估計(jì)值。這個(gè)估計(jì)值h常常比h*小，成為算法A*，用它可選擇最佳路徑。

圖7-20的搜索圖G存在估計(jì)值h(在節(jié)點(diǎn)上用括號(hào)給出)比真值h*大的節(jié)點(diǎn)。例如，節(jié)點(diǎn)A和H的估計(jì)值h是8和4，但到達(dá)目的地的最小真值是7和2。由于這個(gè)費(fèi)用評(píng)價(jià)過大，存在于最短路徑上的節(jié)點(diǎn)H等可以忽略，算法錯(cuò)過了費(fèi)用8的最佳路徑，最終得到費(fèi)用9的滿足要求的路徑。7.3路徑規(guī)劃定義估計(jì)值。這個(gè)估計(jì)值h常常比h*小，697.3路徑規(guī)劃EDFGHIBCAS(7)32(5)(5)(5)(3)(8)(1)(4)(3)221332322441圖7-20

搜索圖G(有的地方估計(jì)值比真值大)7.3路徑規(guī)劃EDFGHIBCAS(7)32(5)(5)(707.3路徑規(guī)劃7.3路徑規(guī)劃71A、起始節(jié)點(diǎn)S被代入OPEN[圖7-21(a)]，子節(jié)點(diǎn)A和B被代人OPEN[圖7-21(b)]。起始節(jié)點(diǎn)S擴(kuò)展后移到CLOSED，B、由于節(jié)點(diǎn)A、B的評(píng)價(jià)值分別為10、8，所以選中擴(kuò)展節(jié)點(diǎn)B。B的子節(jié)點(diǎn)D、E、F，評(píng)價(jià)值分別為9、8、10，全都代人OPEN，擴(kuò)展后節(jié)點(diǎn)B被移到CLOSED[圖7-21(c)]。C、節(jié)點(diǎn)E評(píng)價(jià)值最小，選中擴(kuò)展節(jié)點(diǎn)E。E的子節(jié)點(diǎn)H同評(píng)價(jià)值10都代入OPEN，擴(kuò)展后節(jié)點(diǎn)E被移到CLOSED。7.3路徑規(guī)劃A、起始節(jié)點(diǎn)S被代入OPEN[圖7-21(a)]，子節(jié)點(diǎn)A和72D、OPEN中當(dāng)前評(píng)價(jià)值最小的子節(jié)點(diǎn)是D，所以選中節(jié)點(diǎn)D。D的子節(jié)點(diǎn)H被再次搜索

，節(jié)點(diǎn)D被移到CLOSED。注意到節(jié)點(diǎn)H的值g，由于過去的費(fèi)用6(經(jīng)由節(jié)點(diǎn)E、B返回到節(jié)點(diǎn)S)比新的費(fèi)用7(經(jīng)由節(jié)點(diǎn)D、B返回到S)小，所以不更換指針[圖7-21(d)]。指針仍在E。由于當(dāng)前OPEN上存在評(píng)價(jià)值f都為10的三個(gè)節(jié)點(diǎn)A、H、F，此時(shí)須對(duì)這三個(gè)節(jié)點(diǎn)A、H、F都分別向下擴(kuò)展。7.3路徑規(guī)劃7.3路徑規(guī)劃73E、用中斷連接節(jié)點(diǎn)A，擴(kuò)展節(jié)點(diǎn)C同評(píng)價(jià)值8都代人OPEN，節(jié)點(diǎn)A被移到CLOSED。然后，C的兩個(gè)擴(kuò)展節(jié)點(diǎn)I、H中，選中值f最小的節(jié)點(diǎn)I（實(shí)際上，ACH路徑在先已經(jīng)被否定），節(jié)點(diǎn)I同評(píng)價(jià)值10都代人OPEN，節(jié)點(diǎn)C同評(píng)價(jià)值8都被移到CLOSED。節(jié)點(diǎn)I的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，

OPEN保持F、節(jié)點(diǎn)H的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持7.3路徑規(guī)劃E、用中斷連接節(jié)點(diǎn)A，擴(kuò)展節(jié)點(diǎn)C同評(píng)價(jià)值8都代人OPEN，節(jié)74G、節(jié)點(diǎn)F的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持H、當(dāng)前OPEN上三個(gè)節(jié)點(diǎn)I、H、F的評(píng)價(jià)值都為10,其后的擴(kuò)展節(jié)點(diǎn)都是目標(biāo)G。計(jì)算分別經(jīng)三個(gè)節(jié)點(diǎn)F、H、I到目標(biāo)G的評(píng)價(jià)值,經(jīng)節(jié)點(diǎn)F到目標(biāo)G的評(píng)價(jià)值最小。所以用中斷連接擴(kuò)展節(jié)點(diǎn)F，節(jié)點(diǎn)G連同評(píng)價(jià)值9都代人OPEN，節(jié)點(diǎn)F移到CLOSED[圖7-21(e)]。7.3路徑規(guī)劃G、節(jié)點(diǎn)F的擴(kuò)展節(jié)點(diǎn)即為目標(biāo)，OPEN保持7.3路徑規(guī)劃75在圖7-22的搜索圖G上，所有節(jié)點(diǎn)的估計(jì)值h(在節(jié)點(diǎn)上用括號(hào)給出)常常比真值h*小或相等。

7.3路徑規(guī)劃在圖7-22的搜索圖G上，所有節(jié)點(diǎn)的估計(jì)值h(在767.3路徑規(guī)劃7.3路徑規(guī)劃77A、起始節(jié)點(diǎn)S被代入OPEN，子節(jié)點(diǎn)A、B連同評(píng)價(jià)值f6、8被代人OPEN，起始節(jié)點(diǎn)S擴(kuò)展后移到CLOSED。B、由于節(jié)點(diǎn)A、B的評(píng)價(jià)值f分別為6、8，所以節(jié)點(diǎn)A擴(kuò)展子節(jié)點(diǎn)C、D后移到CLOSED，節(jié)點(diǎn)B、C、D連同各自的評(píng)價(jià)值8、8、9都代入OPEN。7.3路徑規(guī)劃A、起始節(jié)點(diǎn)S被代入OPEN，子節(jié)點(diǎn)A、B連同評(píng)價(jià)值f6、878C、用中斷連接擴(kuò)展節(jié)點(diǎn)B，節(jié)點(diǎn)B擴(kuò)展子節(jié)點(diǎn)E、F。子節(jié)點(diǎn)E、F連同評(píng)價(jià)值8、9都代入OPEN，節(jié)點(diǎn)B擴(kuò)展后代入CLOSED。再次搜索節(jié)點(diǎn)D。這時(shí)，如果注意到節(jié)點(diǎn)D的值g，由于新的費(fèi)用值4(經(jīng)由節(jié)點(diǎn)B返回到節(jié)點(diǎn)S)比過去的費(fèi)用5(經(jīng)由節(jié)點(diǎn)A返回到節(jié)點(diǎn)S)小，所以要更換指針，重新計(jì)算的評(píng)價(jià)值f變?yōu)?[圖7-23(a)]。D、仍然用中斷連接擴(kuò)展節(jié)點(diǎn)C，節(jié)點(diǎn)C擴(kuò)展子節(jié)點(diǎn)I、H。子節(jié)點(diǎn)I、H連同評(píng)價(jià)值10、9都代入OPEN，節(jié)點(diǎn)C移到CLOSED[圖7-23(b)]，7.3路徑規(guī)劃C、用中斷連接擴(kuò)展節(jié)點(diǎn)B，節(jié)點(diǎn)B擴(kuò)展子節(jié)點(diǎn)E、F。子節(jié)點(diǎn)E、79E、當(dāng)前OPEN上五個(gè)節(jié)點(diǎn)I、H、

D、

E

、F。所以選中評(píng)價(jià)值f最小的擴(kuò)展節(jié)點(diǎn)E。然后，把節(jié)點(diǎn)E代入CLOSED，再次搜索節(jié)點(diǎn)H。F、這時(shí)，注意到節(jié)點(diǎn)H的值g，比起過去的費(fèi)用7(經(jīng)由節(jié)點(diǎn)C，A返回節(jié)點(diǎn)S)來新的費(fèi)用6(經(jīng)由節(jié)點(diǎn)E、B返回到節(jié)點(diǎn)S)要小，所以更換指針，重新計(jì)算H的評(píng)價(jià)值f為8[圖7-23(c)]7.3路徑規(guī)劃E、當(dāng)前OPEN上五個(gè)節(jié)點(diǎn)I、H、D、E、F。所以選80G、當(dāng)前OPEN上擴(kuò)展值f最小的節(jié)點(diǎn)H，其后的擴(kuò)展節(jié)點(diǎn)是目標(biāo)G，節(jié)點(diǎn)G以評(píng)價(jià)值8代人OPEN，節(jié)點(diǎn)H代入CLOSEDH、最后，節(jié)點(diǎn)G如果選擇作為值f最小的節(jié)點(diǎn)，則把指針返回到節(jié)點(diǎn)H、E、B、S，最終得到費(fèi)用8的最短路徑。這里，由于節(jié)點(diǎn)H的估計(jì)值h與真值h*相比過小，所以這個(gè)最佳路徑上的節(jié)點(diǎn)必須調(diào)整。7.3路徑規(guī)劃G、當(dāng)前OPEN上擴(kuò)展值f最小的節(jié)點(diǎn)H，其后的擴(kuò)展節(jié)點(diǎn)是目標(biāo)814．AGV的路徑規(guī)劃實(shí)例Dijkstra最短路徑確定法。圖7-24中的節(jié)點(diǎn)1～5是AGV運(yùn)行路線上的岔口，6～18是貨物裝卸點(diǎn)，6是系統(tǒng)的入口，9、15、16是系統(tǒng)的出口，19是AGV停車處。在圖7-24(b)中，將AGV可能的運(yùn)行方向用箭頭進(jìn)行了表示。7.3路徑規(guī)劃4．AGV的路徑規(guī)劃實(shí)例7.3路徑規(guī)劃827.3路徑規(guī)劃7.3路徑規(guī)劃83(1)因?yàn)楣?jié)點(diǎn)1～5是找出6～18貨物裝卸點(diǎn)間最短距離的關(guān)鍵點(diǎn)，故先只考慮系統(tǒng)中的各岔口即節(jié)點(diǎn)l～5間的最短路徑。為此，將相應(yīng)箭頭所表示的距離填人下面的距離矩陣。因節(jié)點(diǎn)1、3