自身位置計(jì)算裝置以及自身位置計(jì)算方法
【專利摘要】自身位置計(jì)算裝置包括：投光器(11)，對(duì)車輛周圍的路面(31)投光圖案光(32a)；以及攝像機(jī)(12)，拍攝包含被投光了圖案光(32a)的區(qū)域的車輛周圍的路面(31)，獲取圖像(38)，由圖像中的圖案光的位置計(jì)算車輛(10)對(duì)于路面(31)的姿態(tài)角，根據(jù)由圖像檢測(cè)到的路面上的多個(gè)特征點(diǎn)的時(shí)間變化，計(jì)算車輛的姿態(tài)變化量。通過對(duì)車輛的初始位置以及姿態(tài)角不斷加上姿態(tài)變化量，計(jì)算車輛的當(dāng)前位置以及姿態(tài)角。在多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)的情況下，將此時(shí)的車輛的當(dāng)前位置以及由圖案光的位置算出的車輛的姿態(tài)角設(shè)定為車輛的初始位置以及姿態(tài)角，開始姿態(tài)變化量的相加。
【專利說明】
自身位置計(jì)算裝置以及自身位置計(jì)算方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及自身位置計(jì)算裝置以及自身位置計(jì)算方法。
【背景技術(shù)】
[0002] 已知獲取由安裝在車輛上的攝像機(jī)拍攝的車輛近旁的圖像，根據(jù)圖像的變化求車 輛的移動(dòng)量的技術(shù)(參照專利文獻(xiàn)1)。在專利文獻(xiàn)1中，為了在車輛的低速且微妙的移動(dòng)中 也高精度地求出移動(dòng)量，從圖像中檢測(cè)特征點(diǎn)，求出在圖像上特征點(diǎn)的位置，由特征點(diǎn)的移 動(dòng)方向以及移動(dòng)距離(移動(dòng)量)求出車輛的移動(dòng)量。
[0003] 現(xiàn)有技術(shù)文獻(xiàn)
[0004] 專利文獻(xiàn)
[0005] 專利文獻(xiàn)1:日本專利特開2008 -175717號(hào)公報(bào)

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

