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

背景技術(shù)：

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

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

在先技術(shù)文獻(xiàn)

非專利文獻(xiàn)

非專利文獻(xiàn)1：andrewj.davison，“real-timesimultaneouslocalizationandmappingwithasinglecamera”，proceedingsofthe9thieeeinternationalconferenceoncomputervisionvolume2，2003，pp.1403-1410

非專利文獻(xiàn)2：richardhartley，andrewzisserman，“multipleviewgeometryincomputervision”，secondedition，cambridge.universitypress，march2004，chapter9

專利文獻(xiàn)

專利文獻(xiàn)1：jp特開2015-146091號公報

專利文獻(xiàn)2：jp特開2014-186694號公報

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

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

對于使用了這樣的圖像的自身設(shè)備位置估算而言，在攝像機(jī)拍攝的方向發(fā)生較大變化的情況下、或在所拍攝到的圖像中特征點較少的情況下，存在如下這樣的問題，即，無法獲得與過去的圖像的共同特征點，自身設(shè)備位置的估算失敗。

此外，在專利文獻(xiàn)1中，公開了如下的自身位置估算方法：基于移動量來估算自身位置的候補(bǔ)點，在地圖上決定多個測定點，基于距離映射將從所述測定點到壁障的距離設(shè)為評價距離，在第1評價工序中，基于評價距離來評價一致的程度，在第2評價工序中，將從外界傳感器朝向測定點的方向的單位向量即測定方向向量與測定點的壁障方向向量的內(nèi)積作為評價函數(shù)來進(jìn)行評價，將在第1評價工序中一致較大、并且在所述第2評價工序中評價函數(shù)較大的候補(bǔ)點估算為自身位置。

此外，在專利文獻(xiàn)2中，公開了一種自主移動式無人輸送車，其特征在于，在起動時，基于存儲在停止主要原因存儲單元中的停止主要原因，來判斷是否能夠從當(dāng)前的自身位置繼續(xù)進(jìn)行自主移動，在判斷為能夠進(jìn)行自主移動的情況下，基于存儲在自身位置存儲單元中的自身位置開始自主移動，在判斷為不能進(jìn)行自主移動的情況下，保持停止?fàn)顟B(tài)。

但是，對于專利文獻(xiàn)1所記載的技術(shù)而言，雖然能夠提高自身位置估算的精度，但存在自身設(shè)備位置的估算失敗的情況下，不能恢復(fù)這樣的課題。

此外，對于專利文獻(xiàn)2所記載的技術(shù)而言，仍然存在判斷為不能進(jìn)行自主移動的情況下，不能恢復(fù)這樣的課題。

因此，本發(fā)明為了解決上述問題而作，其目的在于提供一種能夠從不能進(jìn)行自身設(shè)備位置估算的狀態(tài)快速地恢復(fù)的自主移動裝置等。

用于解決課題的手段

本發(fā)明的自主移動裝置具備攝像部、圖像存儲部、控制部和驅(qū)動部，其中，

所述圖像存儲部存儲所述攝像部所拍攝到的多個圖像的信息，

所述控制部，

對所述驅(qū)動部進(jìn)行驅(qū)動處理使得基于所述攝像部所拍攝到的圖像的信息來自主地移動，

使用存儲在所述圖像存儲部中的多個圖像的信息來估算自身設(shè)備位置，

判定所述自身設(shè)備位置的估算結(jié)果是否唯一確定，

在判定為所述估算結(jié)果不唯一確定的情況下，控制所述驅(qū)動處理使得移動到所述自身設(shè)備位置的估算結(jié)果唯一確定的位置。

發(fā)明效果

根據(jù)本發(fā)明，能夠從不能進(jìn)行自身設(shè)備位置估算的狀態(tài)快速地恢復(fù)

附圖說明

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

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

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

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

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

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

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

圖8是表示第一實施方式所涉及的自主移動控制處理當(dāng)中的移動處理控制的處理的流程圖的圖。

圖9是表示第二實施方式所涉及的自主移動控制處理當(dāng)中的移動處理控制的處理的流程圖的圖。

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

圖11是表示第三實施方式所涉及的自主移動控制處理當(dāng)中的移動處理控制的處理的流程圖的圖。

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

符號說明

100…自主移動裝置，10、50…控制部，11…移動處理部，12…地圖制作部，13…位置估算部，14…估算判定部，15…移動處理控制部，16…旋轉(zhuǎn)拍攝部，20、60…存儲部，21…圖像存儲部，22…地圖存儲部，23…位置變化歷史記錄存儲部，24…位置歷史記錄存儲部，25…特征點存儲部，30…傳感器部，31…加速度傳感器，32…角速度傳感器，33…障礙物傳感器，41…攝像部，42…驅(qū)動部，43…輸入部，44、71…通信部，45…電源，200…服務(wù)器

具體實施方式

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

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

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

驅(qū)動部42是獨立2輪驅(qū)動型，是具備車輪和電動機(jī)的移動單元。自主移動裝置100能夠通過2個車輪的同一方向驅(qū)動來進(jìn)行前后的平行移動(平移移動)，能夠通過2個車輪的反向驅(qū)動來進(jìn)行原地的旋轉(zhuǎn)(朝向變更)，并能夠通過改變2個車輪各自的速度的驅(qū)動來進(jìn)行旋轉(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è)為rr，并將左車輪的轉(zhuǎn)速設(shè)為rl，則朝向變更的旋轉(zhuǎn)量(若以右旋轉(zhuǎn)為正)成為360°×d×(rl-rr)/(2×i)。通過將該平移移動量、旋轉(zhuǎn)量依次進(jìn)行相加，從而驅(qū)動部42作為所謂的里程計(odometry)而發(fā)揮作用，能夠測量自身設(shè)備位置(以移動開始時的位置以及朝向為基準(zhǔn)的位置以及朝向)。

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

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

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

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

