專利名稱：使用傳感器融合來估計車道路徑的系統(tǒng)和方法
技術(shù)領(lǐng)域：
本發(fā)明涉及車輛中的道路和車道幾何形狀檢測。
背景技術(shù)：
該部分的內(nèi)容僅提供與本發(fā)明有關(guān)的背景信息，且可能不構(gòu)成現(xiàn)有技術(shù)。
車輛可配備有幫助車輛操作者管理車輛操作的各種傳感裝置和系統(tǒng)。這種傳感器可以用來描述車輛的操作環(huán)境。 一種傳感系統(tǒng)旨在識別車道幾何形狀并估計相對于車道的車輛位置和取向。全球定位裝置(GPS)或三維(3D)地圖裝置用于將車輛置于相對于地圖的近似位置。然而，已知GPS裝置具有位置誤差、低取樣速率和響應(yīng)于車輛動態(tài)運動的大的滯后時間。這種系統(tǒng)用于一般導航，但是不是很適合于獨立提供用于車輛操作的輸入。示例性系統(tǒng)采用傳感器(例如，車輛運動性能傳感器和照相機系統(tǒng))以將車輛定位在車道內(nèi)且可以與GPS裝置結(jié)合工作。 關(guān)于車道幾何形狀和相對于車道的車輛位置和取向的信息可以從多個源同時生成。將該信息融合成車道和車輛數(shù)據(jù)的估計值能夠通過本領(lǐng)域已知的多種方法來完成。然而，許多這種方法使用歷史航跡和地圖估計來生成車道幾何形狀和相對于車道的車輛位置和取向的估計值。此外，許多方法在計算中使用高斯分布，以考慮未知趨勢。本領(lǐng)域技術(shù)人員將理解，基于歷史數(shù)據(jù)和高斯分布的估計值對輸入的快速變化不敏感且通常包括滯后因子。

發(fā)明內(nèi)容
—種用于估計車輛在道路上的計劃行駛路徑的方法，包括監(jiān)測多個傳感器輸入；
基于所監(jiān)測傳感器輸入來確定車輛前面的道路幾何形狀；基于所監(jiān)測傳感器輸入來確定相
對于所述道路幾何形狀的車輛位置；從所述道路幾何形狀和車輛位置確定車輛前面的多個
質(zhì)點(particle point)，所述質(zhì)點表示可能行駛路徑；以及使用所述多個質(zhì)點中的重復確
定的質(zhì)點來導航車輛，包括刪去所述多個質(zhì)點中的車輛經(jīng)過的質(zhì)點。 —種用于估計車輛在道路上的計劃行駛路徑的方法，所述方法包括 監(jiān)測多個傳感器輸入； 基于所監(jiān)測傳感器輸入來確定車輛前面的道路幾何形狀； 基于所監(jiān)測傳感器輸入來確定相對于所述道路幾何形狀的車輛位置； 從所述道路幾何形狀和車輛位置確定車輛前面的多個質(zhì)點，所述質(zhì)點表示可能行
駛路徑；以及 使用所述多個質(zhì)點中的重復確定的質(zhì)點來導航車輛，包括刪去所述多個質(zhì)點中的車輛經(jīng)過的質(zhì)點。 確定車輛前面的多個質(zhì)點包括 確定用于多個傳感器輸入中的每個的多個質(zhì)點；以及 將用于多個傳感器輸入中的每個的多個質(zhì)點融合以確定車輛前面的多個質(zhì)點。 使用所述多個質(zhì)點中的重復確定的質(zhì)點來導航車輛包括 基于所述多個質(zhì)點中的重復確定的質(zhì)點來確定用于車輛導航的計劃行駛路徑。 基于所監(jiān)測傳感器輸入來確定相對于所述道路幾何形狀的車輛位置包括 確定相對于道路幾何形狀的車輛取向；以及 確定相對于道路幾何形狀的車輛橫向位置。 監(jiān)測多個傳感器輸入包括 監(jiān)測來自于地圖裝置的數(shù)據(jù)；以及 監(jiān)測來自于至少一個附加傳感器輸入的數(shù)據(jù)，所述至少一個附加傳感器輸入選自
包括視覺子系統(tǒng)、車輛運動性能傳感器、雷達、LIDAR、車道對車輛通信系統(tǒng)和車輛對基礎(chǔ)設(shè)
施通信系統(tǒng)的組。 所述方法還包括 當監(jiān)測到來自于所述多個傳感器輸入中的至少一個的新數(shù)據(jù)時，重復地更新車輛位置。 車輛前面的道路幾何形狀包括 道路曲率；禾口 在車輛位置處的切線。 道路曲率通過以下步驟確定 生成樣條，所述樣條描述鄰近車輛的道路曲率；以及 基于所述多個傳感器輸入來確定樣條上的車輛位置。 生成樣條包括 收集來自于地圖數(shù)據(jù)庫的地圖形狀點；以及 用樣條擬合地圖形狀點。 地圖形狀點與局部坐標系相關(guān)聯(lián)。 監(jiān)測多個傳感器輸入包括監(jiān)測全球定位坐標； 其中，確定樣條上的車輛位置包括 基于所述多個傳感器輸入來確定樣條上的車輛位置的初始估計值； 基于全球定位坐標來校正樣條上的車輛位置的初始估計值。 車輛前面的多個質(zhì)點基于 基于車輛運動性能數(shù)據(jù)確定的第一組質(zhì)點； 基于照相機數(shù)據(jù)確定的第二組質(zhì)點；禾口 基于地圖數(shù)據(jù)確定的第三組質(zhì)點。 所述方法還包括 使用最小二乘估計將第一組質(zhì)點、第二組質(zhì)點和第三組質(zhì)點融合成表示行駛路徑的單組質(zhì)點。 融合基于與所述多個質(zhì)點中的每個相關(guān)聯(lián)的位置偏差來執(zhí)行。
確定車輛位置使用卡爾曼濾波器執(zhí)行。 監(jiān)測多個傳感器輸入包括監(jiān)測描述車輛前面的跟蹤車輛軌道的傳感器； 所述方法還包括基于來自于描述車輛前面的跟蹤車輛軌道的傳感器的輸入來確
定車輛前面的道路曲率；以及 其中，確定車輛前面的道路幾何形狀基于車輛前面的道路曲率。 —種用于估計車輛在道路上的計劃行駛路徑的方法，所述方法包括 監(jiān)測多個傳感器輸入，所述傳感器輸入描述車輛前面的道路幾何形狀； 確定用于每個傳感器輸入的一組質(zhì)點，包括 基于相應(yīng)傳感器輸入來確定車輛前面的道路幾何形狀； 基于相應(yīng)傳感器輸入來確定相對于道路幾何形狀的車輛位置；以及 使用道路幾何形狀和車輛位置來確定相應(yīng)傳感器輸入的該組質(zhì)點，所述質(zhì)點表示
可能行駛路徑； 將每個傳感器輸入的所述組質(zhì)點融合成一組融合質(zhì)點；以及 使用該組融合質(zhì)點中的重復確定的質(zhì)點來操作車輛，其中，使用重復確定的質(zhì)點包括刪去所述組融合質(zhì)點中的車輛經(jīng)過的獨立質(zhì)點。
融合所述組質(zhì)點包括使用最小二乘估計。
監(jiān)測多個傳感器輸入包括
監(jiān)測來自于全球定位裝置的數(shù)據(jù)； 監(jiān)測來自于車輛運動性能傳感器的車輛運動性能數(shù)據(jù)；以及
監(jiān)測來自于視覺子系統(tǒng)的照相機數(shù)據(jù)。 融合所述組質(zhì)點基于與每個傳感器輸入的每個質(zhì)點相關(guān)聯(lián)的偏差。


