本發(fā)明涉及無人車輛的全局路徑規(guī)劃，具體涉及一種基于優(yōu)化采樣的無人車快速路徑規(guī)劃方法。

背景技術(shù)：

1、隨著自動駕駛技術(shù)的快速發(fā)展，路徑規(guī)劃作為其核心技術(shù)之一，在提升駕駛安全性和效率方面發(fā)揮著關(guān)鍵作用。路徑規(guī)劃方法根據(jù)規(guī)劃范圍可分為全局和局部兩類：全局規(guī)劃基于已知起點(diǎn)和終點(diǎn)信息，旨在生成全局最優(yōu)路徑；局部規(guī)劃則側(cè)重于實(shí)時動態(tài)調(diào)整，充分考慮道路條件和交通環(huán)境等因素，以確保路徑的實(shí)時性和適應(yīng)性。從算法原理來看，全局路徑規(guī)劃主要包括基于隨機(jī)采樣的方法(如rrt及其改進(jìn)算法rrt*)、基于圖搜索的方法(如a*算法和dijkstra算法)以及基于智能優(yōu)化的方法(如深度學(xué)習(xí)、遺傳算法和蟻群算法)。然而，這些方法均存在一定局限性：圖搜索算法在路徑連續(xù)性、動力學(xué)約束和計算效率方面表現(xiàn)不足；深度學(xué)習(xí)方法的計算成本較高；而rrt類算法則存在搜索盲目性、節(jié)點(diǎn)冗余以及難以滿足車輛運(yùn)動學(xué)約束等問題。

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

1、本發(fā)明的目的在于提供一種基于優(yōu)化采樣的無人車快速路徑規(guī)劃方法。提高傳統(tǒng)rrt算法效率同時提高自動駕駛車輛行駛路徑的平滑性及安全性。

2、為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種基于優(yōu)化采樣的無人車快速路徑規(guī)劃方法，包括下列步驟：

3、步驟1：裁剪采樣空間；

4、步驟2：以偏置概率p進(jìn)行非障礙物空間采樣；

5、步驟3：將人工勢場法引入角度系數(shù)；

6、步驟4：基于改進(jìn)人工勢場法加入引力場、斥力場；

7、步驟5：基于引力、斥力、優(yōu)化后的采樣點(diǎn)選取新節(jié)點(diǎn)并以動態(tài)步長進(jìn)行擴(kuò)展；

8、步驟6：基于b樣條曲線擬合形成最終路徑。

9、可選的，在步驟1的執(zhí)行過程中裁剪采樣空間，首先，確立起點(diǎn)和目標(biāo)點(diǎn)的位置。其次，將起點(diǎn)和目標(biāo)點(diǎn)作為一個矩形的兩個對角點(diǎn)畫矩形，所畫矩形包圍的空間就是新的采樣區(qū)域。最后，讀取障礙物的位置，位于矩形內(nèi)的障礙物區(qū)域同樣不作為采樣區(qū)域。完成地圖裁剪。

10、可選的，在步驟2執(zhí)行過程中，以偏置概率p進(jìn)行非障礙物空間采樣。首先，需要設(shè)定一個偏置概率p,用于控制采樣過程中目標(biāo)點(diǎn)偏向的程度，p需要根據(jù)實(shí)際情況自行調(diào)整，不可過大或過小。過大會導(dǎo)致錯過潛在最優(yōu)路徑，過小則會降低搜索路徑，增加路徑長度?；陔S機(jī)數(shù)函數(shù)獲取一個0到1之間的隨機(jī)數(shù)，將其與定義的偏置概率p進(jìn)行比較，如果小于偏置概率，則本次采樣將以目標(biāo)點(diǎn)作為采樣點(diǎn)，若大于偏置概率則隨機(jī)采樣，具體表達(dá)式如下：

11、

12、其中，p為偏置概率，rand為0到1之間的隨機(jī)樹，xrand為隨機(jī)采樣生成的采樣點(diǎn)，xgoal為以目標(biāo)點(diǎn)作為采樣點(diǎn)采樣所得到的采樣點(diǎn)，xsample為最終生成的采樣點(diǎn)。

13、可選的，在步驟3執(zhí)行過程中將人工勢場法引入角度系數(shù)，傳統(tǒng)人工勢場法中，車輛靠近障礙物時，其受到的斥力主要受排斥力增益系數(shù)以及車輛與障礙物間距離的影響。當(dāng)車輛繞過障礙物后，斥力仍然會干擾其向目標(biāo)點(diǎn)的移動，從而可能導(dǎo)致多余路徑的繞行。針對這一問題，引入障礙物角度系數(shù)kang減少多余路徑繞行，具體表達(dá)式如下：

14、

15、其中，θ為車輛與障礙物之間連線與車輛到目標(biāo)點(diǎn)連線的夾角車輛遠(yuǎn)離障礙物時，θ逐漸增大。

16、可選的，在步驟4執(zhí)行過程中，基于改進(jìn)人工勢場法加入引力場、斥力場。其基本思想是在障礙物周圍構(gòu)建斥力勢場，在目標(biāo)點(diǎn)周圍構(gòu)建引力勢場。引力場的強(qiáng)度與被控對象到目標(biāo)點(diǎn)的距離成正比，而斥力場的強(qiáng)度則與被控對象到障礙物的距離成反比。如此，被控對象在這兩種勢場組成的總勢場中受到斥力作用和引力作用，斥力和引力的合力指引著被控對象的運(yùn)動，搜索無碰撞路徑。具體表達(dá)式如下：