存儲部20由rom(readonlymemory，只讀存儲器)、ram(randomaccessmemory，隨機(jī)存取存儲器)等構(gòu)成，包含圖像存儲部21、地圖存儲部22、位置變化歷史記錄存儲部23、位置歷史記錄存儲部24。在rom中，存儲有控制部10的cpu所執(zhí)行的程序(例如，后述的slam法的運算、自主移動控制處理所涉及的程序)、執(zhí)行程序上預(yù)先需要的數(shù)據(jù)。在ram中，存儲在程序執(zhí)行中被生成或者變更的數(shù)據(jù)。

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

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

在位置變化歷史記錄存儲部23中，存儲由移動處理部11使自身設(shè)備的位置怎樣發(fā)生了變化的歷史記錄。

在位置歷史記錄存儲部24中，存儲由位置估算部13估算出的自身設(shè)備位置的歷史記錄。

作為傳感器部30，具備加速度傳感器31、角速度傳感器32、障礙物傳感器33。加速度傳感器31是測量xyz方向(3軸)的加速度的傳感器。通過對這些值進(jìn)行時間積分，從而能夠求取xyz的各方向的速度，進(jìn)而通過對這些速度進(jìn)行時間積分，從而能夠求取xyz的各方向的位置的變化量(移動距離)(但是由于對傳感器值進(jìn)行二次積分，因而累積誤差變得相當(dāng)大)。

角速度傳感器32是測量角速度(每單位時間的角度移動量)的傳感器。已知與基于車輪的轉(zhuǎn)速來求取自身設(shè)備的朝向相比，使用角速度傳感器32來求取朝向更能提高精度。障礙物傳感器33是探測行進(jìn)過程中的障礙物的傳感器，例如是紅外線傳感器、超聲波傳感器。另外，也可以不搭載獨立的障礙物傳感器33，使用攝像部41來探測障礙物。此外，也可以具備對碰撞到其他物體的情況進(jìn)行探測的碰撞傳感器(bumpersensor)(未圖示)。此外，還可以具備檢測地磁來確定方位的方位傳感器(未圖示)。通過使用方位傳感器，能夠與來自里程計的取得值無關(guān)地取得利用了地磁的絕對朝向的信息。

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

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

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

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

移動處理部11進(jìn)行如下處理：操縱驅(qū)動部42來使自身設(shè)備移動。

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

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

估算判定部14判定位置估算部13是否能夠基于當(dāng)前已取得的信息來估算自身設(shè)備位置。更詳細(xì)而言，若已取得估算自身設(shè)備位置所需的信息，并且基于這些信息的自身設(shè)備位置的估算結(jié)果唯一確定則判定為能夠估算。反之，在未能取得估算自身設(shè)備位置所需的信息或者即使能夠取得但是基于這些信息的自身設(shè)備位置的估算結(jié)果不定(產(chǎn)生多個估算候補(bǔ)等)的情況下，判定為不能估算。

移動處理控制部15在估算判定部14判定為不能估算的情況下，控制移動處理部11以使自身設(shè)備位置自主地移動到能夠估算的位置。

圖3是自主移動裝置100的主流程。控制部10起動如下各線程，即，自身設(shè)備位置估算線程(步驟s101)、地圖制作線程(步驟s102)、閉環(huán)線程(步驟s103)。通過自身設(shè)備位置估算線程、地圖制作線程進(jìn)行動作，從而基于slam法，開始地圖的信息以及視覺里程計(使用地圖和圖像而估算出的自身設(shè)備位置的信息)的生成。然后，控制部10將位置變化歷史記錄計數(shù)器dc(表示向位置變化歷史記錄存儲部23的位置變化歷史記錄的登記數(shù)的變量)進(jìn)行清零(步驟s104)。然后，控制部10判定是否動作結(jié)束(步驟s105)，若動作結(jié)束(步驟s105：“是”)則結(jié)束動作，若動作未結(jié)束(步驟s105：“否”)則地圖制作部12進(jìn)行地圖的信息的制作/更新(步驟s106)。

接著，控制部10判定估算狀態(tài)變量es(表示是否為能夠進(jìn)行位置估算的狀態(tài)的變量)的值是否為“不能進(jìn)行位置估算”(步驟s107)。若是不能進(jìn)行位置估算(步驟s107：“是”)，則進(jìn)行用于自主地移動到能夠進(jìn)行位置估算的位置的處理即“移動處理控制”之后(步驟s110)返回至步驟s105。該“移動處理控制”實現(xiàn)了本實施方式的特征性的功能，關(guān)于其詳情在后面敘述。

若估算狀態(tài)變量es的值不是“不能進(jìn)行位置估算”(步驟s107：“否”)則使位置變化歷史記錄計數(shù)器dc遞增(步驟s108)，為了自主移動而由移動處理部11向驅(qū)動部42指示所希望的動作，并且將伴隨該動作的位置的變化量與時間戳(使用當(dāng)前時刻等能夠與位置歷史記錄存儲部24的時間戳進(jìn)行比較的數(shù)據(jù))一起登記到位置變化歷史記錄存儲部23的第dc個項目(步驟s109)，返回至步驟s105。在該步驟s109中的位置的變化量的登記處理中，移動處理部11相當(dāng)于位置變化歷史記錄登記部。

