本發(fā)明涉及自主移動(dòng)裝置、自主移動(dòng)方法以及程序存儲(chǔ)介質(zhì)。

背景技術(shù)：

根據(jù)用途而自主進(jìn)行移動(dòng)的自主移動(dòng)裝置不斷普及。例如已知為了室內(nèi)的清掃而自主進(jìn)行移動(dòng)的自主移動(dòng)裝置。一般，這樣的自主移動(dòng)裝置需要進(jìn)行實(shí)空間的地圖的作成和實(shí)空間內(nèi)的自機(jī)位置的估計(jì)。

作為用于作成實(shí)空間的地圖的手法，例如已知slam(simultaneouslocalizationandmapping，即時(shí)定位與地圖構(gòu)建)法。利用單鏡頭攝像機(jī)的slam技術(shù)的基本的原理記載于非專(zhuān)利文獻(xiàn)1以及非專(zhuān)利文獻(xiàn)2中，通過(guò)從攝像機(jī)所拍攝的動(dòng)態(tài)圖像的多幀追蹤同一特征點(diǎn)，來(lái)進(jìn)行交替估計(jì)自機(jī)的三維位置(攝像機(jī)位置)和特征點(diǎn)的三維位置(其匯集而構(gòu)成地圖的信息)的處理。

該方法由于交替估計(jì)自機(jī)位置和特征點(diǎn)的三維位置，因此會(huì)發(fā)生累積誤差，有時(shí)在進(jìn)行環(huán)狀的移動(dòng)等而回到初始的位置的情況下也不能認(rèn)識(shí)到已經(jīng)回到了初始的位置的情況。作為修正這樣的累積誤差的方法，有合環(huán)處理(環(huán)解決處理)這樣的技術(shù)，在非專(zhuān)利文獻(xiàn)3公開(kāi)。

在該技術(shù)中，若自機(jī)檢測(cè)到環(huán)(檢測(cè)到拍攝的幀與過(guò)去拍攝幀類(lèi)似的情況)，則將過(guò)去的該位置視作正確的位置，來(lái)修正從那里起到當(dāng)前位置的路徑中的位置，由此進(jìn)行減少誤差的合環(huán)處理。

現(xiàn)有技術(shù)文獻(xiàn)

非專(zhuān)利文獻(xiàn)

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

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

universitypress，march2004，chapter9

非專(zhuān)利文獻(xiàn)3：奈良先端科學(xué)技術(shù)大學(xué)院大學(xué)情報(bào)科學(xué)研究科準(zhǔn)教授佐藤智和、“ssii2015チユ一トリアル特徴點(diǎn)追跡によろ動(dòng)畫(huà)像からの逐次三次元復(fù)元とその応用-座標(biāo)系の基礎(chǔ)から応用事例·最新研究動(dòng)向まで-”、[online]、2015年6月10日、畫(huà)像它ンシングシンポジウ厶チユ一トリアル耩演會(huì)、[2016年2月22日檢索]、因特網(wǎng)＜url：http：//yokoya.naist.jp/paper/datas/1407/ssii％e3％83％81％e3％83％a5％e3％83％bc％e3％83％88％e3％83％aa％e3％82％a2％e3％83％ab％e4％bd％90％e8％97％a4.pdf＞

但在現(xiàn)有技術(shù)中，由于不檢測(cè)環(huán)就不能進(jìn)行合環(huán)處理，因此使誤差減少的機(jī)會(huì)受到限制。

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

本發(fā)明為了解決上述問(wèn)題而提出，目的在于，提供不檢測(cè)環(huán)也能減少誤差的自主移動(dòng)裝置等。

為了達(dá)成上述目的，本發(fā)明的自主移動(dòng)裝置特征在于，具備：基于攝像部所拍攝的多個(gè)圖像的信息來(lái)對(duì)自主的移動(dòng)進(jìn)行控制的移動(dòng)控制部；從所述攝像部所拍攝的圖像取得特征點(diǎn)的特征點(diǎn)取得部；存儲(chǔ)所述特征點(diǎn)取得部所取得的特征點(diǎn)的位置的地圖存儲(chǔ)部；和取得所述特征點(diǎn)取得部所取得的特征點(diǎn)與在所述地圖存儲(chǔ)部存儲(chǔ)有位置的特征點(diǎn)的對(duì)應(yīng)數(shù)即特征點(diǎn)對(duì)應(yīng)數(shù)的特征點(diǎn)對(duì)應(yīng)數(shù)取得部，所述移動(dòng)控制部按照所述特征點(diǎn)對(duì)應(yīng)數(shù)取得部所取得的特征點(diǎn)對(duì)應(yīng)數(shù)來(lái)對(duì)移動(dòng)進(jìn)行控制。

發(fā)明的效果

根據(jù)本發(fā)明，不用檢測(cè)環(huán)也能減少誤差。

附圖說(shuō)明

圖1是表示實(shí)施方式所涉及的自主移動(dòng)裝置的外觀的圖。

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

圖3是表示實(shí)施方式所涉及的自主移動(dòng)控制處理整體的流程圖的圖。

圖4a是表示實(shí)施方式所涉及的自主移動(dòng)控制處理中的自機(jī)位置估計(jì)線程的處理的第1個(gè)流程圖的圖。

圖4b是表示實(shí)施方式所涉及的自主移動(dòng)控制處理中的自機(jī)位置估計(jì)線程的處理的第2個(gè)流程圖的圖。

圖5是表示實(shí)施方式所涉及的自主移動(dòng)控制處理中的地圖作成線程的處理的流程圖的圖。

圖6是表示實(shí)施方式所涉及的自主移動(dòng)控制處理中的合環(huán)線程的處理的流程圖的圖。

圖7是表示實(shí)施方式所涉及的自主移動(dòng)控制處理中的移動(dòng)控制的流程圖的圖。

圖8是表示實(shí)施方式所涉及的自主移動(dòng)裝置的移動(dòng)的具體例的圖。

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

標(biāo)號(hào)的說(shuō)明

100自主移動(dòng)裝置

10、50控制部

11特征點(diǎn)取得部

12地圖作成部

13位置估計(jì)部

14特征點(diǎn)對(duì)應(yīng)數(shù)取得部

15移動(dòng)方向速度變更部

16移動(dòng)方向速度決定部

20、60存儲(chǔ)部

21圖像存儲(chǔ)部

22地圖存儲(chǔ)部

23特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部

24移動(dòng)履歷存儲(chǔ)部

30傳感器部

31障礙物傳感器

41攝像部

42驅(qū)動(dòng)部

43輸入部

44、71通信部

45電源

200服務(wù)器

具體實(shí)施方式

以下參考圖1來(lái)說(shuō)明本發(fā)明的實(shí)施方式所涉及的自主移動(dòng)裝置。

自主移動(dòng)裝置100根據(jù)用途而自主進(jìn)行移動(dòng)。該用途例如是警備監(jiān)視用、室內(nèi)清掃用、寵物用、玩具用等。

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

攝像部41具備單鏡頭的攝像裝置(攝像機(jī))。攝像部41例如以30fps取得圖像(幀)。自主移動(dòng)裝置100基于攝像部41逐次取得的圖像，一邊實(shí)時(shí)認(rèn)識(shí)自機(jī)位置和周?chē)h(huán)境，一邊進(jìn)行自主移動(dòng)。

驅(qū)動(dòng)部42是獨(dú)立2輪驅(qū)動(dòng)型，是具備車(chē)輪和電動(dòng)機(jī)的移動(dòng)單元。自主移動(dòng)裝置100能通過(guò)2個(gè)車(chē)輪的相同方向驅(qū)動(dòng)來(lái)進(jìn)行前后的平行移動(dòng)(并進(jìn)移動(dòng))，能通過(guò)2個(gè)車(chē)輪的相反方向驅(qū)動(dòng)來(lái)進(jìn)行當(dāng)場(chǎng)的旋轉(zhuǎn)(朝向變更)，能通過(guò)改變2個(gè)車(chē)輪各自速度的驅(qū)動(dòng)來(lái)進(jìn)行轉(zhuǎn)彎移動(dòng)(并進(jìn)+旋轉(zhuǎn)(朝向變更)移動(dòng))。另外，在各個(gè)車(chē)輪具備轉(zhuǎn)動(dòng)體編碼器，用轉(zhuǎn)動(dòng)體編碼器測(cè)量車(chē)輪的旋轉(zhuǎn)數(shù)，能利用車(chē)輪的直徑或車(chē)輪間的距離等幾何學(xué)上的關(guān)系來(lái)計(jì)算并進(jìn)移動(dòng)量以及旋轉(zhuǎn)量。例如若將車(chē)輪的直徑設(shè)為d，將旋轉(zhuǎn)數(shù)設(shè)為r(由轉(zhuǎn)動(dòng)體編碼器測(cè)定)，則在該車(chē)輪的接地部分的并進(jìn)移動(dòng)量成為π·d·r。另外，若將車(chē)輪的直徑設(shè)為d，將車(chē)輪間的距離設(shè)為i，將右車(chē)輪的旋轉(zhuǎn)數(shù)設(shè)為rr，將左車(chē)輪的旋轉(zhuǎn)數(shù)設(shè)為rl，則朝向變更的旋轉(zhuǎn)量成為(若將右旋轉(zhuǎn)設(shè)為正)360°×d×(rl-rr)/(2×i)。通過(guò)將該并進(jìn)移動(dòng)量、旋轉(zhuǎn)量逐次加在一起，驅(qū)動(dòng)部42作為所謂的里程計(jì)發(fā)揮功能，能測(cè)量自機(jī)位置(以移動(dòng)開(kāi)始時(shí)的位置以及朝向?yàn)榛鶞?zhǔn)的位置以及朝向)。

