一種基于拓撲地圖的自主泊車路徑協調方法技術

本發明專利技術公開一種基于拓撲地圖的自主泊車路徑協調方法，是基于V2X通信，自主泊車控制器通過停車場服務器提供的停車場拓撲地圖、目標空置車位和動態道路通行矩陣，規劃行車的全局路徑和泊車路徑，然后通過融合協調，形成平滑完整的自主泊車路徑。本發明專利技術方法實現了在車輛運行前就規劃出從停車場入口到車位的自主泊車路徑，實現全局路徑與泊車路徑的無偏差融合，使低速自動駕駛與自動泊車能夠平滑過渡，解決了低速自動駕駛與自動泊車的高效切換問題。

【技術實現步驟摘要】
一種基于拓撲地圖的自主泊車路徑協調方法
本專利技術涉及智能網聯汽車
，具體涉及一種基于拓撲地圖的停車場自主泊車路徑協調方法。
技術介紹
隨著汽車智能化的發展，自動泊車輔助系統(AutomatedParkingAssist,APA)已在實際生活中幫助人們提高了泊車的效率與安全性。然而傳統的自動泊車需要駕駛員在停車場中尋找車位，然后將車輛準確地停在泊車位置，泊車整個過程仍需要駕駛員介入，未能減輕該工況下的駕駛負擔，因而研發更高智能化程度的自主泊車系統，或稱為自動代客泊車(AutomatedValetParking,AVP)，使汽車能夠自主的從停車場入口落客區完成整個低速自動駕駛與自主泊車任務，將人們徹底從泊車工況中解放出來，是彌補APA現有功能缺陷的有效途徑。然而自主泊車的研究尚存在諸多問題與局限性。在自動駕駛的技術背景下，將低速自動駕駛與傳統的自動泊車技術相結合是實現自主泊車的有效途徑。但采用高精度地圖的傳統自動駕駛方案不僅計算代價高，硬件成本高，也難以與現有泊車方法結合，并不適用當前APA系統升級的平滑過渡，也無法針對性地實現自主泊車功能。而低速自動駕駛與自動泊車的高效切換，即如何解決車輛停車位置、切換時機以及車輛到達車位后的再確認，是自主泊車的關鍵問題。
技術實現思路
基于現有技術的不足，本專利技術的目的在于提出一種基于拓撲地圖的自主泊車路徑協調方法，著力于解決車輛從停車場落客區到停車位之間的全局路徑與泊車路徑的融合問題，進而確定低速自動駕駛與泊車切換時的停車位置、切換時機，同時避免了車輛到達車位之后再確認泊車。主要涉及基于激光雷達點云數據的停車場拓撲地圖構建和基于拓撲地圖的自主泊車路徑的協調方法。本專利技術解決以上技術問題所采取的技術方案如下：一種基于拓撲地圖的自主泊車路徑協調方法，該方法用于融合全局路徑與泊車路徑，確定低速自動駕駛與泊車系統切換時的車輛停車位置與切換時機。具體如下：基于V2X通信，自主泊車控制器根據停車場服務器提供的停車場拓撲地圖、目標空置車位和動態道路通行矩陣，規劃行車的全局路徑和泊車路徑，然后通過融合協調，形成平滑完整的自主泊車路徑。包含如下步驟：(1)在停車場端，布置停車場服務器，所述服務器中存儲有停車場拓撲地圖，同時具有根據停車場監控實時更新動態道路通行矩陣和空余車位編號，為車輛分配目標空置車位的能力，所述地圖中包含有道路主節點、停車位對應道路節點、各節點構建的鄰接矩陣、車位信息；在車輛端，布置自主泊車控制器，所述控制器基于V2X通信從停車場服務器下載停車場拓撲地圖，并接收目標空置車位信息和當前動態道路通行矩陣；(2)所述自主泊車控制器根據車輛當前位置及停車場服務器提供的信息，基于停車場拓撲地圖，規劃車輛在道路上可通行的全局路徑，然后根據全局路徑末點的行車方向與停車位之間的方位關系確定泊車方向，再根據停車位泊車類型、全局路徑末點距離車位的橫向距離、停車位頂點坐標信息和車輛的外形參數規劃泊車路徑；(3)經過全局路徑與泊車路徑的拼接、節點優化、平滑處理、路徑擬合，得到無偏差融合且平滑過渡的自主泊車路徑。其中，停車場拓撲地圖構建的方法是，基于激光雷達點云數據：1)在激光雷達采集前的預定義坐標系下，從激光雷達采集的點云數據中提取停車位頂點坐標、車道線節點坐標，停車位頂點指車位的四個角點，車道線節點指各路段車道線的端點；2)根據預定義坐標系的平面中心原點與平面法向量，將步驟1)提取的坐標經過投影變換、四元數變換后變為平面直角坐標系下的坐標；3)選取停車場內某一點作為坐標系原點，建立停車場坐標系，然后將2)中的坐標轉至停車場坐標系下；4)設計停車位頂點、道路主節點和停車位對應道路節點：(a)每個停車位的四個角點都作為停車位頂點；(b)道路主節點：選取同向車道中心線之間的交點作為道路主節點，且主節點之間連線與對應道路側的車位線平行；(c)停車位對應道路節點：在預設的車位坐標系基礎上確定：即在車位中心面向道路方向時，以車位左后頂點為車位坐標系原點，左后頂點到右后頂點的方向為車位坐標系x軸正方向，左后頂點到左前頂點的方向為y軸正方向；然后，過車位中心且平行于y軸的直線與所面向道路的兩個道路主節點連線的交點作為該車位對應的道路節點；5)構建鄰接矩陣：對步驟(4)設計的道路主節點和停車位對應道路節點統一編號，對于任意兩節點i,j，根據從節點i到節點j的直接連接情況，確定矩陣中的元素ai,j為0還是1，可通行設置為1，不可通行為0，最終根據節點總數N構建出N*N的鄰接矩陣Ar+p；6)對地圖信息統一描述，包括道路拓撲信息和車位信息：(a)道路拓撲信息Map包括由道路主節點信息Noder和車位對應道路節點信息Nodep構成的所有節點信息Noder+p，以及由鄰接矩陣構成的節點連接狀態信息：Map＝G(Noder+p,Ar+p)式中，Noder和Nodep中各節點坐標由停車場坐標系橫、縱坐標表示，Nr為道路主節點個數，Np為停車位個數也即車位對應道路節點個數；Ar+p由描述節點之間連接關系的元素ai,j、aj,i構成，ai,j、ai,j∈{0,1}；(b)車位信息表述如下：Pp,i＝{np,i,Tp,i,Wi,Li,Di,hi,Ppv,i}其中，停車場所有車位的車位頂點信息如下:Np為停車位個數，每個車位四個頂點Vlr,Vrr,Vrf,Vlf分別表示車位左后、右后、右前、左前頂點；np,i代表第i個車位的車位編號，Tp,i代表第i個車位的車位類型，Wi，Li分別代表第i個車位的寬和長，Di表示第i個車位前方的通車道路寬，hi表示第i個車位泊車起點的橫向位移參數；Ppv,i代表第i個車位的四個頂點的坐標；并構建車位編號與停車位對應道路節點之間的對應關系如下：P2R＝{np,i,Nodep,i,DPi,LPi}Nodep,i代表第i個車位對應的停車位對應道路節點；DPi代表第i個車位所允許的泊車方向；LPi代表第i個車位直線泊車需要添加的節點編號。其中，基于停車場道路節點設計原則與自主泊車路徑規劃原則，融合全局路徑與泊車路徑：1)基于停車場道路節點設計原則，根據車位信息中的泊車類型，以全局路徑Pathglobal末點的方向判斷泊車方向，距離車位的橫向距離作為泊車規劃的初始條件，進而規劃出泊車路徑Pathparking：Drtdriving,i∈{-1,0,1}式中，(xgp,i,ygp,i)為路徑第i個節點的坐標，Ngp為路徑節點數，Pathparking,i為第i段泊車路徑，(xppi,j,yppi,j)為第i段路徑第k個節點的坐標，Nppi為第i段路徑的節點數，Drtdriving,i為第i段路徑允許車輛的行駛方向，定義1為前進，-1為倒車，0表示路徑不存在；2)將全局路徑末點替換為泊車起始點，若泊車路徑存在前行部分，在節點替換后將全局路徑與泊車路徑拼接共同作為前進段路徑Pathforward，倒車部分作為倒車段路徑Pathbackward，表述如下：Pathforward包括兩段路徑，若泊車規劃不存在車輛前進部分，Pathforward僅取式中第一部分；3)對拼接后的路徑進行節點優化，根據各節點的方向信息判斷相關聯的節點是否在同一道路直線上，如果在同一條直線上，僅保留首末節點，中間點剔除，節點本文檔來自技高網...