在此，位置的變化量，是從在步驟s109中移動處理部11向驅(qū)動部42發(fā)出指示之前的位置起，到驅(qū)動部42按照指示進(jìn)行動作從而自主移動裝置100移動之后的位置為止的(步驟s109中的移動處理的一次動作的)變化量。該變化量既可以使用里程計信息來取得，也可以使用加速度傳感器31或角速度傳感器32的值來取得。通常使用里程計信息更能夠取得高精度的位置變化量，但在通過人的手而被移動的情況等下，里程計信息沒有用處(在該情況下加速度傳感器31或角速度傳感器32的值有用)。因此，期望通過使用里程計信息和傳感器信息這兩者，從而通過傳感器信息來補(bǔ)償在車輪打滑或者通過人的手而被移動的情況等下產(chǎn)生的里程計信息的誤差。

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

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

首先使用圖4來說明自身設(shè)備位置估算線程的初始化處理。位置估算部13將“初始化中”設(shè)置給估算狀態(tài)變量es，并清除位置歷史記錄登記計數(shù)器hc(表示向位置歷史記錄存儲部24的位置歷史記錄的登記數(shù)的變量)和位置歷史記錄檢索計數(shù)器sc(從位置歷史記錄存儲部24取得位置歷史記錄時使用的變量)(步驟s201)。接著將-1設(shè)置給幀計數(shù)器n(表示攝像部41的圖像取得數(shù)的變量)(步驟s202)。接下來位置估算部13判定是否動作結(jié)束(步驟s203)。若動作結(jié)束(步驟s203：“是”)則結(jié)束，若動作未結(jié)束(步驟s203：“否”)，則由攝像部41取得圖像(步驟s204)。圖像例如能夠以30fps取得(所取得的圖像也被稱為幀)。

接著，從所取得的圖像內(nèi)取得2d特征點(步驟s205)。所謂2d特征點，是圖像中的邊緣部分等圖像內(nèi)的特征性的部分，能夠使用sift(scale-invariantfuturetransform，尺度不變特征轉(zhuǎn)換)、surf(speed-uprobustfeatures，加速穩(wěn)健特征)等算法來取得。另外，為了取得2d特征點，也可以使用其他算法。

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

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

若幀計數(shù)器n不為0(步驟s208：“否”)則可知已經(jīng)取得了兩個圖像，因此位置估算部13在這兩個圖像間取得2d特征點的對應(yīng)(實際環(huán)境上的同一點存在于各個圖像中，能取得其對應(yīng))(步驟s209)。在該步驟s209中，位置估算部13相當(dāng)于對應(yīng)特征點取得部。由于若在此取得的特征點的對應(yīng)數(shù)小于5，則不能進(jìn)行后述的兩個圖像間的姿勢的估算，因此估算判定部14判定特征點的對應(yīng)數(shù)是否小于5(步驟s210)。若小于5(步驟s210：“是”)則為了重新取得初始圖像，返回至步驟s202。若特征點的對應(yīng)數(shù)為5點以上(步驟s210：“否”)，則通過使用two-viewstructurefrommotion法，從而能夠估算兩個圖像間的姿勢(取得各個圖像的位置的差分(平移向量t)以及朝向的差分(旋轉(zhuǎn)矩陣r))(步驟s211)。

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

另外，即使特征點的對應(yīng)數(shù)為5點以上，也存在兩個圖像間的姿勢的估算值不唯一確定的情況(存在條件不足而不確定的情況、即使想要通過最小平方法來唯一確定但似乎可靠的估算值不唯一確定而成為多個的情況等)。在位置估算部13估算兩個圖像間的姿勢(取得各個圖像的位置的差分(平移向量t)以及朝向的差分(旋轉(zhuǎn)矩陣r))時，在估算值唯一確定的情況下，估算判定部14判定為能夠估算，在估算值不唯一確定的情況下，估算判定部14判定為不能估算(步驟s212)。

在估算判定部14判定為不能估算的情況下(步驟s212：“否”)，返回至步驟s202，從初始圖像的取得開始重新進(jìn)行處理。在估算判定部14判定為能夠估算的情況下(步驟s212：“是”)，也就是說已經(jīng)求出兩個圖像間的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)，因此前進(jìn)至步驟s213。

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

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

(uv1)’～p(xyz1)’…(1)

