(19)國家知識產權局(12)發(fā)明專利地址100081北京市海淀區(qū)中關村南大街5號事務所(普通合伙)11639審查員李慧本發(fā)明涉及一種復雜環(huán)境下的車輛路徑規(guī)載路徑規(guī)劃算法中重用之前已加載區(qū)域的規(guī)劃-21.一種復雜環(huán)境下的車輛路徑規(guī)劃方法，其特征在于，包括以下步驟：步驟1:選定路徑起始點和終止點；以起止點為基準規(guī)劃出一個矩形，以該矩形作為基準向外擴展，得到將基準矩形包裹在內的規(guī)劃中心矩形；然后，對規(guī)劃中心矩形進行地理信息建模；步驟2:采用增量式加載路徑規(guī)劃算法，提高車輛路徑規(guī)劃效率；在確定規(guī)劃中心矩形，也就是整個規(guī)劃區(qū)域后，將規(guī)劃中心矩形劃分為sp份，將加載區(qū)域標識定義為mark,每個區(qū)域內的已規(guī)劃行列數分別為1d_rowsmark、1d_colsmark,并分別初始化為0;將增量式加載地圖在每次加載地圖回合中增加的行、列數值分別定義為rows、其中，ROWS為規(guī)劃中心矩形總行數，COLS為規(guī)劃中心矩形的總列數，[]為向上取整符在每次的增量式加載新回合中，每個區(qū)域內的已規(guī)劃行列數為1d_rowsmark、1d_=0代表起點所在的規(guī)劃中心矩形區(qū)域，mark=1代表目的地所在的矩形區(qū)域，mark=2代表與起點同左側或右側的矩形區(qū)域，mark=3即為與終點同右側或左側的區(qū)域；如果總行數減去已經加載的行數或列數，小于每次的基礎增量值rows或cols,則本次的規(guī)劃區(qū)域矩形行數/列數增加值等于總行數/列數減去已經加載的行數/列數的值，否則仍按照rows或cols增加；ld_colsmark的計算公式如下：3步驟3:增量式加載路徑規(guī)劃；針對沒有路徑的情況，根據mark標記值加載對應區(qū)域地圖數據，使用啟發(fā)式算法計算首先按照步驟2對地圖進行網格化建模，對路網進行建模，將初始mark賦值為0,再根據mark的值加載相應區(qū)域地圖，并使用啟發(fā)式算法進行計算；步驟3.1:當mark=0時，首先加載以起始點為中心，以1d_rowso、1d_cols?為邊長的矩形范圍內的地理數據；加載內容包括路徑規(guī)劃所需要考慮的所有地理信息；數據加載完成后，使用啟發(fā)式路徑規(guī)劃算法，計算從起始點所在柵格到終止點所在柵格的可行路徑；超過當前已加載矩形范圍的柵格會被認為不可通行，如果待更新柵格點超過了已加載矩形的范圍將不會考慮該柵格，直到規(guī)劃過程中存儲的待擴展柵格點的優(yōu)先隊列為空時，本次搜索結束；從優(yōu)先隊列中取出柵格為擴展柵格點，擴展柵格和以擴展柵格為中心展開的八個方向的柵格統(tǒng)稱為探索柵格點；通過步驟2方法更新1d_rows?、1d_cols值；如果搜索時與mark=1區(qū)域相遇，則轉步驟7;否則mark賦值為1;步驟3.2:當mark=1時，加載以終止點為中心，以1d_rows?、1d_cols?為邊長的矩形范圍內的地理數據；使用啟發(fā)式搜索算法在矩形范圍內從終止點向起始點進行路徑規(guī)劃；同樣不考慮超過當前已加載矩形范圍的柵格；如果搜索時與mark=0區(qū)域相遇，則轉步驟7,否則mark賦值為0,根據步驟2中方法更新1d_rows?、1d_cols?值，進行下一次搜索；ld_cols?為邊長，以mark=0和mark=1矩形區(qū)域相交行列處為邊界，加載mark=2區(qū)域地理信息，從開始柵格點繼續(xù)向終止柵格點方向探索；如果搜索時與mark=1區(qū)域相遇，則轉步驟7;否則mark賦值為3,根據步驟2中方法更新1d_rows?、1d_cols?值，進行下一次搜索；步驟3.4:當mark=3時，以mark=0、mark=1區(qū)域相交行列處為邊界，以1d_rows?、1d_cols?為邊長，與終止點在相交列同側；從開始柵格點繼續(xù)向終止柵格點方向搜索；如果搜索時與mark=1區(qū)域相遇，則轉步驟7;否則mark賦值為2,根據步驟2中方法更新1d_rows3若所有網格全部搜索完畢仍然沒有相遇，則返回起止點之間沒有可行路徑；步驟4:針對野外與路網并存的路徑規(guī)劃；由于路網上行駛車輛的速度、安全性都較野外區(qū)域更高，因此，設車輛在搜索過程中遇到路網的情況選擇上路行駛；當沒有遇到上路點時，采用步驟3中純野外路徑規(guī)劃；當規(guī)劃遇到路網時，當在mark=0區(qū)域搜索中遇到上路點A,凍結該區(qū)域，在mark=1區(qū)域搜索，如果同樣遇到上路點B,則在路距離目標點最近的下路點，以下路點為基準重新開始野外路徑規(guī)劃；步驟5:針對規(guī)劃中路徑對稱性問題，在加載區(qū)域使用增量式搜索算法LPA*,在LPA*算法中引入“代價傾向值”,重用先前增量式加載過程中的信息，避免在新區(qū)域加載時需要重新從開始柵格點計算路徑，提升規(guī)劃效率；步驟6:針對野外與路網并存復雜地圖下的路網斷裂情況：步驟6.1:判斷路網是否斷裂；首先按步驟2至5分別由起止點進行野外路徑規(guī)劃，尋找4上路點；令靠近起始點和終止點的上路點分別為a?、b?,之后在a?、b?之間進行路網規(guī)劃；在將路網數據轉換成的鄰接表結構上使用A*算法，在路網上頂點到終止點的距離估計采用SPFA-BR算法來計算，通過SPFA算法基礎上加入判斷路網斷裂功能以及對最佳下路點搜索功能；點到點的距離采用地圖中路網圖層道路線段距離的真實值，當對a?、b?進行路網規(guī)劃失敗時意味著出現斷路，轉入斷路處理；步驟6.2:尋找下路點，終止點側的下路點選擇原則為路網規(guī)劃中距離起始點最近的點b?作為下路點；起始點側的下路點選擇原則為路網規(guī)劃中距離點b?最近的點a?作為下路點；步驟6.3:得出斷裂路網規(guī)劃結果；保留起始點到點a?的路徑規(guī)劃結果作為path?,點b?到終止點的路徑規(guī)劃結果作為path?,對a?、b?之間使用A*算法在野外區(qū)域規(guī)劃出路徑path?;最終規(guī)劃結果path=path?Upath?Upath?;步驟7:將雙向規(guī)劃的路徑點整理成最終的路徑，并輸出規(guī)劃結果。2.如權利要求1所述的一種復雜環(huán)境下的車輛路徑規(guī)劃方法，其特征在于，步驟1中，對規(guī)劃中心矩形進行地理信息建模，包括以下步驟：步驟1.1:對規(guī)劃區(qū)域進行地理信息建模；采用柵格化處理方式，將地圖根據設置的柵格邊長劃分為若干小柵格；其中，單個柵格其中，1gria表示單個柵格邊長，S表示區(qū)域內部的多邊形面積，n表示區(qū)域內的多邊形個地圖的地理信息數據由多個矢量圖層構成，每個圖層分別存儲區(qū)域內的某類特征地理信息集合；對地理信息數據劃分柵格，對柵格化后的規(guī)劃區(qū)域通過遍歷柵格，在遍歷時將行列數轉換成高斯坐標，借助地理系統(tǒng)引擎獲得所需圖層數據；步驟1.2:土質圖層信息處理；對于土質圖層的信息，在路徑規(guī)劃時參考車輛在不同土質上的平均行駛速度，以車輛在平沙地的行駛速度為基準，在建模階段設置柵格u的土質速度比例uratio,由下式計算得行駛速度；步驟1.3:拓撲路網建模；采用圖論中的有向圖結構存儲路網進行建模，生成符合規(guī)劃需求的有向圖；將地圖道路矢量圖層中的數據構建為拓撲模型，表示為如下式：5將路網采取有向圖建模后，將矢量路網數據以圖的結構存儲，以3.如權利要求1所述的一種復雜環(huán)境下的車輛路徑規(guī)劃方法，其特征在于，步驟5包括如果某些點的代價發(fā)生改變，只計算被該點改變代價受到影響的g*(s),沒有被影響的點的為結點與目標點的距離，在原有啟發(fā)值h(s)的基礎上加入直線軌跡偏離評估值sh,sh的值cross=|(s.x-Send·X)*(start·y-send·y)-(sstart·x-Send·x)*6目標向量b之間的叉積，d表示當前點s到起始點與終止點所在直線的垂直距離，tri是當前點到起始點與終止點的距離之和減去起始點到終止點的距離，點s距離起始點到終點連接的直線越近則該值越小，h??a是根據結點與終止點距離得出啟發(fā)值，sh由d與tri計算得出，7技術領域[0001]本發(fā)明涉及一種復雜環(huán)境下的車輛路徑規(guī)劃方法，屬于智能交通技術領域。背景技術[0002]在智能交通領域，隨著無人駕駛、車路協同等技術快速發(fā)展應用，系統(tǒng)對于車輛路徑規(guī)劃能力提出了越來越高的要求。[0003]車輛路徑規(guī)劃，是根據規(guī)劃目標和車輛性能及其它約束條件，通過算法快速規(guī)劃出一條或者多條最優(yōu)路徑供用戶決策。隨著當前導航技術的發(fā)展，車輛已經不僅局限于拓撲路網環(huán)境，在特殊情況下，處于野外的越野車輛在無路網拓撲或野外與路網并存的情況下，如何高效準確的進行路徑規(guī)劃是值得研究的問題。[0004]除了傳統(tǒng)的大規(guī)模路網的城市環(huán)境外，在地形復雜、存在無道路區(qū)域的野外環(huán)境也同樣面臨路徑規(guī)劃需求。無路網環(huán)境下對地理信息建模，需要綜合考慮眾多環(huán)境地形因者的雙向尋路算法是在柵格路徑規(guī)劃中被廣泛采用的經典算法。但是，經典路徑規(guī)劃算法如Dijkstra算法和A*搜索算法在大規(guī)模網格環(huán)境下效率并不樂觀。Dijkstra算法需要0(n2)的時間復雜度尋找最佳路徑。而A*算法在面對密集障礙物時同樣需要大量時間進行規(guī)劃才能到達終點，計算量會隨搜索空間增大指數級增長。特別是野外路徑規(guī)劃應用場景通常地圖面積較大，海量數據加之計算能力有限，使得規(guī)劃時間過長，是導致大型地圖中尋路性能瓶頸的重要原因。如何進一步提升大規(guī)模地圖下的路徑規(guī)劃效率，是本領域研究的重點。[0005]另外，在路徑規(guī)劃領域中，啟發(fā)式算法通常存在重復搜索的問題，在相同的開始位置和終止位置上一般存在許多可能的代價相同的最優(yōu)路徑，這些路徑是相互對稱的，它們之間唯一的區(qū)別在于車輛移動的順序。解決啟發(fā)式路徑規(guī)劃算法的對稱重復搜索問題，如何針對大規(guī)模地圖中打破路徑對稱性，減少重復搜索，具有重要意義。發(fā)明內容[0006]本發(fā)明的目的是針對現有技術存在的不足和缺陷，為了有效解決在自動駕駛中的車輛路徑規(guī)劃耗時嚴重，以及現有算法存在的因路徑對稱性導致重復搜索、效率低下等技術問題，創(chuàng)造性地提出一種復雜環(huán)境下的車輛路徑規(guī)劃方法。[0007]本方法的創(chuàng)新點在于：首次提出了一種增量式加載與搜索路徑規(guī)劃算法(IncrementalLoadandSearchPathPla規(guī)劃算法中重用之前已加載區(qū)域的規(guī)劃結果，進一步提高路徑規(guī)劃的速度。在加載地圖數據時采用增量式方法，避免一次性加載大量地圖數據，大幅減少了地理信息處理時間。在加載區(qū)域范圍內，使用打破路徑對稱性的增量式搜索路徑規(guī)劃算法，進一步提升了路徑規(guī)劃速度。同時，針對現有基于啟發(fā)式搜索算法的對稱重復搜索問題，引入直線軌跡偏離值，區(qū)別原本具有相同優(yōu)先級的對稱點打破路徑的對稱性，減少算法對冗余柵格的探索，提升了規(guī)劃速度。8[0008]本發(fā)明是采用以下技術方案實現的。[0009]一種復雜環(huán)境下的車輛路徑規(guī)劃方法，包括以下步驟：[0010]步驟1:選定路徑起始點和終止點。以起止點為基準規(guī)劃出一個矩形，以該矩形作為基準向外擴展，得到將基準矩形包裹在內的規(guī)劃中心矩形。[0012]步驟1.1:對規(guī)劃區(qū)域進行地理信息建模。[0013]本發(fā)明采用柵格化處理方式，將地圖根據設置的柵格邊長劃分為若干小柵格。其n表示區(qū)域內的多邊形個數。[0016]地圖的地理信息數據書由多個矢量圖層構成，每個圖層分別存儲區(qū)域內的某類特征地理信息集合。對地理信息數據劃分柵格，如圖1所示，對柵格化后的規(guī)劃區(qū)域通過遍歷柵格，在遍歷時將行列數轉換成高斯坐標，借助地理系統(tǒng)引擎獲得所需圖層數據。[0017]步驟1.2:土質圖層信息處理。[0018]對于土質圖層的信息，在路徑規(guī)劃時參考車輛在不同土質上的平均行駛速度，以車輛在平沙地的行駛速度為基準，在建模階段設置柵格u的土質速度比例uratio,由式2計算[0020]其中，Ysoi?為當前網格土質所對應的車輛行駛速度，Yrlatsana是土質為平沙地時的車輛行駛速度。[0021]步驟1.3:拓撲路網建模。[0022]本發(fā)明采用圖論中的有向圖結構存儲路網進行建模，生成符合規(guī)劃需求的有向[0023]具體地，將地圖道路矢量圖層中的數據構建為拓撲模型，表示為如下式：[0028]其中，G表示整個道路拓撲圖，V表示路網中的道路起止點以及不同道路連接點抽象成的頂點，v;表示第i個頂點；W表示弧的代價值，w;;表示第i和j個結點間的代價值；E是道路抽象成為的邊集，e?表示第i條邊，集合中的每個元素是一條道路，由道路連接的路口[0030]其中，Sa是該道路的起始點id,E是該道路的終止點id;Q是該道路的屬性信息，包括該道路的道路等級、道路的長度，P是道路的定位點集合。9儲路網。別初始化為0。將增量式加載地圖在每次加載地圖回合中增加的行、列數值分別定義為符號。本次的規(guī)劃區(qū)域矩形行數/列數增加值等于總行數/列數減去已經加載的行數/列數的值，[0048]步驟3:增量式加載路徑規(guī)劃。[0049]針對沒有路徑的情況，根據mark標記值加載對應區(qū)域地圖數據，使用啟發(fā)式算法[0050]首先按照步驟2對地圖進行網格化建模，對路網進行建模，將初始mark賦值為0,再根據mark的值加載相應區(qū)域地圖，并使用啟發(fā)式算法進行計算。矩形范圍內的地理數據。加載內容包括路徑規(guī)劃所需要考慮的所有地理信息(如常見的高程信息、水系信息、土質信息和道路信息等)。數據加載完成后，使用啟發(fā)式路徑規(guī)劃算法(如A*、LPA*算法等),計算從起始點所在柵格到終止點所在柵格的可行路徑。[0052]不同于一般的路徑規(guī)劃過程，在本發(fā)明中，超過當前已加載矩形范圍的柵格會被認為不可通行，如果待更新柵格點超過了已加載矩形的范圍將不會考慮該柵格。直到規(guī)劃過程中存儲的待擴展柵格點的優(yōu)先隊列為空時，本次搜索結束。在本發(fā)明中，從優(yōu)先隊列中取出柵格為擴展柵格點，擴展柵格和以擴展柵格為中心展開的八個方向的柵格統(tǒng)稱為探索[0053]步驟3.2:當mark=1時，加載以終止點為中心，以1d_rows?、1d_cols?范圍內的地理數據。使用啟發(fā)式搜索算法在矩形范圍內從終止點向起始點進行路徑規(guī)劃。[0054]同樣不考慮超過當前已加載矩形范圍的柵格。如果搜索時與mark=0區(qū)域相遇，則轉步驟7,否則mark賦值為0,以步驟2中式(10)、(11)更新1d_rows?、1d_cols?值，進行下一次[0055]步驟3.3:當mark=0、1區(qū)域有障礙物，全部搜索完畢未相遇時，域地理信息，從開始柵格點繼續(xù)向終止柵格點方向探索。如果搜索時與mark=1區(qū)域相遇，次搜索。ld_cols?為邊長，與終止點在相交列同側。從開始柵格點繼續(xù)向終止柵格點方向搜索。如果搜索時與mark=1區(qū)域相遇，則轉步驟7。否則mark賦值為2,以步驟2中式(12)、(13)更新1d_[0057]若所有網格全部搜索完畢仍然沒有相遇，則返回起止點之間沒有可行路徑。[0058]由此可見，本發(fā)明通過在不同mark區(qū)域中根據加載后規(guī)劃的情況進行區(qū)域切換，這樣每次只加載一小塊區(qū)域數據，避免了一次性加載大量數據導致機器空間不足或因為計算過程中加載大量數據導致模型計算速度過慢的問題。[0059]步驟4:針對野外與路網并存的路徑規(guī)劃。[0061]當沒有遇到上路點時，采用步驟3中純野外路徑規(guī)劃。當規(guī)劃遇到路網時，當在B,則在路網拓撲上進行從A到B的路徑規(guī)劃。如果路網規(guī)劃失敗(A到B之間沒有連通的路路徑的對稱性，減少冗余柵格的探索，提升規(guī)劃效率。在加載區(qū)域使用增量式搜索算法價發(fā)生改變，只計算被該點改變代價受到影響的g*(s),沒有被影響的點的開始距離不[0073]步驟5.2:打破路徑對稱性，進一步減少冗余搜索。在車輛路徑規(guī)劃時啟發(fā)函數h(s)通常取為結點與目標點的距離，在原有啟發(fā)值h(s)的基礎上加入了直線軌跡偏離評估值sh,sh的值受當前點s到起始點到終點sen的直線距離的影響，在啟發(fā)值h上加點到起始點與終止點的距離之和減去起始點到終止點的距離，點s距離起始點到終點連接點中間連線的點具有更高的優(yōu)先級，從而實現打破對稱路徑尋找上路點。令靠近起始點和終止點的上路點分別為a?、b?,之后在a?、b?之間進行路網規(guī)用來估計路網規(guī)劃階段的啟發(fā)值，因此，本發(fā)明在路網上頂點到終止點的距離估計采用SPFA-BR(ShortestPathFasterAlgorithm-BrokenRoad)算法來計算，通過在經典的SPFA(ShortestPathFasterAlgorithm)算法基礎上加入判斷路網斷裂功能以及對最佳的點b?作為下路點。起始點側的下路點選擇原則為路路點。點b?到終止點的路徑規(guī)劃結果作為path?,對a?、b?之間使用A*算法在野外區(qū)域規(guī)劃出路徑[0091]1.本發(fā)明在加載地圖數據時采用增量式方法，避免了一次性將所有地圖信息加載至內存空間，大幅減少了地理信息處理時間。在加載地理信息后的路徑規(guī)劃階段采用啟發(fā)式搜索算法進行路徑的規(guī)劃，有效縮短規(guī)劃時間，顯著提高了搜索效率。實驗證明，本方法能夠有效縮短規(guī)劃時間。[0092]2.本方法對由于地圖數據缺失或任務規(guī)劃需要造成的路網斷裂等問題，提出了有效的解決方案，能夠判斷路網是否存在斷裂，如果存在斷裂，對于從起點開始的路網選擇距離終點最近的點作為下路點，而對于從終點開始反向規(guī)劃的路網則選擇距離起點最近的點作為下路點。最終保證在路網存在斷裂的情況下，也能夠規(guī)劃出可行的路徑。附圖說明[0093]圖1為本發(fā)明方法的柵格化地理信息建模示意圖；[0094]圖2為具體實施實例中本發(fā)明方法的流程圖；[0095]圖3為具體實施實例中本發(fā)明方法首次加載mark=0區(qū)域地圖信息示意圖；[0096]圖4為具體實施實例中本發(fā)明方法首次加載mark=1區(qū)域后示意圖；[0097]圖5為具體實施實例中地圖增量式加載路徑規(guī)劃成功示意圖；[0098]圖6為具體實施實例中mark=2與mark=3區(qū)域地圖信息加載示意圖；[0099]圖7為具體實施實例中增量式加載mark=2與mark=3區(qū)域后路徑規(guī)劃成功示意[0100]圖8為具體實施實例中遇到路網情況示意圖。具體實施方式[0101]下面結合附圖和實施例，對本發(fā)明方法做進一步詳細說明。[0102]實施例[0103]本實施例敘述了本發(fā)明一種復雜環(huán)境下的車輛路徑規(guī)劃方法的具體實現過程，圖2為本實施例的實現流程示意圖。[0104]從圖2中可以看出，本發(fā)明及本實施例的具體實現步驟如下：[0105]步驟1:在選定起始點和終止點后，以起止點為基準規(guī)確定一個矩形，并以該矩形作為基準向外擴展N米，得到將基準矩形包裹在內的規(guī)劃中心矩形。對規(guī)劃中心矩形進行地理信息建模。[0106]步驟1.1:根據式(1)計算柵格化邊長，將整片區(qū)域柵格化。[0107]步驟1.2:讀取并保存區(qū)域內所有路網信息。[0109]步驟2:沒有遇到路網情況下，增量式加載地形，進行啟發(fā)式搜索。[0110]步驟2.1:根據mark值加載對應區(qū)域的地形信息。當mark=0(整個區(qū)域的左上部分)計算完成后，轉到mark=1(整個區(qū)域的[0111]步驟2.2:以圖5為例，起始點所在區(qū)域為mark=0,終止點所在區(qū)域為mark=1.對mark=0、1區(qū)域進行對稱加載搜索，使用LPA*算法(或A*算法等啟發(fā)式路徑規(guī)劃算法)并使用式(16)(22)計算已加載點的啟發(fā)式信息f=g+h,根據啟發(fā)式信息對加載區(qū)域進行規(guī)劃。根據式(10)、(11)更新每次加載的網格區(qū)域，直到兩個區(qū)域中出現了相遇點則路徑規(guī)劃成功。如圖5所示。圖中白色網格為未加載網格，淺灰色網格代表規(guī)劃中探索過的柵格，黑色網格代表啟發(fā)式搜索中未探索柵格，白色虛線連接代表規(guī)劃出的路徑。[0112]步驟2.3:遇到障礙物情況，以圖6為例，其中起始點所在區(qū)域為mark=0,終止點所在區(qū)域為mark=1,起始點同側的區(qū)域為mark=2(圖6中為左下區(qū)域網格),終止點同側的區(qū)域為mark=3(圖6中為右上區(qū)域網格)。當mark=0、1區(qū)域加載完成卻沒有相遇時(如圖6中因為標識為不可通行的黑色網格區(qū)域阻擋),對mark=2、3區(qū)域進行