本發(fā)明屬于自動(dòng)路徑規(guī)劃領(lǐng)域，具體涉及一種移動(dòng)裝置路徑規(guī)劃方法、存儲(chǔ)介質(zhì)以及系統(tǒng)。

背景技術(shù)：

1、路徑規(guī)劃是自主機(jī)器人導(dǎo)航的核心，其最終目標(biāo)是在復(fù)雜環(huán)境中計(jì)算從起點(diǎn)到目的地的最佳路徑。

2、目前，機(jī)器人路徑規(guī)劃的發(fā)展主要基于ros通信。在ros框架內(nèi)，willow?garage推出了move?base以促進(jìn)機(jī)器人導(dǎo)航和路徑規(guī)劃。在move?base的基礎(chǔ)上，pütz?s提出了movebase?flex，提供了更大的靈活性和擴(kuò)展性。早期的路徑規(guī)劃主要采用精確映射方法，如可視圖、voronoi圖、delaunay三角剖分和自適應(yīng)路線圖等，這些方法旨在捕捉機(jī)器人的搜索空間連通性。像dijkstra和a*這樣的搜索算法被用來在連通圖中找到最優(yōu)解。圖搜索方法涉及工作空間的離散化，并且它們?cè)诟呔S度下的性能往往會(huì)下降。有效的離散化可能會(huì)犧牲完整性，而高分辨率的離散化計(jì)算量大。新計(jì)算方法的出現(xiàn)推動(dòng)了它們?cè)诼窂揭?guī)劃中的應(yīng)用。諸如模糊邏輯控制、rrt算法、狀態(tài)格子方法、prm算法、神經(jīng)網(wǎng)絡(luò)、遺傳算法、蟻群優(yōu)化和模擬退火等方法都已應(yīng)用于機(jī)器人路徑規(guī)劃。

3、然而，上述方案在具體實(shí)施中，存在下列不可忽視的缺陷，1、在高精度地圖中因?yàn)樗阉鞴?jié)點(diǎn)過多，影響其運(yùn)算效率。此外，現(xiàn)有方案是一種貪心算法，它在每個(gè)狀態(tài)處都會(huì)選擇當(dāng)前最優(yōu)的路徑，這種局部最優(yōu)選擇可能會(huì)導(dǎo)致路徑在后續(xù)的搜索過程中發(fā)生變化，從而使得全局路徑的規(guī)劃數(shù)量繁多，導(dǎo)致搜索效率較為低下；2、在局部最優(yōu)獲取完畢后直接連線形成規(guī)劃路徑，而未關(guān)注到在局部連線后回到全局的層面產(chǎn)生的不必要的路徑冗余；3、在障礙物之間形成的狹窄通道內(nèi)需求最佳規(guī)劃路徑時(shí)，只關(guān)注自身輪廓與障礙物的最小距離，而錯(cuò)失了狹窄環(huán)境中穿行獲取的更優(yōu)路徑。

技術(shù)實(shí)現(xiàn)思路

1、針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種移動(dòng)裝置路徑規(guī)劃方法、存儲(chǔ)介質(zhì)以及系統(tǒng)，能夠基于目標(biāo)路徑最短及運(yùn)動(dòng)狀態(tài)改變最少，選擇規(guī)劃路徑，從而全局路徑規(guī)劃搜索柵格數(shù)量減少，搜索效率顯著提升。

2、為實(shí)現(xiàn)上述目的，本發(fā)明提供了如下技術(shù)方案為：

3、一種移動(dòng)裝置路徑規(guī)劃方法，用于在預(yù)定移動(dòng)裝置與預(yù)定目標(biāo)位置之間規(guī)劃路徑，其特征在于，包括以下步驟：

4、步驟s1：識(shí)別包含預(yù)定移動(dòng)裝置、預(yù)定目標(biāo)位置的預(yù)定區(qū)域，并將預(yù)定區(qū)域柵格化處理形成柵格化區(qū)域；

5、步驟s2：在柵格化區(qū)域中，遍歷所有柵格，標(biāo)定中心坐標(biāo)作為柵格坐標(biāo)、標(biāo)定柵格可通行狀態(tài)；及標(biāo)定預(yù)定目標(biāo)位置所在坐標(biāo)作為目標(biāo)位置，從而形成規(guī)劃數(shù)據(jù)地圖；

6、步驟s3：獲取當(dāng)前預(yù)定移動(dòng)裝置所在柵格坐標(biāo)和移動(dòng)方向，分別作為實(shí)時(shí)起點(diǎn)位置及實(shí)時(shí)移動(dòng)方向，同時(shí)獲取與實(shí)時(shí)起點(diǎn)位置相鄰的所有柵格坐標(biāo)作為相鄰柵格簇；

7、步驟s4：基于實(shí)時(shí)起點(diǎn)位置和目標(biāo)位置的連線在相鄰柵格簇中獲取第一中間目標(biāo)柵格組，基于實(shí)時(shí)移動(dòng)方向獲取第二中間目標(biāo)柵格組，基于第一中間目標(biāo)柵格組、第二中間目標(biāo)柵格組獲取鄰接目標(biāo)位置；

