移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法
【專利摘要】移動(dòng)物體位置姿態(tài)估計(jì)裝置具備攝像部(2)、對比圖像獲取部、似然度設(shè)定部(12)以及移動(dòng)物體位置姿態(tài)估計(jì)部(13)。攝像部(2)對移動(dòng)物體周邊進(jìn)行拍攝來獲取攝像圖像。對比圖像獲取部獲取從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像。似然度設(shè)定部(12)將攝像圖像與對比圖像進(jìn)行比較，在攝像圖像內(nèi)的遠(yuǎn)處位置像素與對比圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下將對比圖像的姿態(tài)角似然度設(shè)定得高，在攝像圖像內(nèi)的近處位置像素與對比圖像內(nèi)的近處位置像素一致的情況下將對比圖像的位置似然度設(shè)定得高。移動(dòng)物體位置姿態(tài)估計(jì)部(13)根據(jù)姿態(tài)角似然度被設(shè)定得高的對比圖像的姿態(tài)角來估計(jì)移動(dòng)物體的姿態(tài)角，根據(jù)位置似然度被設(shè)定得高的對比圖像的位置來估計(jì)移動(dòng)物體的位置。
【專利說明】移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種對移動(dòng)物體的位置和姿態(tài)角進(jìn)行估計(jì)的移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法。

【背景技術(shù)】
[0002]作為將三維地圖與照相機(jī)的攝像圖像進(jìn)行比較來計(jì)算移動(dòng)物體的位置的技術(shù)，例如已知有下述的專利文獻(xiàn)I所記載的技術(shù)。在該專利文獻(xiàn)I中，首先，制作從由車輛所具備的車載照相機(jī)獲得的實(shí)際的影像中抽出邊緣得到的邊緣圖像。制作將以三維的方式記錄周圍環(huán)境的邊緣的位置、形狀得到的三維地圖按照車載照相機(jī)的位置和姿態(tài)投影得到的虛擬圖像。然后，調(diào)整車載照相機(jī)的位置和姿態(tài)角以使邊緣圖像與虛擬圖像一致。由此，估計(jì)出車載照相機(jī)在三維空間中的位置和姿態(tài)角。
[0003]專利文獻(xiàn)1:日本特開2009-199572號公報(bào)


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

[0004]然而，在專利文獻(xiàn)I中，在雖然實(shí)際的影像與虛擬圖像一致但該一致的場所距車載照相機(jī)較遠(yuǎn)的情況下，有可能車載照相機(jī)的位置的誤差大。相反地，在一致的位置距車載照相機(jī)近的情況下，有可能車載照相機(jī)的姿態(tài)角的誤差大。
[0005]因此，本發(fā)明是鑒于上述的實(shí)際情況而提出的，其目的在于，提供一種能夠高精度地估計(jì)移動(dòng)物體的位置和姿態(tài)角的移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法。
[0006]本發(fā)明的第一方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置對移動(dòng)物體的位置和姿態(tài)角進(jìn)行估計(jì)，該移動(dòng)物體位置姿態(tài)估計(jì)裝置具備攝像部、對比圖像獲取部、似然度設(shè)定部以及移動(dòng)物體位置姿態(tài)估計(jì)部。攝像部對移動(dòng)物體周邊進(jìn)行拍攝來獲取攝像圖像。對比圖像獲取部獲取從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像。似然度設(shè)定部將由攝像部獲取到的攝像圖像與由對比圖像獲取部獲取到的對比圖像進(jìn)行比較，在攝像圖像內(nèi)的遠(yuǎn)處位置像素與對比圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將對比圖像的姿態(tài)角似然度設(shè)定得高，在攝像圖像內(nèi)的近處位置像素與對比圖像內(nèi)的近處位置像素一致的情況下，將對比圖像的位置似然度設(shè)定得高。移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)由似然度設(shè)定部將姿態(tài)角似然度設(shè)定得高的對比圖像的姿態(tài)角來估計(jì)移動(dòng)物體的姿態(tài)角，根據(jù)由似然度設(shè)定部將位置似然度設(shè)定得高的對比圖像的位置來估計(jì)移動(dòng)物體的位置。
[0007]本發(fā)明的第二方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)方法是對移動(dòng)物體的位置和姿態(tài)角進(jìn)行估計(jì)的移動(dòng)物體位置姿態(tài)估計(jì)方法。在移動(dòng)物體位置姿態(tài)估計(jì)方法中，將對移動(dòng)物體周邊進(jìn)行拍攝的攝像圖像與從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像進(jìn)行比較。在攝像圖像內(nèi)的遠(yuǎn)處位置像素與對比圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將對比圖像的姿態(tài)角似然度設(shè)定得高。在攝像圖像內(nèi)的近處位置像素與對比圖像內(nèi)的近處位置像素一致的情況下，將對比圖像的位置似然度設(shè)定得高。根據(jù)將姿態(tài)角似然度設(shè)定得高的對比圖像的姿態(tài)角來估計(jì)移動(dòng)物體的姿態(tài)角。根據(jù)將位置似然度設(shè)定得高的對比圖像的位置來估計(jì)移動(dòng)物體的位置。

【專利附圖】

【附圖說明】
[0008]圖1是表示本發(fā)明的第一實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置的結(jié)構(gòu)的框圖。
[0009]圖2的(a)表不由攝像部獲取到的攝像圖像，圖2的(b)表不從圖2的(a)的攝像圖像中抽出邊緣得到的邊緣圖像，圖2的(c)是表示由虛擬圖像獲取部獲取到的虛擬圖像，圖2的(d)是表示圖2的(c)中的虛擬位置向右方向偏移了的情況下的虛擬圖像的俯視圖。
[0010]圖3是表示本發(fā)明的第一實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置的動(dòng)作過程的一例的流程圖。
[0011]圖4是用于說明使粒子(候選點(diǎn))移動(dòng)的動(dòng)作的俯視圖。
[0012]圖5是表示距車輛的距離與位置似然度like_p的關(guān)系的一例的曲線圖。
[0013]圖6是表示距車輛的距離與姿態(tài)角似然度like_a的關(guān)系的一例的曲線圖。
[0014]圖7是表示本發(fā)明的第二實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置的動(dòng)作過程的一例的流程圖。

【具體實(shí)施方式】
[0015]下面，參照【專利附圖】