在上述的式(1)中，p為3×4的矩陣，根據(jù)表示攝像機(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如上所述作為該幀姿勢來求取。此外，攝像機(jī)的內(nèi)部參數(shù)a由焦點距離和攝像元件尺寸來決定，因此只要決定攝像部41便成為常數(shù)。

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

(u1v11)’～a(i|0)(xyz1)’…(3)

(u2v21)’～a(r|t)(xyz1)’…(4)

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

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

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

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

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

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

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

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

以上，自身設(shè)備位置估算線程的初始化處理完成，因此進(jìn)入到圖5的自身設(shè)備位置估算(主處理)(步驟s220)。

接下來使用圖5對自身設(shè)備位置估算線程的主處理進(jìn)行說明。首先，位置估算部13將“能夠進(jìn)行位置估算”設(shè)置給估算狀態(tài)變量es(步驟s231)。這是因為在初始化處理中，實現(xiàn)了自身設(shè)備位置的估算，在此時點可以說是處于能夠進(jìn)行位置估算的狀態(tài)。接下來位置估算部13判定是否動作結(jié)束(步驟s232)。若動作結(jié)束(步驟s232：“是”)則結(jié)束，若動作未結(jié)束(步驟s232：“否”)，則由攝像部41取得圖像(步驟s233)，并使幀計數(shù)器n遞增(步驟s234)。

接著，位置估算部13取得所拍攝到的圖像中包含的2d特征點(步驟s235)。然后，從登記在幀數(shù)據(jù)庫中的以前的關(guān)鍵幀(例如關(guān)鍵幀編號為nkf的圖像)的信息中，取得該圖像的信息中所包含的2d特征點當(dāng)中的3d位置已知(成為登記在map點數(shù)據(jù)庫中的map點)的2d特征點，提取與當(dāng)前拍攝到的圖像之間能取得對應(yīng)的2d特征點(對應(yīng)特征點)(步驟s236)。

然后，估算判定部14判定對應(yīng)特征點的個數(shù)是否小于規(guī)定值(例如5)(步驟s237)，在小于規(guī)定值的情況下(步驟s237：“是”)，不能實現(xiàn)似乎可靠的自身設(shè)備位置的估算，因此前進(jìn)至步驟s247。在此，也可以不是立刻前進(jìn)至步驟s247，而是返回至步驟s236，從登記在幀數(shù)據(jù)庫中的關(guān)鍵幀當(dāng)中檢索對應(yīng)特征點的個數(shù)為規(guī)定值以上的關(guān)鍵幀。在該情況下，在登記在幀數(shù)據(jù)庫中的關(guān)鍵幀當(dāng)中未找到對應(yīng)特征點的個數(shù)為規(guī)定值以上的關(guān)鍵幀的情況下前進(jìn)至步驟s247。

若能夠提取規(guī)定值以上的對應(yīng)特征點(步驟s237：“否”)，則位置估算部13從map點數(shù)據(jù)庫中取得對應(yīng)特征點各自的3d位置(xi，yi，zi)(步驟s238)。若將當(dāng)前拍攝到的圖像中包含的對應(yīng)特征點的幀坐標(biāo)設(shè)為(ui，vi)，將該對應(yīng)特征點的3d位置設(shè)為(xi，yi，zi)(i取從1到對應(yīng)特征點數(shù)的值)，則將各對應(yīng)特征點的3d位置(xi，yi，zi)通過以下的算式(5)而投影到幀坐標(biāo)系的值(uxi，vxi)與幀坐標(biāo)(ui，vi)理想而言應(yīng)該是一致的。

(uxivxi1)’～a(r|t)(xiyizi1)’…(5)

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

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

在該自身設(shè)備位置的估算中，存在自身設(shè)備的姿勢的估算值不唯一確定的情況(存在條件不足而不確定的情況(例如兩個圖像中的對應(yīng)特征點數(shù)小于5點的情況)、即使想要通過最小平方法來唯一確定但似乎可靠的估算值也不唯一確定而成為多個的情況(例如在算式(6)中，最小值大于規(guī)定值的情況，或極小值存在多個并且將最小值(最小的極小值)除以第二小的極小值而得到的值大于規(guī)定值的情況)等)。在位置估算部13估算自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)時，在估算值唯一確定的情況下，估算判定部14判定為能夠估算，在估算值不唯一確定的情況下，估算判定部14判定為不能估算(步驟s240)。

在估算判定部14判定為不能估算的情況下(步驟s240：“否”)，前進(jìn)至步驟s247。在估算判定部14判定為能夠估算的情況下(步驟s240：“是”)，也就是說已經(jīng)求出自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)，因此前進(jìn)至步驟s241。

由于求出slam坐標(biāo)下的當(dāng)前的自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)，因此控制部10使位置歷史記錄登記計數(shù)器hc遞增，在位置歷史記錄存儲部24的第hc個項目中，與時間戳(使用當(dāng)前時刻等能夠與位置變化歷史記錄存儲部23的時間戳進(jìn)行比較的數(shù)據(jù))一起登記當(dāng)前的自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)，并將位置歷史記錄登記計數(shù)器hc的值設(shè)置給位置歷史記錄檢索計數(shù)器sc(步驟s241)。在該步驟s241中，控制部10相當(dāng)于位置歷史記錄登記部。然后，位置估算部13通過對當(dāng)前的自身設(shè)備的姿勢乘以比例s，來求取vo(視覺里程計)(步驟s242)。vo能夠作為實際環(huán)境下的自身設(shè)備的位置以及朝向來利用。

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

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

然后，位置估算部13為了使地圖制作線程知道產(chǎn)生了新的關(guān)鍵幀，在地圖制作線程的關(guān)鍵幀隊列中設(shè)置關(guān)鍵幀計數(shù)器nkf(步驟s246)。然后，返回至步驟s232。另外，關(guān)鍵幀計數(shù)器nkf、比例s、map點db、幀db、位置歷史記錄存儲部24、位置變化歷史記錄存儲部23、位置歷史記錄登記計數(shù)器hc、位置歷史記錄檢索計數(shù)器sc、位置變化歷史記錄計數(shù)器dc都存儲在存儲部20中，使得能夠跨越線程來參照值。

接下來對步驟s247以后進(jìn)行說明。這是不能進(jìn)行自身設(shè)備位置的估算時的處理。首先，位置估算部13對估算狀態(tài)變量es設(shè)置“不能進(jìn)行位置估算”(步驟s247)。接著位置估算部13判定是否動作結(jié)束(步驟s248)。若動作結(jié)束(步驟s248：“是”)則結(jié)束，若動作未結(jié)束(步驟s248：“否”)，則由攝像部41取得圖像(步驟s249)。

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

