JP2017211825A - 自己位置推定装置、及び、自己位置推定方法 - Google Patents

Abstract

【課題】自己位置推定装置において、自己位置及び／又は姿勢の推定精度を維持しつつ、これらの推定に要する演算量や時間を減少する。【解決手段】自己位置推定装置１００は、記憶部１４１と、ローカルマップ取得部１４２と、自己位置推定部１４３と、調節部１４４と、を備える。記憶部１４１は、移動環境ＭＥに存在する障害物の位置を表す環境地図Ｍ１を記憶する。ローカルマップ取得部１４２は、移動体１の周囲に存在する障害物の移動体１に対する相対的な位置を表すローカルマップＭ２を取得する。自己位置推定部１４３は、環境地図Ｍ１に設定されたローカルマップ配置領域Ａ内の複数の配置位置のうち、ローカルマップＭ２が環境地図Ｍ１と最も一致する配置位置を第１自己位置と推定する。また、当該配置位置におけるローカルマップの回転角を第１姿勢と推定する。調節部１４４は、所定の条件に従って、ローカルマップ配置領域Ａの大きさを調節する。【選択図】図６

Description

本発明は、移動環境を移動する移動体の自己位置及び／又はその姿勢を推定する装置及びその方法に関する。

従来、車両や自律走行ロボットなどの移動体が移動している環境（移動環境）における、当該移動体の位置である自己位置、及び、当該自己位置における姿勢を推定する装置が知られている。例えば、特許文献１には、作業環境に存在する障害物との間の距離に基づくデータと、カメラにより得られた作業環境の画像（写真）データとに基づいて、自己位置を推定する移動装置が開示されている。
この装置では、距離に基づくデータを用いて推定された自己位置の信頼度に基づいて画像データの検出範囲を拡げ、検出範囲を拡げて取得された画像データにランドマークが含まれるときに、当該画像データに基づく自己位置推定を実行している。

特開２０１５−１４１５８０号公報

上記の装置のように画像データの検出範囲を無条件に拡げることは、移動装置の位置によっては不都合な場合がある。例えば、検出範囲を拡げて取得した画像データ中に、類似形状を有する複数のランドマークが存在する場合、本来のランドマークではなく、形状が類似した他のランドマークを用いて誤った自己位置が算出される可能性がある。

本発明の目的は、移動体の自己位置を、移動環境を表す地図情報とセンサなどにより得られた情報とに基づいて推定する装置において、移動体の自己位置及び／又は姿勢の推定精度を維持しつつ、これらの推定に要する演算量や時間を減少することにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る自己位置推定装置は、移動体の移動環境における自己位置及び／又は姿勢を推定する。自己位置推定装置は、記憶部と、ローカルマップ取得部と、自己位置推定部と、調節部と、を備える。記憶部は、移動環境に存在する障害物の位置を表す環境地図を記憶する。ローカルマップ取得部は、ローカルマップを取得する。ローカルマップは、移動体の周囲に存在する障害物の移動体に対する相対的な位置を表す地図情報である。

自己位置推定部は、第１推定モードを実行する。具体的には、自己位置推定部は、環境地図に設定されたローカルマップ配置領域内の複数の配置位置（自己位置の候補）にてローカルマップを配置及び／又は回転したときに、ローカルマップが環境地図と最も一致する配置位置を、移動体の移動環境における自己位置である第１自己位置と推定する。また、自己位置推定部は、当該配置位置におけるローカルマップの回転角を、第１自己位置における移動体の第１姿勢と推定する。調節部は、所定の条件に従って、ローカルマップ配置領域の大きさを調節する。

上記の自己位置推定装置では、まず、ローカルマップ取得部が、環境地図とマッチングさせるための地図情報であるローカルマップを取得する。次に、調節部が、所定の条件に従って、ローカルマップ配置領域の大きさを調節する。
その後、自己位置推定部が、第１推定モードを実行する。具体的には、自己位置推定部は、所定の条件に従って定められた大きさを有し、環境地図に設定されたローカルマップ配置領域内の複数の配置位置（自己位置の候補）のそれぞれに、上記のローカルマップを配置する。また、当該配置位置においてローカルマップを回転する。このとき、自己位置推定部は、ローカルマップ配置領域内の複数の配置位置のうち、ローカルマップが環境地図と最も一致する配置位置を、移動環境における移動体の自己位置（第１自己位置）と推定する。また、必要に応じて、ローカルマップが環境地図と最も一致する配置位置におけるローカルマップの回転角を、第１自己位置における移動体の姿勢（第１姿勢）と推定する。

上記の自己位置推定装置では、第１推定モード（マップマッチングによる移動体１の第１自己位置及び／又は第１姿勢の推定）の実行時において、所定の条件に従って、ローカルマップを配置する範囲を定めるローカルマップ配置領域の大きさが調整されている。これにより、移動体の自己位置及び／又は姿勢の推定精度を維持しつつ、マップマッチングによるこれらの推定に要する演算量を減らして、それにより当該推定にかかる時間を減少できる。

無障害領域から障害領域へと移動体が移動したら、調節部は、ローカルマップ配置領域の大きさを、所定の時間、予め決められた大きさよりも拡大してもよい。無障害領域は、移動環境において、障害物が少数しか存在しない、又は、全く存在しない領域である。一方、障害領域は、移動環境において、障害物が存在する領域である。
これにより、無障害領域を移動体が移動中に累積された推定誤差を、ローカルマップ配置領域を拡大したマップマッチングにより解消して、移動体の自己位置及び／又は姿勢の推定精度を維持できる。

無障害領域を移動体が移動中に、自己位置推定部は、第１推定モードを実行しなくてもよい。これにより、マップマッチングでは第１自己位置及び／又は第１姿勢の推定が困難な場合に、第１推定モードを実行しない。

単調形状領域から特徴形状領域へと移動体が移動したら、調節部は、ローカルマップ配置領域の大きさを、所定の時間、予め決められた大きさよりも拡大してもよい。単調形状領域は、環境地図が単調な形状を有する領域である。特徴形状領域は、環境地図が特徴的な形状を有する領域である。
これにより、単調形状領域を移動体が移動中に累積された推定誤差を、ローカルマップ配置領域を拡大したマップマッチングにより解消して、移動体の自己位置及び／又は姿勢の推定精度を維持できる。

単調形状領域を移動体が移動中、又は、所定の時間の経過後、調節部は、ローカルマップ配置領域の大きさを、予め決められた大きさとしてもよい。これにより、第１推定モードによる第１自己位置及び／又は第１姿勢の推定に要する演算量を減らしつつ、それにより当該推定にかかる時間を減少できる。

移動体は、移動環境を移動するための主輪を有していてもよい。この場合、自己位置推定部は、第２推定モードをさらに実行可能である。第２推定モードは、主輪の回転量に基づいて、移動体の移動環境における自己位置及び／又は姿勢を、それぞれ、第２自己位置及び／又は第２姿勢と推定するモードである。
この場合、調節部は、少なくとも第２推定モードを実行している状態から、第１推定モードのみを実行する状態へと切り替わってから所定の時間、ローカルマップ配置領域を、通常の第１推定モードの実行時の大きさよりも拡大する。
これにより、主輪の回転量に基づいて推定された第２自己位置及び／又は第２姿勢の推定誤差を、ローカルマップ配置領域を拡大した第１推定モードの実行により解消して、移動体の自己位置及び／又は姿勢の推定精度を維持できる。

移動体が一時停止状態から移動を開始したら、調節部は、ローカルマップ配置領域の大きさを、通常の第１推定モードの実行時よりも拡大してもよい。
これにより、一時停止状態から移動を開始したときに発生する自己位置及び／又は姿勢の推定誤差を、ローカルマップ配置領域を拡大した第１推定モードの実行により解消して、移動体の自己位置及び／又は姿勢の推定精度を維持できる。

移動体が一時停止したら、調節部は、ローカルマップ配置領域の大きさを、移動体が移動中であるときよりも縮小してもよい。これにより、移動体が一時停止しており自己位置及び／又は姿勢にほとんど変化がない場合に、小さいローカルマップ配置領域を用いて、第１自己位置及び／又は第１姿勢の推定に必要な演算量を減少できる。

移動体が環境地図に表されていない未知の障害物に囲まれていたら、調節部は、ローカルマップ配置領域の大きさを、未知の障害物に囲まれていないときよりも縮小してもよい。これにより、未知の障害物の存在があったとしても、環境地図とローカルマップとのマップマッチングが適切に行われる確率が高くなり、第１自己位置及び／又は第１姿勢の推定が正確になる。

記憶部は、属性情報をさらに記憶してもよい。属性情報は、環境地図の所定の位置におけるローカルマップ配置領域の大きさを定める情報である。この場合、調節部は、属性情報に従って、ローカルマップ配置領域の大きさを設定する。
これにより、環境地図（移動環境）の各位置における状況に応じて、ローカルマップ配置領域の大きさを調節できる。

ローカルマップ配置領域の大きさを調節すべき所定の条件が同時に複数発生したら、調節部は、当該複数の条件のうち、優先度がより高い条件に従ってローカルマップ配置領域の大きさを設定してもよい。これにより、優先度が高い条件に合わせてローカルマップ配置領域の大きさを決定できる。

