本申請涉及路徑規(guī)劃，尤其涉及基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法。

背景技術(shù)：

1、路徑規(guī)劃的核心目的是在有障礙物的環(huán)境中，在規(guī)定時間內(nèi)規(guī)劃出一條從起始點到目標點的無碰撞安全路徑。一方面路徑規(guī)劃的結(jié)果需要保證最終路徑與障礙物之間存在足夠的安全距離，避免智能體和障礙物之間的碰撞，同時最終路徑應(yīng)該足夠平滑，從而滿足智能體的運動約束。此外為了保證時效性，路徑規(guī)劃算法需要具有良好的運行速度，從而滿足實際仿真測試的需求。路徑規(guī)劃的應(yīng)用場景廣泛，包括無人車航跡自動規(guī)劃、智能機器人控制、智能交通動態(tài)規(guī)劃、外賣物流配送等。

2、典型的路徑規(guī)劃算法包括基于搜索的dijkstra算法、a*算法、d*算法，基于智能優(yōu)化的遺傳算法、蟻群算法、粒子群算法，以及基于采樣的rrt、rrt*等方法?；谒阉鞯乃惴ê突谥悄軆?yōu)化的算法需要構(gòu)建柵格地圖和結(jié)構(gòu)化建模，計算量大、算法時間復(fù)雜度高，不滿足仿真測試需求，且基于智能優(yōu)化的算法結(jié)果不能保證最優(yōu)。盡管基于采樣的rrt*算法相比基于搜索和智能優(yōu)化的算法規(guī)劃速度更快，但對于仿真環(huán)境下的智能體測試而言，基于采樣的路徑規(guī)劃算法規(guī)劃出的為一條可行路徑，采樣路徑點轉(zhuǎn)折較為突兀，存在路徑可用性差的特點。

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

1、本申請旨在至少在一定程度上解決相關(guān)技術(shù)中的技術(shù)問題之一。

2、為此，本申請的第一個目的在于提出一種基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法，解決了現(xiàn)有方法計算量大、生成路徑可用性差的技術(shù)問題，提高了路徑規(guī)劃的效率，并使得最終路徑更加平滑。

3、本申請的第二個目的在于提出一種計算機設(shè)備。

4、本申請的第三個目的在于提出一種非臨時性計算機可讀存儲介質(zhì)。

5、為達上述目的，本申請第一方面實施例提出了一種基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法，包括：

6、步驟s1：獲取路徑規(guī)劃的起始點、目標點、障礙物信息；

7、步驟s2：基于智能體和障礙物之間的碰撞約束，得到可行采樣點集合；

8、步驟s3：設(shè)置時間步t為1；

9、步驟s4：在當前時間步，從可行采樣點集合中隨機采樣得到新路徑點，并選擇與新路徑點最近的現(xiàn)有路徑點作為新路徑點的父節(jié)點，并計算兩者連線與障礙物之間的最短距離；

10、步驟s5：基于新路徑點、父節(jié)點和計算的最短距離，進行rrt*算法的重新選擇和重新連線機制優(yōu)化；

11、步驟s6：在新路徑點和目標點的距離大于閾值，或，新路徑點與目標點的連線和障礙物之間的最短距離小于安全閾值時，判斷路徑不可行；

12、步驟s7：在路徑不可行時，判斷時間步t是否到達最大迭代次數(shù)，若時間步t未達到最大迭代次數(shù)，設(shè)置時間步t為t+1，若時間步t達到最大迭代次數(shù)，初始化拓展搜索隨機種子，重置時間步t為1，并保留當前采樣的路徑點信息；

13、步驟s8：迭代進行步驟s4-s7，直至找到可行路徑；

14、步驟s9：對可行路徑進行優(yōu)化，得到最終路徑。

15、本申請實施例的基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法，基于傳統(tǒng)的rrt*算法，結(jié)合二維仿真環(huán)境下的障礙物特點，優(yōu)化了算法處理不同形狀障礙物的流程，提高了算法的適用性；基于路徑點和障礙物的位置關(guān)系，利用碰撞約束減少了采樣空間，與此同時引入了路徑規(guī)劃提前終止的機制，進一步提高了規(guī)劃效率；同時，由于原始路徑路徑點存在不光滑、折點較多的問題，在路徑評價中引入了安全代價系數(shù)，提出了基于角度和碰撞約束的路徑點優(yōu)化方法，大幅提高了路徑的可用性，使得最終路徑更加平滑，從而滿足智能體的仿真測試需求。

16、可選地，在本申請的一個實施例中，在障礙物的安全閾值范圍內(nèi)無可行采樣點，其中，與障礙物的距離小于安全閾值的點集合表示為：

17、

18、其中，d(v,i)表示障礙物v與點i的距離，∈為安全閾值；

19、新路徑點、父節(jié)點連線和障礙物之間的最短距離為：

20、

21、其中，i為新路徑點，j為父節(jié)點。

22、可選地，在本申請的一個實施例中，進行rrt*算法的重新選擇和重新連線機制優(yōu)化，包括：

23、計算新路徑點i半徑r內(nèi)的其他路徑點，并選取minpathc(path∪i)的父節(jié)點及其對應(yīng)的路徑path，其中，c(path∪i)＝c(path)+c(j,i)+αmax(0,c(j,i,障礙物集合)-∈)，i表示新路徑點，c(path)表示之前路徑的代價，c(j,i)表示新路徑點i和父節(jié)點j之間的直線距離，c(j,i,障礙物集合)和∈之間的差值表示新路徑點i和父節(jié)點j之間連線和障礙物之間的安全余量，α為路徑安全余量的權(quán)重系數(shù)；

