一種避障路徑平滑優(yōu)化方法技術(shù)

本發(fā)明專利技術(shù)公開了一種避障路徑平滑優(yōu)化方法,該方法首先獲取障礙物區(qū)域范圍；然后獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，并初始定義起始節(jié)點(diǎn)為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；再獲取標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)之間連接線的各點(diǎn)坐標(biāo)；接著通過判斷所獲點(diǎn)坐標(biāo)是否位于障礙物區(qū)域范圍內(nèi)，判斷重新定義后的標(biāo)記起點(diǎn)是否等于標(biāo)記終點(diǎn)，進(jìn)行相應(yīng)操作來獲得平滑路徑；最后將獲得的路徑起始節(jié)點(diǎn)定義為標(biāo)記終點(diǎn)，目標(biāo)節(jié)點(diǎn)定義為標(biāo)記起點(diǎn)，重新進(jìn)行判斷操作，最終得出平滑優(yōu)化后的避障路徑。該方法能夠降低避障搜索路徑中的累計(jì)轉(zhuǎn)折次數(shù)，減小避障搜索路徑中的累計(jì)轉(zhuǎn)折角度，有效減少避障搜索路徑的長(zhǎng)度。

【技術(shù)實(shí)現(xiàn)步驟摘要】
一種避障路徑平滑優(yōu)化方法
本專利技術(shù)涉及一種路徑平滑優(yōu)化方法，尤其是對(duì)避障路徑的平滑優(yōu)化方法。
技術(shù)介紹
在避障路徑規(guī)劃中，通常的路徑搜索算法規(guī)劃出的路徑曲曲折折，冗余點(diǎn)比較多，不是一條由大多單點(diǎn)組成的路徑。如果機(jī)械臂沿此路徑動(dòng)作，勢(shì)必會(huì)產(chǎn)生劇烈的振動(dòng)；如果移動(dòng)機(jī)器裝置沿此路徑前進(jìn)，則會(huì)走較多的彎路。而且它們也都會(huì)消耗較多不必要的能源。在經(jīng)過避障路徑搜索后，為了獲得順暢的最短避障路徑，對(duì)搜索路徑進(jìn)行平滑優(yōu)化，是需要解決的重要問題。
技術(shù)實(shí)現(xiàn)思路
本專利技術(shù)的目的是提供一種避障路徑平滑優(yōu)化方法，期望最終獲得最短的平滑路徑。為實(shí)現(xiàn)上述目的，本專利技術(shù)采用的技術(shù)方案是：一種避障路徑平滑優(yōu)化方法，其特征是包括以下步驟:Step1:獲取障礙物區(qū)域范圍；Step2:獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，并初始定義起始節(jié)點(diǎn)為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step3:獲取標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)之間連接線的各點(diǎn)坐標(biāo)；Step4:判斷Step3中所獲點(diǎn)坐標(biāo)是否位于Step1中所獲障礙物區(qū)域范圍內(nèi)；若位于障礙物區(qū)域范圍內(nèi)，將標(biāo)記終點(diǎn)相鄰點(diǎn)重新定義為標(biāo)記終點(diǎn)，轉(zhuǎn)至Step3執(zhí)行；反之，連接現(xiàn)標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)后，將標(biāo)記終點(diǎn)重新定義為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step5:判斷Step4中新定義標(biāo)記起點(diǎn)是否等于標(biāo)記終點(diǎn)，若相同，停止執(zhí)行；否則，轉(zhuǎn)至Step3執(zhí)行；Step6:將Step5中獲得的路徑起始節(jié)點(diǎn)定義為標(biāo)記終點(diǎn)，目標(biāo)節(jié)點(diǎn)定義為標(biāo)記起點(diǎn)，重新執(zhí)行Step3~Step5，最終得出平滑優(yōu)化后的避障路徑。其中所述障礙物區(qū)域范圍獲取首先要對(duì)障礙物建模，將其由三維空間實(shí)體轉(zhuǎn)化為二維空間的幾何圖塊，然后通過算術(shù)運(yùn)算得到能夠包絡(luò)整個(gè)圖形的邊界點(diǎn)坐標(biāo)。其中所述獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)步驟，首先需要去除搜索路徑中的冗余點(diǎn)，以確保起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)周圍分別只有一個(gè)相鄰節(jié)點(diǎn)，同時(shí)也為了減少后續(xù)節(jié)點(diǎn)搜索的工作量，提高運(yùn)算效率。其中所述Step3和Step4中連接標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)的線的生成方法可以不同；Step4中標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)連接線任意，且可以是虛擬的；Step4中連接標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)的線必須為真實(shí)的直線，且為最短直線。其中所述Step5中獲得的路徑為Step2中搜索路徑的平滑路徑，但不是最短平滑路徑，經(jīng)過Step6進(jìn)一步優(yōu)化后，最終獲得最短的平滑路徑。本專利技術(shù)所提供的避障路徑平滑優(yōu)化方法，可以降低避障搜索路徑中的累計(jì)轉(zhuǎn)折次數(shù)，減小避障搜索路徑中的累計(jì)轉(zhuǎn)折角度，有效減少避障搜索路徑的長(zhǎng)度，從而最終能夠獲得最短的平滑路徑。附圖說明圖1為避障路徑平滑優(yōu)化流程圖；圖2為避障搜索路徑圖；圖3為去除冗余點(diǎn)后的避障搜索路徑圖；圖4為避障平滑路徑圖；圖5為避障平滑優(yōu)化路徑圖。具體實(shí)施方式以下實(shí)施例用于說明本專利技術(shù)，但不用來限制本專利技術(shù)的范圍。如圖1所示，避障路徑平滑優(yōu)化的過程如下：首先對(duì)障礙物建模，將其由三維空間實(shí)體轉(zhuǎn)化為二維空間的幾何圖塊，然后通過算術(shù)運(yùn)算得到能夠包絡(luò)整個(gè)圖形的邊界點(diǎn)坐標(biāo)。接著對(duì)避障搜索路徑中的冗余點(diǎn)進(jìn)行去除，確保起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)周圍分別只有一個(gè)相鄰節(jié)點(diǎn)；在獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)后，初始定義起始節(jié)點(diǎn)為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)。連接標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)（連接線任意，且可以是虛擬的），并獲取其各點(diǎn)坐標(biāo)。對(duì)所獲點(diǎn)坐標(biāo)是否位于障礙物區(qū)域范圍內(nèi)進(jìn)行判斷，若位于障礙物區(qū)域范圍內(nèi)，將標(biāo)記終點(diǎn)相鄰點(diǎn)重新定義為標(biāo)記終點(diǎn)，重新對(duì)兩點(diǎn)進(jìn)行連接、坐標(biāo)獲取和判斷；若不位于障礙物區(qū)域范圍內(nèi)，則直線連接現(xiàn)標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)后，將標(biāo)記終點(diǎn)重新定義為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)。再對(duì)新定義標(biāo)記起點(diǎn)是否等于標(biāo)記終點(diǎn)進(jìn)行判斷，若相同，則停止執(zhí)行；否則，重新對(duì)兩點(diǎn)進(jìn)行連接、坐標(biāo)獲取和判斷直至停止執(zhí)行。對(duì)首輪獲得的平滑路徑起始節(jié)點(diǎn)定義為標(biāo)記終點(diǎn)，目標(biāo)節(jié)點(diǎn)定義為標(biāo)記起點(diǎn)，重新執(zhí)行上述內(nèi)容，最終得出平滑優(yōu)化后的避障路徑。下面參照附圖結(jié)合示例性實(shí)施例對(duì)本專利技術(shù)做進(jìn)一步的詳細(xì)說明。以移動(dòng)機(jī)器裝置的避障路徑平滑優(yōu)化為例，對(duì)本專利技術(shù)進(jìn)行詳細(xì)描述。圖2中，方格可看做放大了的像素點(diǎn)，黑色塊則是二維化的障礙物，紅色方塊線為通過路徑搜索算法得到的避障路徑，從中可以看出路徑曲曲折折，存在折線多、轉(zhuǎn)折次數(shù)多、累積轉(zhuǎn)折角度大、路徑比較長(zhǎng)等問題，需要進(jìn)一步的處理。下面利用本專利技術(shù)的方法進(jìn)行路徑的平滑優(yōu)化。Step1:建立障礙物信息矩陣，將避障搜索路徑加入臨時(shí)節(jié)點(diǎn)集合。Step2:將臨時(shí)節(jié)點(diǎn)集合中的冗余點(diǎn)剔除，獲取起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，原避障搜索路徑變?yōu)橐粭l由單點(diǎn)組成的路徑，如圖3，同時(shí)將起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)賦予標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)變量。Step3:在標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)之間任意連線，將連線坐標(biāo)加入暫存點(diǎn)集合。Step4:循環(huán)判斷暫存點(diǎn)是否位于障礙物信息矩陣，若在其信息矩陣，將標(biāo)記終點(diǎn)相鄰點(diǎn)賦予標(biāo)記終點(diǎn)變量，轉(zhuǎn)至Step3執(zhí)行；反之，實(shí)線連接現(xiàn)標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)，連線點(diǎn)加入永久節(jié)點(diǎn)集合，將標(biāo)記終點(diǎn)賦予標(biāo)記起點(diǎn)變量，目標(biāo)節(jié)點(diǎn)賦予標(biāo)記終點(diǎn)變量。Step5:判斷Step4中新定義標(biāo)記起點(diǎn)是否是標(biāo)記終點(diǎn)，若是，停止執(zhí)行，將永久節(jié)點(diǎn)集合結(jié)果示圖，如圖4；否則，轉(zhuǎn)至Step3執(zhí)行。Step6:將Step5中獲得的路徑起始節(jié)點(diǎn)賦予標(biāo)記終點(diǎn)變量，目標(biāo)節(jié)點(diǎn)賦予標(biāo)記起點(diǎn)變量，重新執(zhí)行Step3~Step5，最終得出平滑優(yōu)化后的避障路徑，如圖5。以上實(shí)施方式僅用于說明本專利技術(shù)，而并非對(duì)本專利技術(shù)的限制，有關(guān)
的普通技術(shù)人員，在不脫離本專利技術(shù)的精神和范圍的情況下，還可以做出各種變化，因此所有等同的技術(shù)方案也屬于本專利技術(shù)的范疇，本專利技術(shù)的專利保護(hù)范圍應(yīng)由其權(quán)利要求限定。本文檔來自技高網(wǎng)...

