本發(fā)明涉及自動(dòng)駕駛，尤其涉及一種基于模糊控制與路徑優(yōu)選的自動(dòng)駕駛停車場(chǎng)路徑規(guī)劃方法。

背景技術(shù)：

1、隨著自動(dòng)駕駛技術(shù)的快速發(fā)展，停車場(chǎng)這一典型的半結(jié)構(gòu)化和動(dòng)態(tài)環(huán)境成為測(cè)試自動(dòng)駕駛系統(tǒng)適應(yīng)性和魯棒性的關(guān)鍵場(chǎng)景，作為自動(dòng)駕駛車輛運(yùn)行的重要環(huán)節(jié)，停車場(chǎng)路徑規(guī)劃需要解決動(dòng)態(tài)環(huán)境感知、多目標(biāo)路徑優(yōu)化以及復(fù)雜控制決策的問題。

2、目前，自動(dòng)駕駛停車場(chǎng)路徑規(guī)劃的研究主要集中于兩類方法：基于靜態(tài)地圖的路徑規(guī)劃方法和基于優(yōu)化算法的動(dòng)態(tài)規(guī)劃方法，基于靜態(tài)地圖的路徑規(guī)劃方法通常依賴事先構(gòu)建的停車場(chǎng)模型，通過傳統(tǒng)的a*算法或dijkstra算法計(jì)算最短路徑，這類方法在靜態(tài)、規(guī)則化的環(huán)境中表現(xiàn)出較好的性能，但在動(dòng)態(tài)停車場(chǎng)中由于無法實(shí)時(shí)感知和應(yīng)對(duì)障礙物的變化，現(xiàn)有技術(shù)主要存在以下問題：

3、1.動(dòng)態(tài)環(huán)境適應(yīng)性不足：靜態(tài)地圖規(guī)劃方法無法實(shí)時(shí)感知障礙物的變化，在車輛行駛過程中難以動(dòng)態(tài)調(diào)整規(guī)劃路徑，導(dǎo)致路徑規(guī)劃結(jié)果不具備實(shí)時(shí)性和適應(yīng)性。

4、2.路徑規(guī)劃效率低下：傳統(tǒng)優(yōu)化算法在處理停車場(chǎng)狹窄通道和復(fù)雜障礙物分布時(shí)，計(jì)算效率較低，在多目標(biāo)優(yōu)化(路徑長(zhǎng)度、避障和通行效率)的場(chǎng)景下易出現(xiàn)規(guī)劃時(shí)間過長(zhǎng)的問題。

5、3.行駛安全性缺失：現(xiàn)有技術(shù)在狹窄車道或復(fù)雜交叉場(chǎng)景中對(duì)車輛行駛狀態(tài)的控制精度有限，面對(duì)動(dòng)態(tài)障礙物時(shí)規(guī)劃路徑無法有效規(guī)避潛在風(fēng)險(xiǎn)，容易導(dǎo)致碰撞或行駛失敗。

6、4.多變量?jī)?yōu)化能力不足：現(xiàn)有路徑規(guī)劃方法在應(yīng)對(duì)停車場(chǎng)中多變量因素時(shí)缺乏對(duì)變量間關(guān)聯(lián)性的綜合分析，難以同時(shí)滿足全局最優(yōu)性和局部靈活性。

7、綜上所述，現(xiàn)有技術(shù)在動(dòng)態(tài)環(huán)境適應(yīng)性、規(guī)劃效率、行駛安全性和多變量?jī)?yōu)化能力方面存在明顯不足，亟需一種新的路徑規(guī)劃方法來有效應(yīng)對(duì)停車場(chǎng)這一復(fù)雜場(chǎng)景下的技術(shù)挑戰(zhàn)。

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

1、本發(fā)明的一個(gè)目的在于提出一種基于模糊控制與路徑優(yōu)選的自動(dòng)駕駛停車場(chǎng)路徑規(guī)劃方法，本發(fā)明在路徑規(guī)劃的實(shí)時(shí)性、多目標(biāo)優(yōu)化能力和行駛安全性方面實(shí)現(xiàn)了顯著提升，為自動(dòng)駕駛車輛在停車場(chǎng)中的安全高效運(yùn)行提供了可靠技術(shù)保障。