另外，也可以取代車(chē)輪而具備履帶，也可以具備多個(gè)(例如2條)腿而用腿行走，由此進(jìn)行移動(dòng)。在這些情況下，也能基于2個(gè)履帶的活動(dòng)、腿的活動(dòng)，與車(chē)輪的情況同樣地測(cè)量自機(jī)的位置、朝向。

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

控制部10由cpu(centralprocessingunit，中央處理器)等構(gòu)成，通過(guò)執(zhí)行存儲(chǔ)于存儲(chǔ)部20的程序來(lái)實(shí)現(xiàn)后述的各部(特征點(diǎn)取得部11、地圖作成部12、位置估計(jì)部13、特征點(diǎn)對(duì)應(yīng)數(shù)取得部14、移動(dòng)方向速度變更部15、移動(dòng)方向速度決定部16)的功能。另外，控制部10具備計(jì)時(shí)器(未圖示)，能對(duì)經(jīng)過(guò)時(shí)間進(jìn)行計(jì)數(shù)。

存儲(chǔ)部20由rom(readonlymemory，只讀存儲(chǔ)器)、ram(randomaccessmemory，隨機(jī)存取存儲(chǔ)器)等構(gòu)成，包含圖像存儲(chǔ)部21、地圖存儲(chǔ)部22、特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23、移動(dòng)履歷存儲(chǔ)部24。在rom中存儲(chǔ)控制部10的cpu所執(zhí)行的程序(例如后述的slam法的運(yùn)算或自主移動(dòng)控制處理所涉及的程序)、執(zhí)行程序上所需的數(shù)據(jù)。在ram中存儲(chǔ)在程序執(zhí)行中作成或變更的數(shù)據(jù)。

在圖像存儲(chǔ)部21中存儲(chǔ)攝像部41所拍攝的圖像。其中為了存儲(chǔ)容量的節(jié)約，可以不存儲(chǔ)拍攝的全部圖像，另外也可以不是存儲(chǔ)圖像自身，而是存儲(chǔ)圖像的特征量。關(guān)于重要的圖像(后述的關(guān)鍵幀)，和圖像的信息一起存儲(chǔ)拍攝該圖像時(shí)的自機(jī)位置(自機(jī)的位置以及朝向)的信息。

在地圖存儲(chǔ)部22中存儲(chǔ)基于后述的slam法或來(lái)自障礙物傳感器31的信息由地圖作成部12作成的地圖(特征點(diǎn)或障礙物的三維位置的信息)。

在特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23中，在后述的自機(jī)位置估計(jì)線程中存儲(chǔ)攝像部41所拍攝的圖像的特征點(diǎn)與存儲(chǔ)于地圖存儲(chǔ)部22的map點(diǎn)的對(duì)應(yīng)數(shù)。

在移動(dòng)履歷存儲(chǔ)部24中存儲(chǔ)移動(dòng)方向以及移動(dòng)距離的履歷。也可以取代存儲(chǔ)移動(dòng)方向以及移動(dòng)距離而存儲(chǔ)自機(jī)的三維位置以及朝向的履歷。移動(dòng)履歷存儲(chǔ)部24成為后進(jìn)先出的堆棧的存儲(chǔ)部。在此，存儲(chǔ)的履歷的數(shù)量相當(dāng)于移動(dòng)履歷存儲(chǔ)部24的堆棧指針的值。

作為傳感器部30而具備障礙物傳感器31。障礙物傳感器31是探測(cè)行駛中的障礙物的傳感器，例如是紅外線傳感器、超聲波傳感器。另外，也可以不搭載獨(dú)立的障礙物傳感器31，而是使用攝像部41探測(cè)障礙物。另外，也可以具備探測(cè)與其他物體碰撞的保險(xiǎn)桿傳感器(未圖示)、檢測(cè)動(dòng)作的角速度傳感器或加速度傳感器(均未圖示)等。

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

通信部44是用于與外部裝置進(jìn)行通信的模塊，在與外部裝置進(jìn)行無(wú)線通信的情況下是包含天線的無(wú)線模塊。例如通信部44是用于進(jìn)行基于bluetooth(注冊(cè)商標(biāo))的近距離無(wú)線通信的無(wú)線模塊。在通信部44中，進(jìn)行自主移動(dòng)裝置100與外部的數(shù)據(jù)的收發(fā)。在對(duì)自主移動(dòng)裝置100指示目的地時(shí)，經(jīng)由該通信部44來(lái)傳遞目的地的信息。

電源45是使自主移動(dòng)裝置100動(dòng)作的電源，一般是內(nèi)置的充電電池，但也可以是太陽(yáng)能電池，也可以是從地面無(wú)線進(jìn)行電力提供的系統(tǒng)。在電源45是充電電池的情況下，通過(guò)自主移動(dòng)裝置100與充電站(基地)對(duì)接來(lái)進(jìn)行充電。

接下來(lái)說(shuō)明控制部10的功能?？刂撇?0包含特征點(diǎn)取得部11、地圖作成部12、位置估計(jì)部13、特征點(diǎn)對(duì)應(yīng)數(shù)取得部14、移動(dòng)方向速度變更部15、移動(dòng)方向速度決定部16，進(jìn)行后述的slam法的運(yùn)算、自主移動(dòng)裝置100的移動(dòng)控制等。另外，控制部10與多線程功能對(duì)應(yīng)，能并行推進(jìn)多個(gè)線程(不同的處理的流程)。

特征點(diǎn)取得部11取得攝像部41所拍攝的圖像中的2d特征點(diǎn)。所謂2d特征點(diǎn)，是圖像中的邊緣部分等圖像內(nèi)的特征性的部分，能使用sift(scale-invariantfuturetransform，尺度不變特征轉(zhuǎn)換)或surf(speed-uprobustfeatures，加速魯棒特征)等算法取得。

地圖作成部12將基于存儲(chǔ)于圖像存儲(chǔ)部21的圖像的信息和該圖像拍攝時(shí)的自機(jī)的位置以及朝向的信息使用slam法估計(jì)出的特征點(diǎn)(map點(diǎn))的三維位置、基于由障礙物傳感器31探測(cè)到障礙物時(shí)自機(jī)的位置以及朝向的信息而得到的該障礙物的三維位置等作為地圖的信息存儲(chǔ)在地圖存儲(chǔ)部22中。

位置估計(jì)部13基于后述的slam法，作為視覺(jué)里程計(jì)來(lái)估計(jì)自機(jī)的位置以及朝向。

特征點(diǎn)對(duì)應(yīng)數(shù)取得部14，取得特征點(diǎn)取得部11所取得的特征點(diǎn)與存儲(chǔ)于地圖存儲(chǔ)部22的map點(diǎn)的對(duì)應(yīng)數(shù)。

移動(dòng)方向速度變更部15基于由特征點(diǎn)對(duì)應(yīng)數(shù)取得部14取得的特征點(diǎn)對(duì)應(yīng)數(shù)來(lái)降低自機(jī)的移動(dòng)速度或停止，或者變更移動(dòng)方向。由此增加了特征點(diǎn)對(duì)應(yīng)數(shù)。

移動(dòng)方向速度決定部16決定自機(jī)的移動(dòng)速度、移動(dòng)方向。為了基于在此決定的移動(dòng)速度、移動(dòng)方向進(jìn)行移動(dòng)，控制部10對(duì)驅(qū)動(dòng)部42進(jìn)行控制。另外，移動(dòng)方向速度變更部15以及移動(dòng)方向速度決定部16相當(dāng)于移動(dòng)控制部。

