自己位置計算裝置及自己位置計算方法
【專利摘要】自己位置計算裝置具備：向車輛周圍的路面(31)投射圖案光(32a)的投光器(11)、拍攝包含投射有圖案光(32a)的區(qū)域的車輛周圍的路面(31)并取得圖像(38)的攝像機(12)，根據(jù)由攝像機(12)取得的圖像(38)中的圖案光(32a)的位置，計算出車輛(10)相對于路面(31)的姿勢角，基于從圖像(38)檢測出的路面上的多個特征點的時間變化，計算出車輛的姿勢變化量。自己位置計算裝置通過車輛的初始位置及姿勢角加上姿勢變化量，計算出車輛的當前位置及姿勢角，且在多個特征點的檢測狀態(tài)不滿足第一基準的情況下，投射圖案光(32a)。
【專利說明】
自己位置計算裝置及自己位置計算方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及自己位置計算裝置及自己位置計算方法。
【背景技術(shù)】
[0002] 已知有如下的技術(shù)，即、取得由搭載于車輛的攝像機拍攝的車輛附近的圖像，并基 于圖像的變化求出車輛的移動量(參照專利文獻1)。專利文獻1中，為了在車輛的低速且細 微的移動中也高精度地求出移動量，從圖像中檢測特征點，求出圖像上的特征點的位置，并 根據(jù)特征點的移動方向及移動距離(移動量)求出車輛的移動量。
[0003] 現(xiàn)有技術(shù)文獻
[0004] 專利文獻
[0005] 專利文獻1:(日本)特開2008 -175717號公報
[0006]但是，在特征點的檢測狀態(tài)差的情況下，難以根據(jù)特征點的位置高精度地求出車 輛的移動量。

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

