智能泊車系統(tǒng)的障礙物檢測與庫位判別方法及實(shí)現(xiàn)系統(tǒng)的制作方法
【專利摘要】一種智能泊車系統(tǒng)的障礙物檢測與庫位判別方法，該方法共包含六個部分：依靠雷達(dá)和定位系統(tǒng)完成數(shù)據(jù)采集；依靠坐標(biāo)變換實(shí)現(xiàn)障礙物數(shù)據(jù)點(diǎn)的全局定位；依靠分析數(shù)據(jù)點(diǎn)間的幾何關(guān)系實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的分類；依靠最佳外接矩形擬合算法實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的擬合；依靠分析矩形擬合結(jié)果間的幾何關(guān)系實(shí)現(xiàn)庫位判別；依靠庫位信息協(xié)議實(shí)現(xiàn)數(shù)據(jù)發(fā)送。本發(fā)明還公開了實(shí)現(xiàn)所述方法的智能泊車系統(tǒng)的障礙物檢測與庫位判別系統(tǒng)，包括傳感器單元和信號處理單元。本發(fā)明對于有前后固定障礙物的庫位，能夠?qū)崿F(xiàn)車輛在一定范圍內(nèi)沿任意路徑進(jìn)行庫位檢測，所用到的庫位檢測算法更符合人類判別庫位幾何信息的規(guī)律，具有較高的檢測精度、可靠性和實(shí)用價值。
【專利說明】智能泊車系統(tǒng)的障礙物檢測與庫位判別方法及實(shí)現(xiàn)系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于汽車【技術(shù)領(lǐng)域】，涉及駕駛輔助技術(shù)，特別是智能泊車系統(tǒng)的障礙物檢 測方法。

【背景技術(shù)】
[0002] 隨著汽車保有量的增大，停車位日益緊張，停車難這一問題變得愈加嚴(yán)重。對于沒 有經(jīng)驗(yàn)的新手而言，將車輛安全泊入庫位非常困難；即使對于有經(jīng)驗(yàn)的駕駛員，將車輛停入 狹小的庫位也不是一件容易的事。開發(fā)智能泊車系統(tǒng)，有利于保證泊車過程的安全性，同時 提高泊車過程的舒適性和便捷性。
[0003] 智能泊車系統(tǒng)一般包括以下幾個模塊：庫位檢測模塊、路徑規(guī)劃模塊、路徑跟蹤模 塊、執(zhí)行模塊和人機(jī)界面模塊。庫位檢測模塊是智能泊車系統(tǒng)的基礎(chǔ)，很大程度上影響著整 個泊車系統(tǒng)的精度和性能。
[0004] 目前，智能泊車系統(tǒng)的庫位檢測方法主要包含以下四類：
[0005] 1.基于超聲波雷達(dá)的庫位檢測。由于車輛前端或后端存在圓角，受超聲波測距傳 感器自身特性的局限，在前后端圓角處存在接收不到回波的現(xiàn)象，這將造成檢測到的庫位 較實(shí)際偏大。2.基于毫米波雷達(dá)的庫位檢測。其優(yōu)點(diǎn)在于對具有相對運(yùn)動的物體檢測精 度較高，但是無法檢測相對靜止的物體。3.基于激光雷達(dá)的庫位檢測。其優(yōu)點(diǎn)在于檢測精 度高，但是成本相對高昂。4.基于視覺技術(shù)的庫位檢測。其難點(diǎn)在于圖像易受到陰影、光 照等環(huán)境條件的影響。此類技術(shù)主要包括：（1)基于環(huán)視攝像頭的圖案識別；（2)基于單目 攝像頭的運(yùn)動立體視覺；（3)基于雙目攝像頭的立體視覺；(4)基于雙目攝像頭的結(jié)構(gòu)光分 析。以上方案各有優(yōu)點(diǎn)，不過仍然存在不足，客觀上存在進(jìn)一步改進(jìn)的迫切需要。


【發(fā)明內(nèi)容】