參考圖3來(lái)說(shuō)明主移動(dòng)裝置100的自主移動(dòng)控制處理的主流程?？刂撇?0首先對(duì)移動(dòng)狀態(tài)變量設(shè)置初始值即「通?！?，對(duì)前次履歷數(shù)設(shè)置初始值即0(步驟s101)。移動(dòng)狀態(tài)變量和前次履歷數(shù)都是在步驟s107的移動(dòng)控制中使用的變量，詳細(xì)在移動(dòng)控制的說(shuō)明中敘述。接下來(lái)，控制部10啟動(dòng)自機(jī)位置估計(jì)線程(步驟s102)。然后啟動(dòng)地圖作成線程(步驟s103)。接下來(lái)啟動(dòng)合環(huán)線程(步驟s104)。通過(guò)自機(jī)位置估計(jì)線程、地圖作成線程動(dòng)作，基于slam法開(kāi)始地圖的信息以及視覺(jué)里程計(jì)(使用地圖和圖像而估計(jì)的自機(jī)位置的信息)的生成。之后，控制部10判定是否動(dòng)作結(jié)束(步驟s105)，若是動(dòng)作結(jié)束(步驟s105；“是”)，則結(jié)束動(dòng)作，若不是動(dòng)作結(jié)束(步驟s105；“否”)，則利用障礙物傳感器31和視覺(jué)里程計(jì)進(jìn)行地圖的更新(步驟s106)。

接下來(lái)，控制部10為了進(jìn)行自主移動(dòng)而進(jìn)行對(duì)驅(qū)動(dòng)部42發(fā)出動(dòng)作指示的移動(dòng)控制(步驟s107)，回到步驟s105。在此的自主移動(dòng)還有為了作成地圖而在房間中到處活動(dòng)這樣的移動(dòng)的情況，還有接受到向目的地的移動(dòng)指示的情況等向指示的目的地的沿著最短路徑的移動(dòng)的情況。另外，還有通過(guò)不指示明確的目的地而指示任務(wù)(地圖的再作成、清掃、寵物等)而時(shí)刻基于周?chē)臓顩r和所指示的任務(wù)來(lái)設(shè)定目的地的情況。

通過(guò)該主流程的處理，能基于地圖的信息自主進(jìn)行移動(dòng)并適宜更新地圖的信息。作為典型的示例，自主移動(dòng)裝置100若在最初置于充電站的狀態(tài)下接通電源，則憑借障礙物傳感器31在家的各房間到處移動(dòng)，通過(guò)障礙物傳感器31來(lái)確定墻壁等的障礙物位置，能作成包含障礙物位置的地圖的信息。若某種程度作成地圖，則能獲知還沒(méi)有地圖的信息但認(rèn)為能移動(dòng)的區(qū)域，還能自主移動(dòng)到該區(qū)域等來(lái)促使更大范圍的地圖的作成。然后，若作成能移動(dòng)的幾乎全域的地圖的信息，就能進(jìn)行利用地圖的信息的有效率的移動(dòng)動(dòng)作。例如不管從房間的哪個(gè)位置都能以最短路徑返回充電站，或者能有效率地進(jìn)行房間的清掃。

參考圖4a以及圖4b來(lái)說(shuō)明在自主移動(dòng)裝置100的主流程(圖3)的步驟s102啟動(dòng)的自機(jī)位置估計(jì)線程。該線程是位置估計(jì)部13最初進(jìn)行初始化處理、之后持續(xù)自機(jī)位置估計(jì)(使用由攝像部41取得的圖像并用視覺(jué)里程計(jì)估計(jì)自機(jī)位置)的處理。

位置估計(jì)部13首先對(duì)估計(jì)狀態(tài)變量設(shè)置「初始化中」(步驟s201)。估計(jì)狀態(tài)變量表示該時(shí)間點(diǎn)的自機(jī)位置估計(jì)處理的狀態(tài)，取「初始化中」、「追蹤成功」、「追蹤失敗」這3個(gè)值。接下來(lái)判定是否是動(dòng)作結(jié)束(步驟s202)。若是動(dòng)作結(jié)束(步驟s202；“是”)，則結(jié)束，若不是動(dòng)作結(jié)束(步驟s202；“否”)，則判定估計(jì)狀態(tài)變量是否是「初始化中」(步驟s203)。若不是初始化中(步驟s203；“否”)，則進(jìn)行步驟s221以后的自機(jī)位置估計(jì)處理，若是初始化中(步驟s203；“是”)，則前進(jìn)到步驟s204來(lái)進(jìn)行初始化處理。首先說(shuō)明初始化處理。

在初始化處理中，位置估計(jì)部13首先對(duì)幀計(jì)數(shù)器n設(shè)置-1(步驟s204)，在攝像部41取得圖像(步驟s205)。圖像例如能以30fps取得(取得的圖像也被稱(chēng)作幀)。接下來(lái)，特征點(diǎn)取得部11取得在攝像部41取得的圖像中所含的2d特征點(diǎn)(步驟s206)。

由于若取得的2d特征點(diǎn)的個(gè)數(shù)少，就不能用后述的two-viewstructurefrommotion(雙視圖運(yùn)動(dòng)恢復(fù)結(jié)構(gòu))法進(jìn)行計(jì)算，因此位置估計(jì)部13將2d特征點(diǎn)的取得數(shù)與基準(zhǔn)值(例如10個(gè))進(jìn)行比較(步驟s207)，在不足基準(zhǔn)值的情況下(步驟s207；“否”)返回步驟s205，直到取得基準(zhǔn)值以上的2d特征點(diǎn)數(shù)為止，重復(fù)圖像的取得和2d特征點(diǎn)的取得。另外，雖然在該時(shí)間點(diǎn)尚未作成地圖的信息，但例如在上述的典型的示例中，由于憑借障礙物傳感器31開(kāi)始在家的各房間到處移動(dòng)，因此只要在該初始化處理中重復(fù)圖像取得和2d特征點(diǎn)取得，就會(huì)一邊移動(dòng)一邊重復(fù)圖像取得。因此能取得各種圖像，總之能期待能取得2d特征點(diǎn)數(shù)多的圖像。

在2d特征點(diǎn)的取得數(shù)為基準(zhǔn)值以上的情況下(步驟s207；“是”)，位置估計(jì)部13使幀計(jì)數(shù)器n遞增(步驟s208)。然后判定幀計(jì)數(shù)器n是否是0(步驟s209)。若幀計(jì)數(shù)器n是0(步驟s209；“是”)，則意味著剛剛僅取得1副圖像，因此為了取得第2副圖像而返回步驟s205。另外雖然未在圖4a的流程圖中記載，但取得第1副圖像時(shí)的自機(jī)的位置與取得第2副圖像時(shí)的自機(jī)的位置有某種程度的分離有助于在這以后的處理提升估計(jì)的姿態(tài)的精度。為此也可以在從步驟s209返回步驟s205時(shí)，在等待通過(guò)主流程(圖3)的步驟s107中的移動(dòng)控制而以里程計(jì)得到的并進(jìn)距離成為給定的距離(例如1m)以上之后再返回步驟s205。

若幀計(jì)數(shù)器n不是0(步驟s209；“否”)，則意味著取得2個(gè)圖像，因此位置估計(jì)部13在這2個(gè)圖像間取得2d特征點(diǎn)的對(duì)應(yīng)(實(shí)際環(huán)境上的同一點(diǎn)存在于各個(gè)圖像中，取該對(duì)應(yīng))(步驟s210)。在此，由于若特征點(diǎn)的對(duì)應(yīng)數(shù)不足5就不能進(jìn)行后述的2個(gè)圖像間的姿態(tài)的估計(jì)，因此位置估計(jì)部13判定特征點(diǎn)的對(duì)應(yīng)數(shù)是否不足5(步驟s211)。如不足5(步驟s211；“是”)，則為了重新取得初始圖像而返回步驟s204。若特征點(diǎn)的對(duì)應(yīng)數(shù)為5點(diǎn)以上(步驟s211；“否”)，則能使用后述的two-viewstructurefrommotion(雙視圖運(yùn)動(dòng)恢復(fù)結(jié)構(gòu))法來(lái)估計(jì)2個(gè)圖像間的姿態(tài)(取得各個(gè)圖像的位置的差分(并進(jìn)矢量t)以及朝向的差分(旋轉(zhuǎn)矩陣r))(步驟s212)。

該two-viewstructurefrommotion(雙視圖運(yùn)動(dòng)恢復(fù)結(jié)構(gòu))法的姿態(tài)的估計(jì)，具體從對(duì)應(yīng)的特征點(diǎn)求取基本矩陣e，將基本矩陣e分解為并進(jìn)矢量t和旋轉(zhuǎn)矩陣r，由此進(jìn)行(詳細(xì)參考非專(zhuān)利文獻(xiàn)2)。另外，由于在此得到的并進(jìn)矢量t(若設(shè)想在三維空間內(nèi)移動(dòng)，則以取得最初的圖像的位置為原點(diǎn)具有x、y、z這3個(gè)要素)的各要素的值與實(shí)際環(huán)境上的值不同(在two-viewstructurefrommotion(雙視圖運(yùn)動(dòng)恢復(fù)結(jié)構(gòu))法中不能得到實(shí)際環(huán)境上的值自身，得到與實(shí)際環(huán)境相似的空間上的值)，因此將它們視作slam空間上的值，以下使用slam空間上的坐標(biāo)(slam坐標(biāo))來(lái)進(jìn)行說(shuō)明。