控制部10判定是否能夠從幀db中發(fā)現(xiàn)與取得圖像的類似度成為基準(zhǔn)圖像類似度以上的關(guān)鍵幀(步驟s251)。若未能發(fā)現(xiàn)(步驟s251：“否”)則返回至步驟s248。若能夠發(fā)現(xiàn)(步驟s251：“是”)則分別對取得圖像的2d特征點和所發(fā)現(xiàn)的類似圖像的2d特征點進(jìn)行取得(步驟s252)，并在這兩個圖像間取得2d特征點的對應(yīng)(步驟s253)。

然后，通過與步驟s239同樣的方法來估算自身設(shè)備的姿勢(步驟s254)。此時，估算判定部14與步驟s240同樣地判定是否能夠估算(步驟s255)，若不能估算(步驟s255：“否”)則返回至步驟s248，若能夠估算(步驟s255：“是”)，也就是說能夠進(jìn)行自身設(shè)備位置的估算，所以前進(jìn)至步驟s231。

這樣，自身設(shè)備位置估算線程的初始化結(jié)束后，在估算狀態(tài)變量es中會設(shè)置“能夠進(jìn)行位置估算”或者“不能進(jìn)行位置估算”中的任意一者，因此在圖3所示的主流程中，若es的值為“能夠進(jìn)行位置估算”，則自主移動裝置100進(jìn)行所希望的動作，若es的值為“不能進(jìn)行位置估算”，則能夠求取能夠進(jìn)行位置估算那樣的位置來進(jìn)行移動處理控制。

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

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

地圖制作部12參照幀db，提取在前關(guān)鍵幀(關(guān)鍵幀編號為mkf-1的關(guān)鍵幀)的2d特征點與當(dāng)前關(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位置計算而進(jìn)入到針對下一個對應(yīng)特征點(map點db中未登記的對應(yīng)特征點)的處理，也可以重新進(jìn)行3d位置計算，對map點db中已經(jīng)登記的3d位置、幀db中的對應(yīng)特征點對應(yīng)的3d位置進(jìn)行更新。

接著，地圖制作部12判定關(guān)鍵幀隊列是否為空(步驟s308)。若為空(步驟s308：“是”)，則針對全部關(guān)鍵幀的姿勢和全部map點的3d位置，進(jìn)行捆綁調(diào)整處理，提高精度后(步驟s309)前進(jìn)至步驟s310。若關(guān)鍵幀隊列不為空(步驟s308：“否”)則前進(jìn)至步驟s310。接著地圖制作部12將mkf設(shè)置到合環(huán)線程的關(guān)鍵幀隊列(步驟s310)，返回至步驟s301。

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

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

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

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

首先，控制部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ù)進(jìn)入到關(guān)鍵幀隊列中。然后，在lkf為2以上的情況下(步驟s404：“是”)，進(jìn)行以下的處理。

控制部10參照幀db，從幀db中檢索與當(dāng)前關(guān)鍵幀(關(guān)鍵幀編號為lkf的關(guān)鍵幀)之間的“關(guān)鍵幀自身的特征”的類似度為基準(zhǔn)圖像類似度以上的關(guān)鍵幀(步驟s405)。

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

接下來關(guān)于自主移動裝置100的主流程(圖3)的步驟s110的處理即移動處理控制，使用圖8來進(jìn)行說明。首先，控制部10判定位置歷史記錄檢索計數(shù)器sc的值是否為0(步驟s501)。若sc為0，則意味著針對位置歷史記錄存儲部24中存儲的全部位置已經(jīng)嘗試了回退處理(returnprocess)但并未變?yōu)槟軌蜻M(jìn)行位置估算的狀態(tài)，在該情況下(步驟s501：“是”)為了在未能進(jìn)行位置的估算的狀態(tài)下進(jìn)行自主動作而進(jìn)行與主流程(圖3)的步驟s108、s109同樣的動作(步驟s502、s503)，并返回至主流程(圖3)的步驟s105。

若sc不為0(步驟s501：“否”)，則取得存儲在位置歷史記錄存儲部24中的第sc個信息(自身設(shè)備的姿勢(位置)的信息以及當(dāng)時的時刻)(步驟s504)。所取得的信息當(dāng)中，自身設(shè)備的姿勢(位置)的信息保存在hp(對從位置歷史記錄存儲部24中取得的自身設(shè)備的位置的信息進(jìn)行保存的變量)中，時刻保存在ht(對從位置歷史記錄存儲部24中取得的時刻進(jìn)行保存的變量)中。然后，判定位置變化歷史記錄計數(shù)器dc的值是否為0(步驟s505)。若dc為0則意味著雖然嘗試了位置變化歷史記錄存儲部23中存儲的全部的位置的變化但并未變?yōu)槟軌蜻M(jìn)行位置估算的狀態(tài)，在該情況下(步驟s505：“是”)為了在未能進(jìn)行位置的估算的狀態(tài)下進(jìn)行自主動作而進(jìn)行與主流程(圖3)的步驟s108、s109同樣的動作(步驟s502、s503)，并返回至主流程(圖3)的步驟s105。