本発明の他の見地に係る自己位置推定方法は、移動環境における移動体の自己位置及び／又は姿勢を推定する方法である。自己位置推定方法は、以下のステップを含む。
◎移動環境に存在する障害物の位置を表す環境地図を生成するステップ。
◎移動体の周囲に存在する障害物の移動体に対する相対的な位置を表すローカルマップを取得するステップ。
◎環境地図に設定され、所定の条件に従って大きさが調節されたローカルマップ配置領域内の複数の配置位置にてローカルマップを配置及び／又は回転したときに、ローカルマップが環境地図と最も一致する配置位置を、移動体の移動環境における自己位置と推定し、及び／又は、当該配置位置におけるローカルマップの回転角を、当該自己位置における移動体の姿勢と推定するステップ。

上記の自己位置推定方法では、移動体の自己位置及び／又は姿勢をマップマッチングにて推定する際に、所定の条件に従って、ローカルマップを配置する範囲を定めるローカルマップ配置領域の大きさが調整されている。これにより、移動体の自己位置及び／又は姿勢の推定精度を維持しつつ、マップマッチングによるこれらの推定に要する演算量を減らして、それにより当該推定にかかる時間を減少できる。

本発明に係る自己位置推定装置及び自己位置推定方法では、移動体の自己位置及び／又は姿勢の推定精度を維持しつつ、これらの推定に要する演算量や時間を減少できる。

本発明の一実施形態が採用された移動体の構成を示す図。 制御部の構成を示す図。 移動体の動作を示すフローチャート。 属性情報が設定される通路の一例を示す図。 水平面にて反射された赤外光により、移動環境に存在しない障害物の像を含むようなローカルマップが取得された場合の一例を示す図。 移動環境に本来存在しない水平面の像がローカルマップに含まれることで、移動体の自己位置及び／又は姿勢の誤推定が発生した場合の一例を示す図。 マップマッチングによって自己位置を推定できない場合の一例を示す図。 マップマッチングによって姿勢を推定できる場合の一例を示す図。 ローカルマップ配置領域の調節方法を示すフローチャート。 囲われ判定が実行される場合の一例を示す図（その１）。 囲われ判定が実行される場合の一例を示す図（その２）。 未知の障害物が存在しない場合に取得された環境地図の一例を示す図。 未知の障害物が存在する場合に取得されたローカルマップの一例を示す図。 囲われの判定方法の一例を示す図。 未知の障害物に囲まれたために自己位置及び／又は姿勢が誤推定された場合の一例を示す図。 囲まれ検知の他の実施形態を示す図。

１．第１実施形態
（１）移動体の構成
自己位置推定装置１００（後述）が用いられる移動体１について説明する。本実施形態の移動体１は、例えば、ユーザにより教示された経路を再現しながら、自律して走行可能な移動体である。
まず、移動体１の構成を、図１を用いて説明する。図１は、本発明の一実施形態が採用された移動体の構成を示す図である。
移動体１は、本体１１を有する。本体１１は、移動体１の本体を構成する例えば筐体である。本実施形態において、後述する「自己位置」は、移動環境ＭＥを表す環境地図Ｍ１上における本体１１の中心の位置（座標）と定義する。また、「自身」との語は、移動体１の本体１１のことを指すこととする。

移動体１は、移動部１２を有する。移動部１２は、例えば、本体１１を移動させる差動二輪型の走行部である。
具体的には、移動部１２は、一対のモータ１２１ａ、１２１ｂを有する。一対のモータ１２１ａ、１２１ｂは、本体１１の底部に設けられた、例えばサーボモータやブラシレスモータなどの電動モータである。一対のモータ１２１ａ、１２１ｂは、制御部１４（後述）と電気的に接続され、制御部１４からの指令に基づいて、それぞれ独立に、任意の回転数及びトルクにてその出力回転軸を回転させる。

移動部１２は、一対の主輪１２３ａ、１２３ｂを有する。一対の主輪１２３ａ、１２３ｂは、それぞれ、一部が移動環境ＭＥの床面（移動面）と接しており、一対のモータ１２１ａ、１２１ｂの出力回転軸に接続される。これにより、主輪１２３ａ、１２３ｂは、それぞれ、モータ１２１ａ、１２１ｂによって独立して回転し、本体１１を移動させる。
上記のように独立回転が可能なことによって、主輪１２３ａと１２３ｂの回転数に差を生じさせて本体１１の姿勢を変化できる。一方、一対の主輪１２３ａ、１２３ｂの回転数が同じであれば、本体１１を直進できる。

モータ１２１ａ、１２１ｂのそれぞれの出力回転軸には、エンコーダ１２５ａ、１２５ｂ（図２）が設けられている。エンコーダ１２５ａ、１２５ｂは、例えば、モータ１２１ａ、１２１ｂの出力回転軸の回転量に基づいたパルス信号を出力するインクリメンタル型のエンコーダである。これにより、自己位置推定部１４３（後述）は、モータ１２１ａ、１２１ｂの回転量、すなわち、主輪１２３ａ、１２３ｂの回転量に基づいて、移動環境ＭＥにおける移動体１（本体１１）の位置及び／又は姿勢を推定できる。

移動体１は、レーザレンジセンサ１３を有する。レーザレンジセンサ１３は、例え
ば、レーザ発振器によりパルス発振されたレーザ光を、移動環境ＭＥ中の構造物（例えば、移動環境ＭＥに配置された柱、棚、壁など）である障害物に放射状に照射し、当該障害物から反射した反射光をレーザ受信器により受信することにより、当該障害物に関する情報を取得するレーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）である。
レーザレンジセンサ１３は、本体１１の前部に配置された第１レーザレンジセンサ１３１と、本体１１の後部に配置された第２レーザレンジセンサ１３３と、を有する。

第１レーザレンジセンサ１３１は、本体１１の前方に左右方向にレーザ光を放射状に発生することにより、第１レーザレンジセンサ１３１を中心とした本体１１の前方の半径２０ｍ程度の円内に含まれる障害物に関する情報を取得する。
一方、第２レーザレンジセンサ１３３は、本体１１の後方に左右方向にレーザ光を放射状に発生することにより、第２レーザレンジセンサ１３３を中心とした本体１１の後方の半径５ｍ程度の円内に含まれる障害物に関する情報を取得する。
なお、上記レーザレンジセンサの検出可能距離は、上記の値に限られず、移動体１の用途等に応じて適宜変更できる。

移動体１は、制御部１４を有する。制御部１４は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ハードディスク装置、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、記憶媒体読み出し装置などの記憶装置、信号変換を行うインターフェースなどを備えたマイコンシステムである。制御部１４は、移動体１の各部の制御を行う。制御部１４の構成については、後ほど詳しく説明する。

移動体１は、補助輪部１５をさらに有していてもよい。補助輪部１５は、２つの補助車輪１５ａ、１５ｂを有する。２つの補助車輪１５ａと１５ｂは、それぞれが独立に回転可能なように取り付けられている。補助輪部１５を備えることにより、移動体１は安定に、かつ、スムーズに移動できる。

（２）制御部の構成
図２を用いて、制御部１４の構成を説明する。図２は、制御部の構成を示す図である。
なお、以下に説明する制御部１４の各部の機能の一部又は全部は、マイコンシステムにおいて実行可能なプログラムとして実現されていてもよい。また、当該プログラムは、当該マイコンシステムの記憶装置に記憶されていてもよい。又は、上記機能の一部又は全部は、カスタムＩＣなどにより実現されていてもよい。

制御部１４は、記憶部１４１を有する。記憶部１４１は、制御部１４を構成するコンピュータシステムの記憶装置の記憶領域の一部である。記憶部１４１は、移動体１を制御するために用いられる各種情報を記憶する。
具体的には、記憶部１４１は、環境地図Ｍ１を記憶する。環境地図Ｍ１は、例えば、移動環境ＭＥに存在する障害物の位置を表す地図情報である。具体的には、環境地図Ｍ１は、移動環境ＭＥを表す座標平面上の障害物の位置を示す座標値データの集合体である。
環境地図Ｍ１が、移動環境ＭＥを表す座標平面上の障害物の位置を示す座標値データとして表されることにより、当該環境地図Ｍ１とローカルマップＭ２（後述）とのマップマッチングにより、移動体１（本体１１）の移動環境ＭＥにおける位置及び／又は姿勢を、それぞれ、移動環境ＭＥを表す座標平面上の座標値及び／又は基準座標軸からの回転角として推定できる。

記憶部１４１は、属性情報ＡＩを記憶する。属性情報ＡＩは、移動環境ＭＥの予め決められた位置において移動体１がすべき動作を記憶した情報である。本実施形態において、属性情報ＡＩは、環境地図Ｍ１の所定の位置における、ローカルマップ配置領域Ａの大きさを定めている。これにより、調節部１４４は、移動体１が環境地図Ｍ１の上記の所定の位置に到達したときに、ローカルマップ配置領域Ａの大きさを、属性情報ＡＩに従って調節できる。

属性情報ＡＩは、環境地図Ｍ１の所定の位置におけるローカルマップ配置領域Ａの大きさだけでなく、当該所定の位置におけるローカルマップ配置領域Ａの形状を定義しておいてもよい。例えば、ローカルマップ配置領域Ａの形状を正方形とする場合には、当該正方形の一辺の長さと、ローカルマップ配置領域Ａの形状が正方形であることを示す識別子とを、属性情報ＡＩに関連付けて記憶してもよい。
その他、移動平面の座標上に定義される領域の式（不等式）を属性情報ＡＩに記憶することによって、任意の形状を有するローカルマップ配置領域Ａを定義してもよい。