2、根據(jù)本發(fā)明實(shí)施例的一種基于模糊控制與路徑優(yōu)選的自動(dòng)駕駛停車場(chǎng)路徑規(guī)劃方法，包括如下步驟：

3、包括以下步驟：

4、s1.采集停車場(chǎng)的環(huán)境數(shù)據(jù)；

5、s2.對(duì)采集的環(huán)境數(shù)據(jù)進(jìn)行融合處理，并基于停車場(chǎng)的實(shí)時(shí)動(dòng)態(tài)生成環(huán)境狀態(tài)圖；

6、s3.基于環(huán)境狀態(tài)圖定義模糊控制輸入變量，同時(shí)定義模糊控制輸出變量，基于模糊控制輸出變量的動(dòng)態(tài)特征，建立模糊規(guī)則庫；

7、s4.利用模糊推理算法對(duì)環(huán)境數(shù)據(jù)集進(jìn)行實(shí)時(shí)計(jì)算，根據(jù)模糊規(guī)則庫輸出車輛的實(shí)時(shí)行駛參數(shù)，并將計(jì)算結(jié)果用于動(dòng)態(tài)調(diào)整路徑規(guī)劃參數(shù)；

8、s5.基于模糊推理結(jié)果構(gòu)建用于動(dòng)態(tài)規(guī)劃的路徑權(quán)重矩陣，路徑權(quán)重矩陣的權(quán)重值根據(jù)車輛與障礙物的相對(duì)距離、路徑的擁堵程度和當(dāng)前環(huán)境的動(dòng)態(tài)特征動(dòng)態(tài)調(diào)整；

9、s6.利用動(dòng)態(tài)規(guī)劃算法基于權(quán)重矩陣計(jì)算從車輛當(dāng)前位置到目標(biāo)停車位的全局最優(yōu)路徑；

10、s7.當(dāng)停車場(chǎng)環(huán)境中的障礙物分布、其他車輛軌跡或目標(biāo)停車位的狀態(tài)發(fā)生變化時(shí)，實(shí)時(shí)更新環(huán)境狀態(tài)圖，并重新基于模糊推理結(jié)果調(diào)整權(quán)重矩陣，同時(shí)利用動(dòng)態(tài)規(guī)劃算法更新全局最優(yōu)路徑；

11、s8.根據(jù)動(dòng)態(tài)規(guī)劃生成的全局最優(yōu)路徑和模糊控制輸出的車輛行駛參數(shù)，生成車輛的控制指令，用于調(diào)整車輛的轉(zhuǎn)向角度、行駛速度和加減速，并引導(dǎo)車輛按照規(guī)劃路徑行駛至目標(biāo)停車位。

12、可選的，所述s1步驟具體包括以下內(nèi)容：

13、s11.通過車載傳感器采集停車場(chǎng)內(nèi)障礙物的空間位置數(shù)據(jù)pobs＝{(xi,yi,zi)}，其中(xi,yi,zi)表示第i個(gè)障礙物的三維空間坐標(biāo)；

14、s12.利用車載攝像頭和雷達(dá)設(shè)備獲取停車場(chǎng)的道路布局?jǐn)?shù)據(jù)lroad；

15、s13.使用動(dòng)態(tài)目標(biāo)跟蹤算法采集其他車輛的移動(dòng)軌跡數(shù)據(jù)tveh；

16、s14.通過停車場(chǎng)管理系統(tǒng)或?qū)崟r(shí)探測(cè)獲取目標(biāo)停車位的分布信息pslot；

17、s15.將采集到的障礙物的空間位置數(shù)據(jù)pobs、道路布局?jǐn)?shù)據(jù)lroad、其他車輛的移動(dòng)軌跡數(shù)據(jù)tveh和目標(biāo)停車位的分布信息pslot按照統(tǒng)一時(shí)間戳進(jìn)行同步，生成一致的停車場(chǎng)環(huán)境數(shù)據(jù)集合：

18、epark＝{pobs,lroad,tveh,pslot}。