若求得2個(gè)圖像間的姿態(tài)(并進(jìn)矢量t以及旋轉(zhuǎn)矩陣r)，則該值成為以最初的圖像為基準(zhǔn)(以取得最初的圖像的位置為slam坐標(biāo)的原點(diǎn)、并進(jìn)矢量是0矢量、旋轉(zhuǎn)矩陣為單位矩陣i)的情況下的第2副圖像的姿態(tài)(取得第2個(gè)圖像時(shí)的自機(jī)的位置(并進(jìn)矢量t)以及朝向(旋轉(zhuǎn)矩陣r))。在此，在求取2個(gè)圖像各自的姿態(tài)(該圖像(幀)拍攝時(shí)的自機(jī)的位置(并進(jìn)矢量t)以及朝向(旋轉(zhuǎn)矩陣r)，也稱(chēng)作幀姿態(tài))的情況下，基于以下的思路由地圖作成部12求取在這2個(gè)圖像間取對(duì)應(yīng)的2d特征點(diǎn)(對(duì)應(yīng)特征點(diǎn))的slam坐標(biāo)上的3d位置(步驟s213)。

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

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

在上述的式(1)中，p是3×4的矩陣，根據(jù)表示攝像機(jī)的內(nèi)部參數(shù)的3×3的矩陣a、和表示該圖像的姿態(tài)(幀姿態(tài))的外部參數(shù)r以及t用以下的式(2)表征。在此，(r|t)表征在旋轉(zhuǎn)矩陣r的右邊排列并進(jìn)列矢量t的矩陣。

p＝a(r|t)...(2)

在上述的式(2)中，如上述那樣求取r以及t，作為該幀姿態(tài)。另外，對(duì)于攝像機(jī)的內(nèi)部參數(shù)a，由于通過(guò)焦點(diǎn)距離和攝像元件尺寸決定，因此只要決定好攝像部41其就成為常數(shù)。

若在2個(gè)圖像間取對(duì)應(yīng)的2d特征點(diǎn)當(dāng)中的一者映現(xiàn)在第1個(gè)圖像的幀坐標(biāo)(u1，v1)和第2個(gè)圖像的幀坐標(biāo)(u2，v2)，則能做出以下的式(3)以及式(4)。在此，i表征單位矩陣，0表征零矢量，(l|r)表征在矩陣l的右邊排列了列矢量r的矩陣。

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

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

在上述的式(3)以及式(4)中，由于能做出分別針對(duì)u1、v1、u2、v2的算式，能做出4個(gè)算式，而未知數(shù)是x、y、z這3個(gè)，因此能求取x、y、z，其成為該2d特征點(diǎn)的slam坐標(biāo)中的3d位置。另外，由于算式的個(gè)數(shù)多于未知數(shù)的個(gè)數(shù)，因此會(huì)有以u(píng)1、v1、u2求得的x、y、z與以u(píng)1、v1、v2求得的x、y、z不同的情況。在這樣的情況下，成為過(guò)剩條件的聯(lián)立一次方程式，一般不存在解，但地圖作成部12使用最小二乘法來(lái)求取最可信的x、y、z。

若求得2d特征點(diǎn)的slam坐標(biāo)中的3d位置(x，y，z)，則將其作為map點(diǎn)，由地圖作成部12登錄到map點(diǎn)數(shù)據(jù)庫(kù)(也稱(chēng)作map點(diǎn)db(database，數(shù)據(jù)庫(kù))，容納在地圖存儲(chǔ)部22中)(步驟s214)。作為登錄在map點(diǎn)數(shù)據(jù)庫(kù)的要素，至少需要「2d特征點(diǎn)的slam坐標(biāo)中的3d位置即x、y、z」和「該2d特征點(diǎn)的特征量」(例如以sift等得到的特征量)。

然后，地圖作成部12判定是否將在2個(gè)圖像間取對(duì)應(yīng)的2d特征點(diǎn)(對(duì)應(yīng)特征點(diǎn))的全部登錄在map點(diǎn)數(shù)據(jù)庫(kù)(步驟s215)，若尚未全部登錄(步驟s215；“否”)，則返回步驟s213，若全部登錄了(步驟s215；“是”)，則前進(jìn)到步驟s216。

然后，位置估計(jì)部13將nkf(表征關(guān)鍵幀(是指成為之后緊接的線程中的處理對(duì)象的圖像)的計(jì)數(shù)器的變量)初始化為0(步驟s216)，將第2個(gè)圖像作為關(guān)鍵幀登錄到幀數(shù)據(jù)庫(kù)(也稱(chēng)作幀db，容納在圖像存儲(chǔ)部21)(步驟s217)。

登錄在幀數(shù)據(jù)庫(kù)的要素是「關(guān)鍵幀編號(hào)」(在登錄時(shí)間點(diǎn)的關(guān)鍵幀計(jì)數(shù)器nkf的值)、「姿態(tài)」(該圖像拍攝時(shí)的自機(jī)在slam坐標(biāo)內(nèi)的位置(并進(jìn)矢量t)以及朝向(旋轉(zhuǎn)矩陣r))、「提取的全部2d特征點(diǎn)」、「在全部2d特征點(diǎn)中作為map點(diǎn)而3d位置已知的點(diǎn)」、「關(guān)鍵幀自身的特征」，但除此以外也可以還登錄「以里程計(jì)測(cè)量的實(shí)際環(huán)境上的姿態(tài)」(基于實(shí)際環(huán)境中的驅(qū)動(dòng)部42所實(shí)現(xiàn)的移動(dòng)距離求得的自機(jī)的位置以及朝向)。

上述中，所謂「關(guān)鍵幀自身的特征」，是用于使求取關(guān)鍵幀間的圖像類(lèi)似度的處理效率化的數(shù)據(jù)，通?？梢允褂脠D像中的2d特征點(diǎn)的直方圖等，但也可以將圖像自身作為「關(guān)鍵幀自身的特征」。另外，「以里程計(jì)測(cè)量的實(shí)際環(huán)境上的姿態(tài)」還能以并進(jìn)矢量t和旋轉(zhuǎn)矩陣r表征，然而通常來(lái)說(shuō)，由于本自主移動(dòng)裝置100在二維平面上上活動(dòng)，因此可以簡(jiǎn)化為二維數(shù)據(jù)，表征為以移動(dòng)開(kāi)始時(shí)的位置(原點(diǎn))以及朝向?yàn)榛鶞?zhǔn)的二維坐標(biāo)(x，y)以及朝向

接下來(lái)，位置估計(jì)部13為了使地圖作成線程獲知生成了關(guān)鍵幀這一情況，對(duì)地圖作成線程的關(guān)鍵幀隊(duì)列(隊(duì)列(queue)成為先進(jìn)先出的數(shù)據(jù)結(jié)構(gòu))設(shè)置關(guān)鍵幀計(jì)數(shù)器nkf(步驟s218)。

以上完成自機(jī)位置估計(jì)線程的初始化處理，位置估計(jì)部13對(duì)估計(jì)狀態(tài)變量設(shè)置「追蹤成功」(步驟s219)，為了獲得slam坐標(biāo)與實(shí)際環(huán)境坐標(biāo)的標(biāo)量對(duì)應(yīng)，將里程計(jì)測(cè)量的并進(jìn)距離(在實(shí)際環(huán)境中的坐標(biāo)求取)除以上述的處理中估計(jì)出的在slam坐標(biāo)的并進(jìn)距離d，由此求取標(biāo)量s(步驟s220)。

然后經(jīng)由步驟s202、步驟s203，前進(jìn)到初始化完畢的情況下的處理的步驟s221。

對(duì)初始化完畢的情況下的處理進(jìn)行說(shuō)明。該處理是自機(jī)位置估計(jì)線程的通常時(shí)的處理，是位置估計(jì)部13逐次估計(jì)當(dāng)前的自機(jī)的位置以及朝向(slam坐標(biāo)內(nèi)的并進(jìn)矢量t和旋轉(zhuǎn)矩陣r)的處理。

首先，位置估計(jì)部13用攝像部41拍攝圖像(步驟s221)，使幀計(jì)數(shù)器n遞增(步驟s222)。然后，特征點(diǎn)取得部11取得用攝像部41拍攝的圖像中所含的2d特征點(diǎn)(步驟s223)。

