本發(fā)明涉及自主移動裝置、自主移動方法。

背景技術(shù)：

根據(jù)用途而自主地移動的自主移動裝置正在逐漸普及。例如，已知為了進行屋內(nèi)的清潔而自主地移動的自主移動裝置。一般來說，這樣的自主移動裝置需要進行實際空間的地圖的制作、和實際空間內(nèi)的自身設(shè)備位置的估算。

作為用于制作實際空間的地圖的方法，例如已知SLAM(Simultaneous Localization And Mapping，即時定位與地圖構(gòu)建)法。使用了單眼攝像機的SLAM技術(shù)的基本原理在非專利文獻1中有記載，進行如下處理，即，通過從攝像機拍攝的運動圖像的多個幀，追蹤同一特征點，來交替地估算自身設(shè)備的三維位置(攝像機位置)和特征點的三維位置(該特征點的三維位置集合而構(gòu)成地圖的信息)。此外，存在一種在實際空間內(nèi)制作環(huán)境地圖并進行自主移動的自主移動裝置(例如，參照專利文獻1)。

在先技術(shù)文獻

非專利文獻

非專利文獻1：Andrew J.Davison，“Real-Time Simultaneous Localization and Mapping with a Single Camera”，Proceedings of the 9th IEEE International Conference on Computer Vision Volume 2，2003，pp.1403-1410

非專利文獻2：Richard Hartley，Andrew Zisserman，“Multiple View Geometry in Computer Vision”，Second Edition，Cambridge.University Press，March 2004，chapter 9

專利文獻

專利文獻1：JP特開2009-169845號公報

技術(shù)實現(xiàn)要素：

發(fā)明要解決的課題

在上述的方法中，會產(chǎn)生累積誤差。而且，一旦由于噪聲等致使估算精度下降，則之后的累積誤差會大至實際使用上不能無視的程度。但是，存在如下問題：即使存在誤差，在通過SLAM法沒有矛盾地實現(xiàn)了位置的估算的期間，不能判斷在自身設(shè)備位置、地圖中是否包含誤差。此外，在專利文獻1所記載的技術(shù)中是讓操作者來判斷是否包含有誤差，自主移動裝置自已不能進行判斷。

因此，本發(fā)明為了解決上述問題而作，其目的在于提供一種能夠進行在自身設(shè)備位置、地圖中是否包含有誤差的判斷的自主移動裝置等。

用于解決課題的手段

為了達成上述目的，本發(fā)明的自主移動裝置的特征在于，具備：

位置計測部，其基于移動單元的移動量，來計測自身設(shè)備位置；

地圖存儲部，其對使用攝像部所拍攝的多個圖像的信息而制作的地圖進行存儲；

位置估算部，其基于所述攝像部所拍攝的多個圖像的信息和存儲在所述地圖存儲部中的地圖的信息，來估算自身設(shè)備位置；

判定部，其判定所述位置計測部計測出的自身設(shè)備位置和所述位置估算部估算出的自身設(shè)備位置之差是否在規(guī)定誤差內(nèi)；和

地圖編輯部，其在所述判定部判定為不在所述規(guī)定誤差內(nèi)的情況下，對存儲在所述地圖存儲部中的地圖進行編輯。

發(fā)明效果

根據(jù)本發(fā)明，能夠進行在自身設(shè)備位置、地圖中是否包含有誤差的判斷。

附圖說明

圖1是表示實施方式所涉及的自主移動裝置的外觀的圖。

圖2是表示實施方式所涉及的自主移動裝置的構(gòu)成的圖。

圖3是表示實施方式所涉及的自主移動控制處理整體的流程圖的圖。

圖4是表示實施方式所涉及的自主移動控制處理當中的自身設(shè)備位置估算線程的處理的流程圖的圖。

圖5是表示實施方式所涉及的自主移動控制處理當中的地圖制作線程的處理的流程圖的圖。

圖6是表示實施方式所涉及的自主移動控制處理當中的閉環(huán)(loop closing)線程的處理的流程圖的圖。

圖7是表示實施方式所涉及的自主移動控制處理當中的誤差判定線程的處理的流程圖的圖。

圖8是表示將控制部等的一部分設(shè)置于外部的服務(wù)器的結(jié)構(gòu)的一例的圖。

符號說明

100…自主移動裝置，10、50…控制部，11…位置計測部，12…地圖制作部，13…位置估算部，14…判定部，15…地圖編輯部，20、60…存儲部，21…圖像存儲部，22…地圖存儲部，30…傳感器部，31…加速度傳感器，32…角速度傳感器，33…障礙物傳感器，41…攝像部，42…驅(qū)動部，43…輸入部，44、71…通信部，45…電源，200…服務(wù)器

具體實施方式

以下，參照圖1，對本發(fā)明的實施方式所涉及的自主移動裝置進行說明。自主移動裝置100根據(jù)用途而自主地移動。該用途，例如是警備監(jiān)視用、屋內(nèi)清潔用、寵物用、玩具用等。

自主移動裝置100在外觀上具備攝像部41、驅(qū)動部42。

攝像部41具備單眼的攝像裝置(攝像機)。攝像部41例如以30fps取得圖像(幀)。自主移動裝置100基于由攝像部41依次取得的圖像，實時地識別自身設(shè)備位置和周圍環(huán)境，同時進行自主移動。

驅(qū)動部42是獨立2輪驅(qū)動型，是具備車輪和電動機的移動單元。自主移動裝置100能夠通過2個車輪的同一方向驅(qū)動來進行前后的平行移動(平移移動)，能夠通過2個車輪的反向驅(qū)動來進行原地的旋轉(zhuǎn)(朝向變更)，并能夠通過改變2個車輪各自的速度的驅(qū)動來進行旋轉(zhuǎn)移動(平移+旋轉(zhuǎn)(朝向變更)移動)。此外，各個車輪具備旋轉(zhuǎn)編碼器，通過旋轉(zhuǎn)編碼器來計測車輪的轉(zhuǎn)速，并通過利用車輪的直徑、車輪間的距離等幾何學(xué)關(guān)系而能夠計算平移移動量以及旋轉(zhuǎn)量。例如，若將車輪的直徑設(shè)為D，并將轉(zhuǎn)速設(shè)為R(由旋轉(zhuǎn)編碼器測量)，則該車輪的接地部分的平移移動量成為π·D·R。此外，若將車輪的直徑設(shè)為D，將車輪間的距離設(shè)為I，將右車輪的轉(zhuǎn)速設(shè)為R_R，并將左車輪的轉(zhuǎn)速設(shè)為R_L，則朝向變更的旋轉(zhuǎn)量(若以右旋轉(zhuǎn)為正)成為360°×D×(R_L-R_R)/(2×I)。通過將該平移移動量、旋轉(zhuǎn)量依次進行相加，從而驅(qū)動部42作為所謂的里程計(Odometry)而發(fā)揮作用，能夠計測自身設(shè)備位置(以移動開始時的位置以及朝向為基準的位置以及朝向)。

從里程計得到的自身設(shè)備的位置以及朝向的精度由于車輪的摩耗、打滑等而成為低精度的情況較多。特別是由于誤差逐漸累積的關(guān)系，精度隨著時間經(jīng)過而惡化。不過，關(guān)于里程計的旋轉(zhuǎn)分量(朝向的信息)，通過使用后述的角速度傳感器信息，能夠提高精度。此外，通過使用檢測地磁來確定方位的方位傳感器(未圖示)，也能夠與來自里程計的取得值無關(guān)地取得利用了地磁的絕對朝向的信息。

另外，也可以取代車輪而具備履帶，還可以通過具備多條(例如兩條)腿并用腿行走來進行移動。在這些情況下，也能夠基于兩個履帶的活動、腿的活動，與車輪的情況同樣地計測自身設(shè)備的位置、朝向。

如圖2所示，自主移動裝置100除了具備攝像部41、驅(qū)動部42以外，還具備控制部10、存儲部20、傳感器部30、輸入部43、通信部44、電源45。

控制部10由CPU(Central Processing Unit，中央處理單元)等構(gòu)成，通過執(zhí)行存儲在存儲部20中的程序，從而實現(xiàn)后述的各部(位置計測部11、地圖制作部12、位置估算部13、判定部14、地圖編輯部15)的功能。

