專利名稱：機(jī)器人三維路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人三維路徑規(guī)劃，特別涉及一種基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的 自主移動(dòng)的機(jī)器人三維路徑規(guī)劃方法。
背景技術(shù)：
自主移動(dòng)機(jī)器人路徑規(guī)劃是指機(jī)器人在有障礙物的工作環(huán)境中，尋找一條從 給定起點(diǎn)到終點(diǎn)的運(yùn)動(dòng)路徑，使機(jī)器人能無碰撞地繞過所有的障礙物。機(jī)器人 路徑規(guī)劃問題可以分為兩種， 一種是基于環(huán)境先驗(yàn)完全信息的全局路徑規(guī)劃， 另一種是基于傳感器信息的局部路徑規(guī)劃，后者環(huán)境是未知或者部分未知的。 全局路徑規(guī)劃已提出的典型方法有可視圖法、圖搜索法、人工勢(shì)場(chǎng)法等。可視 圖法的優(yōu)點(diǎn)是可以求得最短路徑，但缺乏靈活性，并且存在組合爆炸問題。圖 搜索法比較靈活，機(jī)器人的起始點(diǎn)和目標(biāo)點(diǎn)的改變不會(huì)造成連通圖的重新構(gòu)造， 但不是任何時(shí)候都可以獲得最短路徑?？梢晥D法和圖搜索法適用于多邊形障礙 物的避障路徑規(guī)劃問題，但不適用解決圓形障礙物的避障路徑規(guī)劃問題。人工 勢(shì)場(chǎng)法的基本思想是通過尋找路徑點(diǎn)的能量函數(shù)的極小值點(diǎn)而使路徑避開障礙 物，但存在局部極小值問題且不適于尋求最短路徑。文獻(xiàn)[l]給出的基于神經(jīng)網(wǎng)
絡(luò)結(jié)構(gòu)能量函數(shù)的路徑規(guī)劃算法(稱為NA算法)，引入神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)和模擬退 火等方法，計(jì)算簡(jiǎn)單，能避免某些局部極值情況，對(duì)人工勢(shì)場(chǎng)法給予了較大的 改進(jìn)。但NA算法僅適用于二維空間，本發(fā)明提出一種適用于三維空間的基于神 經(jīng)網(wǎng)絡(luò)能量函數(shù)的路徑規(guī)劃算法。

發(fā)明內(nèi)容
本發(fā)明有4點(diǎn)假設(shè)(1)障礙物是三維環(huán)境中的多面體。(2)障礙物為靜 止的。(3)計(jì)算時(shí)障礙物的尺寸按機(jī)器人的半徑作適當(dāng)?shù)臄U(kuò)展。
本發(fā)明的目的在于將NA路徑規(guī)劃算法推廣到三維空間，使機(jī)器人能夠在三 維空間環(huán)境中規(guī)劃出避障路徑，并且使規(guī)劃的無碰路徑達(dá)到最短無碰路徑。為實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案是
定義路徑總能量函數(shù)為碰撞罰函數(shù)與相應(yīng)于路徑長(zhǎng)度部分能量函數(shù)的加權(quán) 和，它是各個(gè)路徑點(diǎn)的函數(shù)，通過移動(dòng)路徑點(diǎn)的位置，使其朝著使總能量函數(shù) 的函數(shù)值減小方向運(yùn)動(dòng)，最終規(guī)劃出避障的最短路徑。
路徑的碰撞罰函數(shù)定義為各路徑點(diǎn)的碰撞罰函數(shù)之和，而一個(gè)點(diǎn)的碰撞罰 函數(shù)是通過它對(duì)各個(gè)障礙物的神經(jīng)網(wǎng)絡(luò)表示得到的，其中模擬退火溫度的初始 溫度，可根據(jù)障礙物的形狀，設(shè)定相應(yīng)于障礙物各邊的不同的初始溫度。
設(shè)置一個(gè)檢測(cè)路徑點(diǎn)位置的檢測(cè)器 一個(gè)神經(jīng)網(wǎng)絡(luò)分類器，在路徑規(guī)劃的 過程中檢測(cè)路徑點(diǎn)的位置P0c,3^),判斷該點(diǎn)是否在障礙物內(nèi)，即是否與障礙物 相碰。
輸入出發(fā)點(diǎn)P(x,，力,W及目標(biāo)點(diǎn)P(^,^, )的坐標(biāo)，對(duì)于/ = 0，初始路徑一般 取為出發(fā)點(diǎn)到目標(biāo)點(diǎn)的直線上均勻分布的點(diǎn)列。通過檢測(cè)器檢測(cè)路徑點(diǎn)/^^,，z,) 在障礙物內(nèi)或者在障礙物外，按照不同的動(dòng)態(tài)運(yùn)動(dòng)方程，計(jì)算出相應(yīng)路徑點(diǎn)的 移動(dòng)位置。


