推移信息(u，v)以及視差信息d作為輸入值，執(zhí)行具有規(guī)定的過程 模型以及觀測(cè)模型的卡爾曼濾波(Kalman Fi 1 ter)，來計(jì)算對(duì)象物的位置以及速度的推斷 值。其中，在本實(shí)施方式中，計(jì)算部32以第一圖像為基準(zhǔn)來計(jì)算推斷值。
[0041] 這里，為了抑制計(jì)算量，計(jì)算部32被設(shè)定為不對(duì)第一圖像的全部像素計(jì)算推斷值， 而對(duì)例如第一圖像的除去周邊部之外的規(guī)定范圍的像素計(jì)算推斷值。
[0042] 圖2是對(duì)象物推斷裝置10推斷對(duì)象物的狀態(tài)時(shí)的概念圖。坐標(biāo)系以第一照相機(jī)16 的光軸中心為原點(diǎn)設(shè)定為如圖2那樣，計(jì)算部32推斷對(duì)象物的位置(X，Y，Z)以及速度(Vx，V Y， Vz)。如式1所示，計(jì)算部32中設(shè)定有將對(duì)象物的位置以及速度以勻速直線運(yùn)動(dòng)模型化的過 程模型。
[0047]這里，v平均為零，遵照協(xié)方差矩陣Q的多元正態(tài)分布。
[0048] [式3]
[0049] v ~N(0，Q)
[0050] 其中，車速是車速傳感器22中的檢測(cè)值，橫擺率是橫擺率傳感器20中的檢測(cè)值。
[0051] 另外，如式4所示，計(jì)算部32中設(shè)定有規(guī)定視差信息d以及推移信息(u，v)與對(duì)象物 的位置(X，Y，Z)以及速度(Vx，Vy，Vz)的關(guān)系的觀測(cè)模型。
[0054]這里，Η是3D-2D射影變換矩陣。若將第一圖像的橫向的焦距設(shè)為fx[pix]、將縱向 的焦距設(shè)定為fy[pix]、將第一照相機(jī)16以及第二照相機(jī)18的基線長(zhǎng)度(光軸間距離)設(shè)定 為B，則Η被如式5那樣表不。
[0055][式 5]
[0057]將其代入式4的式子。
[0060]由于式6的式子是非線性函數(shù)，所以不能直接應(yīng)用卡爾曼濾波。鑒于此，為了對(duì)式6 的式子進(jìn)行線性近似，如式7的式子所示，在時(shí)刻t(t幀）的對(duì)象物的位置的預(yù)測(cè)值(根據(jù)過 程模型計(jì)算)的周圍進(jìn)行泰勒展開并采用到1次項(xiàng)為止。
[0063]若對(duì)式7的式子進(jìn)行整理，則成為
[0066]這里，w平均為零，遵照協(xié)方差矩陣R的多元正態(tài)分布。
[0067] [式9]
[0068] w ~N(0，R)
[0069] 計(jì)算部32基于上述的過程模型以及觀測(cè)模型，執(zhí)行卡爾曼濾波。即，如以下所示， 計(jì)算部32以第一圖像為基準(zhǔn)按每個(gè)幀反復(fù)進(jìn)行基于過程模型的預(yù)測(cè)(式12~式14)以及基 于觀測(cè)模型的更新(式15~式20)。由此，計(jì)算部32推斷對(duì)象物的狀態(tài)(位置/速度）。將卡爾 曼濾波的在t幀中的推斷值以及該推斷值的誤差的協(xié)方差矩陣分別表示為式10以及式11。
[0070] [式 10]
[0071] χ1^[?Γι f, Z, Vxt Vy)
[0072][式 11]
[0074] 式11的xt表示對(duì)象物的狀態(tài)(位置/速度)的真值。
[0075] iwm
[0076] [式 12]
[0082] [更新]
[0083] [式 15]
[0087][式 17]
[0095] 為了執(zhí)行上述預(yù)測(cè)以及更新，計(jì)算部32中預(yù)先設(shè)定有關(guān)于推斷值的初始值(式21) 以及推斷值的誤差的協(xié)方差矩陣的初始值(式22)。
[0096] [式 21]
[0097] 推斷值的初始值：心
[0098] [式 22]
[0099] 推斷值的誤差的協(xié)方差矩陣的初始值：&
[0100] 這些初始值能夠根據(jù)執(zhí)行卡爾曼濾波的模型、條件，來設(shè)定適當(dāng)?shù)某Ａ浚虿捎猛?過模擬、實(shí)驗(yàn)等而求出的值。
[0101]判定部34判定由推移信息計(jì)算部28計(jì)算出的推移信息(u，v)以及由視差信息計(jì)算 部30計(jì)算出的視差信息d是否是異常值。以下，對(duì)于產(chǎn)生推移信息(u，v)以及視差信息d的異 常值的情況，舉出具體例來進(jìn)行說明。
[0102]設(shè)想如圖3(a)所示，將在搭載有對(duì)象物推斷裝置10的汽車的行駛方向的前方進(jìn)行 行駛的汽車（以下，稱為其他車輛38)作為對(duì)象物，通過對(duì)象物推斷裝置10進(jìn)行狀態(tài)推斷的 情況。圖3 (b)表示了視差信息計(jì)算部30計(jì)算出的t-Ι幀、t幀、t+Ι幀中的視差信息d。
[0103] 圖3(b)的對(duì)各幀標(biāo)注的圓形的包圍線表示被正常跟蹤（即，恰當(dāng)?shù)赜?jì)算推移信息） 的像素(追蹤點(diǎn)）。由圖3(b)可知，不管包圍線內(nèi)的像素是否是對(duì)象物的同一部位，從t-Ι幀 到t幀，該像素中的視差信息d都發(fā)生變化。另外，可知在t+Ι幀中，視差信息d正被修正。即， 在t幀中視差信息計(jì)算部30計(jì)算出的包圍線內(nèi)的像素的視差信息d成為偏離了真值的異常 值(偏離值)。
[0104] 這樣的異常值因第一圖像以及第二圖像所包含的噪聲的影響、伴隨以有限的像素 來劃分各圖像而產(chǎn)生的誤差等而引起。另一方面，在t+Ι幀視差信息d正被修正是因視差信 息計(jì)算部30按每個(gè)幀獨(dú)立地計(jì)算視差信息d而引起的。即，這是因?yàn)橛捎谝暡钚畔⒂?jì)算部30 不依賴于過去計(jì)算出的視差信息d地計(jì)算視差信息d，所以即便一旦在視差信息d中產(chǎn)生了 異常值，也能夠在t+Ι幀中計(jì)算出恰當(dāng)?shù)囊暡钚畔。
[0105] 圖3(c)表示推移信息計(jì)算部28計(jì)算出的V -1幀、V幀、V +1幀中的推移信息(u， V)。在V幀中的標(biāo)注有包圍線的部位，追蹤點(diǎn)從其他車輛38移至其他車輛38以外的風(fēng)景(側(cè) 壁）、或此外的車輛40。即，推移信息計(jì)算部28對(duì)V幀的標(biāo)注有包圍線的像素計(jì)算出的推移 信息(u，v)成為異常值(偏離值）。這樣的推移信息(u，v)的異常值主要因模式匹配的錯(cuò)誤而 產(chǎn)生。
[0106] 這里，在V+1幀中，從其他車輛38偏離的追蹤點(diǎn)保持被跟蹤。即，若一旦在推移信 息(u，v)中產(chǎn)生異常值，則追蹤點(diǎn)不會(huì)被修正(不會(huì)恢復(fù)到其他車輛38上的追蹤點(diǎn)）。這是因 為遍及在時(shí)間上位于前后的幀間計(jì)算推移信息(u，v)(光流場(chǎng))而依賴于過去計(jì)算出的推移 信息(u，v)。
[0107] 如上所述，存在推移信息計(jì)算部28計(jì)算出的推移信息(u，v)、視差信息計(jì)算部30計(jì) 算出的視差信息d是因錯(cuò)誤引起的異常值的可能性。鑒于此，判定部34基于推移信息(u，d) 以及視差信息d與計(jì)算部32計(jì)算出的預(yù)測(cè)值(數(shù)13)之差，來判定推移信息(u，v)以及視差信 息d是否是異常值。
[0108] 具體而言，判定部34基于推移信息計(jì)算部28計(jì)算出的推移信息(u，v)與根據(jù)計(jì)算 部32計(jì)算出的預(yù)測(cè)值得到并與推移信息(u，v)對(duì)應(yīng)的預(yù)測(cè)推移信息之差，來判定該推移信 息(u，v)是否是異常值。另外，判定部34基于視差信息計(jì)算部30計(jì)算出的視差信息d與根據(jù) 計(jì)算部32計(jì)算出的預(yù)測(cè)值得到并與視差信息d對(duì)應(yīng)的預(yù)測(cè)視差信息之差，來判定該視差信 息d是否是異常值。
[0109] 即，判定部34使用計(jì)算部32更新卡爾曼濾波時(shí)計(jì)算出的觀測(cè)值以及預(yù)測(cè)值的殘差 (式 16)。
[0110][式 23]
[0112] 良? :推移信息與預(yù)測(cè)信息的差
[0113] & :視差信息與預(yù)測(cè)視差信息的差
[0114] 而且，在推移信息與預(yù)測(cè)信息的差以及視差信息與預(yù)測(cè)視差信息的差分別比規(guī)定 的閾值Thu、Thv、Thd大的情況下，判定部34判定為觀測(cè)值(視差信息d、推移信息(u，d))分別 是異常值。
[0115] [式 24]
[0116] > fku
[0117] [式 25]
[0118] gv； > Th%,
[0119] [式 26]
[0120] gd· > Thd
[0121] 即，在滿足式24以及式25的公式的至少一個(gè)的情況下，判定部34判定為推移信息 (u，v)是異常值。另外，在滿足式26的情況下，判定部34判定為視差信息d是異常值。作為該 閾值Th u、Thv、Thd，可采用通過模擬、實(shí)驗(yàn)等而得到的固定值。
[0122] 其中，作為閾值，不必一定是固定值。例如，遠(yuǎn)方的對(duì)象物的視差信息d受拍攝到的 圖像的噪聲等大幅影響，計(jì)算出的視差信息d的誤差也容易增大。結(jié)果，在采用了固定的閾 值的情況下，存在判定部34總是將視差信息d判定為異常值之虞。
[0123] 鑒于此，由于實(shí)施考慮了觀測(cè)值以及預(yù)測(cè)值的偏差的判定，所以也可以代替作為 固定值的閾值而使用作為變量的閾值。
[0124] 即，也可以使用作為式17所示的殘差的協(xié)方差矩陣的St的元素，對(duì)閾值進(jìn)行調(diào)整 (變動(dòng)）。
[0125] [式 27]
[0127]而且，在以下的情況下，判定部34判定為觀測(cè)值是異常值。

[0134] 即，在滿足式28以及式29的公式的至少一個(gè)的情況下，判定部34判定為推移信息 (u，v)是異常值。另外，在滿足式30的情況下，判定部34判定為視差信息d是異常值。
[0135] 修正部36在判定部34判定為觀測(cè)值的任意一個(gè)是異常值的情況下，對(duì)計(jì)算部32計(jì) 算出的推斷值進(jìn)行修正。另一方面，在觀測(cè)值不是異常值的情況下，修正部36不對(duì)由計(jì)算部 32計(jì)算出的推斷值進(jìn)行修正。
[0136] 這里，在判定為推移信息(u，v)是異常值的情況下、和判定為視差信息d是異常值 的情況下，修正部36利用不同的方法對(duì)推斷值進(jìn)行修正。
[0137] (關(guān)于判定為推移信息是異常值的情況）
[0138] 首先，使用圖4對(duì)在判定為推移信息(u，v)是異常值的情況下，修正部36對(duì)推斷值 進(jìn)行修正的方法進(jìn)行說明。其中，圖4所示的圖像用于表示推移信息計(jì)算部28計(jì)算出的推移 信息(u，v)，與在圖3(c)中說明的情況相同。