17、

18、其中，uatt(x)表示由目標(biāo)點(diǎn)產(chǎn)生的引力勢場，urep(x)表示由障礙物產(chǎn)生的斥力勢場。katt為引力場增益常數(shù)，|x-xgoal|2為當(dāng)前位置到目標(biāo)點(diǎn)距離的平方。ρran代表斥力作用范圍，超出范圍則該障礙物的斥力不再作用于被控對象。krep為排斥力增益系數(shù)，ρ(x,xobs)為被控對象與障礙物之間的距離。

19、可選的，在步驟5的執(zhí)行過程中，得到最終的總勢能向量uall(x),具體表達(dá)式如下：

20、uall(x)＝uatt(x)+urep(x)

21、其中,uall(x)為總勢能。

22、可選的，在步驟5的執(zhí)行過程中，被控對象需要沿著勢能減小的方向移動，因此，定義引力fatt(x)和斥力frep(x)分別為引力場函數(shù)和斥力場函數(shù)的負(fù)梯度，具體表達(dá)式如下：

23、fatt(x)＝-katt|x-xgoal|

24、

25、其中，fatt(x)為目標(biāo)點(diǎn)產(chǎn)生的引力，frep(x)為障礙物產(chǎn)生的斥力，katt為引力場增益常數(shù)，krep為斥力場增益常數(shù)，ρran代表斥力作用范圍，超出范圍則該障礙物的斥力不再作用于被控對象。ρ(x,xobs)為被控對象與障礙物之間的距離。

26、可選的，在步驟5執(zhí)行過程中，步驟5中求得的斥力當(dāng)被控對象繞過障礙物后，斥力仍然會干擾其向目標(biāo)點(diǎn)的移動，從而可能導(dǎo)致多余路徑的繞行。針對這一問題，引入步驟3所述的障礙物角度系數(shù)kang減少多余路徑繞行，具體表達(dá)式如下：

27、

28、其中，frep1(x)為引入障礙物角度系數(shù)kang后的障礙物斥力。

29、可選的，在步驟5執(zhí)行過程中，基于引力、斥力選取新節(jié)點(diǎn)并以動態(tài)步長進(jìn)行擴(kuò)展，首先計算引力分量和斥力分量，并結(jié)合隨機(jī)生成的節(jié)點(diǎn)坐標(biāo)，依據(jù)平行四邊形法則進(jìn)行矢量合成，從而確定合力的方向與大小。在步長確定環(huán)節(jié)，算法通過計算隨機(jī)節(jié)點(diǎn)與鄰近節(jié)點(diǎn)的歐氏距離，采用自適應(yīng)調(diào)整策略：當(dāng)節(jié)點(diǎn)間距小于預(yù)設(shè)步長時，將當(dāng)前距離作為臨時步長；反之，則保持初始步長設(shè)定不變。具體表達(dá)式如下：

30、

31、其中,xnew為新擴(kuò)展節(jié)點(diǎn)，xnear為離隨機(jī)采樣點(diǎn)最近的樹節(jié)點(diǎn)，ptem為動態(tài)步長，u(n)為最終合力向量。

32、可選的，在步驟6執(zhí)行過程中，為了確保車輛在運(yùn)動過程中的平穩(wěn)性，對步驟5最終生成的節(jié)點(diǎn)的連續(xù)性提出了嚴(yán)格要求。三次b樣條曲線以其獨(dú)特的在節(jié)點(diǎn)矢量處實(shí)現(xiàn)二階連續(xù)的特性，這與車輛運(yùn)動學(xué)中對速度、加速度連續(xù)變化的要求高度契合。b樣條曲線函數(shù)通過綜合運(yùn)用插值、逼近和擬合等多種數(shù)學(xué)方法，對路徑規(guī)劃中的不平滑部分進(jìn)行精細(xì)處理。這種處理方式不僅提升了路徑的可行性，也增強(qiáng)了軌跡的實(shí)際可執(zhí)行性。

33、本發(fā)明提供了一種基于優(yōu)化采樣的無人車快速路徑規(guī)劃方法，該方法首先在rrt算法的基礎(chǔ)之上，裁剪采樣空間并以偏置概率p進(jìn)行非障礙物空間采樣；然后引入角度系數(shù)改進(jìn)人工勢場法；接著基于改進(jìn)人工勢場法在rrt算法生成的隨機(jī)樹中引入引力場，在障礙物的周圍建立斥力場；基于引力、斥力、優(yōu)化后的采樣點(diǎn)選取新節(jié)點(diǎn)，并以動態(tài)步長進(jìn)行擴(kuò)展，得到粗解路徑；最后基于b樣條曲線擬合粗解路徑上的節(jié)點(diǎn)，形成最終路徑。本發(fā)明克服了rrt算法搜索隨機(jī)性強(qiáng)、擴(kuò)展節(jié)點(diǎn)冗余的問題，在rrt全局路徑規(guī)劃算法的時間，路徑長度，采樣點(diǎn)數(shù)量以及路徑曲率方面都有明顯優(yōu)化效果。