接下來(lái)，位置估計(jì)部13判定估計(jì)狀態(tài)變量是否是「追蹤成功」(步驟s224)。若是「追蹤成功」(步驟s224；“是”)，則特征點(diǎn)對(duì)應(yīng)數(shù)取得部14從登錄在幀數(shù)據(jù)庫(kù)的1個(gè)之前的關(guān)鍵幀(關(guān)鍵幀編號(hào)為nkf的圖像)的信息取得該圖像的信息中所含的2d特征點(diǎn)當(dāng)中3d位置為已知的(成為登錄在map點(diǎn)數(shù)據(jù)庫(kù)的map點(diǎn))2d特征點(diǎn)，取得與這當(dāng)中在步驟s223取得的特征點(diǎn)取對(duì)應(yīng)的2d特征點(diǎn)(對(duì)應(yīng)特征點(diǎn))的個(gè)數(shù)(特征點(diǎn)對(duì)應(yīng)數(shù))，存儲(chǔ)在特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23(步驟s225)。

若估計(jì)狀態(tài)變量的值不是「追蹤成功」(步驟s224；“否”)，則特征點(diǎn)對(duì)應(yīng)數(shù)取得部14從登錄在幀數(shù)據(jù)庫(kù)的1個(gè)以上之前的關(guān)鍵幀(關(guān)鍵幀編號(hào)不足nkf的圖像)中取得map點(diǎn)與在步驟s223取得的特征點(diǎn)的對(duì)應(yīng)數(shù)成為最大的關(guān)鍵幀、和其特征點(diǎn)對(duì)應(yīng)數(shù)，存儲(chǔ)在特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23(步驟s226)。

然后，位置估計(jì)部13判定特征點(diǎn)對(duì)應(yīng)數(shù)是否大于閾值(例如30，以下稱(chēng)作「基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)」)(步驟s227)，在為基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)以下的情況下(步驟s227；“否”)，由于用slam法估計(jì)的姿態(tài)的精度變差，因此對(duì)估計(jì)狀態(tài)變量設(shè)置「追蹤失敗」(步驟s230)，不進(jìn)行位置的估計(jì)而返回步驟s202。

在特征點(diǎn)對(duì)應(yīng)數(shù)大于基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)的情況下(步驟s227；“是”)，則由特征點(diǎn)對(duì)應(yīng)數(shù)取得部14取得與map點(diǎn)取對(duì)應(yīng)的在步驟s223取得的特征點(diǎn)(對(duì)應(yīng)特征點(diǎn))的個(gè)數(shù)(特征點(diǎn)對(duì)應(yīng)數(shù))，將其存儲(chǔ)在特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23(步驟s228)，其中該map點(diǎn)包含于在步驟s225或步驟s226取得特征點(diǎn)對(duì)應(yīng)數(shù)的關(guān)鍵幀的近旁的關(guān)鍵幀(與取得特征點(diǎn)對(duì)應(yīng)數(shù)的關(guān)鍵幀存在給定的比例以上(例如30％以上)特征點(diǎn)的重疊的關(guān)鍵幀)中。

然后，位置估計(jì)部13判定特征點(diǎn)對(duì)應(yīng)數(shù)是否大于閾值(例如50，以下稱(chēng)作「第2基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)」)(步驟s229)，若為第2基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)以下(步驟s229；“否”)，則對(duì)估計(jì)狀態(tài)變量設(shè)置「追蹤失敗」(步驟s230)，不進(jìn)行位置的估計(jì)而返回步驟s202。若特征點(diǎn)對(duì)應(yīng)數(shù)大于第2基準(zhǔn)對(duì)應(yīng)特征點(diǎn)數(shù)(步驟s229；“是”)，則前進(jìn)到步驟s231。

在步驟s231，位置估計(jì)部13對(duì)估計(jì)狀態(tài)變量設(shè)置追蹤成功(步驟s231)。位置估計(jì)部13從map點(diǎn)數(shù)據(jù)庫(kù)取得在步驟s228取得的對(duì)應(yīng)特征點(diǎn)各自的3d位置(xi，yi，zi)，使用其與在步驟s221取得的圖像所含的對(duì)應(yīng)特征點(diǎn)的對(duì)應(yīng)關(guān)系，來(lái)估計(jì)當(dāng)前的自機(jī)的姿態(tài)(以并進(jìn)矢量t以及旋轉(zhuǎn)矩陣r表征的自機(jī)的位置以及朝向)(步驟s232)。

對(duì)該自機(jī)的姿態(tài)的估計(jì)方法進(jìn)行補(bǔ)足說(shuō)明。若將在步驟s221取得的圖像中所含的對(duì)應(yīng)特征點(diǎn)的幀坐標(biāo)設(shè)為(ui，vi)，將其對(duì)應(yīng)特征點(diǎn)的3d位置設(shè)為(xi，yi，zi)(i取1到特征點(diǎn)對(duì)應(yīng)數(shù)的值)，在將各對(duì)應(yīng)特征點(diǎn)的3d位置(xi，yi，zi)通過(guò)以下的式(5)投影到幀坐標(biāo)系得到的值(uxi，vxi)和幀坐標(biāo)(ui，vi)在理想上應(yīng)當(dāng)一致。

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

但由于實(shí)際上不管在(xi，yi，zi)還是在(ui，vi)都包含誤差，因此(uxi，vxi)和(ui，vi)難得一致。并且未知數(shù)僅是r和t(三維空間中分別為三維，3+3＝6是未知數(shù)的個(gè)數(shù))，數(shù)式卻存在對(duì)應(yīng)特征點(diǎn)的個(gè)數(shù)的2倍(相對(duì)于對(duì)應(yīng)特征點(diǎn)的1個(gè)而存在幀坐標(biāo)的u、v各自對(duì)應(yīng)的算式)，因此成為過(guò)剩條件的聯(lián)立一次方程式，如上述那樣用最小二乘法來(lái)求取。具體地，位置估計(jì)部13求取使以下的式(6)的成本函數(shù)e1最小化的姿態(tài)(并進(jìn)矢量t以及旋轉(zhuǎn)矩陣r)。其成為用slam法求得的slam坐標(biāo)中的自機(jī)的姿態(tài)(以并進(jìn)矢量t以及旋轉(zhuǎn)矩陣r表征的自機(jī)的位置以及朝向)。如此，位置估計(jì)部13補(bǔ)給自機(jī)的姿態(tài)(步驟s232)。

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

由于求得slam坐標(biāo)中的前的自機(jī)的姿態(tài)(并進(jìn)矢量t以及旋轉(zhuǎn)矩陣r)，因此位置估計(jì)部13通過(guò)將其與標(biāo)量s相乘來(lái)求取vo(視覺(jué)里程計(jì))(步驟s233)。vo能作為實(shí)際環(huán)境中的自機(jī)的位置以及朝向而利用。

接下來(lái)，位置估計(jì)部13根據(jù)拍攝即將登錄在幀db之前的關(guān)鍵幀(關(guān)鍵幀編號(hào)為nkf的圖像)時(shí)的自機(jī)的位置來(lái)判定是否移動(dòng)了給定的距離(例如1m，以下稱(chēng)作「基準(zhǔn)并進(jìn)距離」)以上(步驟s234)，在若移動(dòng)了給定的距離以上(步驟s234；“是”)，則使關(guān)鍵幀計(jì)數(shù)器nkf遞增(步驟s235)，然后將當(dāng)前幀作為關(guān)鍵幀登錄在幀db(步驟s236)。若僅移動(dòng)了不足基準(zhǔn)并進(jìn)距離(步驟s234；“否”)，則返回步驟s202。

在此，與基準(zhǔn)并進(jìn)距離進(jìn)行比較的自機(jī)的移動(dòng)距離，可以從里程計(jì)取得從之前緊挨的關(guān)鍵幀到當(dāng)前幀的并進(jìn)距離(兩幀的并進(jìn)矢量的差的矢量的絕對(duì)值(要素的平方和的平方根))，也可以根據(jù)上述的vo求取。登錄在幀db的內(nèi)容如上述那樣是「關(guān)鍵幀編號(hào)」、「姿態(tài)」、「提取的全部2d特征點(diǎn)」、「在全部2d特征點(diǎn)中作為map點(diǎn)而3d位置已知的點(diǎn)」、「關(guān)鍵幀自身的特征」。

然后，位置估計(jì)部13為了使地圖作成線程獲知產(chǎn)生了新的關(guān)鍵幀，對(duì)地圖作成線程的關(guān)鍵幀隊(duì)列設(shè)置關(guān)鍵幀計(jì)數(shù)器nkf(步驟s237)。然后返回步驟s202。另外，關(guān)鍵幀計(jì)數(shù)器nkf、標(biāo)量s、map點(diǎn)db、幀db、特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)在存儲(chǔ)部20，以能跨線程對(duì)值進(jìn)行參考。

接下來(lái)，參考圖5來(lái)說(shuō)明在自主移動(dòng)裝置100的主流程(圖3)的步驟s103啟動(dòng)的地圖作成線程。在該線程中，地圖作成部12計(jì)算關(guān)鍵幀中的對(duì)應(yīng)特征點(diǎn)的3d位置，來(lái)作成地圖的信息(map點(diǎn)db)。