存儲部20由ROM(Read Only Memory，只讀存儲器)、RAM(Random Access Memory，隨機存取存儲器)等構(gòu)成，包含圖像存儲部21、地圖存儲部22。在ROM中，存儲有控制部10的CPU執(zhí)行的程序(例如，后述的SLAM法的運算、自主移動控制處理所涉及的程序)、執(zhí)行程序上預(yù)先需要的數(shù)據(jù)。在RAM中，存儲在程序執(zhí)行過程中被生成或者變更的數(shù)據(jù)。

在圖像存儲部21中，存儲攝像部41所拍攝的圖像。但是，為了節(jié)約存儲容量，可以不存儲所拍攝的所有的圖像，此外也可以不存儲圖像本身，而存儲圖像的特征量。關(guān)于重要的圖像(后述的關(guān)鍵幀)，與圖像的信息一起，存儲由上述的里程計所計測出的自身設(shè)備位置(自身設(shè)備的位置以及朝向)的信息以及通過后述的SLAM法而估算出的自身設(shè)備位置(自身設(shè)備的位置以及朝向)的信息。

在地圖存儲部22中，存儲由地圖制作部12基于后述的SLAM法、來自障礙物傳感器33的信息而制作的地圖(特征點、障礙物的三維位置的信息)。

作為傳感器部30，具備加速度傳感器31、角速度傳感器32、障礙物傳感器33。加速度傳感器31是測量XYZ方向(3軸)的加速度的傳感器。通過對這些值進行時間積分，從而能夠求取XYZ的各方向的速度，進而通過對這些速度進行時間積分，從而能夠求取XYZ的各方向的位置的變化量(移動距離)。(但是由于對傳感器值進行二次積分，因而累積誤差變得相當大)。角速度傳感器32是測量角速度(每單位時間的角度移動量)的傳感器。已知與基于車輪的轉(zhuǎn)速來求取自身設(shè)備的朝向相比，使用角速度傳感器32來求取朝向更能提高精度。障礙物傳感器33是探測行進過程中的障礙物的傳感器，例如是紅外線傳感器、超聲波傳感器。另外，也可以不搭載獨立的障礙物傳感器33，使用攝像部41來探測障礙物。此外，也可以具備對碰撞到其他物體的情況進行探測的碰撞傳感器(bumper sensor)(未圖示)。此外，還可以具備檢測地磁來確定方位的方位傳感器(未圖示)。通過使用方位傳感器，能夠與來自里程計的取得值無關(guān)地取得利用了地磁的絕對朝向的信息。

作為輸入部43，具備用于操作自主移動裝置100的操作按鈕。操作按鈕，例如，包含電源按鈕、模式切換按鈕(對清潔模式、寵物模式等進行切換)、初始化按鈕(重新進行地圖的制作)等。作為輸入部43，也可以具備進行聲音的輸入的麥克風(未圖示)、和識別對自主移動裝置100的操作指示的聲音的聲音識別部。

通信部44是用于與外部裝置進行通信的模塊，在與外部裝置進行無線通信的情況下是包含天線的無線模塊。例如，通信部44是用于進行基于Bluetooth(注冊商標)的近距離無線通信的無線模塊。在通信部44中，進行自主移動裝置100與外部的數(shù)據(jù)授受等。

電源45是使自主移動裝置100進行動作的電源，一般來說是內(nèi)置的充電電池，但也可以是太陽能電池，還可以是從地面利用無線進行電力供給的系統(tǒng)。在電源45是充電電池的情況下，自主移動裝置100通過與充電站(基座)進行對接而被充電。

接著，對控制部10的功能進行說明?？刂撇?0包含位置計測部11、地圖制作部12、位置估算部13、判定部14、地圖編輯部15，進行后述的SLAM法的運算、自主移動裝置100的移動指示等。此外，控制部10適應(yīng)多線程功能，能夠并行進行多個線程(不同的處理的流程)。

位置計測部11基于驅(qū)動部42的車輪和電動機的活動，來計測自身設(shè)備位置。具體而言，在地面沒有高低差且車輪不會打滑這樣的前提下，基于各車輪的直徑(D)以及轉(zhuǎn)速(R：由旋轉(zhuǎn)編碼器測量)，該車輪的接地部分的移動距離被求取為π·D·R，因此根據(jù)這些數(shù)據(jù)和車輪間的距離，能夠求取平移移動量以及平移方向和朝向的變化量(旋轉(zhuǎn)角度)。通過對此依次相加，作為里程計，能夠計測自身設(shè)備的位置以及朝向。在地面存在高低差的情況下需要求取考慮了高度方向的平移移動量，而這通過由加速度傳感器31掌握高度的變化量來求取。此外關(guān)于車輪的打滑，能夠通過除了驅(qū)動用的車輪以外還事先搭載移動量計測用的車輪來減小誤差。

地圖制作部12將基于存儲在圖像存儲部21中的圖像的信息以及該圖像拍攝時的自身設(shè)備的位置以及朝向的信息使用SLAM法而估算出的特征點的三維位置(Map點)、基于由障礙物傳感器33探測到障礙物時的自身設(shè)備的位置以及朝向的信息而得到的該障礙物的三維位置等，作為地圖的信息而存儲到地圖存儲部22中。

位置估算部13基于后述的SLAM法，作為視覺里程計，估算自身設(shè)備的位置以及朝向。

判定部14判定由位置計測部11計測出的位置以及朝向與由位置估算部13估算出的位置以及朝向之差是否在規(guī)定誤差內(nèi)。若由里程計(位置計測部11)能夠計測正確的位置以及朝向，并且由SLAM法(視覺里程計：位置估算部13)能夠估算正確的位置以及朝向，則它們會大致一致，在該情況下能夠推測為存儲在地圖存儲部22中的地圖的信息也正確。但是，若位置以及朝向的計測、估算中包含有誤差，則會發(fā)生誤差的累積，計測值和估算值會變得不一致。在這樣的情況下，能夠推測為存儲在地圖存儲部22中的地圖的信息中也累積有誤差。因此，通過判定這兩者之差是否在規(guī)定誤差內(nèi)，能夠判定地圖的信息是否正確。

地圖編輯部15在由判定部14判定為存儲在地圖存儲部22中的地圖的信息不正確的情況下，對該地圖進行編輯。地圖的編輯的最簡單的處理是消除該地圖的處理。

圖3是自主移動裝置100的主流程?？刂撇?0最初在清除初始化完成標志后起動自身設(shè)備位置估算線程(步驟S101)，接著起動如下各線程，即，地圖制作線程(步驟S102)、閉環(huán)線程(步驟S103)、誤差判定線程(步驟S104)。通過自身設(shè)備位置估算線程、地圖制作線程進行動作，從而基于SLAM法，開始地圖的信息以及視覺里程計(使用地圖和圖像而估算出的自身設(shè)備位置的信息)的生成。誤差判定線程實現(xiàn)了本實施方式的特征性的功能，關(guān)于其詳細情況在后面敘述。

然后，控制部10判定是否動作結(jié)束(步驟S105)，若動作結(jié)束(步驟S105：“是”)則結(jié)束動作，則并非動作結(jié)束(步驟S105：“否”)則地圖制作部12進行地圖的信息的制作/更新(步驟S106)。接著控制部10為了進行自主移動而對驅(qū)動部42指示所希望的動作(步驟S107)，并返回至步驟S105。由此，能夠在基于地圖的信息自主地進行移動的同時，適當更新地圖的信息。

作為典型的示例，自主移動裝置100首先若在置于充電站的狀態(tài)下接通電源45，則借助障礙物傳感器33，在家中的各房間到處移動，由障礙物傳感器33來確定墻壁等障礙物位置，能夠制作包含障礙物位置的地圖的信息。若將地圖制作一定程度，則能夠知道雖然還沒有地圖的信息但可以認為能夠移動的區(qū)域，在該區(qū)域自主地進行移動等，也能夠促進更廣范圍的地圖的制作。然后，若制作了能夠移動的大致整個區(qū)域的地圖的信息，則能夠進行利用了地圖的信息的高效的移動動作。例如能夠?qū)崿F(xiàn)無論從房間的什么位置都以最短路徑返回到充電站，或者高效地進行房間的清潔。