19、可選的，所述s2步驟具體包括以下內(nèi)容：

20、s21.對(duì)停車場(chǎng)環(huán)境數(shù)據(jù)集合epark進(jìn)行完整性驗(yàn)證，使每個(gè)數(shù)據(jù)子集中的關(guān)鍵參數(shù)無缺失或冗余；

21、s22.障礙物空間位置數(shù)據(jù)pobs應(yīng)用卡爾曼濾波方法降低數(shù)據(jù)采集中的噪聲干擾，生成經(jīng)過濾波優(yōu)化的障礙物位置數(shù)據(jù)p′obs；

22、s23.對(duì)道路布局?jǐn)?shù)據(jù)lroad進(jìn)行平滑處理，通過多項(xiàng)式擬合方法優(yōu)化路徑節(jié)點(diǎn)坐標(biāo)，生成優(yōu)化后的道路布局?jǐn)?shù)據(jù)l′road；

23、s24.使用軌跡預(yù)測(cè)算法對(duì)其他車輛的移動(dòng)軌跡數(shù)據(jù)tveh進(jìn)行時(shí)間序列分析，預(yù)測(cè)未來時(shí)刻其他車輛的可能位置，生成軌跡預(yù)測(cè)數(shù)據(jù)t′veh；

24、s25.基于優(yōu)化后的障礙物位置數(shù)據(jù)p′obs、道路布局?jǐn)?shù)據(jù)l′road、軌跡預(yù)測(cè)數(shù)據(jù)t′veh和停車位分布數(shù)據(jù)p′slot，采用區(qū)域分割算法生成停車場(chǎng)環(huán)境狀態(tài)圖：

25、genv＝{robs,rfree,rslot}；

26、其中，障礙物區(qū)域robs的計(jì)算為：

27、

28、其中，nobs表示障礙物的數(shù)量，(x′i,y′i,z′i)表示障礙物i的中心坐標(biāo)，robs,i,i}為障礙物i的影響半徑；

29、可通行區(qū)域rfree的計(jì)算為：

30、

31、其中，nroad表示道路的數(shù)量，l′road,j表示道路j的優(yōu)化路徑節(jié)點(diǎn)集合；

32、停車位區(qū)域rslot的計(jì)算為：

33、

34、其中，nslot表示停車位的數(shù)量，pslot,k表示停車位k的空間坐標(biāo)集合。

35、可選的，所述s3步驟具體包括以下內(nèi)容：

36、s31.基于停車場(chǎng)環(huán)境狀態(tài)圖提取車輛與障礙物的相對(duì)距離變量drel：

37、

38、其中，(xv,yv,zv)表示車輛當(dāng)前位置，robs表示障礙物區(qū)域，drel用于衡量車輛與最近障礙物的距離；

39、基于停車場(chǎng)環(huán)境狀態(tài)圖提取車輛行駛速度變量vveh，通過車載速度傳感器測(cè)量車輛行駛速度，并將其劃分為低、中、高三個(gè)模糊集合；

40、基于停車場(chǎng)環(huán)境狀態(tài)圖提取車輛轉(zhuǎn)向角度變量asteer，車輛當(dāng)前轉(zhuǎn)向角度通過方向盤角度傳感器獲得，定義為負(fù)轉(zhuǎn)向角、零轉(zhuǎn)向角和正轉(zhuǎn)向角三個(gè)模糊集合；

41、基于停車場(chǎng)環(huán)境狀態(tài)圖提取路徑擁堵程度變量cpath，基于可通行區(qū)域rfree和其他車輛的預(yù)測(cè)軌跡數(shù)據(jù)t′veh，計(jì)算路徑節(jié)點(diǎn)的擁堵權(quán)重：

42、

43、其中，表示位置(x,y)是否在其他車輛預(yù)測(cè)軌跡中；

44、s32.定義模糊控制輸出變量：

45、優(yōu)化后的車輛行駛速度v′veh，定義為低速、中速和高速三個(gè)模糊集合，用于動(dòng)態(tài)調(diào)整車輛在不同場(chǎng)景中的速度；