[0006] 但是，在特征點(diǎn)的檢測(cè)狀態(tài)差的情況下，難以由特征點(diǎn)的位置高精度地求車輛的 移動(dòng)量。
[0007] 本發(fā)明鑒于上述課題而完成，其目的是提供可以與特征點(diǎn)的檢測(cè)狀態(tài)無關(guān)，高精 度且穩(wěn)定地估計(jì)車輛的當(dāng)前位置的自身位置計(jì)算裝置以及自身位置計(jì)算方法。
[0008] 本發(fā)明的一個(gè)方式的自身位置計(jì)算裝置包括:投光器，對(duì)車輛周圍的路面投光圖 案光;拍攝包含被投光了圖案光的區(qū)域的車輛周圍的路面，獲取圖像的攝像單元。自身位置 計(jì)算裝置由攝像單元獲取的圖像中的圖案光的位置，計(jì)算車輛對(duì)于路面的姿態(tài)角，根據(jù)由 圖像檢測(cè)到的路面上的多個(gè)特征點(diǎn)的時(shí)間變化，計(jì)算車輛的姿態(tài)變化量。然后，通過對(duì)車輛 的初始位置以及姿態(tài)角加上姿態(tài)變化量，計(jì)算車輛的當(dāng)前位置以及姿態(tài)角。自身位置計(jì)算 裝置在多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，較差的狀態(tài)的情況下，將此時(shí)的車輛的當(dāng)前 位置以及由圖案光的位置算出的車輛的姿態(tài)角設(shè)定為車輛的初始位置以及姿態(tài)角，開始姿 態(tài)變化量的相加。
【附圖說明】
[0009] 圖1是表示第1實(shí)施方式的自身位置計(jì)算裝置的整體結(jié)構(gòu)的方框圖。
[0010] 圖2是表示投光器11以及攝像機(jī)12對(duì)車輛10的安裝例子的外觀圖。
[0011] 圖3(a)是表示由投光器11和攝像機(jī)12之間的基線長(zhǎng)Lb和各亮點(diǎn)光在圖像上的坐 標(biāo)(UpL)，計(jì)算被照射了各亮點(diǎn)光的路面31上的位置的狀況的圖，圖3(b)是表示根據(jù)從與 照射了圖案光32a的區(qū)域不同的其它區(qū)域33檢測(cè)到的特征點(diǎn)的時(shí)間變化，求攝像機(jī)12的移 動(dòng)方向34的狀況的示意圖。
[0012] 圖4(a)以及圖4 (b)是表示對(duì)于由攝像機(jī)12獲取的圖像實(shí)施了二值化處理的圖案 光32a的圖像的圖，圖4(a)表示圖案光32a整體，圖4(b)擴(kuò)大表示一個(gè)亮點(diǎn)光S P，圖4(c)是表 示由圖案光提取單元21提取的各亮點(diǎn)光\的重心的位置凡的圖。
[0013] 圖5是用于說明計(jì)算距離以及姿態(tài)角的變化量的方法的示意圖。
[0014] 圖6 (a)表示在時(shí)刻t獲取的第1幀（圖像)38的一例。圖6 (b)表示在從時(shí)刻t經(jīng)過了 時(shí)間A t的時(shí)刻(t+ Δ t)獲取的第2幀38'。
[0015]圖7是表示與在前后幀之間取得對(duì)應(yīng)關(guān)系的特征點(diǎn)的數(shù)對(duì)應(yīng)的特征點(diǎn)I的檢測(cè)狀 態(tài)的判斷結(jié)果的曲線圖。
[0016] 圖8是表示使用了圖1的自身位置計(jì)算裝置的自身位置計(jì)算方法的一例的流程圖。
[0017] 圖9是表示圖8的步驟S09的詳細(xì)的步驟的流程圖。
[0018] 圖10(a)以及圖10(b)是表示車輛10的搖擺角以及車寬方向移動(dòng)量的估計(jì)誤差的 例子的曲線圖。
[0019] 圖11是表示第2實(shí)施方式的自身位置計(jì)算裝置的整體結(jié)構(gòu)的方框圖。
[0020]圖12是表示第3實(shí)施方式的自身位置計(jì)算裝置的整體結(jié)構(gòu)的方框圖。
【具體實(shí)施方式】
[0021]參照附圖，說明實(shí)施方式。對(duì)附圖的記載中同一部分附加同一標(biāo)號(hào)，省略說明。 [0022](第1實(shí)施方式）
[0023] (硬件結(jié)構(gòu)）
[0024] 首先，參照?qǐng)D1，說明第1實(shí)施方式的自身位置計(jì)算裝置的硬件結(jié)構(gòu)。自身位置計(jì)算 裝置包括:投光器11;攝像機(jī)12;以及引擎控制單元(ECU)13。投光器11被安裝在車輛上，對(duì) 車輛周圍的路面投光圖案光。攝像機(jī)12是被安裝在車輛上，拍攝包含被投光了圖案光的區(qū) 域的車輛周圍的路面而獲取圖像的攝像單元的一個(gè)例子。ECU13是控制投光器11，并且執(zhí)行 由攝像機(jī)12獲取的圖像估計(jì)車輛的移動(dòng)量的一連串的信息處理循環(huán)的控制單元的一個(gè)例 子。
[0025] 攝像機(jī)12是使用了固體攝像元件，例如CCD以及CMOS的數(shù)字?jǐn)z像機(jī)，獲取可進(jìn)行圖 像處理的數(shù)字圖像。攝像機(jī)12的攝像對(duì)象是車輛周圍的路面，在車輛周圍的路面中，包含車 輛的前部、后部、側(cè)部、車輛底部的路面。例如，如圖2所示，攝像機(jī)12可以安裝在車輛10的前 部，具體地說安裝在前保險(xiǎn)杠上。
[0026] 調(diào)整攝像機(jī)12所設(shè)置的高度以及朝向，并且，自動(dòng)調(diào)整攝像機(jī)12具有的鏡頭的焦 距以及光圈，以便可以拍攝車輛10的前方的路面31上的特征點(diǎn)（紋理）以及由投光器11投光 的圖案光32b。攝像機(jī)12每隔規(guī)定的時(shí)間間隔反復(fù)進(jìn)行攝像，獲取一連串的圖像(幀)群。由 攝像機(jī)12獲取的圖像數(shù)據(jù)被轉(zhuǎn)發(fā)至ECU13，存儲(chǔ)在ECU13具有的存儲(chǔ)器中。
[0027]如圖2所示，投光器11朝向攝像機(jī)12的攝像范圍內(nèi)的路面31，投光具有包含正方形 或長(zhǎng)方形的柵格像的規(guī)定形狀的圖案光32b。攝像機(jī)12拍攝通過路面31照射的圖案光。投光 器11例如具有激光指示器以及衍射光柵。通過以衍射光柵衍射從激光指示器射出的激光， 如圖2~圖4所示，投光器11生成由柵格像或者排列為矩陣狀的多個(gè)亮點(diǎn)光S P構(gòu)成的圖案光 (3213,32&)。在圖3以及圖4所示的例子中，生成5\7的亮點(diǎn)光5 [)構(gòu)成的圖案光32&。
[0028]返回圖1，ECU13由具有CPU、存儲(chǔ)器、以及輸入輸出單元的微控制器構(gòu)成，通過執(zhí)行 預(yù)先安裝的計(jì)算機(jī)程序，構(gòu)成自身位置計(jì)算裝置具有的多個(gè)信息處理單元。ECU13對(duì)每圖像 (幀)反復(fù)執(zhí)行根據(jù)由攝像機(jī)12獲取的圖像來計(jì)算車輛的當(dāng)前位置的一連串的信息處理循 環(huán)。E⑶13也可以與車輛10有關(guān)的其它控制中使用的E⑶兼用。
[0029] 在多個(gè)信息處理單元中包括：圖案光提取單元21;姿態(tài)角計(jì)算單元22;姿態(tài)變化量 計(jì)算單元24;自身位置計(jì)算單元26;圖案光控制單元27;檢測(cè)狀態(tài)判斷單元30;以及計(jì)算狀 態(tài)判斷單元35。在姿態(tài)變化量計(jì)算單元24中包含特征點(diǎn)檢測(cè)單元23。
[0030] 圖案光提取單元21從存儲(chǔ)器讀入由攝像機(jī)12獲取的圖像，從圖像提取圖案光的位 置。如圖3(a)所示，例如，投光器11向路面31投光被排列為矩陣狀的多個(gè)亮點(diǎn)光構(gòu)成的圖案 光32a，用攝像機(jī)12檢測(cè)被路面31反射的圖案光32a。如圖4(a)以及圖4(b)所示，圖案光提取 單元21通過對(duì)由攝像機(jī)12獲取的圖像實(shí)施二值化處理，僅提取亮點(diǎn)光心的圖像。如圖4(c) 所示，圖案光提取單元21通過計(jì)算各亮點(diǎn)光S P的重心的位置He，即亮點(diǎn)光&在圖像上的坐標(biāo) (山，％)，提取圖案光32a的位置。坐標(biāo)在以攝像機(jī)12的攝像元件的像素為單位，5X7的亮點(diǎn) 光心的情況下，"j"為1以上35以下的自然數(shù)。亮點(diǎn)光心的圖像上的坐標(biāo)(UpL)作為表示圖 案光32a的位置的數(shù)據(jù)，存儲(chǔ)在存儲(chǔ)器中。
[0031] 姿態(tài)角計(jì)算單元22從存儲(chǔ)器讀入表示圖案光32a的位置的數(shù)據(jù)，根據(jù)由攝像機(jī)12 獲取的圖像中的圖案光32a的位置，計(jì)算車輛10對(duì)于路面31的距離以及姿態(tài)角。例如，如圖3 (a)所示，根據(jù)投光器11與攝像機(jī)12之間的基線長(zhǎng)Lb和各亮點(diǎn)光在圖像上的坐標(biāo)(山，％)，使 用三角測(cè)量的原理，計(jì)算照射了各亮點(diǎn)光的路面31上的位置，作為對(duì)于攝像機(jī)12的相對(duì)位 置。然后，姿態(tài)角計(jì)算單元22根據(jù)各亮點(diǎn)光對(duì)于攝像機(jī)12的相對(duì)位置，計(jì)算投光了圖案光 32a的路面31的平面式，即，攝像機(jī)12對(duì)于路面31的距離以及姿態(tài)角(法線向量）。而且，由于 攝像機(jī)12對(duì)于車輛10的安裝位置以及攝像方向已知，所以在實(shí)施方式中，作為車輛10對(duì)于 路面31的距離以及姿態(tài)角的一個(gè)例子，計(jì)算攝像機(jī)12對(duì)于路面31的距離以及姿態(tài)角。以后， 將攝像機(jī)12對(duì)于路面31的距離以及姿態(tài)角簡(jiǎn)稱為"距離以及姿態(tài)角"。由姿態(tài)角計(jì)算單元22 算出的距離以及姿態(tài)角被存儲(chǔ)在存儲(chǔ)器中。
[0032]具體地說，由于攝像機(jī)12以及投光器11被分別固定在車輛10上，所以圖案光32a的 照射方向、攝像機(jī)12和投光器11的距離(基線長(zhǎng)Lb)是已知的。因此，姿態(tài)角計(jì)算單元22使用 三角測(cè)量的原理，可以從各亮點(diǎn)光在圖像上的坐標(biāo)(山，％)求被照射了各亮點(diǎn)光的路面31上 的位置，作為對(duì)于攝像機(jī)12的相對(duì)位置(XpYpZj)。
[0033] 而且，各亮點(diǎn)光對(duì)于攝像機(jī)12的相對(duì)位置(?，乃，ZJ不存在于同一平面上的情況 較多。這是因?yàn)?，與路面31上表露出的瀝青的凹凸相應(yīng)，各亮點(diǎn)光的相對(duì)位置產(chǎn)生變化。因 此，也可以使用最小二乘法，求與各亮點(diǎn)光的距離誤差的平方和最小的平面式。
[0034] 特征點(diǎn)檢測(cè)單元23從存儲(chǔ)器讀入由攝像機(jī)12獲取的圖像，根據(jù)從存儲(chǔ)器讀入的圖 像，檢測(cè)路面31上的特征點(diǎn)。特征點(diǎn)檢測(cè)單元23為了檢測(cè)路面31上的特征點(diǎn)，例如，可以使 用 "D · G · Lowe，"Distinctive Image Features from Scale-In variant Keypoints，'' Int. J.Comput.Vis.，vol ·60，ηο·2，ρρ·91-110，Νον·200"，或者，"金澤靖，金谷健一， 匕° 二一夕匕、^ (7) fc (6) 畫像特徴點(diǎn)抽出，"信學(xué)誌，νο 1 · 87，no · 12，ρρ · 1043-1048， Dec.2004"中記載的方法。
[0035]具體地說，特征點(diǎn)檢測(cè)單元23例如使用哈里斯(Harris)運(yùn)算符或者SUSAN算符，檢 測(cè)物體的頂點(diǎn)那樣與周圍相比亮度值較大地變化的點(diǎn)作為特征點(diǎn)?；蛘撸卣鼽c(diǎn)檢測(cè)單元 23也可以使用SIFT (Scale-Invariant Feature Transform，尺度不變特征變換)特征量，檢 測(cè)在其周圍亮度值按照某個(gè)規(guī)則性變化的點(diǎn)作為特征點(diǎn)。然后，特征點(diǎn)檢測(cè)單元23對(duì)從一 個(gè)圖像檢測(cè)到的特征點(diǎn)的總數(shù)N進(jìn)行計(jì)數(shù)，對(duì)各特征點(diǎn)附加識(shí)別號(hào)（i(lfiSN))。各特征點(diǎn) 在圖像上的位置(Un被存儲(chǔ)在ECU13內(nèi)的存儲(chǔ)器中。圖6(a)以及圖6(b)表示從攝像機(jī)12 獲取的圖像檢測(cè)到的特征點(diǎn)Te的例子。各特征點(diǎn)在圖像上的位置(UnVd被存儲(chǔ)在存儲(chǔ)器 中。
[0036] 而且，實(shí)施方式中，路面31上的特征點(diǎn)主要設(shè)想大小為lcm以上2cm以下的瀝青混 合物的顆粒。為了檢測(cè)該特征點(diǎn)，攝像機(jī)12的分辨率為VGA(約30萬像素）。而且，攝像機(jī)12對(duì) 于路面31的距離約為70cm。進(jìn)而，攝像機(jī)12的攝像方向從水平面約45度（deg)向路面31傾 斜。而且，將攝像機(jī)12獲取的圖像轉(zhuǎn)發(fā)至ECU13時(shí)的亮度數(shù)值為0~255(0:最暗，255:最亮） 的范圍內(nèi)。
[0037]姿態(tài)變化量計(jì)算單元24從存儲(chǔ)器讀入在每個(gè)固定的信息處理循環(huán)中拍攝的幀中、 前次幀中包含的多個(gè)特征點(diǎn)在圖像上的位置(UnVO,進(jìn)而，從存儲(chǔ)器讀入本次幀中包含的 多個(gè)特征點(diǎn)在圖像上的位置(UnVO。然后，根據(jù)多個(gè)特征點(diǎn)在圖像上的位置變化，求車輛的 姿態(tài)變化量。這里，所謂"車輛的姿態(tài)變化量"包含對(duì)于路面31的"距離以及姿態(tài)角"的變化 量，以及路面上的"車輛(攝像機(jī)12)的移動(dòng)量"雙方。以下，說明距離以及姿態(tài)角的變化量以 及車輛的移動(dòng)量的計(jì)算方法。
[0038]圖6(a)是表示在時(shí)刻t獲取的第1幀（圖像)38的一例。如圖5或者圖6(a)所示，考慮 在第1幀38中，例如分別計(jì)算三個(gè)特征點(diǎn)Tel、Te2、Te3的相對(duì)位置(Xi，Yi，Zi)的情況。在該情 況下，可以將由特征點(diǎn)Te 1、Te2、Te3確定的平面G視為路面。由此，姿態(tài)變化量計(jì)算單元24可 以根據(jù)相對(duì)位置(XnYnZO，求攝像機(jī)12對(duì)于路面(平面G)的距離以及姿態(tài)角(法線向量）。 進(jìn)而，姿態(tài)變化量計(jì)算單元24可以通過已知的攝像機(jī)模型，求各特征點(diǎn)T el、Te2、Te3之間的距 離（1 i，12，13)以及連結(jié)各個(gè)特征點(diǎn)Tel、Te2、T e3的直線所成的角度。圖5的攝像機(jī)12表示第1幀 中的攝像機(jī)的位置。
[0039]而且，作為表示對(duì)于攝像機(jī)12的相對(duì)位置的3維坐標(biāo)(Xi，Yi，Zi)，將攝像機(jī)12的攝 像方向設(shè)定為Z軸，在以攝像方向?yàn)榉ň€并且包含攝像機(jī)12的平面內(nèi)，設(shè)定相互正交的X軸 以及Y軸。另一方面，作為圖像38上的坐標(biāo)，將水平方向以及垂直方向分別設(shè)定為V軸以及U 軸。
[0040] 圖6 (b)表示在從時(shí)刻t經(jīng)過了時(shí)間△ t的時(shí)刻（t+ △ t)獲取的第2幀。圖5的攝像機(jī) 12'表示拍攝第2幀38'時(shí)的攝像機(jī)的位置。如圖5或者圖6(b)所示，在第2幀38'中，攝像機(jī) 12'拍攝特征點(diǎn)1' (31丄2、1\33，特征點(diǎn)檢測(cè)單元23檢測(cè)特征點(diǎn)1'(31丄 2丄3。在該情況下，姿態(tài)變 化量計(jì)算單元24不僅可以由時(shí)刻t的各特征點(diǎn)Tel、Te2、Te3的相對(duì)位置(Xi，Yi，Zi )、各特征點(diǎn) 在第2幀38'上的位和攝像機(jī)12的攝像機(jī)模型，計(jì)算時(shí)間At的攝像機(jī)12的移動(dòng) 量（AL)，還可以計(jì)算距離以及姿態(tài)角的變化量。例如，通過解以下的（1)~(4)式構(gòu)成的聯(lián) 立方程式，姿態(tài)變化量計(jì)算單元24可以計(jì)算攝像機(jī)12(車輛）的移動(dòng)量（AL)以及距離以及 姿態(tài)角的變化量。而且，（1)式是攝像機(jī)12作為沒有失真和光軸偏差的理想的針孔攝像機(jī)而 模型化的式子，λ?是常數(shù)，f是焦點(diǎn)距離。攝像機(jī)模型的參數(shù)只要預(yù)先校準(zhǔn)即可。
[0041]
[0042] (xi-X2)2+(yi-y2)2+(zi-Z2)2 = li2 · · · (2)
[0043] ( X3-X2 )2+ (y312 )2+ ( Z3-Z2 )2 = 122…（3 )
[0044] (xi-X3)2+(yi-y3)2+(zi-Z3)2 = l32 · · · (4)
[0045] 圖3(b)示意地表示在攝像機(jī)12的攝像范圍中，從與被照射了圖案光32a的區(qū)域不 同的其它區(qū)域33檢測(cè)到的特征點(diǎn)的時(shí)間變化，求攝像機(jī)12的移動(dòng)方向34的狀況。而且，在圖 6(a)以及圖6(b)中，將表示各特征點(diǎn)T e的位置的變化方向以及變化量的向量Dte重疊在圖像 中表示。姿態(tài)變化量計(jì)算單元24不僅可以計(jì)算時(shí)間At中的攝像機(jī)12的移動(dòng)量（AL)，還可 以同時(shí)計(jì)算距離以及姿態(tài)角的變化量。由此，姿態(tài)變化量計(jì)算單元24考慮距離以及姿態(tài)角 的變化量，可以高精度地計(jì)算6自由度的移動(dòng)量（△ L)。即，即使由于車輛10的旋轉(zhuǎn)或加減速 導(dǎo)致的搖擺運(yùn)動(dòng)或者俯仰運(yùn)動(dòng)，距離或姿態(tài)角產(chǎn)生變化，也可以抑制移動(dòng)量（AL)的估計(jì)誤 差。
[0046] 而且，姿態(tài)變化量計(jì)算單元24也可以不使用全部算出相對(duì)位置的特征點(diǎn)，而根據(jù) 特征點(diǎn)之間的位置關(guān)系選定最佳的特征點(diǎn)。作為選定方法，例如，可以使用對(duì)極幾何(對(duì)極 極線幾何，R · I · Hart ley : "A linear method for reconstruction from lines and points，"Proc·5th International Conference on Computer Vi sion , Cambridge , Massachusetts,pp.882-887(1995))〇
[0047] 為了在前后幀之間將特征點(diǎn)相對(duì)應(yīng)，例如可以預(yù)先將檢測(cè)到的特征點(diǎn)的周邊的小 區(qū)域的圖像記錄在存儲(chǔ)器中，由亮度或顏色信息的類似度進(jìn)行判斷。具體地說，ECU13在存 儲(chǔ)器中記錄以檢測(cè)到的特征點(diǎn)為中心的相當(dāng)于5X5(水平X垂直)像素的圖像。例如，若亮 度信息在20像素以上、收斂在誤差1%以下，則姿態(tài)變化量計(jì)算單元24判斷為在前后幀之間 取得對(duì)應(yīng)關(guān)系的特征點(diǎn)。
[0048]這樣，在從以后的定時(shí)獲取的圖像38'也檢測(cè)到算出了相對(duì)位置(XnYnZO的特征 點(diǎn)Tel、Te2、Te3的情況下，姿態(tài)變化量計(jì)算單元24可以根據(jù)路面上的多個(gè)特征點(diǎn)的時(shí)間變 化，計(jì)算"車輛的姿態(tài)變化量"。
[0049]自身位置計(jì)算單元26根據(jù)由姿態(tài)變化量計(jì)算單元24算出的"距離以及姿態(tài)角的變 化量"計(jì)算距離以及姿態(tài)角。進(jìn)而，由姿態(tài)變化量計(jì)算單元24算出"車輛的移動(dòng)量"計(jì)算車輛 的當(dāng)前位置。
[0050] 具體地說，在設(shè)定由姿態(tài)角計(jì)算單元22(圖1參照)算出的距離以及姿態(tài)角作為起 點(diǎn)的情況下，通過對(duì)該起點(diǎn)（距離以及姿態(tài)角），逐次加上姿態(tài)變化量計(jì)算單元24中算出的 各幀的每一個(gè)的距離以及姿態(tài)角的變化量(進(jìn)行積分運(yùn)算），將距離以及姿態(tài)角更新為最新 的數(shù)值。而且，設(shè)定由姿態(tài)角計(jì)算單元22算出了距離以及姿態(tài)角時(shí)的車輛位置作為起點(diǎn)（車 輛的初始位置），通過從該初始位置逐次加上車輛的移動(dòng)量(進(jìn)行積分運(yùn)算），計(jì)算車輛的當(dāng) 前位置。例如，通過設(shè)定與地圖上的位置核對(duì)后的起點(diǎn)(車輛的初始位置），可以逐次計(jì)算地 圖上的車輛的當(dāng)前位置。
[0051] 這樣，只要可以繼續(xù)檢測(cè)在前后幀間取得對(duì)應(yīng)關(guān)系的3點(diǎn)以上的特征點(diǎn)，則可以通 過繼續(xù)加上距離以及姿態(tài)角的變化量的處理(積分運(yùn)算），不使用圖案光32a地繼續(xù)將距離 或姿態(tài)角更新為最新的數(shù)值。其中，在最初的信息處理循環(huán)中，也可以使用利用圖案光32a 算出的距離以及姿態(tài)角，或者規(guī)定的初始距離以及初始姿態(tài)角。即，成為積分運(yùn)算的起點(diǎn)的 距離以及姿態(tài)角既可以使用圖案光32a計(jì)算，或者，也可以使用規(guī)定的初始值。優(yōu)選規(guī)定的 初始距離以及初始姿態(tài)角是至少考慮了至車輛10的乘員以及裝載物的距離以及姿態(tài)角。例 如，在車輛10的點(diǎn)火開關(guān)為接通狀態(tài)，并且檔位從泊車檔移動(dòng)到了其它的檔位時(shí)，投光圖案 光32a，并使用從圖案光32a算出的距離以及姿態(tài)角作為規(guī)定的初始位置以及初始姿態(tài)角即 可。由此，可以求出在沒有發(fā)生車輛10的旋轉(zhuǎn)或加減速導(dǎo)致的搖擺（roll)運(yùn)動(dòng)或者俯仰 (pitch)運(yùn)動(dòng)時(shí)的距離和姿態(tài)角。
[0052] 而且，在實(shí)施方式中，通過計(jì)算距離以及姿態(tài)角的變化量，并且逐次加上距離以及 姿態(tài)角的變化量，將距離以及姿態(tài)角更新為最新的數(shù)值。但是，也可以僅將攝像機(jī)12對(duì)于路 面31的姿態(tài)角作為其變化量的計(jì)算以及更新的對(duì)象。在該情況下，只要攝像機(jī)12對(duì)于路面 31的距離假定為固定即可。由此，可以考慮姿態(tài)角的變化量，抑制移動(dòng)量（AL)的估計(jì)誤差， 同時(shí)輕減E⑶13的運(yùn)算負(fù)載，并且提高運(yùn)算速度。
[0053]檢測(cè)狀態(tài)判斷單元30判斷特征點(diǎn)檢測(cè)單元23的特征點(diǎn)Te的檢測(cè)狀態(tài)是否為不滿 足第1基準(zhǔn)的較差的狀態(tài)。例如，隧道內(nèi)的混凝土路面那樣，在路面上瀝青混合物的顆粒等 的花紋或凹凸較少的情況下，可從圖像檢測(cè)的特征點(diǎn)變少。在該情況下，在前后幀間取得對(duì) 應(yīng)關(guān)系的特征點(diǎn)的連續(xù)性檢測(cè)變得困難，距離以及姿態(tài)角的更新精度降低。
[0054]因此，例如，在算出對(duì)于攝像機(jī)12的相對(duì)位置的特征點(diǎn)中，在從之后的信息處理循 環(huán)中獲取的圖像中也檢測(cè)出的特征點(diǎn)的數(shù)為規(guī)定的閾值(例如，三個(gè)）以下的情況下，檢測(cè) 狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。即，如圖7所示， 在前后幀間不能檢測(cè)出取得對(duì)應(yīng)的4點(diǎn)以上的特征點(diǎn)的情況下，檢測(cè)狀態(tài)判斷單元30判斷 為特征點(diǎn)T e的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。而且，如圖6所示，為了求距離和姿 態(tài)角的變化量，需要在前后幀間取得對(duì)應(yīng)的至少三個(gè)特征點(diǎn)。這是因?yàn)?，為了確定平面G而 需要三個(gè)特征點(diǎn)。由于為了提高估計(jì)精度而需要更多的特征點(diǎn)，所以優(yōu)選規(guī)定的閾值設(shè)為 4、或者5以上。
[0055]在檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)滿足第1基準(zhǔn)的情況下，自身位 置計(jì)算單元26維持積分運(yùn)算的起點(diǎn)。另一方面，在檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢 測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)的情況下，自身位置計(jì)算單元26在相同的信息處理循 環(huán)中，將姿態(tài)角計(jì)算單元22(參照?qǐng)D1)算出的距離以及姿態(tài)角，以及那時(shí)的車輛位置設(shè)定為 新的起點(diǎn)(車輛的姿態(tài)角以及初始位置），從該起點(diǎn)起開始車輛的姿態(tài)變化量的相加。
[0056]而且，在第1實(shí)施方式中，檢測(cè)狀態(tài)判斷單元30根據(jù)在前后幀間取得對(duì)應(yīng)關(guān)系的特 征點(diǎn)的數(shù)，判斷了特征點(diǎn)的檢測(cè)狀態(tài)，但是也可以根據(jù)從一個(gè)圖像檢測(cè)出的特征點(diǎn)的總數(shù) N，判斷特征點(diǎn)的檢測(cè)狀態(tài)。即，也可以在在特征點(diǎn)的總數(shù)N為規(guī)定的閾值(例如9)以下的情 況下，判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。只要考慮在檢測(cè)出的特征 點(diǎn)中存在不能對(duì)應(yīng)的特征點(diǎn)的情況，將規(guī)定的閾值(3)的3倍的數(shù)(9)設(shè)定為閾值即可。 [0057]計(jì)算狀態(tài)判斷單元35判斷姿態(tài)角計(jì)算單元22的距離以及姿態(tài)角的計(jì)算狀態(tài)是否 不滿足第2基準(zhǔn)，為較差的狀態(tài)。例如，路面31上的臺(tái)階被照射了圖案光的情況下，因?yàn)槁访?31上的臺(tái)階比瀝青的凹凸大，所以距離以及姿態(tài)角的計(jì)算精度極端地降低。在特征點(diǎn)的檢 測(cè)狀態(tài)比第1基準(zhǔn)差，并且距離以及姿態(tài)角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)，為較差的狀態(tài)的情 況下，高精度地檢測(cè)距離以及姿態(tài)角，以及其變化量的手段就沒有了。
[0058]因此，例如，在通過姿態(tài)角計(jì)算單元22算出的距離以及姿態(tài)角的標(biāo)準(zhǔn)偏差大于規(guī) 定的閾值的情況下，計(jì)算狀態(tài)判斷單元35判斷為姿態(tài)角計(jì)算單元22的距離以及姿態(tài)角的計(jì) 算狀態(tài)不滿足第2基準(zhǔn)，為較差的狀態(tài)。而且，在35個(gè)亮點(diǎn)光中檢測(cè)出的亮點(diǎn)光的數(shù)不足3點(diǎn) 的情況下，由于在原理上不能求出路面31的平面式，所以判斷為姿態(tài)角計(jì)算單元22的距離 以及姿態(tài)角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)，為較差的狀態(tài)。在使用最小二乘法求出平面式的情 況下，也可以在與各亮點(diǎn)光的距離誤差的最大值的絕對(duì)值為某個(gè)閾值(例如0.05m)以上的 情況下，判斷為姿態(tài)角計(jì)算單元22的距離以及姿態(tài)角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)，為較差的 狀態(tài)。
[0059] 在檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)， 并且在計(jì)算狀態(tài)判斷單元35判斷為姿態(tài)角計(jì)算單元22的距離以及姿態(tài)角的計(jì)算狀態(tài)不滿 足第2基準(zhǔn)，為較差的狀態(tài)的情況下，自身位置計(jì)算單元26使用前一個(gè)信息處理循環(huán)中的距 離以及姿態(tài)角，以及車輛的當(dāng)前位置作為積分運(yùn)算的起點(diǎn)。由此，可以抑制車輛的移動(dòng)量的 計(jì)算誤差。
[0060] 圖案光控制單元27控制投光器11的圖案光32a的投光。例如，在車輛10的點(diǎn)火開關(guān) 為接通狀態(tài)，與自身位置計(jì)算裝置起動(dòng)同時(shí)，圖案光控制單元27開始圖案光32a的投光。之 后，圖案光控制單元27連續(xù)投光圖案光32a，直至自身位置計(jì)算裝置停止?；蛘撸部梢愿糸_ 規(guī)定的時(shí)間間隔，反復(fù)投光的接通/關(guān)斷?；蛘?，也可以只有在檢測(cè)狀態(tài)判斷單元30判定為 特征點(diǎn)I的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)的情況下，暫時(shí)地投光圖案光32a。
[0061 ](信息處理循環(huán)）
[0062]接著，作為由攝像機(jī)12獲取的圖像38估計(jì)車輛10的移動(dòng)量的自身位置計(jì)算方法的 一例，參照?qǐng)D8以及圖9說明通過ECU13反復(fù)執(zhí)行的信息處理循環(huán)。圖8的流程圖所示的信息 處理循環(huán)，在車輛10的點(diǎn)火開關(guān)為接通狀態(tài)，自身位置計(jì)算裝置起動(dòng)的同時(shí)開始，直至自身 位置計(jì)算裝置停止，被反復(fù)執(zhí)行。
[0063]在圖8的步驟S01中，圖案光控制單元27控制投光器11，對(duì)車輛周圍的路面31投光 圖案光32a。在圖8的流程圖中，說明連續(xù)投光圖案光32a的例子。
[0064]進(jìn)至步驟S03，ECU13控制攝像機(jī)12,拍攝包含被投光了圖案光32a的區(qū)域的車輛周 圍的路面31，獲取圖像38』〇]13將由攝像機(jī)12獲取的圖像數(shù)據(jù)存儲(chǔ)在存儲(chǔ)器中。
[0065]而且，E⑶13可以自動(dòng)控制攝像機(jī)12的光圈。也可以由前一個(gè)信息處理循環(huán)中獲取 的圖像38的平均亮度反饋控制攝像機(jī)12的光圈，以使其成為亮度值的最大值和最小值的中 間值。而且，因?yàn)橥豆饬藞D案光32a的區(qū)域亮度值高，所以也可以從除去了提取了圖案光32a 的部分的區(qū)域，求出平均亮度值。
[0066]進(jìn)至步驟S05,首先，圖案光提取單元21從存儲(chǔ)器讀入由攝像機(jī)12獲取的圖像38， 如圖4(c)所示，從圖像38提取圖案光32a的位置。圖案光提取單元21將作為表示圖案光32a 的位置的數(shù)據(jù)而算出的亮點(diǎn)光&在圖像上的坐標(biāo)(山，^存儲(chǔ)在存儲(chǔ)器中。
[0067]在步驟S05中，姿態(tài)角計(jì)算單元22從存儲(chǔ)器讀入表示圖案光32a的位置的數(shù)據(jù)，由 圖案光32a的位置計(jì)算距離以及姿態(tài)角，存儲(chǔ)在存儲(chǔ)器中。
[0068] 進(jìn)至步驟S07，ECU13從圖像38檢測(cè)特征點(diǎn)，提取在前后的信息處理循環(huán)之間取得 對(duì)應(yīng)關(guān)系的特征點(diǎn)，由特征點(diǎn)在圖像上的位置(UnVO計(jì)算距離以及姿態(tài)角的變化量，以及 車輛的移動(dòng)量。
[0069]具體地說，首先，特征點(diǎn)檢測(cè)單元23從存儲(chǔ)器讀入由攝像機(jī)12獲取的圖像38,從圖 像38檢測(cè)路面31上的特征點(diǎn)，將各特征點(diǎn)在圖像上的位置(Ui，Vi)存儲(chǔ)在存儲(chǔ)器中。姿態(tài)變 化量計(jì)算單元24從存儲(chǔ)器讀入各特征點(diǎn)在圖像上的位置(UnVO,由距離以及姿態(tài)角和特征 點(diǎn)在圖像上的位置(Un，計(jì)算特征點(diǎn)對(duì)于攝像機(jī)12的相對(duì)位置(H A)。而且，姿態(tài)變 化量計(jì)算單元24使用在前一個(gè)信息處理循環(huán)的步驟S09中設(shè)定的距離以及姿態(tài)角。姿態(tài)變 化量計(jì)算單元24將特征點(diǎn)對(duì)于攝像機(jī)12的相對(duì)位置(Xi，Yi，Zi)存儲(chǔ)在存儲(chǔ)器中。
[0070] 然后，姿態(tài)變化量計(jì)算單元24從存儲(chǔ)器讀入特征點(diǎn)在圖像上的位置(&，￥〇，&& 在前一個(gè)信息處理循環(huán)的步驟S07中算出的特征點(diǎn)的相對(duì)位置(Χη YnZO。姿態(tài)變化量計(jì)算 單元24使用在前后的信息處理循環(huán)之間取得對(duì)應(yīng)關(guān)系的特征點(diǎn)的相對(duì)位置(ΧηΥηΖΟ以及 圖像上的位置(UnVO,計(jì)算距離以及姿態(tài)角的變化量。進(jìn)而，由前一次信息處理循環(huán)中的特 征點(diǎn)的相對(duì)位置(Xi，Yi，Zi)和本次信息處理循環(huán)中的特征點(diǎn)的相對(duì)位置(Xi，Yi，Zi)，計(jì)算車 輛的移動(dòng)量。步驟S07中算出的"距離以及姿態(tài)角的變化量"以及"車輛的移動(dòng)量"，在步驟 S11的處理中被使用。
[0071] 進(jìn)至步驟S09，ECU13根據(jù)特征點(diǎn)的檢測(cè)狀態(tài)，以及圖案光的距離以及姿態(tài)角的計(jì) 算狀態(tài)，設(shè)定積分運(yùn)算的起點(diǎn)。細(xì)節(jié)參照?qǐng)D9,如后所述。
[0072] 進(jìn)至步驟S11，自身位置計(jì)算單元26由步驟S09的處理中設(shè)定的積分運(yùn)算的起點(diǎn)、 以及步驟S07的處理中算出的車輛的移動(dòng)量，計(jì)算車輛的當(dāng)前位置。
[0073]這樣，本實(shí)施方式的自身位置計(jì)算裝置通過反復(fù)執(zhí)行上述的一連串的信息處理循 環(huán)，累計(jì)車輛10的移動(dòng)量，可以計(jì)算車輛10的當(dāng)前位置。
[0074] 參照?qǐng)D9的流程圖，說明圖8的步驟S09的詳細(xì)的步驟。在步驟S900中，ECU13判斷本 次信息處理循環(huán)是否為初次。然后，在為初次的情況下，即，在沒有前一次信息處理循環(huán)的 數(shù)據(jù)的情況下，將處理進(jìn)至步驟S905,在不是初次的情況下，將處理進(jìn)至步驟S901。
[0075] 在步驟S901中，檢測(cè)狀態(tài)判斷單元30判斷是否特征點(diǎn)檢測(cè)單元23的特征點(diǎn)Te的檢 測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。在判斷為較差的情況下（步驟S901中"是"），進(jìn)至步 驟S903,在判斷為未變差的情況下（步驟S901中"否"），進(jìn)至步驟S909。
[0076]在步驟S909中，E⑶13維持當(dāng)前設(shè)定的積分運(yùn)算的起點(diǎn)。
[0077] 在步驟S903中，計(jì)算狀態(tài)判斷單元35判斷是否姿態(tài)角計(jì)算單元22的距離以及姿態(tài) 角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)，為較差的狀態(tài)。例如，判斷在距離以及姿態(tài)角的計(jì)算上是否 成功。在判斷為成功的情況下(步驟S903中"是"），進(jìn)至步驟S905,在判斷為不成功的情況下 (步驟S903中"否"），進(jìn)至步驟S907。
[0078]在步驟S905中，ECU13將車輛的當(dāng)前位置設(shè)定作為起點(diǎn)，進(jìn)而，將在相同的信息處 理循環(huán)的步驟S05中算出的距離以及姿態(tài)角設(shè)定作為積分運(yùn)算的起點(diǎn)。開始以該距離以及 姿態(tài)角作為起點(diǎn)的新的積分運(yùn)算。而且，以車輛的當(dāng)前位置作為起點(diǎn)，重新開始車輛的移動(dòng) 量的積分運(yùn)算。
[0079]在步驟S907中，E⑶13設(shè)定車輛的當(dāng)前位置作為起點(diǎn)，進(jìn)而，設(shè)定在前一次信息處 理循環(huán)中采用的距離以及姿態(tài)角作為積分運(yùn)算的起點(diǎn)。以該距離以及姿態(tài)角作為起點(diǎn)開始 新的積分運(yùn)算。而且，以車輛的當(dāng)前位置作為起點(diǎn)，重新開始車輛的移動(dòng)量的積分運(yùn)算。之 后，將處理進(jìn)至圖8的步驟S11。
[0080]如以上說明的那樣，按照第1實(shí)施方式，得到以下的作用效果。
[0081 ]因?yàn)樵谔卣鼽c(diǎn)的檢測(cè)狀態(tài)差時(shí)，姿態(tài)變化量計(jì)算單元24的距離以及姿態(tài)角的計(jì)算 精度降低，所以車輛10的移動(dòng)量的估計(jì)誤差變大。圖10(a)以及圖10(b)是表示車輛10的搖 擺角(姿態(tài)角的一例）以及移動(dòng)量(車寬方向）的估計(jì)誤差的例子的曲線圖，表示車輛10在無 傾斜的平坦的路面上以固定的速度直行的情況的、搖擺角的估計(jì)值以及移動(dòng)量的計(jì)算值的 時(shí)間變化。圖中的"Pi"以及"p2"與特征點(diǎn)的檢測(cè)狀態(tài)無關(guān)，表示維持移動(dòng)量算出的起點(diǎn)，以 及距離以及姿態(tài)角的積分運(yùn)算的起點(diǎn)的比較例。"Qi"以及"Q 2"表示真值。因?yàn)檐囕v10在直 行，所以搖擺角以及車寬方向的移動(dòng)量的真值(Qi，Q2)仍為0。但是，在時(shí)刻t~t2期間，在特 征點(diǎn)的檢測(cè)狀態(tài)差的混凝土路面行駛，所以在路面的平面式上產(chǎn)生誤差，在搖擺角上出現(xiàn) 估計(jì)誤差。搖擺角的誤差被反映作為車寬方向的移動(dòng)量的誤差。比較例根據(jù)特征點(diǎn)的檢測(cè) 狀態(tài)，不重置移動(dòng)量算出的起點(diǎn)，以及搖擺角的積分運(yùn)算的起點(diǎn)。因此，車輛10的移動(dòng)量的 誤差擴(kuò)大。
[0082] 按照第1實(shí)施方式，在特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)的情況下， 可以將由圖案光32a算出的誤差小的距離以及姿態(tài)角作為起點(diǎn)，重新開始積分運(yùn)算。由此， 可以高精度并且穩(wěn)定地估計(jì)車輛10的當(dāng)前位置。
[0083] 而且，在特征點(diǎn)的檢測(cè)狀態(tài)差的路面中，例如，包含成為特征點(diǎn)的對(duì)象的花紋或凹 凸少的路面。因?yàn)榛y或凹凸少的路面的平坦性高，所以由圖案光32a算出的距離以及姿態(tài) 角中包含的干擾成分少。另一方面，在特征點(diǎn)的檢測(cè)狀態(tài)好的路面31中，因?yàn)橛捎跒r青的凹 凸，圖案光的反射位置不穩(wěn)定，所以由圖案光32a算出的距離以及姿態(tài)角包含較多的干擾成 分。按照第1實(shí)施方式，根據(jù)特征點(diǎn)的檢測(cè)狀態(tài)，適當(dāng)?shù)胤珠_使用利用了特征點(diǎn)的距離以及 姿態(tài)角的更新、和利用了圖案光32a的距離以及姿態(tài)角的計(jì)算，可以高精度并且穩(wěn)定地求出 距離以及姿態(tài)角。
[0084]進(jìn)而，存在由于路面31的照度急劇地變化，圖像38的對(duì)比度降低，特征點(diǎn)的檢測(cè)狀 態(tài)差的情況。按照第1實(shí)施方式，通過直接判斷特征點(diǎn)的檢測(cè)狀態(tài)，不需要另外設(shè)置照度計(jì) 等傳感器，成本被輕減，有助于裝置的輕量化。
[0085]在通過姿態(tài)變化量計(jì)算單元24取得了對(duì)應(yīng)的特征點(diǎn)的數(shù)為規(guī)定的閾值以下的情 況下，檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)(步驟 S901中"是"）。在取得對(duì)應(yīng)的特征點(diǎn)的數(shù)減少時(shí)，距離以及姿態(tài)角的更新精度降低。在該情 況下，通過將移動(dòng)量算出的起點(diǎn)重置為由圖案光32a算出的距離以及姿態(tài)角，可以抑制距離 以及姿態(tài)角的更新精度、以及車輛10的移動(dòng)量的計(jì)算精度的降低。
[0086]在特征點(diǎn)的檢測(cè)狀態(tài)比第1基準(zhǔn)差（S901中"是"），并且距離以及姿態(tài)角的計(jì)算狀 態(tài)不滿足第2基準(zhǔn)，為較差的狀態(tài)的情況下(步驟S903中"否"），將移動(dòng)量算出的起點(diǎn)重置為 前一個(gè)信息處理循環(huán)中的距離以及姿態(tài)角。即，使用檢測(cè)狀態(tài)判斷單元30判斷為多個(gè)特征 點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)之前的自身位置計(jì)算單元26算出的距離以及 姿態(tài)角。由此，可以抑制車輛10的移動(dòng)量的估計(jì)誤差。
[0087](第1變形例）
[0088]檢測(cè)狀態(tài)判斷單元30也可以取代特征點(diǎn)的數(shù)而根據(jù)特征點(diǎn)的分布，判斷特征點(diǎn)I 的檢測(cè)狀態(tài)。例如，也可以在由圖像38檢測(cè)的特征點(diǎn)的密度為規(guī)定的閾值密度以下的情況 下，檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。如果特 征點(diǎn)的密度降低，則特征點(diǎn)的總數(shù)N也減少，所以取得對(duì)應(yīng)關(guān)系的特征點(diǎn)的數(shù)也減少?；蛘?， 也可以在從將圖像38均等地分割的各區(qū)域未檢測(cè)到特征點(diǎn)的情況下，檢測(cè)狀態(tài)判斷單元30 判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。例如，只要判斷是否由將圖像38 進(jìn)行4等分而從各區(qū)域檢測(cè)到即可。
[0089] 通過檢測(cè)狀態(tài)判斷單元30根據(jù)由圖像38檢測(cè)到的特征點(diǎn)的密度判斷特征點(diǎn)的檢 測(cè)狀態(tài)，可以抑制距離以及姿態(tài)角的更新精度、以及車輛10的移動(dòng)量的計(jì)算精度的降低。
[0090] 而且，進(jìn)而如果檢測(cè)到的特征點(diǎn)的密度低，則特征點(diǎn)間的距離遠(yuǎn)，由特征點(diǎn)構(gòu)成的 平面與路面31偏離的可能性高，難以高精度地計(jì)算距離以及姿態(tài)角。這是因?yàn)?，一般來說， 鋪裝路面為了排水而設(shè)置傾斜(傾斜），所以不平坦。而且，所說的可檢測(cè)的特征點(diǎn)的密度低 的情況，如上述那樣，也有路面31的凹凸或花紋少的情況。由此，在該情況下，從圖案光32a 算出的距離以及姿態(tài)角中包含的干擾成分少。由此，根據(jù)特征點(diǎn)的檢測(cè)狀態(tài)，適當(dāng)?shù)胤珠_使 用利用了特征點(diǎn)的距離以及姿態(tài)角的更新、以及利用了圖案光32a的距離以及姿態(tài)角的計(jì) 算，可以高精度地并且穩(wěn)定地求距離以及姿態(tài)角。
[0091] (第2變形例）
[0092]進(jìn)而，檢測(cè)狀態(tài)判斷單元30也可以根據(jù)由攝像機(jī)12獲取的圖像38的亮度平均值， 判斷特征點(diǎn)的檢測(cè)狀態(tài)。例如，在圖像38的亮度平均值大于上限值Bmax，或者小于下限值Bmin 的情況下，檢測(cè)狀態(tài)判斷單元30也可以判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的 狀態(tài)。即，在很亮或者很暗的地方判斷為特征點(diǎn)的檢測(cè)狀態(tài)差。例如，作為上限值B max以及下 限值Bmin，分別設(shè)定200、50即可。
[0093](第3變形例）
[0094] 在圖9的步驟S907中，E⑶13也可以取代在前一次信息處理循環(huán)中采用的距離以及 姿態(tài)角，將規(guī)定的初始距離以及初始姿態(tài)角設(shè)定作為積分運(yùn)算的起點(diǎn)。詳細(xì)地說，在檢測(cè)狀 態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)，并且計(jì)算狀態(tài)判斷 單元35判斷為姿態(tài)角計(jì)算單元22的距離以及姿態(tài)角的計(jì)算狀態(tài)與第2基準(zhǔn)相比也不滿足， 為較差的狀態(tài)的情況下，自身位置計(jì)算單元26也可以設(shè)定至少考慮了至車輛10的乘員以及 裝載物的規(guī)定的初始距離以及初始姿態(tài)角，作為積分運(yùn)算的起點(diǎn)。例如，可以使用起動(dòng)了自 身位置計(jì)算裝置緊后的信息處理循環(huán)的步驟S05中算出的距離以及姿態(tài)角。由此，可以將不 發(fā)生車輛10的旋轉(zhuǎn)或加減速導(dǎo)致的搖擺運(yùn)動(dòng)或者俯仰運(yùn)動(dòng)時(shí)的距離或姿態(tài)角作為起點(diǎn)，進(jìn) 行距離以及姿態(tài)角的更新以及移動(dòng)量的計(jì)算。
[0095](第2實(shí)施方式）
[0096]如圖11所示，與圖1相比，第2實(shí)施方式的自身位置計(jì)算裝置在ECU13還構(gòu)成平面計(jì) 算單元36這一點(diǎn)有所不同。平面計(jì)算單元36由多個(gè)特征點(diǎn)的相對(duì)位置(XnY^ZD計(jì)算包含 圖6所示的特征點(diǎn)的平面G。在平面計(jì)算單元36進(jìn)行的平面G的計(jì)算精度為規(guī)定的 閾值精度以下的情況下，檢測(cè)狀態(tài)判斷單元30判斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為 較差的狀態(tài)。其它的結(jié)構(gòu)與第1實(shí)施方式相同，省略說明。
[0097]例如，平面計(jì)算單元36算出相對(duì)位置(XnYnZO的特征點(diǎn)在一條直線上的情況下， 判斷為平面G的計(jì)算精度為規(guī)定的閾值精度以下。具體地說，使用霍夫變換計(jì)算各特征點(diǎn)的 傾斜和離平面G的中心的距離的分布。在檢測(cè)出的特征點(diǎn)中的90%的特征點(diǎn)在從某個(gè)角度 起一5度(deg)以上、+5度(deg)以下的范圍、并且離平面G的中心的距離為一 10像素以上、+ 10像素以下的范圍內(nèi)的情況下，判斷為特征點(diǎn)在一條直線上。而且，進(jìn)行該霍夫變換時(shí)的坐 標(biāo)，使用圖像上的位置(UuVO。
[0098]在存在路面31上涂畫的、包含人行橫道以及分區(qū)線的道路標(biāo)識(shí)時(shí)，有檢測(cè)到的特 征點(diǎn)偏向確定的帶狀或者線狀分布的情況。在該情況下，特征點(diǎn)排列在一條直線上的可能 性高，特征點(diǎn)形成的平面G的計(jì)算精度降低。這不能從特征點(diǎn)的數(shù)目或密度來判斷。按照第2 實(shí)施方式，可以根據(jù)特征點(diǎn)形成的平面G的計(jì)算精度，適當(dāng)?shù)嘏袛嗵卣鼽c(diǎn)的檢測(cè)狀態(tài)。
[0099](第3實(shí)施方式）
[0100]如圖12所示，與圖1相比，在作為檢測(cè)車輛周圍的照度的變化量的照度檢測(cè)單元的 一例，第3實(shí)施方式的自身位置計(jì)算裝置還包括照度傳感器37這一點(diǎn)上有所不同。在由照度 傳感器37檢測(cè)到的照度的變化量為規(guī)定的閾值照度以上的情況下，檢測(cè)狀態(tài)判斷單元30判 斷為特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，為較差的狀態(tài)。其它的結(jié)構(gòu)與第1實(shí)施方式相同，省 略說明。
[0101] 在攝像機(jī)12的光圈以及ISO靈敏度的自動(dòng)調(diào)整不能追隨車輛周圍的照度的急劇變 化的情況下，在由攝像機(jī)12獲取的圖像中，產(chǎn)生全白或者赤褐色的現(xiàn)象。在該情況下，難以 從圖像檢測(cè)特征點(diǎn)，特征點(diǎn)的檢測(cè)狀態(tài)惡化。因此，將照度傳感器37與攝像機(jī)12以及投光器 11同樣地安裝在車輛的前方，根據(jù)照度傳感器37檢測(cè)到的照度變化量，判斷特征點(diǎn)的檢測(cè) 狀態(tài)。事先檢知不能通過攝像機(jī)12檢測(cè)特征點(diǎn)的情況，由對(duì)路面31投光的圖案光32a計(jì)算距 離和姿態(tài)角。由此，可以適當(dāng)?shù)嘏袛嗵卣鼽c(diǎn)的檢測(cè)狀態(tài)。
[0102] 如上述那樣記載了本發(fā)明的第1至第3實(shí)施方式，成為本公開的一部分的論述以及 附圖不應(yīng)理解為限定本發(fā)明。由本公開，本領(lǐng)域的技術(shù)人員當(dāng)然明白各種代替實(shí)施的方式、 實(shí)施例以及運(yùn)用技術(shù)。
[0103]而且，圖2表示了在車輛10的前面安裝了攝像機(jī)12和投光器11的例子，但是也可以 朝向車輛10的側(cè)方、后方、正下方設(shè)置。而且，在本實(shí)施方式中作為車輛10的一個(gè)例子，在圖 2中圖示了四輪的乘用汽車，但能夠適用于全部摩托車、貨車、或者例如運(yùn)送建筑機(jī)械的特 殊車輛等，能夠拍攝道路的路面或者墻面上的特征點(diǎn)的移動(dòng)體(車輛）。
[0104] 標(biāo)號(hào)說明
[0105] 10 車輛
[0106] 11投光器
[0107] 12攝像機(jī)(攝像單元）
[0108] 21圖案光提取單元
[0109] 22姿態(tài)角計(jì)算單元
[0110] 23特征點(diǎn)檢測(cè)單元
[0111] 24姿態(tài)變化量計(jì)算單元
[0112] 26自身位置計(jì)算單元
[0113] 30檢測(cè)狀態(tài)判斷單元
[0114] 31 路面
[0115] 32a，32b 圖案光
[0116] 35計(jì)算狀態(tài)判斷單元
[0117] 36平面計(jì)算單元
[0118] 37照度傳感器(照度檢測(cè)單元）
[0119] Te特征點(diǎn)
【主權(quán)項(xiàng)】
1. 一種自身位置計(jì)算裝置，其特征在于，包括： 投光器，對(duì)車輛周圍的路面投光圖案光； 攝像單元，安裝在所述車輛上，拍攝包含了被投光所述圖案光的區(qū)域的車輛周圍的路 面，獲取圖像； 姿態(tài)角計(jì)算單元，由通過所述攝像單元獲取的圖像中的所述圖案光的位置，計(jì)算車輛 對(duì)于所述路面的姿態(tài)角； 姿態(tài)變化量計(jì)算單元，根據(jù)由通過所述攝像單元獲取的圖像檢測(cè)到的所述路面上的多 個(gè)特征點(diǎn)的時(shí)間變化，計(jì)算所述車輛的姿態(tài)變化量； 自身位置計(jì)算單元，通過在所述車輛的初始位置以及姿態(tài)角上不斷加上所述姿態(tài)變化 量，計(jì)算所述車輛的當(dāng)前位置以及姿態(tài)角；以及 檢測(cè)狀態(tài)判斷單元，判斷所述姿態(tài)角變化量計(jì)算單元中的所述多個(gè)特征點(diǎn)的檢測(cè)狀 態(tài)， 在所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)的情況下，所述自身位置計(jì)算單元將此 時(shí)的所述車輛的當(dāng)前位置以及所述姿態(tài)角計(jì)算單元的車輛的姿態(tài)角，設(shè)定為所述車輛的初 始位置以及姿態(tài)角，開始所述姿態(tài)變化量的相加。2. 如權(quán)利要求1所述的自身位置計(jì)算裝置，其特征在于， 在所述姿態(tài)變化量計(jì)算單元中的所述多個(gè)特征點(diǎn)的數(shù)為規(guī)定的閾值以下的情況下，所 述檢測(cè)狀態(tài)判斷單元判斷為不滿足第1基準(zhǔn)。3. 如權(quán)利要求1所述的自身位置計(jì)算裝置，其特征在于， 在由所述圖像檢測(cè)到的所述特征點(diǎn)的密度為規(guī)定的閾值密度以下的情況下，所述檢測(cè) 狀態(tài)判斷單元判斷為不滿足第1基準(zhǔn)。4. 如權(quán)利要求1所述的自身位置計(jì)算裝置，其特征在于， 還包括:平面計(jì)算單元，由從所述圖像檢測(cè)到的所述多個(gè)特征點(diǎn)的相對(duì)位置，計(jì)算包含 所述多個(gè)特征點(diǎn)的平面， 在所述平面計(jì)算單元的所述平面的計(jì)算精度為規(guī)定的閾值精度以下的情況下，所述檢 測(cè)狀態(tài)判斷單元判斷為不滿足第1基準(zhǔn)。5. 如權(quán)利要求1所述的自身位置計(jì)算裝置，其特征在于， 還包括:照度檢測(cè)單元，檢測(cè)車輛周圍的照度的變化量， 在由所述照度檢測(cè)單元檢測(cè)到的所述照度的變化量為規(guī)定的閾值照度以上的情況下， 所述檢測(cè)狀態(tài)判斷單元判斷為不滿足第1基準(zhǔn)。6. 如權(quán)利要求1至5的任意一項(xiàng)所述的自身位置計(jì)算裝置，其特征在于， 還包括:計(jì)算狀態(tài)判斷單元，判斷所述姿態(tài)角計(jì)算單元的所述姿態(tài)角的計(jì)算狀態(tài)， 在所述檢測(cè)狀態(tài)判斷單元判斷為所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，并且所 述計(jì)算狀態(tài)判斷單元判斷為所述姿態(tài)角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)的情況下，所述自身位 置計(jì)算單元對(duì)于至少考慮了至所述車輛的乘員以及裝載物的規(guī)定的初始姿態(tài)角，開始所述 姿態(tài)變化量的相加。7. 如權(quán)利要求1至5的任意一項(xiàng)所述的自身位置計(jì)算裝置，其特征在于， 還包括:計(jì)算狀態(tài)判斷單元，判斷所述姿態(tài)角計(jì)算單元的所述姿態(tài)角的計(jì)算狀態(tài)， 在所述檢測(cè)狀態(tài)判斷單元判斷為所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)，并且所 述計(jì)算狀態(tài)判斷單元判斷為所述姿態(tài)角的計(jì)算狀態(tài)不滿足第2基準(zhǔn)的情況下，所述自身位 置計(jì)算單元對(duì)于所述檢測(cè)狀態(tài)判斷單元判斷為所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn) 之前算出的姿態(tài)角，開始所述姿態(tài)變化量的相加。8. -種自身位置計(jì)算方法，其特征在于，包括： 對(duì)車輛周圍的路面投光圖案光的步驟； 拍攝包含被投光了所述圖案光的區(qū)域的車輛周圍的路面，獲取圖像的步驟； 由所述圖像中的所述圖案光的位置，計(jì)算車輛對(duì)于所述路面的姿態(tài)角的步驟； 根據(jù)由所述圖像檢測(cè)到的所述路面上的多個(gè)特征點(diǎn)的時(shí)間變化，計(jì)算所述車輛的姿態(tài) 變化量的姿態(tài)角變化量計(jì)算步驟； 通過對(duì)所述車輛的初始位置以及姿態(tài)角，不斷加上所述姿態(tài)變化量，計(jì)算所述車輛的 當(dāng)前位置以及姿態(tài)角的自身位置計(jì)算步驟;以及 判斷所述姿態(tài)角變化量計(jì)算步驟中的所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)的步驟， 在所述自身位置計(jì)算步驟中，在所述多個(gè)特征點(diǎn)的檢測(cè)狀態(tài)不滿足第1基準(zhǔn)的情況下， 將此時(shí)的所述車輛的當(dāng)前位置以及由所述圖案光的位置算出的車輛的姿態(tài)角，設(shè)定為所述 車輛的初始位置以及姿態(tài)角，開始所述姿態(tài)變化量的相加。
【文檔編號(hào)】G01C21/28GK105981086SQ201480074969
【公開日】2016年9月28日
【申請(qǐng)日】2014年2月24日
【發(fā)明人】山口郎, 山口一郎, 西內(nèi)秀和
【申請(qǐng)人】日產(chǎn)自動(dòng)車株式會(huì)社