その他、属性情報ＡＩは、移動環境ＭＥ（環境地図Ｍ１）の所定の位置における移動体１の速度や姿勢などを記憶してもよい。例えば、昇降機が近傍に存在する位置では一時停止するなどの動作を属性情報ＡＩに記憶してもよい。また、移動体１の用途に基づいた動作（例えば、移動環境ＭＥの所定の位置において、特定の広告を表示するなど）を属性情報ＡＩに記憶してもよい。

制御部１４は、ローカルマップ取得部１４２を有する。ローカルマップ取得部１４２は、レーザレンジセンサ１３から取得した信号に基づいて、ローカルマップＭ２を取得する。具体的には、ローカルマップ取得部１４２は、レーザレンジセンサ１３からレーザ光を照射したタイミングと、反射光をレーザレンジセンサ１３（レーザ受信器）にて受信したタイミングとの時間差から、レーザレンジセンサ１３と障害物との距離を算出する。また、例えば、反射光を受信した時のレーザ受信器の受光面の角度から、本体１１から見た障害物が存在する方向を算出できる。

その後、ローカルマップ取得部１４２は、上記の時間差から算出される本体１１から見た障害物の相対的な距離と、反射光を受光したときの受光面の角度とを、移動環境ＭＥを表す座標平面上の座標値に変換することにより、移動体１（本体１１）の周囲に存在する障害物の移動体１に対する相対的な位置を表すローカルマップを取得する。
例えば、移動体１から障害物までの距離がＲと算出され、レーザレンジセンサ１３の受光面の角度が移動体１の直進方向（図１）に対してα（反時計回りを正の角度とする）となっている場合、障害物の移動体１に対する相対的な位置は、移動体１の中心を原点とした座標系において、（Ｒ＊ｃｏｓα，Ｒ＊ｓｉｎα）と算出できる。

制御部１４は、自己位置推定部１４３を有する。自己位置推定部１４３は、移動体１が移動環境ＭＥを移動中に、移動体１の移動環境ＭＥにおける位置（自己位置と呼ぶ）、及び／又は、所定の軸（例えば、上記のＸ軸）に対して移動体１が向いている角度（姿勢と呼ぶ）を推定する。本実施形態において、自己位置推定部１４３は、上記の自己位置及び／又は姿勢を推定するモードとして、第１推定モードと第２推定モードとを実行可能である。

第１推定モードは、ローカルマップ取得部１４２により得られたローカルマップと、記憶部１４１に記憶された環境地図Ｍ１とをマップマッチングすることにより、移動体１の自己位置及び／又は姿勢を推定するモードである。第１推定モードによる推定された自己位置及び姿勢を、それぞれ、第１自己位置及び第１姿勢と呼ぶことにする。

第１推定モードの実行時において、自己位置推定部１４３は、まず、主輪１２３ａ、１２３ｂの回転量に基づいて算出された環境地図Ｍ１上の位置にローカルマップＭ２を配置する。次に、自己位置推定部１４３は、当該位置において、ローカルマップＭ２を、主輪１２３ａ、１２３ｂの回転量に基づいて算出された姿勢（角度）だけ回転して、環境地図Ｍ１とローカルマップＭ２とのマップマッチングを行う。
本実施形態において、自己位置推定部１４３は、主輪１２３ａ、１２３ｂの回転量に基づいて推定された位置及び／又は姿勢の推定誤差を考慮して、上記のローカルマップＭ２の配置位置や回転角度に幅を持たせる。このような、マップマッチングを実行する際のローカルマップＭ２の配置位置の範囲や回転角度の変更幅を、「ローカルマップ配置領域Ａ」と呼ぶ。

自己位置推定部１４３が、環境地図Ｍ１とローカルマップＭ２とをマップマッチングして第１自己位置及び／又は第１姿勢を推定する際に（第１推定モードの実行時に）、ローカルマップＭ２の配置位置及び回転角度に幅を持たせることにより、主輪１２３ａ、１２３ｂの回転量に基づく自己位置及び／又は姿勢の推定誤差と、マップマッチングにおける自己位置及び／又は姿勢の推定誤差（主に、レーザレンジセンサ１３により取得されるローカルマップＭ２及び／又は環境地図Ｍ１に含まれる誤差に起因する）とを相殺して、正確に第１自己位置及び／又は第１姿勢を推定できる。

一方、第２推定モードは、エンコーダ１２５ａ、１２５ｂから取得したモータ１２１ａ、１２１ｂ（主輪１２３ａ、１２３ｂ）の回転量に基づいて、移動体１の環境地図Ｍ１上における自己位置及び／又は姿勢を推定するモードである。第２推定モードにより推定された自己位置及び姿勢を、それぞれ、第２自己位置及び第２姿勢と呼ぶことにする。
主輪１２３ａ、１２３ｂの回転量に基づく自己位置及び／又は姿勢の推定は、例えば、前回推定された自己位置及び／又は姿勢に、主輪１２３ａ、１２３ｂの回転量に基づいて算出された移動距離及び／又は姿勢変化を加算することにより実行できる。

第２推定モードは、例えば、マップマッチングでは正確な自己位置や姿勢の推定が困難な場合に実行される。例えば、レーザレンジセンサ１３により検知できる障害物が全く存在しない領域、又は、障害物が存在してもその数が少なすぎて正確なマップマッチングを実行できない領域（無障害領域と呼ぶ）を移動体１が移動中に、第２推定モードが実行される。

その他、例えば、２つの直線的な壁により形成されている直線状の通路のような、単調な形状を有する移動環境ＭＥの領域であり、単調な形状のローカルマップや環境地図Ｍ１しか生成されない領域（単調形状領域と呼ぶ）を移動体１が移動中にも、第２推定モードが実行される。

制御部１４は、調節部１４４を有する。調節部１４４は、所定の条件に従って、ローカルマップを環境地図Ｍ１に配置する配置位置の範囲（ローカルマップ配置領域Ａ）の大きさを調節する。例えば、ローカルマップ配置領域Ａを一辺の長さがＬである正方形と定義した場合には、調節部１４４は、当該Ｌの値を所定の条件に従って調節する。
その他、ローカルマップ配置領域Ａを、例えば、半径がＲである円と定義した場合には、当該Ｒの値を所定の条件に従って調節することにより、ローカルマップ配置領域Ａの大きさを調節できる。

一方、ローカルマップ配置領域Ａにおいて、ローカルマップＭ２の各配置位置における回転角度（姿勢）の変更幅がΔθと定められている場合には、調節部１４４は、回転角度（姿勢）の変更幅を示すΔθを所定の条件に従って調節することにより、ローカルマップ配置領域Ａの大きさを調節できる。

上記の記憶部１４１と、ローカルマップ取得部１４２と、自己位置推定部１４３と、調節部１４４と、が本実施形態に係る自己位置推定装置１００を構成する。

制御部１４は、走行制御部１４５を有する。走行制御部１４５は、モータ１２１ａ、１２１ｂを制御する。走行制御部１４５は、例えば、モータ１２１ａ、１２１ｂのそれぞれの制御量を算出し、当該制御量に基づいた駆動電力をモータ１２１ａ、１２１ｂのそれぞれに出力するモータドライバである。走行制御部１４５は、エンコーダ１２５ａ、１２５ｂから入力したモータ１２１ａ、１２１ｂの単位時間あたりの回転量（回転速度）が、所望の回転速度となるように、モータ１２１ａ、１２１ｂの制御量を算出している（フィードバック制御）。

走行制御部１４５は、ユーザによる切替などにより、自律モード又は手動モードのいずれかを実行可能となっている。自律モードの実行時においては、走行制御部１４５は、例えば、記憶部１４１に記憶された走行スケジュールＴＳに示された各通過位置（例えば、環境地図Ｍ１上の座標値）と、自己位置推定部１４３において推定された自己位置及び／又は姿勢との差に基づいて、モータ１２１ａ、１２１ｂのそれぞれの制御量を算出して、算出された制御量に基づいた駆動電力を、これらのモータに出力する。
これにより、走行制御部１４５は、自律モードの実行時においては、本体１１を、上記の走行スケジュールＴＳに従って、自律的に移動できる。

一方、手動モードの実行時においては、走行制御部１４５は、例えば、無線又は有線にて移動体１と通信可能なコントローラ又はコンピュータシステム、又は、移動体１に設けられた操作ハンドル（図示せず）などを用いたユーザの操作を受け付けて、当該ユーザの操作の操作量に基づいて、モータ１２１ａ、１２１ｂを制御する。これにより、移動体１は、ユーザの操作により移動可能となる。

制御部１４が上記の構成を有することにより、自己位置推定装置１００は、第１推定モード（マップマッチングによる自己位置及び／又は姿勢の推定）の実行時において、所定の条件に従ってローカルマップ配置領域Ａの大きさを調整できる。
その結果、移動体１の自己位置及び／又は姿勢の推定精度を維持しつつ、マップマッチングによるこれらの推定に要する演算量が無駄に増大することを回避できる。また、これらの推定にかかる時間を減少できる。