若dc不為0(步驟s505：“否”)，則取得存儲在位置變化歷史記錄存儲部23中的第dc個信息(位置變化量以及當(dāng)時的時刻)(步驟s506)。所取得的信息當(dāng)中，位置變化量保存在dp(對從位置變化歷史記錄存儲部23中取得的位置變化量進(jìn)行保存的變量)中，時刻保存在dt(對從位置變化歷史記錄存儲部23中取得的時刻進(jìn)行保存的變量)中。然后，判定從位置歷史記錄存儲部24中取得的時刻ht與從位置變化歷史記錄存儲部23中取得的時刻dt相比是否為過去的時刻(是否為較小的值)(步驟s507)。

若ht與dt相比為過去(步驟s507：“是”)，則自身設(shè)備還未完全返回到從位置歷史記錄存儲部24中取得的過去的hp的位置，所以前進(jìn)至步驟s508。若ht和dt為相同時刻或dt一方為過去(步驟s507：“否”)則為了進(jìn)行移動處理控制中的最后的移動處理而前進(jìn)至步驟s510。

在步驟s508中，基于登記在位置變化歷史記錄存儲部23中的位置變化量dp，由移動處理控制部15使移動處理部11進(jìn)行消除該變化量的移動處理(步驟s508)。例如若dp為“向前方前進(jìn)了10cm”，則此處的移動處理成為“向后方退回10cm”的處理。然后，使位置變化歷史記錄計數(shù)器dc遞減(步驟s509)并返回至步驟s505。

步驟s510也與步驟s508同樣地，基于登記在位置變化歷史記錄存儲部23中的位置變化量dp，由移動處理控制部15使移動處理部11進(jìn)行消除該變化量的移動處理(步驟s510)。通過該移動處理，從而(如果存儲在位置變化歷史記錄存儲部23中的信息、移動處理部11的移動距離沒有誤差)自主移動裝置100應(yīng)該會返回到過去實現(xiàn)了位置估算的位置(由hp所示的位置)，所以為了正確地進(jìn)行自身設(shè)備位置估算線程中的自身設(shè)備位置估算，該最后的移動處理期望降低速度來進(jìn)行。特別是在步驟s507的判定時若ht與dt的值相等，則通過該移動應(yīng)該會返回到hp所示的位置，所以也可以在步驟s510與步驟s511之間(為了等待自身設(shè)備位置估算線程中的自身設(shè)備位置估算的進(jìn)行)追加規(guī)定時間不移動而停止的處理。

接著，使位置變化歷史記錄計數(shù)器dc遞減(步驟s511)，并使位置歷史記錄檢索計數(shù)器sc遞減(步驟s512)，返回至主流程的步驟s105。使位置歷史記錄檢索計數(shù)器sc遞減，是為了在即使通過本次的移動處理控制也未能在自身設(shè)備位置估算線程中實現(xiàn)自身設(shè)備位置估算的情況下，在下次的移動處理控制中，進(jìn)一步返回到過去的位置來嘗試自身設(shè)備位置估算。但是，若在到下次為止的期間自身設(shè)備位置估算成功，則在圖5的步驟s241中位置歷史記錄登記計數(shù)器hc的值被設(shè)置到位置歷史記錄檢索計數(shù)器sc。通過進(jìn)行這樣的移動處理控制，從而能夠返回到過去實現(xiàn)了自身設(shè)備位置估算的位置，能夠進(jìn)行自身設(shè)備位置估算。

接下來，作為與上述不同的移動處理控制，對進(jìn)行圖9的流程圖所示的移動處理控制的第二實施方式進(jìn)行說明。第二實施方式除了移動處理控制的流程圖以外都與第一實施方式相同。

首先，控制部10判定位置歷史記錄檢索計數(shù)器sc的值是否為0(步驟s601)。若sc為0，則意味著針對位置歷史記錄存儲部24中存儲的全部位置已經(jīng)嘗試了回退處理但并未變?yōu)槟軌蜻M(jìn)行位置估算的狀態(tài)，在該情況下(步驟s601：“是”)為了在未能進(jìn)行位置的估算的狀態(tài)下進(jìn)行自主動作而進(jìn)行與主流程(圖3)的步驟s108、s109同樣的動作(步驟s602、s603)，并返回至主流程(圖3)的步驟s105。

若sc不為0(步驟s601：“否”)，則取得存儲在位置歷史記錄存儲部24中的第sc個信息(自身設(shè)備的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)的信息hp以及當(dāng)時的時刻ht)(步驟s604)。然后，將位置變化歷史記錄計數(shù)器dc的值設(shè)置給位置變化歷史記錄工作計數(shù)器wc(在從位置變化歷史記錄存儲部23中取得位置變化的歷史記錄時使用的變量)，并對位置變化歷史記錄堆棧(保存位置變化量的堆棧)進(jìn)行初始化(步驟s605)。另外，堆棧為先入后出的數(shù)據(jù)結(jié)構(gòu)。

然后，控制部10判定位置變化歷史記錄工作計數(shù)器wc是否為0(步驟s606)。若wc為0則意味著雖然嘗試了位置變化歷史記錄存儲部23中存儲的全部的位置的變化但并未變?yōu)槟軌蜻M(jìn)行位置估算的狀態(tài)，在該情況下(步驟s606：“是”)為了在未能進(jìn)行位置的估算的狀態(tài)下進(jìn)行自主動作而進(jìn)行與主流程(圖3)的步驟s108、s109同樣的動作(步驟s602、s603)，并返回至主流程(圖3)的步驟s105。