首先地圖作成部12判定是否是動(dòng)作結(jié)束(步驟s301)。若是動(dòng)作結(jié)束(步驟s301；“是”)，則結(jié)束，若不是動(dòng)作結(jié)束(步驟s301；“否”)，則判定關(guān)鍵幀隊(duì)列是否是空(步驟s302)。若關(guān)鍵幀隊(duì)列是空(步驟s302；“是”)，則返回步驟s301，若不是空(步驟s302；“否”)，則從關(guān)鍵幀隊(duì)列取出數(shù)據(jù)來(lái)對(duì)mkf(表征在地圖作成線程中進(jìn)行處理的關(guān)鍵幀的關(guān)鍵幀編號(hào)的變量)設(shè)置(步驟s303)。地圖作成部12判定mkf是否大于0(步驟s304)，在mkf為0的情況下(步驟s304；“否”)返回步驟s301，并等待數(shù)據(jù)進(jìn)入到關(guān)鍵幀隊(duì)列。在mkf為1以上的情況下(步驟s304；“是”)前進(jìn)到以下的處理。

地圖作成部12參考幀db，提取能在前關(guān)鍵幀(關(guān)鍵幀編號(hào)為mkf-1的關(guān)鍵幀)的2d特征點(diǎn)和當(dāng)前關(guān)鍵幀(關(guān)鍵幀編號(hào)為mkf的關(guān)鍵幀)的2d特征點(diǎn)取對(duì)應(yīng)的2d特征點(diǎn)(對(duì)應(yīng)特征點(diǎn))(步驟s305)。由于在幀db中還登錄各個(gè)關(guān)鍵幀的姿態(tài)(并進(jìn)矢量t和旋轉(zhuǎn)矩陣r)，因此能用與自機(jī)位置估計(jì)線程的初始化時(shí)的處理時(shí)同樣的方法計(jì)算對(duì)應(yīng)特征點(diǎn)的3d位置。地圖作成部12將能計(jì)算3d位置的對(duì)應(yīng)特征點(diǎn)作為map點(diǎn)登錄在map點(diǎn)db(步驟s306)。地圖作成部12對(duì)于幀db中的其他關(guān)鍵幀也對(duì)能計(jì)算本次3d位置的2d特征點(diǎn)登錄3d位置(步驟s307)。

另外，在地圖作成部12所提取的對(duì)應(yīng)特征點(diǎn)在map點(diǎn)db登錄完畢的情況下，也可以跳過(guò)3d位置計(jì)算而前進(jìn)到對(duì)下一對(duì)應(yīng)特征點(diǎn)(在map點(diǎn)db未登錄的對(duì)應(yīng)特征點(diǎn))的處理，重新進(jìn)行3d位置計(jì)算，來(lái)更新在map點(diǎn)db登錄完畢的3d位置、和針對(duì)幀db中的對(duì)應(yīng)特征點(diǎn)的3d位置。

接下來(lái)，地圖作成部12判定關(guān)鍵幀隊(duì)列是否是空(步驟s308)。若是空(步驟s308；“是”)，則對(duì)全關(guān)鍵幀的姿態(tài)和全map點(diǎn)的3d位置進(jìn)行光束平差處理(bundleadjustment)，來(lái)謀求精度提升(步驟s309)。然后，若進(jìn)行光束平差處理的結(jié)果是找到誤差大的map點(diǎn)，則將其從map點(diǎn)db刪除(步驟s310)，前進(jìn)到步驟s311。若關(guān)鍵幀隊(duì)列不是空(步驟s308；“否”)，則不進(jìn)行任何處理地前進(jìn)到步驟s311。

在步驟s311，地圖作成部12對(duì)合環(huán)線程的關(guān)鍵幀隊(duì)列設(shè)置mkf(步驟s311)，返回步驟s301。

另外，所謂光束平差處理，是同時(shí)估計(jì)攝像機(jī)姿態(tài)(關(guān)鍵幀姿態(tài))和map點(diǎn)的3d位置的非線性最優(yōu)化法，是使在將map點(diǎn)投影在關(guān)鍵幀上時(shí)產(chǎn)生的誤差成為最小的進(jìn)行最優(yōu)化的處理。

通過(guò)進(jìn)行該光束平差的處理，能謀求關(guān)鍵幀姿態(tài)和map點(diǎn)的3d位置的精度提升。但不進(jìn)行該處理也只是不能實(shí)現(xiàn)精度提升，并不會(huì)產(chǎn)生特別問(wèn)題。因此在沒(méi)有其他處理的狀態(tài)時(shí)(例如關(guān)鍵幀隊(duì)列是空的狀態(tài))也不需要每次都進(jìn)行該處理。

另外，若進(jìn)行光束平差的處理，則有時(shí)會(huì)找到投影在關(guān)鍵幀上時(shí)的誤差大于給定的值的map點(diǎn)。對(duì)于這樣誤差大的map點(diǎn)，由于會(huì)給slam估計(jì)帶來(lái)不良影響，因此在步驟s310從map點(diǎn)db中刪除。另外，也可以不是從map點(diǎn)db中刪除，而是在map點(diǎn)db中設(shè)立用于識(shí)別是需要注意誤差大的map點(diǎn)的標(biāo)記。

接下來(lái)，參考圖6來(lái)說(shuō)明在主移動(dòng)裝置100的主流程(圖3)的步驟s104啟動(dòng)的合環(huán)線程。在該線程中，控制部10持續(xù)檢查能否進(jìn)行合環(huán)處理，在能進(jìn)行合環(huán)處理的情況下進(jìn)行合環(huán)處理。另外，所謂合環(huán)處理，是在認(rèn)識(shí)到回到以前來(lái)過(guò)的相同場(chǎng)所的情況下，利用以前在該相同場(chǎng)所時(shí)的姿態(tài)的值與當(dāng)前的姿態(tài)的值的偏離來(lái)修正從以前來(lái)過(guò)時(shí)到現(xiàn)在為止的軌跡中的關(guān)鍵幀、關(guān)聯(lián)的map點(diǎn)的3d位置。

首先，控制部10判定是否是動(dòng)作結(jié)束(步驟s401)。若是動(dòng)作結(jié)束(步驟s401；“是”)，則結(jié)束。若不是動(dòng)作結(jié)束(步驟s401；“否”)，則判定關(guān)鍵幀隊(duì)列是否是空(步驟s402)。若關(guān)鍵幀隊(duì)列是空(步驟s402；“是”)，則返回步驟s401，若關(guān)鍵幀隊(duì)列不是空(步驟s402；“否”)，則從關(guān)鍵幀隊(duì)列取出數(shù)據(jù)來(lái)對(duì)lkf(表征合環(huán)線程進(jìn)行處理的關(guān)鍵幀的關(guān)鍵幀編號(hào)的變量)設(shè)置(步驟s403)。接下來(lái)，控制部10判定lkf是否大于1(步驟s404)。在lkf為0或1的情況下(步驟s404；“否”)，返回步驟s401，等待數(shù)據(jù)進(jìn)入到關(guān)鍵幀隊(duì)列。然后，在lkf為2以上的情況下(步驟s404；“是”)，進(jìn)行以下的處理。

控制部10參考幀db，從幀db檢索當(dāng)前關(guān)鍵幀(關(guān)鍵幀編號(hào)為lkf的關(guān)鍵幀)與「關(guān)鍵幀自身的特征」的類(lèi)似度成為給定的類(lèi)似度(例如0.9，以下稱(chēng)作「基準(zhǔn)圖像類(lèi)似度」)以上的關(guān)鍵幀(步驟s405)。在此，關(guān)于該類(lèi)似度，在以特征矢量表征圖像(關(guān)鍵幀)的特征的情況下，能將使2個(gè)圖像的特征矢量的絕對(duì)值(要素的平方和的平方根)歸一化為1的參數(shù)彼此的內(nèi)積設(shè)為該2個(gè)圖像的類(lèi)似度。另外可以將2個(gè)圖像的特征矢量(使絕對(duì)值歸一化為1的參數(shù))的距離(各要素的差的平方和的平方根)的倒數(shù)設(shè)為類(lèi)似度。

控制部10判定是否發(fā)現(xiàn)「關(guān)鍵幀自身的特征」的類(lèi)似度成為基準(zhǔn)圖像類(lèi)似度以上的關(guān)鍵幀(步驟s406)，若未發(fā)現(xiàn)(步驟s406；“否”)，則返回步驟s401，若發(fā)現(xiàn)了(步驟s406；“是”)，則修正從發(fā)現(xiàn)的關(guān)鍵幀到當(dāng)前關(guān)鍵幀的軌跡中的關(guān)鍵幀的姿態(tài)和軌跡中的關(guān)鍵幀所含的map點(diǎn)的3d位置(步驟s407)。例如，控制部10將當(dāng)前關(guān)鍵幀的姿態(tài)修正為與發(fā)現(xiàn)的關(guān)鍵幀的姿態(tài)相同的姿態(tài)。然后，使用發(fā)現(xiàn)的關(guān)鍵幀的姿態(tài)與當(dāng)前關(guān)鍵幀的姿態(tài)的差分，對(duì)從發(fā)現(xiàn)的關(guān)鍵幀到當(dāng)前關(guān)鍵幀的軌跡中的各關(guān)鍵幀的姿態(tài)線性地加進(jìn)補(bǔ)正。進(jìn)而，對(duì)于這些各關(guān)鍵幀中所含的map點(diǎn)的3d位置，也按照各關(guān)鍵幀的姿態(tài)的補(bǔ)正量進(jìn)行修正。然后返回步驟s401。