46、優(yōu)化后的轉(zhuǎn)向角度a′steer，定義為負(fù)調(diào)整角、零調(diào)整角和正調(diào)整角三個(gè)模糊集合，用于優(yōu)化車輛的轉(zhuǎn)向控制；

47、路徑優(yōu)先級(jí)ppriority，定義為低優(yōu)先級(jí)、中優(yōu)先級(jí)和高優(yōu)先級(jí)三個(gè)模糊集合，用于標(biāo)識(shí)路徑規(guī)劃中的優(yōu)選程度；

48、s33.基于停車場(chǎng)場(chǎng)景特征和車輛行駛安全性需求，定義模糊規(guī)則庫，用于關(guān)聯(lián)模糊輸入變量與模糊輸出變量的映射關(guān)系：

49、若drel為“近”，且cpath為“高”，則v′veh為“低速”，a′steer為“正調(diào)整角”，ppriority為“低優(yōu)先級(jí)”；

50、若drel為“遠(yuǎn)”，且cpath為“低”，則v′veh為“高速”，a′steer為“零調(diào)整角”，ppriority為“高優(yōu)先級(jí)”。

51、s34.將模糊控制輸入變量drel,vveh,asteer,cpath和模糊控制輸出變量v′veh,a′steer,ppriority應(yīng)用至模糊推理模塊。

52、可選的，所述s4步驟具體包括以下內(nèi)容：

53、s41.基于模糊控制輸入變量和模糊控制輸出變量建立多輸入多輸出的模糊推理模型：

54、ffuzzy:(drel,vveh,asteer,cpath)→(v′veh,a′steer,ppriority)；

55、其中，ffuzzy表示模糊推理函數(shù)，用于映射輸入變量至輸出變量；

56、s42.對(duì)停車場(chǎng)環(huán)境狀態(tài)圖genv提取的輸入變量進(jìn)行模糊化處理，使用隸屬度函數(shù)將車輛與障礙物的相對(duì)距離變量drel模糊化為近、中、遠(yuǎn)模糊集合，對(duì)車輛行駛速度變量vveh應(yīng)用三角隸屬度函數(shù)，劃分為低速、中速和高速模糊集合，對(duì)車輛轉(zhuǎn)向角度變量asteer應(yīng)用梯形隸屬度函數(shù)，劃分為負(fù)調(diào)整角、零調(diào)整角和正調(diào)整角模糊集合，對(duì)路徑擁堵程度變量cpath應(yīng)用高斯隸屬度函數(shù)，劃分為低擁堵、中擁堵和高擁堵模糊集合‘

57、s43.基于模糊規(guī)則庫將模糊化的輸入變量通過模糊推理模型ffuzzy進(jìn)行計(jì)算，使用最小-最大復(fù)合規(guī)則將輸入變量的隸屬度結(jié)合規(guī)則庫映射到輸出變量隸屬度，輸出模糊控制變量；

58、s44.對(duì)模糊推理結(jié)果v′veh,a′steer,ppriority進(jìn)行解模糊化處理，采用重心法計(jì)算輸出變量的實(shí)際值：

59、解模糊化車輛行駛速度v′veh：

60、

61、其中，μv(v)表示速度變量的模糊隸屬度函數(shù)；

62、解模糊化車輛轉(zhuǎn)向角度a′steer：

63、

64、其中，μa(a)表示轉(zhuǎn)向角度變量的模糊隸屬度函數(shù)；

65、解模糊化路徑優(yōu)先級(jí)ppriority：

66、

67、其中，μp(p)表示路徑優(yōu)先級(jí)變量的模糊隸屬度函數(shù)；

68、s45.將解模糊結(jié)果v′veh,a′steer,ppriority作為路徑規(guī)劃模塊的輸入。

69、可選的，所述s5步驟具體包括以下內(nèi)容：

70、s51.基于停車場(chǎng)環(huán)境狀態(tài)圖genv以及模糊推理結(jié)果(v′veh,a′steer,ppriority)，初始化動(dòng)態(tài)路徑權(quán)重矩陣

71、

72、其中，i和j分別表示路徑網(wǎng)絡(luò)中的不同節(jié)點(diǎn)，表示節(jié)點(diǎn)i到節(jié)點(diǎn)j的初始權(quán)重值；