若wc不為0(步驟s606：“否”)，則控制部10取得存儲在位置變化歷史記錄存儲部23中的第wc個信息(位置變化量dp以及當(dāng)時的時刻dt)(步驟s607)。然后，判定從位置歷史記錄存儲部24中取得的時刻ht與從位置變化歷史記錄存儲部23中取得的時刻dt相比是否為過去的時刻(是否為較小的值)(步驟s608)。

若ht與dt相比為過去(步驟s608：“是”)，則自身設(shè)備還未完全返回到從位置歷史記錄存儲部24中取得的過去的hp的位置，所以前進(jìn)至步驟s609。若ht和dt為相同時刻或dt一方為過去(步驟s608：“否”)則為了根據(jù)位置變化歷史記錄來累計估算自身設(shè)備的位置而前進(jìn)至步驟s611。

在步驟s609中，控制部10將位置變化量dp的值壓入到位置變化歷史記錄堆棧中。然后，使位置變化歷史記錄工作計數(shù)器wc遞減(步驟s610)并返回至步驟s606。由此，到目前為止的位置變化都被積累到位置變化歷史記錄堆棧中。

在步驟s611中，控制部10將從位置歷史記錄存儲部24中取得的過去的hp的姿勢(平移向量t以及旋轉(zhuǎn)矩陣r)作為初始值而設(shè)置給mp(為了求取自身設(shè)備位置的估算值而使用的變量)，進(jìn)行從位置變化歷史記錄堆棧中取出數(shù)據(jù)(dp)并加到mp上的處理，直到位置變化歷史記錄堆棧成為空為止。由此，在mp中，能夠得到通過從過去的能夠進(jìn)行自身設(shè)備位置估算的狀態(tài)時的位置hp起對位置變化歷史記錄dp進(jìn)行累計而得到的當(dāng)前的自身設(shè)備位置的估算值。在該處理中，控制部10相當(dāng)于累計位置估算部。

接著，移動處理控制部15控制移動處理部11，使得從當(dāng)前的自身設(shè)備位置的估算值mp，移動到過去的自身設(shè)備位置hp(步驟s612)。具體而言，進(jìn)行控制，使得進(jìn)行從hp的平移向量中減去mp的平移向量的量的平移移動，并進(jìn)行對hp的旋轉(zhuǎn)矩陣乘以mp的旋轉(zhuǎn)矩陣的逆矩陣而得到的旋轉(zhuǎn)矩陣所表示的旋轉(zhuǎn)量的旋轉(zhuǎn)動作。通過該移動處理控制，從而(如果存儲在位置變化歷史記錄存儲部23中的信息、移動處理部11的移動距離沒有誤差)自主移動裝置100應(yīng)該會返回到過去實現(xiàn)了位置估算的位置(由hp所示的位置)，所以也可以在步驟s612與步驟s613之間(為了等待自身設(shè)備位置估算線程中的自身設(shè)備位置估算的進(jìn)行)追加規(guī)定時間不移動而停止的處理。

接著，使位置歷史記錄檢索計數(shù)器sc遞減(步驟s613)，并返回至主流程的步驟s105。使位置歷史記錄檢索計數(shù)器sc遞減，是為了在即使通過本次的移動處理控制也未能在自身設(shè)備位置估算線程中實現(xiàn)自身設(shè)備位置估算的情況下，在下次的移動處理控制中，進(jìn)一步返回到過去的位置來嘗試自身設(shè)備位置估算。但是，若在到下次為止的期間自身設(shè)備位置估算成功，則在圖5的步驟s241中位置歷史記錄登記計數(shù)器hc的值被設(shè)置到位置歷史記錄檢索計數(shù)器sc。通過進(jìn)行這樣的移動處理控制，從而能夠返回到過去實現(xiàn)了自身設(shè)備位置估算的位置，能夠進(jìn)行自身設(shè)備位置估算。

第二實施方式若與第一實施方式相比較，則針對位置變化歷史記錄存儲部23中所存儲的位置變化并不是一個一個進(jìn)行移動處理，而是匯總來進(jìn)行移動處理，因此存在如下這樣的優(yōu)點，即，移動處理控制時的驅(qū)動部42的誤差不容易積累。

至此說明的兩個實施方式雖然都進(jìn)行了返回到過去實現(xiàn)了自身設(shè)備位置估算的位置的處理，但是它們都存在如下這樣的問題，即，若存儲在位置變化歷史記錄存儲部23中的信息或基于移動處理部11的移動距離不正確則不能返回到過去的正確位置。

因此，對不需要位置變化歷史記錄存儲部23、位置歷史記錄存儲部24的第三實施方式進(jìn)行說明。圖10中示出第三實施方式的構(gòu)成，若與第一或第二實施方式相比較，則在以下方面不同，即，在控制部10中存在旋轉(zhuǎn)拍攝部16，以及在存儲部20中不存在位置變化歷史記錄存儲部23、位置歷史記錄存儲部24而存在特征點存儲部25。

關(guān)于圖3～圖7的各流程，僅是省略與位置變化歷史記錄計數(shù)器dc、位置變化歷史記錄存儲部23、位置歷史記錄登記計數(shù)器hc、位置歷史記錄檢索計數(shù)器sc相關(guān)的處理，其余與第三實施方式的處理流程相同。移動處理控制的處理流程與第一、第二實施方式較大不同，因此以下使用圖11來進(jìn)行說明。