[0007] 本發(fā)明是鑒于上述課題而創(chuàng)立的，其目的在于，提供一種自己位置計算裝置及自 己位置計算方法，無論特征點的檢測狀態(tài)如何，均能夠高精度且穩(wěn)定地推定車輛的當前位 置。
[0008] 本發(fā)明的一方式的自己位置(本車輛位置local location)計算裝置具備：投光 器，其向車輛周圍的路面投射圖案光;攝像部，其拍攝包含投射圖案光的區(qū)域在內(nèi)的車輛周 圍的路面并取得圖像。自己位置計算裝置根據(jù)由攝像部取得的圖像中的圖案光的位置，計 算出車輛相對于路面的姿勢角，并基于根據(jù)圖像檢測出的路面上的多個特征點的時間變 化，計算出車輛的姿勢變化量。而且，通過在車輛的初始位置及姿勢角加上姿勢變化量，計 算出車輛的當前位置及姿勢角。自己位置計算裝置在多個特征點的檢測狀態(tài)不滿足第一基 準而差的狀態(tài)的情況下，投射圖案光。
【附圖說明】
[0009] 圖1是表示實施方式的自己位置計算裝置的整體構(gòu)成的塊圖；
[0010] 圖2是表示投光器11及攝像機12向車輛10的搭載例的外觀圖；
[0011] 圖3(a)是表示根據(jù)投光器11和攝像機12之間的基線長度Lb、各光點的圖像上的坐 標(UpVj)，計算出照射各光點的路面31上的位置的情形的圖，圖3(b)是表示根據(jù)從與照射 圖案光32a的區(qū)域不同的另一區(qū)域33檢測出的特征點的時間變化，求出攝像機12的移動方 向34的情形的不意圖；
[0012 ]圖4 (a)及圖4 (b)是表示對由攝像機12取得的圖像實施了二值化處理的圖案光32a 的圖像的圖，圖4(a)表不圖案光32a整體，圖4(b)表不擴大一個光點Sp，圖4(c)是表不由圖 案光抽取部21抽取的各光點S p的重心的位置凡的圖；
[0013] 圖5是用于說明計算距離及姿勢角的變化量及攝像機12的移動量（AL)的方法的 示意圖；
[0014] 圖6(a)表示在時刻t取得的第一幀（圖像)38的一例，圖6(b)表示在從時刻t經(jīng)過時 間At的時刻(t+At)取得的第二幀38' ；
[0015]圖7是表示使用了圖1的自己位置計算裝置的自己位置計算方法的一例的流程圖；
[0016] 圖8是表示圖7的步驟S05的詳細順序的流程圖；
[0017] 圖9是表示按照圖8的流程執(zhí)行的信息處理的一例的圖，圖9(a)表示姿勢角計算部 22是否根據(jù)圖案光32a計算出距離及姿勢角，圖9(b)表示圖案光投射實施標志的狀態(tài)，圖9 (c)表示取得對應的特征點的數(shù)量；
[0018] 圖10是表示圖7的步驟S09的詳細順序的流程圖；
[0019] 圖11(a)及圖11(b)是表示車輛10的側(cè)滾角及車寬方向移動量的推定誤差的例的 圖；
[0020] 圖12是表示第二實施方式的步驟S05的詳細順序的流程圖；
[0021] 圖13是表示按照圖12的流程執(zhí)行的信息處理的一例的圖，圖13(a)表示姿勢角計 算部22是否根據(jù)圖案光32a計算出距離及姿勢角，圖13(b)表示圖案光投射實施標志的狀 態(tài)，圖13 (c)表示取得對應的特征點的數(shù)量；
[0022]圖14是表示第三實施方式的步驟S05的詳細順序的流程圖；
[0023]圖15是表示按照圖14的流程執(zhí)行的信息處理的一例的圖，圖15(a)表示姿勢角計 算部22是否根據(jù)圖案光32a計算出距離及姿勢角，圖15(b)表示圖案光投射實施標志的狀 態(tài)，圖15 (c)表示取得對應的特征點的數(shù)量；
[0024]圖16是表示第四實施方式的步驟S05的詳細順序的流程圖；
[0025]圖17是表示按照圖16的流程執(zhí)行的信息處理的一例的圖，圖17(a)表示姿勢角計 算部22是否根據(jù)圖案光32a計算出距離及姿勢角，圖17(b)表示圖案光投射實施標志的狀 態(tài)，圖17 (c)表示取得對應的特征點的數(shù)量。
【具體實施方式】
[0026]參照【附圖說明】實施方式。附圖的記載中，對相同部分標注相同的符號并省略說明。 [0027](第一實施方式）
[0028](硬件構(gòu)成）
[0029]首先，參照圖1說明第一實施方式的自己位置計算裝置的硬件構(gòu)成。自己位置計算 裝置具備:投光器11、攝像機12、發(fā)動機控制單元(ECU)13。投光器11搭載于車輛上，向車輛 周圍的路面投射圖案光。攝像機12是搭載于車輛上而拍攝包含投射圖案光的區(qū)域在內(nèi)的車 輛周圍的路面并取得圖像的攝像部的一例。ECU13是控制投光器11且執(zhí)行根據(jù)由攝像機12 取得的圖像推定車輛的移動量的一連串的信息處理循環(huán)的控制部的一例。
[0030] 攝像機12是使用了固體攝像元件、例如CCD及CMOS的數(shù)字攝像機，取得可以進行圖 像處理的數(shù)字圖像。攝像機12的拍攝對象是車輛周圍的路面，車輛周圍的路面包含車輛的 前部、后部、側(cè)部、車輛底部的路面。例如，如圖2所示，攝像機12可以搭載于車輛10的前部， 具體而言，可以搭載于前保險杠上。
[0031] 調(diào)整設(shè)置攝像機12的高度及方向且自動調(diào)整攝像機12所具備的透鏡的焦點及光 圈，以可拍攝車輛10的前方路面31上的特征點（紋理)及由投光器11投射的圖案光32b。攝像 機12每隔規(guī)定的時間間隔進行重復拍攝，取得一連串的圖像(幀)組。由攝像機12取得的圖 像數(shù)據(jù)向E⑶13傳送，并存儲于E⑶13所具備的存儲器。
[0032]如圖2所示，投光器11向攝像機12的拍攝范圍內(nèi)的路面31，投射具有包含正方形或 長方形的光柵圖像的規(guī)定形狀的圖案光32b。攝像機12拍攝由路面31照射的圖案光。投光器 11具備例如激光指示器及衍射光柵。將從激光指示器射出的激光利用衍射光柵進行衍射， 由此，如圖2~圖4所示，投光器11生成光柵圖像或由行列狀地排列的多個光點心構(gòu)成的圖 案光(32b、32a)。圖3及圖4所示的例子中，生成由5X7的光點S p構(gòu)成的圖案光32a。
[0033] 返回圖1，ECU13由具備CPU、存儲器及輸入輸出部的微型控制器構(gòu)成，通過執(zhí)行預 先安裝的計算機程序，構(gòu)成自己位置計算裝置具備的多個信息處理部。ECU13對每個圖像 (幀）重復執(zhí)行根據(jù)由攝像機12取得的圖像計算出車輛的當前位置的一連串的信息處理循 環(huán)。E⑶13也可以兼用作車輛10的其它控制所使用的E⑶。
[0034]多個信息處理部包含：圖案光抽取部21、姿勢角計算部22、姿勢變化量計算部24、 自己位置計算部26、圖案光控制部27、檢測狀態(tài)判斷部30、計算狀態(tài)判斷部35。姿勢變化量 計算部24包含特征點檢測部23。
[0035] 圖案光抽取部21從存儲器讀取由攝像機12取得的圖像，根據(jù)圖像抽取圖案光的位 置。如圖3(a)所示，例如，投光器11向路面31投射由行列狀地排列的多個光點構(gòu)成的圖案光 32a，并利用攝像機12檢測被路面31反射的圖案光32a。圖案光抽取部21通過對由攝像機12 取得的圖像實施二值化處理，如圖4(a)及圖4(b)所示，僅抽取光點S p的圖像。如圖4(c)所 示，圖案光抽取部21通過計算出各光點Sp的重心的位置He，即光點心的圖像上的坐標(Uj、 V j)，而抽取圖案光32a的位置。坐標在以攝像機12的攝像元件的像素為單位且5X7的光點 Sp的情況下，"j"為1以上且35以下的自然數(shù)。光點&的圖像上的坐標㈨、％)作為表示圖案 光32a的位置的數(shù)據(jù)存儲于存儲器。
[0036] 姿勢角計算部22從存儲器讀取表示圖案光32a的位置的數(shù)據(jù)，根據(jù)由攝像機12取 得的圖像中的圖案光32a的位置，計算出車輛10相對于路面31的距離及姿勢角。例如，如圖3 (a)所示，根據(jù)投光器11和攝像機12之間的基線長度Lb、各光點的圖像上的坐標〇]>%)，使 用三角測量原理，將照射各光點的路面31上的位置作為相對于攝像機12的相對位置而計算 出。而且，姿勢角計算部22根據(jù)各光點相對于攝像機12的相對位置，計算出投射圖案光32a 的路面31的平面方程式即攝像機12相對于路面31的距離及姿勢角(法向矢量）。此外，攝像 機12相對于車輛10的搭載位置及拍攝方向已知，因此，實施方式中，作為車輛10相對于路面 31的距離及姿勢角的一例，計算出攝像機12相對于路面31的距離及姿勢角。以后，將攝像機 12相對于路面31的距離及姿勢角簡稱為"距離及姿勢角"。由姿勢角計算部22計算出的距離 及姿勢角存儲于存儲器。
[0037] 具體而言，攝像機12及投光器11分別固定于車輛10上，因此，圖案光32a的照射方 向、攝像機12和投光器11的距離(基線長度Lb)已知。因此，姿勢角計算部22可以使用三角測 量的原理，將從各光點的圖像上的坐標(UpV j)照射各光點的路面31上的位置作為相對于攝 像機12的相對位置(Xj，Yj，Z j)而求出。
[0038]此外，各光點相對于攝像機12的相對位置(?，乃，Zj)大多情況下不存在于同一平 面上。這是由于，根據(jù)在路面31上露出的瀝青的凹凸不同，各光點的相對位置進行改變。因 此，也可以使用最小二乘法，求出與各光點的距離誤差的平方和成為最小那樣的平面方程 式。
[0039]特征點檢測部23從存儲器讀取由攝像機12取得的圖像，并根據(jù)從存儲器讀取的圖 像，檢測路面31上的特征點。特征點檢測部23為了檢測路面31上的特征點，可以使用例如 uD.G.Lowe, uDistinctive Image Features from Scale-Invariant Keypoints /' Int · J. Comput .Vis.，vo 1.60,否.2, pp .91-110,否 ν· 200"，或，"金澤靖，金谷健 一，匕。 二一夕匕'、：％^(7)化^)(7)畫像(7)特徴點抽出（用于計算機圖像的圖像的特征點抽取)"信學 志，vol · 87，否· 12，pp · 1043-1048，Dec · 2004" 所記載的方法。
[0040]具體而言，特征點檢測部23使用例如哈里斯(Harris)運算符或SUSAN操作符，以物 體的頂點那樣輝度值比周圍大幅變化的點為特征點進行檢測?；蛘?，特征點檢測部23也可 以使用SIFT(Scale_Invariant Feature Transform)特征量，以根據(jù)在其周圍輝度值以一 定規(guī)則性變化的點為特征點進行檢測。而且，特征點檢測部23對由1個圖像檢測的特征點的 總數(shù)N進行計數(shù)，并對各特征點標注識別編號（i(l<i<N))。各特征點的圖像上的位置(仏、 V 1)存儲于ECU13內(nèi)的存儲器。圖6(a)及圖6(b)表示根據(jù)由攝像機12取得的圖像檢測出的特 征點Te的例子。各特征點的圖像上的位置(U 1J1)存儲于存儲器中。
[0041]此外，實施方式中，路面31上的特征點主要假定大小為Icm以上且2cm以下的瀝青 混合物的粒子。為了檢測該特征點，攝像機12的分辨率為VGA(約30萬像素）。另外，攝像機12 相對于路面31的距離大致為70cm。另外，攝像機12的拍攝方向從水平面向路面31傾斜約 45deg。另外，將由攝像機12取得的圖像向ECU13傳送時的輝度數(shù)值為0~255(0:最暗，255: 最殼)的范圍內(nèi)。
[0042]姿勢變化量計算部24從存儲器讀取按照一定的信息處理循環(huán)拍攝的幀中、上一次 幀所包含的多個特征點的圖像上的位置(UhV1),且從存儲器讀取此次幀所包含的多個特征 點的圖像上的位置(UhV 1)13而且，基于多個特征點的圖像上的位置變化，求出車輛的姿勢變 化量。在此，"車輛的姿勢變化量"包含相對于路面31的"距離及姿勢角"的變化量及在路面 上的"車輛(攝像機12)的移動量"雙方。以下，說明距離及姿勢角的變化量及車輛的移動量 的計算方法。
[0043] 圖6(a)表示在時刻t取得的第一幀（圖像)38的一例。如圖5或圖6(a)所示，考慮在 第一幀38分別計算出例如3個特征點T el、Te2、Te3的相對位置(XiUi)的情況。在該情況下， 可以將由特征點T el、Te2、Te3特定的平面G看作路面。因此，姿勢變化量計算部24可以根據(jù)相 對位置(X 1J1)，求出攝像機12相對于路面(平面G)的距離及姿勢角(法向矢量）。另外，姿 勢變化量計算部24可以根據(jù)已知的攝像機型號，求出各特征點T el、Te2、Te3之間的距離（h、 12、1 3)及連結(jié)各個特征點Te3l Je32Je33的直線構(gòu)成的角度。圖5的攝像機12表示第一幀時的攝 像機的位置。
[0044] 此外，作為表示相對于攝像機12的相對位置的三維坐標(Xi、Yi、Zi )，將攝像機12的 拍攝方向設(shè)定成Z軸，將拍攝方向設(shè)為法線且在包含攝像機12的平面內(nèi)設(shè)定相互正交的X軸 及Y軸。另一方面，作為圖像38上的坐標，將水平方向及垂直方向分別設(shè)定成V軸及U軸。
[0045] 圖6 (b)表示在從時刻t經(jīng)由時間△ t的時刻（t+ △ t)取得的第二幀。圖5的攝像機 12'表示拍攝第二幀38'時的攝像機的位置。如圖5或圖6(b)所示，在第二幀38'，攝像機12' 拍攝特征點T e3l Je32Je33,特征點檢測部23檢測出特征點Te3l Je32Je33t3在該情況下，姿勢變化量 計算部24根據(jù)時刻t時的各特征點Tel、Te2、Te3的相對位置(Xi、Yi、Zi )、各特征點的第二幀38 ' 上的位和攝像機12的攝像機型號，不僅可以計算出時間At時的攝像機12(車 輛)的移動量（AL)，而且還可以計算出距離及姿勢角的變化量。例如，通過解開以下的由式 (1)~(4)構(gòu)成的聯(lián)合方程式，姿勢變化量計算部24可以計算出攝像機12的移動量（AL)及 距離及姿勢角的變化量。此外，式（1)是攝像機12作為沒有失真或沒有光軸偏離的理想性的 針孔攝像機而模擬化的式子，Ai為常數(shù)，f為焦點距離。攝像機型號的參數(shù)只要預先進行校 準即可。
[0046]
[0047]
[0048]
[0049]
[0050] 圖3(b)示意性地表示攝像機12的拍攝范圍中，根據(jù)與照射圖案光32a的區(qū)域不同 的其它區(qū)域33檢測出的特征點的時間變化，求出攝像機12的移動方向34的情形。另外，圖6 (a)及圖6 (b)中，將表示各特征點Te的位置的變化方向及變化量的矢量Dte與圖像重疊表示。 姿勢變化量計算部24不僅可以計算出時間△ t時的攝像機12的移動量（△ L)，而且還可以同 時計算出距離及姿勢角的變化量。因此，姿勢變化量計算部24考慮距離及姿勢角的變化量， 可以高精度地計算出6個自由度的移動量（△ L)。即，即使距離或姿勢角根據(jù)車輛10的轉(zhuǎn)向 或加減速產(chǎn)生的側(cè)滾運動或俯仰運動不同而變化，也可以抑制移動量（△ U的推定誤差。 [0051 ]此外，姿勢變化量計算部24也可以不使用計算出相對位置的全部特征點，而基于 特征點彼此的位置關(guān)系選定最佳的特征點。作為選定方法，例如可以使用對極幾何(對極線 I.Hartley:uA linear method for reconstruction from lines and points," Proc.5th International Conference on Computer Vision,Cambridge,Massachusetts, pp.882-887(1995))〇
[0052]就在前后幀間使特征點對應而言，只要將例如檢測出的特征點周邊的小區(qū)域的圖 像記錄于存儲器，并根據(jù)輝度或色信息的類似度進行判斷即可。具體而言，ECU13將以檢測 出的特征點為中心的5 X 5(水平X垂直)像素的圖像記錄于存儲器中。姿勢變化量計算部24 如果將例如輝度信息收納于20像素以上且誤差1 %以下，則判斷為在前后幀間取得對應關(guān) 系的特征點。
[0053]這樣，從在后面的時刻取得的圖像38'也檢測到計算出相對位置(X1 JnZ1)的特征 點Tel、Te2、Te3時，姿勢變化量計算部24可以基于路面上的多個特征點的時間變化，計算出 "車輛的姿勢變化量"。
[0054]自己位置計算部26根據(jù)由姿勢變化量計算部24計算出的"距離及姿勢角的變化 量"計算出距離及姿勢角。另外，根據(jù)由姿勢變化量計算部24計算出的"車輛的移動量"計算 出車輛的當前位置。
[0055]具體而言，在將由姿勢角計算部22(參照圖1)計算出的距離及姿勢角設(shè)定為起點 的情況下，對該起點(距離及姿勢角)逐次加上(進行積分運算）由姿勢變化量計算部24計算 出的各個幀的距離及姿勢角的變化量，由此，將距離及姿勢角更新成最新的數(shù)值。另外，將 由姿勢角計算部22計算出距離及姿勢角時的車輛位置設(shè)定為起點（車輛的初始位置），且從 該初始位置逐次加上(進行積分運算)車輛的移動量，由此，計算出車輛的當前位置。例如， 通過設(shè)定與地圖上的位置核對的起點（車輛的初始位置），可以逐次計算出地圖上的車輛的 當前位置。
[0056]這樣，如果可以繼續(xù)檢測在前后幀間取得對應關(guān)系的3點以上的特征點，則通過繼 續(xù)加上距離及姿勢角的變化量的處理(積分運算），不使用圖案光32a，也可以將距離或姿勢 角繼續(xù)更新成最新的數(shù)值。其中，在最初的信息處理循環(huán)中，也可以使用通過使用圖案光 32a計算出的距離及姿勢角或規(guī)定的初始距離及初始姿勢角。即，成為積分運算的起點的距 離及姿勢角也可以使用圖案光32a計算出，或也可以使用規(guī)定的初始值。規(guī)定的初始距離及 初始姿勢角優(yōu)選為至少考慮了車輛10的乘坐人員及搭載物的距離及姿勢角。例如，在車輛 10的點火開關(guān)為接通狀態(tài)，且移動位置從停車場移動至另一位置時，只要投射圖案光32a， 且將根據(jù)圖案光32a計算出的距離及姿勢角用作規(guī)定的初始距離及初始姿勢角即可。由此， 可以求出不產(chǎn)生車輛10的轉(zhuǎn)向或加減速引起的滾動運動或俯仰運動時的距離及姿勢角。 [0057] 此外，實施方式中，通過計算出距離及姿勢角的變化量，且逐次加上距離及姿勢角 的變化量，將距離及姿勢角更新成最新的數(shù)值。但是，也可以僅將攝像機12相對于路面31的 姿勢角設(shè)為其變化量而計算出并作為更新的對象。在該情況下，攝像機12相對于路面31的 距離只要假定為一定即可。由此，也可以考慮姿勢角的變化量，抑制移動量（AL)的推定誤 差，同時減輕E⑶13的運算負荷，且提高運算速度。
[0058]檢測狀態(tài)判斷部30判斷特征點檢測部23進行的特征點Te的檢測狀態(tài)是否不滿足 第一基準而是差的狀態(tài)。例如，如隧道內(nèi)的混凝土路面那樣，在路面上瀝青混合物的顆粒等 的狀況或凹凸較少的情況下，從圖像可檢測的特征點變少。在該情況下，在前后幀間取得對 應關(guān)系的特征點的繼續(xù)檢測變得困難，距離及姿勢角的更新精度降低。
[0059] 因此，檢測狀態(tài)判斷部30在例如計算出相對于攝像機12的相對位置的特征點中、 從在后面的信息處理循環(huán)中取得的圖像也可檢測出的特征點的數(shù)量不是規(guī)定的閾值（例 如，4個）以上的情況下，判斷為多個特征點的檢測狀態(tài)不滿足第一基準，而是差的狀態(tài)。即， 在不能檢測前后幀間取得對應的4個以上的特征點的情況下，檢測狀態(tài)判斷部30判斷為特 征點T e的檢測狀態(tài)不滿足第一基準，而是差的狀態(tài)。此外，如圖6所示，為了求出距離及姿勢 角的變化量，需要在前后幀間取得對應的至少3個特征點。這是由于，特定平面G需要3個特 征點。提高推定精度需要更多的特征點，因此，規(guī)定的閾值優(yōu)選為4或5以上。
[0060] 圖案光控制部27基于檢測狀態(tài)判斷部30的判斷結(jié)果，控制投光器11進行的圖案光 32a的投射。在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)滿足第一基準而不是差的 狀態(tài)的情況下，圖案光控制部27將圖案光32a熄滅。另一方面，在檢測狀態(tài)判斷部30判斷為 多個特征點的檢測狀態(tài)不滿足第一基準而是差的狀態(tài)的情況下，圖案光控制部27投射圖案 光32a。具體地，圖案光控制部27基于檢測狀態(tài)判斷部30的判斷結(jié)果，切換圖案光投射實施 標志的0N/0FF，并對投光器1發(fā)送圖案光投射實施標志。投光器1基于圖案光投射實施標志， 在下面的信息處理循環(huán)中，將圖案光32a進行投射或熄滅。
[0061] 另外，檢測狀態(tài)判斷部30判斷特征點檢測部23進行的特征點Te3的檢測狀態(tài)是否不 滿足比第一基準更低的第二基準而是差的狀態(tài)。例如，在不能檢測前后幀間取得對應的3個 以上的特征點的情況下，檢測狀態(tài)判斷部30判斷為特征點Te的檢測狀態(tài)不滿足第二基準而 是差的狀態(tài)。
[0062]在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)滿足第二基準且不是差的狀 態(tài)的情況下，姿勢角計算部22不能根據(jù)圖案光32a的位置計算出距離及姿勢角。另一方面， 在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)不滿足第二基準而是差的狀態(tài)的情況 下，不能根據(jù)特征點T e3特定平面G，因此，姿勢角計算部22根據(jù)圖案光32a的位置計算出距離 及姿勢角。
[0063]自己位置計算部26在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)滿足第二 基準的情況下，維持積分運算的起點。另一方面，在檢測狀態(tài)判斷部30判斷為多個特征點的 檢測狀態(tài)不滿足第二基準而是差的狀態(tài)的情況下，自己位置計算部26在相同的信息處理循 環(huán)中，將姿勢角計算部22(參照圖1)計算出的距離及姿勢角及此時的車輛位置設(shè)定成新的 起點(車輛的姿勢角及初始位置），并從該起點起開始車輛的姿勢變化量的加算。
[0064]此外，第一實施方式中，檢測狀態(tài)判斷部30基于在前后幀間取得對應關(guān)系的特征 點的數(shù)量，判斷多個特征點的檢測狀態(tài)，但也可以基于從1個圖像檢測出的特征點的總數(shù)N， 判斷多個特征點的檢測狀態(tài)。即，在從1個圖像檢測出的特征點的總數(shù)N為規(guī)定的閾值(例如 9)以下的情況下，也可以判斷為多個特征點的檢測狀態(tài)差。只要考慮檢測出的特征點中有 未取得對應的特征點，且將規(guī)定的閾值(4)的3倍的數(shù)(12)設(shè)定成閾值即可。
[0065]計算狀態(tài)判斷部35判斷姿勢角計算部22計算距離及姿勢角的計算狀態(tài)是否不滿 足第三基準而是差的狀態(tài)。例如，在圖案光照射至路面31上的臺階的情況下，路面31上的臺 階比瀝青的凹凸大，因此，距離及姿勢角的計算精度極低。在多個特征點的檢測狀態(tài)不滿足 第二基準而是差的狀態(tài)，且距離及姿勢角的計算狀態(tài)不滿足第三基準而是差的狀態(tài)的情況 下，就沒有高精度地檢測距離及姿勢角及其變化量的手段。
[0066] 于是，計算狀態(tài)判斷部35在例如由姿勢角計算部22計算出的距離及姿勢角的標準 偏差比規(guī)定的閾值大的情況下，判斷為由姿勢角計算部22計算距離及姿勢角的計算狀態(tài)不 滿足第三基準而是差的狀態(tài)。另外，在35個的光點中不能檢測出的光點的數(shù)量低于3點的情 況下，原則上不能求出路面31的平面方程式，因此，判斷為由姿勢角計算部22計算距離及姿 勢角的計算狀態(tài)不滿足第三基準而是差的狀態(tài)。在使用最小二乘法求出平面方程式時，與 各光點的距離誤差的最大值的絕對值成為某個閾值(例如0.05m)以上的情況下，也可以判 斷為由姿勢角計算部22計算距離及姿勢角的計算狀態(tài)不滿足第三基準而是差的狀態(tài)。
[0067] 在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)不滿足第二基準而是差的狀 態(tài)，且計算狀態(tài)判斷部35判斷為由姿勢角計算部22計算距離及姿勢角的計算狀態(tài)不滿足第 三基準而是差的狀態(tài)的情況下，自己位置計算部26使用之前的信息處理循環(huán)中的距離及姿 勢角及車輛的當前位置作為積分運算的起點。由此，可抑制車輛的移動量的計算誤差。 [0068](信息處理循環(huán)）
[0069]接著，作為根據(jù)由攝像機12取得的圖像38推定車輛10的移動量的自己位置計算方 法的一例，參照圖7說明由ECU13重復執(zhí)行的信息處理循環(huán)。圖7的流程圖所示的信息處理循 環(huán)在車輛10的點火開關(guān)成為接通狀態(tài)且自己位置計算裝置啟動的同時開始，并重復執(zhí)行到 自己位置計算裝置停止。
[0070]圖7的步驟SOl中，投光器11將圖案光32a投射或熄滅。具體而言，投光器11在上一 次的信息處理循環(huán)的步驟S05,基于從圖案光控制部27發(fā)送的圖案光投射實施標志的ON/ OFF，切換圖案光32a的投光/熄滅。參照圖8對圖案光投射實施標志進行后述。
[0071]進入步驟S03，ECU13控制攝像機12,拍攝包含投射圖案光32a的區(qū)域的車輛周圍的 路面31，而取得圖像38 ACU13將由攝像機12取得的圖像數(shù)據(jù)存儲于存儲器中。
[0072]此外，E⑶13可自動控制攝像機12的光圈。也可以根據(jù)之前的信息處理循環(huán)中取得 的圖像38的平均輝度，對攝像機12的光圈進行反饋控制，以成為輝度值的最大值和最小值 的中間值。另外，投射圖案光32a的區(qū)域的輝度值較高，因此，也可以從除抽取了圖案光32a 的部分以外的區(qū)域，求出平均輝度值。
[0073]進入步驟S05,檢測狀態(tài)判斷部30判斷多個特征點的檢測狀態(tài)。圖案光控制部27基 于檢測狀態(tài)判斷部30的判斷結(jié)果切換圖案光投射實施標志，并發(fā)送至投光器11。而且，姿勢 角計算部22基于檢測狀態(tài)判斷部30的判斷結(jié)果計算出或不計算出距離及姿勢角。步驟S05 的詳情參照圖8及圖9進行后述。
[0074]進入步驟S07，ECU13根據(jù)圖像38檢測特征點，且抽取前后的信息處理循環(huán)之間取 得對應的特征點，并根據(jù)特征點的圖像上的位置OJ1J1)計算出距離及姿勢角的變化量及車 輛的移動量。
[0075]具體而言，首先，特征點檢測部23從存儲器讀取由攝像機12取得的圖像38,且從圖 像38檢測路面31上的特征點，并將各特征點的圖像上的位置(Ui、V1)存儲于存儲器。姿勢變 化量計算部24從存儲器讀取各特征點的圖像上的位置(Ui、V 1),并根據(jù)距離及姿勢角和特征 點的圖像上的位置OJ1J1)計算出特征點相對于攝像機12的相對位置(HZ 1)。此外，姿勢 變化量計算部24使用之前的信息處理循環(huán)的步驟S09中設(shè)定的起點（距離及姿勢角）。姿勢 變化量計算部24將特征點相對于攝像機12的相對位置(X i、Yi、Zi)存儲于存儲器中。
[0076]而且，姿勢變化量計算部24從存儲器讀取特征點的圖像上的位置(U1J1)和之前的 信息處理循環(huán)的步驟S07中計算出的特征點的相對位置(Xi、Yi、Zi)。姿勢變化量計算部24使 用前后的信息處理循環(huán)之間取得對應的特征點的相對位置(X 1 JnZ1)及圖像上的位置(仏、 V1),計算出距離及姿勢角的變化量。另外，根據(jù)上一次的信息處理循環(huán)中的特征點的相對 位置(X 1U1)和此次的信息處理循環(huán)中的特征點的相對位置(HZ1),計算出車輛的移 動量。步驟S07中計算出的"距離及姿勢角的變化量"及"車輛的移動量"在步驟Sll的處理中 使用。
[0077]進入步驟S09，ECU13根據(jù)多個特征點的檢測狀態(tài)及圖案光的距離及姿勢角的計算 狀態(tài)，設(shè)定積分運算的起點。詳情參照圖10進行后述。
[0078] 進入步驟Sll，自己位置計算部26根據(jù)步驟S09的處理中設(shè)定的積分運算的起點、 及步驟S07的處理中計算出的"距離及姿勢角的變化量"及"車輛的移動量"，計算出距離及 姿勢角、及車輛的當前位置。
[0079]這樣，本實施方式的自己位置計算裝置通過重復執(zhí)行上述的一連串的信息處理循 環(huán)并累計車輛10的移動量，可以計算出車輛10的當前位置。
[0080]參照圖8的流程，說明圖7的步驟S05的詳細順序。首先，在步驟S501中，檢測狀態(tài)判 斷部30判斷是否檢測有4個以上的前后幀間取得對應的特征點。在檢測有4個以上特征點的 情況下（S501中，是），根據(jù)特征點的時間變化可計算出距離及姿勢角的變化量。于是，進入 步驟S505,圖案光控制部27將圖案光投射實施標志設(shè)定成OFF狀態(tài)，且發(fā)送至投光器11。然 后，不根據(jù)圖案光32a的位置計算出距離及姿勢角，并進入圖7的步驟S07。
[0081]另一方面，在未檢測4個以上特征點的情況下（S501中，否），判斷為多個特征點的 檢測狀態(tài)不滿足第一基準而是差的狀態(tài)，因此，進入步驟S503。而且，圖案光控制部27將圖 案光投射實施標志設(shè)定成ON狀態(tài)，并發(fā)送至投光器11。然后，進入步驟S507。
[0082]步驟S507中，檢測狀態(tài)判斷部30判斷是否檢測有3個以上的前后幀間取得對應的 特征點。在檢測有3個以上特征點的情況下（S507中，是），根據(jù)特征點的時間變化可計算出 距離及姿勢角的變化量。因此，不根據(jù)圖案光32a的位置計算出距離及姿勢角，進入圖7的步 驟 S07。
[0083]另一方面，在未檢測有3個以上特征點的情況下（S507中，否），不能根據(jù)特征點的 時間變化計算出距離及姿勢角的變化量。因此，進入步驟S509,姿勢角計算部22根據(jù)圖案光 32a的位置計算出距離及姿勢角。然后，進入圖7的步驟S07。
[0084]步驟S509中，首先，圖案光抽取部21從存儲器讀取由攝像機12取得的圖像38,如圖 4(c)所示，從圖像38抽取圖案光32a的位置。圖案光抽取部21將作為表示圖案光32a的位置 的數(shù)據(jù)計算出的光點&的圖像上的坐標(山、％)存儲于存儲器中。
[0085]姿勢角計算部22從存儲器讀取表示圖案光32a的位置的數(shù)據(jù)，根據(jù)圖案光32a的位 置計算出距離及姿勢角，并存儲于存儲器中。
[0086]參照圖9說明按照圖8的流程執(zhí)行的信息處理的一例。圖9(a)表示姿勢角計算部22 是否根據(jù)圖案光32a計算出距離及姿勢角，圖9(b)表示圖案光投射實施標志的狀態(tài)，圖9(c) 表示取得對應的特征點的數(shù)量。圖9 (a)~（c)的橫軸表示時間的經(jīng)過，t0、11、t2、......表示 信息處理循環(huán)（以后，均簡稱為"循環(huán)"）。
[0087]循環(huán)t0~t3中，取得對應的特征點的數(shù)量為4個以上，因此，圖案光投射實施標志 為OFF狀態(tài)，"三維測量實施標志"也為OFF狀態(tài)。如果"三維測量實施標志"為OFF狀態(tài)，則姿 勢角計算部22不根據(jù)圖案光32a計算出距離及姿勢角。循環(huán)t4~t6中，取得對應的特征點的 數(shù)量成為3個時，圖8的步驟S501判斷為否，步驟S503中圖案光投射實施標志成為ON狀態(tài)，但 在步驟S507中判斷為是，因此，三維測量實施標志仍為OFF狀態(tài)。循環(huán)t7~tlO中取得對應的 特征點的數(shù)量成為4個以上時，圖案光投射實施標志成為OFF狀態(tài)。循環(huán)112~114中，取得對 應的特征點的數(shù)量成為低于3個，因此，步驟S501及S507中均判斷為否，因此，圖案光投射實 施標志及三維測量實施標志成為ON狀態(tài)，實施利用姿勢角計算部22進行距離及姿勢角的計 算。
[0088] 參照圖10的流程說明圖7的步驟S09的詳細順序。步驟S900中，E⑶13判斷此次的信 息處理循環(huán)是否為初次。而且，在為初次的情況下，即在沒有上一次的信息處理循環(huán)的數(shù)據(jù) 的情況下，將處理進入步驟S905，在不是初次的情況下，將處理進入步驟S901。
[0089]步驟S901中，檢測狀態(tài)判斷部30判斷由特征點檢測部23檢測的特征點Te的檢測狀 態(tài)是否不滿足第二基準而是差的狀態(tài)。即，如與圖8的步驟S507相同，判斷取得對應的特征 點Te的數(shù)是否為3個以上。在判斷為不是3個以上的情況下（步驟S901中為是），進入步驟 S903,在判斷為3個以上的情況下(步驟S901中為否），進入步驟S909。
[0090] 步驟S909中，E⑶13維持當前設(shè)定的積分運算的起點。
[0091] 步驟S903中，計算狀態(tài)判斷部35判斷由姿勢角計算部22計算距離及姿勢角的計算 狀態(tài)是否不滿足第三基準而是差的狀態(tài)。例如在相同的信息處理循環(huán)的步驟S509中判斷距 離及姿勢角的計算是否成功。在判斷為成功的情況下(步驟S903中為是），進入步驟S905,在 判斷為不成功的情況下(步驟S903中為否），進入步驟S907。
[0092] 步驟S905中，E⑶13將步驟S509中由姿勢角計算部22計算出的距離及姿勢角、及此 時的車輛的當前位置設(shè)定為積分運算的起點。以該距離及姿勢角為起點開始新的積分運 算。另外，以車輛的當前位置為起點開始新的積分運算。
[0093]步驟S907中，E⑶13將上一次的信息處理循環(huán)中采用的距離及姿勢角、及上一次的 信息處理循環(huán)中的車輛的當前位置設(shè)定為積分運算的起點。以該距離及姿勢角為起點開始 新的積分運算。另外，以車輛的當前位置為起點開始新的積分運算。然后，使處理進入圖8的 步驟Sll。
[0094]如以上說明，根據(jù)第一實施方式，可得到以下的作用效果。
[0095]當多個特征點的檢測狀態(tài)差時，由姿勢變化量計算部24計算出的距離及姿勢角的 計算精度降低，因此，車輛10的移動量的推定誤差變大。圖11(a)及圖11(b)是表示車輛10的 側(cè)滾角(姿勢角的一例)及移動量(車寬方向）的推定誤差的例子的圖，表示車輛10在無傾斜 的平坦的路面上以一定的速度直行時的、側(cè)滾角的推定值及移動量的計算值的時間變化。 圖中的 uP1 "及"p2"表示不管多個特征點的檢測狀態(tài)如何，均維持移動量計算的起點及距離 及姿勢角的積分運算的起點的比較例。"Qi"及"Q 2"表示真實值。車輛10進行直行，因此，側(cè)滾 角及車寬方向的移動量的真實值(Qi、Q2)為保持為0的狀態(tài)。但是，在時刻tl~t2之間，由于 在多個特征點的檢測狀態(tài)差的混凝土路面行駛，因此，路面的平面方程式中產(chǎn)生誤差，側(cè)滾 角中出現(xiàn)推定誤差。側(cè)滾角的誤差會作為車寬方向的移動量的誤差反映。比較例中，根據(jù)多 個特征點的檢測狀態(tài)，不能將移動量計算的起點及側(cè)滾角的積分運算的起點重置至根據(jù)圖 案光32a計算出的距離及姿勢角、及此時的車輛的當前位置。因此，車輛10的移動量的誤差 會擴大。
[0096]根據(jù)第一實施方式，在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)不滿足第 一基準而是差的狀態(tài)的情況下(步驟S501中為否），圖案光控制部27投射圖案光32a。由此， 可以根據(jù)圖案光32a的位置計算出距離及姿勢角。因此，以根據(jù)圖案光32a計算出的誤差較 小的距離及姿勢角為起點，可重新開始積分運算。因此，可以高精度且穩(wěn)定地推定車輛10的 當前位置。
[0097]另外，多個特征點的檢測狀態(tài)變差的路面包含例如成為特征點的對象的狀況或凹 凸較少的路面。狀況或凹凸較少的路面的平坦性較高，因此，根據(jù)圖案光32a計算出的距離 及姿勢角所包含的噪聲成分較少。另一方面，在多個特征點的檢測狀態(tài)良好的路面31，由于 瀝青的凹凸，圖案光32a的反射位置不穩(wěn)定，因此，根據(jù)圖案光32a計算出的距離及姿勢角包 含較多的噪聲成分。根據(jù)第一實施方式，可以根據(jù)多個特征點的檢測狀態(tài)，適當?shù)胤譃槭褂?了特征點的距離及姿勢角的更新和使用了圖案光32a的距離及姿勢角的計算，并高精度且 穩(wěn)定地求出距離及姿勢角。
[0098]另外，由于路面31的照度的急劇變化，有時圖像38的對比度降低，且多個特征點的 檢測狀態(tài)變差。根據(jù)第一實施方式，通過直接判斷多個特征點的檢測狀態(tài)，不需要額外設(shè)置 照度計等傳感器，可減少成本，且有助于裝置的輕量化。
[0099]根據(jù)第一實施方式，在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)滿足第一 基準而不是差的狀態(tài)的情況下(步驟S501中為是），圖案光控制部27停止圖案光32a的投射。 僅在多個特征點的檢測狀態(tài)不滿足第一基準而是差的狀態(tài)的情況下可以投射圖案光32a， 因此，可以僅在需要的情況下投射圖案光32a，并抑制投光所需要的耗電量。
[0100] 圖7的步驟S05中的圖案光32a的投光及熄滅（步驟S503、S505)的判斷在下面的信 息處理步驟的步驟SOl中反映。因此，為了使用圖案光32a計算出距離及姿勢角，需要預先在 之前的循環(huán)中將圖案光投射實施標志切換成ON狀態(tài)。于是，第一實施方式中，如果是多個特 征點的檢測狀態(tài)不滿足第一基準而是差的狀態(tài)，則將圖案光投射實施標志切換成ON狀態(tài) (S05)。在其下面的循環(huán)中，投射圖案光32a(S01)。而且，在相同的循環(huán)中，在多個特征點的 檢測狀態(tài)不滿足第二基準而是差的狀態(tài)的情況下(S507中為否），姿勢角計算部22可以根據(jù) 圖案光32a的位置計算出距離及姿勢角。這樣，通過使用兩個基準(第一基準，第二基準)判 斷多個特征點的檢測狀態(tài)，可以適當?shù)乜刂茍D案光32a的投光時間及距離及姿勢角的計算 時間。
[0101] (第二實施方式）
[0102] 第二實施方式中，說明根據(jù)從開始圖案光32a的投射起的經(jīng)過時間控制使用了圖 案光32a的距離及姿勢角的計算時間的例子。第二實施方式的自己位置計算裝置的硬件構(gòu) 成如圖1所示，因此，省略說明。另外，由ECU13重復執(zhí)行的信息處理循環(huán)除了步驟S05之外， 也如圖7所示那樣，因此，省略說明。后述的第三及第四實施方式也同樣。
[0103] 參照圖12說明第二實施方式的步驟S05的詳細順序。圖12所示的流程中，代替圖8 的步驟S507,而實施步驟S511。其它步驟與圖8相同，而省略說明。ECU13測量從開始圖案光 32a的投射起的經(jīng)過時間。
[0104]具體而言，E⑶13在步驟S503中測量從將圖案光投射實施標志從OFF狀態(tài)切換至ON 狀態(tài)起的經(jīng)過時間。步驟S511中，ECU13判斷從切換圖案光投射實施標志起是否經(jīng)過第一規(guī) 定時間。如果未經(jīng)過第一規(guī)定時間（S511中為否），則不根據(jù)圖案光32a的位置計算出距離及 姿勢角，而進入步驟S07。另一方面，如果經(jīng)過第一規(guī)定時間（S511中為是），則進入步驟 S509,根據(jù)圖案光32a的位置計算出距離及姿勢角，然后，進入步驟S07。
[0105] 此外，第一規(guī)定時間例如圖13所示，可以設(shè)定兩次的信息處理循環(huán)(Dl)。即，將圖 案光投射實施標志從OFF狀態(tài)切換至ON狀態(tài)的信息處理循環(huán)（til)及下一次的信息處理循 環(huán)(tl2)中，不計算出距離及姿勢角(在S511中為否），或者在下一次(第二次)的信息處理循 環(huán)(tl3)中，首次計算出距離及姿勢角(S511中為是，S509)。
[0106] 如之前敘述，為了使用圖案光32a計算出距離及姿勢角，需要預先在之前的循環(huán)中 將圖案光投射實施標志切換成ON狀態(tài)。第二實施方式中，圖案光控制部27從開始圖案光32a 的投射起經(jīng)過第一規(guī)定時間(Dl)之后，姿勢角計算部22開始根據(jù)圖案光32a的位置計算出 距離及姿勢角。由此，可以適當?shù)乜刂茍D案光32a的投光時間及距離及姿勢角的計算時間。
[0107] (第三實施方式）
[0108] 第三實施方式中，說明如下例子，即，在開始圖案光32a的投射后，即使判斷為多個 特征點的檢測狀態(tài)滿足第一基準且不是差的狀態(tài)，也將圖案光32a的投射從該判斷起繼續(xù) 到經(jīng)過第二規(guī)定時間為止。
[0109] 在檢測狀態(tài)判斷部30判斷為多個特征點的檢測狀態(tài)是不比第一基準差的情況下， 圖案光控制部27在從檢測狀態(tài)判斷部30判斷起經(jīng)過第二規(guī)定時間之后，停止圖案光32a的 投射。
[0110] 參照圖14說明第三實施方式的步驟S05的詳細順序。相對于圖8,圖14所示的流程 還具備步驟S513及步驟S515。其它步驟與圖8相同而省略說明。
[0111] ECU13測量從判斷為取得對應的特征點的數(shù)量為4個以上起的經(jīng)過時間。在判斷為 取得對應的特征點的數(shù)量為4個以上的情況下（S501中為是），進入步驟S513，ECUl 3判斷為 取得對應的特征點的數(shù)量為4個以上后，判斷是否經(jīng)過第二規(guī)定時間。如果經(jīng)過第二規(guī)定時 間（S513中為是），則進入步驟S505,圖案光控制部27將圖案光投射實施標志從ON狀態(tài)向OFF 狀態(tài)切換。另一方面，如果未經(jīng)過第二規(guī)定時間（S513中為否），則進入步驟S515,圖案光控 制部27將圖案光投射實施標志維持成ON狀態(tài)。
[0112] 此外，第二規(guī)定時間例如圖15所示，可以設(shè)定兩次的信息處理循環(huán)(D2)。即，取得 對應的特征點的數(shù)量成為4個以上的信息處理循環(huán)(t6、tl4)及下一次的信息處理循環(huán)(t7、 tl5)中將圖案光投射實施標志維持成ON狀態(tài)(S513中為否，S515)，且在下一次(第二次）的 信息處理循環(huán)（t8、tl6)中，首次將圖案光投射實施標志切換成OFF狀態(tài)（S513中為是， S505)〇
[0113] 由此，如果暫時開始圖案光32a的投射，則之后即使改善多個特征點的檢測狀態(tài)， 圖案光32a也不立即熄滅。即，圖案光32a的投光狀態(tài)中可以具有滯后(滯后效應），因此，可 以抑制圖案光32a的閃爍，且減輕閃爍引起的煩惱。
[0114](第四實施方式）
[0115] 第四實施方式中，與第三實施方式對稱地說明圖案光32a的熄滅狀態(tài)中具有滯后 的例子。在使圖案光32a熄滅后，即使多個特征點的檢測狀態(tài)不滿足第一基準而變差，從該 判斷起圖案光控制部27也將圖案光32a的熄滅繼續(xù)到經(jīng)過第三規(guī)定時間。在檢測狀態(tài)判斷 部30判斷為多個特征點的檢測狀態(tài)不滿足第一基準而是差的狀態(tài)的情況下，圖案光控制部 27在從檢測狀態(tài)判斷部30判斷起經(jīng)過第三規(guī)定時間之后，開始圖案光32a的投光(投射）。
[0116] 參照圖16說明與第四實施方式相關(guān)的步驟S05的詳細順序。相對于圖8,圖16所示 的流程還具備步驟S517。另外，步驟S501中，判斷取得對應的特征點的數(shù)量是否為5個以上。 即，在取得對應的特征點的數(shù)量不是5個以上的情況下，檢測狀態(tài)判斷部30判斷為多個特征 點的檢測狀態(tài)不滿足第一基準而是差的狀態(tài)。其它步驟與圖8相同并省略說明。
[0117] ECU13測量從判斷為取得對應的特征點的數(shù)量不是5個以上起的經(jīng)過時間。在判斷 為取得對應的特征點的數(shù)量不是5個以上的情況下（S501中為否），進入步驟S517，ECU13判 斷為取得對應的特征點的數(shù)量不是5個以上后，判斷是否經(jīng)過第三規(guī)定時間。如果經(jīng)過第三 規(guī)定時間（S517中為是），則進入步驟S503,圖案光控制部27將圖案光投射實施標志切換成 ON狀態(tài)。另一方面，如果未經(jīng)過第三規(guī)定時間（S517中為否），則進入步驟S505,圖案光控制 部27將圖案光投射實施標志維持成OFF狀態(tài)。
[0118]此外，第三規(guī)定時間可以如例如圖17所示設(shè)定兩次的信息處理循環(huán)(D3)。即，取得 對應的特征點的數(shù)量低于5個的信息處理循環(huán)(t2、t9)及下一次的信息處理循環(huán)（t3、tl0) 中，將圖案光投射實施標志維持成OFF狀態(tài)(S517中為否，S505)，且在下一次(第二次）的信 息處理循環(huán)(t4、tll)中，首次將圖案光投射實施標志切換成ON狀態(tài)(S517中為是，S503)。
[0119]由此，如果暫時停止圖案光32a的投射，則之后，即使多個特征點的檢測狀態(tài)惡化， 也立即不開始圖案光32a的投射。即，圖案光32a的熄滅狀態(tài)中可以具有滯后(滯后效應），因 此，可以抑制圖案光32a的閃爍，并減少閃爍產(chǎn)生的煩惱。
[0120] 如上所述，記載了本發(fā)明的第一~第四實施方式，但不應理解為形成該公開的一 部分的論述及附圖限定于本發(fā)明。根據(jù)本公開內(nèi)容，對于本領(lǐng)域技術(shù)人員來說，可知有各種 代替實施方式、實施例及運用技術(shù)。
[0121] 圖2表示將攝像機12和投光器11安裝于車輛10的前面的例子，但也可以朝向車輛 10的側(cè)方、后方、正下方設(shè)置。另外，本實施方式中，作為車輛10的一例，圖2中表示了四輪的 乘用車，但可以適用于摩托車、貨車、或例如搬運工程機械的特殊車輛等、可拍攝道路的路 面或壁面上的特征點的全部移動體(車輛）。
[0122] 符號說明
[0123] 10 車輛
[0124] 11投光器
[0125] 12攝像機(攝像部）
[0126] 21圖案光抽取部
[0127] 22姿勢角計算部
[0128] 23特征點檢測部
[0129] 24姿勢變化量計算部
[0130] 26自己位置計算部
[0131] 27圖案光控制部
[0132] 30檢測狀態(tài)判斷部
[0133] 31 路面
[0134] 32a、32b 圖案光
[0135] 35計算狀態(tài)判斷部
[0136] Te特征點
【主權(quán)項】
1. 一種自己位置計算裝置，其特征在于，具備： 投光器，其向車輛周圍的路面投射圖案光； 攝像部，其搭載于所述車輛，拍攝包含投射圖案光的區(qū)域在內(nèi)的車輛周圍的路面并取 得圖像； 姿勢角計算部，其根據(jù)由所述攝像部取得的圖像中的所述圖案光的位置，計算出車輛 相對于所述路面的姿勢角； 姿勢變化量計算部，其基于根據(jù)由所述攝像部取得的圖像檢測出的所述路面上的多個 特征點的時間變化，計算出所述車輛的姿勢變化量； 自己位置計算部，其通過在所述車輛的初始位置及姿勢角加上所述姿勢變化量，計算 出所述車輛的當前位置及姿勢角； 檢測狀態(tài)判斷部，其判斷所述姿勢變化量計算部中的所述多個特征點的檢測狀態(tài)； 圖案光控制部，其控制所述投光器進行的圖案光的投射， 在所述檢測狀態(tài)判斷部判斷為所述多個特征點的檢測狀態(tài)不滿足第一基準的情況下， 所述圖案光控制部投射圖案光。2. 如權(quán)利要求1所述的自己位置計算裝置，其特征在于， 在所述檢測狀態(tài)判斷部判斷為所述多個特征點的檢測狀態(tài)滿足第一基準的情況下，所 述圖案光控制部停止圖案光的投射。3. 如權(quán)利要求1或2所述的自己位置計算裝置，其特征在于， 在所述檢測狀態(tài)判斷部判斷為所述多個特征點的檢測狀態(tài)不滿足比所述第一基準低 的第二基準的情況下，所述姿勢角計算部根據(jù)所述圖案光的位置計算出車輛相對于所述路 面的姿勢角。4. 如權(quán)利要求1或2所述的自己位置計算裝置，其特征在于， 從所述圖案光控制部開始圖案光的投射起，經(jīng)過第一規(guī)定時間之后，所述姿勢角計算 部根據(jù)所述圖案光的位置開始計算出車輛相對于所述路面的姿勢角。5. 如權(quán)利要求1~4中任一項所述的自己位置計算裝置，其特征在于， 在所述檢測狀態(tài)判斷部判斷為所述多個特征點的檢測狀態(tài)不滿足第一基準的情況下， 所述圖案光控制部在從所述檢測狀態(tài)判斷部進行判斷起經(jīng)過第二規(guī)定時間之后，停止圖案 光的投射。6. 如權(quán)利要求1~4中任一項所述的自己位置計算裝置，其特征在于， 在所述檢測狀態(tài)判斷部判斷為所述多個特征點的檢測狀態(tài)不滿足第一基準的情況下， 所述圖案光控制部在從所述檢測狀態(tài)判斷部進行判斷起經(jīng)過第三規(guī)定時間之后，開始圖案 光的投射。7. -種自己位置計算方法， 向車輛周圍的路面投射圖案光， 拍攝包含投射所述圖案光的區(qū)域在內(nèi)的車輛周圍的路面，并取得圖像， 根據(jù)所述圖像中的所述圖案光的位置，計算出車輛相對于所述路面的姿勢角， 基于根據(jù)所述圖像檢測出的所述路面上的多個特征點的時間變化，計算出所述車輛的 姿勢變化量， 通過在所述車輛的初始位置及姿勢角加上所述姿勢變化量，計算出所述車輛的當前位 置及姿勢角， 其特征在于， 在判斷為所述多個特征點的檢測狀態(tài)不滿足第一基準的情況下，投射所述圖案光。
【文檔編號】G08G1/16GK106062849SQ201480076204
【公開日】2016年10月26日
【申請日】2014年2月24日
【發(fā)明人】山口郎, 山口一郎, 西內(nèi)秀和
【申請人】日產(chǎn)自動車株式會社