【技術保護點】
1.一種基于拓撲地圖的自主泊車路徑協調方法，其特征在于，基于V2X通信，自主泊車控制器根據停車場服務器提供的停車場拓撲地圖、目標空置車位和動態道路通行矩陣，規劃行車的全局路徑和泊車路徑，然后通過融合協調，形成平滑完整的自主泊車路徑；包含如下步驟：(1)在停車場端，布置停車場服務器，所述服務器中存儲有停車場拓撲地圖，同時具有根據停車場監控實時更新動態道路通行矩陣和空余車位編號，為車輛分配目標空置車位的能力，所述地圖中包含有道路主節點、停車位對應道路節點、各節點構建的鄰接矩陣、車位信息；在車輛端，布置自主泊車控制器，所述控制器基于V2X通信從停車場服務器下載停車場拓撲地圖，并接收目標空置車位信息和當前動態道路通行矩陣；(2)所述自主泊車控制器根據車輛當前位置及停車場服務器提供的信息，基于停車場拓撲地圖，規劃車輛在道路上可通行的全局路徑，然后根據全局路徑末點的行車方向與停車位之間的方位關系確定泊車方向，再根據停車位泊車類型、全局路徑末點距離車位的橫向距離、停車位頂點坐標信息和車輛的外形參數規劃泊車路徑；(3)經過全局路徑與泊車路徑的拼接、節點優化、平滑處理、路徑擬合，得到無偏差融合且平滑過渡的自主泊車路徑。...