8、步驟s5：重復(fù)步驟s4，直至當(dāng)前鄰接目標(biāo)位置為目標(biāo)位置。

9、優(yōu)選地，在步驟s2中，柵格可通行狀態(tài)分為“可通行”和“不可通行”兩種，當(dāng)柵格所在位置存在障礙物使得預(yù)定移動(dòng)裝置無法通行時(shí)，柵格可通行狀態(tài)分為“不可通行”，其余為“可通行”。

10、優(yōu)選地，步驟s4包括以下子步驟：

11、步驟s4-1：將起點(diǎn)位置的全周劃分為n個(gè)依次連續(xù)且封閉的角度域；

12、步驟s4-2：獲取實(shí)時(shí)起點(diǎn)位置和目標(biāo)位置的連線，連線在相鄰柵格簇內(nèi)經(jīng)過的柵格、及在相鄰柵格簇中與柵格相鄰的所有柵格，共同形成第一中間目標(biāo)柵格組；

13、步驟s4-3：獲取實(shí)時(shí)移動(dòng)方向的延長(zhǎng)線在相鄰柵格簇內(nèi)經(jīng)過的柵格、及在相鄰柵格簇中與柵格相鄰的所有柵格，共同形成第二中間目標(biāo)柵格組；

14、步驟s4-4：將第一中間目標(biāo)柵格組、第二中間目標(biāo)柵格組的冗余柵格刪除，形成鄰接目標(biāo)位置。

15、進(jìn)一步地，在步驟s4-1中，n為8，角度域呈45°扇缺；

16、在步驟s4-2中，實(shí)時(shí)起點(diǎn)位置和目標(biāo)位置的連線的表達(dá)式如下：

17、

18、θ為連線的方向角，(xe,ye)為目標(biāo)位置，(xs,ys)為實(shí)時(shí)起點(diǎn)位置。

19、進(jìn)一步地，當(dāng)鄰接目標(biāo)位置的數(shù)量為多個(gè)時(shí)，通過啟發(fā)式函數(shù)選定鄰接目標(biāo)位置的一個(gè)。

20、進(jìn)一步地，本發(fā)明還包括步驟s6：將預(yù)定裝置的初始位置和鄰接目標(biāo)位置定位為{pi,i＝0,1,2…n}，定義{pj,j＝1,2…(n-1)}，遍歷pj，若pj不在直線pj-1?pj+1上，且直線pj-1pj+1經(jīng)過的所有柵格均為“可通行”狀態(tài)，則刪去pj；否則，保留pj。

21、更進(jìn)一步地，本發(fā)明還包括步驟s7：將所有保留的pi連線形成規(guī)劃路徑，基于動(dòng)態(tài)窗口法速度空間評(píng)價(jià)函數(shù)g(vi,ωi)對(duì)規(guī)劃路徑的局部路徑進(jìn)行評(píng)價(jià)并基于g(vi,ωi)進(jìn)行選定：

22、g(vi,ωi)＝

23、σ[α·heading(vi,ωi)+β·dist(vi,ωi)+γ·velocity(vi,ωi)+δ·moving(vi,ωi)]

24、heading(vi,ωi)＝π-δθ

25、△θ為規(guī)劃路徑終點(diǎn)切線與目標(biāo)位置的角度差；

26、dist(vi,ωi)＝min(norm(pr+v·δt·[cos(ω·δt)，sin(ω·δt)]-po,i))

27、pr為預(yù)定移動(dòng)裝置當(dāng)前的位置；po,i為第i個(gè)“不可通行”的位置；norm()為歐式距離的函數(shù)，用于計(jì)算兩點(diǎn)間直線距離，△t為規(guī)劃路徑預(yù)測(cè)的時(shí)間步長(zhǎng)；

28、velocity(vi,ωi)＝|v|

29、v為當(dāng)前線速度

30、

31、α，β，γ，δ為g(vi,ωi)中各項(xiàng)的權(quán)重系數(shù)，σ為歸一化函數(shù)，vi為第i個(gè)采集點(diǎn)的線速度，ωi為第i個(gè)采集點(diǎn)的角速度，且基于moving(vi,ωi)的評(píng)價(jià)值對(duì)局部路徑的變向角度進(jìn)行確定。

32、更進(jìn)一步地，預(yù)定移動(dòng)裝置的最大/最小速度vm為：