（３）移動体の動作
図３を用いて、移動環境ＭＥを移動中の移動体１の動作を説明する。図３は、移動体の動作を示すフローチャートである。
以下では、ユーザによる移動体１の操作を移動体１に教示するとともに環境地図Ｍ１を作成し、当該教示された操作に従って、移動体１を自律的に移動させる場合の動作を説明する。

まず、走行モードを手動モードに設定し、ユーザによる移動体１の動作と、移動体１が移動する移動環境ＭＥを表す環境地図Ｍ１と、を移動体１に教示する（ステップＳ１）。
具体的には、ユーザの操作により移動体１を移動環境ＭＥにて移動中に、まず、ローカルマップ取得部１４２が、所定の時間間隔毎に、ローカルマップＭ２を取得する。次に、自己位置推定部１４３が、現段階までに作成された環境地図Ｍ１とローカルマップＭ２とをマップマッチングすることにより、移動体１（本体１１）の自己位置及び／又は姿勢を推定する。

自己位置及び／又は姿勢を推定後、自己位置推定部１４３は、現段階までに作成された環境地図Ｍ１に、今回取得したローカルマップＭ２を推定自己位置で座標変換したものを合体して、新たな環境地図Ｍ１を作成する。また、自己位置推定部１４３は、推定した自己位置及び／又は姿勢と、当該自己位置及び／又は姿勢を推定した時刻とを関連づけて走行スケジュールＴＳを作成し、記憶部１４１に記憶する。
また、環境地図Ｍ１及び走行スケジュールＴＳを移動体１に教示中に、ユーザは、必要に応じて、移動体１を操作する入力装置を用いて、ローカルマップ配置領域Ａの大きさを設定する。ユーザによりローカルマップ配置領域Ａの大きさが設定されると、自己位置推定部１４３は、上記の入力装置から入力したローカルマップ配置領域Ａの大きさと、当該ローカルマップ配置領域Ａが設定された時の移動体１の自己位置及び／又は姿勢と、を関連付けて、属性情報ＡＩとして記憶部１４１に記憶する。

例えば、ユーザは、図４Ａに示すような、下り坂を有し、当該下り坂を通過後に水平面が存在し、かつ、当該水平面の突き当たりに壁（垂直面）が存在するような通路を移動体１が移動中に、属性情報ＡＩを設定する。図４Ａは、属性情報が設定される通路の一例を示す図である。
移動体１が移動中の通路が下り坂になっており、当該下り坂の通過後に水平面が存在する場合、移動体１が当該下り坂を移動中に、レーザレンジセンサ１３から出力された赤外光が、水平面にて反射され、当該反射された赤外光をレーザレンジセンサ１３が受光してしまうことがある。

当該水平面にて反射された赤外光の存在により、ローカルマップ取得部１４２は、図４Ｂに示すように、移動環境ＭＥには本来存在しない障害物の像（水平面の像）を含むようなローカルマップＭ２を取得してしまう。図４Ｂは、水平面にて反射された赤外光により、移動環境に存在しない障害物の像を含むようなローカルマップが取得された場合の一例を示す図である。

本来存在しない水平面の像を含むローカルマップＭ２を環境地図Ｍ１にマッチングさせた場合、例えば、図４Ｃに示すように、ローカルマップＭ２中の当該水平面の像が、環境地図Ｍ１中の水平面の突き当たりに存在する壁の像であると誤認識される場合がある。その結果、自己位置推定部１４３が、第１推定モードの実行時において、移動体１の第１自己位置及び／又は第１姿勢を、図４Ｃの点線に示すように誤推定することがある。図４Ｃは、移動環境に本来存在しない水平面の像がローカルマップに含まれることで、移動体の自己位置及び／又は姿勢の誤推定が発生した場合の一例を示す図である。
このような誤推定を回避するために、図４Ｃの一点鎖線にて示すようなローカルマップ配置領域Ａを環境地図Ｍ１に設定し、第１推定モードの実行時に、自己位置推定部１４３が、ローカルマップＭ２を本来の位置から過剰に遠い位置に配置して、上記の図４Ｃに示すような誤推定を発生させないようにする。

その他、例えば、移動体１の主輪１２３ａ、１２３ｂが接触する移動面が水などにより濡れる位置などにおいては、主輪１２３ａ、１２３ｂが滑りやすく、自己位置推定部１４３における自己位置及び／又は姿勢の推定誤差が発生しやすい。従って、移動面が濡れる位置においては、大きなローカルマップ配置領域Ａを属性情報ＡＩにて設定し、移動体１が、当該位置において推定誤差を発生しにくくできる。

なお、環境地図Ｍ１及び走行スケジュールＴＳを作成中に、自己位置推定部１４３は、走行制御部１４５を介して、ユーザによる移動体１の操作量（例えば、モータ１２１ａ、１２１ｂの回転数や、当該回転数に基づいて算出できる移動体１の移動速度など）を入力し、当該入力した操作量を走行スケジュールＴＳに関連付けてもよい。

上記のローカルマップＭ２の取得、自己位置及び／又は姿勢の推定、走行スケジュールＴＳ及び環境地図Ｍ１の作成、及び、属性情報ＡＩの設定を、ユーザが移動体１を移動終了位置まで移動するまでの間繰り返し実行することにより、移動体１が移動を開始した移動開始位置から移動終了位置までの走行スケジュールＴＳ、環境地図Ｍ１、及び属性情報ＡＩを生成して、記憶部１４１に記憶できる。

なお、環境地図Ｍ１、走行スケジュールＴＳ、及び／又は属性情報ＡＩは、ユーザにより移動体１を操作しながら教示する場合に限られず、例えば、他の入力装置（キーボードやマウスなど）を用いて、移動体１の操作を実行することなく作成することもできる。
また、上記のように、ローカルマップＭ２に本来存在しない障害物が含まれる結果、環境地図Ｍ１においても本来存在しない障害物が含まれてしまった場合には、環境地図Ｍ１を編集して、当該存在しない障害物の像を削除する。

環境地図Ｍ１、走行スケジュールＴＳ、及び属性情報ＡＩを生成後、必要に応じて、移動体１を上記の移動開始位置へ配置後、例えば、移動体１を操作するための入力装置などを用いて、移動体１の走行モードを、移動体１を走行スケジュールＴＳに従って自律的に移動させる自律モードへと切り替える。
走行モードが自律モードに切り替わり、移動体１が自律的な移動を開始すると、まず、ローカルマップ取得部１４２が、上記にて説明したようにしてローカルマップＭ２を取得して、記憶部１４１に記憶する（ステップＳ２）。

ローカルマップＭ２を取得後、自己位置推定部１４３が、移動体１の自己位置及び／又は姿勢を推定する。移動体１の自己位置及び／又は姿勢を推定するにあたり、自己位置推定部１４３は、マップマッチングによる第１自己位置及び／又は第１姿勢の推定（第１推定モード）を実行するか、及び／又は、主輪１２３ａ、１２３ｂの回転量に基づく第２自己位置及び／又は第２姿勢の推定（第２推定モード）を実行するかを決定する。
そのため、自己位置推定部１４３は、まず、移動環境ＭＥにおいて、障害物が少数しか存在しない、又は、全く存在しない無障害領域を移動体１が移動中であるか否かを判定する（ステップＳ３）。

例えば、ステップＳ２にて取得したローカルマップＭ２が、「空」の（障害物の像を表すデータが一切含まれない）ローカルマップＭ２であるか、又は、少数の障害物しか含まないローカルマップＭ２である場合（ステップＳ３において「Ｙｅｓ」の場合）、自己位置推定部１４３は、移動体１が無障害領域を移動中であると判定する。
移動体１が無障害領域を移動中であると判定すると、自己位置推定部１４３は、第１推定モードを実行しないと決定する。なぜなら、無障害領域にて取得されるローカルマップＭ２は、マップマッチングを正確に実行できるだけの十分な障害物の情報を含んでいないからである。
この場合、自己位置推定部１４３は、第２推定モードのみを実行し、主輪１２３ａ、１２３ｂの回転量に基づいて推定された第２自己位置及び／又は第２姿勢を、移動体１の自己位置及び／又は姿勢として推定する（ステップＳ４）。

なお、ステップＳ３において、例えば、第１推定モード及び／又は第２推定モードを実行して、移動体１の自己位置及び／又は姿勢がすでに推定されている場合には、推定された自己位置（又は姿勢）に基づいて、移動体１が無障害領域を移動中であるか否かを判定してもよい。例えば、推定された自己位置が、移動環境ＭＥを表す座標平面において、障害物が全く又は少数しか存在しない無障害領域中に含まれる座標値となっている場合に、移動体１が無障害領域を移動中であると判定できる。

上記のように、無障害領域を移動体１が移動中に、自己位置推定部１４３が、第１推定モードを実行しないと決定することにより、マップマッチングでは第１自己位置及び／又は第１姿勢の推定が困難な場合に、無駄に第１推定モードを実行することを回避できる。

一方、例えば、障害物の像が存在するローカルマップＭ２が取得されているか、又は、推定された自己位置が無障害領域外の座標値である場合（ステップＳ３において「Ｎｏ」の場合）、自己位置推定部１４３は、第１推定モードを実行可能と決定する。なぜなら、障害物の像が存在するローカルマップＭ２が取得されていれば、マップマッチングを実行することが可能だからである。
第１推定モードを実行可能と決定後、自己位置推定部１４３は、調節部１４４に対して、ローカルマップ配置領域Ａの大きさを調節するよう指令する（ステップＳ５）。ステップＳ５におけるローカルマップ配置領域Ａの調節方法については、後ほど詳しく説明する。