關(guān)于在自主移動裝置100的主流程(圖3)的步驟S101起動的自身設(shè)備位置估算線程，使用圖4來進行說明。該線程是如下處理，即，位置估算部13首先進行初始化處理，之后持續(xù)進行自身設(shè)備位置估算(使用由攝像部41取得的圖像通過視覺里程計來估算自身設(shè)備位置)。

位置估算部13，首先，判定是否動作結(jié)束(步驟S201)。若動作結(jié)束(步驟S201：“是”)則結(jié)束，若并非動作結(jié)束(步驟S201：“否”)，則判定是否已經(jīng)初始化完成(步驟S202)。若初始化完成(步驟S202：“是”)則進行步驟S221以后的自身設(shè)備位置估算處理，若尚未初始化完成(步驟202：“否”)則前進到步驟S203進行初始化處理。首先對初始化處理進行說明。

在初始化處理中，位置估算部13首先將-1設(shè)定給幀計數(shù)器N(步驟S203)，由攝像部41取得圖像(步驟S204)。圖像例如能夠以30fps取得(所取得的圖像也被稱為幀)。接著，從所取得的圖像內(nèi)取得2D特征點(步驟S205)。所謂2D特征點，是圖像中的邊緣部分等圖像內(nèi)的特征性的部分，能夠使用SIFT(Scale-Invariant Future Transform，尺度不變特征轉(zhuǎn)換)、SURF(Speed-Up Robust Features，加速穩(wěn)健特征)等算法來取得。另外，為了取得2D特征點，也可以使用其他算法。

若所取得的2D特征點的個數(shù)較少，則無法進行利用后述的Two-view Structure from Motion法(雙視圖運動恢復(fù)結(jié)構(gòu)法)的計算，因此位置估算部13在步驟S206中，將2D特征點的取得數(shù)與基準值(例如10個)進行比較，在小于基準值的情況下(步驟S206：“否”)返回至步驟S204，反復(fù)進行圖像的取得和2D特征點的取得，直到得到基準值以上的2D特征點數(shù)為止。另外，在此時點，雖然還未能制作地圖的信息，但例如在上述的典型的示例中，由于借助障礙物傳感器33，在家中的各房間開始到處移動，因此只要在該初始化處理中反復(fù)進行圖像取得和2D特征點取得，便會一邊移動一邊反復(fù)進行圖像取得，所以能夠取得各種各樣的圖像，能夠期待終歸能取得2D特征點數(shù)較多的圖像。

在2D特征點的取得數(shù)為基準值以上的情況下(步驟S206：“是”)，位置估算部13使幀計數(shù)器N遞增(步驟S207)。然后，判定幀計數(shù)器N是否為0(步驟S208)。若幀計數(shù)器N為0(步驟S208：“是”)則僅僅才取得一個圖像，所以為了取得第2張圖像而返回至步驟S204。另外雖然在圖4的流程圖中并未記載，但取得第1張圖像時的自身設(shè)備的位置和取得第2張圖像時的自身設(shè)備的位置在一定程度上遠離，會提高通過之后的處理而估算的姿勢的精度。因此，在從步驟S208返回至步驟S204時，也可以追加如下處理，即，進行等待，直到通過主流程(圖3)的步驟S107中的給驅(qū)動部42的動作指示從而基于里程計的平移距離成為規(guī)定距離(例如1m)以上為止。

若幀計數(shù)器N不為0(步驟S208：“否”)則可知已經(jīng)取得了兩個圖像，因此位置估算部13在這兩個圖像間取得2D特征點的對應(yīng)(實際環(huán)境上的同一點存在于各個圖像中，能提取其對應(yīng))(步驟S209)。由于若特征點的對應(yīng)數(shù)小于5，則不能進行后述的兩個圖像間的姿勢的估算，因此位置估算部13判定特征點的對應(yīng)數(shù)是否小于5(步驟S210)。若小于5(步驟S210：“是”)則為了重新取得初始圖像，返回至步驟S203。若特征點的對應(yīng)數(shù)為5點以上(步驟S210：“否”)，則通過使用Two-view Structure from Motion法，從而能夠估算兩個圖像間的姿勢(取得各個圖像的位置的差分(平移向量t)以及朝向的差分(旋轉(zhuǎn)矩陣R))(步驟S211)。

該估算具體而言根據(jù)對應(yīng)的特征點來求取基礎(chǔ)矩陣E，通過將基礎(chǔ)矩陣E分解為平移向量t和旋轉(zhuǎn)矩陣R來得到，但其詳情在非專利文獻2中已經(jīng)作了說明，在此不再贅述。另外，在此得到的平移向量t(若設(shè)想在三維空間內(nèi)移動，則以取得最初的圖像的位置為原點，具有X、Y、Z這3個要素)的各要素的值與實際環(huán)境上的值不同(Two-view Structure from Motion法不能得到實際環(huán)境上的值本身，而得到與實際環(huán)境相似的空間上的值。)，因此將它們視為SLAM空間上的值，以下使用SLAM空間上的坐標(SLAM坐標)來進行說明。

若求出兩個圖像間的姿勢(平移向量t以及旋轉(zhuǎn)矩陣R)，則其值成為以最初的圖像為基準(以取得最初的圖像的位置為SLAM坐標的原點，平移向量為0向量，旋轉(zhuǎn)矩陣為單位矩陣I。)的情況下的第二張圖像的姿勢(取得第二個圖像時的自身設(shè)備的位置(平移向量t)以及朝向(旋轉(zhuǎn)矩陣R))。在此，在求出兩個圖像各自的姿勢(是該圖像(幀)拍攝時的自身設(shè)備的位置(平移向量t)以及朝向(旋轉(zhuǎn)矩陣R)，也稱為幀姿勢)的情況下，基于以下的思想，由地圖制作部12來求取在這兩個圖像間能取得對應(yīng)的2D特征點(對應(yīng)特征點)在SLAM坐標中的3D位置(步驟S212)。

若將2D特征點在圖像中的坐標(幀坐標：已知)設(shè)為(u，v)，并將該2D特征點在SLAM坐標中的3D位置(未知)設(shè)為(X，Y，Z)，則將它們用齊次坐標表示時的它們的關(guān)系，使用透視投影矩陣P由下述的算式(1)來表示。在此，“～”記號表示“除以非零的常數(shù)倍后相等”(即，相等或成為常數(shù)(非零)倍)，“’”記號表示“轉(zhuǎn)置”。

(u v 1)’～P(X Y Z 1)’…(1)

在上述的算式(1)中，P為3×4的矩陣，根據(jù)表示攝像機的內(nèi)部參數(shù)的3×3的矩陣A、和表示該圖像的姿勢(幀姿勢)的外部參數(shù)R以及t由以下的算式(2)來表示。在此，(R|t)表示在旋轉(zhuǎn)矩陣R的右側(cè)排列平移列向量t而得到的矩陣。

P＝A(R|t)…(2)

在上述的算式(2)中，R以及t如上所述作為該幀姿勢來求取。此外，攝像機的內(nèi)部參數(shù)A由焦點距離和攝像元件尺寸來決定，因此只要決定攝像部41便成為常數(shù)。

若假設(shè)在兩個圖像間取得了對應(yīng)的2D特征點當中的一個映射于第一個圖像的幀坐標(u₁，v₁)和第二個圖像的幀坐標(u₂，v₂)，則能夠得到以下的算式(3)以及算式(4)。在此，I表示單位矩陣，0表示零向量，(L|r)表示在矩陣L的右側(cè)排列列向量r而得到的矩陣。

(u₁ v₁ 1)’～A(I|0)(X Y Z 1)’…(3)

(u₂ v₂ 1)’～A(R|t)(X Y Z 1)’…(4)

在上述的算式(3)以及算式(4)中，由于能夠形成關(guān)于u₁、v₁、u₂、v₂各自的算式，因此算式得到4個而未知數(shù)為X、Y、Z這3個，因此能夠求取X、Y、Z，這成為該2D特征點在SLAM坐標中的3D位置。另外，由于算式的個數(shù)多于未知數(shù)的個數(shù)，因此有可能存在例如以u₁、v₁、u₂求取的X、Y、Z與以u₁、v₁、v₂求取的X、Y、Z不同的情況。這樣的情況下，成為過剩條件的聯(lián)立一次方程式，一般而言無解，但地圖制作部12使用最小平方法，來求取似乎最可靠的X、Y、Z。

