用于自主駕駛車輛的路徑規(guī)劃的精確曲率估計算法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明主要涉及一種使用地圖數(shù)據(jù)庫的形狀點來識別道路曲率的系統(tǒng)和方法，更 具體的，涉及一種在半自主或自主駕駛車輛上使用地圖數(shù)據(jù)庫形狀點和多點多項式曲線擬 合算法來識別道路曲率的系統(tǒng)和方法。
【背景技術(shù)】
[0002] 現(xiàn)代化車輛的操作變得越來越自主，即，車輛能夠在越來越少的駕駛員干預(yù)的前 提下提供駕駛控制。巡航控制系統(tǒng)已經(jīng)在車輛上使用了數(shù)年，其中，車輛操作員可以設(shè)置特 定的車輛速度，該車輛將維持在這個速度，而不需要駕駛員操作節(jié)氣門。適應(yīng)性巡航控制 系統(tǒng)在本領(lǐng)域中最近得到了發(fā)展，其中，系統(tǒng)不僅維持設(shè)定的速度，并且當(dāng)使用多個傳感器 (例如雷達(dá)和相機(jī)）檢測到前車緩慢移動的情況時還使得車輛自動減速。一些現(xiàn)代化車輛 還提供自主駐車，其中，車輛將自動提供停車用的轉(zhuǎn)向控制。一些車輛系統(tǒng)在駕駛員做出會 影響車輛穩(wěn)定性的急轉(zhuǎn)彎變化時還會進(jìn)行干預(yù)。一些車輛系統(tǒng)試圖將車輛維持在道路車道 的中心附近。此外，全自主車輛已經(jīng)證明了可以高達(dá)30mph在模擬郊區(qū)交通中行駛，遵守全 部的道路規(guī)則。
[0003] 隨著車輛系統(tǒng)的改進(jìn)，它們將變得越來越自主，以實現(xiàn)成為完全自主駕駛車輛的 目標(biāo)。例如，未來的車輛很可能將應(yīng)用自主系統(tǒng)，用于車道改道、通過、離開交通、進(jìn)入交通 等等。平滑的操作和自動的車道對中以及車道改道控制對于自主駕駛車輛上的駕駛員和乘 客舒適是很重要的。然而，由于傳感器和促動器的延時，測量到的車輛狀態(tài)與實際的車輛狀 態(tài)可能是不一樣的。這種差別可導(dǎo)致不恰當(dāng)?shù)穆窂缴桑@將影響車道改道生硬性。
[0004] 受讓給本申請的受讓人的于2012年5月1日授權(quán)的且合并在此作為參考的、名 稱為 "Path Generation Algorithm for Automated Lane Centering and Lane Changing Control System(用于自動車道對中和車道改道控制系統(tǒng)的路徑生成算法）"的美國專利 8, 170, 739公開了一種為自動車道對中和/或車道改道提供路徑生成的系統(tǒng)。該系統(tǒng)采用 一個或多個前向相機(jī)，其檢測車輛前方的車道標(biāo)識線，用于識別車輛正在行駛的行駛車道。 相機(jī)所檢測的道路標(biāo)識信息用于確定車輛的中心車道，其可用于識別道路的曲率、車輛的 航向角度、車輛的位置等等。所需路徑生成處理器接收來自相機(jī)的信號、車輛狀態(tài)信息和車 輛的轉(zhuǎn)向角以及車輛改道的請求。該系統(tǒng)還包括路徑預(yù)測處理器，其基于包括車輛縱向速 度、車輛橫向速度、車輛偏航率以及車輛轉(zhuǎn)向角的車輛狀態(tài)信息來預(yù)測車輛的路徑。所需的 路徑信息和預(yù)測的路徑信息進(jìn)行比較，生成錯誤信號，誤差信號被發(fā)送給改道控制器，其提 供轉(zhuǎn)向角信號以使得車輛轉(zhuǎn)向并減小錯誤信號，其中，路徑生成處理器采用五階多項方程 式以基于輸入信號確定車輛所需的路徑。
[0005] 上述討論的系統(tǒng)通常需要知曉道路曲率，以提供平滑的車輛路徑規(guī)劃。在本領(lǐng)域 中已知的是使用車上的GPS接收器和相關(guān)的地圖數(shù)據(jù)庫來識別車輛正在行駛的道路的曲 率。GPS接收器根據(jù)地球上的經(jīng)度和煒度識別車輛的位置，地圖數(shù)據(jù)庫使用該位置來協(xié)調(diào)車 輛在哪條道路上，并提供多個形狀點，所述形狀點也識別車輛周圍的經(jīng)度和煒度的道路的 位置。地圖數(shù)據(jù)庫可用的形狀點通常依賴于道路曲率因而是分開的，其中，對于更大曲率的 道路而言，形狀點間隔更加緊密。形狀點可用線連接，并將該線擬合成曲線方程式，道路的 曲率可被確定，其中，線的曲率穿過由數(shù)據(jù)庫識別的每個形狀點。然而，因為在道路調(diào)查中 存在錯誤，地圖數(shù)據(jù)庫的形狀點可能并不精確位于道路的中心，可能多多少少偏離道路，這 使得在由形狀點生成的曲線中產(chǎn)生錯誤。
[0006] 由于形狀點位置具有錯誤，形狀點之間的線的曲率可能無法精確地識別任何特定 位置下道路曲率的正確半徑。由于地圖數(shù)據(jù)庫中形狀點的局外點足夠顯著以使得道路曲率 的半徑特別不準(zhǔn)確，不能可靠地采用使用地圖數(shù)據(jù)庫形狀點來識別道路曲率的已知技術(shù)。

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