ローカルマップ配置領域Ａの大きさを調節後、自己位置推定部１４３は、さらに、第１推定モードの実行のみで、正確な自己位置及び／又は姿勢の推定が可能か否かを決定する。
具体的には、自己位置推定部１４３は、環境地図Ｍ１及び／又はローカルマップＭ２が単調な形状を有する単調形状領域を移動体１が移動中であるか否かを判定する（ステップＳ６）。

例えば、ステップＳ２にて取得したローカルマップＭ２が特徴的な形状を有している場合（ステップＳ６において「Ｎｏ」の場合）、すなわち、環境地図Ｍ１が特徴的な形状を有する特徴形状領域を移動体１が移動していると判定された場合、自己位置推定部１４３は、第１推定モードの実行のみで、正確な自己位置及び／又は姿勢の推定ができると判定する。この場合、自己位置推定部１４３は、第１推定モードのみを実行して、移動体１の自己位置及び／又は姿勢を推定する（ステップＳ７）。

具体的には、まず、自己位置推定部１４３は、環境地図Ｍ１上に、ローカルマップ配置領域Ａを設定する。例えば、ローカルマップ配置領域Ａを一辺の長さがＬである正方形と定義した場合に、主輪１２３ａ、１２３ｂの回転量に基づいて、ローカルマップＭ２を配置すべき位置が（Ｘ１，Ｙ１）と算出されていると、自己位置推定部１４３は、移動環境ＭＥを表す座標平面において、−Ｌ／２＋Ｘ１≦Ｘ≦Ｌ／２＋Ｘ１、及び、−Ｌ／２＋Ｙ１≦Ｙ≦Ｌ／２＋Ｙ１との２つの不等式を定義して、環境地図Ｍ１上にローカルマップ配置領域Ａを設定できる。
その他、例えば、ローカルマップ配置領域Ａを半径がＲである円と定めた場合には、当該座標平面において、（Ｘ−Ｘ１）²＋（Ｙ−Ｙ１）²≦Ｒ²との不等式を定義することにより、環境地図Ｍ１上にローカルマップ配置領域Ａを設定できる。

また、例えば、ローカルマップＭ２の回転角度幅（姿勢についてのローカルマップ配置領域Ａの大きさ）をΔθと定義した場合に、主輪１２３ａ、１２３ｂの回転量に基づいて、ローカルマップＭ２の回転角度がθ１と推定されていると、自己位置推定部１４３は、当該座標平面において、−Δθ／２＋θ１≦θ≦Δθ／２＋θ１との不等式を定義することにより、環境地図Ｍ１上に姿勢についてのローカルマップ配置領域Ａを設定できる。この場合、自己位置推定部１４３は、−Δθ／２＋θ１≦θ≦Δθ／２＋θ１との不等式で示される角度範囲θにて、ローカルマップＭ２を回転できる。

上記のようにして環境地図Ｍ１上にローカルマップ配置領域Ａを設定後、自己位置推定部１４３は、環境地図Ｍ１に設定したローカルマップ配置領域Ａ内の複数の配置位置にローカルマップＭ２を配置し、また、当該複数の配置位置のそれぞれにおいて、ローカルマップ配置領域Ａに示された範囲内の複数の回転角度にてローカルマップＭ２を回転する。
ローカルマップＭ２はローカルマップ取得部１４２により取得されたものであるので、ローカルマップＭ２も、環境地図Ｍ１と同様に、移動環境ＭＥを表す座標平面上の障害物の位置を示す座標値データの集合体である。
従って、ローカルマップＭ２の環境地図Ｍ１上のローカルマップ配置領域Ａへの配置は、例えば、座標値データであるローカルマップＭ２を配置位置へ平行移動させる行列を用いた演算により達成できる。また、ローカルマップＭ２の配置位置における回転は、座標値データであるローカルマップＭ２を回転させる行列を用いた演算により達成できる。

その後、自己位置推定部１４３は、上記の複数の配置位置及び複数の回転角度のうち、ローカルマップＭ２が環境地図Ｍ１と最も一致（マッチング）する配置位置及び回転角度を、それぞれ、第１自己位置及び第１姿勢と推定する。このようなマップマッチングによる自己位置及び／姿勢の推定は、例えば、「尤度計算」により実行できる。
自己位置推定部１４３は、上記のようにして推定した第１自己位置を、移動体１の移動環境ＭＥにおける自己位置であると推定する。また、上記の第１姿勢を、当該自己位置（第１自己位置）における移動体１の姿勢であると推定する。

一方、例えば、ステップＳ２にて取得したローカルマップＭ２が単調な形状を有している場合（ステップＳ６において「Ｎｏ」の場合）、すなわち、環境地図Ｍ１が単調な形状を有する単調形状領域を移動体１が移動していると判定された場合、自己位置推定部１４３は、第１推定モードの実行のみでは、正確な自己位置及び／又は姿勢の推定ができないと判定する。
この場合、自己位置推定部１４３は、第１推定モードと第２推定モードとの両方を実行して、移動体１の自己位置及び／又は姿勢を推定する（ステップＳ８）。単調形状領域を移動体１が移動中、自己位置推定部１４３は、第２推定モードを用いて移動体１の自己位置を推定し、第１推定モードを用いて移動体の姿勢を推定する。

具体的には、自己位置推定部１４３は、まず、主輪１２３ａ、１２３ｂの回転量に基づいて第２自己位置を推定し（第２推定モードの実行）、当該第２自己位置を、移動環境ＭＥにおける移動体１の自己位置と推定する。
次に、自己位置推定部１４３は、第２推定モードにて推定された第２自己位置に、上記のようにしてローカルマップ配置領域Ａを設定する。その後、自己位置推定部１４３は、第１推定モードを実行して、ローカルマップ配置領域Ａに定められた回転角度の範囲内でローカルマップＭ２を回転し、環境地図Ｍ１とローカルマップＭ２とが最も一致したときの回転角度である第１姿勢を、上記の自己位置における移動体１の姿勢と推定する。

上記のように、移動体１が単調形状領域を移動中に、自己位置推定部１４３が、第１推定モード及び第２推定モードを実行して移動体１の自己位置及び／又は姿勢を推定することにより、ローカルマップＭ２や環境地図Ｍ１の形状が単純でも、移動体１の移動方向などによっては、マップマッチングでも正確な自己位置及び／又は姿勢が推定できる場合には、第１推定モードにて第１自己位置及び／又は第１姿勢を正確に推定できる。また、マップマッチングにより自己位置及び／又は姿勢が推定できない場合には、第２推定モードにて自己位置及び／又は姿勢を推定できる。

なお、単調形状領域を移動体１が移動中、ローカルマップ配置領域Ａの大きさは、通常の第１推定モードの実行時の大きさとして予め決められた大きさに設定されている。これにより、第１推定モードによる第１自己位置及び／又は第１姿勢の推定に要する演算量が無駄に増大することを回避して、当該推定にかかる時間を減少できる。

例えば、図５Ａに示すように、環境地図Ｍ１が２つの壁により形成された直線的な経路を示す単調形状領域を、移動体１が当該直線的な経路の延長方向に沿って移動している場合には、マップマッチング（第１推定モード）により、当該延長方向に沿った方向の移動体１の自己位置を正確には推定できない。なぜなら、図５Ａに示すように、このような領域において取得されるローカルマップＭ２は、経路の延長方向のどこに配置しても、環境地図Ｍ１とマッチングしてしまうからである。このような場合には、自己位置推定部１４３は、主輪１２３ａ、１２３ｂの回転量に基づいて、すなわち、第２推定モードを実行して、移動体１の自己位置を推定する。図５Ａは、マップマッチングによって自己位置を推定できない場合の一例を示す図である。
なお、ローカルマップＭ２が、直線的な経路の幅方向（経路の延長方向に垂直な方向）において環境地図Ｍ１とずれている場合には、第１推定モードの実行により、直線的な経路の幅方向における移動体１の自己位置は推定できる。

一方、例えば、図５Ｂの左図に示すようなローカルマップＭ２が取得された場合に、当該ローカルマップＭ２を、主輪１２３ａ、１２３ｂの回転量に基づいて推定された環境地図Ｍ１上の自己位置に設定後、当該自己位置を中心としてローカルマップＭ２を環境地図Ｍ１と一致するまで回転すると、図５Ｂの右図に示すように、当該回転角度を移動体１の姿勢として一義的に推定できる。図５Ｂは、マップマッチングによって姿勢を推定できる場合の一例を示す図である。

上記のようにして、移動体１が移動中の領域毎に第１推定モード及び／又は第２推定モードを適宜選択して、移動体１の自己位置及び／又は姿勢を推定後、走行制御部１４５が、当該推定された自己位置及び／又は姿勢に基づいて、モータ１２１ａ、１２１ｂを制御する（ステップＳ９）。
具体的には、走行制御部１４５は、まず、走行スケジュールＴＳにおいて、上記にて推定された自己位置及び／又は姿勢に最も近い位置及び／又は姿勢の次の位置及び／又は姿勢を、移動体１が移動すべき目標位置及び／又は目標姿勢と設定する。
次に、走行制御部１４５は、当該目標位置及び／又は目標姿勢と、上記にて推定された自己位置及び／又は姿勢との差に基づいて、モータ１２１ａ、１２１ｂのそれぞれの制御量を算出して、算出された制御量に基づいた駆動電力を、これらのモータに出力する。これにより、移動体１は、上記の目標位置及び／又は目標姿勢に向かって移動できる。