若求出2D特征點在SLAM坐標中的3D位置(X，Y，Z)，則由地圖制作部12將其作為Map點而登記到Map點數(shù)據(jù)庫(也稱為Map點DB(Database)，被保存在地圖存儲部22中)中(步驟S213)。作為登記到Map點數(shù)據(jù)庫中的要素，至少需要“2D特征點在SLAM坐標中的3D位置即X、Y、Z”、和“該2D特征點的特征量”(例如通過SIFT等而得到的特征量)。此外，若將“時間戳”(后述的關(guān)鍵幀計數(shù)器NKF(表示當前的關(guān)鍵幀編號的變量)在向Map點數(shù)據(jù)庫的登記時間點的值等)追加到向Map點數(shù)據(jù)庫的登記要素中，則在對Map點數(shù)據(jù)庫進行編輯(返回到過去的狀態(tài)等)時會很便利。

然后，地圖制作部12判定是否已經(jīng)將在兩個圖像間取得了對應(yīng)的2D特征點(對應(yīng)特征點)全部登記到Map點數(shù)據(jù)庫中(步驟S214)，若尚未完成全部登記(步驟S214：“否”)則返回至步驟S212，若全部登記完成(步驟S214：“是”)則前進至步驟S215。

接下來位置估算部13將NKF(表示關(guān)鍵幀(是指成為后續(xù)的線程中的處理對象的圖像)的計數(shù)器的素數(shù))初始化為0(步驟S215)，將第二個圖像作為關(guān)鍵幀而登記到幀數(shù)據(jù)庫(也稱為幀DB(Database)，被保存在圖像存儲部21中)(步驟S216)。

登記到幀數(shù)據(jù)庫中的要素是：“關(guān)鍵幀編號”(登記時間點上的關(guān)鍵幀計數(shù)器NKF的值)、“姿勢”(該圖像拍攝時的自身設(shè)備在SLAM坐標內(nèi)的位置(平移向量t)以及朝向(旋轉(zhuǎn)矩陣R))、“由里程計計測出的實際環(huán)境上的姿勢”(根據(jù)實際環(huán)境中的基于驅(qū)動部42的移動距離而求取的自身設(shè)備的位置以及朝向)、“所提取出的全部的2D特征點”、“全部的2D特征點當中作為Map點而已知3D位置的點”、“關(guān)鍵幀自身的特征”。

上述中，所謂“關(guān)鍵幀自身的特征”，是用于使求取關(guān)鍵幀間的圖像類似度的處理變得高效的數(shù)據(jù)，通常使用圖像中的2D特征點的直方圖等為好，但也可以將圖像自身設(shè)為“關(guān)鍵幀自身的特征”。此外，“由里程計計測出的實際環(huán)境上的姿勢”，也能夠通過平移向量t和旋轉(zhuǎn)矩陣R來表示，但通常，由于該自主移動裝置100在二維平面上活動，因此也可以簡化為二維數(shù)據(jù)，作為以移動開始時的位置(原點)以及朝向為基準的二維坐標(X，Y)以及朝向φ來表示。

接著，位置估算部13為了使地圖制作線程知道生成了關(guān)鍵幀的情況，在地圖制作線程的關(guān)鍵幀隊列(隊列(Queue)成為先入先出的數(shù)據(jù)結(jié)構(gòu))中，設(shè)置關(guān)鍵幀計數(shù)器NKF(步驟S217)。

以上，自身設(shè)備位置估算線程的初始化處理完成，因此位置估算部13設(shè)置初始化完成標志(步驟S218)。

然后，位置估算部13為了得到SLAM坐標與實際環(huán)境坐標的比例(scale)對應(yīng)，通過將基于里程計的平移距離(以實際環(huán)境下的坐標來求取)除以通過上述處理而估算的SLAM坐標下的平移距離d，來求取比例S(步驟S219)。

接著，位置估算部13清除在后述的誤差判定線程中使用的計數(shù)器NI(步驟S220)，經(jīng)由步驟S201、步驟S202，進入到初始化完成的情況下的處理即步驟S221。

說明初始化完成的情況下的處理。該處理是自身設(shè)備位置估算線程的通常時的處理，是位置估算部13依次估算當前的自身設(shè)備的位置以及朝向(在SLAM坐標內(nèi)的平移向量t和旋轉(zhuǎn)矩陣R)的處理。

位置估算部13由攝像部41對圖像進行拍攝(步驟S221)，使幀計數(shù)器N遞增(步驟S222)。然后，取得所拍攝的圖像中包含的2D特征點(步驟S223)。接著，從登記到幀數(shù)據(jù)庫中的以前的關(guān)鍵幀(例如關(guān)鍵幀編號為NKF的圖像)的信息中，取得該圖像的信息中所包含的2D特征點當中的3D位置已知(成為登記到Map點數(shù)據(jù)庫中的Map點)的2D特征點，提取在與當前拍攝到的圖像之間能取得對應(yīng)的2D特征點(對應(yīng)特征點)(步驟S224)。

然后，位置估算部13判定對應(yīng)特征點的個數(shù)是否小于規(guī)定個數(shù)(例如10。以下稱為“基準對應(yīng)特征點數(shù)”。)(步驟S225)，由于在小于基準對應(yīng)特征點數(shù)的情況下(步驟S225：“是”)利用SLAM法估算的姿勢的精度較差，因此不進行位置的估算而返回至步驟S221中的圖像的取得。在此，也可以不是立刻返回至步驟S221而是返回至步驟S224，從登記到幀數(shù)據(jù)庫中的關(guān)鍵幀當中檢索對應(yīng)特征點的個數(shù)為基準對應(yīng)特征點數(shù)以上的關(guān)鍵幀。在該情況下，在登記到幀數(shù)據(jù)庫中的關(guān)鍵幀當中未找到對應(yīng)特征點的個數(shù)為基準對應(yīng)特征點數(shù)以上的關(guān)鍵幀的情況下返回至步驟S221。

位置估算部13若能夠提取基準對應(yīng)特征點數(shù)以上的對應(yīng)特征點(步驟S225：“否”)，則從Map點數(shù)據(jù)庫中取得對應(yīng)特征點各自的3D位置(X_i，Y_i，Z_i)(步驟S226)。若將當前拍攝到的圖像中包含的對應(yīng)特征點的幀坐標設(shè)為(u_i，v_i)，將該對應(yīng)特征點的3D位置設(shè)為(X_i，Y_i，Z_i)(i取從1到對應(yīng)特征點數(shù)的值)，則將各對應(yīng)特征點的3D位置(X_i，Y_i，Z_i)通過以下的算式(5)而投影到幀坐標系的值(ux_i，vx_i)與幀坐標(u_i，v_i)理想而言應(yīng)該是一致的。

(ux_i vx_i 1)’～A(R|t)(X_i Y_i Z_i 1)’…(5)

實際上在(X_i，Y_i，Z_i)中以及在(u_i，v_i)中都包含有誤差，因此(ux_i，vx_i)和(u_i，v_i)一致的情況很少存在。而且，未知數(shù)僅為R和t(三維空間的情況下分別為三維，3+3＝6是未知數(shù)的個數(shù))，而數(shù)學(xué)式存在對應(yīng)特征點的個數(shù)的2倍(對于一個對應(yīng)特征點，存在與幀坐標u、v分別對應(yīng)的算式)，因此成為過剩條件的聯(lián)立一次方程式，如上所述通過最小平方法來求取。具體而言，位置估算部13求取使以下的算式(6)的成本函數(shù)E1成為最小的姿勢(平移向量t以及旋轉(zhuǎn)矩陣R)。這成為通過SLAM法而求取的SLAM坐標下的自身設(shè)備的姿勢(由平移向量t以及旋轉(zhuǎn)矩陣R表示的自身設(shè)備的位置以及朝向)。以此方式，位置估算部13估算自身設(shè)備的姿勢(步驟S227)。

【數(shù)學(xué)式1】

由于求取到SLAM坐標下的當前的自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣R)，因此位置估算部13通過將其乘以比例S，來求取VO(視覺里程計)(步驟S228)。VO能夠作為實際環(huán)境下的自身設(shè)備的位置以及朝向來利用。