【技術(shù)保護(hù)點(diǎn)】
一種避障路徑平滑優(yōu)化方法，其特征在于包括以下步驟：Step1:?獲取障礙物區(qū)域范圍；Step2:?獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，并初始定義起始節(jié)點(diǎn)為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step3:?獲取標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)之間連接線的各點(diǎn)坐標(biāo)；Step4:?判斷Step3中所獲點(diǎn)坐標(biāo)是否位于Step1中所獲障礙物區(qū)域范圍內(nèi)；若位于障礙物區(qū)域范圍內(nèi)，將標(biāo)記終點(diǎn)相鄰點(diǎn)重新定義為標(biāo)記終點(diǎn)，轉(zhuǎn)至Step3執(zhí)行；反之，連接現(xiàn)標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)后，將標(biāo)記終點(diǎn)重新定義為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step5:?判斷Step4中新定義標(biāo)記起點(diǎn)是否等于標(biāo)記終點(diǎn)，若相同，停止執(zhí)行；否則，轉(zhuǎn)至Step3執(zhí)行；Step6:?將Step5中獲得的路徑起始節(jié)點(diǎn)定義為標(biāo)記終點(diǎn)，目標(biāo)節(jié)點(diǎn)定義為標(biāo)記起點(diǎn)，重新執(zhí)行Step3~Step5，最終得出平滑優(yōu)化后的避障路徑。