走行制御部１４５が上記のようにモータ１２１ａ、１２１ｂを制御後、走行スケジュールＴＳに示された最終の目標位置に到達していないか、又は、移動体１の移動がユーザなどにより停止されていない場合（ステップＳ１０において「Ｎｏ」の場合）、プロセスはステップＳ２に戻る。
一方、走行スケジュールＴＳに示された最終の目標位置に到達したか、又は、移動体１の移動がユーザなどにより停止された場合（ステップＳ１０において「Ｙｅｓ」の場合）、制御部１４は、移動体１の移動を停止する。

上記のステップＳ１〜Ｓ１０を実行することにより、自己位置推定部１４３は、マップマッチングにより移動体１の自己位置及び／又は姿勢が推定できる場合には、第１推定モードを実行して推定される第１自己位置及び／又は第１姿勢を、移動体１の自己位置及び／又は姿勢として精度よく推定できる。
その一方、マップマッチングにより移動体１の自己位置及び／又は姿勢が推定できない場合には、第２推定モードを実行して推定される第２自己位置及び／又は第２姿勢を、移動体１の自己位置及び／又は姿勢として推定できる。
その結果、走行制御部１４５は、移動体１がどの領域を移動している場合でも、移動体１の自己位置及び／又は姿勢の推定結果を入力して、当該推定結果に基づいて、モータ１２１ａ、１２１ｂ（主輪１２３ａ、１２３ｂ）を制御できる。

（４）ローカルマップ配置領域の調節方法
次に、上記のステップＳ５において実行される、ローカルマップ配置領域Ａの調節方法について、図６を用いて説明する。図６は、ローカルマップ配置領域の調節方法を示すフローチャートである。
本実施形態において、調節部１４４は、ローカルマップ配置領域Ａを調節すべき所定の条件に従って、ローカルマップ配置領域Ａの大きさを調節する。具体的には、調節部１４４は、（ｉ）ローカルマップ配置領域Ａの大きさが属性情報ＡＩにより指定された場合、（ｉｉ）第２推定モードが停止されて第１推定モードのみの推定が開始された場合、（ｉｉｉ）一時停止から移動を開始した場合、（ｉｖ）障害物により移動体１が囲まれた場合、及び（ｖ）移動体１が一時停止中、との５つの条件が発生した時に、ローカルマップ配置領域Ａの大きさを調節する。

従って、調節部１４４は、まず、移動体１の現在の自己位置（及び／又は姿勢）において設定されている属性情報ＡＩが記憶部１４１に記憶されているか否かを判定する（ステップＳ５０１）。
現在の自己位置にて設定された属性情報ＡＩが存在する場合（ステップＳ５０１において「Ｙｅｓ」の場合）、調節部１４４は、ローカルマップ配置領域Ａの大きさを、属性情報ＡＩに示された大きさに調節し（ステップＳ５０２）、プロセスを終了する。

上記のように、調節部１４４が、属性情報ＡＩに従って、ローカルマップ配置領域Ａの大きさを設定することにより、環境地図Ｍ１（移動環境ＭＥ）の各位置における状況（例えば、通路が（下り）坂になっているとの状況、又は、移動面が滑りやすい状況など）に応じて、ローカルマップ配置領域Ａの大きさを調節できる。

その結果、例えば、移動体１が推定誤差を生じやすい領域を移動中には、ローカルマップ配置領域Ａを拡大して当該推定誤差を素早く解消できる。その一方、誤推定が発生しやすい領域を移動中には、ローカルマップ配置領域Ａを縮小して、誤推定を発生する位置にローカルマップＭ２が配置されたり、誤推定を発生する角度にてローカルマップＭ２が回転されたりすることを回避できる。

一方、移動体１の現在の自己位置及び／又は姿勢において属性情報ＡＩが設定されていない場合（ステップＳ５０１において「Ｎｏ」の場合）、調節部１４４は、第２推定モードが停止されて第１推定モードのみの推定が開始されたか否かを判定する（ステップＳ５０３）。すなわち、調節部１４４は、第２推定モードにて累積された自己位置及び／又は姿勢（第２自己位置及び／又は第２姿勢）の推定誤差を、第１推定モードの実行により解消するか否かを決定する。

図３のフローチャートに示されているように、第２推定モードが停止され、第１推定モードのみにて自己位置及び／又は姿勢の推定が開始されるタイミングとしては、（Ｉ）移動体１が無障害領域から障害領域（特徴形状領域）へと移動したタイミングと、（ＩＩ）移動体１が単調形状領域から特徴形状領域へと移動したタイミングと、が存在する。

従って、移動体１の現在の自己位置及び／又は姿勢から、移動体１が、特徴形状領域と無障害領域との境界近傍、又は、特徴形状領域と単調形状領域との境界近傍から、当該特徴形状領域中に向かって移動中であることが示されている場合（ステップＳ５０３において「Ｙｅｓ」の場合）、調節部１４４は、第２推定モードの実行により累積された推定誤差を解消するために、ローカルマップ配置領域Ａを拡大する（ステップＳ５０４）。

ローカルマップ配置領域Ａを拡大する場合の大きさは、例えば、第２推定モードによる第２自己位置及び／又は第２姿勢の推定を継続した時間が長くなればなるほど、大きくするようにする。なぜなら、主輪１２３ａ、１２３ｂの回転量に基づく第２自己位置及び／又は第２姿勢の推定誤差は、第２推定モードの実行時間が長くなればなるほど累積されて大きくなるからである。

ローカルマップ配置領域Ａを拡大後、調節部１４４は、ローカルマップ配置領域Ａの拡大を開始してから、所定の時間が経過したか否かを判定する（ステップＳ５０５）。 ローカルマップ配置領域Ａを拡大する所定の時間は、例えば、第２推定モードによる第２自己位置及び／又は第２姿勢の推定を継続した時間が長くなればなるほど、長くする。
ローカルマップ配置領域Ａの拡大を開始してから所定の時間が経過していない場合（ステップＳ５０５において「Ｎｏ」の場合）、調節部１４４は、ローカルマップ配置領域Ａを拡大したマップマッチングによる第１自己位置及び／又は第１領域の推定が、累積された推定誤差を解消できるだけ実行されていないと判定する。この場合、調節部１４４は、ローカルマップ配置領域Ａを拡大したまま保持する。

一方、ローカルマップ配置領域Ａの拡大を開始してから所定の時間が経過した場合（ステップＳ５０５において「Ｙｅｓ」の場合）、調節部１４４は、第２推定モードの実行により累積された自己位置及び／又は姿勢の推定誤差が解消されたと判定し、ローカルマップ配置領域Ａの大きさを、予め決められた大きさ（例えば、推定誤差を解消しない通常の第１推定モードにおける大きさ）に戻す（ステップＳ５０６）。

上記のように、第２推定モードの停止後に、ローカルマップ配置領域Ａを拡大した状態にて第１推定モードを実行することにより、自己位置推定部１４３は、移動体１が移動中であっても、第２推定モードの実行により累積された推定誤差を解消しつつ、第１自己位置及び第１姿勢を推定できる。なぜなら、ローカルマップ配置領域Ａを拡大することにより、自己位置推定部１４３は、移動体１の移動能力にて定められる範囲を超えてローカルマップＭ２を環境地図Ｍ１上に配置できるからである。

また、ローカルマップ配置領域Ａを拡大した第１推定モードを所定の時間だけ実行後に、ローカルマップ配置領域Ａの大きさを予め決められた大きさに戻す（縮小する）ことにより、累積された推定誤差を解消後に、第１自己位置及び／又は第１姿勢の推定に要する演算量が無駄に増大することを回避できる。その結果、第１自己位置及び／又は第１姿勢の推定にかかる時間を減少できる。

移動体１が特徴形状領域を継続的に移動しているか、又は、上記のステップＳ５０４〜Ｓ５０６を実行したことにより、第２推定モードの実行により累積された推定誤差が存在しない場合（ステップＳ５０３において「Ｙｅｓ」の場合）、調節部１４４は、移動体１が一時停止した後に移動を開始したか否かを判定する（ステップＳ５０７）。
例えば、調節部１４４は、エンコーダ１２５ａ、１２５ｂから出力されるパルス数が、ほぼ０から急激に上昇した場合に、移動体１が一時停止後に移動を開始したと判定できる。

移動体１が一時停止後に移動を開始した場合（ステップＳ５０７において「Ｙｅｓ」の場合）、調節部１４４は、ローカルマップ配置領域Ａを所定の時間だけ拡大する（ステップＳ５０４〜Ｓ５０６）。
移動体１が一時停止状態から移動を開始したときに、ローカルマップ配置領域Ａの大きさを、通常の第１推定モードの実行時よりも拡大することにより、自己位置推定部１４３は、一時停止状態から移動を開始したときに発生する自己位置及び／又は姿勢の推定誤差（主に、主輪１２３ａ、１２３ｂと移動面との間の滑りにより発生する）を、ローカルマップ配置領域Ａを拡大した第１推定モードの実行により解消して、移動体の自己位置及び／又は姿勢の推定精度を維持できる。