接著，位置估算部13判定從對登記到幀DB中的前一個關(guān)鍵幀(關(guān)鍵幀編號為NKF的圖像)進行拍攝時的自身設(shè)備的位置起是否移動了規(guī)定距離(例如1m。以下稱為“基準平移距離”。)以上(步驟S229)，若移動了基準平移距離以上(步驟S229：“是”)則使關(guān)鍵幀計數(shù)器NKF遞增后(步驟S230)，將當前幀作為關(guān)鍵幀而登記到幀DB中(步驟S231)。若僅移動不足基準平移距離(步驟S229：“否”)則返回至步驟S201。

在此，與基準平移距離進行比較的自身設(shè)備的移動距離既可以從里程計取得從前一個關(guān)鍵幀到當前幀的平移距離(兩幀的平移向量的差的向量的絕對值(要素的平方和的平方根))，也可以從上述的VO(視覺里程計)求取。登記到幀DB中的內(nèi)容如上所述是：“關(guān)鍵幀編號”、“姿勢”、“由里程計計測出的實際環(huán)境上的姿勢”、“所提取出的全部的2D特征點”、“全部的2D特征點當中作為Map點而已知3D位置的點”、“關(guān)鍵幀自身的特征”。

然后，位置估算部13為了使地圖制作線程知道產(chǎn)生了新的關(guān)鍵幀，在地圖制作線程的關(guān)鍵幀隊列中設(shè)置關(guān)鍵幀計數(shù)器NKF(步驟S232)。然后，返回至步驟S201。另外，關(guān)鍵幀計數(shù)器NKF、計數(shù)器NI、比例S、Map點DB、幀DB存儲在存儲部20中，使得能夠跨越線程來參照值。

接下來，關(guān)于在自主移動裝置100的主流程(圖3)的步驟S102起動的地圖制作線程，使用圖5來進行說明。該線程由地圖制作部12計算關(guān)鍵幀中的對應(yīng)特征點的3D位置，來制作地圖的信息(Map點DB)。

首先，地圖制作部12判定是否動作結(jié)束(步驟S301)。若動作結(jié)束(步驟S301：“是”)則結(jié)束，若并非動作結(jié)束(步驟S301：“否”)，則判定關(guān)鍵幀隊列是否為空(步驟S302)。若關(guān)鍵幀隊列為空(步驟S302：“是”)則返回至步驟301，若不為空(步驟S302：“否”)，則從關(guān)鍵幀隊列中取出數(shù)據(jù)設(shè)定給MKF(表示在地圖制作線程中處理的關(guān)鍵幀的關(guān)鍵幀編號的變量)(步驟S303)。地圖制作部12判定MKF是否大于0(步驟S304)，在MKF為0的情況下(步驟S304：“否”)返回至步驟S301，等待數(shù)據(jù)進入到關(guān)鍵幀隊列。在MKF為1以上的情況下(步驟S304：“是”)，進入到以下的處理。

地圖制作部12參照幀DB，提取在前關(guān)鍵幀(關(guān)鍵幀編號為MKF-1的關(guān)鍵幀)的2D特征點與當前關(guān)鍵幀(關(guān)鍵幀編號為MKF的關(guān)鍵幀)的2D特征點之間能取得對應(yīng)的2D特征點(對應(yīng)特征點)(步驟S305)。由于在幀DB中還登記有各關(guān)鍵幀的姿勢(平移向量t和旋轉(zhuǎn)矩陣R)，因此能夠通過與自身設(shè)備位置估算線程的初始化時的處理時同樣的方法來計算對應(yīng)特征點的3D位置。地圖制作部12將計算出了3D位置的對應(yīng)特征點作為Map點而登記到Map點DB中(步驟S306)。地圖制作部12對于幀DB也針對本次計算出了3D位置的2D特征點來登記3D位置(步驟S307)。

另外，在地圖制作部12提取出的對應(yīng)特征點已經(jīng)登記在Map點DB中的情況下，可以跳過3D位置計算而進入到針對下一個對應(yīng)特征點(Map點DB中未登記的對應(yīng)特征點)的處理，也可以重新進行3D位置計算，對Map點DB中已經(jīng)登記的3D位置、幀DB中的對應(yīng)特征點對應(yīng)的3D位置進行更新。

接著，地圖制作部12判定關(guān)鍵幀隊列是否為空(步驟S308)。若為空(步驟S308：“是”)，則針對全部關(guān)鍵幀的姿勢和全部Map點的3D位置，進行捆綁調(diào)整處理，提高精度后(步驟S309)前進至步驟S310。若關(guān)鍵幀隊列不為空(步驟S308：“否”)則前進至步驟310。接著地圖制作部12將MKF設(shè)置到閉環(huán)線程的關(guān)鍵幀隊列(步驟S310)，返回至步驟S301。

另外，所謂捆綁調(diào)整處理，是對攝像機姿勢(關(guān)鍵幀姿勢)和Map點的3D位置同時進行估算的非線性最優(yōu)化法，進行在使Map點投影在關(guān)鍵幀上時產(chǎn)生的誤差最小那樣的最優(yōu)化。

通過進行該捆綁調(diào)整的處理，從而能夠提高關(guān)鍵幀姿勢和Map點的3D位置的精度。但是，即使不進行該處理，僅是不能實現(xiàn)精度提高，也并不會發(fā)生特別問題。因此，即使在沒有其他處理的狀態(tài)時(例如，關(guān)鍵幀隊列為空的狀態(tài))，也不必每次都進行該處理。

此外，若進行捆綁調(diào)整的處理，則有時會發(fā)現(xiàn)投影在關(guān)鍵幀上時的誤差大于規(guī)定值的Map點。關(guān)于這樣的誤差較大的Map點，由于會給SLAM估算造成不良影響，因此可以從Map點DB以及幀DB中進行刪除，或者設(shè)立用于識別是誤差較大的需要注意的Map點的標志。另外，捆綁調(diào)整的處理在本實施方式中為可選的處理，所以在此省略處理的詳細情況。

接下來關(guān)于在自主移動裝置100的主流程(圖3)的步驟S103起動的閉環(huán)線程，使用圖6來進行說明。在該線程中控制部10連續(xù)不斷地檢查是否能夠進行閉環(huán)處理，在能夠進行的情況下進行閉環(huán)處理。另外，所謂閉環(huán)處理，是指在識別出已返回到以前來到過的相同場所的情況時，使用以前位于該相同場所時的姿勢的值和當前的姿勢的值的偏差，對從以前來到時到當前為止的軌跡中的關(guān)鍵幀、關(guān)聯(lián)的Map點的3D位置進行修正。

首先，控制部10判定是否動作結(jié)束(步驟S401)。若動作結(jié)束(步驟S401：“是”)則結(jié)束。若并非動作結(jié)束(步驟S401：“否”)則判定關(guān)鍵幀隊列是否為空(步驟S402)。若關(guān)鍵幀隊列為空(步驟S402：“是”)則返回至步驟S401，若關(guān)鍵幀隊列不為空(步驟S402：“否”)，則從關(guān)鍵幀隊列中取出數(shù)據(jù)并設(shè)定給LKF(表示在閉環(huán)線程中處理的關(guān)鍵幀的關(guān)鍵幀編號的變量)(步驟S403)。接著控制部10判定LKF是否大于1(步驟S404)。在LKF為0或1的情況下(步驟S404：“否”)返回至步驟S401，等待數(shù)據(jù)進入到關(guān)鍵幀隊列中。然后，在LKF為2以上的情況下(步驟S404：“是”)，進行以下的處理。

控制部10參照幀DB，從幀DB中檢索當前關(guān)鍵幀(關(guān)鍵幀編號為LKF的關(guān)鍵幀)與“關(guān)鍵幀自身的特征”的類似度為規(guī)定的類似度(例如0.9。以下稱為“基準圖像類似度”。)以上的關(guān)鍵幀(步驟S405)。在此，關(guān)于該類似度，在由特征向量表示了圖像(關(guān)鍵幀)的特征的情況下，能夠?qū)蓚€圖像的特征向量的內(nèi)積的絕對值歸一化為1而得到的值、或?qū)蓚€圖像的特征向量的絕對值(要素的平方和的平方根)歸一化為1而得到的值彼此的內(nèi)積設(shè)為該兩個圖像的類似度。此外，也可以將兩個圖像的特征向量(將絕對值歸一化為1而得到的值)的距離(各要素的差的平方和的平方根)的倒數(shù)設(shè)為類似度。

