本發(fā)明涉及一種車輛軌跡追蹤裝置及方法，特別是一種具路徑誤差修正的車輛軌跡追蹤裝置及方法。

背景技術(shù)：

先進駕駛輔助系統(tǒng)(Advanced-Driver Assistance System,ADAS)是自動駕駛車輛中的主要基礎(chǔ)之一，諸如障礙物辨識感測、輔助駕駛決策、車間與車內(nèi)通訊等各方面都算是先進駕駛輔助系統(tǒng)中的重要子系統(tǒng)，主要透過各類感測器取得車輛自身信息、周圍路況信息等，再由電子控制單元進行信息分析處理，以產(chǎn)生相應(yīng)的控制信號。

請參考圖9，為傳統(tǒng)車輛軌跡追蹤的控制邏輯示意圖，圖中表示先進駕駛輔助系統(tǒng)中的一控制器100可依據(jù)一已知的目標(biāo)軌跡200及回授得到的車輛行駛的實際軌跡，比較該目標(biāo)軌跡200與實際軌跡之間的誤差量，根據(jù)該誤差量估算出一轉(zhuǎn)向角度，以該轉(zhuǎn)向角度控制車輛適當(dāng)?shù)卣{(diào)整移動路徑，目的是希望車輛盡可能地追蹤該目標(biāo)軌跡200而進行運動。

再請參考圖10，當(dāng)車輛依據(jù)圖9中的控制邏輯自動調(diào)整自身行駛路徑時，實際上可能發(fā)生如第一修正路徑310或第二修正路徑320的運動情況。第一修正路徑310代表在修正過程中，因為修正過慢，車輛在變換路徑時(如轉(zhuǎn)向)與目標(biāo)軌跡200產(chǎn)生明顯差距。另一種情況是第二修正路徑320，代表在修正過程中，因幅度過大，偏移過頭后而再次修正回目標(biāo)軌跡200。

會產(chǎn)生如圖10所述的第一修正路徑310或第二修正路徑320問題，原因在于控制器100僅比較該目標(biāo)軌跡200與實際軌跡之間的誤差量，未考慮在修正過程中車輛本身的動態(tài)變化。當(dāng)車輛的運動過程是如同第一修正路徑310或第二修正路徑320時，因短時間內(nèi)偏移幅度明顯且頻繁，恐會對車上乘客造成不適或有車輛翻覆的風(fēng)險。

技術(shù)實現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問題是提供一種可維持車輛穩(wěn)定轉(zhuǎn)向，避免發(fā)生車輛翻覆等意外的具路徑誤差修正的車輛軌跡追蹤方法。

為了實現(xiàn)上述目的，本發(fā)明提供了一種具路徑誤差修正的車輛軌跡追蹤方法，包含：

擷取車輛即時信息及一目標(biāo)軌跡；

根據(jù)該車輛即時信息，建立一預(yù)測軌跡；

根據(jù)該車輛即時信息，判斷出一車輛橫擺率門檻值；

計算該車輛橫擺率門檻值所對應(yīng)的一轉(zhuǎn)向角；

估算該轉(zhuǎn)向角所對應(yīng)的一側(cè)向誤差修正量；

判斷該側(cè)向誤差修正量是否小于或等于該目標(biāo)軌跡與該預(yù)測軌跡之間的一誤差值；

當(dāng)該側(cè)向誤差修正量小于或等于該目標(biāo)軌跡與該預(yù)測軌跡的誤差值，以該側(cè)向誤差修正量所對應(yīng)的轉(zhuǎn)向角控制車輛轉(zhuǎn)向；

當(dāng)該側(cè)向誤差修正量大于該目標(biāo)軌跡與該預(yù)測軌跡的誤差值，以該誤差值所對應(yīng)的轉(zhuǎn)向角控制車輛轉(zhuǎn)向。

為了更好地實現(xiàn)上述目的，本發(fā)明還提供了一種具路徑誤差修正的車輛軌跡追蹤裝置，包含：

多個感測器，用以分別感測多種不同的車輛即時信息；

一軌跡預(yù)測單元，依據(jù)該些車輛即時信息，建立一預(yù)測軌跡；及

一軌跡修正單元，依據(jù)該些車輛即時信息計算出一側(cè)向誤差修正量，并判斷該側(cè)向誤差修正量是否大于一誤差值，該誤差值為該目標(biāo)軌跡與該預(yù)測軌跡的誤差；當(dāng)該側(cè)向誤差修正量小于或等于該誤差值，輸出該側(cè)向誤差修正量所對應(yīng)的轉(zhuǎn)向角；當(dāng)該側(cè)向誤差修正量大于該誤差值，輸出該誤差值所對應(yīng)的轉(zhuǎn)向角；

其中，該軌跡修正單元在計算該誤差修正量時，根據(jù)車輛即時信息，判斷出一車輛橫擺率門檻值，計算該車輛橫擺率門檻值所對應(yīng)的一轉(zhuǎn)向角，并根據(jù)該轉(zhuǎn)向角估算出該側(cè)向誤差修正量。

小于或等于小于或等于本發(fā)明的技術(shù)效果在于：

本發(fā)明將該預(yù)測軌跡與目標(biāo)軌跡的誤差值作為一合理的誤差修正門檻，并 參考車輛即時信息，估算出一誤差修正量，在不超過該修正門檻的前提下，可根據(jù)該誤差修正量控制車輛的偏轉(zhuǎn)幅度，避免車輛轉(zhuǎn)向幅度過大而發(fā)生意外，令車輛可以因應(yīng)各種道路狀況追蹤該目標(biāo)軌跡而平穩(wěn)行駛。

以下結(jié)合附圖和具體實施例對本發(fā)明進行詳細(xì)描述，但不作為對本發(fā)明的限定。

附圖說明

圖1為本發(fā)明的控制邏輯示意圖；

圖2為本發(fā)明車輛軌跡追蹤的路徑示意圖；

圖3為本發(fā)明的方法流程圖；

圖4為本發(fā)明估算預(yù)測軌跡的計算示意圖；

圖5為本發(fā)明利用腳踏車模型計算轉(zhuǎn)向角的示意圖；

圖6為本發(fā)明依據(jù)側(cè)向誤差修正量控制車輛行進的示意圖；

圖7為本發(fā)明依據(jù)目標(biāo)軌跡與預(yù)測軌跡的誤差值ê控制車輛行進的示意圖；

圖8為本發(fā)明裝置的方塊圖；

圖9為傳統(tǒng)車輛軌跡追蹤的控制邏輯示意圖；

圖10為傳統(tǒng)車輛軌跡追蹤的路徑示意圖。

其中，附圖標(biāo)記

10 感測器

20 軌跡預(yù)測單元

30 軌跡修正單元

100 控制器

200 目標(biāo)軌跡

310 第一修正路徑

320 第二修正路徑

TP 預(yù)測軌跡

TG 目標(biāo)軌跡

TE 規(guī)劃軌跡

具體實施方式

下面結(jié)合附圖對本發(fā)明的結(jié)構(gòu)原理和工作原理作具體的描述：

請參考圖1、2所示，本發(fā)明根據(jù)車輛的即時信息建立一預(yù)測軌跡TP，該預(yù)測軌跡TP與一己獲得的目標(biāo)軌跡TG比較而得出一誤差值ê，并根據(jù)車輛的即時信息計算出一側(cè)向誤差修正量以決定應(yīng)利用該誤差值ê或側(cè)向誤差修正量所對應(yīng)之轉(zhuǎn)向角度，控制車輛變換路徑。

首先說明圖2，目標(biāo)軌跡TG是以實線表示，為根據(jù)現(xiàn)有技術(shù)即能建立，故不再贅述；預(yù)測軌跡TP是本發(fā)明中所建立，后面將有更進一步的說明；本發(fā)明依據(jù)車輛的即時信息所計算出之側(cè)向誤差修正量即對應(yīng)于一規(guī)劃軌跡TE，其中，在控制車輛行進時，視計算結(jié)果選用依據(jù)預(yù)測軌跡TP或規(guī)劃軌跡TE而前進，以使車輛的實際運動路徑盡可能維持與該目標(biāo)軌跡TG一致。以下將詳細(xì)說明本發(fā)明方法的詳細(xì)技術(shù)。

請參考圖3，本發(fā)明的方法包含有下述步驟：

S31：擷取車輛即時信息及一目標(biāo)軌跡TG，其中車輛即時信息可包含但不限于車速、側(cè)向加速度、駕駛者車輛模型、行駛情境等。

S32：建立一預(yù)測軌跡TP，利用車輛即時信息，估算預(yù)測軌跡TP。本實施例以圖4所示的模型為例說明，車輛前輪(以矩形圖案示意)的輪距中心以(x,y)表示其目前位置，在下一時間的預(yù)測位置以表示，其中D代表輪距中心(x,y)至轉(zhuǎn)向瞬心的長度，δ_f代表前輪轉(zhuǎn)角的角度。因此基于車輛從過去到目前位置所構(gòu)成的歷史軌跡，可連續(xù)估算出不同時間的預(yù)測位置藉此形成一預(yù)測軌跡TP，如與目標(biāo)軌跡TG有所不同，即存在目標(biāo)軌跡TG與預(yù)測軌跡TP的誤差值ê。

S33：判斷車輛橫擺率門檻值γ_th，根據(jù)車輛即時信息通過查表方式，判斷出對應(yīng)目前車輛即時信息的車輛橫擺率門檻值γ_th，因為是利用查表法判斷出，可利用現(xiàn)有的車輛模型配備模擬程序，預(yù)先建立一對照表，該對照表記錄車輛不同狀態(tài)下所對應(yīng)的車輛橫擺率門檻值γ_th。在本實施例中，該對照表記錄不同車速分別對應(yīng)的車輛橫擺率門檻值γ_th，即根據(jù)車輛的即時車速進行查表， 判斷對應(yīng)該即時車速的橫擺率門檻值γ_th。

S34：計算轉(zhuǎn)向角，根據(jù)查表得到的該橫擺率門檻值γ_th，計算出一對應(yīng)的轉(zhuǎn)向角δ_f，本實施例利用如圖5的腳踏車模型，依據(jù)下式計算出轉(zhuǎn)向角δ_f：

其中，代表橫擺率，L代表車輛長度，v_x代表縱向車速，v_ch代表車速。

S35：估算側(cè)向誤差修正量當(dāng)轉(zhuǎn)向角δ_f計算出來，再結(jié)合車輛的即時車速，計算出側(cè)向誤差修正量可利用下面所列的車輛側(cè)向運動狀態(tài)空間方程序求出：

各已知參數(shù)代表的意義如下：

v_ch為車速 v_x為縱向車速 v_y為側(cè)向車速

C_αf為前輪轉(zhuǎn)向剛性 C_αr為后輪轉(zhuǎn)向剛性

L為車輛長度 m為車輛質(zhì)量

a為前軸至重心長度 b為后軸至重心長度

Iz為轉(zhuǎn)動慣量 y為車輛側(cè)向位移

ψ為偏航角 r為橫擺率 δ_f為前輪轉(zhuǎn)角

其中，側(cè)向誤差修正量即是上述方程式中的

S36：判斷側(cè)向誤差修正量e 是否小于或等于目標(biāo)軌跡與預(yù)測軌跡的誤差值

S37：若側(cè)向誤差修正量小于或等于目標(biāo)軌跡與預(yù)測軌跡的誤差值ê，以側(cè)向誤差修正量所對應(yīng)的轉(zhuǎn)向角δ_f控制車輛偏轉(zhuǎn)。如圖6所示，目標(biāo)軌跡TG與預(yù)測軌跡TP的誤差值ê大于計算出來的側(cè)向誤差修正量因此可以利用該側(cè)向誤差修正量對應(yīng)的轉(zhuǎn)向角δ_f控制車輛，使車輛沿著規(guī)劃軌跡TE前 進，并更趨近于目標(biāo)軌跡TG。

S38：若側(cè)向誤差修正量大于目標(biāo)軌跡與預(yù)測軌跡的誤差值ê，以誤差值ê所對應(yīng)的轉(zhuǎn)向角δ_{f_new}控制車輛偏轉(zhuǎn)。如圖7所示，若計算出的側(cè)向誤差修正量大于誤差值ê時，假設(shè)仍依用該誤差修正量對應(yīng)的轉(zhuǎn)向角δ_f控制車輛沿規(guī)劃軌跡TE前進，可明顯看出車輛相對于目標(biāo)軌跡TG將會偏轉(zhuǎn)過頭，勢必需再次修正回目標(biāo)軌跡TG，故不會采用該轉(zhuǎn)向角δ_f。相反的，在此情況下，將會根據(jù)目標(biāo)軌跡與預(yù)測軌跡的誤差值ê，反算出對應(yīng)之轉(zhuǎn)向角δ_{f_new}，以該轉(zhuǎn)向角δ_{f_new}控制車輛偏轉(zhuǎn)。

請參考圖8，為本發(fā)明具路徑誤差修正的車輛軌跡追蹤裝置的方塊圖，該裝置是安裝在車輛中，可執(zhí)行如圖3所示的方法，該裝置包含有：

多個感測器10，用以分別感測多種不同的車輛即時信息，該感測器10的種類可包含有車速感測器、加速度感測器、鏡頭、定位裝置(GPS)、慣性測量器(IMU)等不同類型的一種或多種組合；

一軌跡預(yù)測單元20，依據(jù)車輛即時信息，建立預(yù)測軌跡TP；及

一軌跡修正單元30，依據(jù)車輛即時信息計算出一側(cè)向誤差修正量并判斷該側(cè)向誤差修正量是否大于一誤差值ê，以決定輸出對應(yīng)該側(cè)向誤差修正量之轉(zhuǎn)向角δ_f或誤差值ê對應(yīng)之轉(zhuǎn)向角δ_{f_new}，令車輛根據(jù)輸出的轉(zhuǎn)向角變換路徑。

其中，該軌跡修正單30可執(zhí)行如步驟S33～S38的詳細(xì)流程，故不再贅述。該軌跡預(yù)測單元20及該軌跡修正單元30可以整合成同一個單元，或由單一個微處理器實現(xiàn)。

綜上所述，本發(fā)明依據(jù)車輛的即時信息建立一預(yù)測軌跡TP，該預(yù)測軌跡TP與一己獲得的目標(biāo)軌跡TG比較而得出一誤差值ê，該誤差值ê即代表車輛轉(zhuǎn)向的最大可容許量，當(dāng)車輛轉(zhuǎn)角不超過該最大可容許量，即能確保車輛行駛平穩(wěn)，避免車輛翻覆等危險情況，乘客可享有較舒適的感受。本發(fā)明參考車輛即時信息，估算出一誤差修正量，在車輛轉(zhuǎn)角不超過該最大可容許量的前提下，可根據(jù)該誤差修正量控制車輛，使其運動路線更趨近于一目標(biāo)軌跡。

當(dāng)然，本發(fā)明還可有其它多種實施例，在不背離本發(fā)明精神及其實質(zhì)的情況下，熟悉本領(lǐng)域的技術(shù)人員當(dāng)可根據(jù)本發(fā)明作出各種相應(yīng)的改變和變形，但 這些相應(yīng)的改變和變形都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護范圍。