現(xiàn)在將通過示例的方式參考附圖來描述一個或多個實施例，在附圖中 圖1示出了根據(jù)本發(fā)明的使用傳感器來獲取車輛前面的道路幾何形狀數(shù)據(jù)的示
例性車輛； 圖2示出了根據(jù)本發(fā)明的示例性過程，藉此，傳感器輸入可以用于生成車道幾何形狀和相對于車道的車輛位置的估計值； 圖3示出了根據(jù)本發(fā)明的示例性過程，其中，來自于地圖數(shù)據(jù)庫的信息可以用于建立車輛區(qū)域中的道路幾何形狀模型； 圖4以圖表形式示出了根據(jù)本發(fā)明的示例性迭代方法，以尋找相對于估計道路幾何形狀的車輛近似位置； 圖5示出了根據(jù)本發(fā)明的示例性車輛姿態(tài)定位過程； 圖6示出了根據(jù)本發(fā)明的在車輛的橫向模型內(nèi)作出的示例性確定； 圖7示出了根據(jù)本發(fā)明的示例性質(zhì)點組，包括用于導航車輛的一組融合質(zhì)點； 圖8示出了根據(jù)本發(fā)明的沿車輛前面的計劃車道的質(zhì)點的示例性使用，以估計車
道幾何形狀； 圖9示出了根據(jù)本發(fā)明的在使用雷達數(shù)據(jù)確定的公路中的示例性跟蹤車輛軌道的使用；
圖10是示出了根據(jù)本發(fā)明的用于確定選定跟蹤車輛軌道的曲率的示例性控制方案的流程圖；禾口 圖11是根據(jù)本發(fā)明的用于使用雷達數(shù)據(jù)確定跟蹤車輛軌道的流程圖。
具體實施例方式
現(xiàn)在參考附圖，其中，附圖顯示僅僅是為了示出某些示例性實施例，而不是為了限制所述實施例，圖1示出了根據(jù)本發(fā)明的使用傳感器來獲取車輛前面的道路幾何形狀數(shù)據(jù)的示例性車輛。示例性車輛包括旨在用于高速公路上的乘用車輛，但是應(yīng)當理解的是，本文所述的本發(fā)明可應(yīng)用于任何車輛或其它系統(tǒng)，以監(jiān)測遠程車輛和其它目標的位置和軌跡。車輛包括控制系統(tǒng)，所述控制系統(tǒng)包含在各種時刻執(zhí)行的各種算法和標定值。控制系統(tǒng)優(yōu)選為總體車輛控制結(jié)構(gòu)的子組，所述總體車輛控制結(jié)構(gòu)提供協(xié)調(diào)的車輛系統(tǒng)控制?？刂葡到y(tǒng)監(jiān)測來自于各個傳感器的描述車輛操作環(huán)境的輸入，綜合相關(guān)信息和輸入，并執(zhí)行算法來控制各種致動器以實現(xiàn)控制目標，包括諸如避免碰撞和適應(yīng)性巡航控制的參數(shù)。車輛控制結(jié)構(gòu)包括多個分布式處理器和裝置，包括提供諸如防抱死制動、牽引控制和車輛穩(wěn)定性的功能的系統(tǒng)控制器。 在圖1的示例性實施例中，車輛10被示出且包括視覺子系統(tǒng)16。視覺子系統(tǒng)16使用能夠生成車輛前面的區(qū)域的數(shù)字圖像表示的照相機或成像裝置。來自于視覺子系統(tǒng)16的數(shù)據(jù)用于描述車輛前面的狀況，且被轉(zhuǎn)換成參考車輛10中心軸線的XY坐標系20。視覺子系統(tǒng)的示例性視場由虛線示出。道路上的行駛車道根據(jù)車道標記25A和25B示出，且共同特征可以被視覺檢測且用于描述相對于車輛10的車道幾何形狀。以這種方式，通過本領(lǐng)域技術(shù)人員已知的方法，從圖像或照相機數(shù)據(jù)分析獲得的信息可以用作相對于車輛10向前行駛的狀況。 每個處理器可采用任何合適的形式，包括一個或多個專用集成電路(ASIC)、電子電路、執(zhí)行一個或多個軟件或固件程序的中央處理單元(優(yōu)選為微處理器)以及相關(guān)存儲器和儲存器(只讀、可編程只讀、隨機存取、硬盤驅(qū)動器等)、組合邏輯電路、輸入/輸出電路和裝置、合適的信號調(diào)節(jié)和緩沖電路、以及提供所述功能的其它合適部件的各種組合?？刂颇K具有一組處理算法，所述處理算法包括存儲在存儲器中且被執(zhí)行提供期望功能的常駐軟件程序指令和標定值。所述算法優(yōu)選在預定循環(huán)期間被執(zhí)行。所述算法例如由中央處理單元執(zhí)行，并且可操作以監(jiān)測來自感測裝置和其它網(wǎng)絡(luò)控制模塊的輸入以及執(zhí)行控制和診斷程序從而控制致動器的操作。在持續(xù)進行的發(fā)動機和車輛操作期間，循環(huán)以規(guī)則間隔例如每3. 125、6. 25、 12. 5、25和100毫秒被執(zhí)行。替代性地，算法可響應(yīng)于事件發(fā)生而被執(zhí)行。 由車輛10使用的傳感器，例如視覺子系統(tǒng)16或其它雷達或測距裝置，優(yōu)選定位在車輛10內(nèi)相對于車輛前面的視場的相對不受阻礙的位置。也應(yīng)當理解的是，這些傳感器中的每個提供道路的實際細節(jié)或車輛前面的道路上的目標的估計值。應(yīng)當理解的是，這些估計值不是準確的位置，且每個估計值的偏差標準是可能的。還應(yīng)當理解的是，這些傳感器的特征是互補的，因為一些傳感器比其它傳感器在估計某些參數(shù)方面更可靠。常規(guī)傳感器具有不同的操作范圍和角視界，且能夠估計在其操作范圍內(nèi)的不同參數(shù)。例如，雷達傳感器通?？梢怨烙嬆繕说木嚯x、距離變化率和方位位置，但是在估計檢測目標的范圍方面通常不穩(wěn)固。具有視覺處理器的照相機在估計目標的形狀和方位位置方面更穩(wěn)固，但是在估計目 標的距離和距離變化率方面效率較低。掃描式LIDAR關(guān)于估計距離和方位位置方面有效且 準確地執(zhí)行，但是通常不能估計距離變化率，因而關(guān)于新目標獲取/識別方面不準確。超聲 傳感器能夠估計距離，但是通常不能估計或計算距離變化率和方位位置。描述車輛運動性 能(例如速度和偏航速率)的傳感器不準確，尤其是已知在跟蹤車輛運動的較小變化方面 不好。此外，應(yīng)當理解的是，每個傳感器技術(shù)的性能受到不同環(huán)境狀況的影響。因而，常規(guī) 傳感器存在參數(shù)差異，其可操作疊加為傳感器融合提供機會。 描述車輛前面的操作環(huán)境的傳感器和子系統(tǒng)通常提供輸出(即傳感器數(shù)據(jù))，包 括距離R、基于時間的距離變化R—dot、和角度 (優(yōu)選相對于車輛的縱軸線)，所述輸出可 寫成測量矢量(。)。示例性短程雷達子系統(tǒng)具有160度的視場(F0V)和三十米的最大量 程。示例性長程雷達子系統(tǒng)具有17度的視場和220米的最大量程。示例性前視子系統(tǒng)具 有45度的視場和五十(50)米的最大量程。對于每個子系統(tǒng)，視場優(yōu)選圍繞車輛10的縱軸 線取向。車輛優(yōu)選相對于坐標系(稱為XY坐標系20)取向，其中，車輛10的縱軸線建立X 軸，中心位于便于車輛和信號處理的點處，Y軸通過在水平面內(nèi)垂直于車輛10的縱軸線的 軸線建立，因而平行于道路表面。 優(yōu)選控制模塊包括控制器，其中，算法和相關(guān)標定值被存儲且配置成從可用傳感 器接收估計數(shù)據(jù)并將數(shù)據(jù)聚類為車輛前面的狀況的有用估計，并將聚類的觀測結(jié)果融合以 確定所需車道幾何形狀和相對車輛位置估計。應(yīng)當理解的是，使用不同傳感系統(tǒng)和技術(shù)來 融合數(shù)據(jù)得到穩(wěn)固的結(jié)果。同樣，應(yīng)當理解的是，在該技術(shù)中可以使用任何數(shù)量的傳感器。
給出了一種生成和保持系統(tǒng)內(nèi)的道路和車道幾何形狀估計的方法，其中，歷史測 量值用于評估或預測隨后的航跡數(shù)據(jù)。示例性系統(tǒng)基于在時間T時的函數(shù)來作出估計以描 述時間T+1時的系統(tǒng)狀態(tài)。通常，為了支持實時估計，使用表示高斯分布的信息陣列來估計 未知誤差的影響。這種系統(tǒng)允許收集和融合車輛前面的道路狀況的估計值。
用以生成和保持車輛前面的狀況的估計值的上述方法用于快速而有效地生成可 用于需要近似車輛位置的方案中的估計值。然而，本領(lǐng)域技術(shù)人員應(yīng)當理解的是，使用歷史 數(shù)據(jù)和高斯分布的估計值包括基于平均和正態(tài)分布假設(shè)的內(nèi)在誤差。例如，在車道幾何形 狀估計操作中，在建立車輛經(jīng)過的估計安全計劃行駛路徑時，車輛后面的直車道對車輛前 面的道路的急彎沒有實際減輕影響。關(guān)于車輛前面的車道的數(shù)據(jù)的散度不必通過借助于高 斯分布應(yīng)用隨機矢量以求解散度而改進。使用歷史平均和標準化或高斯分布的方法，例如 依賴于卡爾曼濾波器的方法，通常包括導致對道路幾何形狀的變化或過渡的時間滯后的誤 差因子。 公開了一種方法，該方法通過融合來自于多個數(shù)據(jù)輸入的當前測量值來生成車道 幾何形狀和車輛位置(包括相對于車道的取向)的估計值，而不會招致基于歷史數(shù)據(jù)或標 準化分布的誤差。示例性實施例包括GPS數(shù)據(jù)、視覺照相機子系統(tǒng)和車輛運動性能的輸入。 應(yīng)當理解的是，車道幾何形狀和相對于車道幾何形狀的車輛位置可以基于同一組數(shù)據(jù)，或 者可以使用不同數(shù)據(jù)源來確定車道幾何形狀和車輛位置。 總體道路幾何形狀是通過使用GPS裝置和3D地圖可容易獲得的信息。給定來自 于GPS裝置的近似位置，局部化道路幾何形狀可以表示道路形狀點列表。類似地，包括全球 緯度測量值和全球經(jīng)度測量值的GPS坐標可通過GPS裝置獲得。附加的地圖實施例可包括
7配置成傳輸?shù)貓D數(shù)據(jù)和道路幾何形狀和從基于地面的監(jiān)測系統(tǒng)和其它車輛接收地圖數(shù)據(jù) 和道路幾何形狀的車輛對基礎(chǔ)設(shè)施和車輛對車輛通信系統(tǒng)。至少包括車輛速度和偏航速率 的車輛運動性能可通過監(jiān)測車輛操作和/或監(jiān)測加速計讀數(shù)的傳感器獲得。照相機數(shù)據(jù)可 用于將車輛定位到實際行駛車道。通過照相機數(shù)據(jù)限定車道傳感系數(shù)(即，y = a+bx+cx2， 其中，x是車道縱向偏移，且y是距車道中心的橫向偏移)。通過該數(shù)據(jù)，前向車道估計模塊 可估計車道的曲率、距車道中心的橫向偏移、和相對于車道切線的車輛取向。
使用傳感器數(shù)據(jù)的方法可以用于確定道路幾何形狀和相對于道路幾何形狀的車 輛位置?；谶@些被確定的值，可以生成車輛的導航指令，以控制道路表面上的車輛。這種 導航指令可包括多個示例性實施例。圖2示出了根據(jù)本發(fā)明的示例性過程，藉此，傳感器輸 入可以用于生成車道幾何形狀和相對于車道的車輛位置的估計值。示例性過程包括地圖幾 何形狀模型模塊202、車輛姿態(tài)定位模塊204、曲率估計模塊206和車輛橫向跟蹤模塊208。 地圖幾何形狀模型模塊202輸入地圖路點(由本領(lǐng)域已知的方法確定)，包括確定從起始或 當前點到目的地或者通過地圖數(shù)據(jù)庫中的點的總體路徑；并輸出車輛區(qū)域中的車道幾何形 狀。該車道幾何形狀可以描述為包括該區(qū)域中的道路的幾何形狀表示的弧線。車輛姿態(tài)定 位模塊204輸入來自于地圖幾何形狀模型模塊的車道幾何形狀、來自于GPS裝置的GPS坐 標、和來自于視覺子系統(tǒng)的照相機數(shù)據(jù)，并輸出相對于車輛區(qū)域中的車道幾何形狀的估計 車輛位置。相對于車道幾何形狀或弧線的該車輛位置可以描述為弧長參數(shù)(Sm)。曲率估計 模塊206輸入照相機數(shù)據(jù)、來自于車輛傳感器的車輛運動性能數(shù)據(jù)(例如車輛速度和偏航 速率)以及Sm，并輸出曲率(K)或在車輛位置處的道路曲線度量。最后，車輛橫向跟蹤模塊 208輸入照相機數(shù)據(jù)、車輛運動性能數(shù)據(jù)和K，并輸出關(guān)于相對于當前車道中心的車輛位置 和相對于車道當前前進方向的車輛角度取向的數(shù)據(jù)。以這種方式，關(guān)于當前位置和車輛行 駛的當前輸入可以用于生成與車輛區(qū)域中的車道幾何形狀以及相對于車道的車輛位置和 取向有關(guān)的數(shù)據(jù)。 如上所述，地圖幾何形狀模型模塊輸入地圖路點并輸出車輛區(qū)域中的車道幾何形 狀。具體地，地圖幾何形狀模型監(jiān)測地圖數(shù)據(jù)庫中所述的地圖形狀點的輸入，并建立表示形 狀點的幾何形狀模型。圖3示出了根據(jù)本發(fā)明的示例性過程，其中，來自于地圖數(shù)據(jù)庫的信 息可以用于建立車輛區(qū)域中的道路幾何形狀模型。示例性過程包括收集來自于地圖數(shù)據(jù) 庫的描述道路幾何形狀的地圖形狀點(302)。地圖數(shù)據(jù)庫以全球坐標的形式提供地圖形狀 點，通常依照緯度位置、經(jīng)度位置和高度或海拔來描述位置。全球坐標然后轉(zhuǎn)換為局部坐標 系，通常識別鄰近車輛位置的點作為靜態(tài)參考點并將任何其它位置描述為距參考點的南北 位移(即，緯度位移)和距參考點的東西位移(即，經(jīng)度位移)(304)。接下來，使用樣條來 擬合地圖形狀點，以生成幾何形狀或近似所表示的道路幾何形狀的弧線(306)。最后，在車 輛的估計位置確定擬合樣條的切線和曲率(308和310)。 描述了地圖幾何形狀模型內(nèi)的示例性確定。假定Klati， lorO |i = 1，…，W為 形狀點。選出一個點作為參考點，可以將形狀點轉(zhuǎn)換為局部坐標Kei，rOli二l，…，Nh 表示距參考點的東西位移和南北位移。定義序列{(Si，ei，ni) |i = 1，…，Nh其中<formula>formula see original document page 8</formula>獲得二維三次樣條函數(shù)以擬合形狀點，如下所述:
其中，s是弧長參數(shù)，e和n分別是位移的東西和南北分j 量計算如下
于是，在s時的梯度矢
且，取向角度《計算如下"二atan2(nt，et) 最后，在s時的曲率k可以計算如下
/C 二 其中' 如上所述，車輛姿態(tài)定位模塊204輸入來自于地圖幾何形狀模型模塊202的車道 幾何形狀、來自于GPS裝置的GPS坐標和照相機數(shù)據(jù)，并輸出相對于車輛區(qū)域中的車道幾何 形狀的估計車輛位置。本領(lǐng)域技術(shù)人員將理解，一個問題可以描述地圖到監(jiān)測GPS數(shù)據(jù)的 定位。地圖幾何形狀由樣條函數(shù)表示，例如方程1所述的函數(shù)。該樣條描述離散位置，其中，
假定存在道路車道。GPS數(shù)據(jù)測量的點以示例性形式尸:
7
返回。在GPS裝置中一些偏差
的不準確性和不精確性是正常的。在樣條函數(shù)中誤差也是固有的。P與地圖幾何形狀樣條 相對精確地相符。樣條函數(shù)描述車道中的點(例如車道中心)，且實際車輛位置將通常以 可測量的量偏離車道中心。車輛在地圖上的近似位置必須基于P和區(qū)域中的估計道路幾何 形狀來確定。 一種校正P和道路幾何形狀表示之間的偏差的示例性方案在于尋找最接近的
f (sj ，使得.S". = arg mm i |p — ^J i該示例性過程用于近似且可迭代地T
應(yīng)用以尋找道路曲線中的車輛位置并在監(jiān)測數(shù)據(jù)變化時改進估計位置。 圖4以圖表形式示出了根據(jù)本發(fā)明的示例性迭代方法，以尋找車輛相對于估計道
路幾何形狀的近似位置。假定s。為sm的初始猜測值?；￠L參數(shù)的校正可以寫成如下 其中Pm = f (s。)，《。換句話說，校正值A(chǔ) s為猜測位置s。處的梯度的單位 矢量上的投影。 本領(lǐng)域技術(shù)人員將理解，GPS測量值通常不如車輛傳感器讀數(shù)那么頻繁地更新。對 于大多數(shù)車載GPS接收器而言，lHz的示例性更新速率是常見的。此外，更新不總是被接收 且可能在衛(wèi)星信號視場被阻礙的城市區(qū)域或其它區(qū)域中有噪音?？梢允褂脼V波技術(shù)來補償 GPS信號更新的緩慢速率。 示例性車輛姿態(tài)定位模塊使用卡爾曼濾波器。車輛姿態(tài)建模為矢量且由東西位移 (e)、南北位移(n)、相對于車道的取向(①)、和弧長(s)構(gòu)成。由于慣性，車輛姿態(tài)并不突
9然變化。因而，假設(shè)以下恒定轉(zhuǎn)向模型= e+vcos(4> +《)AT+w工= n+vsin(4> +《)AT+w2 4)* = (j) + coAT = k AT+w3 st = s+vAT [6] 其中，v是車輛速度；"是車輛偏航速率；AT是從先前循環(huán)的時間差；l是道路
的當前取向(參見(2)) ;k是基于地圖曲線的道路當前曲率；巧，^和^是表示未建模擾 動的過程噪音項。 圖5示出了根據(jù)本發(fā)明的車輛姿態(tài)定位模塊204的示例性控制方案?？刂品桨冈?圖5中示出且在本文描述為包括分立元件。這種圖示是為了便于描述，應(yīng)當認識到，由這些 元件執(zhí)行的功能可組合在一個或多個裝置中，例如在軟件、硬件和/或?qū)Ｓ眉呻娐分袑?施。如上所述，車輛姿態(tài)定位模塊204輸入來自于地圖幾何形狀模型模塊202的車道幾何 形狀、來自于GPS裝置的GPS坐標和來自于視覺子系統(tǒng)的照相機數(shù)據(jù)，并輸出相對于車輛區(qū) 域中的車道幾何形狀的估計車輛位置。 只要分別在步驟502、542和582監(jiān)測來自于GPS、車輛運動性能傳感器或照相機裝 置的新數(shù)據(jù)，就重復地更新車輛位置。在監(jiān)測新數(shù)據(jù)之后，計算時間差AT，其描述在步驟 504、544和584車輛位置更新之間的時間變化。時間差A T從當前和先前循環(huán)之間的時標 的差異進行計算。不同數(shù)據(jù)源的示例性循環(huán)時間包括對于GPS數(shù)據(jù)的1秒，對于運動性能 數(shù)據(jù)的20ms，對于照相機數(shù)據(jù)的50ms。然后，使用方程5來計算預測車輛姿態(tài)。當GPS數(shù) 據(jù)可用時，車輛位置使用卡爾曼濾波器更新(508)。車輛姿態(tài)的測量值更新使用以下GPS測 量方程來確定e印,e+、 [7]
ngps = n+k2 [8] 其中，(e,，rv)是車輛的GPS測量位置；、和1^2是測量噪音。在使用GPS測量值 更新車輛姿態(tài)之后，使用方程5來計算正確的弧長參數(shù)(s)。該步驟對于通過去除由方程6 中的推算導航過程引起的累計誤差來獲得正確的K和l值是重要的。
當新的運動性能數(shù)據(jù)可用時，預測車輛位置可使用多種方法中的一種來確定 (546)。第一方法包括基于時間差和車輛軌跡來插值未來位置。用于預測車輛位置的第二 方法包括分析近似地圖點和車輛位置。假定Pm為地圖曲線上距當前車輛位置的距離最近 的點，表示為P二 (e，n)。假定矢量m表示在Pm處地圖曲線的法線。那么，垂直距離d可以 表示為d二 (P-Pm)V其中，法線m計算為: m =
—o -rV—
—1 o —y— 車輛位置然后使用卡爾曼濾波器更新(548)。 當新的照相機數(shù)據(jù)可用時，使用卡爾曼濾波器確定預測車輛位置(586)且更新車 輛位置(588)。預測車輛位置使用以下測量方程和卡爾曼濾波器確定 a = d+k3 [9] b = (t+k4 [10] 其中，a和b是照相機車道傳感參數(shù)；d是當前車輛位置距行駛車道中心的垂直距離；小表示相對于行駛車道的車輛取向；k3和k4是未建模測』
如上所述，曲率估計模塊206輸入照相機數(shù)據(jù)、來自于車輛傳感器的車輛運動性 能數(shù)據(jù)(例如車輛速度和偏航速率)以及弧長參數(shù)Sm，并輸出描述道路的曲線度量的融合 曲率(Kfus)。 一旦車輛定位在由s表示的地圖曲線中，那么可以通過方程4尋找相應(yīng)的地圖 曲率K,。 應(yīng)當注意，存在三個信息源來估計道路曲率地圖曲率(Kmap)、照相機曲率(K，= 2c)、基于偏航速率的曲率(Kyaw= "/v)。所使用的來自于一個信息源的曲率數(shù)據(jù)可以用于 估計車輛的計劃行駛路徑的質(zhì)點。例如，地圖曲率數(shù)據(jù)可以用于估計車輛的第一計劃行駛 路徑。每個獨立信息源(例如，照相機曲率和基于偏航速率的曲率)可以被獨立地使用以 估計車輛的附加計劃行駛路徑。計劃行駛路徑中的質(zhì)點可以融合在一起以生成用于車輛導 航的單個計劃行駛路徑。 設(shè)想用于融合質(zhì)點的多種方法。第一方法包括使用最小二乘估計分析來融合質(zhì) 點。在本文描述一種示例性方法以將上述曲率融合在一起。假定kfus表示融合曲率，其中 偏差為ofus2。假定omap2， 0，2和0，2分別表示地圖曲率、基于偏航速率的曲率和照相機 曲率的偏差。我們得出以下更新方程。 0124] 當?shù)貓D曲率估計值可用時，那么
0125]<formula>formula see original document page 11</formula>
0126] 且<formula>formula see original document page 11</formula>
0128] 當偏航速率曲率估計值可用時，那么
0129] <formula>formula see original document page 11</formula>
0130] 且<formula>formula see original document page 11</formula>
0132] 當照相機曲率估計值可用時，那么
<formula>formula see original document page 11</formula>
且<formula>formula see original document page 11</formula>
在雷達曲率估計值o Mda/可用的附加實施例中，融合曲率可通過以下方程確定2 丄 2 = — +d
"mrtflr卞"""' [17] 且具有偏差o fus2的融合曲率可使用以下方程確定
= ^2 在上述方程中，偏差omap2， 0，2和0，2分別表示來自于不同源(地圖、車輛傳感 器和照相機)的曲率信息的置信度。信息源的偏差越高，該源對融合曲率的貢獻越少。可以 采用一些啟發(fā)式規(guī)則來選擇三個源的不同權(quán)重。例如，當偏航速率高時，我們選擇小oyaw2 來推導融合曲率。以這種方式，可以生成用于車輛導航的融合質(zhì)點。 相對于道路幾何形狀的車輛位置可以被改進，以包括在行駛車道內(nèi)的車輛取向和 橫向位置，以便控制在道路表面上的車輛。如上所述，車輛橫向跟蹤模塊輸入照相機數(shù)據(jù)、 車輛運動性能數(shù)據(jù)和K，并輸出關(guān)于相對于當前車道中心的車輛位置和相對于車道當前前 進方向的車輛角度取向的數(shù)據(jù)。圖6示出了根據(jù)本發(fā)明的在車輛的橫向模型內(nèi)作出的示例 性確定。車輛橫向跟蹤模塊監(jiān)測車輛運動性能的輸入(車輪速度v和偏航速率co)和車道 傳感參數(shù)的輸入?？梢允褂每柭鼮V波器來整合來自于車輛運動性能和車道傳感裝置的數(shù) 據(jù)。如圖6所示，橫向偏移h是距車道中心的位移。K，d是道路的估計曲率。K，是由瞬 時車輛路徑估計的曲率，即Kyaw = "/v?？柭鼮V波器的測量方程表示為b = O，a = h。 如果更新誤差大于閾值，那么實施門邏輯。換句話說，如果預測和實際測量值之間的差大于 閾值，那么在當前時間瞬時忽略實際測量值。 上述方法可以用于描述道路幾何形狀和相對于道路幾何形狀的車輛位置。這種導 航指令可以包括多個示例性實施例。圖7示出了根據(jù)本發(fā)明的示例性質(zhì)點組，包括用于導 航車輛的一組融合質(zhì)點。每組質(zhì)點表示由特定方法確定的車輛可能行駛路徑，在該示例中， 包括車輛運動性能、照相機數(shù)據(jù)和地圖數(shù)據(jù)。環(huán)繞圖7所示的每個質(zhì)點的圓表示與每種方 法相關(guān)聯(lián)的質(zhì)點的行駛路徑上的定位偏差。所述圓表示質(zhì)點的估計偏差。第一組質(zhì)點基于 車輛運動性能確定且在該圖中由三角形圖標表示。第二組質(zhì)點基于來自于視覺子系統(tǒng)的照 相機數(shù)據(jù)確定且在該圖中由方形圖標表示。第三組質(zhì)點基于來自于地圖數(shù)據(jù)庫的地圖數(shù)據(jù) 確定且在該圖中由菱形圖標表示。該組融合質(zhì)點基于第一、第二和第三組質(zhì)點使用上文所 述或者本領(lǐng)域已知的方法確定，包括根據(jù)其偏差修正每個質(zhì)點的影響。應(yīng)當理解的是，該組 融合質(zhì)點表示車輛的計劃行駛路徑。 圖8示出了根據(jù)本發(fā)明的沿車輛前面的計劃車道的質(zhì)點的示例性使用，以估計車 道幾何形狀。在連續(xù)時間間隔時的質(zhì)點的重復生成(質(zhì)點隔開短的距離累計量)可以用于 增強車輛前面的估計車道幾何形狀。質(zhì)點的多次重復生成增加了從多個可能行駛路徑生成 的計劃行駛路徑的置信度，這通過包括基于使用多個車輛視角生成的質(zhì)點、通過增強多次 計算并通過允許濾波質(zhì)點失常來實現(xiàn)。當車輛經(jīng)過質(zhì)點時，所述質(zhì)點可以被廢棄且僅僅使 用與車輛未經(jīng)過的計劃行駛路線相對應(yīng)的質(zhì)點。以這種方式，可以使用沿估計路徑的車輛 前面的質(zhì)點投影來估計車輛可能行駛通過的車道幾何形狀。 應(yīng)當理解的是，如參考圖7和8所述，車輛的可能行駛路徑包括描述車輛行駛的可 能安全通路的多個質(zhì)點。可能行駛路徑可以組合或融合在多個不同組合中的一種以確定車輛的計劃行駛路徑。在一個實施例中，可能行駛路徑可使用權(quán)重組合以確定車輛的計劃行 駛路徑。例如，在預定情況下，使用全球定位/數(shù)字地圖數(shù)據(jù)確定的車輛的可能行駛路徑可 給予比使用車輛運動性能確定的可能行駛路徑更大的權(quán)重。 上述方法使用車道幾何形狀確定和車輛位置確定的示例性方法，包括基于車輛運 動性能、照相機或視覺系統(tǒng)數(shù)據(jù)和全球定位/數(shù)字地圖數(shù)據(jù)的傳感器輸入。在附加實施例 中，雷達數(shù)據(jù)可與上述傳感器輸入組合或者替代地使用。 圖9示出了根據(jù)本發(fā)明的在使用雷達數(shù)據(jù)確定的公路中的示例性跟蹤車輛軌道 的使用。跟蹤車輛軌道使用上文所述的雷達曲率估計值來確定，且可使用下文所述的示例 性控制方案來確定。圖9示出了主車輛900以及第一車輛905和第二車輛906。在第一車 輛905和第二車輛906之后示出了估計跟蹤車輛軌道A和B。 圖10是示出了根據(jù)本發(fā)明的用于確定選定跟蹤車輛軌道的曲率的示例性控制方 案600的流程圖。控制方案600在圖10中示出且在本文描述為包括分立元件。這種圖示 是為了便于描述且應(yīng)當認識到，由這些元件執(zhí)行的功能可組合在一個或多個裝置中，例如 在軟件、硬件和/或?qū)Ｓ眉呻娐分袑嵤?。在由主車輛獲得新的雷達數(shù)據(jù)(602)時，使用控 制方案600。新的雷達數(shù)據(jù)用于更新每個跟蹤前進車輛軌道的車輛軌道(604)，如下文參考 圖11所述。使用每個跟蹤車輛軌道來估計車道幾何形狀曲線擬合(606)。
假定跟蹤車輛軌道由N個點{(Xl，yi)， (x2，y2)，…，(xn，yn)}表示，可以使用拋物 線模型(例如a^+bx+c二y)來擬合所述N個點，可以通過使得過約束線性方程Aa 二b的 殘差最小化來導出，其中 <formula>formula see original document page 13</formula>
=(ATA)—Vb，其中，跟蹤車輛軌道的估計曲率為2a。 在估計車道幾何形狀之后，每個跟蹤車輛軌道與預定閾值進行比較(608)?；?一致相關(guān)的車道曲率選擇大于預定閾值的預定數(shù)量或百分比的跟蹤車輛軌道(例如70% ) (610和612)。如果預定百分比的跟蹤車輛軌道可用，那么控制方案600以高的置信度輸出 所有跟蹤車輛軌道的取樣平均值曲率(614)。如果預定百分比的跟蹤車輛軌道不可用，那 么控制方案600以低的置信度輸出所有跟蹤車輛軌道的取樣平均值曲率(616)。以這種方 式，一組跟蹤車輛軌道可以被分析且用于生成曲率，以在本文所述的方法中使用。
圖11是根據(jù)本發(fā)明的用于使用雷達數(shù)據(jù)確定跟蹤車輛軌道的流程圖。跟蹤車輛 軌道可在步驟604使用上文所述的示例性控制方案600確定。步驟604在圖11中示出，且 在本文描述為包括分立元件。這種圖示是為了便于描述，應(yīng)當認識到，由這些元件執(zhí)行的功 能可組合在一個或多個裝置中，例如在軟件、硬件和/或?qū)Ｓ眉呻娐分袑嵤?。車輛雷達 裝置的視場中的目標被識別和收集，且車輛位置和取向使用雷達數(shù)據(jù)進行估計(702)。每 個識別目標被分配至匹配的車輛軌道(704)。如果沒有車輛軌道匹配，可生成新的車輛軌 道。識別的目標位置轉(zhuǎn)換為全球坐標系(例如全球位置系統(tǒng)坐標)，且添加到跟蹤車輛軌道 (706)。從航跡去除沒有識別目標的跟蹤車輛軌道(708)。 上文所述的方法使用車道幾何形狀確定和車輛位置確定的示例性方法，包括基于車輛運動性能、照相機或視覺系統(tǒng)數(shù)據(jù)和全球定位/數(shù)字地圖數(shù)據(jù)的傳感器輸入。然而應(yīng) 當理解的是，可以使用傳感器輸入的不同或附加組合，例如，附加地或替代地包括雷達數(shù) 據(jù)、LIDAR數(shù)據(jù)、超聲數(shù)據(jù)、車輛對車輛通信、或車輛對基礎(chǔ)設(shè)施通信。車輛對車輛通信可以 用于描述諸如另一車輛相對于主車輛的當前位置或者另一車輛在公路上行駛的路徑的信 息。車輛對基礎(chǔ)設(shè)施通信可以用于傳輸諸如引導交通圖案、道路幾何形狀信息和局部化偏 移信息的信息，從而改進全球定位數(shù)據(jù)對道路幾何形狀的準確性。這種組合可以基于多個 因素進行選擇，包括環(huán)境狀況(例如，天氣或光線水平)和針對當前狀況優(yōu)化的傳感器輸 入。附加地或替代地，傳感器輸入可以基于在例如上述偏差中體現(xiàn)的確定步驟中所確定的 置信度來選擇或者運動增加。 關(guān)于車道幾何形狀和相對于車道的車輛位置和取向的實時和可靠信息可以用于 多種應(yīng)用或車輛控制方案。如上所述，所述信息可以用于車輛在公路上的導航控制。在其 它示例中，這種信息可以用于車輛操作應(yīng)用，包括車道保持、前燈調(diào)整、改進導航輔助、和霧 天警示。然而，本領(lǐng)域技術(shù)人員將理解，大量的應(yīng)用可以使用這種信息，且本發(fā)明不旨在限 于本文所述的特定實施例。 本發(fā)明已經(jīng)描述了某些優(yōu)選實施例及其變型。在閱讀和理解說明書之后，本領(lǐng)域 技術(shù)人員可想到其它的變型和修改。因而，本發(fā)明不旨在限于作為用于實施本發(fā)明的最佳 模式公開的特定實施例，而本發(fā)明將包括落入所附權(quán)利要求范圍內(nèi)的所有實施例。
權(quán)利要求
一種用于估計車輛在道路上的計劃行駛路徑的方法，所述方法包括監(jiān)測多個傳感器輸入；基于所監(jiān)測傳感器輸入來確定車輛前面的道路幾何形狀；基于所監(jiān)測傳感器輸入來確定相對于所述道路幾何形狀的車輛位置；從所述道路幾何形狀和車輛位置確定車輛前面的多個質(zhì)點，所述質(zhì)點表示可能行駛路徑；以及使用所述多個質(zhì)點中的重復確定的質(zhì)點來導航車輛，包括刪去所述多個質(zhì)點中的車輛經(jīng)過的質(zhì)點。
2. 根據(jù)權(quán)利要求1所述的方法，其中，確定車輛前面的多個質(zhì)點包括 確定用于多個傳感器輸入中的每個的多個質(zhì)點；以及將用于多個傳感器輸入中的每個的多個質(zhì)點融合以確定車輛前面的多個質(zhì)點。
3. 根據(jù)權(quán)利要求1所述的方法，其中，使用所述多個質(zhì)點中的重復確定的質(zhì)點來導航 車輛包括基于所述多個質(zhì)點中的重復確定的質(zhì)點來確定用于車輛導航的計劃行駛路徑。
4. 根據(jù)權(quán)利要求1所述的方法，其中，基于所監(jiān)測傳感器輸入來確定相對于所述道路 幾何形狀的車輛位置包括確定相對于道路幾何形狀的車輛取向；以及 確定相對于道路幾何形狀的車輛橫向位置。
5. 根據(jù)權(quán)利要求1所述的方法，其中，監(jiān)測多個傳感器輸入包括 監(jiān)測來自于地圖裝置的數(shù)據(jù)；以及監(jiān)測來自于至少一個附加傳感器輸入的數(shù)據(jù)，所述至少一個附加傳感器輸入選自包括 視覺子系統(tǒng)、車輛運動性能傳感器、雷達、LIDAR、車道對車輛通信系統(tǒng)和車輛對基礎(chǔ)設(shè)施通 信系統(tǒng)的組。
6. 根據(jù)權(quán)利要求5所述的方法，還包括當監(jiān)測到來自于所述多個傳感器輸入中的至少一個的新數(shù)據(jù)時，重復地更新車輛位置。
7. 根據(jù)權(quán)利要求1所述的方法，其中，車輛前面的道路幾何形狀包括 道路曲率；禾口在車輛位置處的切線。
8. 根據(jù)權(quán)利要求7所述的方法，其中，道路曲率通過以下步驟確定 生成樣條，所述樣條描述鄰近車輛的道路曲率；以及 基于所述多個傳感器輸入來確定樣條上的車輛位置。
9. 根據(jù)權(quán)利要求8所述的方法，其中，生成樣條包括 收集來自于地圖數(shù)據(jù)庫的地圖形狀點；以及 用樣條擬合地圖形狀點。
10. 根據(jù)權(quán)利要求9所述的方法，其中，地圖形狀點與局部坐標系相關(guān)聯(lián)。
全文摘要
本發(fā)明涉及使用傳感器融合來估計車道路徑的系統(tǒng)和方法。一種用于估計車輛在道路上的計劃行駛路徑的方法，包括監(jiān)測多個傳感器輸入；基于所監(jiān)測傳感器輸入來確定車輛前面的道路幾何形狀；基于所監(jiān)測傳感器輸入來確定相對于所述道路幾何形狀的車輛位置；從所述道路幾何形狀和車輛位置確定車輛前面的多個質(zhì)點，所述質(zhì)點表示可能行駛路徑；以及使用所述多個質(zhì)點中的重復確定的質(zhì)點來導航車輛，包括刪去所述多個質(zhì)點中的車輛經(jīng)過的質(zhì)點。
文檔編號B60W40/06GK101793528SQ20101010546
公開日2010年8月4日 申請日期2010年1月26日 優(yōu)先權(quán)日2009年1月26日
發(fā)明者S·曾 申請人:通用汽車環(huán)球科技運作公司