73、為適應(yīng)自動(dòng)駕駛停車場(chǎng)路徑規(guī)劃的主題，在初始化時(shí)充分考慮模糊控制輸出變量對(duì)路徑規(guī)劃的影響，令：

74、

75、其中，φ(·)表示基于車輛行駛速度、轉(zhuǎn)向角度以及路徑優(yōu)先級(jí)的初始權(quán)重生成函數(shù)，用于在路徑網(wǎng)絡(luò)構(gòu)建初始階段賦予各節(jié)點(diǎn)間連線的相對(duì)重要程度；

76、s52.在車輛行駛過程中，根據(jù)車輛與障礙物的相對(duì)距離drel、路徑擁堵程度cpath以及當(dāng)前環(huán)境動(dòng)態(tài)特征對(duì)權(quán)重進(jìn)行實(shí)時(shí)調(diào)度：

77、

78、其中，wij(t)表示節(jié)點(diǎn)i到節(jié)點(diǎn)j在時(shí)間t時(shí)的權(quán)重值，ψ(drel)表示基于車輛與障礙物相對(duì)距離drel的風(fēng)險(xiǎn)評(píng)估函數(shù)，用于在距離過近時(shí)放大權(quán)重增量，α1為調(diào)節(jié)該影響的系數(shù)，ω(cpath)表示與路徑擁堵程度cpath對(duì)應(yīng)的加權(quán)函數(shù)，用于反映停車場(chǎng)道路通行量的變化趨勢(shì)，α2為調(diào)節(jié)該影響的系數(shù)，γ(edyn(t))表示結(jié)合停車場(chǎng)環(huán)境動(dòng)態(tài)特征的修正函數(shù)，edyn(t)反映可通行區(qū)域rfree在時(shí)間t的變化信息，通過對(duì)障礙物新增或車輛流量變化進(jìn)行實(shí)時(shí)感知，從而動(dòng)態(tài)修正權(quán)重值；

79、s53.在環(huán)境狀態(tài)檢測(cè)及模糊推理結(jié)果發(fā)生變化時(shí)，基于權(quán)重wij(t)對(duì)動(dòng)態(tài)路徑權(quán)重矩陣進(jìn)行實(shí)時(shí)更新，令各節(jié)點(diǎn)間權(quán)重隨車輛與障礙物之間距離的變化、路徑擁堵程度的波動(dòng)以及停車場(chǎng)環(huán)境的動(dòng)態(tài)特征及時(shí)調(diào)整。

80、可選的，所述s6步驟具體包括以下內(nèi)容：

81、s61.基于動(dòng)態(tài)路徑權(quán)重矩陣wpath構(gòu)建路徑搜索空間g＝(v,e)，其中v＝{v1,v2,…,vn}表示路徑網(wǎng)絡(luò)中的節(jié)點(diǎn)集，包含車輛當(dāng)前位置節(jié)點(diǎn)vstart、目標(biāo)停車位節(jié)點(diǎn)vgoal以及其他中間節(jié)點(diǎn)，e＝{(vi,vj)}表示節(jié)點(diǎn)vi和vj之間的邊集，對(duì)應(yīng)的邊權(quán)重由wij確定；

82、s62.為從車輛當(dāng)前位置vstart到目標(biāo)停車位vgoal生成全局最優(yōu)路徑popt，定義動(dòng)態(tài)規(guī)劃目標(biāo)函數(shù)：

83、

84、其中，p＝{vstart,v1,v2,…,vgoal}表示候選路徑，cost(p)為路徑總權(quán)重，需滿足：

85、

86、目標(biāo)函數(shù)以路徑總權(quán)重最小為優(yōu)化目標(biāo)，同時(shí)結(jié)合動(dòng)態(tài)規(guī)劃中的狀態(tài)轉(zhuǎn)移方程確保路徑的可行性：

87、

88、其中，cost(vj)表示到達(dá)節(jié)點(diǎn)vj的最小代價(jià)，pred(vj)表示節(jié)點(diǎn)vj的前繼節(jié)點(diǎn)集；