【技術特征摘要】
1.一種基于拓撲地圖的自主泊車路徑協調方法，其特征在于，基于V2X通信，自主泊車控制器根據停車場服務器提供的停車場拓撲地圖、目標空置車位和動態道路通行矩陣，規劃行車的全局路徑和泊車路徑，然后通過融合協調，形成平滑完整的自主泊車路徑；包含如下步驟：(1)在停車場端，布置停車場服務器，所述服務器中存儲有停車場拓撲地圖，同時具有根據停車場監控實時更新動態道路通行矩陣和空余車位編號，為車輛分配目標空置車位的能力，所述地圖中包含有道路主節點、停車位對應道路節點、各節點構建的鄰接矩陣、車位信息；在車輛端，布置自主泊車控制器，所述控制器基于V2X通信從停車場服務器下載停車場拓撲地圖，并接收目標空置車位信息和當前動態道路通行矩陣；(2)所述自主泊車控制器根據車輛當前位置及停車場服務器提供的信息，基于停車場拓撲地圖，規劃車輛在道路上可通行的全局路徑，然后根據全局路徑末點的行車方向與停車位之間的方位關系確定泊車方向，再根據停車位泊車類型、全局路徑末點距離車位的橫向距離、停車位頂點坐標信息和車輛的外形參數規劃泊車路徑；(3)經過全局路徑與泊車路徑的拼接、節點優化、平滑處理、路徑擬合，得到無偏差融合且平滑過渡的自主泊車路徑。2.根據權利要求1所述的基于拓撲地圖的自主泊車路徑協調方法，其特征在于，停車場拓撲地圖構建的方法是：基于激光雷達點云數據，1)在激光雷達采集前的預定義坐標系下，從激光雷達采集的點云數據中提取停車位頂點坐標、車道線節點坐標，停車位頂點指車位的四個角點，車道線節點指各路段車道線的端點；2)根據預定義坐標系的平面中心原點與平面法向量，將步驟1)提取的坐標經過投影變換、四元數變換后變為平面直角坐標系下的坐標；3)合理選取坐標系原點，建立停車場坐標系，然后將2)中的坐標轉換至停車場坐標系下；4)設計停車位頂點、道路主節點和停車位對應道路節點：(a)每個停車位的四個角點都作為停車位頂點；(b)道路主節點：選取同向車道中心線之間的交點作為道路主節點，且主節點之間連線與對應道路側的車位線平行；(c)停車位對應道路節點：在預設的車位坐標系基礎上確定：即在車位中心面向道路方向時，以車位左后頂點為車位坐標系原點，左后頂點到右后頂點的方向為車位坐標系x軸正方向，左后頂點到左前頂點的方向為y軸正方向；然后，過車位中心且平行于y軸的直線與所面向道路的兩個道路主節點連線的交點作為該車位對應的道路節點；5)構建鄰接矩陣：對步驟(4)設計的道路主節點和停車位對應道路節點統一編號，對于任意兩節點i,j，根據從節點i到節點j的直接連接情況，確定矩陣中的元素ai,j為0還是1，可通行設置為1，不可通行為0，最終根據節點總數N構建出N*N的鄰接矩陣Ar+p；6)對地圖信息統一描述，包括道路拓撲信息和車位信息：(a)道路拓撲信息Map包括由道路主節點信息Noder和車位對應道路節點信息Nodep構成的所有節點信息Noder+p，以及由鄰接矩陣構成的節點...

【專利技術屬性】
技術研發人員：羅禹貢，王永勝，江發潮，陳銳，黃晨，秦兆博，
申請(專利權)人：清華大學，
類型：發明
國別省市：北京,11