接下來(lái)說(shuō)明自主移動(dòng)裝置100的主流程(圖3)的步驟s107的處理即移動(dòng)控制。其中，雖然基本上是為了自主向目的地移動(dòng)而對(duì)驅(qū)動(dòng)部42發(fā)出動(dòng)作指示，但若特征點(diǎn)對(duì)應(yīng)數(shù)較少，則通過(guò)降低速度或停止，或者變更移動(dòng)方向來(lái)進(jìn)行使特征點(diǎn)對(duì)應(yīng)數(shù)增加的處理。

另外，由于在移動(dòng)控制的處理中使用圖3的步驟s101中簡(jiǎn)單說(shuō)明的移動(dòng)狀態(tài)變量和前次履歷數(shù)這2個(gè)變量，因此進(jìn)行補(bǔ)足說(shuō)明。移動(dòng)狀態(tài)變量具有「通?！?、「低速度」、「停止」、「返回」的任一者的值，用在移動(dòng)速度或移動(dòng)方向的變更中。設(shè)置存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的移動(dòng)履歷的個(gè)數(shù)，在后述的「返回」的移動(dòng)后在決定是否設(shè)定另外的目的地時(shí)使用前次履歷數(shù)。

接下來(lái)參考圖7來(lái)說(shuō)明移動(dòng)控制。首先，移動(dòng)方向速度變更部15判定移動(dòng)狀態(tài)變量的值是否是「返回」(步驟s501)。若移動(dòng)狀態(tài)變量的值是「返回」(步驟s501；“是”)，則前進(jìn)到步驟s519。步驟s519以后的處理在之后敘述。

若移動(dòng)狀態(tài)變量的值不是「返回」(步驟s501；“否”)，則移動(dòng)方向速度變更部15判定是否目的地是未設(shè)定或者是否抵達(dá)了目的地(步驟s502)。若目的地是未設(shè)定或抵達(dá)了目的地(步驟s502；“是”)，則控制部10設(shè)定目的地(步驟s503)，前進(jìn)到步驟s504。在步驟s503，控制部10相當(dāng)于目的地設(shè)定部。若設(shè)定了目的地且尚未抵達(dá)目的地(步驟s502；“否”)，則前進(jìn)到步驟s504。

在步驟s504，移動(dòng)方向速度決定部16基于當(dāng)前位置、目的地以及存儲(chǔ)于地圖存儲(chǔ)部22的地圖的信息來(lái)決定移動(dòng)方向(步驟s504)。

接下來(lái)，移動(dòng)方向速度變更部15判定存儲(chǔ)于特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23的特征點(diǎn)對(duì)應(yīng)數(shù)是否大于第1移動(dòng)變更閾值(例如100)(步驟s505)。若大于第1移動(dòng)變更閾值(步驟s505；“是”)，則對(duì)移動(dòng)狀態(tài)變量設(shè)置「通?！?步驟s506)，前進(jìn)到步驟s509。

若存儲(chǔ)于特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23的特征點(diǎn)對(duì)應(yīng)數(shù)為第1移動(dòng)變更閾值以下(步驟s505；“否”)，則移動(dòng)方向速度變更部15判定特征點(diǎn)對(duì)應(yīng)數(shù)是否大于第2移動(dòng)變更閾值(例如30)(步驟s507)。若大于第2移動(dòng)變更閾值(步驟s507；“是”)，則對(duì)移動(dòng)狀態(tài)變量設(shè)置「低速度」(步驟s508)，前進(jìn)到步驟s509。另外，第1移動(dòng)變更閾值和第2移動(dòng)變更閾值都能設(shè)定為數(shù)十以上的任意的值，但第1移動(dòng)變更閾值設(shè)定為大于第2移動(dòng)變更閾值的值。

在步驟s509，移動(dòng)方向速度決定部16將在步驟s504決定的移動(dòng)方向、和從移動(dòng)狀態(tài)變量的值所表示的速度得到的移動(dòng)距離存儲(chǔ)到移動(dòng)履歷存儲(chǔ)部24。另外，也可以取代移動(dòng)方向以及移動(dòng)距離，而將在自機(jī)位置估計(jì)線程取得的自機(jī)的姿態(tài)(位置(并進(jìn)矢量t)以及朝向(旋轉(zhuǎn)矩陣r))存儲(chǔ)到移動(dòng)履歷存儲(chǔ)部24。

然后，移動(dòng)方向速度決定部16基于在步驟s504決定的移動(dòng)方向、和移動(dòng)狀態(tài)變量的值所表示的速度來(lái)驅(qū)動(dòng)驅(qū)動(dòng)部42(步驟s510)，結(jié)束處理。

若存儲(chǔ)于特征點(diǎn)對(duì)應(yīng)數(shù)存儲(chǔ)部23的特征點(diǎn)對(duì)應(yīng)數(shù)為第2變更閾值以下(步驟s507；“否”)，則移動(dòng)方向速度變更部15判定為移動(dòng)狀態(tài)變量的值是「停止」(步驟s511)。若移動(dòng)狀態(tài)變量的值不是「停止」(步驟s511；“否”)，則對(duì)移動(dòng)狀態(tài)變量設(shè)置「停止」(步驟s512)，開(kāi)始控制部10所具備的計(jì)時(shí)器的計(jì)數(shù)(步驟s513)，結(jié)束。

若移動(dòng)狀態(tài)變量的值是「停止」(步驟s511；“是”)，則移動(dòng)方向速度變更部15判定在步驟s513起始的計(jì)時(shí)器的值是否大于基準(zhǔn)停止時(shí)間(例如10秒)(步驟s514)。若計(jì)時(shí)器的值為基準(zhǔn)停止時(shí)間以下(步驟s514；“否”)，則結(jié)束。

若計(jì)時(shí)器的值大于基準(zhǔn)停止時(shí)間(步驟s514；“是”)，則移動(dòng)方向速度變更部15對(duì)動(dòng)作狀態(tài)變量設(shè)置「返回」(步驟s515)，判定在該時(shí)間點(diǎn)存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的履歷的個(gè)數(shù)是否大于前次履歷數(shù)(步驟s516)。若存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的履歷的個(gè)數(shù)大于前次履歷數(shù)(步驟s516；“是”)，則結(jié)束。

若存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的履歷的個(gè)數(shù)為前次履歷數(shù)以下(步驟s516；“否”)，則控制部10對(duì)前次履歷數(shù)設(shè)置存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的履歷的個(gè)數(shù)(步驟s517)，設(shè)定另外目的地(步驟s518)。在步驟s518，控制部10相當(dāng)于目的地變更部。在步驟s518之后結(jié)束。

在步驟s501中動(dòng)作狀態(tài)變量是「返回」的情況下(步驟s501；“是”)，移動(dòng)方向速度決定部16將與最后存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的移動(dòng)方向相反的方向決定為下一移動(dòng)時(shí)的移動(dòng)方向，將最后存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的移動(dòng)距離決定為下一移動(dòng)時(shí)的移動(dòng)距離(步驟s519)。在存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的信息是自機(jī)的位置以及朝向的情況下，移動(dòng)方向速度決定部16將用于從當(dāng)前位置返回到最后存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的自機(jī)的位置以及朝向的移動(dòng)方向以及移動(dòng)距離決定為下一移動(dòng)時(shí)的移動(dòng)方向以及移動(dòng)距離(步驟s519)。

接下來(lái)，移動(dòng)方向速度變更部15為了判定是否返回了給定的數(shù)量的履歷，判定從前次履歷數(shù)減去在該時(shí)間點(diǎn)存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的履歷的個(gè)數(shù)的數(shù)量是否為給定返回?cái)?shù)(例如10)以上(步驟s520)。若不足給定返回?cái)?shù)(步驟s520；“否”)，則由于還在繼續(xù)「返回」的移動(dòng)，因此就這樣前進(jìn)到步驟s522。

若為給定返回?cái)?shù)以上(步驟s520；“是”)，則為了使「返回」的移動(dòng)結(jié)束而對(duì)動(dòng)作狀態(tài)變量設(shè)置「通?！?步驟s521)，前進(jìn)到步驟s522。