【技術(shù)特征摘要】
1.一種避障路徑平滑優(yōu)化方法，其特征在于包括以下步驟：Step1:獲取障礙物區(qū)域范圍；Step2:獲取避障搜索路徑的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，并初始定義起始節(jié)點(diǎn)為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step3:獲取標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)之間連接線的各點(diǎn)坐標(biāo)；Step4:判斷Step3中所獲點(diǎn)坐標(biāo)是否位于Step1中所獲障礙物區(qū)域范圍內(nèi)；若位于障礙物區(qū)域范圍內(nèi)，將標(biāo)記終點(diǎn)相鄰點(diǎn)重新定義為標(biāo)記終點(diǎn)，轉(zhuǎn)至Step3執(zhí)行；反之，連接現(xiàn)標(biāo)記起點(diǎn)和標(biāo)記終點(diǎn)后，將標(biāo)記終點(diǎn)重新定義為標(biāo)記起點(diǎn)，目標(biāo)節(jié)點(diǎn)為標(biāo)記終點(diǎn)；Step5:判斷Step4中新定義標(biāo)記起點(diǎn)是否等于標(biāo)記終點(diǎn)，若相同，停止執(zhí)行；否則，轉(zhuǎn)至Step3執(zhí)行；Step6:將Step5中獲得的路徑起始節(jié)點(diǎn)定義為標(biāo)記終點(diǎn)，目標(biāo)節(jié)點(diǎn)定義為標(biāo)記起點(diǎn)，重新執(zhí)行S...

【專利技術(shù)屬性】
技術(shù)研發(fā)人員：朱成俊，蘇君，賈慶成，韓艷贊，趙巖，魏允，余小飛，王存才，王俊京，司銅生，倪江南，唐靜，崔庚彥，李永飚，華昱立，
申請(qǐng)(專利權(quán))人：河南工業(yè)職業(yè)技術(shù)學(xué)院，
類型：發(fā)明
國(guó)別省市：河南;41