專利名稱：道路曲率估計(jì)和車輛目標(biāo)狀態(tài)估計(jì)系統(tǒng)的制作方法


在附圖中圖1說明了關(guān)于預(yù)測碰撞傳感系統(tǒng)的硬件的方框圖；圖2說明了預(yù)測碰撞傳感系統(tǒng)采用的雷達(dá)波束的覆蓋圖；圖3描述了用以說明預(yù)測碰撞傳感系統(tǒng)的工作的驅(qū)動(dòng)情況；圖4說明了預(yù)測碰撞傳感系統(tǒng)的硬件及相關(guān)信號(hào)處理算法的方框圖；圖5說明了預(yù)測碰撞傳感系統(tǒng)的相關(guān)信號(hào)處理算法的流程圖；圖6說明了用于確定道路的曲率參數(shù)的幾何圖形；圖7說明了圓弧的幾何圖形；圖8a-d說明了用于直線道路的目標(biāo)位置、橫向速率及道路曲率參數(shù)的估計(jì)的例子；圖9a-b說明了在對(duì)應(yīng)圖8a-d的直線道路上的來自無約束(unconstrained)及有約束(constrained)濾波的目標(biāo)狀態(tài)RMS誤差的例子；圖10a-d說明了用于曲線道路的目標(biāo)位置、橫向速率和道路曲率參數(shù)的估計(jì)的例子；圖11a-b說明了對(duì)應(yīng)圖10a-d的曲線道路的來自無約束和有約束濾波的目標(biāo)狀態(tài)RMS誤差的例子；圖12a-d說明了用于包含車道轉(zhuǎn)換的直線道路的目標(biāo)位置、橫向速率及相關(guān)RMS誤差的估計(jì)的例子，及圖13a-d說明了用于包含車道轉(zhuǎn)換的曲線道路的目標(biāo)位置、橫向速率及其RMS誤差的估計(jì)的例子。
具體實(shí)施例方式
參考圖1，合并在主車輛12中的預(yù)測碰撞傳感系統(tǒng)10包括用于傳感主車輛12外部的物體的雷達(dá)系統(tǒng)14；和一套傳感器，包括橫擺率傳感器16，如陀螺傳感器，及速度傳感器18，用于傳感主車輛12的運(yùn)動(dòng)。橫擺率傳感器16和速度傳感器18分別提供對(duì)主車輛12的橫擺率和速度的測量。雷達(dá)系統(tǒng)14，如多普勒雷達(dá)系統(tǒng)，包括天線20和雷達(dá)處理器22，其中雷達(dá)處理器22產(chǎn)生射頻信號(hào)，它由天線22發(fā)送并被在其視角范圍內(nèi)的物體反射。雷達(dá)處理器22將天線20接收的相關(guān)反射的射頻信號(hào)進(jìn)行解調(diào)，并探測一個(gè)信號(hào)，該信號(hào)對(duì)應(yīng)于天線20發(fā)送的射頻信號(hào)所輻射到的一個(gè)或多個(gè)物體。例如，雷達(dá)系統(tǒng)14提供主車輛12固定坐標(biāo)內(nèi)的目標(biāo)范圍、范圍速率和方位角測量。參考圖2，天線20適合用于產(chǎn)生射頻能量的雷達(dá)波束23，其例如對(duì)應(yīng)于波束控制元件24被電或機(jī)械掃描一個(gè)方向角范圍，如+/-γ，如+/-50度，并且它離主車輛12約有例如100米的距離范圍，這個(gè)距離范圍足夠遠(yuǎn)，可使目標(biāo)在與主車輛12可能發(fā)生碰撞之前足夠遠(yuǎn)的地方就被探測到，從而潛在減輕了主車輛12的動(dòng)作，從而避免了可能發(fā)生的碰撞或減輕了損傷或受傷。雷達(dá)處理器22、橫擺率傳感器16和速度傳感器18可操作地連接到信號(hào)處理器26上，信號(hào)處理器26根據(jù)相關(guān)的預(yù)測碰撞傳感算法工作，確定是否可能與物體碰撞，如目標(biāo)車輛36(圖3中所示)，如果是，還要確定采取相應(yīng)的行動(dòng)，例如，如下一個(gè)或多個(gè)行動(dòng)激活相關(guān)警告系統(tǒng)28或安全系統(tǒng)30(如前部安全氣囊系統(tǒng))，或采用規(guī)避行動(dòng)的車輛控制系統(tǒng)32(如相關(guān)制動(dòng)或轉(zhuǎn)向系統(tǒng))，采取以上一個(gè)或多個(gè)方式以便避免可能發(fā)生的碰撞或減輕后果。
參考圖3，主車輛12沿著多車道道路34移動(dòng)，道路可為直線也可為曲線，另外顯示了目標(biāo)車輛36朝著主車輛12以相反的方向移動(dòng)。通常，在道路34上可能有任何數(shù)目的目標(biāo)車輛36，每個(gè)目標(biāo)車輛都與主車輛12以相同或相反的方向移動(dòng)。這些目標(biāo)車輛36可在主車道38上也可在與主車道38相鄰或分離但通常與主車道38平行的鄰近車道40上。為了分析起見，假設(shè)主車輛12沿著車道38的中心線41穩(wěn)步移動(dòng)，沒有在車道內(nèi)偏移，而且所有平行車道38、40的道路曲率是相同的。假設(shè)道路曲率很小，以致主車輛12和所有可探測到的目標(biāo)車輛36的方向角間的差值小于15度。
參考圖4，預(yù)測碰撞傳感系統(tǒng)10采用分別來自速度傳感器18和橫擺率傳感器16的主車輛12的速度Uh和橫擺率ωh的測量值；來自安裝在主車輛12上的雷達(dá)系統(tǒng)14的對(duì)于所有目標(biāo)車輛36的目標(biāo)范圍r，范圍速率 和方位角η的測量值；以及所有這些測量值的對(duì)應(yīng)誤差協(xié)方差矩陣，以對(duì)于每一取樣距離估計(jì)固定主坐標(biāo)系統(tǒng)上每一個(gè)目標(biāo)的二維位置、速度和加速度 優(yōu)選具有盡可能小的誤差。預(yù)測碰撞傳感系統(tǒng)10包括1)道路曲率估計(jì)子系統(tǒng)42，采用主車輛運(yùn)動(dòng)傳感器，即橫擺率傳感器16和速度傳感器18的測量值來估計(jì)道路34的曲率；2)無約束(unconstrained)目標(biāo)狀態(tài)估計(jì)子系統(tǒng)44，用于估計(jì)雷達(dá)波束23探照到并被雷達(dá)處理器22探測到的目標(biāo)的狀態(tài)；3)有約束(constrained)目標(biāo)狀態(tài)估計(jì)子系統(tǒng)46，用于估計(jì)目標(biāo)上的約束條件(constraint)的狀態(tài)，假設(shè)目標(biāo)被約束在道路34上，可在主車道38上也可在鄰近車道40內(nèi)，車道38和40每一個(gè)都有可能；4)目標(biāo)狀態(tài)判定子系統(tǒng)48，用于確定目標(biāo)狀態(tài)的最佳估值是無約束目標(biāo)狀態(tài)還是受到一個(gè)約束條件約束的目標(biāo)狀態(tài)；及5)目標(biāo)狀態(tài)合并子系統(tǒng)50，用于將無約束的目標(biāo)狀態(tài)估計(jì)與目標(biāo)狀態(tài)判定子系統(tǒng)48所識(shí)別的適當(dāng)約束條件合并，以產(chǎn)生合并的目標(biāo)狀態(tài)。目標(biāo)狀態(tài)的最佳估值——可為無約束目標(biāo)狀態(tài)也可為合并目標(biāo)狀態(tài)——被判決或控制子系統(tǒng)用來確定主車輛12是否存在與目標(biāo)發(fā)生碰撞的危險(xiǎn)，如果有，則決定并實(shí)行最佳行動(dòng)路線以減輕后果，如采用警告系統(tǒng)28或安全系統(tǒng)30，或車輛控制系統(tǒng)32，或這些措施的結(jié)合。如果可能，道路34的幾何結(jié)構(gòu)作為目標(biāo)運(yùn)動(dòng)的一個(gè)約束條件，提供了更為精確的目標(biāo)狀態(tài)估計(jì)，因而改善了對(duì)應(yīng)于此所采取任何行動(dòng)的可靠性。
另外參考圖5，說明了探測狀態(tài)的方法500，即主車輛12可視范圍內(nèi)的目標(biāo)的運(yùn)動(dòng)狀態(tài)變化，該方法的步驟例如通過信號(hào)處理器26執(zhí)行，在步驟(502)和(504)中，主車輛12相對(duì)于道路34的速度Uh和橫擺率ωh分別從速度傳感器18和橫擺率傳感器16中讀取。然后，在步驟(506)中，道路34的曲率參數(shù)及其相關(guān)協(xié)方差用第一52和第二54卡爾曼濾波器估計(jì)，它們分別估計(jì)主車輛12的狀態(tài)(即主車輛12的運(yùn)動(dòng)狀態(tài)變化)及其相關(guān)協(xié)方差，然后估計(jì)道路34的曲率參數(shù)及其相關(guān)協(xié)方差，如下文所述，其中道路34的曲率參數(shù)及其相關(guān)協(xié)方差隨后被有約束目標(biāo)狀態(tài)估計(jì)子系統(tǒng)46用來在可能發(fā)生的目標(biāo)車輛36的可能位置上產(chǎn)生相關(guān)約束條件。
嚴(yán)格定義和構(gòu)造的道路34可用一套參數(shù)來描述，包括曲率，其中道路34的弓形部分的曲率由下式定義C=1R---(1)]]>這里R為弓形部分的半徑。通常，對(duì)于一段光滑道路34來說，曲率變化可借用所謂的回旋曲線模型描述成沿著道路34的距離l的函數(shù)，即C=C0+dCdll=C0+C1l---(2)]]>這里C1＝1/A2而A稱為回旋曲線參數(shù)。
參考圖6，定義行進(jìn)方向的方向角θ由下式給定θ=θ0+∫01C(τ)dτ---(3)]]>將方程(2)帶入方程(3)得到Δθ＝θ-θ0＝C0l+C1l2/2(4)參考圖6，道路34的方程，即x-y坐標(biāo)內(nèi)的道路方程由下式給定x=x0+∫01cosθ(τ)dτ---(5)]]>及
假設(shè)方向角θ在15度內(nèi)，即|θ|＜15°，方程(5)和方程(6)可近似為Δx＝x-x0≈l (7)Δy=y-y0≈C0l2/2+C1l3/6≈C0Δx22+C1Δx36---(8)]]>因此，道路34可用曲率系數(shù)C0和C1表示的增量道路方程來模擬。這個(gè)增量道路方程描述了道路形狀的主要范圍1)直線道路34C0＝0和C1＝1；2)，環(huán)形道路34C1＝0；及3)具有任意形狀的一般道路34，其方向角θ的變化小于15度C0＞0。
道路曲率參數(shù)C0和C1采用來自主車輛12的運(yùn)動(dòng)傳感器(橫擺率傳感器16和速度傳感器18)的數(shù)據(jù)來估計(jì)，它基于主車輛12沿著道路34或相關(guān)主車道38的中心線41運(yùn)動(dòng)的假設(shè)。
道路曲率參數(shù)C0和C1根據(jù)與來自可得到的主車輛12運(yùn)動(dòng)傳感器的主車輛12的橫擺率ωh和速度Uh測量值對(duì)應(yīng)的ω， ，U， 數(shù)據(jù)來計(jì)算。但是，通常分別來自橫擺率傳感器16和速度傳感器18的橫擺率ωh和速度Uh的測量值是有噪聲的。由第一卡爾曼濾波器52實(shí)現(xiàn)的主狀態(tài)濾波器有利于根據(jù)相關(guān)的橫擺率ωh和速度Uh的有噪測量值來估計(jì)ω， ，U， ；之后，由第二卡爾曼濾波器54實(shí)現(xiàn)的曲率濾波器用來產(chǎn)生曲率參數(shù)C0和C1的平滑估計(jì)。用于主狀態(tài)濾波器的主車輛12的動(dòng)力學(xué)遵循一套預(yù)定義的動(dòng)力學(xué)方程(在這種情況下為常速度)由下式給定x‾k+1h=Fkh·x‾kh+w‾kh,w‾kh~N(0,Qkh)---(9)]]>z‾kh=Hkh·x‾kh+v‾kh,v‾kh~N(0,Rkh)---(10)]]>其中Fkh=1T000100001T0001,Hkh=10000010,x‾kh=UU·ωω·k,]]>及z‾kh=Uhωhk---(11)]]>
且其中T為采樣周期，上標(biāo)(·)h用來表示濾波器為主濾波器，而Uh和ωh為主車輛12速度和橫擺率測量值。第一卡爾曼濾波器52用來估計(jì)主狀態(tài) 及它的誤差協(xié)方差Pk|kh，如圖4中所示。
來自第一卡爾曼濾波器52即主狀態(tài)濾波器的主狀態(tài)估計(jì)用來產(chǎn)生合成測量值，它輸入到第二卡爾曼濾波器54上，即曲率系數(shù)濾波器，其中相關(guān)的卡爾曼濾波器52、54根據(jù)卡爾曼濾波過程工作，將在下文附錄中更充分說明。道路曲率參數(shù)C0和C1及主狀態(tài)變量ω， ，U， 間的關(guān)系推導(dǎo)如下根據(jù)方程(4)，道路曲率的半徑R通常表示為沿著道路的距離l的函數(shù)R(l)，如圖7中所示。在方程(4)兩邊都對(duì)時(shí)間取導(dǎo)數(shù)，得到θ·=C0·l.+C1·l·l.=(C0+C1·l)·l.---(12)]]>注意θ.=ω,]]>主車輛12的橫擺率，及l(fā).=U,]]>主車輛12的速度，將方程(2)的回旋曲線模型帶入方程(12)中，得到ω＝C·U (13)或C=ωU---(14)]]>回旋曲線參數(shù)C0由l＝0處的曲率值給定，或C0=C|l=0=ωU---(15)]]>對(duì)方程(14)的兩邊都求導(dǎo)，得到C·=ω·U-ω·U·U2---(16)]]>使用根據(jù)方程(2)的C1的定義，C1可用主狀態(tài)來表示，如下C1=dCdl=dCdt·dtdl=C·U=ω·U2-ω·U·U3---(17)]]>第二卡爾曼濾波器54即曲率濾波器產(chǎn)生曲率估計(jì) 和 ，其系統(tǒng)方程由下式給定Fx‾k+1C=FkC·x‾kC+w‾kC,w‾kC~N(0,QkC)---(18)]]>
z‾kC=HC·x‾kC+vkC,vkC~N(0,RkC)---(19)]]>其中FkC=1Δt·U^01,Hkh=1001,x‾kC=C0C1k---(19a)]]>Δt為第二卡爾曼濾波器54的更新時(shí)間周期，而測量向量zkC的元素值由曲率濾波器的狀態(tài)變量——即回旋曲線參數(shù)C0和C1——的對(duì)應(yīng)值給定。測量值zkC從估計(jì)的狀態(tài) 轉(zhuǎn)換得來，如下z‾kC=ω^U^ω·^U^2-ω^·U·^U^3k|k---(20)]]>及測量值的相關(guān)協(xié)方差由下式給定RkC=JkCPk|kh(JkC)T---(21)]]>其中JkC=∂C0C1∂x‾h|x‾h=x‾^k|kh=-ω^U^201U^0-2·ω·^U^3+3·ω^·U·^U^4-ω·^U^3-U·^U^31U^2k|k---(22)]]>應(yīng)該理解，估計(jì)道路34的曲率參數(shù)的其它系統(tǒng)和方法也可替代上述的道路曲率估計(jì)子系統(tǒng)42。例如，道路曲率參數(shù)也可通過可視系統(tǒng)，根據(jù)道路34的圖像來估計(jì)，可以取代也可結(jié)合基于來自相關(guān)運(yùn)動(dòng)傳感器的速度Uh和橫擺率ωh的測量值的上述系統(tǒng)。而且，應(yīng)該理解，橫擺率可以各種方式或采用各種手段測量或判斷，例如可以采用(但不限于)橫擺陀螺傳感器、轉(zhuǎn)向角傳感器、差動(dòng)輪速度傳感器或基于GPS的傳感器；及其結(jié)合；或其測量值的函數(shù)(即特別是其中的轉(zhuǎn)向角速率的函數(shù))。
再次參考圖5，在步驟(508)中，目標(biāo)范圍r、范圍速度 和方位角η的測量值從雷達(dá)處理器22中讀取，用作擴(kuò)展卡爾曼濾波器56即主濾波器的輸入，其在步驟(510)中產(chǎn)生無約束目標(biāo)狀態(tài)的估值——即目標(biāo)的動(dòng)力學(xué)狀態(tài)變量——這個(gè)估值是隨之運(yùn)動(dòng)的主車輛12的局部坐標(biāo)系統(tǒng)(即主固定坐標(biāo)系統(tǒng))中的相對(duì)值。在步驟(512)中，無約束目標(biāo)狀態(tài)，即目標(biāo)速率和加速度，被轉(zhuǎn)換成當(dāng)前時(shí)刻固定在主車輛12上的絕對(duì)坐標(biāo)系統(tǒng)的絕對(duì)坐標(biāo)，如圖3中所示，以便與絕對(duì)坐標(biāo)系統(tǒng)一致，在絕對(duì)坐標(biāo)系統(tǒng)中推導(dǎo)得到道路約束方程，且為了產(chǎn)生約束目標(biāo)狀態(tài)的估值，當(dāng)用于下文所述的相關(guān)約束方程時(shí)，相關(guān)的曲率參數(shù)假設(shè)為常數(shù)。絕對(duì)坐標(biāo)系統(tǒng)疊加當(dāng)前瞬時(shí)的空間中的移動(dòng)坐標(biāo)系統(tǒng)，以便實(shí)現(xiàn)步驟(512)中的轉(zhuǎn)換，它通過將主車輛12的x和y方向上的速率和加速度加到對(duì)應(yīng)的目標(biāo)估值上。
來自擴(kuò)展卡爾曼濾波器56的輸出的根據(jù)步驟(512)中的坐標(biāo)轉(zhuǎn)換的結(jié)果被分成如下部分，分別對(duì)應(yīng)相對(duì)于主車輛12的目標(biāo)車輛36的x和y位置，其中，上標(biāo)1是指目標(biāo)車輛36的無約束目標(biāo)狀態(tài)X‾^tk|k1=x‾^t1y‾^t1k|k]]>及Ptk|k1=PxtPxyt1Pyxt1Pyt1k|k---(23)]]>再次參考圖5，緊隨步驟(506)和(512)的步驟(514)到(524)在下文將進(jìn)行更充分說明，在這些步驟中，加載并檢驗(yàn)?zāi)繕?biāo)車輛36的可能軌跡上的各種約束條件，以判斷目標(biāo)車輛36是否可能按照一個(gè)可能的約束條件行進(jìn)。例如，假設(shè)約束條件來自于包括主車道38和可能的鄰近車道40的一組車道，可能按照一個(gè)可能約束條件而行進(jìn)的目標(biāo)車輛36可能在主車道38或者在可能的鄰近車道40上行進(jìn)。在步驟(524)中，假設(shè)目標(biāo)車輛36在主車道38或一個(gè)可能的鄰近車道40上行進(jìn)，這個(gè)假設(shè)要針對(duì)每一個(gè)可能的車道進(jìn)行測試。如果這個(gè)假設(shè)不滿足一個(gè)可能的車道，那么，在步驟(526)中，假設(shè)目標(biāo)的狀態(tài)為無約束狀態(tài)，然后將它用于對(duì)應(yīng)的后續(xù)預(yù)測碰撞傳感分析和控制。否則，從步驟(524)，在步驟(528)中，目標(biāo)狀態(tài)通過目標(biāo)狀態(tài)合并子系統(tǒng)50計(jì)算得到，它合并了無約束目標(biāo)狀態(tài)和步驟(524)中識(shí)別的最可能的相關(guān)的約束狀態(tài)。
在討論步驟(514)到(524)的過程之前，先就與主車輛12在相同車道上運(yùn)動(dòng)的目標(biāo)車輛36的情況，對(duì)合并無約束目標(biāo)狀態(tài)和有約束的狀態(tài)的過程進(jìn)行說明，其中步驟(514)到(524)用于判定目標(biāo)是否可能受到約束條件的約束，如果是，還要判定最可能的約束條件。假設(shè)約束條件僅在y方向有效，與主車輛12沿著其車道38的中心線41穩(wěn)步移動(dòng)而沒有車道內(nèi)偏移及所有平行車道38、40的道路曲率相同的假設(shè)一致，同時(shí)假設(shè)在當(dāng)前瞬時(shí)，絕對(duì)坐標(biāo)系統(tǒng)固定在主車輛12上。假設(shè)目標(biāo)車輛36與主車輛12在相同的車道38上移動(dòng)，采用包含估計(jì)系數(shù)的道路約束方程，在步驟(514)中，約束狀態(tài)變量用橫向運(yùn)動(dòng)變量給出y‾‾^tk|k=y‾y·‾y··‾tk|k=C^0(x^1)2/2+C^1(x^1)3/6C^0x^1x·^1+C^1(x^1)2x·^1/2C^0(x·^1)2+C^0x^1x··^1+C^1x^1(x·^1)2+C^1(x^1)2x··^1/2---(24)]]>及P‾ytk|k=Ak1Pxtk|k(Ak1)T+Ak2Pk|kC(Ak2)T---(25)]]>其中Ak1=C^0x^1+C^1(x^1)2/200C^0x·^1+C^1x^1x·^1C^0x^1+C^1(x^1)2/20C^0x··^1+C^1(x·^1)2+C^1x^1x··^12C^0x.+^2C^1x^1x·^^C^0x^1+C^1(x^1)2/2---(26)]]>及Ak2=(x^1)2/2(x^1)3/6x^1·x·^1(x^1)2·x·^1/2(x·^1)2+x^1·x··^1x^1·(x·^1)2+(x^1)2·x··^1/2---(27)]]>在步驟(528)中，兩個(gè)y坐標(biāo)估值，一個(gè)來自主濾波器而另—個(gè)來自道路約束，這兩個(gè)值合并為如下 y‾^tk|kf=Pytk|kf[(P‾ytk|k)-1y‾‾^tk|k+(Pytk|k1)-1y‾^tk|k1]---(29)]]>最后，目標(biāo)狀態(tài)的組合估值為
X‾^tk|k=x‾^tk|k1y‾^tk|kf---(30)]]>及Ptk|k=PxtP‾xytP‾xyt′Pytfk|k---(31)]]>其中P‾xyt=Pxt(Ak1)t---(32)]]>在步驟(530)中，如果從步驟(514)到(524)目標(biāo)車輛36被判定在主車道38上運(yùn)動(dòng)，則這個(gè)組合估值將作為目標(biāo)狀態(tài)的估值的輸出。
返回到步驟(514)到(524)的過程，該過程用于判定目標(biāo)是否可能受到約束條件的約束，如果是，則判定最可能的約束條件；根據(jù)目標(biāo)沿著相同的道路34的假設(shè)，如果目標(biāo)車輛36已知在特定車道上運(yùn)動(dòng)，那么用該車道的估計(jì)道路參數(shù)作為估計(jì)目標(biāo)運(yùn)動(dòng)的主濾波器中的約束條件是合理的。但是，目標(biāo)車輛36當(dāng)前在哪一車道的信息通常是不可得到的，尤其是當(dāng)目標(biāo)在彎曲的道路34上移動(dòng)時(shí)。因?yàn)榈缆贩匠?8)僅適用于主中心坐標(biāo)系統(tǒng)內(nèi)的主車道38，約束濾波需要知道目標(biāo)所在的車道，因此對(duì)于不同的車道需要不同的約束方程。忽略平行車道間的道路曲率參數(shù)的差異，即假設(shè)每個(gè)道路的曲率相同，則任意道路的道路方程可寫成y=mB+C^0x22+C^1x36,m=0,±1,±2,·····---(33)]]>其中B為車道寬度，m表示將描述的車道(m＝0對(duì)應(yīng)于主車道38，m＝1對(duì)應(yīng)于右邊鄰近車道40，m＝-1對(duì)應(yīng)于左邊鄰近車道40等等)。由于預(yù)先不了解目標(biāo)車道位置，形成多約束系統(tǒng)(類似于所謂的多模型系統(tǒng))的多個(gè)約束條件中的每一個(gè)條件都要進(jìn)行檢驗(yàn)，以判定識(shí)別哪一個(gè)約束條件有效。多約束(MC)系統(tǒng)屬于有限數(shù)個(gè)NC約束條件之一。在任何給定時(shí)刻僅有一個(gè)約束條件有效。這種系統(tǒng)稱作混合系統(tǒng)——它們具有連續(xù)(噪聲)狀態(tài)變量和不連續(xù)個(gè)約束條件。
下面的定義和模型假設(shè)有助于解決這個(gè)問題約束方程y‾tk=f‾tk(x‾tk)---(34)]]>其中，ftk表示在采樣周期過程的在終止時(shí)刻tk有效的約束條件。約束條件在可能的NC個(gè)約束條件之中f‾tk∈{f‾j}j=1NC---(35)]]> 用約束條件ftkj在時(shí)刻tk的狀態(tài)估值Pytk|kj，Pxytk|kj在約束條件ftkj下的時(shí)刻tk的協(xié)方差矩陣μtk-1j在tk-1時(shí)刻目標(biāo)滿足約束條件j的概率約束條件轉(zhuǎn)移過程是一個(gè)具有已知轉(zhuǎn)移概率的馬爾可夫鏈P{f‾tk=f‾j|f‾tk-1=f‾i}=pij---(36)]]>為了實(shí)現(xiàn)馬爾可夫模型——對(duì)于包含超過一個(gè)可能約束狀態(tài)的系統(tǒng)——假設(shè)在每一掃描時(shí)刻目標(biāo)會(huì)從約束狀態(tài)i轉(zhuǎn)換到約束狀態(tài)j的概率為pij。假設(shè)這些概率為已知的先驗(yàn)概率(a priori)，且可用如下所示的概率轉(zhuǎn)換矩陣表示新狀態(tài) fj正確(fj有效)的先驗(yàn)概率為P(f‾j|Z0)=μt0j---j=1,···,NC---(38)]]>
其中Z0為先驗(yàn)信息及Σj=1NCμt0j=1---(39)]]>因?yàn)檎_約束條件在假定的NC個(gè)可能約束條件中。
約束目標(biāo)狀態(tài)估計(jì)子系統(tǒng)46用來判定目標(biāo)狀態(tài)是否對(duì)應(yīng)可能的約束狀態(tài)，如果是，然后用來判定最可能的約束狀態(tài)。
判定的方法之一是由Bar-Shalom提出的多模型濾波算法，其中，NC個(gè)并聯(lián)濾波器同時(shí)并行工作。
另一種方法中，多約束(MC)估計(jì)算法采用無約束狀態(tài)估值 作為測量值，混合并更新NC個(gè)約束條件的狀態(tài)估值，同時(shí)計(jì)算與每個(gè)約束條件有關(guān)的似然性函數(shù)及概率。在多約束(MC)估計(jì)算法的一個(gè)實(shí)施例中，約束狀態(tài)估值輸出是所有約束條件的狀態(tài)估值的復(fù)合。如果約束狀態(tài)估值有效，即如果約束狀態(tài)估值對(duì)應(yīng)無約束狀態(tài)估值，那么目標(biāo)狀態(tài)通過合并約束和無約束狀態(tài)估值給定，否則目標(biāo)狀態(tài)由無約束狀態(tài)估值給定。多約束(MC)估計(jì)算法的實(shí)施例包括如下步驟1.根據(jù)多約束條件的狀態(tài)變量的估計(jì)在步驟(514)中，采用多車道道路方程(33)替換方程(24)中的第一行，多約束狀態(tài)估值由下式給定y‾‾^tk|k0j=y‾y·‾y··‾tk|k=Bj+C^0·(x^1)2/2+C^1·(x^1)3/6C^0·x^1·x·^1+C^1·(x^1)2·x·/2^C^0·(x·^1)2+C^0·x^1·x··^1+C^1·x^1·(x·^1)2+C^1·(x^1)2·x··^1/2---(40)]]>其中，Bj=0,±,...,±NC-12-B,]]>而B為車道寬度。換言之，約束狀態(tài)估值對(duì)應(yīng)于目標(biāo)車輛36所處的每一個(gè)可能車道的中心線的y位置。
相關(guān)協(xié)方差由下式給定
P‾ytk|k0j=Ak1·Pxtk|k·(Ak1)T+Ak2·Pk|kC·(Ak2)T---(41)]]>其中Ak1和Ak2由方程(26)和方程(27)給定，Pxtk|k來自方程(23)而Pk|kC來自曲率濾波器。
2.以約束條件為條件的更新在步驟(516)中，以有效約束條件為條件的狀態(tài)估值和協(xié)方差被更新，以及每一個(gè)約束條件j＝1，...NC的約束條件似然性函數(shù)也被更新。對(duì)應(yīng)約束條件j的更新狀態(tài)估值和協(xié)方差采用測量值 得到，如下y‾‾^tk|kj=y‾‾^tk|k0j+P‾ytk|k0j(P‾ytk|k0j+Pytk|k1)-1(y‾^tk|k1-y‾‾^tk|k0j)---(42)]]>P‾ytk|kj=P‾ytk|k0j-P‾ytk|k0j(P‾ytk|k0j+Pytk|k1)-1P‾ytk|k0j---(43)]]>P‾xytk|kj=P‾xytk|k0j-P‾ytk|k0j(P‾ytk|k0j+Pytk|k1)-1P‾ytk|k0j---(44)]]>3.似然性計(jì)算在步驟(518)中，對(duì)應(yīng)約束條件j的似然性函數(shù)以無約束目標(biāo)狀態(tài)估值的ytk|k1來估算，假設(shè)每一個(gè)約束條件j＝1，...NC的約束條件狀態(tài)估值周圍的測量值為高斯分布，如下Λtkj=N(y‾tk|k1;y‾‾^tk|k0j,P‾ytk|k0j+Pytk|k1)---(45)]]>其中，高斯分布N(；，)具有均值 及相關(guān)協(xié)方差Pytk|k0j+Pytk|k1。
4.約束條件概率估算在步驟(520)中，就每一個(gè)約束條件j＝1，...NC計(jì)算更新的約束概率，如下μtkj=1aΛtkja‾j---(46)]]>其中，aj為約束條件j有效時(shí)變換后的概率，由下式給定a‾j=Σi=1NCpij·μtk-1i---(47)]]>及標(biāo)準(zhǔn)化常數(shù)為a=Σj=1NCΛtkja‾j---(48)]]>
5.整體狀態(tài)估值和協(xié)方差在步驟(522)中，最新的約束條件狀態(tài)估值和協(xié)方差的組合由下式給定y‾‾^tk|k=Σj=1NCμtk|kj·y‾‾^tk|kj---(49)]]>P‾ytk|k=Σj=1NCμtkj·[P‾ytk|kj+(y‾‾^tk|kj-y‾‾^tk|k)·(y‾‾^tk|kj-y‾‾^tk|k)′]---(50)]]>P‾xytk|k=Σj=1NCμtkj·P‾xytk|kj---(51)]]>在上述算法中步驟(522)的估計(jì)器輸出用作方程(28)和(29)中所述的合并過程的約束估值，而方程(51)的結(jié)果取代方程(32)的結(jié)果用于方程(31)中。
當(dāng)目標(biāo)車輛36不沿著道路34或改變車道時(shí)，強(qiáng)加于目標(biāo)運(yùn)動(dòng)狀態(tài)變量的道路約束將產(chǎn)生不正確的估計(jì)，它可能比采用相關(guān)的無約束估值更差。但是，噪聲相關(guān)的估計(jì)誤差可能導(dǎo)致正確的道路約束變得無效。因此，將兩種情況合并的措施是有利的，即當(dāng)它們有效，如當(dāng)目標(biāo)車輛36沿著特定車道時(shí)使約束條件保持有效；而當(dāng)它們無效，如當(dāng)目標(biāo)車輛36偏離車道時(shí)，迅速將它們移走。無約束目標(biāo)狀態(tài)估值在道路約束有效上起很大作用，因?yàn)樗峁┝霜?dú)立的目標(biāo)狀態(tài)估值。
一種方法就是檢驗(yàn)以下的假設(shè)無約束目標(biāo)狀態(tài)估值滿足道路約束方程，或等價(jià)于約束估值和無約束估計(jì)各自對(duì)應(yīng)同一目標(biāo)。最佳檢驗(yàn)要求采用到時(shí)刻tk過程中所有有效目標(biāo)狀態(tài)估值，這通常是不切實(shí)際的。實(shí)際方法是順序假設(shè)檢驗(yàn)，即只根據(jù)最近狀態(tài)估值來進(jìn)行檢驗(yàn)。根據(jù)上文所用的符號(hào)，有約束和無約束目標(biāo)狀態(tài)估值間的差值(僅y方向)表示為δ‾^tk=y‾^tk|k1-y‾‾^tk|k---(52)]]>作為如下的估值δ‾tk=y‾tk1-y‾‾tk---(53)]]>
其中ytk1為實(shí)際目標(biāo)狀態(tài)且ytk為沿著道路34(或車道)移動(dòng)的目標(biāo)的真實(shí)狀態(tài)。在步驟(524)中，“同一目標(biāo)”假設(shè)被檢驗(yàn)，即H0:δ‾tk=0---(54)]]>對(duì)H1:δ‾tk≠0---(55)]]>主濾波器誤差y‾~tk1=y‾tk1-y‾^tk|k1---(56)]]>假設(shè)獨(dú)立于誤差y‾‾~tk=y‾‾tk-y‾‾^tk|k---(57)]]>其是來自約束條件的。在假設(shè)H0的前提下，差值 的協(xié)方差由下式給定Ptkδ=E(δ‾~tkδ‾~tkt)=E[(y‾~tk1-y‾‾~tk)(y‾~tk1-y‾~tk1)′]=Pytk|k1+P‾ytk|k---(58)]]>假設(shè)估計(jì)誤差為高斯型，H0對(duì)H1的檢驗(yàn)如下如果ρtk=δ‾^tkt(Ptkδ)-1δ‾^tk≤γ---(59)]]>接受H0選擇的門限使得P(ρtk>γ|H0)=α---(60)]]>其中α為預(yù)定義誤差容許值。注意，基于上述高斯誤差假設(shè)，ρtk是自由度為ny的卡方(chi-square)分布。門限的選擇是很重要的設(shè)計(jì)因素，應(yīng)該基于特殊的應(yīng)用需要。在道路上的車輛碰撞預(yù)測中，注意到主車道38上的目標(biāo)處在碰撞路線，認(rèn)為它比鄰近車道40之一的目標(biāo)更危險(xiǎn)。因此，對(duì)于主車道38上的目標(biāo)希望具有高門限(低誤差容許值)，因?yàn)榧s束濾波可提供精確的目標(biāo)狀態(tài)估計(jì)，同時(shí)這個(gè)目標(biāo)的“改變車道”操縱不會(huì)對(duì)主車輛12構(gòu)成威脅。另一方面，鄰近車道40上的目標(biāo)通常認(rèn)為是從旁邊經(jīng)過的車輛。雖然約束濾波可進(jìn)一步減少錯(cuò)誤報(bào)警率，但是這個(gè)目標(biāo)“改變車道”操縱(進(jìn)入主車道38)會(huì)對(duì)主車輛12構(gòu)成實(shí)際的威脅。因此，如果錯(cuò)誤報(bào)警速率已經(jīng)足夠低，對(duì)于鄰近車道的目標(biāo)，希望具有低門限(高誤差容許值)。
根據(jù)上述分析，假設(shè)檢驗(yàn)方案對(duì)不同車道上的目標(biāo)有效采用不同的門限值，多約束濾波算法提供目標(biāo)當(dāng)前最可能所處車道的信息。假設(shè)在道路34上有NC個(gè)可能的車道，每一個(gè)車道都用一個(gè)約束方程來描述，對(duì)于目標(biāo)的具有最大概率μtkj的約束方程對(duì)應(yīng)于tk時(shí)刻(當(dāng)前時(shí)刻)該目標(biāo)最可能位于的車道。用lt來表示這個(gè)最可能的車道，則μtklt=maxj{μtkj,j=1,···,r}---(61)]]>無約束狀態(tài)估值和車道lt約束狀態(tài)估值間的差值(僅y方向)表示為δ‾^tklt=y‾^tk|k1-y‾‾^tk|klt---(62)]]>是如下的估值δ‾tklk=y‾tk1-y‾‾tklt---(63)]]>其中ytk1為實(shí)際目標(biāo)狀態(tài)而ytkl1為沿著車道lt移動(dòng)的目標(biāo)的實(shí)際狀態(tài)。
對(duì)于“同一目標(biāo)”假設(shè)的檢驗(yàn)由下式給定H0:δ‾tklt=0---(64)]]>對(duì)H1:δ‾tklt≠0---(65)]]>約束估計(jì)誤差由下式給定y‾‾~tklt=y‾‾tk-y‾‾^tk|klt---(66)]]>假設(shè)估計(jì)誤差是獨(dú)立的且為高斯型，H0H1的檢驗(yàn)變成如果ρtklt=(δ‾^tklt)′(Ptkδl)-1δ‾^tklt≤γlt---(67)]]>則接受H0其中Ptkδt=E[(δ‾tklt-δ‾^tklt)(δ‾tklt-δ‾^tklt)′]=E[(y‾~tk1-y‾‾~tklt)(y‾~tk1-y‾‾~tklt)′]=Pytk|k1+P‾ytk|klt---(68)]]>
且門限使得P(ρtklt>γlt|H0,lt)=αlt---(69)]]>其中γlt∈{γj}j=1r]]>且αlt∈{αj}j=1r---(70)]]>當(dāng)目標(biāo)車輛36離開其當(dāng)前車道時(shí)，這種車道自適應(yīng)假設(shè)檢驗(yàn)方案提供將目標(biāo)狀態(tài)估計(jì)輸出迅速轉(zhuǎn)換到無約束估值，同時(shí)主車道38上的目標(biāo)的估計(jì)精度通過約束濾波得到充分改善。
在多約束(MC)估計(jì)算法的另一實(shí)施例中，用于假設(shè)檢驗(yàn)的約束狀態(tài)估值是分散的有約束目標(biāo)狀態(tài)估值中的最可能的情況(即根據(jù)“勝者全得”戰(zhàn)略)，而不是所有的有約束目標(biāo)狀態(tài)估值的復(fù)合組合。如果最可能的有約束狀態(tài)估計(jì)有效，即如果最可能的有約束狀態(tài)估值對(duì)應(yīng)無約束狀態(tài)估計(jì)，則目標(biāo)狀態(tài)通過將最可能的有約束狀態(tài)估計(jì)和無約束狀態(tài)估計(jì)合并給定；否則目標(biāo)狀態(tài)由無約束狀態(tài)估計(jì)給定。
然而在多約束(MC)估計(jì)算法的又一實(shí)施例中，要對(duì)每一個(gè)有約束狀態(tài)估值進(jìn)行假設(shè)檢驗(yàn)。如果沒有一個(gè)假設(shè)滿足，則目標(biāo)狀態(tài)由無約束狀態(tài)估值給定。如果有一個(gè)假設(shè)滿足，則目標(biāo)狀態(tài)通過將對(duì)應(yīng)的有約束狀態(tài)估值和無約束狀態(tài)估值合并給定。如果不只一個(gè)假設(shè)滿足，則通過對(duì)多個(gè)方法的表決結(jié)果或用不同的相關(guān)門限重復(fù)假設(shè)檢驗(yàn)來識(shí)別最可能的有約束狀態(tài)。
通常，約束條件的個(gè)數(shù)(即道路車道的個(gè)數(shù))隨時(shí)間而改變，此外相關(guān)參數(shù)如道路的車道寬度也會(huì)改變，以適應(yīng)主車道12環(huán)境的變化。例如，一個(gè)旅途中的主車輛12可在一車道公路、具有反向通行的兩車道公路、具有中心轉(zhuǎn)向車道的三車道公路、兩個(gè)車道反向通行的四車道公路或多車道分開的高速公路上行進(jìn)。
對(duì)四種情況進(jìn)行采用有約束和無約束濾波的道路車輛跟蹤仿真。在所有情況中，主車道12以15.5m/s速度移動(dòng)，目標(biāo)車輛36以15.5m/s在相同的道路34上逼近。在x方向上，目標(biāo)的初始位置離主車輛125m，假設(shè)所有車道的車道寬為3.6m。車輛速度傳感器的測量偏差為0.02m/s，陀螺橫擺率測量偏差為0.0063rad/s。雷達(dá)范圍、范圍速率和方位角測量偏差分別為0.5m、1m/s及1.5°。然后用100個(gè)蒙特卡羅法(Monte-Carlo)分布的相關(guān)跟蹤濾波器產(chǎn)生仿真結(jié)果。
在第一種情況，主車輛12和目標(biāo)車輛36在直線道路34(C0＝0及C1＝1)上移動(dòng)，且目標(biāo)車輛36在相同的車道上朝著主車車輛12移動(dòng)。圖8a-d顯示了無約束和有約束濾波方案的目標(biāo)狀態(tài)估計(jì)和道路曲線估計(jì)結(jié)果，圖9a-b顯示了有約束和無約束濾波方案的平均目標(biāo)車輛36的橫向位置、速率和加速度RMS誤差。有約束濾波的估計(jì)誤差充分減小。在48次雷達(dá)掃描前，當(dāng)目標(biāo)車輛36距離主車輛12大于65m時(shí)，有約束濾波使得目標(biāo)橫向速率估計(jì)的誤差減少量大于40％，橫向加速度估計(jì)的誤差減少量超過60％。當(dāng)目標(biāo)車輛36距離主車輛12小于65米時(shí)，這個(gè)條件與碰撞預(yù)測更為相關(guān)，通過有約束濾波，大于50％的橫向位置估計(jì)誤差和大于90％的橫向速率和加速度估計(jì)誤差減少了。
在第二種情況，主車輛12和目標(biāo)車輛36在彎曲道路34(C0＝-10-5及C1＝-3×10-5)上移動(dòng)，且目標(biāo)車輛36在同一車道上朝著主車輛12移動(dòng)。圖10a-d顯示了無約束和有約束濾波方案的目標(biāo)狀態(tài)估計(jì)和曲率估計(jì)結(jié)果，圖11a-b顯示了有約束和無約束濾波方案的平均目標(biāo)車輛36橫向位置、速率和加速度RMS誤差。當(dāng)目標(biāo)車輛36距離主車輛12小于65米時(shí)，有約束濾波的估計(jì)誤差在大約48次雷達(dá)掃描后充分地減小了。當(dāng)目標(biāo)車輛36距離主車輛12大約100m時(shí)，在20次雷達(dá)掃描前，有約束和無約束濾波的估計(jì)誤差相同。目標(biāo)車輛36所在位置與主車輛12的距離在100m和65m之間時(shí)，有約束濾波導(dǎo)致橫向速率和加速度估計(jì)的誤差減少了大約30％，且當(dāng)目標(biāo)車輛36距離主車輛12小于65m時(shí)，通過有約束濾波使得橫向位置估計(jì)誤差減少量大于50％，橫向速率和加速度估計(jì)誤差減少量大于90％。當(dāng)目標(biāo)車輛36很遠(yuǎn)時(shí)，有約束濾波的改善有限由道路曲率參數(shù)的估計(jì)誤差致使，它使得約束誤差與主車輛12和目標(biāo)車輛36之間的距離成比例。這在彎曲道路34的情況更為明顯，其中曲率估計(jì)誤差變得更大，且使得遠(yuǎn)距離目標(biāo)車輛36的車道位置更不明確。
在第三種情況，主車輛12和目標(biāo)車輛36在直線道路34(C0＝0及C1＝0)上移動(dòng)，目標(biāo)車輛36最初在左邊鄰近車道上逼近。在t＝2.2s(55個(gè)雷達(dá)掃描)，目標(biāo)車輛36開始脫離它的車道，轉(zhuǎn)向主車道38，這導(dǎo)致在t＝4s(100次雷達(dá)掃描)時(shí)發(fā)生碰撞。圖12a-d顯示了有約束和無約束濾波方案的目標(biāo)狀態(tài)估計(jì)結(jié)果及橫向位置和速率RMS誤差。約束條件有效性假設(shè)檢驗(yàn)的誤差容限水平(方程(69))，對(duì)于主車道38選擇α1，對(duì)于所有鄰近車道40選擇α＝0.5。盡管在目標(biāo)車輛36轉(zhuǎn)向前，未驗(yàn)證的有約束濾波充分降低了估計(jì)誤差，但是在目標(biāo)車輛36開始遠(yuǎn)離它的車道(左邊鄰近車道)后，相關(guān)的目標(biāo)狀態(tài)估計(jì)結(jié)果是不正確的，它的RMS誤差要比無約束濾波的誤差更大，意味著沒有即時(shí)移走目標(biāo)車輛36開始脫離它的車道后變成無效的道路約束。另一方面，經(jīng)驗(yàn)證的有約束濾波的性能非常接近于無約束濾波的性能，在目標(biāo)36轉(zhuǎn)向前略微減小了估計(jì)誤差，且具有的目標(biāo)狀態(tài)估計(jì)結(jié)果和RMS誤差與在目標(biāo)車輛36開始從它的車道轉(zhuǎn)向后的無約束濾波相同，意味著在目標(biāo)車輛36開始脫離它的車道后，道路約束被即時(shí)移走了。
第四種情況類似于第三種情況，唯一的差別是車輛在彎曲道路34(C0＝-10-5及C1＝-3×10-5)而不是直線道路。目標(biāo)車輛36在t＝2.2s時(shí)開始脫離車道，在t＝4s時(shí)發(fā)生碰撞。圖13a-d顯示了無約束和有約束濾波方案的目標(biāo)狀態(tài)估計(jì)結(jié)果及橫向位置和速率RMS誤差。誤差容限水平與第三種情況相同，且結(jié)果和觀察數(shù)據(jù)與第三種情況相同。在目標(biāo)36開始脫離車道后，通過所提出的約束條件驗(yàn)證，道路約束被即時(shí)移走。通常，假設(shè)目標(biāo)車輛36橫向運(yùn)動(dòng)的估計(jì)精度通常受到很差的雷達(dá)角度分辨率的限制，有約束濾波在目標(biāo)車輛36的橫向運(yùn)動(dòng)的估計(jì)精度上的整體改善很大。
因此，在直線和彎曲道路34上的道路車輛跟蹤的仿真結(jié)果表明當(dāng)目標(biāo)車輛36在主車道38上時(shí)，預(yù)測碰撞傳感系統(tǒng)10可充分減小目標(biāo)車輛36橫向運(yùn)動(dòng)的估計(jì)誤差。當(dāng)目標(biāo)車輛36從鄰近車道轉(zhuǎn)到主車道38上時(shí)，預(yù)測碰撞傳感系統(tǒng)10即時(shí)探測到這個(gè)動(dòng)作，并移走道路約束，以避免另外的錯(cuò)誤約束結(jié)果。由于很差的雷達(dá)角度分辨率通常會(huì)引起很差的橫向運(yùn)動(dòng)估計(jì)，預(yù)測碰撞傳感系統(tǒng)10提供了對(duì)目標(biāo)車輛36橫向運(yùn)動(dòng)的估計(jì)精度的極大改善，這有利于早期可靠的道路車輛碰撞預(yù)測。
雖然特定的實(shí)施例在上述詳細(xì)說明中進(jìn)行了詳細(xì)解釋并在附圖中顯示，本領(lǐng)域的技術(shù)人員應(yīng)當(dāng)明白，各種細(xì)節(jié)上的修正及替換都可根據(jù)本發(fā)明的總體教導(dǎo)發(fā)展得到。因此，所公開的特定布置僅作為例證說明且不作為對(duì)本發(fā)明的范圍的限制，本發(fā)明的范圍在所附權(quán)利要求及其等同物的整個(gè)范圍中給出。
附錄—卡爾曼濾波器的說明卡爾曼濾波器用來從一組有噪測量值中估計(jì)受噪聲影響的動(dòng)態(tài)系統(tǒng)的狀態(tài)和相關(guān)測量值。
系統(tǒng)動(dòng)力學(xué)關(guān)系由下式定義xk+1＝Fk·xk+wk，wk～N(0，Qk) (A-1)其中xk為系統(tǒng)狀態(tài)向量，F(xiàn)k為系統(tǒng)矩陣而wk為對(duì)應(yīng)每一個(gè)狀態(tài)變量的噪聲變量的相關(guān)向量，每一個(gè)噪聲變量的平均值都為0，方差由相關(guān)方差向量Qk的對(duì)應(yīng)元素給定。相關(guān)系統(tǒng)測量值的動(dòng)力學(xué)關(guān)系由下式給定zk＝Hk·xk+vk，vk～N(0，Rk) (A-2)其中zk為系統(tǒng)測量值向量，Hk為測量值矩陣而vk為對(duì)應(yīng)每一個(gè)測量變量的噪聲變量的相關(guān)向量，每一個(gè)噪聲變量的均值為0，方差由相關(guān)方差向量Rk的對(duì)應(yīng)元素給定。相關(guān)協(xié)方差矩陣Rk的元素值可根據(jù)對(duì)相關(guān)典型的工作條件組的相關(guān)系統(tǒng)的典型測量值的分析來決定先驗(yàn)值。相關(guān)協(xié)方差矩陣Qk的元素值解釋了模型誤差。通常，相關(guān)矩陣Fk、Qk、Hk、Rk可隨時(shí)間改變。
假設(shè)在k時(shí)刻的測量值zk及狀態(tài)xk-1|k-1的初始值和k-1時(shí)刻的相關(guān)協(xié)方差Pk-1|k-1，卡爾曼濾波器用來估計(jì)k時(shí)刻的相關(guān)狀態(tài)xk|k及相關(guān)協(xié)方差Pk|k。
濾波過程的第一步是根據(jù)k-1時(shí)刻的估值來計(jì)算k時(shí)刻的狀態(tài)xk|k-1的估值和相關(guān)協(xié)方差Pk|k-1，如下xk|k-1＝Fk·xk-1|k-1(A-3)Pk|k-1=Fk·Pk-1|k-1·FkT+Qk---(A-4)]]>下一步是預(yù)計(jì)k時(shí)刻的測量值 和相關(guān)協(xié)方差矩陣Sk，如下z‾^k=Hk·x‾k|k-1---(A-5)]]>Sk=cov(z^‾k)=Hk·Pk|k-1·HkT+Rk---(A-6)]]>下一步是計(jì)算用于更新狀態(tài)向量xk|k和相關(guān)協(xié)方差矩陣Pk|k的增益矩陣Gk，如下Gk=Pk|k-1·HkT·Sk-1---(A-7)]]>最后，估計(jì)k時(shí)刻的狀態(tài)向量xk|k和相關(guān)協(xié)方差矩陣Pk|k，它對(duì)應(yīng)于相關(guān)測量值zk，如下x‾k|k=x‾k|k-1+Gk·(z‾k-z^‾k)---(A-8)]]>Pk|k=Pk|k-1-Gk·Sk·GkT---(A-9)]]>
權(quán)利要求
1.一種估計(jì)道路曲率的方法，包括a.通過測量道路上行進(jìn)的主車輛的縱向速度來產(chǎn)生速度測量值；b.通過測量或確定所述主車輛的橫擺率來產(chǎn)生橫擺率測量值；及c.估計(jì)至少一個(gè)曲率參數(shù)，其中所述的至少一個(gè)曲率參數(shù)表示道路曲率，估計(jì)至少一個(gè)曲率參數(shù)的操作對(duì)應(yīng)于所述的速度測量值和所述的橫擺率測量值。
2.如權(quán)利要求1所述的估計(jì)道路曲率的方法，其中，所述的至少一個(gè)曲率參數(shù)包含曲率的回旋曲線模型的至少一個(gè)參數(shù)，所述曲率的回旋曲線模型以所述主車輛的坐標(biāo)系統(tǒng)為參考。
3.如權(quán)利要求2所述的估計(jì)道路曲率的方法，其中，所述的至少一個(gè)曲率參數(shù)包含回旋曲線模型的第一和第二參數(shù)，其中所述的第一參數(shù)是常數(shù)，且所述的第二參數(shù)是對(duì)沿著道路的距離的曲率的靈敏度。
4.如權(quán)利要求1所述的估計(jì)道路曲率的方法，其中，估計(jì)至少一個(gè)曲率參數(shù)的操作包括用至少一個(gè)卡爾曼濾波器來處理所述的速度和角速度的測量值。
5.如權(quán)利要求1所述的估計(jì)道路曲率的方法，其中，所述的至少一個(gè)卡爾曼濾波器包括第一和第二卡爾曼濾波器，所述第一卡爾曼濾波器適于根據(jù)所述速度測量值和所述橫擺率測量值來估計(jì)第一組狀態(tài)變量，而第二卡爾曼濾波器適于根據(jù)所述第一組狀態(tài)變量的所述估值來估計(jì)所述至少一個(gè)曲率參數(shù)。
6.如權(quán)利要求5所述的估計(jì)道路曲率的方法，其中，所述的第一組狀態(tài)變量包括車輛速率、車輛加速度、車輛橫擺率和車輛橫擺加速度。
7.一種估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，包括a.產(chǎn)生道路曲率的估值；b.估計(jì)目標(biāo)車輛的無約束狀態(tài)及其相關(guān)的協(xié)方差；c.建立目標(biāo)車輛的至少一個(gè)預(yù)期約束條件，其中至少一個(gè)預(yù)期約束條件對(duì)應(yīng)于所述的道路曲率的估值；d.估計(jì)目標(biāo)車輛的至少一個(gè)有約束狀態(tài)及其相關(guān)的協(xié)方差，它對(duì)應(yīng)于目標(biāo)車輛的所述至少一個(gè)預(yù)期約束條件；e.確定目標(biāo)車輛的最可能狀態(tài)，其中所述的目標(biāo)車輛的最可能狀態(tài)從所述的目標(biāo)車輛的無約束狀態(tài)和目標(biāo)車輛的所述至少一個(gè)有約束狀態(tài)中選擇；及f.如果目標(biāo)車輛的所述至少一個(gè)有約束狀態(tài)是最可能狀態(tài)，則將目標(biāo)車輛的無約束狀態(tài)及其協(xié)方差和所述最可能狀態(tài)的相關(guān)有約束狀態(tài)及協(xié)方差合并，并輸出目標(biāo)的至少一個(gè)合并狀態(tài)及其相關(guān)的合并協(xié)方差，作為目標(biāo)的估計(jì)狀態(tài)或協(xié)方差；否則輸出目標(biāo)的至少一個(gè)無約束狀態(tài)及其相關(guān)的無約束協(xié)方差，作為目標(biāo)的估計(jì)狀態(tài)或協(xié)方差。
8.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，估計(jì)道路曲率的操作包括a.通過測量在道路上行進(jìn)的主車輛的縱向速度，產(chǎn)生速度測量值；b.通過測量或確定所述主車輛的橫擺率，產(chǎn)生橫擺率測量值；及c.估計(jì)至少一個(gè)曲率參數(shù)，其中，所述的至少一個(gè)曲率參數(shù)表示道路的曲率，且估計(jì)至少一個(gè)曲率參數(shù)的操作對(duì)應(yīng)于所述速度測量值和對(duì)應(yīng)于所述的橫擺率測量值。
9.如權(quán)利要求8所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述至少一個(gè)曲率參數(shù)包括曲率的回旋曲線模型的至少一個(gè)參數(shù)，且所述的曲率的回旋曲線模型是以所述主車輛的坐標(biāo)系統(tǒng)為參考的。
10.如權(quán)利要求9所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述的至少一個(gè)曲率參數(shù)包括回旋曲線模型的第一和第二參數(shù)，其中，所述第一參數(shù)為常數(shù)，且所述第二參數(shù)是沿著道路距離曲率的靈敏度。
11.如權(quán)利要求8所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，估計(jì)至少一個(gè)曲率參數(shù)的操作包括用至少一個(gè)卡爾曼濾波器來處理所述的速度和橫擺率的測量值。
12.如權(quán)利要求11所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述的至少一個(gè)卡爾曼濾波器包括第一和第二卡爾曼濾波器，所述第一卡爾曼濾波器適于根據(jù)所述速度測量值和所述橫擺率測量值來估計(jì)第一組狀態(tài)變量，且所述第二卡爾曼濾波器適于根據(jù)所述第一組狀態(tài)變量的所述估值來估計(jì)所述的至少一個(gè)曲率參數(shù)。
13.如權(quán)利要求12所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述第一組狀態(tài)變量包括車輛速率、車輛加速度、車輛橫擺率及車輛橫擺加速度。
14.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，估計(jì)目標(biāo)車輛的無約束狀態(tài)和相關(guān)協(xié)方差的操作包括a.測量目標(biāo)車輛相對(duì)于主車輛的范圍、范圍速率和方位角；及b.根據(jù)目標(biāo)車輛相對(duì)于主車輛的范圍、范圍速率和方位角的所述測量值來估計(jì)目標(biāo)車輛的所述無約束狀態(tài)。
15.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，還包括將目標(biāo)車輛的所述無約束狀態(tài)轉(zhuǎn)換位對(duì)應(yīng)于道路曲率的所述估值的坐標(biāo)系統(tǒng)的操作。
16.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，目標(biāo)車輛的所述至少一個(gè)預(yù)期約束條件應(yīng)用于目標(biāo)車輛的橫向位置，且目標(biāo)車輛的縱向位置是無約束的。
17.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，目標(biāo)車輛的所述至少一個(gè)預(yù)期約束條件包括多個(gè)預(yù)期約束條件，且至少兩個(gè)不同的預(yù)期約束條件對(duì)應(yīng)于道路的不同車道。
18.如權(quán)利要求7所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，確定目標(biāo)車輛的最可能狀態(tài)的操作包括a.確定至少一個(gè)似然值，其中所述的至少一個(gè)似然值對(duì)應(yīng)于目標(biāo)車輛的所述的至少一個(gè)有效預(yù)期約束條件，且所述至少一個(gè)似然值對(duì)應(yīng)于相應(yīng)的至少一個(gè)分布函數(shù)，該分布函數(shù)以對(duì)應(yīng)于目標(biāo)車輛的所述無約束狀態(tài)的值來估計(jì)。b.確定目標(biāo)車輛的第一有約束狀態(tài)的概率，其中，所述概率對(duì)應(yīng)于所述的至少一個(gè)似然值；c.檢驗(yàn)至少一個(gè)假設(shè)，即目標(biāo)車輛的所述無約束狀態(tài)對(duì)應(yīng)于所述第一有約束狀態(tài)，及d.響應(yīng)于檢驗(yàn)所述至少一個(gè)假設(shè)的操作，識(shí)別目標(biāo)車輛的最可能狀態(tài)。
19.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，確定目標(biāo)車輛的最可能狀態(tài)的操作包括建立從目標(biāo)車輛的第一狀態(tài)轉(zhuǎn)換到第二狀態(tài)的至少一個(gè)先驗(yàn)概率，其中，所述的第一和第二狀態(tài)可以是相同的狀態(tài)或者不同的狀態(tài)，且目標(biāo)車輛的所述第一有約束狀態(tài)的所述概率對(duì)應(yīng)于所述的至少一個(gè)先驗(yàn)概率。
20.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述至少一個(gè)分布函數(shù)對(duì)應(yīng)于目標(biāo)車輛的特定有約束狀態(tài)的所述協(xié)方差。
21.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述至少一個(gè)有約束狀態(tài)包括多個(gè)有約束狀態(tài)，所述的第一有約束狀態(tài)包括所述多個(gè)有約束狀態(tài)的組合。
22.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述第一有約束狀態(tài)包括目標(biāo)車輛的所述至少一個(gè)有約束狀態(tài)的最可能狀態(tài)。
23.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述第一有約束狀態(tài)包括目標(biāo)車輛的所述至少一個(gè)有約束狀態(tài)。
24.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，檢驗(yàn)至少一個(gè)假設(shè)的操作對(duì)應(yīng)于目標(biāo)車輛的所述狀態(tài)的最近估值。
25.如權(quán)利要求18所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，檢驗(yàn)至少一個(gè)假設(shè)的操作包括識(shí)別與目標(biāo)車輛的所述至少一個(gè)有約束狀態(tài)的最可能狀態(tài)對(duì)應(yīng)的目標(biāo)車輛的特定有約束狀態(tài)，計(jì)算與所述無約束狀態(tài)和所述特定有約束狀態(tài)間差值對(duì)應(yīng)的函數(shù)的值，并將所述函數(shù)的所述值與一門限比較，由此，如果所述函數(shù)的所述值小于所述門限，則滿足所述假設(shè)。
26.如權(quán)利要求25所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，所述門限對(duì)應(yīng)于與目標(biāo)車輛的所述特定有約束狀態(tài)有關(guān)的特定約束條件。
27.如權(quán)利要求26所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，對(duì)于與目標(biāo)車輛對(duì)主車輛的威脅增大對(duì)應(yīng)的所述特定有約束狀態(tài)，所述閾值相對(duì)較低。
28.如權(quán)利要求27所述的估計(jì)道路上的目標(biāo)車輛的狀態(tài)的方法，其中，與目標(biāo)車輛對(duì)主車輛的威脅增大對(duì)應(yīng)的所述特定有約束狀態(tài)選自與主車輛在同一車道上移動(dòng)的目標(biāo)車輛及改變到與主車輛的所述車道相同的目標(biāo)車輛。
29.一種用于估計(jì)道路曲率的系統(tǒng)，包括a.速度傳感器，適于測量道路上車輛的縱向速度；b.橫擺率傳感器，適于測量所述車輛的橫擺率；及c.與所述速度傳感器和所述橫擺率傳感器連接的處理器，其中，所述處理器適于估計(jì)對(duì)應(yīng)于來自所述速度傳感器和所述橫擺率傳感器的測量值的道路曲率。
30.如權(quán)利要求29所述的用于估計(jì)道路曲率的系統(tǒng)，其中，所述處理器包括至少一個(gè)卡爾曼濾波器，且響應(yīng)于對(duì)縱向速度的所述測量和對(duì)橫擺率的所述測量，所述至少一個(gè)卡爾曼濾波器適于產(chǎn)生至少一個(gè)曲率參數(shù)的估值，其中，所述至少一個(gè)曲率參數(shù)表示道路曲率。
31.如權(quán)利要求30所述的用于估計(jì)道路曲率的系統(tǒng)，其中，所述至少一個(gè)卡爾曼濾波器包括第一和第二卡爾曼濾波器，響應(yīng)于對(duì)縱向速度的所述所述和對(duì)橫擺率的所述測量，所述第一個(gè)卡爾曼濾波器適于產(chǎn)生一個(gè)輸出，該輸出包括橫擺率、橫擺加速度、縱向速度和縱向加速度的估值；且響應(yīng)于來自所述第一個(gè)卡爾曼濾波器的所述輸出，所述第二個(gè)卡爾曼濾波器適于產(chǎn)生所述至少一個(gè)曲率參數(shù)的所述估值。
32.一種用于估計(jì)道路上的目標(biāo)車輛的狀態(tài)的系統(tǒng)，包括a.道路曲率估計(jì)子系統(tǒng)，用于估計(jì)在主車輛行進(jìn)道路上的道路曲率；b.目標(biāo)狀態(tài)估計(jì)子系統(tǒng)，可操作地與所述主車輛連接，其中，所述目標(biāo)狀態(tài)子系統(tǒng)適于跟蹤道路上的目標(biāo)車輛；及c.至少一個(gè)處理器，可操作地與所述道路曲率估計(jì)子系統(tǒng)和所述目標(biāo)狀態(tài)估計(jì)子系統(tǒng)連接或者其一部分，其中，響應(yīng)于所述道路曲率估計(jì)子系統(tǒng)所估計(jì)的所述曲率和響應(yīng)于來自所述目標(biāo)狀態(tài)估計(jì)子系統(tǒng)的目標(biāo)運(yùn)動(dòng)的測量，所述處理器適于確定目標(biāo)車輛是否在道路的特定車道上行進(jìn)。
33.如權(quán)利要求32所述的用于估計(jì)道路上的目標(biāo)車輛的狀態(tài)的系統(tǒng)，其中，用于估計(jì)道路曲率的所述道路曲率估計(jì)子系統(tǒng)包括a.速度傳感器，適于測量道路上的所述主車輛的縱向速度；及b.橫擺率傳感器，適于測量道路上的所述主車輛的橫擺率，其中，所述至少一個(gè)處理器可操作地與所述速度傳感器和所述橫擺率傳感器連接，且響應(yīng)于來自所述速度傳感器和所述橫擺率傳感器的測量值，所述處理器適于估計(jì)道路曲率。
34.如權(quán)利要求32所述的用于估計(jì)道路上的估計(jì)目標(biāo)車輛的狀態(tài)的系統(tǒng)，其中，所述目標(biāo)狀態(tài)估計(jì)子系統(tǒng)包括可操作地與所述主車輛連接的雷達(dá)系統(tǒng)。
35.如權(quán)利要求32所述的用于估計(jì)道路上的目標(biāo)車輛的狀態(tài)的系統(tǒng)，其中，所述處理器包括擴(kuò)展卡爾曼濾波器，響應(yīng)于所述目標(biāo)狀態(tài)估計(jì)子系統(tǒng)的范圍、范圍速率和方位角的測量，擴(kuò)展卡爾曼濾波器適于估計(jì)目標(biāo)運(yùn)動(dòng)的測量。
36.如權(quán)利要求35所述的用于估計(jì)道路上的目標(biāo)車輛的狀態(tài)的系統(tǒng)，其中，目標(biāo)運(yùn)動(dòng)的所述測量包括相對(duì)于主車輛的坐標(biāo)系統(tǒng)的目標(biāo)位置的測量。
37.如權(quán)利要求36所述的用于估計(jì)道路上的目標(biāo)車輛的狀態(tài)的系統(tǒng)，其中，目標(biāo)運(yùn)動(dòng)的所述測量還包括相對(duì)于主車輛的所述坐標(biāo)系統(tǒng)的目標(biāo)速度的測量。
全文摘要
第一卡爾曼濾波器(52)根據(jù)主車輛(12)上的各個(gè)傳感器(16，18)產(chǎn)生的相關(guān)有噪測量來估計(jì)橫擺率(ω)和車輛速度(U)的實(shí)際測量，且第二卡爾曼濾波器(54)估計(jì)道路曲率的回旋曲線的參數(shù)(C
文檔編號(hào)B60K31/00GK1668938SQ03816967
公開日2005年9月14日 申請(qǐng)日期2003年7月15日 優(yōu)先權(quán)日2002年7月15日
發(fā)明者從山, 沈?qū)? 洪瑯 申請(qǐng)人:汽車系統(tǒng)實(shí)驗(yàn)室公司