控制部10判定是否發(fā)現(xiàn)了“關(guān)鍵幀自身的特征”的類似度為基準圖像類似度以上的關(guān)鍵幀(步驟S406)，若未發(fā)現(xiàn)(步驟S406：“否”)則去往步驟S408，若發(fā)現(xiàn)(步驟S406：“是”)，則對從發(fā)現(xiàn)的關(guān)鍵幀到當前關(guān)鍵幀的軌跡中的關(guān)鍵幀的姿勢、和軌跡中的關(guān)鍵幀所包含的Map點的3D位置進行修正(步驟S407)。例如，控制部10將當前關(guān)鍵幀的姿勢修正為與所發(fā)現(xiàn)的關(guān)鍵幀的姿勢相同的姿勢。然后，使用所發(fā)現(xiàn)的關(guān)鍵幀的姿勢與當前關(guān)鍵幀的姿勢的差分，對從所發(fā)現(xiàn)的關(guān)鍵幀到當前關(guān)鍵幀的軌跡中的各關(guān)鍵幀的姿勢施加線性校正。進而關(guān)于這些各關(guān)鍵幀所包含的Map點的3D位置也根據(jù)各關(guān)鍵幀的姿勢的校正量來進行修正。

接著控制部10將LKF設(shè)置到誤差判定線程的關(guān)鍵幀隊列中，返回至步驟S401(步驟S408)。

接下來關(guān)于在自主移動裝置100的主流程(圖3)的步驟S104起動的誤差判定線程，使用圖7來進行說明。該線程是由判定部14對位置估算部13利用SLAM而估算的位置(視覺里程計下的位置)和位置測量部11通過里程計而求取的位置之差進行判定，若該差不在規(guī)定誤差內(nèi)，則意味著所制作的地圖的信息中包含的誤差較大，因此由地圖編輯部15來進行消除地圖的處理。

在該線程中控制部10首先判定是否動作結(jié)束(步驟S501)，若動作結(jié)束(步驟501：“是”)則結(jié)束，若并未動作結(jié)束(步驟501：“否”)，則判定誤差判定線程的關(guān)鍵幀隊列是否為空(步驟S502)。若關(guān)鍵幀隊列為空(步驟S502：“是”)則返回至步驟S501，等待數(shù)據(jù)進入到關(guān)鍵幀隊列中。

若關(guān)鍵幀隊列不為空(步驟S502：“否”)，則控制部10從關(guān)鍵幀隊列中取出數(shù)據(jù)來設(shè)定給IKF(表示在誤差判定線程中處理的關(guān)鍵幀的關(guān)鍵幀編號的變量)(步驟S503)。接下來判定IKF是否大于0(步驟S504)，在IKF為0的情況下(步驟504：“否”)，返回至步驟S501，等待數(shù)據(jù)進入到關(guān)鍵幀隊列中。在IKF為1以上的情況下(步驟S504：“是”)，進行以下的處理。

控制部10參照幀DB，對從前關(guān)鍵幀(關(guān)鍵幀編號為IKF-1的關(guān)鍵幀)的里程計取得的旋轉(zhuǎn)角φ₁與從當前關(guān)鍵幀(關(guān)鍵幀編號為IKF的關(guān)鍵幀)的里程計取得的旋轉(zhuǎn)角φ₂的差分的絕對值φ_d(＝|φ₁-φ₂|，自身設(shè)備的朝向的變化量)進行求取，并對φ_d和規(guī)定的旋轉(zhuǎn)角φ_r(例如5°。以下稱為“基準旋轉(zhuǎn)角φ_r”或“基準朝向變化量”。)進行比較(步驟S505)。求取φ_d時，控制部10相當于朝向變化量取得部。

若φ_d為φ_r以上(步驟S505：“否”)，則控制部10清除計數(shù)器NI(步驟S506)，并返回至步驟S501，等待下一關(guān)鍵幀。然后，若φ_d小于φ_r(步驟505：“是”)，則使計數(shù)器NI遞增(步驟S507)，并判定計數(shù)器NI是否小于規(guī)定的積蓄數(shù)C(例如10。以下稱為“基準積蓄數(shù)C”。)(步驟S508)。若計數(shù)器NI小于基準積蓄數(shù)C(步驟S508：“是”)，則返回至步驟S501，繼續(xù)等待下一關(guān)鍵幀。由于計數(shù)器NI每當自主移動裝置100前進基準平移距離時遞增，因此控制部10基于NI的值，能夠取得自主移動裝置100的位置的變化量為基準平移距離×NI。在此，控制部10相當于位置變化量取得部。然后，由于若計數(shù)器NI為基準積蓄數(shù)C以上則判定部14進行以下的判定，因此決定進行該判定的條件的基準位置變化量成為基準平移距離×C。

若計數(shù)器NI為基準積蓄數(shù)C以上(步驟508：“否”)，則判定部14基于從C次前的關(guān)鍵幀(關(guān)鍵幀編號為IKF-C的關(guān)鍵幀)到當前關(guān)鍵幀(關(guān)鍵幀編號為IKF的關(guān)鍵幀)的姿勢變化來求取SLAM坐標下的平移距離，設(shè)為T1(步驟S509)。此外，判定部14求取從C次前的關(guān)鍵幀到當前關(guān)鍵幀的基于里程計的平移距離，設(shè)為T2(步驟S510)。對T1、T2的求取方法更詳細說明如下。

由于在幀DB中保存有各關(guān)鍵幀的“姿勢”(該圖像拍攝時的自身設(shè)備在SLAM坐標內(nèi)的位置(平移向量t)以及朝向(旋轉(zhuǎn)矩陣R))、“從里程計取得的實際環(huán)境上的姿勢”(以實際環(huán)境的距離求取的位置以及朝向)”，因此簡單而言，將C次前的關(guān)鍵幀的平移向量t₁與當前關(guān)鍵幀的平移向量t2之差的大小(|t₁-t₂|)設(shè)為T1(使用SLAM坐標的平移向量t計算)、T2(使用從里程計取得的值計算)即可?；蛘撸部梢葬槍腃次前的關(guān)鍵幀到當前關(guān)鍵幀的C+1個關(guān)鍵幀，將C個相鄰的關(guān)鍵幀的平移向量之差的大小相加得到的值(∑|t_i-t_i+1|)設(shè)為T1或T2。

然后，判定部14清除計數(shù)器NI(步驟S511)。此外，因為T1是SLAM坐標下的值，所以將為了變換為實際環(huán)境的距離而乘以比例S得到的值設(shè)為T3(步驟S512)。然后，判定部14判定T2與T3的差異是否處于規(guī)定誤差范圍內(nèi)(流程圖的例中為1.5倍以內(nèi))(步驟S513以及步驟S514)。若在規(guī)定誤差范圍內(nèi)(T2小于T3×1.5(步驟S513：“是”)并且大于T3÷1.5(步驟S514：“是”)的情況下)，則地圖的信息(Map點DB的信息、幀DB的信息)不是那么不準確，所以返回至步驟S501，繼續(xù)等待下一關(guān)鍵幀。

在T2與T3的差異超過規(guī)定誤差范圍的情況下(T2為T3×1.5以上(步驟S513：“否”)或者為T3÷1.5以下(步驟S514：“否”)的情況下)，由于所制作的地圖的信息(Map點DB的信息、幀DB的信息)不準確，因此地圖編輯部15將初始化完成標志、Map點DB、幀DB、各線程的關(guān)鍵幀隊列全部進行清除(步驟S515)。然后控制部10返回至步驟S501，從最初重新開始處理。由于初始化完成標志、關(guān)鍵幀隊列都被清除，所以所有的線程會從最初重新開始處理。通過這樣，能夠防止自主移動裝置100在所制作的地圖中包含有誤差的狀態(tài)下繼續(xù)活動。