[0007] 本公開描述了一種在車輛系統(tǒng)中使用地圖數(shù)據(jù)庫的形狀點識別道路曲率的系統(tǒng) 和方法。該方法包括確定用于確定道路曲率的關(guān)注的范圍，并從地圖數(shù)據(jù)庫收集從車輛當(dāng) 前位置至限定了道路位置的關(guān)注范圍的端點的形狀點。該方法將世界測地系統(tǒng)84(WGS84) 坐標(biāo)系下的形狀點轉(zhuǎn)換為UTM坐標(biāo)系，接著使用轉(zhuǎn)換后的形狀點擬合一組多項方程式以限 定曲率。該方法確定該一組多項方程式是否超過預(yù)定的曲率精確度閾值，如果是，那么使用 轉(zhuǎn)換后的形狀點將多組多項方程式與關(guān)注范圍內(nèi)的多個鄰近道路段擬合。該方法接著使用 多組多項方程式的解來確定在任意道路位置處的道路曲率。
[0008] 本申請還包括下述技術(shù)方案：
[0009] 1. -種在車輛的車輛系統(tǒng)中確定道路曲率的方法，所述方法包括：
[0010] 確定關(guān)注的范圍，以用于確定道路曲率；
[0011] 從車輛上的地圖數(shù)據(jù)庫收集從車輛的當(dāng)前位置至限定道路位置的關(guān)注范圍的端 點的形狀點，其中，所述形狀點通過世界測地系統(tǒng)84(WGS84)坐標(biāo)系限定；
[0012] 將所述形狀點從所述WGS84坐標(biāo)系轉(zhuǎn)換為通用橫軸墨卡托投影（UTM)坐標(biāo)系；
[0013] 使用轉(zhuǎn)換后的形狀點擬合一組多項方程式以限定道路曲率；
[0014] 確定該一組多項方程式超過預(yù)定的曲率精確度閾值；
[0015] 使用轉(zhuǎn)換后的形狀點將多組多項方程式與關(guān)注范圍內(nèi)的多個道路段擬合；以及
[0016] 使用多組多項方程式的解來確定在道路位置處的道路曲率。
[0017] 2.根據(jù)技術(shù)方案1所述的方法，其中，擬合一組多項方程式包括：擬合兩個三階多 項方程式以限定道路曲率。
[0018] 3.根據(jù)技術(shù)方案2所述的方法，其中，擬合一組多項方程式包括：基于弧長擬合兩 個多項方程式，其中，弧長限定為任意兩個形狀點之間的距離。
[0019] 4.根據(jù)技術(shù)方案3所述的方法，其中，擬合一組多項方程式包括：限定每個形狀點 為：
[0021] 其中，s是弧長，p和q是未知系數(shù)。
[0022] 5.根據(jù)技術(shù)方案4所述的方法，其中，擬合一組多項方程式包括：通過最小化成本 函數(shù)來解方程式。
[0023] 6.根據(jù)技術(shù)方案5所述的方法，其中，所述成本函數(shù)J限定為：
[0025] 7.根據(jù)技術(shù)方案5所述的方法，其中，確定一組多項方程式超過預(yù)定精確度閾值 包括：確定成本函數(shù)最小值超過精確度閾值。
[0026] 8.根據(jù)技術(shù)方案1所述的方法，其中，將多組多項方程式與多個道路段擬合包括： 將一組多項方程式與多個連續(xù)段單獨地擬合，其中，重疊部分限定在兩個段之間，并且虛擬 的形狀點在重疊段中從之前的段被限定。
[0027] 9.根據(jù)技術(shù)方案8所述的方法，其中，每個道路段的每組多項方程式是兩個三階 多項方程式。
[0028] 10.根據(jù)技術(shù)方案1所述的方法，其中，從地圖數(shù)據(jù)庫收集形狀點包括收集限定了 車輛后方道路的形狀點。
[0029] 11.根據(jù)技術(shù)方案1所述的方法，其中，確定在任意道路位置處的道路曲率包括： 如下計算曲率κ :
[0031] 12.根據(jù)技術(shù)方案1所述的方法，其進(jìn)一步包括：基于多項方程式的解來確定車輛 的航向角度。
[0032] 13.根據(jù)技術(shù)方案12所述的方法，其中，確定車輛航向角度包括使用方程式
[0033] 其中：
[0035] 并且其中，Sl是從起始點P i至點P 弧長。
[0036] 14. -種在車輛的車輛系統(tǒng)中確定道路曲率的方法，所述方法包括：
[0037] 從車輛上的地圖數(shù)據(jù)庫收集從車輛當(dāng)前位置至限定道路位置的關(guān)注范圍的端點 的形狀點；
[0038] 使用形狀點來擬合三階多