基于floyd 算法的移動(dòng)機(jī)器人最短路徑規(guī)劃研究[j]new

1、 第 10 期 石為人 等：基于 Floyd 算法的移動(dòng)機(jī)器人最短路徑規(guī)劃研究 2089域；2)移動(dòng)機(jī)器人形狀大小為 1?1 的正方形；3)障礙物形狀不作限定，所占面積為 1~n，n 取值范圍[1,100]；將移動(dòng)機(jī)器人的工作環(huán)境以柵格地圖的形式進(jìn)行分塊，每個(gè)柵格形狀大小為 1?1，并對(duì)每個(gè)柵格進(jìn)行編號(hào)，從坐標(biāo)原點(diǎn)開(kāi)始，沿 X 軸編號(hào)，編號(hào)形式如圖 1 所示

2、，圖中灰色部分為障礙物位置。圖 1 柵格地圖模型Fig.1 Model of grid map2.2 移動(dòng)機(jī)器人在柵格地圖中的移動(dòng)方向假定移動(dòng)機(jī)器人所在位置為點(diǎn)(xs, ys)，移動(dòng)機(jī)器人的移動(dòng)方向有 8 個(gè)，方向表示如圖 2 所示，移動(dòng)一個(gè)單元格后機(jī)器人的位置分別為(xs+1, ys+1)、(xs, ys+1)、(xs–1, ys+1)、(xs–1, ys)、(xs–1, ys–1)、(x, ys–1)、(xs+1, ys–1)、(x

3、s+1, ys)，需要移動(dòng)的距離分別是。 ( 2,1, 2,1, 2,1, 2,1)圖 2 移動(dòng)機(jī)器人移動(dòng)方向和移動(dòng)距離圖Fig.2 Moving direction and distance of a mobile robot3 移動(dòng)機(jī)器人最短路徑規(guī)劃算法的實(shí)現(xiàn)3.1 Floyd 算法的基本思想[11]Floyd 算法的基本思想是：假設(shè)求從節(jié)點(diǎn) vi 到 vj 的最短路徑。如果從 vi 到 vj 有弧，則從 vi 到 vj 存在一條

4、長(zhǎng)度為 Aij 的路徑，該路徑不一定是最短路徑，尚需進(jìn)行n 次試探。首先考慮路徑是否存在，如果存在，則比較和的路徑長(zhǎng)度，取長(zhǎng)度較短者為從 vi 到 vj 的中間節(jié)點(diǎn)的序號(hào)不大于 1 的最短路徑。假如在路徑上再增加一個(gè)節(jié)點(diǎn) v2，也就是說(shuō)，如果和分別是當(dāng)前找到的中間節(jié)點(diǎn)的序號(hào)不大于 1 的最短路徑，那么就有可能是從 vi 到 vj 的中間節(jié)點(diǎn)的序號(hào)不大于 2 的最短路徑。將它和已經(jīng)得到的從 vi 到 vj 中間節(jié)點(diǎn)序號(hào)不大于1 的最短路徑

5、相比較，從中選出中間節(jié)點(diǎn)的序號(hào)不大于2 的最短路徑之后，再增加一個(gè) v3，繼續(xù)進(jìn)行試探。依次類推，經(jīng)過(guò) n 次比較后，求得從 vi 到 vj 的最短路徑。3.2 移動(dòng)機(jī)器人最短路徑規(guī)劃的流程Floyd 算法是建立在已知節(jié)點(diǎn)的權(quán)值及方向的基礎(chǔ)上的，因此移動(dòng)機(jī)器人的最短路徑規(guī)劃從算法節(jié)點(diǎn)選擇、節(jié)點(diǎn)的帶權(quán)有向圖入手進(jìn)行研究。3.2.1 節(jié)點(diǎn)的選擇垂線法是指在指定連線上作垂線，通過(guò)垂線與障礙物邊沿的交點(diǎn)，確定 Floyd 算法中的節(jié)點(diǎn)的方法，通

6、過(guò)垂線法能有效的將環(huán)境中最短路徑上節(jié)點(diǎn)選擇出來(lái)，垂線法選擇節(jié)點(diǎn)的流程為：1）連接始點(diǎn) v0、終點(diǎn) vt，找出連線上所經(jīng)過(guò)的障礙物 Oi，圖 3(a)中障礙物為 O1、O2；2）在障礙物 O1、O2 上，以 v0vt 的連線（簡(jiǎn)稱為t）作垂線 m，垂線 m 與 t 的交點(diǎn)為 Qi，垂線 m 與障礙物邊沿的交點(diǎn)為 Ui；3）分別選擇 t 兩邊 UiQi 距離最大的點(diǎn) vi，納入節(jié)點(diǎn)集合 S 中，圖 3(b)中障礙物 O1 選擇的節(jié)點(diǎn)為 v1