另外，在T2與T3的差異超過規(guī)定范圍的情況下，可知該時間點的地圖的信息是不準確的，但在之前的誤差判定線程的處理中，在T2與T3的差異處于規(guī)定范圍內(nèi)的時間點，地圖的信息有可能并不是那么不準確。因此，在步驟S514為“是”的情況下，也可以在返回至步驟S501之前，由地圖編輯部15將該時間點的地圖的信息(Map點DB)作為“似乎可靠的地圖”而保存到地圖保存部(未圖示)，在步驟S515中，地圖編輯部15不清除初始化完成標志、幀DB、各線程的關(guān)鍵幀隊列，而使Map點DB返回到所保存的以前的Map點DB的狀態(tài)。通過這樣，能夠有效利用過去制作的地圖的信息。

作為使Map點DB返回到以前的狀態(tài)的方法，除了將Map點DB整體的信息作為“似乎可靠的地圖”進行保存的方法以外，還存在與Map點DB的各要素一起保存時間戳(雖然可以使用時刻信息，但使用關(guān)鍵幀計數(shù)器NKF的值較為合理)的信息的方法。在該方法中，在步驟S514為“是”的情況下，在返回至步驟S501之前，地圖編輯部15將該時間點的時間戳(時刻或NKF)設(shè)定給作為時刻存儲部的變量TS(該TS自身也可以設(shè)為堆棧狀，使得能夠依次返回到過去)，在步驟S515中地圖編輯部15進行如下處理，即，消除Map點DB內(nèi)的信息當中的時間戳為TS以后的信息，TS以前的信息原樣保留。若進行這樣的處理，則能夠?qū)⑦^去的似乎可靠的Map點的信息原樣保存，僅消除最近的(可以認為誤差較大的)Map點的信息。

該思想也能夠應(yīng)用于幀DB，地圖編輯部15也可以在步驟S515中，消除幀DB內(nèi)的信息當中的關(guān)鍵幀編號比TS大的信息，TS以前的信息原樣保留。通過這樣，從而能夠?qū)⑦^去的似乎可靠的關(guān)鍵幀的信息原樣保存，僅消除最近的(可以認為誤差較大的)關(guān)鍵幀的信息。

另外，在上述實施方式中，成為步驟S505中的比較對象的旋轉(zhuǎn)角φ_d使用從里程計取得的旋轉(zhuǎn)角來進行了計算，但也可以使用從視覺里程計取得的旋轉(zhuǎn)角(根據(jù)幀DB中保存的各關(guān)鍵幀的“姿勢(由SLAM坐標內(nèi)的平移向量t和旋轉(zhuǎn)矩陣R表示的位置以及朝向)”的R得到的旋轉(zhuǎn)角)來計算。具體而言，若將前關(guān)鍵幀的姿勢的旋轉(zhuǎn)矩陣設(shè)為R1，并將當前幀的姿勢的旋轉(zhuǎn)矩陣設(shè)為R2，則通過計算R3＝R2×R1^-1，能夠得到表示從前關(guān)鍵幀到當前關(guān)鍵幀之間的旋轉(zhuǎn)的旋轉(zhuǎn)矩陣R3(R1^-1表示R1的逆矩陣(旋轉(zhuǎn)矩陣中必定存在逆矩陣))，因而在此基于所得到的旋轉(zhuǎn)矩陣R3的各要素，能夠求取各軸的旋轉(zhuǎn)角。在旋轉(zhuǎn)矩陣為3×3的情況下，存在各軸的旋轉(zhuǎn)角根據(jù)旋轉(zhuǎn)矩陣不唯一地求取的情況，但實際上自主移動裝置100移動大多是在地面等的平面上，因此通過近似為2×2的旋轉(zhuǎn)矩陣，能夠唯一地決定旋轉(zhuǎn)角。通過這樣使用視覺里程計，能夠不受驅(qū)動部42的經(jīng)年劣化、打滑等的影響地取得旋轉(zhuǎn)角φ_d。

考慮驅(qū)動部42的經(jīng)年劣化、打滑等的影響，控制部10可以在經(jīng)年劣化較少并且沒有打滑等的影響的路面上進行移動的情況下，使用從里程計取得的旋轉(zhuǎn)角，在其他的情況下使用從視覺里程計取得的旋轉(zhuǎn)角。

進而，在自主移動裝置100還具備方位傳感器的情況下，也可以使各關(guān)鍵幀取得時的方位傳感器的值也保存在幀DB中，在步驟S505中，控制部10判定基于由方位傳感器得到的方位的旋轉(zhuǎn)角φ_d是否小于基準旋轉(zhuǎn)角φ_r。

此外，在圖7的流程圖中，將旋轉(zhuǎn)角φ_d設(shè)為了從前關(guān)鍵幀起的旋轉(zhuǎn)角，但也可以在步驟S505中判定從計數(shù)器NI＝0的時間點的關(guān)鍵幀起的旋轉(zhuǎn)角是否小于基準旋轉(zhuǎn)角φ_r。由此，能夠?qū)⒁稽c一點向相同方向旋轉(zhuǎn)地移動的情況從誤差判定線程中的判定對象中排除。

此外，在圖7的流程圖中，將計數(shù)器NI的閾值即基準積蓄數(shù)C設(shè)為了10，將對T2與T3的差異進行判定時的規(guī)定誤差范圍設(shè)為了1.5倍，但C的值也可以不是10，規(guī)定誤差范圍也可以不是1.5倍。例如，也可以將C設(shè)為5并將規(guī)定誤差范圍設(shè)為1.3倍，或者將C設(shè)為20并將規(guī)定誤差范圍設(shè)為2倍。使規(guī)定誤差范圍在比1大的值的范圍內(nèi)越接近1，判定條件越嚴格，即使地圖的信息(Map點DB等信息)中僅包含有一點誤差，地圖編輯部15也會將地圖的信息清除。

此外，在圖7的步驟S507～步驟S508等中，也可以不使用計數(shù)器NI的值，而將從里程計取得的平移距離作為閾值來進行判定。例如也可以設(shè)為“直到里程計的平移距離成為基準平移距離以上為止”。這只要如下處理即可：在自身設(shè)備位置估算線程的步驟S220以及誤差判定線程的步驟S506以及步驟S511中取代設(shè)為“NI＝0”而設(shè)為“將從里程計取得的平移t設(shè)定給tI1”，在步驟S507中設(shè)為“將從里程計取得的平移t設(shè)定給tI2”，在步驟S508中進行|t11-tI2|(位置的變化量)是否小于基準平移距離的判定。

此外，作為不使用計數(shù)器NI的其他的方法，還可以在關(guān)鍵幀間，在從里程計取得的旋轉(zhuǎn)角一直為0的狀態(tài)下(即完全不旋轉(zhuǎn)而直線前進)檢測出平移距離D時，將里程計平移距離T2設(shè)為D來進行判定。這只要如下處理即可：在自身設(shè)備位置估算線程的步驟S220以及誤差判定線程的步驟S506以及步驟S511中取代設(shè)為“NI＝0”而設(shè)為“將從里程計取得的旋轉(zhuǎn)R設(shè)定給RI，將從里程計取得的平移t設(shè)定給tI1”，在步驟S505中設(shè)為“將從里程計取得的旋轉(zhuǎn)R與RI進行比較，若與RI保持相同(由于未旋轉(zhuǎn))則前進到步驟S507，若成為與RI不同的值(由于發(fā)生了旋轉(zhuǎn))則前進到步驟S506”，在步驟S507中設(shè)為“將從里程計取得的平移t設(shè)定給tI2”，在步驟S508中進行“|tI1-tI2|＜D”的判定。此時，步驟S510成為“將D設(shè)定給T2”。

進而，在由所搭載的加速度傳感器31，檢測出與自主移動裝置100的自主移動明顯不同的加速度的情況下，可以考慮例如人提起移動體來使其移動等，因此在該情況下控制部10將計數(shù)器NI清除為0。具體而言，緊挨在圖7的步驟S505之前(或之后)，控制部10追加“將加速度傳感器31的值與從里程計計算出的加速度進行比較，若產(chǎn)生超過規(guī)定范圍的差則前進到步驟S506”的處理即可。通過這樣，從而誤差判定線程中的判定動作被重置，因此能夠防止不經(jīng)意地清除地圖的信息(Map點DB等)的情況。從加速度傳感器31取得加速度的值時，控制部10相當于加速度取得部。此外，從里程計計算加速度時，控制部10相當于加速度計算部。另外，由于加速度作為三維的向量值而獲得，因此加速度的差也成為三維的向量值，但在是否產(chǎn)生了超過上述的規(guī)定范圍的差的判定中，只要通過差的向量的絕對值(要素的平方和的平方根)是否超過規(guī)定值來判定即可。