一方、移動体１が一時停止した後に移動を開始した状態でないと判定された場合（ステップＳ５０７において「Ｎｏ」の場合）、調節部１４４は、移動体１が環境地図Ｍ１に表されていない未知の障害物（障害物）により囲まれているか否かを判定する（ステップＳ５０８）。移動体１が未知の障害物（障害物）により囲まれているか否かは、具体的には、以下のようにして判定する。
ここで、図７Ａに示すように、移動環境ＭＥにおいて、壁Ｗ１に囲まれた領域に１つの障害物Ｏ１が存在する領域に、移動体１が存在する場合を考える。また、環境地図Ｍ１は、図７Ａに示す、壁Ｗ１に囲まれた領域に１つの障害物Ｏ１が存在する状態の時に生成されたものとする。
その後、移動体１が、上記の領域に自律的に移動してきたときに、図７Ｂに示すように、障害物Ｏ１の近傍に複数の障害物Ｏ２〜Ｏ７が存在したとする。図７Ａ及び図７Ｂは、囲われ判定が実行される場合の一例を示す図である。

上記の場合、図７Ａに示す状態のときに取得される環境地図Ｍ１は、図８Ａに示すように、壁Ｗ１に対応する像と、障害物Ｏ１に対応する像とが含まれるものとなる。一方、移動体１が自律的に上記の領域に移動し、当該領域に図７Ｂに示すような未知の障害物Ｏ２〜Ｏ７が存在するようになった場合に得られるローカルマップＭ２は、例えば、図８Ｂに示すようなものとなる。図８Ｂに示すローカルマップＭ２では、壁Ｗ１及び障害物Ｏ１に対応する像に加えて、未知の障害物Ｏ２〜Ｏ７に対応する像が含まれる。
図８Ａは、未知の障害物が存在しない場合に取得された環境地図の一例を示す図である。図８Ｂは、未知の障害物が存在する場合に取得されたローカルマップの一例を示す図である。

図８Ａに示す環境地図Ｍ１（図９においては、薄い太実線にて示した像）と図８Ｂに示すローカルマップＭ２（図９においては、黒細線にて示した像）とを、図９に示すようにマップマッチングしたときに、例えば、移動体１を中心とした所定の半径を有する半円形状である囲われ判定領域ＳＡ内に、環境地図Ｍ１には現れていない像が所定の数以上存在する場合（図９に示す例では、４つ）に、調節部１４４は、移動体１が未知の障害物（障害物）Ｏ２〜Ｏ７に囲まれていると判定する。図９は、囲われの判定方法の一例を示す図である。

上記の判定方法を実行した結果、移動体１が未知の障害物（障害物）により囲まれていると判定された場合（ステップＳ５０８において「Ｙｅｓ」の場合）、調節部１４４は、ローカルマップ配置領域Ａの大きさを、未知の障害物に囲まれていないとき（例えば、通常の第１推定モードの実行時における、予め決められた大きさ）よりも縮小する（ステップＳ５０９）。
例えば、調節部１４４は、ローカルマップ配置領域Ａの大きさを、（予め決められた大きさ）−（囲まれ度＊０．５）との関係式により算出される大きさに縮小する。ここで、「囲まれ度」は、例えば、囲われ判定時に囲われ判定領域ＳＡ内に含まれると判断された、未知の障害物に対応する像の数に基づいて決定できる。

移動体１が自律的に移動中に環境地図Ｍ１に現れていない未知の障害物に移動体１が囲まれて、図８Ａに示す環境地図Ｍ１と、図８Ｂに示すローカルマップＭ２とが取得された場合、例えば、図１０に示すように、未知の障害物に対応する像が、環境地図Ｍ１の壁として誤推定されて、移動体１の自己位置及び／又は姿勢が誤推定される可能性がある。図１０は、未知の障害物に囲まれたために自己位置及び／又は姿勢が誤推定された場合の一例を示す図である。

このような場合に、調節部１４４が、ローカルマップ配置領域Ａの大きさを調節して、例えば、図１０において一点鎖線にて示すような縮小したローカルマップ配置領域Ａを設定することにより、ローカルマップＭ２が、環境地図Ｍ１において、図１０に示すような誤推定を発生させる位置に配置されることを回避できる。すなわち、未知の障害物の存在により、環境地図Ｍ１とローカルマップＭ２とのマップマッチングが不適切に行われて、第１自己位置及び／又は第１姿勢の誤推定が発生することを回避できる。

なお、上記のステップＳ５０８において移動体１が未知の障害物により囲まれていると判定された場合に、走行制御部１４５は、モータ１２１ａ、１２１ｂへの電力供給を停止し、主輪１２３ａ、１２３ｂの回転を停止することで、移動体１を停止させてもよい。このように移動体１を停止させる場合、調節部１４４は、ステップＳ５０９において、ローカルマップ配置領域Ａの大きさを０としてもよい。
ローカルマップ配置領域Ａの大きさを０とすることにより、第１自己位置及び／又は第１姿勢の誤推定を回避できる。

一方、移動体１が未知の障害物（障害物）により囲まれていないと判定された場合（ステップＳ５０８において「Ｎｏ」の場合）、調節部１４４は、移動体１が一時停止中であるか否かを判定する（ステップＳ５１０）。例えば、調節部１４４は、エンコーダ１２５ａ、１２５ｂから出力されるパルス数が（ほぼ）０である場合に、移動体１が一時停止中であると判定できる。

移動体１が一時停止中であると判定された場合（ステップＳ５１０において「Ｙｅｓ」の場合）、調節部１４４は、ローカルマップ配置領域Ａの大きさを、例えば、０か０に近い大きさとして、移動体１が移動中であるときよりも縮小する（ステップＳ５０９）。
これにより、自己位置及び／又は姿勢にほとんど変化がない場合に、小さいローカルマップ配置領域Ａを用いて、ローカルマップＭ２を必要以上に移動させることなく、第１自己位置及び／又は第１姿勢の推定に必要な演算量を減少できる。

一方、上記のステップＳ５０１〜Ｓ５１０の実行を試みた結果、現在、ローカルマップ配置領域Ａの大きさを調節すべき所定の条件がいずれも発生していないと判定された場合（ステップＳ５１０において「Ｎｏ」の場合）、調節部１４４は、ローカルマップ配置領域Ａの大きさを、予め決められた大きさ（通常の第１推定モードの実行時における大きさ）とする（ステップＳ５１１）。
これにより、ローカルマップ配置領域Ａの大きさを調節すべき上記の条件がいずれも発生していない場合に、調節部１４４は、ローカルマップ配置領域Ａの大きさを、予め決められた大きさとできる。

上記のステップＳ５０１〜Ｓ５１１を実行することにより、調節部１４４は、ローカルマップ配置領域Ａの大きさを調節すべき所定の条件に従って、ローカルマップ配置領域Ａの大きさを適切な大きさに調節できる。これにより、移動体１の自己位置及び／又は姿勢の推定精度を維持しつつ、マップマッチングによるこれらの推定に要する演算量が無駄に増大することを回避して、これらの推定にかかる時間を減少できる。

また、上記のステップＳ５０１〜Ｓ５１１を実行することにより、調節部１４４は、（ｉ）ローカルマップ配置領域Ａの大きさが属性情報により指定された場合、（ｉｉ）第２推定モードが停止されて第１推定モードのみの推定が開始された場合、（ｉｉｉ）一時停止から移動を開始した場合、（ｉｖ）障害物により移動体１が囲まれた場合、及び（ｖ）移動体１が停止中、との順番で優先的にローカルマップ配置領域Ａの大きさを調節できる。
すなわち、図６に示すように、調節部１４４が、上記の（ｉ）〜（ｖ）の条件を順番に判定することにより、これらの条件が同時に複数発生しても、より優先度の高い条件に従って、ローカルマップ配置領域Ａの大きさが調節される。

例えば、属性情報ＡＩが設定された位置において移動体１が一時停止している場合には、図６のフローチャートがステップＳ５０１において「Ｙｅｓ」となり、その結果ステップＳ５０２が実行されて、ローカルマップ配置領域Ａの大きさを調節するプロセスが終了している。つまり、属性情報ＡＩが設定された位置に移動体１が存在すると判定された場合には、属性情報ＡＩに従ってローカルマップ配置領域Ａの大きさが調節された後に、移動体１が一時停止しているか否かの判定が実行されていない。

なお、図６に示すローカルマップ配置領域の大きさの調節処理において、上記の（ｉ）〜（ｖ）の判定の順番を適宜変更することにより、各条件の優先度を任意に決定できる。

２．他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
（Ａ）囲われ検知の他の実施形態
上記の第１実施形態において、ステップＳ５０８において実行される囲まれ検知は、１つの囲まれ判定領域ＳＡのみを用いて実行されていた。しかし、これに限られず、ステップＳ５０８において、調節部１４４は、複数の判定領域を用いて囲まれ検知を実行してもよい。例えば、図１１に示すように、所定の中心角を有する扇形の複数の囲まれ判定領域ＳＡ１〜ＳＡ６を用いて、各囲まれ判定領域にいくつ未知の障害物を示す像が含まれているかを判定してもよい。図１１は、囲まれ検知の他の実施形態を示す図である。