【附圖說明】本發(fā)明的實(shí)施方式。
[0016](第一實(shí)施方式)
[0017]參照圖1說明本發(fā)明的第一實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置的結(jié)構(gòu)。第一實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置具備E⑶(Engine Control Unit:發(fā)動(dòng)機(jī)控制單元)1、照相機(jī)(攝像部的一例)2、三維地圖數(shù)據(jù)庫3以及車輛傳感器組
4。在車輛傳感器組4中包括GPS接收機(jī)41、油門踏板傳感器42、轉(zhuǎn)向傳感器(steeringsensor) 43、制動(dòng)傳感器(brake sensor) 44、車速傳感器45、加速度傳感器46、車輪速度傳感器47以及偏行率傳感器等其它傳感器48。此外，E⑶I實(shí)際由ROM、RAM、運(yùn)算電路等構(gòu)成。ECU I通過按照保存在ROM中的移動(dòng)物體位置姿態(tài)估計(jì)用的計(jì)算機(jī)程序來執(zhí)行處理，由此作為虛擬圖像獲取部11 (對比圖像獲取部的一例)、似然度設(shè)定部12、移動(dòng)物體位置姿態(tài)估計(jì)部13發(fā)揮功能。
[0018]照相機(jī)2例如使用CXD等固體攝像元件。下面說明移動(dòng)物體是車輛的情況。照相機(jī)2例如以能夠拍攝車輛前方的方向(姿態(tài)角)設(shè)置在車輛的前端部分(位置)處。照相機(jī)2每隔規(guī)定時(shí)間拍攝車輛周邊來獲取攝像圖像。獲取到的攝像圖像被供給到ECU I。
[0019]三維地圖數(shù)據(jù)庫3例如存儲(chǔ)有包含路面顯示的周圍環(huán)境的邊緣等的三維位置信息。在本實(shí)施方式中，在三維地圖數(shù)據(jù)庫3中，作為三維位置信息，除了白線、停止線、人行橫道、路面標(biāo)志等路面顯示以外，還存儲(chǔ)有路邊石、建筑物等構(gòu)造物的邊緣信息。這些三維位置信息被定義為邊緣的集合體。在邊緣為長的直線的情況下，將邊緣例如一米一米地進(jìn)行切割，因此不存在極長的邊緣。在直線的情況下，各邊緣具有表示直線的兩端點(diǎn)的三維位置信息。在曲線的情況下，各邊緣具有表示曲線的兩端點(diǎn)和中央點(diǎn)的三維位置信息。
[0020]車輛傳感器組4連接在E⑶I上。車輛傳感器組4向E⑶I供給由各傳感器41?48檢測出的各種傳感器值。ECU I通過使用車輛傳感器組4的輸出值來分別計(jì)算車輛的大致位置以及表示車輛在單位時(shí)間內(nèi)前進(jìn)的移動(dòng)量的里程。
[0021]E⑶I是使用由照相機(jī)2拍攝到的攝像圖像和三維地圖數(shù)據(jù)庫3中存儲(chǔ)的三維位置信息來進(jìn)行車輛的位置和姿態(tài)角的估計(jì)的電子控制部件。此外，ECU I也可以兼用作用于進(jìn)行其它控制的ECU。
[0022]特別地，移動(dòng)物體位置姿態(tài)估計(jì)裝置將由照相機(jī)2拍攝到的攝像圖像與從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像進(jìn)行比較，來估計(jì)車輛的位置和姿態(tài)角。此外，在實(shí)施方式中，作為“從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像”的一例，使用“將三維地圖數(shù)據(jù)變換為從虛擬位置和虛擬姿態(tài)角拍攝到的圖像所得到的虛擬圖像”。在此，假設(shè)得到如圖2的(a)所示那樣的攝像圖像、得到如圖2的(b)那樣的邊緣圖像。另一方面，假設(shè)將三維位置信息以照相機(jī)2的位置和姿態(tài)角投影得到的虛擬圖像如圖2的(c)那樣。當(dāng)將圖2的(b)的攝像圖像與圖2的(c)的虛擬圖像進(jìn)行比較時(shí)，遠(yuǎn)處位置㈧和近處位置⑶都一致，因此能夠估計(jì)為生成虛擬圖像的虛擬位置和虛擬姿態(tài)角相當(dāng)于本車輛的位置和姿態(tài)角。但是，在虛擬位置向右方向偏移了的情況下，虛擬圖像成為如圖2的(d)所示那樣。在這種情況下，當(dāng)將圖2的(a)的攝像圖像與圖2的⑷的攝像圖像進(jìn)行比較時(shí)，雖然遠(yuǎn)處位置㈧一致，但是近處位置(B)大幅地偏移。相反地，使圖2的(c)的虛擬圖像的虛擬姿態(tài)角偏移(未圖示)，當(dāng)與圖2的(a)的攝像圖像進(jìn)行比較時(shí)，雖然近處位置(B) —致，但是遠(yuǎn)處位置(A)大幅偏移。
[0023]著眼于這樣的現(xiàn)象，移動(dòng)物體位置姿態(tài)估計(jì)裝置在攝像圖像內(nèi)的近處位置像素與虛擬圖像內(nèi)的近處位置像素一致的情況下，判斷為虛擬圖像的虛擬位置可能正確。相反地，移動(dòng)物體位置姿態(tài)估計(jì)裝置在攝像圖像內(nèi)的遠(yuǎn)處位置像素與虛擬圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，判斷為虛擬圖像的虛擬姿態(tài)角可能正確。
[0024]下面，參照圖3所示的位置姿態(tài)估計(jì)算法說明移動(dòng)物體位置姿態(tài)估計(jì)裝置的動(dòng)作。此外，在本實(shí)施方式中，設(shè)為對車輛的3自由度的位置(前后方向、橫方向、上下方向)以及3自由度的姿態(tài)角(傾倒、俯仰、橫擺)進(jìn)行估計(jì)。另外，圖3所示的位置姿態(tài)估計(jì)算法由E⑶I例如以10msec左右的間隔連續(xù)地進(jìn)行。
[0025]首先，在步驟SI中，E⑶I獲取由照相機(jī)2拍攝到的影像，基于該影像中包含的攝像圖像計(jì)算邊緣圖像。本實(shí)施方式中的邊緣是指像素的亮度尖銳地變化的位置。作為邊緣檢測方法，例如能夠使用Canny法。不限于此，邊緣檢測方法除此之外還可以使用微分邊緣檢測等各種方法。
[0026]另外，期望E⑶I從照相機(jī)2的攝像圖像中抽出邊緣的亮度變化的方向、邊緣附近的色彩等。由此，在后述的步驟S5和步驟S6中，也可以還使用在三維地圖數(shù)據(jù)庫3中也已事先記錄的這些邊緣以外的信息設(shè)定位置似然度、姿態(tài)角似然度來估計(jì)車輛的位置和姿態(tài)角。
[0027]在接下來的步驟S2中，ECU I基于從車輛傳感器組4獲得的傳感器值，計(jì)算相對于通過前一周期的位置姿態(tài)估計(jì)算法計(jì)算出的車輛的位置的移動(dòng)量、即里程。此外，在開始位置姿態(tài)估計(jì)算法后最初的周期的情況下，將里程計(jì)算為零。
[0028]ECU I使用從車輛傳感器組4獲得的各種傳感器值，計(jì)算車輛在單位時(shí)間內(nèi)前進(jìn)的移動(dòng)量、即里程。作為該里程計(jì)算方法，例如基于在將車輛運(yùn)動(dòng)限定在平面上之后由各車輪的車輪速度傳感器和轉(zhuǎn)向傳感器檢測出的傳感器值計(jì)算車速和橫擺率，并計(jì)算單位時(shí)間內(nèi)的移動(dòng)量和旋轉(zhuǎn)量即可。ECU I可以用車速、GPS接收機(jī)41的定位值的差分來代替車輪速度，也可以用轉(zhuǎn)輪角來代替偏行率傳感器。此外，里程的計(jì)算方法可以想到各種計(jì)算方法，但是只要能夠計(jì)算里程，就可以使用任意的方法。
[0029]在接下來的步驟S3中，E⑶I (對比圖像獲取部)獲取從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像。具體地說，作為對比圖像獲取部的一例的虛擬圖像獲取部11基于在步驟S2中計(jì)算出的里程來計(jì)算多個(gè)虛擬(預(yù)測)位置和虛擬(預(yù)測)姿態(tài)角的候選。多個(gè)虛擬位置和虛擬姿態(tài)角的候選是本車位置和姿態(tài)角的候選。此時(shí)，虛擬圖像獲取部11從在前一周期的步驟S6中估計(jì)出的車輛位置移動(dòng)與在本次的步驟S2中計(jì)算出的里程相應(yīng)的量。虛擬圖像獲取部11在移動(dòng)后的車輛位置附近計(jì)算多個(gè)虛擬位置和虛擬姿態(tài)角的候選。其中，在開始位置姿態(tài)估計(jì)算法后最初的周期的情況下，ECU I不具有前次的車輛位置信息。因此，虛擬圖像獲取部11將來自車輛傳感器組4中包括的GPS接收機(jī)41的數(shù)據(jù)設(shè)為初始位置信息?；蛘?，虛擬圖像獲取部11也可以事先存儲(chǔ)在前次停車時(shí)最后計(jì)算出的車輛位置和姿態(tài)角，將其作為初始位置和姿態(tài)角信息。
[0030]此時(shí)，虛擬圖像獲取部11將因車輛傳感器組4的測量誤差、通信遲滯產(chǎn)生的里程的誤差、關(guān)于里程所考慮不到的車輛的動(dòng)態(tài)特性考慮在內(nèi)，生成多個(gè)有可能取得車輛的位置、姿態(tài)角的真值的多個(gè)虛擬位置和虛擬姿態(tài)角的候選。關(guān)于該虛擬位置和虛擬姿態(tài)角的候選，針對位置和姿態(tài)角的6自由度的參數(shù)分別設(shè)定誤差的上下限，在該誤差的上下限的范圍內(nèi)使用隨機(jī)數(shù)表等隨機(jī)地設(shè)定。
[0031]此外，在本實(shí)施方式中，制作500個(gè)虛擬位置和虛擬姿態(tài)角的候選。另外，針對位置和姿態(tài)角的6自由度的參數(shù)的誤差的上下限按照前后方向、橫方向、上下方向、傾倒、俯仰、橫擺的順序設(shè)為 ±0.05[m] > +0.05[m] > +0.05[m] > ±0.5[deg] > ±0.5[deg] >±0.5[deg]。關(guān)于制作該虛擬位置和虛擬姿態(tài)角的候選的個(gè)數(shù)、針對位置和姿態(tài)角的6自由度的參數(shù)的誤差的上下限，期望對車輛的駕駛狀態(tài)、路面的狀況進(jìn)行檢測或估計(jì)，從而適當(dāng)?shù)刈兏?。例如，在產(chǎn)生了急轉(zhuǎn)彎、打滑等的情況下，平面方向(前后方向、橫方向、橫擺)的誤差變大的可能性高，因此期望使這三個(gè)參數(shù)的誤差的上下限變大且增加虛擬位置和虛擬姿態(tài)角的候選的制作個(gè)數(shù)。
[0032]在步驟S3中，虛擬圖像獲取部11也可以使用所謂的粒子過濾器來設(shè)定多個(gè)虛擬位置和虛擬姿態(tài)角的候選。此時(shí)，虛擬圖像獲取部11使在前一周期的步驟S6中生成的多個(gè)虛擬位置和虛擬姿態(tài)角的候選、即各粒子(候選點(diǎn))的位置和姿態(tài)角移動(dòng)與里程相應(yīng)的量。具體地說，如圖4所示，使在前一周期估計(jì)出的車輛V(tl)的位置和姿態(tài)角的粒子P以及周圍的粒子Pl?P5移動(dòng)與里程相應(yīng)的量。其結(jié)果，虛擬圖像獲取部11設(shè)定用于估計(jì)新的車輛V(t2)的位置和姿態(tài)角的粒子PlO?15。由此，虛擬圖像獲取部11計(jì)算本次的多個(gè)虛擬位置和虛擬姿態(tài)角的候選。即，將各粒子的位置和姿態(tài)角設(shè)為多個(gè)虛擬位置和虛擬姿態(tài)角的候選。此外，更為優(yōu)選的是，將因車輛傳感器組4的測量誤差、通信遲滯產(chǎn)生的里程的誤差、關(guān)于里程所考慮不到的車輛的動(dòng)態(tài)特性考慮在內(nèi)，使各粒子的位置信息和姿態(tài)角信息移動(dòng)與里程相應(yīng)的量，之后，如上述那樣在針對位置和姿態(tài)角的6自由度的參數(shù)的誤差的上下限的范圍內(nèi)使用隨機(jī)數(shù)表等使誤差隨機(jī)地變化。
[0033]但是，在開始位置姿態(tài)估計(jì)算法后最初的周期的情況下，各粒子不具有位置信息和姿態(tài)角信息。因此，也可以將車輛傳感器組4中包括的GPS接收機(jī)41的檢測數(shù)據(jù)設(shè)為初始位置信息?；蛘?，也可以基于在前次停車時(shí)最后估計(jì)出的車輛位置來設(shè)定各粒子的位置信息和姿態(tài)角信息。在本實(shí)施方式中，在最初的周期的情況下，基于在前次停車時(shí)最后估計(jì)出的車輛位置來針對位置和姿態(tài)角的6自由度的參數(shù)設(shè)定誤差的上下限。然后，在該誤差的上下限的范圍內(nèi)使用隨機(jī)數(shù)表等隨機(jī)地設(shè)定各粒子的位置信息和姿態(tài)角。在本實(shí)施方式中，在最初的周期的情況下制作500個(gè)粒子。另外，針對各粒子的6自由度的參數(shù)的誤差的上下限按照前后方向、橫方向、上下方向、傾倒、俯仰、橫擺的順序設(shè)為±0.05[m]、土 0.05 [m]、±0.05 [m]、土 0.5 [deg]、土 0.5 [deg]、土 0.5 [deg]。
[0034]在接下來的步驟S4中，虛擬圖像獲取部11針對在步驟S3中制作的多個(gè)虛擬位置和虛擬姿態(tài)角的候選分別制作虛擬圖像(投影圖像)。此時(shí)，虛擬圖像獲取部11例如將三維地圖數(shù)據(jù)庫3中存儲(chǔ)的邊緣等的三維位置信息變換成為從虛擬位置和虛擬姿態(tài)角拍攝到的照相機(jī)圖像，來制作虛擬圖像。該虛擬圖像是用于評價(jià)虛擬位置和虛擬姿態(tài)角的各候選是否與本車輛實(shí)際的位置和姿態(tài)角相符的評價(jià)用圖像。在向虛擬圖像的變換處理中，需要表示照相機(jī)2的位置的外部參數(shù)和照相機(jī)2的內(nèi)部參數(shù)。關(guān)于外部參數(shù)，只要通過預(yù)先測量車輛位置(例如中心位置)至照相機(jī)2的相對位置，基于虛擬位置和虛擬姿態(tài)角的候選計(jì)算即可。另外，關(guān)于內(nèi)部參數(shù)，只要預(yù)先進(jìn)行校準(zhǔn)即可。
[0035]此外，更為優(yōu)選的是，在能夠從在步驟SI中由照相機(jī)2拍攝到的攝像圖像中抽出邊緣的亮度變化的方向、邊緣附近的色彩等的情況下，也能夠?qū)⑺鼈兊娜S位置信息事先記錄到三維地圖數(shù)據(jù)庫3中，制作虛擬圖像。
[0036]在步驟S5中，ECU I (似然度設(shè)定部12)針對在步驟S3中設(shè)定的多個(gè)虛擬位置和虛擬姿態(tài)角的候選，分別將在步驟SI中制作出的邊緣圖像與在步驟S4中制作出的虛擬圖像進(jìn)行比較。似然度設(shè)定部12根據(jù)比較的結(jié)果，針對每個(gè)虛擬位置和虛擬姿態(tài)角的候選設(shè)定位置似然度和姿態(tài)角似然度。位置似然度是表示虛擬位置的候選相對于車輛實(shí)際的位置的正確度有多少的指標(biāo)。姿態(tài)角似然度是表示虛擬姿態(tài)角的候選相對于車輛實(shí)際的姿態(tài)角的正確度有多少的指標(biāo)。虛擬圖像與邊緣圖像的一致度越高，似然度設(shè)定部12將位置似然度或姿態(tài)角似然度設(shè)定得越高。此外，稍后記述求位置似然度或姿態(tài)角似然度的方法。
[0037]似然度設(shè)定部12將攝像圖像與虛擬圖像進(jìn)行比較，在攝像圖像內(nèi)的遠(yuǎn)處位置像素與虛擬圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將虛擬圖像的姿態(tài)角似然度設(shè)定得高。似然度設(shè)定部12將攝像圖像與虛擬圖像進(jìn)行比較，在攝像圖像內(nèi)的近處位置像素與虛擬圖像內(nèi)的近處位置像素一致的情況下，將虛擬圖像的位置似然度設(shè)定得高。遠(yuǎn)處位置像素和近處位置像素例如也可以根據(jù)攝像圖像和虛擬圖像的位置來設(shè)定。例如，也可以將相對于攝像圖像和虛擬圖像內(nèi)的縱方向上的中間位置向下規(guī)定幅度的下方向的范圍設(shè)定為遠(yuǎn)處位置像素存在的圖像區(qū)域。另外，也可以將相對于攝像圖像和虛擬圖像內(nèi)的縱方向上的底部位置向上規(guī)定幅度的上方向的范圍設(shè)定為近處位置像素存在的圖像區(qū)域。在求將三維地圖投影于照相機(jī)2中時(shí)的虛擬圖像時(shí)，由于假定了照相機(jī)2的位置，因此能夠求出該距離。
[0038]具體地說，似然度設(shè)定部12判斷虛擬圖像上與邊緣圖像上的邊緣是否一致、即在虛擬圖像上的邊緣所存在的像素坐標(biāo)位置是否存在邊緣圖像上的邊緣。似然度設(shè)定部12在虛擬圖像上與邊緣圖像上的邊緣一致的情況下，關(guān)于該一致的邊緣，參照三維地圖數(shù)據(jù)庫3求出該一致的邊緣在三維空間上的位置。然后，計(jì)算求出了位置似然度的虛擬位置和虛擬姿態(tài)角的候選的位置信息與該一致的邊緣部的距離L (單位:m)，將該距離L的倒數(shù)設(shè)為位置似然度like_p (單位:無)。此外，似然度設(shè)定部12在虛擬圖像上與邊緣圖像上的邊緣不一致的情況下，對位置似然度like_p設(shè)定O。
[0039]似然度設(shè)定部12針對虛擬圖像上的所有像素實(shí)施該處理。似然度設(shè)定部12在距車輛近的部分處邊緣圖像與虛擬圖像一致的情況下，對該像素設(shè)定較大的位置似然度like_p。相反地，E⑶I在距車輛較遠(yuǎn)的部分處邊緣圖像與虛擬圖像一致的情況下，對該像素設(shè)定較小的位置似然度like_p。似然度設(shè)定部12將所有像素的位置似然度like_p的總和設(shè)為關(guān)于虛擬圖像的位置似然度LIKE_P (單位■?無)。
[0040]似然度設(shè)定部12在求位置似然度like_p時(shí)，也可以如圖5所示那樣與距車輛的距離相應(yīng)地設(shè)置上限值。似然度設(shè)定部12在規(guī)定距離Lthp以上的近處邊緣圖像(攝像圖像)的像素與虛擬圖像的像素一致的情況下，將位置似然度like_p設(shè)定為規(guī)定的上限值Ithp0或者，也可以距車輛的距離越近，似然度設(shè)定部12使似然度like_p的增加幅度越少。此外，在本實(shí)施方式中，將距離Lthp設(shè)定為1.0 [m]。
[0041]具體地說，似然度設(shè)定部12預(yù)先準(zhǔn)備記述了距車輛的距離與位置似然度like_p的關(guān)系的位置似然度設(shè)定映射表。ECU I參照位置似然度設(shè)定映射表，根據(jù)邊緣圖像與虛擬圖像一致的距車輛的距離來設(shè)定位置似然度like_p。
[0042]由此，即使在針對距車輛(照相機(jī)2)的距離極近的部分抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小位置估計(jì)誤差。
[0043]E⑶I還求姿態(tài)角似然度。似然度設(shè)定部12在虛擬圖像上與邊緣圖像上的邊緣部一致的情況下，與求出位置似然度LIKE_P時(shí)同樣地，求出一致的像素距車輛的距離L (單位:m)。似然度設(shè)定部12將距車輛的距離L除以10得到的值設(shè)為姿態(tài)角似然度like_a(單位:無)。此外，在虛擬圖像上與邊緣圖像上的邊緣部不一致的情況下，對姿態(tài)角似然度like_a設(shè)定O。
[0044]似然度設(shè)定部12針對虛擬圖像上的所有像素實(shí)施設(shè)定姿態(tài)角似然度like_a的處理。似然度設(shè)定部12在距車輛較遠(yuǎn)的部分處邊緣圖像與虛擬圖像一致的情況下，針對該像素設(shè)定較大的姿態(tài)角似然度like_a。相反地，似然度設(shè)定部12在距車輛較近的部分處邊緣圖像與虛擬圖像一致的情況下，針對該像素設(shè)定較小的姿態(tài)角似然度like_a。似然度設(shè)定部12將所有像素的姿態(tài)角似然度like_a的總和設(shè)為關(guān)于虛擬圖像的姿態(tài)角似然度LIKE_A(單位:無)ο
[0045]似然度設(shè)定部12在求姿態(tài)角似然度like_a時(shí)，也可以如圖6所示那樣與距車輛的距離相應(yīng)地設(shè)置上限值。E⑶I在規(guī)定距離Ltha以上的遠(yuǎn)處邊緣圖像(攝像圖像)的像素與虛擬圖像的像素一致的情況下，將姿態(tài)角似然度like_a設(shè)定為規(guī)定的上限值ltha?；蛘?，也可以距車輛的距離越遠(yuǎn)，ECU I使姿態(tài)角似然度like_a的增加幅度越少。此外，在本實(shí)施方式中，將距離Ltha設(shè)定為30.0 [m]。
[0046]具體地說，似然度設(shè)定部12預(yù)先準(zhǔn)備記述了距車輛的距離與姿態(tài)角似然度like_a的關(guān)系的姿態(tài)角似然度設(shè)定映射表。ECU I參照姿態(tài)角似然度設(shè)定映射表，根據(jù)邊緣圖像與虛擬圖像一致的距車輛的距離來設(shè)定姿態(tài)角似然度like_a。
[0047]由此，即使在針對距車輛(照相機(jī)2)的距離極遠(yuǎn)的部分抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小姿態(tài)角估計(jì)誤差。
[0048]似然度設(shè)定部12關(guān)于各虛擬圖像求出位置似然度LIKE_P和姿態(tài)角似然度LIKE_A。即，關(guān)于虛擬位置和虛擬姿態(tài)角的各候選計(jì)算位置似然度LIKE_P和姿態(tài)角似然度LIKE_A。似然度設(shè)定部12使用關(guān)于多個(gè)虛擬位置和虛擬姿態(tài)角的候選的所有結(jié)果進(jìn)行歸一化使得位置似然度LIKE_P、姿態(tài)角似然度LIKE_A各自的合計(jì)值為I。
[0049]在接下來的步驟S6中，移動(dòng)物體位置姿態(tài)估計(jì)部13使用在步驟S5中求出了位置似然度LIKE_P和姿態(tài)角似然度LIKE_A的多個(gè)虛擬位置和虛擬姿態(tài)角的候選來計(jì)算車輛的最終的位置和姿態(tài)角。移動(dòng)物體位置姿態(tài)估計(jì)部13根據(jù)姿態(tài)角似然度LIKE_A被設(shè)定得高的虛擬圖像的姿態(tài)角來估計(jì)車輛實(shí)際的姿態(tài)角。移動(dòng)物體位置姿態(tài)估計(jì)部13根據(jù)位置似然度LIKE_P被設(shè)定得高的虛擬圖像的位置來估計(jì)車輛實(shí)際的位置。
[0050]此時(shí)，移動(dòng)物體位置姿態(tài)估計(jì)部13例如也可以將位置似然度LIKE_P被設(shè)定得最高的虛擬圖像的虛擬位置和虛擬姿態(tài)角計(jì)算為車輛實(shí)際的位置和姿態(tài)角?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以將姿態(tài)角似然度LIKE_A被設(shè)定得最高的虛擬圖像的虛擬位置和虛擬姿態(tài)角計(jì)算為車輛實(shí)際的位置和姿態(tài)角?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以將位置似然度LIKE_P與姿態(tài)角似然度LIKE_A之和最高的虛擬圖像的虛擬位置和虛擬姿態(tài)角計(jì)算為車輛實(shí)際的位置和姿態(tài)角?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對虛擬位置和虛擬姿態(tài)角的各候選進(jìn)行與各虛擬圖像的位置似然度LIKE_P相應(yīng)的加權(quán)，將加權(quán)后的虛擬位置和虛擬姿態(tài)角的平均值計(jì)算為車輛實(shí)際的位置和姿態(tài)角。或者，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對虛擬位置和虛擬姿態(tài)角的各候選進(jìn)行與各虛擬圖像的姿態(tài)角似然度LIKE_A相應(yīng)的加權(quán)，將加權(quán)后的虛擬位置和虛擬姿態(tài)角的平均值計(jì)算為車輛實(shí)際的位置和姿態(tài)角?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對虛擬位置和虛擬姿態(tài)角的各候選進(jìn)行與各虛擬圖像的位置似然度LIKE_P和姿態(tài)角似然度LIKE_A之和相應(yīng)的加權(quán)，將加權(quán)后的虛擬位置和虛擬姿態(tài)角的平均值計(jì)算為車輛實(shí)際的位置和姿態(tài)角。
[0051]在步驟S3中使用粒子過濾器的情況下，首先，針對各粒子設(shè)定與位置似然度LIKE_P和姿態(tài)角似然度LIKE_A各自相應(yīng)的權(quán)重。然后，移動(dòng)物體位置姿態(tài)估計(jì)部13將位置似然度LIKE_P最高的粒子的虛擬位置設(shè)為車輛實(shí)際的位置?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對多個(gè)粒子的虛擬位置進(jìn)行與位置似然度LIKE_P相應(yīng)的加權(quán)，將加權(quán)后的多個(gè)粒子的虛擬位置的平均值計(jì)算為車輛實(shí)際的位置?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以將姿態(tài)角似然度LIKE_A最高的粒子的虛擬姿態(tài)角計(jì)算為車輛實(shí)際的姿態(tài)角?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對多個(gè)粒子的虛擬姿態(tài)角進(jìn)行與姿態(tài)角似然度LIKE_A相應(yīng)的加權(quán)，將加權(quán)后的多個(gè)粒子的虛擬姿態(tài)角的平均值計(jì)算為車輛實(shí)際的姿態(tài)角。
[0052]并且，虛擬圖像獲取部11根據(jù)位置似然度LIKE_P和姿態(tài)角似然度LIKE_A進(jìn)行各粒子的重采樣。即，根據(jù)多個(gè)虛擬圖像的姿態(tài)角似然度以及多個(gè)虛擬圖像的位置似然度來重新設(shè)定多個(gè)虛擬位置和虛擬姿態(tài)角。
[0053]具體地說，虛擬圖像獲取部11以位置似然度LIKE_P與姿態(tài)角似然度LIKE_A之和最高的粒子為中心進(jìn)行各粒子的重采樣。或者，虛擬圖像獲取部11將各粒子的位置信息與姿態(tài)角信息暫時(shí)分離，關(guān)于只有位置信息的粒子，根據(jù)位置似然度LIKE_P進(jìn)行重采樣，關(guān)于只有姿態(tài)角信息的粒子，根據(jù)姿態(tài)角似然度LIKE_A進(jìn)行重采樣。之后，虛擬圖像獲取部11將只有位置信息的粒子的位置信息和只有姿態(tài)角信息的粒子的姿態(tài)角信息隨機(jī)地組合，也可以重新構(gòu)成新的具有位置信息和姿態(tài)角信息的粒子。
[0054]E⑶I通過重復(fù)進(jìn)行如上所述的步驟SI至步驟S6，能夠依次計(jì)算車輛的位置和姿態(tài)角。
[0055]如以上詳細(xì)說明的那樣，根據(jù)作為本實(shí)施方式示出的移動(dòng)物體位置姿態(tài)估計(jì)裝置，將攝像圖像與虛擬圖像進(jìn)行比較，在攝像圖像內(nèi)的遠(yuǎn)處位置像素與虛擬圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將虛擬圖像的姿態(tài)角似然度設(shè)定得高。另一方面，在攝像圖像內(nèi)的近處位置像素與虛擬圖像內(nèi)的近處位置像素一致的情況下將虛擬圖像的位置似然度設(shè)定得高。然后，移動(dòng)物體位置姿態(tài)估計(jì)裝置根據(jù)將姿態(tài)角似然度設(shè)定得高的虛擬圖像的虛擬姿態(tài)角來估計(jì)移動(dòng)物體的實(shí)際的姿態(tài)角。另一方面，根據(jù)將位置似然度設(shè)定得高的虛擬圖像的虛擬位置來估計(jì)移動(dòng)物體的實(shí)際的位置。
[0056]根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，針對位置和姿態(tài)角分別設(shè)定似然度，根據(jù)到實(shí)際的影像與虛擬影像一致的場所的距離，能夠分別調(diào)整位置和姿態(tài)角來進(jìn)行估計(jì)。具體地說，在距照相機(jī)2遠(yuǎn)的場所中攝像圖像與虛擬圖像一致的情況下，虛擬姿態(tài)角雖然接近真值但有可能虛擬位置的誤差大。因此，能夠?qū)⒆藨B(tài)角似然度設(shè)定得更大，但是不將位置似然度設(shè)定得那么大。另一方面，在距照相機(jī)2近的場所中攝像圖像與虛擬圖像一致的情況下，將位置似然度設(shè)定得更大，相反地不將姿態(tài)角似然度設(shè)定得那么大。
[0057]在攝像圖像與虛擬圖像一致的場所距照相機(jī)2遠(yuǎn)的情況下，存在虛擬位置的誤差大的情況。相反地，在攝像圖像與虛擬圖像一致的場所距照相機(jī)2近的情況下，存在虛擬姿態(tài)角的誤差大的情況。根據(jù)該移動(dòng)物體位置姿態(tài)估計(jì)裝置，能夠高精度地估計(jì)移動(dòng)物體的位置和姿態(tài)角。
[0058]另外，在規(guī)定距離以上的遠(yuǎn)處攝像圖像的像素與虛擬圖像的像素一致的情況下，設(shè)定虛擬圖像的姿態(tài)角似然度以使姿態(tài)角似然度的增加幅度減少或使其成為規(guī)定的上限值。由此，根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，能夠避免在一致的影像中包含有距照相機(jī)2的距離極遠(yuǎn)的部分時(shí)根據(jù)該遠(yuǎn)處部分中的一致度來決定姿態(tài)角似然度。因此，即使在從距照相機(jī)2的距離極遠(yuǎn)的部分中抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小姿態(tài)角的估計(jì)誤差。
[0059]并且，在規(guī)定距離以上的近處攝像圖像的像素與虛擬圖像的像素一致的情況下，設(shè)定虛擬圖像的位置似然度以使位置似然度的增加幅度減少或者使其成為規(guī)定的上限值。由此，避免在距照相機(jī)2 —定距離以內(nèi)的兩個(gè)影像一致的情況下使位置似然度過大。因而，根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，即使在從距照相機(jī)2的距離極近的部分抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小位置的估計(jì)誤差。
[0060]并且，根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，設(shè)定多個(gè)粒子(候選點(diǎn))來設(shè)定關(guān)于各粒子的位置似然度LIKE_P和姿態(tài)角似然度LIKE_A。根據(jù)關(guān)于各粒子的位置似然度LIKE_P和姿態(tài)角似然度LIKE_A，求出車輛的位置和姿態(tài)角。然后，能夠根據(jù)位置似然度LIKE_P和姿態(tài)角似然度LIKE_A對粒子進(jìn)行重采樣。
[0061]在此，在粒子過濾器中，在估計(jì)的次數(shù)設(shè)為η的情況下，由于將估計(jì)精度設(shè)為a倍，因此分散的粒子的個(gè)數(shù)在理論上也需要以a的η次方倍地增加(確率π # f 7.第4章 3 節(jié)((著)Sebastian Thrun/ffolfram Burgard/Dieter Fox,(訳)上田隆一，(発行)(株)每日2 S Λ 二 ’一'> 3 > < ))。在現(xiàn)有技術(shù)中，例如在同時(shí)對三維中的位置信息和姿態(tài)角信息進(jìn)行估計(jì)的情況下，其次數(shù)為6次。因此，如果想要將估計(jì)精度升高至2倍，則運(yùn)算時(shí)間成為64倍，如果想要將估計(jì)精度升高至3倍，則運(yùn)算時(shí)間增加到729倍。
[0062]與此相對地，根據(jù)本實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置，能夠?qū)⑽恢眯畔⒑妥藨B(tài)角信息分開進(jìn)行處理。因而，在理論上想要將估計(jì)精度升高至2倍的情況下，運(yùn)算時(shí)間為23X2 = 16倍，在想要將估計(jì)精度升高至3倍的情況下，運(yùn)算時(shí)間為33X2 = 54倍，因此能夠大幅地減輕運(yùn)算負(fù)荷。
[0063](第二實(shí)施方式)
[0064]在本發(fā)明的第二實(shí)施方式中，說明針對車輛的位置和姿態(tài)角分別使用不同的虛擬圖像進(jìn)行估計(jì)的移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法。
[0065]具體地說，在第一實(shí)施方式中，設(shè)定多個(gè)由虛擬位置和虛擬姿態(tài)角的組合構(gòu)成的候選點(diǎn)(粒子)，根據(jù)關(guān)于各候選點(diǎn)獲取到的虛擬圖像，估計(jì)出車輛的位置和姿態(tài)角。與此相對地，在第二實(shí)施方式中，基于多個(gè)虛擬位置獲取車輛附近區(qū)域的虛擬圖像，使用車輛附近區(qū)域的虛擬圖像來估計(jì)車輛的位置。然后，基于多個(gè)虛擬姿態(tài)角獲取車輛遠(yuǎn)處區(qū)域的虛擬圖像，使用車輛遠(yuǎn)處區(qū)域的虛擬圖像來估計(jì)車輛的姿態(tài)角。也就是說，在車輛的位置和姿態(tài)角的估計(jì)中使用不同的虛擬圖像的點(diǎn)不同。
[0066]此外，第二實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置的硬件結(jié)構(gòu)與圖1所示的結(jié)構(gòu)相同，省略說明。但是，E⑶I的軟件結(jié)構(gòu)、即虛擬圖像獲取部11、似然度設(shè)定部12、移動(dòng)物體位置姿態(tài)估計(jì)部13如下面所示那樣是不同的。
[0067]接著，參照圖7所示的位置姿態(tài)估計(jì)算法說明移動(dòng)物體位置姿態(tài)估計(jì)裝置的動(dòng)作。此外，在本實(shí)施方式中，設(shè)為對車輛的3自由度的位置(前后方向、橫方向、上下方向)以及3自由度的姿態(tài)角(傾倒、俯仰、橫擺)進(jìn)行估計(jì)。另外，圖7所示的位置姿態(tài)估計(jì)算法由E⑶I例如以10msec左右的間隔連續(xù)地進(jìn)行。
[0068]步驟SI?S2的處理與參照圖3說明的步驟SI?S2相同，省略說明。
[0069]在步驟S2之后進(jìn)入步驟S13，E⑶I (虛擬圖像獲取部11)根據(jù)在步驟S2中計(jì)算出的里程來計(jì)算具有6自由度的初始預(yù)測位置和初始預(yù)測姿態(tài)角。具體地說，虛擬圖像獲取部11將從在前一周期的步驟S16中估計(jì)出的車輛的位置移動(dòng)了與在本次的步驟S2中計(jì)算出的里程相應(yīng)的量得到的位置設(shè)定為初始預(yù)測位置。其中，在開始位置姿態(tài)估計(jì)算法后最初的周期的情況下，ECUl不具有前次的車輛位置信息。因此，虛擬圖像獲取部11將來自車輛傳感器組4中包括的GPS接收機(jī)41的數(shù)據(jù)設(shè)為初始預(yù)測位置?；蛘撸摂M圖像獲取部11也可以事先存儲(chǔ)在前次停車時(shí)最后計(jì)算出的車輛位置和姿態(tài)角，設(shè)為初始預(yù)測位置和初始預(yù)測姿態(tài)角。
[0070]進(jìn)入步驟S14，虛擬圖像獲取部11在步驟S13中計(jì)算出的初始預(yù)測位置及其附近設(shè)定多個(gè)虛擬位置。此時(shí)，虛擬圖像獲取部11將因車輛傳感器組4的測量誤差、通信遲滯產(chǎn)生的里程的誤差、關(guān)于里程所考慮不到的車輛的動(dòng)態(tài)特性考慮在內(nèi)，生成有可能取得車輛的位置的真值的多個(gè)虛擬位置。關(guān)于虛擬位置，針對位置所包含的3自由度的參數(shù)分別設(shè)定誤差的上下限，在該誤差的上下限的范圍內(nèi)以固定間隔設(shè)定。在本實(shí)施方式中，針對位置所包含的3自由度的參數(shù)的誤差的上下限按前后方向、橫方向、上下方向的順序設(shè)為±0.2[m]、±0.l[m]、±0.05 [m]，分別以0.01 [m]的間隔設(shè)定虛擬位置。因而，制作40X20X10 = 8000的虛擬位置。關(guān)于誤差的上下限和規(guī)定間隔，期望對車輛的駕駛狀態(tài)、路面的狀況進(jìn)行檢測或估計(jì)，從而適當(dāng)?shù)刈兏＠缭诋a(chǎn)生了急轉(zhuǎn)彎、打滑等的情況下，平面方向(前后方向、橫方向)的誤差變大的可能性高。因此，優(yōu)選的是，使三個(gè)參數(shù)的誤差的上下限變大。
[0071]進(jìn)入步驟S15，虛擬圖像獲取部11使用評價(jià)點(diǎn)投影方法，在步驟S14中設(shè)定的各虛擬位置制作車輛附近的區(qū)域的虛擬圖像。其中，步驟S15中的虛擬圖像的制作是為了估計(jì)車輛I的位置。因此，僅將三維地圖數(shù)據(jù)庫3中存儲(chǔ)的邊緣等的三維位置信息中的位于車輛I附近的信息利用評價(jià)點(diǎn)投影手段變換為從虛擬位置和虛擬姿態(tài)角拍攝到的虛擬圖像。在本實(shí)施方式中，僅對位于距各虛擬位置的距離3m以內(nèi)的邊緣等的三維位置信息進(jìn)行投影來制作虛擬圖像。此外，作為在步驟S15中使用的虛擬姿態(tài)角，只要使用在前一周期的步驟S19中估計(jì)出的車輛的姿態(tài)角、或者在步驟S13中求出的初始預(yù)測姿態(tài)角即可。
[0072]進(jìn)入步驟S16，似然度設(shè)定部12將在步驟S15中制作出的車輛附近的區(qū)域的各虛擬圖像與攝像圖像進(jìn)行比較。具體地說，關(guān)于在步驟S15中制作出的各個(gè)虛擬圖像，將虛擬圖像中包含的邊緣與由攝像圖像中包含的邊緣構(gòu)成的邊緣圖像進(jìn)行比較，計(jì)算虛擬圖像內(nèi)的邊緣與邊緣圖像內(nèi)的邊緣的一致度。例如對在車輛附近的虛擬圖像與邊緣圖像之間邊緣一致的像素?cái)?shù)、即在邊緣所存在的虛擬圖像上的像素坐標(biāo)位置存在邊緣圖像上的邊緣的像素?cái)?shù)進(jìn)行計(jì)數(shù)來作為一致度。然后，虛擬圖像內(nèi)的邊緣與攝像圖像內(nèi)的邊緣的一致度越高，似然度設(shè)定部12將位置似然度設(shè)定得越高。
[0073]接著，移動(dòng)物體位置姿態(tài)估計(jì)部13根據(jù)由似然度設(shè)定部12將位置似然度設(shè)定得高的虛擬圖像的虛擬位置來估計(jì)車輛的位置。例如，移動(dòng)物體位置姿態(tài)估計(jì)部13將在步驟S15中制作出的多個(gè)虛擬圖像中的一致的像素?cái)?shù)最多的虛擬圖像的虛擬位置計(jì)算為車輛I實(shí)際的位置?；蛘?，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對各虛擬位置進(jìn)行與各虛擬圖像的位置似然度相應(yīng)的加權(quán)，將加權(quán)后的虛擬位置的平均值計(jì)算為車輛實(shí)際的位置。
[0074]這樣，通過步驟S14?S16的處理，來基于多個(gè)虛擬位置獲取車輛附近區(qū)域的虛擬圖像，能夠使用車輛附近區(qū)域的虛擬圖像來估計(jì)車輛的位置。
[0075]接著，進(jìn)入步驟S17，虛擬圖像獲取部11參照在步驟S13中計(jì)算出的初始預(yù)測姿態(tài)角來設(shè)定多個(gè)虛擬姿態(tài)角。此時(shí)，虛擬圖像獲取部11將因車輛傳感器組4的測量誤差、通信遲滯產(chǎn)生的里程的誤差、關(guān)于里程所考慮不到的車輛的動(dòng)態(tài)特性考慮在內(nèi)，生成有可能取得車輛的姿態(tài)角的真值的多個(gè)虛擬姿態(tài)角。關(guān)于虛擬姿態(tài)角，針對姿態(tài)角所包含的3自由度的參數(shù)分別設(shè)定誤差的上下限，在該誤差的上下限的范圍內(nèi)以規(guī)定間隔設(shè)定。在本實(shí)施方式中，針對姿態(tài)角所包含的3自由度的參數(shù)的誤差的上下限按傾倒、俯仰、橫擺的順序設(shè)為±0.5 [deg]、± 0.5 [deg]、± 0.5 [deg],分別以0.05 [deg]的間隔設(shè)定虛擬姿態(tài)角。因而，制作20X20X20 = 8000的虛擬位置。與步驟S14的位置的情況同樣地，關(guān)于誤差的上下限和規(guī)定間隔，期望對車輛的駕駛狀態(tài)、路面的狀況進(jìn)行檢測或估計(jì)，從而適當(dāng)?shù)刈兏＠缭诋a(chǎn)生了急轉(zhuǎn)彎、打滑等的情況下，橫擺角的誤差變大的可能性高。因此，優(yōu)選的是，使橫擺角的誤差的上下限變大、或者在如爬坡那樣的情況下使俯仰角的誤差的上下限變大。
[0076]進(jìn)入步驟S18，虛擬圖像獲取部11利用評價(jià)點(diǎn)投影方法，針對在步驟S17中設(shè)定的各虛擬姿態(tài)角制作距車輛遠(yuǎn)的區(qū)域的虛擬圖像。其中，步驟S18中的虛擬圖像的制作是為了估計(jì)車輛I的姿態(tài)角。因此，僅將三維地圖數(shù)據(jù)庫3中所存儲(chǔ)的邊緣等的三維位置信息中的位于距車輛I遠(yuǎn)處的信息利用評價(jià)點(diǎn)投影手段變換為從虛擬位置和虛擬姿態(tài)角拍攝到的虛擬圖像。在本實(shí)施方式中，僅對位于距在步驟S13中計(jì)算出的初始預(yù)測位置或在步驟S16中計(jì)算出的車輛I的位置的距離20m以遠(yuǎn)的邊緣等的三維位置信息進(jìn)行投影來制作虛擬圖像。此外，作為在步驟S18中使用的虛擬位置，只要使用在前一周期的步驟S19中估計(jì)出的車輛的位置、或者在步驟S13中求出的初始預(yù)測位置即可。此外，即使投影到極遠(yuǎn)處，投影的處理也花費(fèi)時(shí)間，而且照相機(jī)2的分辨率也無法區(qū)分邊緣是否一致。因此，在本實(shí)施方式中，不對距離50m以外的邊緣等的三維位置信息進(jìn)行投影。
[0077]進(jìn)入步驟S19，似然度設(shè)定部12將在步驟S18中制作出的距車輛遠(yuǎn)的區(qū)域的各虛擬圖像與攝像圖像進(jìn)行比較。具體地說，關(guān)于在步驟S18中制作出的各個(gè)虛擬圖像，將虛擬圖像中包含的邊緣與由攝像圖像中包含的邊緣構(gòu)成的邊緣圖像進(jìn)行比較，計(jì)算虛擬圖像內(nèi)的邊緣與邊緣圖像內(nèi)的邊緣的一致度。例如，對在車輛遠(yuǎn)處的虛擬圖像與邊緣圖像之間邊緣一致的像素?cái)?shù)、即在邊緣所存在的虛擬圖像上的像素坐標(biāo)位置存在邊緣圖像上的邊緣的像素?cái)?shù)進(jìn)行計(jì)數(shù)來作為一致度。而且，虛擬圖像內(nèi)的邊緣與攝像圖像內(nèi)的邊緣的一致度越高，似然度設(shè)定部12將姿態(tài)角似然度設(shè)定得越高。
[0078]接著，移動(dòng)物體位置姿態(tài)估計(jì)部13根據(jù)由似然度設(shè)定部12將姿態(tài)角似然度設(shè)定得高的虛擬圖像的虛擬姿態(tài)角來估計(jì)車輛的姿態(tài)角。例如，移動(dòng)物體位置姿態(tài)估計(jì)部13將在步驟S18中制作出的多個(gè)虛擬圖像中的一致的像素?cái)?shù)最多的虛擬圖像的虛擬姿態(tài)角計(jì)算為車輛I實(shí)際的姿態(tài)角。或者，移動(dòng)物體位置姿態(tài)估計(jì)部13也可以針對各虛擬姿態(tài)角進(jìn)行與各虛擬圖像的姿態(tài)角似然度相應(yīng)的加權(quán)，將加權(quán)后的虛擬姿態(tài)角的平均值計(jì)算為車輛實(shí)際的姿態(tài)角。
[0079]這樣，通過步驟S17?S19的處理，來基于多個(gè)虛擬姿態(tài)角獲取車輛遠(yuǎn)處區(qū)域的虛擬圖像，能夠使用車輛遠(yuǎn)處區(qū)域的虛擬圖像來估計(jì)車輛的姿態(tài)角。
[0080]E⑶I通過重復(fù)進(jìn)行如上所述的步驟SI?步驟S19的處理，能夠依次計(jì)算車輛的位置和姿態(tài)角。
[0081]如以上說明的那樣，在本實(shí)施方式中，針對車輛的位置和姿態(tài)角分別使用不同的虛擬圖像進(jìn)行估計(jì)。具體地說，基于多個(gè)虛擬位置獲取車輛附近區(qū)域的虛擬圖像，使用車輛附近區(qū)域的虛擬圖像來估計(jì)車輛的位置。然后，基于多個(gè)虛擬姿態(tài)角獲取車輛遠(yuǎn)處區(qū)域的虛擬圖像，使用車輛遠(yuǎn)處區(qū)域的虛擬圖像來估計(jì)車輛的姿態(tài)角。由此，針對位置和姿態(tài)角設(shè)定不同的似然度，根據(jù)到實(shí)際的影像與虛擬影像一致的場所的距離，能夠分別調(diào)整位置和姿態(tài)角來進(jìn)行估計(jì)。因此，能夠高精度地估計(jì)移動(dòng)物體的位置和姿態(tài)角。
[0082]另外，在規(guī)定距離以上的遠(yuǎn)處的攝像圖像的像素與虛擬圖像的像素一致的情況下，也可以設(shè)定虛擬圖像的姿態(tài)角似然度以使姿態(tài)角似然度的增加幅度減少或者使其成為規(guī)定的上限值。由此，根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，能夠避免在一致的影像中包含有距照相機(jī)2的距離極遠(yuǎn)的部分時(shí)根據(jù)該遠(yuǎn)處部分中的一致度來決定姿態(tài)角似然度。因此，SP使在從距照相機(jī)2的距離極遠(yuǎn)的部分中抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小姿態(tài)角的估計(jì)誤差。
[0083]并且，在規(guī)定距離以上的近處攝像圖像的像素與虛擬圖像的像素一致的情況下，設(shè)定虛擬圖像的位置似然度以使位置似然度的增加幅度減少或者使其成為規(guī)定的上限值。由此，避免在距照相機(jī)2 —定距離以內(nèi)的兩個(gè)影像一致的情況下使位置似然度過大。因而，根據(jù)移動(dòng)物體位置姿態(tài)估計(jì)裝置，即使在從距照相機(jī)2的距離極近的部分抽出的邊緣中存在噪聲、誤差，也能夠抑制其影響，從而能夠減小位置的估計(jì)誤差。
[0084]以上按實(shí)施方式說明了本發(fā)明的內(nèi)容，但是本發(fā)明不限定于這些記載，能夠進(jìn)行各種變形、改進(jìn)，這對于本領(lǐng)域技術(shù)人員來說是顯而易見的。
[0085]在上述的實(shí)施方式中將車輛作為例子，但是只要是搭載有至少一臺(tái)以上的照相機(jī)的移動(dòng)物體，則也能夠應(yīng)用于飛機(jī)、船舶等。
[0086]另外，在上述的實(shí)施方式中求出了車輛的6自由度的位置(前后方向、橫方向、上下方向)以及姿態(tài)角(傾倒、俯仰、橫擺)，但是不限于此。在例如對不具有懸架等的在工廠等中使用的無人搬運(yùn)車等那樣3自由度的位置(前后方向、橫方向)和姿態(tài)角(橫擺)進(jìn)行估計(jì)的情況下，也能夠應(yīng)用本實(shí)施方式。具體地說，只要是這樣的車輛，上下方向的位置以及傾倒和俯仰之類的姿態(tài)角就是固定的，因此只要預(yù)先測量出這些參數(shù)、或者參照三維地圖數(shù)據(jù)庫3求出即可。
[0087]此外，在第一和第二實(shí)施方式中，作為獲取“從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像的對比圖像獲取部”，例示了“將三維地圖數(shù)據(jù)變換為從虛擬位置和虛擬姿態(tài)角拍攝到的圖像來獲取虛擬圖像的虛擬圖像獲取部11”。但是，“對比圖像獲取部”不限定于此。例如，對比圖像獲取部也可以獲取照相機(jī)2過去拍攝到的攝像圖像來作為對比圖像。在這種情況下，移動(dòng)物體位置姿態(tài)估計(jì)部13根據(jù)拍攝到由似然度設(shè)定部12將姿態(tài)角似然度設(shè)定得高的對比圖像時(shí)的車輛的姿態(tài)角來估計(jì)車輛的姿態(tài)角、根據(jù)拍攝到由似然度設(shè)定部12將位置似然度設(shè)定得高的對比圖像時(shí)的車輛的位置來估計(jì)車輛的位置即可。由此，針對位置和姿態(tài)角分別設(shè)定似然度，根據(jù)到當(dāng)前的攝像影像與過去的攝像圖像(對比圖像)一致的場所的距離，能夠分別調(diào)整位置和姿態(tài)角來進(jìn)行估計(jì)。因此，能夠高精度地估計(jì)移動(dòng)物體的位置和姿態(tài)角。
[0088]日本特愿2012-049372號(申請日:2012年3月6日)的全部內(nèi)容在此處被引用。
[0089]產(chǎn)業(yè)h的可利用件
[0090]根據(jù)本實(shí)施方式所涉及的移動(dòng)物體位置姿態(tài)估計(jì)裝置和移動(dòng)物體位置姿態(tài)估計(jì)方法，在遠(yuǎn)處位置處攝像圖像與虛擬圖像一致的情況下，使姿態(tài)角似然度變高，在近處位置處攝像圖像與虛擬圖像一致的情況下，使位置似然度變高，因此能夠根據(jù)位置誤差大的遠(yuǎn)處的一致來設(shè)定姿態(tài)角似然度，根據(jù)姿態(tài)角誤差大的近處的一致來設(shè)定位置似然度。由此，能夠高精度地估計(jì)移動(dòng)物體的位置和姿態(tài)角。因此，本發(fā)明具有產(chǎn)業(yè)上的可利用性。
[0091]附圖標(biāo)記說明
[0092]1:E⑶；2:照相機(jī)(攝像部)；3:三維地圖數(shù)據(jù)庫；11:虛擬圖像獲取部(對比圖像獲取部)；12:似然度設(shè)定部；13:移動(dòng)物體位置姿態(tài)估計(jì)部。
【權(quán)利要求】
1.一種移動(dòng)物體位置姿態(tài)估計(jì)裝置，對移動(dòng)物體的位置和姿態(tài)角進(jìn)行估計(jì)，該移動(dòng)物體位置姿態(tài)估計(jì)裝置的特征在于，具有: 攝像部，其對上述移動(dòng)物體周邊進(jìn)行拍攝來獲取攝像圖像； 對比圖像獲取部，其獲取從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像； 似然度設(shè)定部，其將由上述攝像部獲取到的攝像圖像與由上述對比圖像獲取部獲取到的對比圖像進(jìn)行比較，在上述攝像圖像內(nèi)的遠(yuǎn)處位置像素與上述對比圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將上述對比圖像的姿態(tài)角似然度設(shè)定得高，在上述攝像圖像內(nèi)的近處位置像素與上述對比圖像內(nèi)的近處位置像素一致的情況下，將上述對比圖像的位置似然度設(shè)定得高；以及 移動(dòng)物體位置姿態(tài)估計(jì)部，其根據(jù)由上述似然度設(shè)定部將上述姿態(tài)角似然度設(shè)定得高的上述對比圖像的姿態(tài)角來估計(jì)上述移動(dòng)物體的姿態(tài)角，根據(jù)由上述似然度設(shè)定部將上述位置似然度設(shè)定得高的上述對比圖像的位置來估計(jì)上述移動(dòng)物體的位置。
2.根據(jù)權(quán)利要求1所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于， 上述對比圖像獲取部是將三維地圖數(shù)據(jù)變換為從虛擬位置和虛擬姿態(tài)角拍攝到的圖像來獲取虛擬圖像的虛擬圖像獲取部， 上述似然度設(shè)定部將由上述攝像部獲取到的攝像圖像與由上述虛擬圖像獲取部獲取到的虛擬圖像進(jìn)行比較，在上述攝像圖像內(nèi)的遠(yuǎn)處位置像素與上述虛擬圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將上述虛擬圖像的姿態(tài)角似然度設(shè)定得高，在上述攝像圖像內(nèi)的近處位置像素與上述虛擬圖像內(nèi)的近處位置像素一致的情況下，將上述虛擬圖像的位置似然度設(shè)定得高， 上述移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)由上述似然度設(shè)定部將上述姿態(tài)角似然度設(shè)定得高的上述虛擬圖像的虛擬姿態(tài)角來估計(jì)上述移動(dòng)物體的姿態(tài)角，根據(jù)由上述似然度設(shè)定部將上述位置似然度設(shè)定得高的上述虛擬圖像的虛擬位置來估計(jì)上述移動(dòng)物體的位置。
3.根據(jù)權(quán)利要求1所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于，上述對比圖像獲取部獲取上述攝像部過去拍攝到的上述攝像圖像作為上述對比圖像，上述移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)拍攝到由上述似然度設(shè)定部將上述姿態(tài)角似然度設(shè)定得高的上述對比圖像時(shí)的上述移動(dòng)體的姿態(tài)角來估計(jì)上述移動(dòng)物體的姿態(tài)角，根據(jù)拍攝到由上述似然度設(shè)定部將上述位置似然度設(shè)定得高的上述對比圖像時(shí)的上述移動(dòng)體的位置來估計(jì)上述移動(dòng)物體的位置。
4.根據(jù)權(quán)利要求1?3中的任一項(xiàng)所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于， 上述似然度設(shè)定部在上述攝像圖像和上述對比圖像內(nèi)的遠(yuǎn)處位置像素中的、規(guī)定距離以上的遠(yuǎn)處的上述攝像圖像的像素與上述對比圖像的像素一致的情況下，設(shè)定上述對比圖像的姿態(tài)角似然度以使似然度的增加幅度減少或者使似然度的增加幅度成為規(guī)定的上限值。
5.根據(jù)權(quán)利要求1?4中的任一項(xiàng)所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于， 上述似然度設(shè)定部在上述攝像圖像和上述對比圖像內(nèi)的近處位置像素中的、規(guī)定距離以上的近處的上述攝像圖像的像素與上述對比圖像的像素一致的情況下，設(shè)定上述對比圖像的位置似然度以使似然度的增加幅度減少或者使似然度的增加幅度成為規(guī)定的上限值。
6.根據(jù)權(quán)利要求2、4以及5中的任一項(xiàng)所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于， 上述虛擬圖像獲取部設(shè)定多個(gè)設(shè)定了虛擬位置和虛擬姿態(tài)角的候選點(diǎn)，針對各上述候選點(diǎn)獲取虛擬圖像，上述似然度設(shè)定部將各虛擬圖像與攝像圖像進(jìn)行比較來設(shè)定姿態(tài)角似然度和位置似然度，上述移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)多個(gè)虛擬圖像的姿態(tài)角似然度來估計(jì)上述移動(dòng)物體的姿態(tài)角，根據(jù)多個(gè)虛擬圖像的位置似然度來估計(jì)上述移動(dòng)物體的位置， 上述虛擬圖像獲取部根據(jù)多個(gè)虛擬圖像的姿態(tài)角似然度以及多個(gè)虛擬圖像的位置似然度，重新設(shè)定多個(gè)上述候選點(diǎn)。
7.根據(jù)權(quán)利要求1所述的移動(dòng)物體位置姿態(tài)估計(jì)裝置，其特征在于， 上述對比圖像獲取部設(shè)定多個(gè)位置，針對各上述位置獲取離上述移動(dòng)體近的區(qū)域的上述對比圖像， 上述似然度設(shè)定部將離上述移動(dòng)體近的區(qū)域的各對比圖像與上述攝像圖像進(jìn)行比較，上述對比圖像內(nèi)的邊緣與上述攝像圖像內(nèi)的邊緣的一致度越高，則將上述位置似然度設(shè)定得越高， 上述移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)由上述似然度設(shè)定部將上述位置似然度設(shè)定得高的上述對比圖像的位置，來估計(jì)上述移動(dòng)物體的位置， 上述對比圖像獲取部設(shè)定多個(gè)姿態(tài)角，針對各上述姿態(tài)角獲取離上述移動(dòng)體遠(yuǎn)的區(qū)域的上述對比圖像， 上述似然度設(shè)定部將離上述移動(dòng)體遠(yuǎn)的區(qū)域的各對比圖像與上述攝像圖像進(jìn)行比較，上述對比圖像內(nèi)的邊緣與上述攝像圖像內(nèi)的邊緣的一致度越高，則將上述姿態(tài)角似然度設(shè)定得越高， 上述移動(dòng)物體位置姿態(tài)估計(jì)部根據(jù)由上述似然度設(shè)定部將上述姿態(tài)角似然度設(shè)定得高的上述對比圖像的姿態(tài)角，來估計(jì)上述移動(dòng)物體的姿態(tài)角。
8.一種移動(dòng)物體位置姿態(tài)估計(jì)方法，對移動(dòng)物體的位置和姿態(tài)角進(jìn)行估計(jì)，該移動(dòng)物體位置姿態(tài)估計(jì)方法的特征在于，包括以下步驟: 將對上述移動(dòng)物體周邊進(jìn)行拍攝得到的攝像圖像與從預(yù)定的位置和姿態(tài)角觀察時(shí)的對比圖像進(jìn)行比較； 在上述攝像圖像內(nèi)的遠(yuǎn)處位置像素與上述對比圖像內(nèi)的遠(yuǎn)處位置像素一致的情況下，將上述對比圖像的姿態(tài)角似然度設(shè)定得高，在上述攝像圖像內(nèi)的近處位置像素與上述對比圖像內(nèi)的近處位置像素一致的情況下，將上述對比圖像的位置似然度設(shè)定得高；以及 根據(jù)上述姿態(tài)角似然度被設(shè)定得高的上述對比圖像的姿態(tài)角來估計(jì)上述移動(dòng)物體的姿態(tài)角，根據(jù)上述位置似然度被設(shè)定得高的上述對比圖像的位置來估計(jì)上述移動(dòng)物體的位置。
【文檔編號】G06T17/05GK104204726SQ201380013066
【公開日】2014年12月10日 申請日期:2013年2月28日 優(yōu)先權(quán)日:2012年3月6日
【發(fā)明者】山口一郎, 古城直樹 申請人:日產(chǎn)自動(dòng)車株式會(huì)社