此外，在車輪發(fā)生了較大打滑的情況下，在極端的情況下發(fā)生了空轉(zhuǎn)的情況等下，由于驅(qū)動車輪的電動機的負荷突然變輕，因而流過電動機的電流發(fā)生變化。因此，通過確認里程計的測量結(jié)果與電動機電流的關(guān)系，能夠檢測發(fā)生了較大的打滑或空轉(zhuǎn)。若檢測出較大的打滑或空轉(zhuǎn)，則控制部10將計數(shù)器NI清除為0，由此誤差判定線程中的判定動作被重置，能夠從判定對象中排除。這具體而言，緊挨在圖7的步驟S505的之前(或之后。前述的加速度傳感器的判定與該打滑判定可以先進行任意一者)，控制部10追加“將電動機電流的實際的值與從里程計估算的電動機電流進行比較，若產(chǎn)生超過規(guī)定范圍的差則前進到步驟S506”的處理即可。取得流過電動機的電流的值時，控制部10相當于電流取得部。此外，基于里程計的信息對流過電動機的電流的值進行估算時，控制部10相當于電流估算部。通過這樣，從而能夠僅利用未發(fā)生較大的打滑或空轉(zhuǎn)等的狀況來進行判定，能夠防止不經(jīng)意地清除地圖的信息(Map點DB等)的情況。

此外，由于驅(qū)動部42經(jīng)年劣化，變得不能保持里程計的精度，因此控制部10也可以根據(jù)累積移動距離的增大，增大旋轉(zhuǎn)角φ_d的閾值φ_r。由此，在伴隨里程計的精度降低的旋轉(zhuǎn)動作的平移移動時也會進行判定，能夠清除里程計的精度較差時的地圖的信息(Map點DB等)。

進而，也可以根據(jù)利用環(huán)境來改變閾值φ_r。例如，也可以控制部10使用反射傳感器(未圖示)來測量行進面，若存在鏡面反射則設(shè)想為容易發(fā)生打滑的行進面而增大閾值φ_r，若存在擴散反射則設(shè)想為不易發(fā)生打滑的行進面而減小閾值φ_r。由此，能夠?qū)崿F(xiàn)根據(jù)行進面的狀態(tài)，若是容易發(fā)生打滑的行進面則在旋轉(zhuǎn)動作比較多的情況下也進行判定，若是不易發(fā)生打滑的行進面則僅在幾乎不存在旋轉(zhuǎn)動作的情況下進行判定。

此外，也可以定期地由控制部10使自主移動裝置100直線前進移動，使判定部14強制執(zhí)行里程計與視覺里程計之差的判定。由此，能夠防止在地圖的信息(Map點DB等)中包含誤差的狀態(tài)下連續(xù)進行自主移動。

在上述實施方式中，示出了攝像部41為單眼的單眼SLAM的例子，但即使是使用了多個攝像部的復(fù)眼SLAM也能夠采用同樣的構(gòu)成。例如在自主移動裝置100具備兩臺攝像部41的情況下，即使不移動從相同位置也能夠取得兩個圖像，所以在自身設(shè)備位置估算線程的初始化處理中，通過步驟S204的一次動作能夠取得兩個圖像。若在這兩個圖像中各自包含有基準值以上的特征點(步驟S206中進行判定)，則在步驟S207中進行“N＝N+2”，步驟S208會始終前進到“否”。然后，將兩個圖像中的一個作為前幀，將另一個作為當前幀來進行處理的情況下，在步驟S219中計算比例S時的“里程計平移距離”只要使用兩個攝像部間的距離即可。由此即使在從里程計取得的位置的精度較低的情況下也能夠穩(wěn)定地進行初始化。

但是，該情況下的兩個攝像部間的距離大多成為比通常使用的基準平移距離(例如1m)短的距離，在從里程計得到的平移距離的精度較高的情況下，有時反倒是進行與單眼SLAM同樣的初始化處理(例如僅使用多個攝像部當中的第一攝像部的初始化處理)能獲得較高的精度。因此，也可以進行與單眼SLAM同樣的初始化處理和前面段落所記載的基于兩個攝像部的初始化處理這兩者，分別開始制作地圖的信息(Map點DB、幀DB)，中途進行與誤差判定線程同樣的處理來判定各自的誤差，然后采用誤差較小的一方的地圖的信息。

另外，在上述的各實施方式中，將基準平移距離的值設(shè)定為了“例如1m”，但由于根據(jù)自主移動裝置自身的大小、速度、自主移動的環(huán)境、移動范圍等，最佳的基準平移距離會改變，因此關(guān)于基準平移距離的設(shè)定方法進行補充。

作為基準平移距離的設(shè)定方法，可以按將從關(guān)鍵幀上的所有Map點(3D位置已知的2D特征點)的3D位置到自身設(shè)備位置的距離進行平均得到的值(＝到在關(guān)鍵幀上觀察到的所有Map點的進深的平均距離)與平移距離之比來進行設(shè)定(例如，將到Map點的進深的平均距離的5％的距離設(shè)為基準平移距離等)。在此使用的“所有Map點”能夠通過由控制部10參照Map點DB來提取。此外，也可以根據(jù)實際環(huán)境上的平移距離的大小，例如若在桌子上活動則將10cm設(shè)定為基準平移距離，若在房間來回活動則將1m設(shè)定為基準平移距離，若在屋外的廣場等來回活動則將10m等設(shè)定為基準平移距離，還可以將與驅(qū)動部42的車輪的直徑相應(yīng)的值(例如車輪的直徑的10倍)設(shè)定為基準平移距離。

在作為基準平移距離而以實際環(huán)境上的距離進行設(shè)定的情況下，將SLAM坐標下的平移距離與比例S相乘來變換為實際環(huán)境上的距離之后進行比較(反之也同樣，在作為基準平移距離而設(shè)定SLAM坐標下的距離的情況下，將實際環(huán)境上的距離除以比例S來變換為SLAM坐標下的距離之后進行比較)。

另外，本發(fā)明的自主移動裝置100的各功能通過通常的PC(Personal Computer，個人計算機)等計算機也能夠?qū)嵤?。具體而言，在上述實施方式中，自主移動裝置100所進行的自主移動控制處理的程序作為預(yù)先存儲在存儲部20的ROM中的程序來進行了說明。但是，也可以將程序保存在軟盤、CD-ROM(Compact Disc Read Only Memory，光盤只讀存儲器)，DVD(Digital Versatile Disc，數(shù)字多用途盤)以及MO(Magneto-Optical Disc，磁光盤)等計算機可讀取的記錄介質(zhì)中來進行分發(fā)，通過將該程序讀入到計算機中進行安裝，從而構(gòu)成能夠?qū)崿F(xiàn)上述的各功能的計算機。

此外，該計算機既可以內(nèi)置于自主移動裝置100，也可以獨立于自主移動裝置100而存在。即，如圖8所示，也可以將控制部、存儲部設(shè)置于外部的服務(wù)器200。自主移動裝置100的控制部10也可以利用云計算技術(shù)，將由攝像部41、傳感器部30等取得的數(shù)據(jù)經(jīng)由通信部44發(fā)送到外部的服務(wù)器200，在使該服務(wù)器200進行了運算處理之后，經(jīng)由通信部44來接收服務(wù)器200所進行的運算結(jié)果，并對驅(qū)動部42等進行控制。在圖8中，服務(wù)器200的控制部50構(gòu)成為具備地圖制作部12、位置估算部13、判定部14以及地圖編輯部15，但只要在服務(wù)器200的控制部50和自主移動裝置100的控制部10中的任意一方設(shè)有各部即可。關(guān)于是服務(wù)器200的控制部50具備各部還是自主移動裝置100的控制部10具備各部是任意的。

以上，對本發(fā)明的優(yōu)選實施方式進行了說明，但本發(fā)明并不限定于這種特定的實施方式，在本發(fā)明中，包含權(quán)利要求書所記載的發(fā)明及其均等的范圍。