[0006] 本發(fā)明的目的在于提供一種智能泊車系統(tǒng)的障礙物檢測與庫位判別方法及實(shí)現(xiàn) 系統(tǒng)，從而實(shí)現(xiàn)庫位檢測功能，對于有前后固定障礙物的平行、垂直、斜向庫位等都能可靠、 高效地檢出并獲得庫位信息。
[0007] 為達(dá)到以上目的，本發(fā)明所采用的解決方案是：
[0008] -種智能泊車系統(tǒng)的障礙物檢測與庫位判別方法，包括：步驟（a)進(jìn)行數(shù)據(jù)采集； 步驟（b)進(jìn)行坐標(biāo)變換；步驟（C)進(jìn)行數(shù)據(jù)分類；步驟（d)進(jìn)行數(shù)據(jù)擬合；步驟（e)進(jìn)行庫 位判別。
[0009] 進(jìn)一步：步驟（a)包括：（1)以一定的步長依次接收車輛定位數(shù)據(jù)與雷達(dá)數(shù)據(jù)； (2)根據(jù)雷達(dá)的特性與實(shí)際布置情況，篩選出有效的數(shù)據(jù)；（3)若有多個雷達(dá)參與環(huán)境感 知，則進(jìn)行數(shù)據(jù)融合。
[0010] 步驟（b)包括：（1)將雷達(dá)坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至車輛坐標(biāo)系；（2)將車輛 坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至定位系統(tǒng)坐標(biāo)系；（3)將定位系統(tǒng)坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn) 換至全局坐標(biāo)系。 toon] 步驟（c)包括：（1)對于新掃描的數(shù)據(jù)點(diǎn)，求出該點(diǎn)至各已有障礙物的最短距離； (2)若該最小距離值在設(shè)定閾值內(nèi)，則該點(diǎn)歸屬于對應(yīng)物體，否則成立一個新物體。
[0012] 步驟（d)包括：（1)對于同一物體的數(shù)據(jù)點(diǎn)，令其外接矩形的一邊即特征邊與全局 坐標(biāo)系X軸的夾角從負(fù)45度至正45度遍歷，對于該范圍內(nèi)任意的夾角，均可唯一確定外接 矩形；(2)求出使外接矩形特征邊兩端點(diǎn)至物體的距離之和，在上述夾角范圍內(nèi)使該"距離 和"最小的夾角值所對應(yīng)的外接矩形即為所要擬合的結(jié)果。
[0013] 步驟（e)包括如下判別條件：（1)相鄰兩障礙物外接矩形的最短距離大于設(shè)定閾 值；（2)前方障礙物已檢出部分的外接矩形長邊長度大于設(shè)定閾值；（3)相鄰兩障礙物外接 矩形的長邊夾角小于設(shè)定閾值。
[0014] 還包括進(jìn)行數(shù)據(jù)發(fā)送步驟：（1)對符合判別條件的庫位，提取前后障礙物的外接 矩形，并將數(shù)據(jù)轉(zhuǎn)換至庫位坐標(biāo)系；（2)以一定的協(xié)議將數(shù)據(jù)發(fā)送至智能泊車系統(tǒng)路徑規(guī) 劃模塊。
[0015] 實(shí)現(xiàn)上述方法的智能泊車系統(tǒng)的障礙物檢測與庫位判別系統(tǒng)，包括：（1)傳感器 單元：位于車輛車身的兼具角度和距離信號的二維雷達(dá)、具有相對定位功能的定位系統(tǒng)； (2)信號處理單元：雷達(dá)數(shù)據(jù)處理模塊。
[0016] 所述信號處理單元包括數(shù)據(jù)采集模塊、坐標(biāo)變換模塊、數(shù)據(jù)分類模塊、數(shù)據(jù)擬合模 塊、庫位判別模塊與數(shù)據(jù)發(fā)送模塊；
[0017] 優(yōu)選的，數(shù)據(jù)采集模塊用于二維雷達(dá)與定位系統(tǒng)數(shù)據(jù)的采集；坐標(biāo)變換模塊用于 雷達(dá)數(shù)據(jù)點(diǎn)的全局定位；數(shù)據(jù)分類模塊用于分析坐標(biāo)變換后數(shù)據(jù)點(diǎn)之間的幾何位置關(guān)系， 從而確定每一個數(shù)據(jù)點(diǎn)的物體歸屬；數(shù)據(jù)擬合模塊利用最佳外接矩形擬合算法實(shí)現(xiàn)數(shù)據(jù)點(diǎn) 的擬合；庫位判別模塊用于分析矩形擬合結(jié)果間的幾何關(guān)系，從而判斷庫位是否符合泊車 路徑規(guī)劃的條件；數(shù)據(jù)發(fā)送模塊用于整理庫位信息并按協(xié)議發(fā)送至智能泊車系統(tǒng)的路徑規(guī) 劃模塊。
[0018] 所述傳感器單元中的位于車輛車身的二維雷達(dá)，其有效感知角度范圍不小于60 度，有效感知距離不小于4米；或者，優(yōu)選的，二維檢測平面應(yīng)與地面平行，高度介于0. 5米 至1米之間；或者，優(yōu)選的，
[0019] 所述傳感器單元中的具有相對定位功能的定位系統(tǒng)，選自以下一種或兩種以上的 組合：外置安裝在車輛上的動態(tài)精度在IOcm以內(nèi)的GPS全球定位系統(tǒng)；通過內(nèi)置傳感器采 集輪速與車輛轉(zhuǎn)角等數(shù)據(jù)的車輛內(nèi)部參數(shù)位姿估計系統(tǒng)；通過外置傳感器采集外部參考數(shù) 據(jù)，并附以SLAM即時定位與地圖重構(gòu)等技術(shù)的車輛外部參數(shù)位姿估計系統(tǒng)；或者，優(yōu)選的， 所述二維雷達(dá)采用激光雷達(dá)。
[0020] 由于采用了上述技術(shù)方案，本發(fā)明具有以下有益效果：通過融合雷達(dá)傳感器得到 的障礙物角度與距離信息以及定位系統(tǒng)得到的車輛位姿信息，可實(shí)現(xiàn)車輛在一定范圍內(nèi)沿 任意路徑進(jìn)行庫位檢測，使低精度雷達(dá)產(chǎn)品獲得更高精度的庫位檢測結(jié)果；依靠最佳外接 矩形擬合算法，該技術(shù)方案相較傳統(tǒng)庫位檢測方法能夠獲得更符合人類判斷規(guī)律的結(jié)果， 可靠性更強(qiáng)，并能以更少的運(yùn)算量完成任務(wù)。