図１１に示すような複数の囲まれ判定領域ＳＡ１〜ＳＡ６を用いて囲まれ判定を実行することにより、移動体１の移動方向毎に、囲まれ判定を実行できる。移動体１の移動方向毎に囲まれ検知を実行することにより、例えば、特定の方向に向けて特定の動作を移動体１にさせることができる。
例えば、移動体１が広告宣伝ロボットである場合には、未知の障害物が最も多く含まれる囲まれ判定領域が存在する方向に向けて、宣伝広告を集中的に実行してもよい。
その他、例えば、障害物の像が全く含まれない囲まれ判定領域が存在する場合には、当該方向に移動体１を移動させてもよい。

（Ｂ）環境地図についての他の実施形態
上記の第１実施形態において、環境地図Ｍ１は移動環境ＭＥ全体を表す１つの地図情報としていた。しかし、環境地図Ｍ１は移動環境ＭＥを表していればよいので、環境地図Ｍ１を１つの地図情報としなくてもよい。
例えば、環境地図Ｍ１を複数の副環境地図に分割してもよい。この場合、副環境地図が移動環境ＭＥのどの位置の地図情報であるかを判別するために、例えば、副環境地図を取得した時刻を、当該副環境地図に関連付けてもよい。
このように、環境地図Ｍ１を複数の副環境地図に分割することにより、移動環境ＭＥが環状である場合に、移動体１の移動開始位置にて取得したローカルマップを用いて作成した環境地図の部分と、環状の移動環境ＭＥを移動開始位置の近傍まで戻ってきたときに取得したローカルマップを用いて作成した環境地図の部分とが、一致しなくなるという問題が発生しにくくなる。

（Ｃ）ローカルマップ配置領域の他の実施形態
上記の第１実施形態において、ローカルマップ配置領域Ａは、主輪１２３ａ、１２３ｂの回転量に基づいて推定される位置及び／又は姿勢の推定誤差を考慮した範囲として定義されていた。しかし、ローカルマップ配置領域Ａは、これに限られない。
主輪１２３ａ、１２３ｂの回転量に基づいて推定される自己位置及び／又は姿勢には、推定誤差が含まれるものの、あまりにも大きな推定誤差が発生する可能性は低い。また、主輪１２３ａ、１２３ｂの回転量に基づいて推定された自己位置及び／又は姿勢は、実際の移動体１の自己位置及び／又は姿勢に近い傾向にある。

従って、ローカルマップ配置領域Ａを、例えば、主輪１２３ａ、１２３ｂの回転量に基づいて推定される位置及び／又は姿勢の推定誤差の発生確率に基づいて定義してもよい。例えば、ローカルマップ配置領域Ａを、ローカルマップ配置領域Ａの中心（主輪１２３ａ、１２３ｂの回転量に基づいて推定された位置及び／姿勢）を平均とした正規分布関数（確率密度関数）を用いて定義してもよい。
この場合、調節部１４４は、例えば、ローカルマップ配置領域Ａの大きさを、当該正規分布関数の標準偏差の大きさを調節することにより調節できる。

ローカルマップ配置領域Ａを正規分布関数（確率密度関数）により定義した場合には、自己位置推定部１４３は、例えば、環境地図Ｍ１とローカルマップＭ２との一致度と、ローカルマップ配置領域Ａを表す正規分布関数の値（ローカルマップ配置領域Ａの中心から離れるほど小さくなる値）との積が最大となるローカルマップＭ２の配置位置及び／又は回転角度を、それぞれ、第１推定位置及び／又は第１姿勢と推定できる。

本発明は、移動環境を移動する移動体の自己位置及び／又はその姿勢を推定する装置に広く適用できる。

１００ 自己位置推定装置
１ 移動体
１１ 本体
１２ 移動部
１２１ａ、１２１ｂ モータ
１２３ａ、１２３ｂ 主輪
１２５ａ、１２５ｂ エンコーダ
１３ レーザレンジセンサ
１３１ 第１レーザレンジセンサ
１３３ 第２レーザレンジセンサ
１４ 制御部
１４１ 記憶部
１４２ ローカルマップ取得部
１４３ 自己位置推定部
１４４ 調節部
１４５ 走行制御部
１５ 補助輪部
１５ａ、１５ｂ 補助車輪
Ａ ローカルマップ配置領域
ＡＩ 属性情報
Ｍ１ 環境地図
Ｍ２ ローカルマップ
ＭＥ 移動環境
Ｏ１〜Ｏ７ 障害物
ＳＡ 判定領域

Claims (12)

1. 移動環境における移動体の自己位置及び／又は姿勢を推定する自己位置推定装置であって、
   前記移動環境に存在する障害物の位置を表す環境地図を記憶する記憶部と、
   前記移動体の周囲に存在する前記障害物の前記移動体に対する相対的な位置を表すローカルマップを取得するローカルマップ取得部と、
   前記環境地図に設定されるローカルマップ配置領域内の複数の配置位置にて前記ローカルマップを配置及び／又は回転したときに、前記ローカルマップが前記環境地図と最も一致する前記配置位置を、前記移動体の前記移動環境における前記自己位置である第１自己位置と推定し、及び／又は、当該配置位置における前記ローカルマップの回転角を、前記第１自己位置における前記移動体の第１姿勢と推定する、第１推定モードを実行する自己位置推定部と、
   所定の条件に従って前記ローカルマップ配置領域の大きさを調節する調節部と、
   を備える、自己位置推定装置。
2. 前記移動環境において、前記障害物が少数しか存在しない、又は、全く存在しない無障害領域から、前記障害物が存在する障害領域へと前記移動体が移動したら、前記調節部は、前記ローカルマップ配置領域の大きさを、所定の時間、予め決められた大きさよりも拡大する、
   請求項１に記載の自己位置推定装置。
3. 前記無障害領域を前記移動体が移動中に、前記自己位置推定部は、前記第１推定モードを実行しない、請求項２に記載の自己位置推定装置。
4. 前記環境地図が単調な形状を有する単調形状領域から、前記環境地図が特徴的な形状を有する特徴形状領域へと前記移動体が移動したら、前記調節部は、前記ローカルマップ配置領域の大きさを、所定の時間、予め決められた大きさよりも拡大する、
   請求項１〜３のいずれかに記載の自己位置推定装置。
5. 前記単調形状領域を前記移動体が移動中、又は、前記所定の時間の経過後、前記調節部は、前記ローカルマップ配置領域の大きさを、前記予め決められた大きさとする、請求項４に記載の自己位置推定装置。
6. 前記移動体は前記移動環境を移動するための主輪を有し、
   前記自己位置推定部は、前記主輪の回転量に基づいて、前記移動体の前記移動環境における自己位置及び／又は姿勢を、それぞれ、第２自己位置及び／又は第２姿勢と推定する第２推定モードをさらに実行可能であり、
   少なくとも前記第２推定モードを実行している状態から、前記第１推定モードのみを実行する状態へと切り替わってから所定の時間、前記調節部は、前記ローカルマップ配置領域を、通常の前記第１推定モードの実行時の大きさよりも拡大する、
   請求項１〜５のいずれかに記載の自己位置推定装置。
7. 前記移動体が一時停止状態から移動を開始したら、前記調節部は、前記ローカルマップ配置領域の大きさを、通常の前記第１推定モードの実行時よりも拡大する、請求項１〜６のいずれかに記載の自己位置推定装置。
8. 前記移動体が一時停止したら、前記調節部は、前記ローカルマップ配置領域の大きさを、通常の前記第１推定モードの実行時よりも縮小する、請求項１〜７のいずれかに記載の自己位置推定装置。
9. 前記移動体が、前記環境地図に表されていない未知の障害物に囲まれていたら、前記調節部は、前記ローカルマップ配置領域の大きさを、前記未知の障害物に囲まれていないときよりも縮小する、請求項１〜８のいずれかに記載の自己位置推定装置。
10. 前記記憶部は、前記環境地図の所定の位置における前記ローカルマップ配置領域の大きさを定める属性情報をさらに記憶し、
    前記調節部は、前記属性情報に従って、前記ローカルマップ配置領域の大きさを設定する、請求項１〜９のいずれかに記載の自己位置推定装置。
11. 前記ローカルマップ配置領域の大きさを調節すべき前記所定の条件が同時に複数発生したら、前記調節部は、当該複数の条件のうち、優先度がより高い条件に従って前記ローカルマップ配置領域の大きさを設定する、請求項１〜１０のいずれかに記載の自己位置推定装置。
12. 移動環境における移動体の自己位置及び／又は姿勢を推定する自己位置推定方法であって、
    前記移動環境に存在する障害物の位置を表す環境地図を生成するステップと、
    前記移動体の周囲に存在する前記障害物の前記移動体に対する相対的な位置を表すローカルマップを取得するステップと、
    前記環境地図に設定され、所定の条件に従って大きさが調節されたローカルマップ配置領域内の複数の配置位置にて前記ローカルマップを配置及び／又は回転したときに、前記ローカルマップが前記環境地図と最も一致する前記配置位置を、前記移動体の前記移動環境における前記自己位置と推定し、及び／又は、当該配置位置における前記ローカルマップの回転角を、当該自己位置における前記移動体の姿勢と推定するステップと、
    を含む、自己位置推定方法。

Images