圖1是一個(gè)點(diǎn)到一個(gè)障礙物的罰函數(shù)的神經(jīng)網(wǎng)絡(luò)。
圖2是用本發(fā)明的基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法進(jìn)行路徑規(guī) 劃的仿真實(shí)驗(yàn)結(jié)果圖。
圖3是用本發(fā)明的基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法進(jìn)行路徑規(guī) 劃的仿真實(shí)驗(yàn)過程。
具體實(shí)施例方式
本發(fā)明的基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法的路徑規(guī)劃具體實(shí)施 方法分為如下幾個(gè)步驟進(jìn)行
步驟l:定義路徑總能量函數(shù)。一條路徑的碰撞罰函數(shù)定義為各路徑點(diǎn)的碰撞罰函數(shù)之和，而一個(gè)點(diǎn)的碰 撞罰函數(shù)是通過它對(duì)各個(gè)障礙物的三層前向神經(jīng)網(wǎng)絡(luò)表示得到的。圖l表示了 一個(gè)點(diǎn)到一個(gè)障礙物的罰函數(shù)的神經(jīng)網(wǎng)絡(luò)。輸入層三個(gè)結(jié)點(diǎn)的輸入值分別是路 徑點(diǎn)的橫坐標(biāo)、縱坐標(biāo)和豎坐標(biāo)x、 _y、 Z，中間層的每個(gè)結(jié)點(diǎn)相應(yīng)于障礙物的 一個(gè)面的不等式限制條件，輸入層和中間層的連接權(quán)系數(shù)等于不等式中x、 >'、 Z前面的系數(shù)，中間層每個(gè)結(jié)點(diǎn)的閾值等于相應(yīng)不等式中的常數(shù)項(xiàng)。中間層到輸 出層的連接權(quán)為l，輸出層結(jié)點(diǎn)的閾值取為不等式的個(gè)數(shù)減去O. 5后的負(fù)數(shù)。該連續(xù)網(wǎng)絡(luò)的運(yùn)算關(guān)系為<formula>formula see original document page 7</formula>其中各符號(hào)的含義為，C:頂層結(jié)點(diǎn)輸出；頂層結(jié)點(diǎn)輸入；頂層結(jié)點(diǎn)閾值；0 ,:中間層第m個(gè)結(jié)點(diǎn)的輸出；/f 中間層第w個(gè)結(jié)點(diǎn)的閾值；w，， ,，i、 發(fā)函數(shù)為常用的S形函數(shù)，即模擬退火溫度變化公式為:<formula>formula see original document page 7</formula>:中間層第附個(gè)結(jié)點(diǎn)的輸入； m: 第w個(gè)不等式限制條件的系數(shù)。激<formula>formula see original document page 7</formula>其中y 是模擬退火初始溫度。艮是相應(yīng)于障礙物每一面的模擬退火初始溫 度，可以根據(jù)障礙物的形狀，設(shè)定各面的不同的初始溫度，這樣對(duì)于一些不對(duì)稱圖形可避免路徑規(guī)劃的局部極小值情況。 整條路徑相應(yīng)于碰撞函數(shù)能量為<formula>formula see original document page 8</formula>(8)其中X是障礙物的個(gè)數(shù)，W是路徑點(diǎn)的個(gè)數(shù)，Cf表示第f個(gè)路徑點(diǎn)nw,，^對(duì)第 /t個(gè)障礙物的碰撞函數(shù)。相應(yīng)于路徑長(zhǎng)度部分的能量函數(shù)定義為所有線段長(zhǎng)度的平方和，即<formula>formula see original document page 8</formula>(9)整條路徑的總能量函數(shù)定義為<formula>formula see original document page 8</formula>其中^和，分別表示對(duì)每一部分的權(quán)重(w, +wt =1 )。 步驟2:根據(jù)出發(fā)點(diǎn)坐標(biāo)/^,'"，z,)及目標(biāo)點(diǎn)坐標(biāo)/^,v,h， 取為出發(fā)點(diǎn)到目標(biāo)點(diǎn)的直線上均勻分布的點(diǎn)列-,)，初始路徑一般<formula>formula see original document page 8</formula>(11)步驟3:對(duì)于路徑點(diǎn)〃(^》.,，0,( = 2,3,...,. '—i，用檢測(cè)器檢測(cè)是否在障礙物內(nèi)，根據(jù)不同情況分別進(jìn)行下面兩種操作 (1)若路徑點(diǎn)/^,,,,,2,)在障礙物內(nèi)，則按下列運(yùn)動(dòng)方程移動(dòng)<formula>formula see original document page 8</formula>(2)若au,，."在障礙物之外，則按下列運(yùn)動(dòng)方程移動(dòng)i<formula>formula see original document page 8</formula>步驟4:重復(fù)執(zhí)行步驟3，直到路徑收斂。 仿真實(shí)驗(yàn)圖2是用本發(fā)明的基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法進(jìn)行路徑規(guī)劃的仿真實(shí)驗(yàn)結(jié)果圖。圖3是用本發(fā)明的基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法進(jìn)行路徑規(guī)劃的仿真實(shí)驗(yàn)過程。本發(fā)明給出了一種基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法，使機(jī)器人 能夠在三維空間環(huán)境中規(guī)劃出最短避障路徑，為自主移動(dòng)機(jī)器人在三維空間的 最優(yōu)路徑規(guī)劃提供了一個(gè)簡(jiǎn)捷有效的方法。
權(quán)利要求
1、一種機(jī)器人三維路徑規(guī)劃方法，其特征在于解決自主移動(dòng)機(jī)器人在有障礙物的三維空間的路徑規(guī)劃問題，規(guī)劃出的避障路徑達(dá)到最短避障路徑。
2、 根據(jù)權(quán)利要求1所述的機(jī)器人三維路徑規(guī)劃方法，其特征在于定義 路徑總能量函數(shù)為碰撞罰函數(shù)與相應(yīng)于路徑長(zhǎng)度部分能量函數(shù)的加權(quán)和，它 是各個(gè)路徑點(diǎn)的函數(shù)，通過移動(dòng)路徑點(diǎn)的位置，使其朝著使總能量函數(shù)的函 數(shù)值減小方向運(yùn)動(dòng)，最終規(guī)劃出避障的最短路徑，路徑的碰撞罰函數(shù)定義為各路徑點(diǎn)的碰撞罰函數(shù)之和，而一個(gè)點(diǎn)的碰撞 罰函數(shù)是通過它對(duì)各個(gè)障礙物的神經(jīng)網(wǎng)絡(luò)表示得到的，其中模擬退火溫度的 初始溫度，可根據(jù)障礙物的形狀，設(shè)定相應(yīng)于障礙物各面的不同的初始溫度，設(shè)置一個(gè)檢測(cè)路徑點(diǎn)位置的檢測(cè)器 一個(gè)神經(jīng)網(wǎng)絡(luò)分類器，在路徑規(guī)劃 的過程中檢測(cè)路徑點(diǎn)的位置，判斷其是否在障礙物內(nèi)，即是否與障礙物相碰，初始路徑一般取為出發(fā)點(diǎn)到目標(biāo)點(diǎn)的直線上均勻分布的點(diǎn)列，通過檢測(cè) 路徑點(diǎn)在障礙物內(nèi)或者在障礙物外，按照不同的動(dòng)態(tài)運(yùn)動(dòng)方程，計(jì)算出相應(yīng)路徑點(diǎn)的移動(dòng)位置。
3、 根據(jù)權(quán)利要求1所述的機(jī)器人三維路徑規(guī)劃方法，其特征在于基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的三維路徑規(guī)劃算法的路徑規(guī)劃具體實(shí)施方法分為如下幾 個(gè)步驟進(jìn)行步驟l:定義路徑總能量函數(shù)。一條路徑的碰撞罰函數(shù)定義為各路徑點(diǎn)的碰撞罰函數(shù)之和，而一個(gè)點(diǎn)的 碰撞罰函數(shù)是通過它對(duì)各個(gè)障礙物的三層前向神經(jīng)網(wǎng)絡(luò)表示得到的，輸入層 的三個(gè)結(jié)點(diǎn)的輸入分別是路徑點(diǎn)的橫坐標(biāo)、縱坐標(biāo)和豎坐標(biāo)x、 _y、 Z，中間 層的每個(gè)結(jié)點(diǎn)相應(yīng)于障礙物的一個(gè)面的不等式限制條件，輸入層和中間層的 連接權(quán)系數(shù)等于不等式中"_y、 Z前面的系數(shù)，中間層每個(gè)結(jié)點(diǎn)的閾值等于 相應(yīng)不等式中的常數(shù)項(xiàng)。中間層到輸出層的連接權(quán)為1，輸出層結(jié)點(diǎn)的閾值取為不等式的個(gè)數(shù)減去0. 5后的負(fù)數(shù)， 該連續(xù)網(wǎng)絡(luò)的運(yùn)算關(guān)系為<formula>formula see original document page 3</formula> (1)<formula>formula see original document page 3</formula>(2)<formula>formula see original document page 3</formula>(3)<formula>formula see original document page 3</formula>( 4 )其中各符號(hào)的含義為，C:頂層結(jié)點(diǎn)輸出；/(;:頂層結(jié)點(diǎn)輸入；:頂層結(jié) 點(diǎn)閾值；中間層第m個(gè)結(jié)點(diǎn)的輸出；/ ,，,中間層第w個(gè)結(jié)點(diǎn)的輸入； ， 中間層第附個(gè)結(jié)點(diǎn)的閾值；wt ,， wz 激發(fā)函數(shù)為常用的S形函數(shù)，即 第^個(gè)不等式限制條件的系數(shù)<模擬退火溫度變化公式為:(5)■ + e1og(l + ,)1og(l + 0(6)(7)其中p是模擬退火初始溫度。A,是相應(yīng)于障礙物每一個(gè)面的模擬退火初始溫度，可以根據(jù)障礙物的形狀，設(shè)定各面的不同的初始溫度，這樣對(duì)于一些不對(duì)稱圖形可避免路徑規(guī)劃的局部極小值情況， 整條路徑相應(yīng)于碰撞函數(shù)部分的能量為W 人'五『:s s《(8)其中《是障礙物的個(gè)數(shù)，iV是路徑點(diǎn)的個(gè)數(shù)，Cf表示第/個(gè)路徑點(diǎn)/^,^,)對(duì)第A個(gè)障礙物的碰撞函數(shù)，相應(yīng)于路徑長(zhǎng)度部分的能量函數(shù)定義為所有線段長(zhǎng)度的平方和，即對(duì)所有路徑點(diǎn)尸(x,乂)， /"，2，…,yV，定義<formula>formula see original document page 4</formula>整條路徑的總能量函數(shù)定義為￡ (10) 其中w,和氣分別表示對(duì)每一部分的權(quán)重(W/+^=i)，步驟2:根據(jù)出發(fā)點(diǎn)坐標(biāo)/^， q)及目標(biāo)點(diǎn)坐標(biāo)/Xx^;v，^.)，初始路徑一 般取為出發(fā)點(diǎn)到目標(biāo)點(diǎn)的直線上均勻分布的點(diǎn)列<formula>formula see original document page 4</formula>(11)步驟3:對(duì)于路徑點(diǎn)/^,,%2,),, = 2,3,..,^-1，用檢測(cè)器檢測(cè)是否在障礙物內(nèi)，根據(jù)不同情況分別進(jìn)行下面兩種操作 (1)若路徑點(diǎn)/^,，^z,)在障礙物內(nèi)，則按下列運(yùn)動(dòng)方程移動(dòng)<formula>formula see original document page 4</formula>(2)若PK，y,A)在障礙物之外，則按下列運(yùn)動(dòng)方程移動(dòng)<formula>formula see original document page 4</formula>步驟4:重復(fù)執(zhí)行步驟3，直到路徑收斂。
全文摘要
本發(fā)明涉及自主移動(dòng)機(jī)器人三維路徑規(guī)劃，特別涉及一種基于神經(jīng)網(wǎng)絡(luò)能量函數(shù)的自主移動(dòng)機(jī)器人三維路徑規(guī)劃算法，定義路徑總能量函數(shù)為碰撞罰函數(shù)與相應(yīng)于路徑長(zhǎng)度部分能量函數(shù)的加權(quán)和，在路徑規(guī)劃的過程中檢測(cè)路徑點(diǎn)的位置是否在障礙物內(nèi)，根據(jù)路徑點(diǎn)位于障礙物內(nèi)外的不同位置用不同的動(dòng)態(tài)運(yùn)動(dòng)方程移動(dòng)路徑點(diǎn)的位置，使其朝著使總能量函數(shù)的函數(shù)值減小的方向運(yùn)動(dòng)，最終規(guī)劃出最短的避障路徑。
文檔編號(hào)G06N3/00GK101231714SQ200710193060
公開日2008年7月30日 申請(qǐng)日期2007年12月5日 優(yōu)先權(quán)日2007年12月5日
發(fā)明者庫(kù)盧莫夫, 禹建麗 申請(qǐng)人:中原工學(xué)院