【專利附圖】

【附圖說明】
[0021] 圖1是本發(fā)明的二維雷達(dá)庫位檢測流程圖。
[0022] 圖2(a)是本發(fā)明的智能泊車系統(tǒng)的定位系統(tǒng)坐標(biāo)系的定義示意圖。
[0023] 圖2(b)是本發(fā)明的智能泊車系統(tǒng)的全局坐標(biāo)系的定義示意圖。
[0024] 圖2(c)是本發(fā)明的智能泊車系統(tǒng)的車輛坐標(biāo)系與雷達(dá)坐標(biāo)系的定義示意圖。
[0025] 圖2(d)是本發(fā)明的智能泊車系統(tǒng)的全局坐標(biāo)系下各坐標(biāo)系的關(guān)系示意圖。
[0026] 圖3是本發(fā)明的數(shù)據(jù)分類實(shí)施例示意圖。
[0027] 圖4(a)是本發(fā)明的數(shù)據(jù)擬合（最佳外接矩形擬合算法）實(shí)施例示意圖（tana =-1)。
[0028] 圖4(b)是本發(fā)明的數(shù)據(jù)擬合（最佳外接矩形擬合算法）實(shí)施例示意圖（變量dis 最小時的tan a )。
[0029] 圖5是本發(fā)明的數(shù)據(jù)擬合實(shí)施例dis-a關(guān)系圖。
[0030] 圖6是本發(fā)明的庫位判別實(shí)施例示意圖。
[0031] 圖7是本發(fā)明的庫位檢測結(jié)果示意圖（全局坐標(biāo)系G，單位：米）。
[0032] 圖8是本發(fā)明的庫位檢測術(shù)語示意圖（右側(cè)平行泊車?yán)?br>
【具體實(shí)施方式】
[0033] 以下結(jié)合附圖所示實(shí)施例（右側(cè)平行泊車）對本發(fā)明作進(jìn)一步的說明。如未特殊 說明，本說明書中的庫位檢測相關(guān)術(shù)語參照圖8 ;如未特殊說明，本說明書中的"航向角"在 笛卡爾坐標(biāo)系下以X軸正方向?yàn)?,逆時針為正。
[0034] 本發(fā)明一種智能泊車系統(tǒng)的障礙物檢測與庫位判別方法，包括：依靠雷達(dá)和定位 系統(tǒng)完成數(shù)據(jù)采集；依靠坐標(biāo)變換實(shí)現(xiàn)障礙物數(shù)據(jù)點(diǎn)的全局定位；依靠分析數(shù)據(jù)點(diǎn)間的幾 何關(guān)系實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的分類；依靠最佳外接矩形擬合算法實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的擬合；依靠分析矩形 擬合結(jié)果間的幾何關(guān)系實(shí)現(xiàn)庫位判別；依靠庫位信息協(xié)議實(shí)現(xiàn)數(shù)據(jù)發(fā)送。
[0035] -種智能泊車系統(tǒng)的障礙物檢測與庫位判別系統(tǒng)，包括：傳感器單元：位于車輛 車身的兼具角度和距離信號的二維雷達(dá)（如激光雷達(dá)等）、具有相對定位功能的定位系統(tǒng) (如GPS全球定位系統(tǒng)、車輛本地位姿估計系統(tǒng)等）；（2)信號處理單元：雷達(dá)數(shù)據(jù)處理模 塊。
[0036] 所述信號處理單元包括數(shù)據(jù)采集模塊、坐標(biāo)變換模塊、數(shù)據(jù)分類模塊、數(shù)據(jù)擬合模 塊、庫位判別模塊與數(shù)據(jù)發(fā)送模塊。數(shù)據(jù)采集模塊用于二維雷達(dá)與定位系統(tǒng)數(shù)據(jù)的采集；坐 標(biāo)變換模塊用于雷達(dá)數(shù)據(jù)點(diǎn)的全局定位；數(shù)據(jù)分類模塊用于分析坐標(biāo)變換后數(shù)據(jù)點(diǎn)之間的 幾何位置關(guān)系，從而確定每一個數(shù)據(jù)點(diǎn)的物體歸屬；數(shù)據(jù)擬合模塊利用最佳外接矩形擬合 算法實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的擬合；庫位判別模塊用于分析矩形擬合結(jié)果間的幾何關(guān)系，從而判斷庫 位是否符合泊車路徑規(guī)劃的條件；數(shù)據(jù)發(fā)送模塊用于整理庫位信息并按協(xié)議發(fā)送至智能泊 車系統(tǒng)的路徑規(guī)劃模塊。
[0037] 所述傳感器單元中的位于車輛車身的二維雷達(dá)，其有效感知角度范圍不小于60 度，有效感知距離不小于4米，二維檢測平面應(yīng)與地面平行，高度介于0. 5米至1米之間。 [0038] 所述傳感器單元中的具有相對定位功能的定位系統(tǒng)，可以是外置安裝在車輛上的 動態(tài)精度在IOcm以內(nèi)的GPS全球定位系統(tǒng)；也可以是通過內(nèi)置傳感器采集輪速與車輛轉(zhuǎn)角 等數(shù)據(jù)的車輛內(nèi)部參數(shù)位姿估計系統(tǒng)；也可以是通過外置傳感器采集外部參考數(shù)據(jù)，并附 以SLAM即時定位與地圖重構(gòu)等技術(shù)的車輛外部參數(shù)位姿估計系統(tǒng)；也可以是以上系統(tǒng)的 組合。
[0039] 所述信號處理單元中的數(shù)據(jù)采集模塊進(jìn)行數(shù)據(jù)采集的方法，包括如下步驟：（1) 以一定的步長依次接收車輛定位數(shù)據(jù)與雷達(dá)數(shù)據(jù)；（2)根據(jù)雷達(dá)的特性與實(shí)際布置情況， 篩選出有效的數(shù)據(jù)；（3)若有多個雷達(dá)參與環(huán)境感知，則需進(jìn)行數(shù)據(jù)融合。
[0040] 所述信號處理單元中的坐標(biāo)變換模塊進(jìn)行坐標(biāo)變換的方法，包括如下步驟：（1) 將雷達(dá)坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至車輛坐標(biāo)系；（2)將車輛坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換 至定位系統(tǒng)坐標(biāo)系；(3)將定位系統(tǒng)坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至全局坐標(biāo)系。
[0041] 所述信號處理單元中的數(shù)據(jù)分類模塊進(jìn)行數(shù)據(jù)分類的方法，包括如下步驟：（1) 對于新掃描的數(shù)據(jù)點(diǎn)，求出該點(diǎn)至各已有障礙物的最短距離；(2)若該最小距離值在設(shè)定 閾值內(nèi)，則該點(diǎn)歸屬于對應(yīng)物體，否則成立一個新物體。
[0042] 所述信號處理單元中的數(shù)據(jù)擬合模塊進(jìn)行數(shù)據(jù)擬合的方法，包括如下步驟：（1) 對于同一物體的數(shù)據(jù)點(diǎn)，令其外接矩形的一邊（稱為特征邊）與全局坐標(biāo)系X軸的夾角從 負(fù)45度至正45度遍歷，對于該范圍內(nèi)任意的夾角，均可唯一確定外接矩形；（2)求出使外 接矩形特征邊兩端點(diǎn)至物體的距離之和，在上述夾角范圍內(nèi)使該"距離和"最小的夾角值所 對應(yīng)的外接矩形即為所要擬合的結(jié)果。
[0043] 所述信號處理單元中的庫位判別模塊進(jìn)行庫位判別的方法，包括如下判別條件： (1)相鄰兩障礙物外接矩形的最短距離大于設(shè)定閾值；（2)前方障礙物已檢出部分的外接 矩形長邊長度大于設(shè)定閾值；（3)相鄰兩障礙物外接矩形的長邊夾角小于設(shè)定閾值。
[0044] 所述信號處理單元中的數(shù)據(jù)發(fā)送模塊進(jìn)行數(shù)據(jù)發(fā)送的方法，包括如下步驟：（1) 對符合判別條件的庫位，提取前后障礙物的外接矩形，并將數(shù)據(jù)轉(zhuǎn)換至庫位坐標(biāo)系；(2)以 一定的協(xié)議將數(shù)據(jù)發(fā)送至智能泊車系統(tǒng)路徑規(guī)劃模塊。
[0045] 具體而言，傳感器單元包括：位于車輛車身的兼具角度和距離信號的二維雷達(dá) (如激光雷達(dá)等）、具有相對定位功能的定位系統(tǒng)（如GPS全球定位系統(tǒng)、車輛本地位姿估 計系統(tǒng)等）。
[0046] (1)二維雷達(dá)
[0047] 位于車輛車身的二維雷達(dá)，其有效感知角度范圍不小于60度，有效感知距離不小 于4米，二維檢測平面應(yīng)與地面平行，高度介于0. 5米至1米之間。雷達(dá)的布置方式為：對 于泊車位在車輛所在道路右側(cè)的情況（右側(cè)方案），可以在車輛右側(cè)車身靠前的位置正向 車輛右側(cè)安裝雷達(dá)，也可以在車輛右前轉(zhuǎn)向燈位置附近斜向安裝雷達(dá)，無論采用何種安裝 方式，車輛坐標(biāo)系V(如圖2(c))的第三、四象限應(yīng)各至少涵蓋雷達(dá)10度的檢測角度范圍， 以保證正向、背向行車方向一側(cè)的數(shù)據(jù)點(diǎn)都能被準(zhǔn)確掃描；對于泊車位在車輛所在道路左 側(cè)的情況（左側(cè)方案），布置方式為右側(cè)方案基于車輛坐標(biāo)系X軸的鏡像翻轉(zhuǎn)；考慮到智能 泊車系統(tǒng)功能的完整性，車輛應(yīng)同時配備右側(cè)方案與左側(cè)方案兩套雷達(dá)設(shè)備。
[0048] (2)定位系統(tǒng)
[0049] 具有相對定位功能的定位系統(tǒng)，可以是外置安裝在車輛上的動態(tài)精度在IOcm以 內(nèi)的GPS全球定位系統(tǒng)，GPS的定位參考點(diǎn)水平位置可位于車輛后軸中點(diǎn)；也可以是基于內(nèi) 置傳感器采集輪速與車輛轉(zhuǎn)角等數(shù)據(jù)及車輛內(nèi)部參數(shù)的位姿估計系統(tǒng)，輪速信號可由能反 映車輪運(yùn)動的傳感器信號（如ABS制動防抱死系統(tǒng)的輪速傳感器信號等）分析獲得，車輛 轉(zhuǎn)角信號可由能反映轉(zhuǎn)向輪轉(zhuǎn)角的傳感器信號（如安裝在轉(zhuǎn)向柱上的轉(zhuǎn)向盤轉(zhuǎn)角傳感器 信號等）分析獲得；也可以是通過外置傳感器采集外部參考數(shù)據(jù)，并附以SLAM即時定位與 地圖重構(gòu)等技術(shù)的車輛外部參數(shù)位姿估計系統(tǒng)，外置傳感器可以是用于檢測的雷達(dá)本身， 也可以是攝像頭等其它傳感設(shè)備；具有相對定位功能的定位系統(tǒng)也可以是以上各類系統(tǒng)的 組合。
[0050] 信號處理單元包括數(shù)據(jù)采集模塊、坐標(biāo)變換模塊、數(shù)據(jù)分類模塊、數(shù)據(jù)擬合模塊、 庫位判別模塊與數(shù)據(jù)發(fā)送模塊，各模塊組成的工作流程如圖1所示。
[0051] (1)數(shù)據(jù)采集模塊
[0052] 數(shù)據(jù)采集模塊用于二維雷達(dá)與定位系統(tǒng)數(shù)據(jù)的采集。具體步驟為：
[0053] 1)以一定的步長依次接收車輛定位數(shù)據(jù)與雷達(dá)數(shù)據(jù)。數(shù)據(jù)接收步長由傳感器數(shù)據(jù) 刷新頻率決定。安排雷達(dá)數(shù)據(jù)處理順序時，對于右側(cè)泊車工況應(yīng)按雷達(dá)平面逆時針方向依 次處理數(shù)據(jù)點(diǎn)，左側(cè)泊車工況應(yīng)按雷達(dá)平面順時針方向依次處理數(shù)據(jù)點(diǎn)。
[0054] 2)根據(jù)雷達(dá)的特性與實(shí)際布置情況，篩選出有效的數(shù)據(jù)。不同雷達(dá)處理超出檢測 范圍的數(shù)據(jù)點(diǎn)的方式不同；而由于車身嵌入式安裝的需要，并非雷達(dá)全部角度范圍的數(shù)據(jù) 都能被使用。此時需根據(jù)實(shí)際情況對接收的雷達(dá)數(shù)據(jù)進(jìn)行篩選，以減少后續(xù)數(shù)據(jù)處理量。
[0055] 3)若有多個雷達(dá)參與環(huán)境感知，則需進(jìn)行數(shù)據(jù)融合。數(shù)據(jù)融合的內(nèi)容包括：數(shù)據(jù) 點(diǎn)的疊加、重疊部分?jǐn)?shù)據(jù)的冗余算法等。
[0056] (2)坐標(biāo)變換模塊
[0057] 坐標(biāo)變換模塊用以實(shí)現(xiàn)障礙物數(shù)據(jù)點(diǎn)的全局定位，即通過處理采集得到的雷達(dá)數(shù) 據(jù)與車輛定位數(shù)據(jù)，將所有的有效數(shù)據(jù)點(diǎn)轉(zhuǎn)換至全局坐標(biāo)系。各坐標(biāo)系定義如圖2(a)? (d)所示。其中，以笛卡爾坐標(biāo)系下定位系統(tǒng)原點(diǎn)作為定位系統(tǒng)坐標(biāo)系E的原點(diǎn)，以定位系 統(tǒng)〇航向角方向作為定位系統(tǒng)坐標(biāo)系E的X軸正方向；以笛卡爾坐標(biāo)系下庫位檢測起始點(diǎn) 處車輛后軸中點(diǎn)作為全局坐標(biāo)系G的原點(diǎn)，以此時車輛車頭方向作為全局坐標(biāo)系G的X軸 正方向；以笛卡爾坐標(biāo)系下任意時刻車輛后軸中點(diǎn)作為車輛坐標(biāo)系V的原點(diǎn)，以車輛車頭 方向作為車輛坐標(biāo)系V的X軸正方向；以笛卡爾坐標(biāo)系下任意時刻雷達(dá)檢測介質(zhì)發(fā)射中心 點(diǎn)為雷達(dá)坐標(biāo)系L的原點(diǎn)，以雷達(dá)0角度方向作為雷達(dá)坐標(biāo)系L的X軸正方向；以笛卡爾坐 標(biāo)系下檢出庫位時前方障礙物外接矩形左后角點(diǎn)作為庫位坐標(biāo)系P的原點(diǎn)，以此時前方障 礙物外接矩形沿道路方向的邊（即長邊）所在方向作為庫位坐標(biāo)系P的X軸正方向。
[0058] 雷達(dá)坐標(biāo)系L下的數(shù)據(jù)點(diǎn)坐標(biāo)可根據(jù)下式轉(zhuǎn)換至全局坐標(biāo)系G :
[0059] pointG = pointLHLVHVEHEG (式 2-1)
[0060] 式中，pointy為雷達(dá)坐標(biāo)系L下雷達(dá)數(shù)據(jù)點(diǎn)的坐標(biāo)，Hw為雷達(dá)坐標(biāo)系L至車輛坐 標(biāo)系V的坐標(biāo)變換矩陣，其它依此類推。
[0061] 具體地，以上坐標(biāo)變換過程可分解為三步（按圖2(a)?（d)所示的坐標(biāo)定義）：
[0062] 1)將雷達(dá)坐標(biāo)系L下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至車輛坐標(biāo)系V。
[0063] pointvx = cos Θ LV*p〇intLX_sin Θ LV*p〇intLY+XLV (式 2-2)
[0064] pointVY = sin Θ LV*p〇intLX+cos Θ LV*p〇intLY+YLV (式 2-3)
[0065] 其中，pointy Pointw分別為雷達(dá)坐標(biāo)系L下雷達(dá)數(shù)據(jù)點(diǎn)的橫縱坐標(biāo)，Xw、Yui分 別為雷達(dá)坐標(biāo)系L原點(diǎn)在車輛坐標(biāo)系V下的橫縱坐標(biāo)，Θ w為雷達(dá)坐標(biāo)系L的X軸正方向 在車輛坐標(biāo)系V中的航向角，其它依此類推。
[0066] 2)將車輛坐標(biāo)系V下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至定位系統(tǒng)坐標(biāo)系E。