首先，旋轉(zhuǎn)拍攝部16對旋轉(zhuǎn)拍攝標(biāo)志rf(表示當(dāng)前是否是旋轉(zhuǎn)拍攝的處理中的變量)設(shè)置1(步驟s701)，并對累積旋轉(zhuǎn)角φs進(jìn)行清零(步驟s702)。接著，由攝像部41取得圖像(步驟s703)。接著，從所取得的圖像內(nèi)取得2d特征點(步驟s704)。然后，將所取得的2d特征點的個數(shù)和累積旋轉(zhuǎn)角φs登記到特征點存儲部25中(步驟s705)。

接著，旋轉(zhuǎn)拍攝部16判定旋轉(zhuǎn)拍攝標(biāo)志rf是否為2(步驟s706)。若rf為2(步驟s706：“是”)則意味著1圈旋轉(zhuǎn)的拍攝完成，所以前進(jìn)至步驟s712。若rf不為2(步驟s706：“否”)，則判定累積旋轉(zhuǎn)角φs+旋轉(zhuǎn)角φ1是否小于360°(步驟s707)。在此，旋轉(zhuǎn)角φ1是基于旋轉(zhuǎn)拍攝部16的一次旋轉(zhuǎn)動作的旋轉(zhuǎn)角，例如是30°。φ1期望設(shè)定攝像部41的視角的一半以下的角度，使得能夠從周圍的環(huán)境拍攝盡可能多的特征點。

旋轉(zhuǎn)拍攝部16在累積旋轉(zhuǎn)角φs+旋轉(zhuǎn)角φ1小于360°的情況下(步驟s707：“是”)，對驅(qū)動部42發(fā)出指示以進(jìn)行旋轉(zhuǎn)角φ1的旋轉(zhuǎn)動作(步驟s708)，在累積旋轉(zhuǎn)角φs上加上旋轉(zhuǎn)角φ1(步驟s709)，之后返回至步驟s703。此處的旋轉(zhuǎn)動作若是能夠通過使驅(qū)動部42的兩個車輪反向等速旋轉(zhuǎn)等而不移動位置地旋轉(zhuǎn)，則期望進(jìn)行不移動位置的旋轉(zhuǎn)動作。在只能使兩個車輪向相同方向旋轉(zhuǎn)的情況下，期望進(jìn)行旋轉(zhuǎn)半徑盡可能小的旋轉(zhuǎn)動作。

旋轉(zhuǎn)拍攝部16在累積旋轉(zhuǎn)角φs+旋轉(zhuǎn)角φ1為360°以上的情況下(步驟s707：“否”)，對驅(qū)動部42發(fā)出指示以進(jìn)行旋轉(zhuǎn)角(360°-φs)的旋轉(zhuǎn)動作(步驟s710)，并對旋轉(zhuǎn)拍攝標(biāo)志rf設(shè)置2(步驟s711)，之后返回至步驟s703。

在步驟s712中，移動處理控制部15從特征點存儲部25中檢索登記到特征點存儲部25中的2d特征點的個數(shù)最多時的累積旋轉(zhuǎn)角φs，并控制移動處理部11以進(jìn)行旋轉(zhuǎn)角φs的旋轉(zhuǎn)動作(步驟s713)。然后，移動處理控制部15控制移動處理部11以慢慢地向前方移動規(guī)定距離(例如基準(zhǔn)平移距離的3倍)(步驟s714)。通過慢慢地移動，從而能夠增加在自身設(shè)備位置估算線程中能夠估算自身設(shè)備位置的定時。

然后，旋轉(zhuǎn)拍攝部16將旋轉(zhuǎn)拍攝標(biāo)志rf清零(步驟s715)，并返回至主流程的步驟s105。通過進(jìn)行這樣的動作，從而向2d特征點的個數(shù)多的方向進(jìn)行移動，所以自身設(shè)備位置的估算成功的可能性得到提高。另外，在本實施方式中僅關(guān)注了2d特征點的個數(shù)，但也可以不僅關(guān)注個數(shù)還關(guān)注質(zhì)量，利用2d特征點的個數(shù)和質(zhì)量來定義評價函數(shù)，在步驟s712中進(jìn)行對該2d特征點的評價函數(shù)最高的旋轉(zhuǎn)角φs進(jìn)行檢索的處理。

此外，也可以適當(dāng)切換進(jìn)行第一、第二、第三的各實施方式的移動處理控制。例如，可以考慮如下的實施方式等：首先進(jìn)行第一實施方式的移動處理控制，若即便如此也不能進(jìn)行自身設(shè)備位置估算，則進(jìn)行第三實施方式的移動處理控制的實施方式；首先進(jìn)行第二實施方式的移動處理控制，若即便如此也不能進(jìn)行自身設(shè)備位置估算，則進(jìn)行第三實施方式的移動處理控制的實施方式；首先進(jìn)行第一實施方式的移動處理控制，若即便如此也不能進(jìn)行自身設(shè)備位置估算，則進(jìn)行第二實施方式的移動處理控制，若即便如此也不能進(jìn)行自身設(shè)備位置估算，則進(jìn)行第三實施方式的移動處理控制的實施方式。

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

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

在作為基準(zhǔn)平移距離而以實際環(huán)境上的距離進(jìn)行設(shè)定的情況下，對slam坐標(biāo)下的平移距離乘以比例s來變換為實際環(huán)境上的距離之后進(jìn)行比較(反之也同樣，在作為基準(zhǔn)平移距離而設(shè)定slam坐標(biāo)下的距離的情況下，將實際環(huán)境上的距離除以比例s來變換為slam坐標(biāo)下的距離之后進(jìn)行比較)。

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

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

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