24、對于新路徑點i半徑r內(nèi)的其他路徑點，判斷選擇新路徑點i作為父節(jié)點時的路徑代價和原路徑代價的大小，若通過新路徑點i到達自身的代價比原有的路徑小，選擇新路徑點i作為新的父節(jié)點。

25、可選地，在本申請的一個實施例中，新路徑點和目標點的距離為d(i,target)，新路徑點與目標點的連線構(gòu)成的線段和障礙物之間的最短距離為c(i,target,障礙物集合)。

26、可選地，在本申請的一個實施例中，對可行路徑進行優(yōu)化，得到最終路徑，包括：

27、對可行路徑進行路徑點修正，刪除可行路徑中的多余路徑點，并增加輔助路徑點；

28、在可行路徑的轉(zhuǎn)折點數(shù)量大于閾值，或，可行路徑中存在轉(zhuǎn)角的角度大于閾值時，對可行路徑進行反向優(yōu)化；

29、對可行路徑進行b樣條優(yōu)化，得到最終的可行路徑。

30、可選地，在本申請的一個實施例中，刪除可行路徑中的多余路徑點，包括：

31、從起始點出發(fā)，計算其他路徑點和起始點之間的連線是否滿足和障礙物之間的安全閾值，并計算路徑之間的轉(zhuǎn)彎角度是否滿足條件，并記錄滿足條件的路徑點組合，并選取下標間隔最大的路徑點組合，將選取的路徑點組合中間的路徑點刪除；

32、增加輔助路徑點，包括：

33、利用線性插值的方式，以一定的距離在連續(xù)路徑點之間增加路徑點。

34、可選地，在本申請的一個實施例中，對可行路徑進行b樣條優(yōu)化，包括：

35、將可行路徑倒序排列，并對從目標點到起始點的路徑進行路徑點修正。

36、為達上述目的，本發(fā)明第二方面實施例提出了一種計算機設(shè)備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，處理器執(zhí)行計算機程序時，實現(xiàn)上述基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法。

37、為了實現(xiàn)上述目的，本發(fā)明第三方面實施例提出了一種非臨時性計算機可讀存儲介質(zhì)，其上存儲有計算機程序，當計算機程序被處理器執(zhí)行時，實現(xiàn)上述基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法。

38、本申請附加的方面和優(yōu)點將在下面的描述中部分給出，部分將從下面的描述中變得明顯，或通過本申請的實踐了解到。

技術(shù)特征：

1.一種基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法，其特征在于，包括：

2.如權(quán)利要求1所述的方法，其特征在于，在障礙物的安全閾值范圍內(nèi)無可行采樣點，其中，與障礙物的距離小于安全閾值的點集合表示為：

3.如權(quán)利要求1所述的方法，其特征在于，所述進行rrt*算法的重新選擇和重新連線機制優(yōu)化，包括：

4.如權(quán)利要求1所述的方法，其特征在于，所述新路徑點和目標點的距離為d(i,target)，所述新路徑點與目標點的連線構(gòu)成的線段和障礙物之間的最短距離為c(i,tar?get,障礙物集合)。

5.如權(quán)利要求1所述的方法，其特征在于，所述對所述可行路徑進行優(yōu)化，得到最終路徑，包括：

6.如權(quán)利要求5所述的方法，其特征在于，所述刪除可行路徑中的多余路徑點，包括：

7.如權(quán)利要求1所述的方法，其特征在于，所述對可行路徑進行b樣條優(yōu)化，包括：

8.一種計算機設(shè)備，其特征在于，包括存儲器、處理器及存儲在所述存儲器上并可在所述處理器上運行的計算機程序，所述處理器執(zhí)行所述計算機程序時，實現(xiàn)如權(quán)利要求1-7中任一所述的方法。

9.一種非臨時性計算機可讀存儲介質(zhì)，其上存儲有計算機程序，其特征在于，所述計算機程序被處理器執(zhí)行時實現(xiàn)如權(quán)利要求1-7中任一所述的方法。

技術(shù)總結(jié)
本申請?zhí)岢鲆环N基于碰撞約束和路徑點優(yōu)化的快速路徑規(guī)劃方法，包括：獲取路徑規(guī)劃起始點、目標點、障礙物信息；基于碰撞約束得到可行采樣點集合；設(shè)置時間步t為1；在當前時間步，從可行采樣點集合中隨機采樣得到新路徑點，并選擇最近的現(xiàn)有路徑點作為其父節(jié)點，并計算兩者連線和障礙物的最短距離；進行RRT*算法的重新選擇和重新連線機制優(yōu)化；判斷當前路徑是否可行；在不可行時，若t未達到最大迭代次數(shù)，設(shè)置t為t+1，若達到，初始化拓展搜索隨機種子，重置t為1，并保留當前采樣的路徑點信息；迭代進行上述過程，直至找到可行路徑，并對其優(yōu)化，得到最終路徑。采用上述方案的本發(fā)明提高了路徑規(guī)劃的效率，并使得最終路徑更加平滑。

技術(shù)研發(fā)人員：趙千川,鄭凱鋒,魏海姣,馮亮
受保護的技術(shù)使用者：清華大學
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/6