【權(quán)利要求】
1. 一種智能泊車系統(tǒng)的障礙物檢測與庫位判別方法，其特征在于：包括：步驟（a)進(jìn)行 數(shù)據(jù)采集；步驟（b)進(jìn)行坐標(biāo)變換；步驟（c)進(jìn)行數(shù)據(jù)分類；步驟（d)進(jìn)行數(shù)據(jù)擬合；步驟 (e)進(jìn)行庫位判別。
2. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（a)包括：（1)以一定的步長依次 接收車輛定位數(shù)據(jù)與雷達(dá)數(shù)據(jù)；（2)根據(jù)雷達(dá)的特性與實(shí)際布置情況，篩選出有效的數(shù)據(jù)； (3)若有多個雷達(dá)參與環(huán)境感知，則進(jìn)行數(shù)據(jù)融合。
3. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（b)包括：（1)將雷達(dá)坐標(biāo)系下的數(shù) 據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至車輛坐標(biāo)系；（2)將車輛坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至定位系統(tǒng)坐標(biāo)系； (3)將定位系統(tǒng)坐標(biāo)系下的數(shù)據(jù)點(diǎn)坐標(biāo)轉(zhuǎn)換至全局坐標(biāo)系。
4. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（c)包括：（1)對于新掃描的數(shù)據(jù)點(diǎn)， 求出該點(diǎn)至各已有障礙物的最短距離；（2)若該最小距離值在設(shè)定閾值內(nèi)，則該點(diǎn)歸屬于 對應(yīng)物體，否則成立一個新物體。
5. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（d)包括：（1)對于同一物體的數(shù)據(jù) 點(diǎn)，令其外接矩形的一邊即特征邊與全局坐標(biāo)系X軸的夾角從負(fù)45度至正45度遍歷，對于 該范圍內(nèi)任意的夾角，均可唯一確定外接矩形；（2)求出使外接矩形特征邊兩端點(diǎn)至物體 的距離之和，在上述夾角范圍內(nèi)使該"距離和"最小的夾角值所對應(yīng)的外接矩形即為所要擬 合的結(jié)果。
6. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（e)包括如下判別條件：（1)相鄰兩 障礙物外接矩形的最短距離大于設(shè)定閾值；（2)前方障礙物已檢出部分的外接矩形長邊長 度大于設(shè)定閾值；（3)相鄰兩障礙物外接矩形的長邊夾角小于設(shè)定閾值。
7. 根據(jù)權(quán)利要求1所述的方法，其特征在于：還包括進(jìn)行數(shù)據(jù)發(fā)送步驟：（1)對符合判 別條件的庫位，提取前后障礙物的外接矩形，并將數(shù)據(jù)轉(zhuǎn)換至庫位坐標(biāo)系；（2)以一定的協(xié) 議將數(shù)據(jù)發(fā)送至智能泊車系統(tǒng)路徑規(guī)劃模塊。
8. 實(shí)現(xiàn)權(quán)利要求1至7中任一所述方法的智能泊車系統(tǒng)的障礙物檢測與庫位判別系 統(tǒng)，其特征在于：包括：（1)傳感器單元：位于車輛車身的兼具角度和距離信號的二維雷達(dá)、 具有相對定位功能的定位系統(tǒng)；（2)信號處理單元：雷達(dá)數(shù)據(jù)處理模塊。
9. 根據(jù)權(quán)利要求8所述的智能泊車系統(tǒng)的障礙物檢測與庫位判別系統(tǒng)，其特征在于： 所述信號處理單元包括數(shù)據(jù)采集模塊、坐標(biāo)變換模塊、數(shù)據(jù)分類模塊、數(shù)據(jù)擬合模塊、 庫位判別模塊與數(shù)據(jù)發(fā)送模塊； 優(yōu)選的，數(shù)據(jù)采集模塊用于二維雷達(dá)與定位系統(tǒng)數(shù)據(jù)的采集；坐標(biāo)變換模塊用于雷達(dá) 數(shù)據(jù)點(diǎn)的全局定位；數(shù)據(jù)分類模塊用于分析坐標(biāo)變換后數(shù)據(jù)點(diǎn)之間的幾何位置關(guān)系，從而 確定每一個數(shù)據(jù)點(diǎn)的物體歸屬；數(shù)據(jù)擬合模塊利用最佳外接矩形擬合算法實(shí)現(xiàn)數(shù)據(jù)點(diǎn)的擬 合；庫位判別模塊用于分析矩形擬合結(jié)果間的幾何關(guān)系，從而判斷庫位是否符合泊車路徑 規(guī)劃的條件；數(shù)據(jù)發(fā)送模塊用于整理庫位信息并按協(xié)議發(fā)送至智能泊車系統(tǒng)的路徑規(guī)劃模 塊。
10. 根據(jù)權(quán)利要求8所述的智能泊車系統(tǒng)的障礙物檢測與庫位判別系統(tǒng)，其特征在于： 所述傳感器單元中的位于車輛車身的二維雷達(dá)，其有效感知角度范圍不小于60度，有 效感知距離不小于4米；或者，優(yōu)選的，二維檢測平面應(yīng)與地面平行，高度介于0. 5米至1米 之間；或者，優(yōu)選的， 所述傳感器單元中的具有相對定位功能的定位系統(tǒng)，選自以下一種或兩種以上的組 合：外置安裝在車輛上的動態(tài)精度在l〇cm以內(nèi)的GPS全球定位系統(tǒng)；通過內(nèi)置傳感器采 集輪速與車輛轉(zhuǎn)角等數(shù)據(jù)的車輛內(nèi)部參數(shù)位姿估計系統(tǒng)；通過外置傳感器采集外部參考數(shù) 據(jù)，并附以SLAM即時定位與地圖重構(gòu)等技術(shù)的車輛外部參數(shù)位姿估計系統(tǒng)；或者，優(yōu)選的， 所述二維雷達(dá)采用激光雷達(dá)。
【文檔編號】B60R16/02GK104354656SQ201410557306
【公開日】2015年2月18日 申請日期:2014年10月20日 優(yōu)先權(quán)日:2014年10月20日
【發(fā)明者】陳慧, 王宇辰 申請人:同濟(jì)大學(xué)