33、vm＝{(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

34、基于加速度的預(yù)定移動(dòng)裝置的最大/最小速度vd為：

35、

36、基于制動(dòng)距離的預(yù)定移動(dòng)裝置的最大/最小速度vm為：

37、

38、一種路徑規(guī)劃存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序/指令，其特征在于，計(jì)算機(jī)程序/指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述的移動(dòng)裝置路徑規(guī)劃方法的步驟。

39、一種路徑規(guī)劃系統(tǒng)，包括imu模塊、輪速傳感器、激光雷達(dá)、存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上的計(jì)算機(jī)程序，其特征在于，處理器執(zhí)行計(jì)算機(jī)程序以實(shí)現(xiàn)上述的移動(dòng)裝置路徑規(guī)劃方法的步驟。

40、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

41、1.因?yàn)楸景l(fā)明的移動(dòng)裝置路徑規(guī)劃方法包括以下步驟：首先將預(yù)定區(qū)域柵格化處理形成柵格化區(qū)域；接著在柵格化區(qū)域中，遍歷所有柵格，標(biāo)定柵格坐標(biāo)、標(biāo)定柵格可通行狀態(tài)；及目標(biāo)位置，從而形成規(guī)劃數(shù)據(jù)地圖；接著獲取實(shí)時(shí)起點(diǎn)位置及實(shí)時(shí)移動(dòng)方向，同時(shí)獲取相鄰柵格簇；然后基于實(shí)時(shí)起點(diǎn)位置和目標(biāo)位置的連線在相鄰柵格簇中獲取第一中間目標(biāo)柵格組，基于實(shí)時(shí)移動(dòng)方向獲取第二中間目標(biāo)柵格組，基于第一中間目標(biāo)柵格組、第二中間目標(biāo)柵格組獲取鄰接目標(biāo)位置；最后重復(fù)上步，直至當(dāng)前鄰接目標(biāo)位置為目標(biāo)位置，顯然，第一中間目標(biāo)柵格組的選取是基于目標(biāo)路徑最短，即實(shí)時(shí)起點(diǎn)位置與目標(biāo)位置的連線為兩位置間最短距離，及第二中間目標(biāo)柵格組的選取是基于運(yùn)動(dòng)狀態(tài)改變最少，即較之實(shí)時(shí)起點(diǎn)位置的運(yùn)動(dòng)方向改變最少，而鄰接目標(biāo)位置是基于第一中間目標(biāo)柵格組、第二中間目標(biāo)柵格組得到的，因此，本發(fā)明能夠基于目標(biāo)路徑最短及運(yùn)動(dòng)狀態(tài)改變最少，選擇規(guī)劃路徑，從而全局路徑規(guī)劃搜索柵格數(shù)量減少，搜索效率顯著提升。

42、2.因?yàn)樵诒景l(fā)明的步驟s6中：將預(yù)定裝置的初始位置和鄰接目標(biāo)位置定位為{pi,i＝0,1,2…n}，定義{pj,j＝1,2…(n-1)}，遍歷pj，若pj不在直線pj-1pj+1上，且直線pj-1?pj+1經(jīng)過的所有柵格均為“可通行”狀態(tài)，則刪去pj；否則，保留pj，因此，本發(fā)明在通過局部最優(yōu)獲取規(guī)劃路徑，在將規(guī)劃路徑回到全局的層面上進(jìn)行刪除，使得呈三角形兩條邊的連續(xù)路徑形成了單一直線的路徑，從而使得最后得到的規(guī)劃路徑更優(yōu)。

43、3.因?yàn)楸景l(fā)明將所有保留的pi連線形成規(guī)劃路徑，基于動(dòng)態(tài)窗口法速度空間評(píng)價(jià)函數(shù)g(vi,ωi)對(duì)所述規(guī)劃路徑的局部路徑進(jìn)行評(píng)價(jià)、選定，而g(vi,ωi)為heading(vi,ωi),dist(vi,ωi),velocity(vi,ωi)以及moving(vi,ωi)函數(shù)的加權(quán)求和的歸一化結(jié)果，而heading(vi,ωi)與實(shí)時(shí)移動(dòng)方向的切線、預(yù)定移動(dòng)裝置與目標(biāo)位置的連線的夾角；dist(vi,ωi)與潛在規(guī)劃路徑與“不可通行”柵格的最短距離；moving(vi,ωi)與實(shí)時(shí)移動(dòng)方向的切線、當(dāng)前運(yùn)動(dòng)趨勢(shì)方向的夾角均呈線性關(guān)系，而且每個(gè)子函數(shù)均通過預(yù)設(shè)的權(quán)重系數(shù)加權(quán)，因此，本發(fā)明的預(yù)定移動(dòng)裝置，在多個(gè)障礙物形成狹窄通道內(nèi)需求最佳規(guī)劃路徑時(shí)，能通基于對(duì)目標(biāo)位置相對(duì)于自身的朝向角度大小，運(yùn)動(dòng)趨勢(shì)角度改變的大小以及自身輪廓與障礙物的最小距離的綜合考慮，來獲取更優(yōu)的規(guī)劃路徑，而且通過可調(diào)的預(yù)設(shè)的權(quán)重系數(shù)，使得g(vi,ωi)能夠根據(jù)實(shí)際場(chǎng)景情況做更靈活地調(diào)整。