89、s63.在路徑規(guī)劃過程中，基于停車場(chǎng)場(chǎng)景特性添加約束條件，包括避開障礙物，使路徑中的每條邊均滿足路徑擁堵約束，優(yōu)先選擇擁堵程度較低的路徑；路徑長(zhǎng)度約束；

90、s64.利用動(dòng)態(tài)路徑權(quán)重矩陣的動(dòng)態(tài)規(guī)劃算法搜索最優(yōu)路徑，初始化車輛當(dāng)前位置vstart的代價(jià)為0，其他節(jié)點(diǎn)代價(jià)為無窮大；從起始節(jié)點(diǎn)vstart開始，依次更新每個(gè)節(jié)點(diǎn)的代價(jià)cost(vj)和前繼節(jié)點(diǎn)；重復(fù)更新，直到到達(dá)目標(biāo)停車位節(jié)點(diǎn)vgoal或搜索空間遍歷完成；通過前繼節(jié)點(diǎn)回溯生成全局最優(yōu)路徑：

91、popt＝{vstart,v1,v2,…,vgoal}。

92、本發(fā)明的有益效果是：

93、(1)本發(fā)明通過將模糊控制與動(dòng)態(tài)規(guī)劃算法深度結(jié)合實(shí)現(xiàn)了對(duì)停車場(chǎng)動(dòng)態(tài)環(huán)境的實(shí)時(shí)響應(yīng)能力，模糊控制模塊能夠基于車輛與障礙物的相對(duì)距離、路徑擁堵程度以及環(huán)境動(dòng)態(tài)特征實(shí)時(shí)調(diào)整動(dòng)態(tài)路徑權(quán)重矩陣，使得路徑規(guī)劃能夠快速適應(yīng)障礙物的新增、移動(dòng)以及通行路徑的動(dòng)態(tài)變化。，相比于傳統(tǒng)基于靜態(tài)地圖的路徑規(guī)劃方法，本發(fā)明的路徑規(guī)劃時(shí)間顯著減少，并能夠在車輛行駛過程中動(dòng)態(tài)更新最優(yōu)路徑，確保在復(fù)雜停車場(chǎng)場(chǎng)景下路徑規(guī)劃的實(shí)時(shí)性和適應(yīng)性。

94、(2)本發(fā)明采用動(dòng)態(tài)規(guī)劃算法結(jié)合多變量?jī)?yōu)化策略，在路徑規(guī)劃中綜合考慮了路徑長(zhǎng)度、障礙物分布和路徑擁堵程度多維約束條件。，通過動(dòng)態(tài)調(diào)整權(quán)重矩陣并引入多目標(biāo)優(yōu)化函數(shù)規(guī)劃路徑不僅滿足全局最短路徑的要求，還能有效規(guī)避障礙物和高擁堵區(qū)域，確保路徑規(guī)劃的全局最優(yōu)性與局部靈活性，通過路徑權(quán)重計(jì)算模型和實(shí)時(shí)優(yōu)化策略，有效解決了這一問題，大幅提升了路徑規(guī)劃的效率與質(zhì)量。

95、(3)本發(fā)明通過模糊推理對(duì)車輛的行駛速度、轉(zhuǎn)向角度和路徑優(yōu)先級(jí)進(jìn)行動(dòng)態(tài)優(yōu)化控制，結(jié)合實(shí)時(shí)更新的路徑權(quán)重矩陣，顯著提高了車輛在狹窄車道、復(fù)雜交叉口及動(dòng)態(tài)障礙物環(huán)境中的行駛安全性，能夠根據(jù)車輛與障礙物的相對(duì)距離及環(huán)境動(dòng)態(tài)特征，在狹窄場(chǎng)景中優(yōu)化轉(zhuǎn)向角度和速度控制，減少碰撞風(fēng)險(xiǎn)，此外，本發(fā)明特別針對(duì)停車場(chǎng)這一動(dòng)態(tài)、非結(jié)構(gòu)化場(chǎng)景優(yōu)化了路徑規(guī)劃與車輛控制的耦合設(shè)計(jì)，使車輛在復(fù)雜環(huán)境中的適應(yīng)性更強(qiáng)。