在步驟s522，移動(dòng)方向速度變更部15刪除最后存儲(chǔ)于移動(dòng)履歷存儲(chǔ)部24的移動(dòng)履歷(步驟s522)。然后，移動(dòng)方向速度決定部16基于在步驟s504決定的移動(dòng)方向、和移動(dòng)狀態(tài)變量的值所表示的速度，來(lái)驅(qū)動(dòng)驅(qū)動(dòng)部42(步驟s510)，結(jié)束處理。

步驟s503中的目的地的設(shè)定例如由通信部44取得目的地的信息來(lái)設(shè)定。這時(shí)，通信部44相當(dāng)于目的地取得部。另外，步驟s518中的另外目的地的設(shè)定例如也通過(guò)通信部44取得另外目的地的信息來(lái)設(shè)定。

參考圖8，以具體例來(lái)說(shuō)明通過(guò)讓以上的處理作為主流程(圖3)所示的步驟s107的移動(dòng)控制處理來(lái)反復(fù)執(zhí)行數(shù)次從而自主移動(dòng)裝置100停止或返回的樣子。若特征點(diǎn)對(duì)應(yīng)數(shù)大于第2移動(dòng)變更閾值，則向目的地持續(xù)移動(dòng)(圖8的1.步驟s506或步驟s508)。若特征點(diǎn)對(duì)應(yīng)數(shù)為第2移動(dòng)變更閾值以下，則停止(圖8的2.步驟s512)。

由于在停止的期間通過(guò)在后臺(tái)工作的地圖作成線程來(lái)持續(xù)進(jìn)行向map點(diǎn)數(shù)據(jù)庫(kù)的map點(diǎn)的登錄，因此，若由此特征點(diǎn)對(duì)應(yīng)數(shù)變得大于第2移動(dòng)變更閾值，則移動(dòng)又重開(kāi)(在步驟s514；以“否”結(jié)束后，這以后的移動(dòng)控制的步驟s506或步驟s508)。但若即使停止基準(zhǔn)停止時(shí)間，特征點(diǎn)對(duì)應(yīng)數(shù)也維持在第2移動(dòng)變更閾值以下，則返回給定返回?cái)?shù)(圖8的3.步驟s515、步驟s520)。另外，在圖8中將給定返回?cái)?shù)設(shè)為2。

若返回給定返回?cái)?shù)，則再度向目的地移動(dòng)(圖8的4.)。若在再度向著目的地的過(guò)程中特征點(diǎn)對(duì)應(yīng)數(shù)大于第2移動(dòng)變更閾值的狀態(tài)持續(xù)，則就這樣持續(xù)移動(dòng)(圖8的5.步驟s506或步驟s508)。但在相比于前次前進(jìn)的履歷沒(méi)有行進(jìn)的情況下，即使要移動(dòng)到該目的地，也會(huì)因特征點(diǎn)對(duì)應(yīng)數(shù)少而中途而多次返回，因此變更目的地(圖8的5’.步驟s518)，返回給定返回?cái)?shù)(圖8的6.步驟s520)。在返回給定返回?cái)?shù)后，向變更的另外目的地移動(dòng)(圖8的7.步驟s506或步驟s508)。

通過(guò)進(jìn)行以上那樣的移動(dòng)控制，能將能進(jìn)行自機(jī)位置估計(jì)線程的自機(jī)位置的估計(jì)的狀態(tài)保持盡可能長(zhǎng)的時(shí)間。另外，也可以在圖7的步驟s508變更在步驟s504決定的移動(dòng)方向。該變更既可以變更為隨機(jī)的方向，也可以由自主移動(dòng)裝置100當(dāng)場(chǎng)轉(zhuǎn)動(dòng)360度并拍攝圖像，將移動(dòng)方向變更到拍攝的圖像中特征點(diǎn)多的圖像的方向。另外，在圖7的步驟s518設(shè)定的「另外目的地」在存在多個(gè)目的地的情況下，依次變更為第1候補(bǔ)→第2候補(bǔ)→第3候補(bǔ)即可。另外，在對(duì)相同的目的地存在多個(gè)路徑的情況下，也可以不改變目的地而改變路徑。另外，在沒(méi)有特別目的地而自由移動(dòng)的情況下，也可以隨機(jī)設(shè)定目的地。

在沒(méi)有特別目的地的情況下，期望向特征點(diǎn)多的方向移動(dòng)。作為這樣的移動(dòng)方法，例如能舉出如下方法：自主移動(dòng)裝置100當(dāng)場(chǎng)轉(zhuǎn)動(dòng)360度并每給定的角度就拍攝成為目的地候補(bǔ)的方向的圖像，將拍攝的圖像中特征點(diǎn)最多的圖像的方向作為新的目的地，朝向該方向移動(dòng)。在此，控制部10由于對(duì)每個(gè)給定的角度選定目的地候補(bǔ)，因此相當(dāng)于目的地候補(bǔ)選定部。然后，特征點(diǎn)取得部11由于從目的地候補(bǔ)的方向的圖像取得特征點(diǎn)，因此相當(dāng)于目的地候補(bǔ)特征點(diǎn)取得部。通過(guò)進(jìn)行這樣的處理，相比于隨機(jī)移動(dòng)，能向特征點(diǎn)對(duì)應(yīng)數(shù)變多的方向移動(dòng)的可能性變高，易于恢復(fù)到能進(jìn)行自機(jī)位置的估計(jì)的狀態(tài)。

另外，作為特征點(diǎn)對(duì)應(yīng)數(shù)變少的原因，除了在周?chē)木吧陨硖卣鼽c(diǎn)少以外，還能舉出周?chē)刀毓鈺r(shí)間變長(zhǎng)，在用攝像部41拍攝的圖像中出現(xiàn)抖動(dòng)。在圖7的處理中，由于若特征點(diǎn)對(duì)應(yīng)數(shù)變少就降低速度或停止，因此成為讓拍攝圖像中的抖動(dòng)變少的移動(dòng)控制。關(guān)于這點(diǎn)，也可以在未在特征點(diǎn)對(duì)應(yīng)數(shù)取得部14取得特征點(diǎn)對(duì)應(yīng)數(shù)的狀態(tài)下，也由明亮度傳感器(未圖示)或攝像部41取得周?chē)拿髁炼?，若比給定的明亮度要暗，則降低速度或停止，由此能取得抖動(dòng)少的圖像。

另外，本發(fā)明的自主移動(dòng)裝置100的各功能還能通過(guò)通常的pc(personalcomputer，個(gè)人計(jì)算機(jī))等計(jì)算機(jī)實(shí)施。具體地，在上述實(shí)施方式中，說(shuō)明為自主移動(dòng)裝置100進(jìn)行的自主移動(dòng)控制處理的程序預(yù)先存儲(chǔ)在存儲(chǔ)部20的rom中。但也可以將程序存放在軟盤(pán)、cd-rom(compactdiscreadonlymemory，只讀光盤(pán))、dvd(digitalversatiledisc，數(shù)字多功能盤(pán))以及mo(magneto-opticaldisc，光磁盤(pán))等計(jì)算機(jī)可讀的記錄介質(zhì)來(lái)分發(fā)，通過(guò)將該程序讀入到計(jì)算機(jī)進(jìn)行安裝來(lái)實(shí)現(xiàn)上述的各功能，構(gòu)成這樣的計(jì)算機(jī)。

另外，該計(jì)算機(jī)既可以內(nèi)置于自主移動(dòng)裝置100，也可以與自主移動(dòng)裝置100分開(kāi)存在。即，也可以如圖9所示那樣將控制部、存儲(chǔ)部設(shè)置在外部的服務(wù)器200。通過(guò)云計(jì)算技術(shù)，自主移動(dòng)裝置100的控制部10將在攝像部41、傳感器部30等取得的數(shù)據(jù)經(jīng)由通信部44發(fā)送給外部的服務(wù)器200，在使該服務(wù)器200進(jìn)行運(yùn)算處理后，經(jīng)由通信部44接收服務(wù)器200進(jìn)行的運(yùn)算結(jié)果，來(lái)控制驅(qū)動(dòng)部42等。在圖9中，服務(wù)器200的控制部50成為具備特征點(diǎn)取得部11、地圖作成部12、位置估計(jì)部13、特征點(diǎn)對(duì)應(yīng)數(shù)取得部14、移動(dòng)方向速度變更部15以及移動(dòng)方向速度決定部16的構(gòu)成，但在服務(wù)器200的控制部50和自主移動(dòng)裝置100的控制部10的任一者具備各部即可。關(guān)于是服務(wù)器200的控制部50具備各部還是自主移動(dòng)裝置100的控制部10具備各部，則是任意。

以上說(shuō)明了本發(fā)明優(yōu)選的實(shí)施方式，但本發(fā)明并不限定于相關(guān)的特定的實(shí)施方式，在本發(fā)明中包含權(quán)利要求書(shū)所記載的發(fā)明和其等同的范圍。