特許 6435781 自己位置推定装置及び自己位置推定装置を備えた移動体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6435781

(24)【登録日】2018年11月22日

(45)【発行日】2018年12月12日

(54)【発明の名称】自己位置推定装置及び自己位置推定装置を備えた移動体

(51)【国際特許分類】

   G05D 1/02 20060101AFI20181203BHJP

【ＦＩ】

   G05D1/02 L

【請求項の数】4

【全頁数】13

(21)【出願番号】特願2014-223207(P2014-223207)

(22)【出願日】2014年10月31日

(65)【公開番号】特開2016-91202(P2016-91202A)

(43)【公開日】2016年5月23日

【審査請求日】2017年6月12日

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】110000110

【氏名又は名称】特許業務法人快友国際特許事務所

(72)【発明者】

【氏名】伊藤 誠悟

【審査官】 山村 秀政

(56)【参考文献】

【文献】 特開２０１０−０８６４１６（ＪＰ，Ａ）

【文献】 特開２０１３−２２５３３６（ＪＰ，Ａ）

【文献】 特開２０１０−１６０７７７（ＪＰ，Ａ）

【文献】 特開２０１４−２０３３４４（ＪＰ，Ａ）

【文献】 国際公開第２０１２／０４６４２６（ＷＯ，Ａ１）

【文献】 国際公開第２０１１／１０１９４５（ＷＯ，Ａ１）

【文献】 特表２０１１−５０１１３４（ＪＰ，Ａ）

【文献】 特開昭６３−３０７３７９（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｄ １／０２

(57)【特許請求の範囲】

【請求項1】

特定の移動領域内を移動する移動体の自己位置を推定する装置であって、
前記移動体に設置されており、予め設定された走査角度範囲にレーザ光を照射すると共に、前記走査角度範囲内に存在している物体から反射される光を受光し、前記移動体から前記物体までの距離及び前記移動体に対する前記物体の方位を計測する距離センサと、
前記距離センサが前記物体から反射される光を受光するときに、前記距離センサで受光される光の輝度を計測する輝度センサと、
前記移動領域内に存在している物体の位置及び当該物体から反射される光の輝度を記憶している環境地図記憶部と、
前記距離センサで計測される計測結果と、前記輝度センサで計測される輝度データと、前記環境地図記憶部に記憶されている環境地図を用いて、前記移動体の位置を推定する位置推定部と、を有し、
前記環境地図記憶部は、前記移動領域を分割して得られる複数の部分領域のそれぞれに対して、当該部分領域に存在する物体の位置を第１の多次元分布データとして記憶しており、
前記第１の多次元分布データは、前記部分領域内の物体が存在している位置を計測した複数の位置情報を、前記複数の位置情報の平均と分散に変換することによって表わされるデータである、
自己位置推定装置。

【請求項2】

前記部分領域は、前記移動領域を３次元のグリッドに分割して得られる領域である、請求項１に記載の自己位置推定装置。

【請求項3】

前記環境地図記憶部は、前記複数の部分領域のそれぞれに対して、当該部分領域に存在する物体から反射される光の輝度を第２の多次元分布データとして記憶しており、
前記第２の多次元分布データは、前記部分領域内の物体が存在している位置から反射される光の輝度を計測した複数の輝度情報を、前記複数の輝度情報の平均と分散に変換することによって表わされるデータである、請求項１又は２に記載の自己位置推定装置。

【請求項4】

請求項１〜３のいずれか一項に記載の自己位置推定装置を備える移動体。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書に関する技術は、移動体の自己位置を推定する技術に関する。

【背景技術】

【0002】

距離センサと環境地図（例えば、占有格子地図）を用いて移動体の位置を推定する技術が知られている。この技術では、まず、移動体が移動する移動領域内に存在している物体（例えば、床面、壁等）の位置を記述した環境地図が作成される。移動体には、距離センサが設置される。距離センサは、予め設定された走査角度範囲にレーザ光を照射する。レーザ光を照射した方向に物体が存在すると、照射されたレーザ光は物体で反射され、その反射光が距離センサで検出される。これによって、距離センサ（すなわち、移動体）から物体までの距離と、距離センサ（移動体）に対する物体の方位が計測される。移動体が移動領域内を移動するときは、距離センサによる計測結果と、事前に作成した環境地図とを用いて、移動体の位置を推定する。このような自己位置推定に係る技術が、非特許文献１に開示されている。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】“Monte Carlo Localization for Mobile Robots” Proc. of the IEEE International Conference on Robotics and Automation (ICRA) (1999)

【発明の概要】

【発明が解決しようとする課題】

【0004】

従来技術においては、距離センサによって移動体の周辺の物体（床面、壁等）を検出し、その検出結果から移動体周辺の構造的特徴を抽出し、その構造的特徴を環境地図と照合することで移動体の位置を推定している。このため、移動体の周辺の構造的変化が少ない場所においては、移動体の位置推定精度が低下するという問題があった。

【0005】

本明細書は、移動体の周辺の構造的変化が少ない場合であっても、移動体の位置推定精度を向上することができる技術を開示する。

【0006】

本明細書の自己位置推定装置は、特定の移動領域内を移動する移動体の自己位置を推定する。自己位置推定装置は、移動体に設置されており、予め設定された走査角度範囲にレーザ光を照射すると共に、走査角度範囲内に存在している物体から反射される光を受光し、移動体から物体までの距離及び移動体に対する物体の方位を計測する距離センサと、距離センサが物体から反射される光を受光するときに、距離センサで受光される光の輝度を計測する輝度センサと、移動領域内に存在している物体の位置情報及び当該物体から反射される光の輝度情報を記憶している環境地図記憶部と、距離センサで計測される計測結果と、輝度センサで計測される輝度データと、環境地図記憶部に記憶されている環境地図を用いて、移動体の位置を推定する位置推定部と、を有する。

【0007】

上記の位置推定装置では、移動領域内に存在している物体の位置情報と、その物体から反射される光の輝度情報とが環境地図記憶部に記憶されている。移動体の位置を推定するときは、距離センサによる計測と共に、距離センサで受光される反射光の輝度が計測される。そして、距離センサで計測される計測結果と、輝度センサで計測される輝度データと、環境地図記憶部に記憶されている環境地図を用いて、移動体の位置が推定される。ここで、距離センサからのレーザ光が物体で反射されると、その反射光の輝度は、物体の材質の変化によって変化する。したがって、移動体周辺の構造物の変化が少ない場合であっても、移動体周辺の構造物の材質が変化している場合は、その材質の変化を検出することができる。これによって、移動体周辺の構造的変化が少ない場所においても、移動体の位置を精度よく推定することができる。

【図面の簡単な説明】

【0008】

【図1】本実施例の移動体の全体構成を示すブロック図。

【図2】環境地図を作成するときの自己位置推定装置の動作を説明するためのフローチャート。

【図3】多次元正規分布データの一例。

【図4】距離センサの位置情報による地図と、その位置情報を多次元正規分布データに変換した後の地図とを比較して示す図。

【図5】輝度付多次元正規分布データを用いた環境地図の一例。

【図6】移動体が移動領域を移動する際の自己位置推定装置の自己位置推定処理の手順を示すフローチャート。

【図7】本実施例の位置推定精度と、比較例の位置推定精度を併せて示す図。

【図8】輝度情報が有用な環境の一例。

【図9】輝度情報が有用な環境の他の例。

【図10】輝度情報が有用な環境の他の例。

【図11】輝度情報が有用な環境の他の例。

【発明を実施するための最良の形態】

【0009】

以下に説明する実施例の主要な特徴を列記しておく。なお、以下に記載する技術要素は、それぞれ独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。

【0010】

（特徴１） 上記の自己位置推定装置においては、環境地図記憶部は、移動領域を分割して得られる複数の部分領域のそれぞれに対して、部分領域に存在する物体の位置情報を記憶していてもよい。この場合、物体の位置情報は、部分領域内に存在している物体の位置を確率的に記述した多次元分布データであることが好ましい。このような構成によると、部分領域内の物体の位置が多次元分布データを用いて確率的に記述されるため、部分領域を大きな領域としても、自己位置の推定精度が低下することを抑制することができる。すなわち、部分領域内が一の物体によって占有されていると仮定する環境地図の場合、部分領域内に物体が観測されると、その部分領域全体に物体が存在するとされてしまう。このため、部分領域を大きくすると（地図の解像度を低くすると）、その分だけ自己位置推定精度も低下する。一方、部分領域内の物体の位置を多次元分布データを用いて確率的に記述すると、部分領域を大きくしても、その部分領域の一部に物体が存在していると確率的に記述される。その結果、部分領域を大きくしても（地図の解像度を低くしても）、自己位置推定精度が低下することを抑制することができる。また、部分領域を大きくできるため、比較的に広い移動領域の環境地図を少ないデータで記述することができる。

【0011】

（特徴２） 上記の自己位置推定装置においては、部分領域は、移動領域を３次元のグリッドに分割して得られる領域であってもよい。３次元グリッドによる環境地図は、データ容量が大きくなる。このため、多次元分布データを用いて環境地図を作成することで、環境地図のデータ容量を効果的に少なくすることができる。

【0012】

（特徴３） 上記の自己位置推定装置においては、輝度情報は、部分領域内に存在している物体から反射される光の輝度を確率的に記述した多次元分布データであってもよい。輝度情報についても多次元分布データで記述することで、環境地図の容量が増大することを抑制することができる。

【0013】

なお、上記の自己位置推定装置は、移動領域内を移動する移動体（例えば、車輪型ロボット、歩行ロボット等）に装備することができる。移動体に自己位置推定装置を装備することで、移動体は自己の位置を精度よく推定でき、移動領域内を安全に自律移動することができる。

【0014】

（実施例）以下、本実施例に係る移動体１０について説明する。移動体１０は、左右に駆動輪を有する車輪駆動型の移動体であり、特定の移動領域内を自律的に走行する。移動体１０は、移動領域内を走行する際に、移動領域内における自己位置を推定し、その自己位置推定結果に基づいて目的地まで走行する。なお、厳密には、移動体１０は、自身の位置及び方位を推定するが、以下では、移動体１０の位置と方位をまとめて「自己位置」と称する。

【0015】

図１に示すように、移動体１０は、モータ１２ａ，１２ｂと、エンコーダ１４ａ，１４ｂと、環境地図記憶装置１６と、レーザレンジファインダ１８（以下、ＬＲＦ１８と称する）と、輝度センサ２０と、コンピュータ２２を備える。モータ１２ａは左車輪を駆動し、モータ１２ｂは右車輪を駆動する。移動体１０の左右の車輪はモータ１２ａ，１２ｂによって独立して駆動され、左右の車輪の回転速度を変えることで、移動体１０は進行方向を変えることができる。

【0016】

エンコーダ１４ａは、左車輪の回転角度を検出し、コンピュータ２２に出力する。エンコーダ１４ｂは、右車輪の回転角度を検出し、コンピュータ２２に出力する。コンピュータ２２は、エンコーダ１４ａ，１４ｂから入力される左右の車輪の回転角度に基づいてモータ１２ａ，１２ｂを駆動することで、移動体１０を所望の目的地まで移動させることができる。

【0017】

ＬＲＦ１８は、移動体１０に搭載される２次元走査型または３次元走査型の距離計測計であり、レーザ光を予め設定された走査角度範囲内に射出する。具体的には、ＬＲＦ１８は、移動体１０の進行方向に対して水平方向及び垂直方向に所定の角度範囲（例えば、進行方向に対して左右方向及び上下方向にそれぞれ６０°）でレーザ光を射出する。レーザ光の走査角度範囲内に物体が存在すると、射出したレーザ光が物体で反射されてＬＲＦ１８に戻ってくる。ＬＲＦ１８は、レーザ光を出射してから受光するまでの時間を計測することで、ＬＲＦ１８（すなわち、移動体１０）から物体までの距離を計測する。また、ＬＲＦ１８からレーザ光を射出した方向（すなわち、物体から反射されるレーザ光の入射角度）は既知であるため、ＬＲＦ１８に対する物体の方位も特定することができる。ＬＲＦ１８で取得した物体の位置情報は、コンピュータ２２に出力される。

【0018】

輝度センサ２０は、ＬＲＦ１８が物体から反射される光を受光するときに、ＬＲＦ１８で受光される光の輝度を計測する。物体から反射される光の輝度は、物体の材質によって変化する。例えば、アスファルト路面から反射される光の輝度は、芝生面から反射される光の輝度よりも大きくなる。輝度センサ２０で計測される物体の輝度情報は、コンピュータ２２に出力される。なお、輝度センサ２０には、フォトダイオードを利用した公知のセンサを用いることができる。

【0019】

環境地図記憶装置１６は、移動体１０が移動する移動領域の環境地図を記憶する。具体的には、移動領域内に存在している物体（例えば、床面、壁、障害物等）の位置情報と、それら物体から反射される光の輝度情報とを関連付けて記憶している。環境地図記憶装置１６に記憶される環境地図の構成及び作成手順については、後で詳述する。

【0020】

コンピュータ２２は、演算処理を行うＣＰＵと、演算処理のデータが一時的に記憶されるＲＡＭと、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭを備えている。コンピュータ２２は、ＣＰＵがＲＯＭに記憶された演算プログラムを実行することで、環境地図作成部２４、自己位置推定部２６、走行制御部２８として機能する。環境地図作成部２４は、ＬＲＦ１８及び輝度センサ２０の検出結果を用いて環境地図を作成する。自己位置推定部２６は、ＬＲＦ１８及び輝度センサ２０の検出結果と、環境地図記憶装置１６に記憶されている環境地図を用いて、移動体１０の自己位置を推定する。走行制御部２８は、自己位置推定部２６で推定される自己位置からモータの駆動量を計算し、その計算した駆動量でモータ（車輪）を駆動する。これによって、移動体１０を目標位置まで移動させる。なお、コンピュータ２２による移動体１０の走行制御（すなわち、走行制御部２８としての動作）については、公知の方法で行うことができる。このため、以下では、コンピュータ２２による環境地図作成処理（すなわち、環境地図作成部２４としての動作）と、自己位置推定処理（すなわち、自己位置推定部２６としての動作）について詳細に説明する。

【0021】

まず、図２を参照して、コンピュータ２２で行われる環境地図作成処理について説明する。環境地図は、移動体１０の移動領域を３次元のグリッド（ボクセル）に分割して表現される。具体的には、分割したグリッドに物体（例えば、床面、壁、障害物等）が存在しない場合は、当該グリッドには物体が存在しないことを記憶し、分割したグリッドに物体（例えば、床面、壁、障害物等）が存在する場合は、当該グリッドに存在する物体を多次元正規分布データ（平均値、共分散）で表現する。後述するように、本実施例では、グリッドに存在する物体を多次元正規分布データで表現することで、グリッドの大きさ（解像度）を大きくしても自己位置推定の精度の低下を抑制している。グリッドの大きさ（解像度）は、所望の自己位置推定精度が得られるように適宜設定することができる。

【0022】

環境地図を作成するためには、まず、移動体１０を移動領域内の適宜の位置（詳細には、位置及び方位）に位置決めし、その位置においてＬＲＦ１８による距離計測及び輝度センサ２０による輝度計測を行う。すなわち、ＬＲＦ１８は、レーザ光を２次元に走査しながら物体から反射される反射光を受光し、各走査角（垂直及び水平方向）における物体の有無及び物体までの距離を計測する。ＬＲＦ１８による距離計測と同時に、輝度センサ２０により、ＬＲＦ１８で受光される光（物体から反射された光）の輝度を計測する。ＬＲＦ１８及び輝度センサ２０による計測結果は、コンピュータ２２に送信される。なお、ＬＲＦ１８及び輝度センサ２０による計測は、移動体１０が移動する移動領域内の複数個所で行われる。これによって、移動領域内の全範囲についてＬＲＦ１８及び輝度センサ２０による計測が行われる。なお、本実施例とは異なり、移動体１０に移動領域内を走行させながらＬＲＦ１８による距離計測と輝度センサ２０による輝度計測を行い、走行時に計測されるデータ群を処理することで環境地図を作成してもよい。

【0023】

ＬＲＦ１８及び輝度センサ２０の計測が行われると、まず、コンピュータ２２は、ＬＲＦ１８から送信される移動体１０の移動領域内の物体の位置情報ｐ（ＬＲＦ１８の計測データ）と、輝度センサ２０から送信される移動体１０の移動領域内の物体の輝度情報ｑ（輝度センサ２０の計測データ）を受信する（Ｓ１２）。受信した位置情報ｐと輝度情報ｑは、コンピュータ２２内のＲＡＭに記憶される。次に、コンピュータ２２は、グリッド内にある位置情報ｐを、位置情報ｐの多次元正規分布データに変換する（Ｓ１４）。すなわち、ＬＲＦ１８による測定を行ったときの移動体１０の位置は既知であり、また、位置情報ｐを取得したときのレーザ光の照射方向も既知である。このため、ＬＲＦ１８から物体までの距離が計測されると、その物体の位置（ｘ，ｙ，ｚ）を特定することができる。物体の位置（ｘ，ｙ，ｚ）が特定できると、その物体が移動領域内のどのグリッド（ｉ，ｊ，ｋ）（ｘ方向にｉ番目、ｙ方向にｊ番目、ｚ方向にｋ番目のグリッド）に位置するかを特定することができる。このため、まず、コンピュータ２２は、ＬＲＦ１８で得られた測定データ（物体の位置情報ｐ（ｘ，ｙ，ｚ））をグリッド毎に分類する。ＬＲＦ１８で得られた物体の位置情報ｐ（ｘ，ｙ，ｚ）をグリッド毎に分類すると、コンピュータ２２は、グリッド毎に、そのグリッドに分類された複数の位置情報ｐ（ｘ，ｙ，ｚ）を多次元正規分布データに変換する。すなわち、各グリッドに分類された複数の位置情報ｐ（ｘ，ｙ，ｚ）を、そのグリッド内にある複数の位置情報ｐ（ｘ，ｙ，ｚ）の平均μ（ｘ_ａｖｅ，ｙ_ａｖｅ，ｚ_ａｖｅ）と分散Σ（σ_ｘ，σ_ｙ，σ_ｚ）により表される多次元正規分布データに変換する。コンピュータ２２は、全てのグリッドに対して、ステップＳ１４を実行する。位置情報ｐの平均μ_ｐ（ｘ_ａｖｅ，ｙ_ａｖｅ，ｚ_ａｖｅ）と分散Σ_ｐ（σ_ｘ，σ_ｙ，σ_ｚ）は、下記の式で表される。

【0024】

【数1】

【0025】

上記の式におけるｎは、グリッド内にある位置情報の個数を示している。

【0026】

次に、コンピュータ２２は、ステップＳ１２で受信した輝度情報ｑに対して、ステップＳ１４と同様の処理を実施し、グリッド内にある輝度情報ｑを、輝度情報ｑの多次元正規分布データに変換する（Ｓ１６）。上述したように本実施例では、輝度センサ２０による輝度計測は、ＬＲＦ１８による距離計測と同時に行われ、輝度センサ２０で取得される輝度情報ｑ（輝度データ）は、ＬＲＦ１８で取得される位置情報と関連付けられている。すなわち、特定された物体の位置（ｘ，ｙ，ｚ）と、その地点から反射される光の輝度情報ｑとが関連づけられて、輝度情報ｑ（ｘ，ｙ，ｚ，ｑ）としてＲＡＭに格納されている。したがって、ステップＳ１６では、まず、コンピュータ２２は、輝度情報ｑ（ｘ，ｙ，ｚ，ｑ）をグリッド毎に分類し、次いで、グリッド毎に分類された複数の輝度情報ｑ（ｘ，ｙ，ｚ，ｑ）を多次元正規分布データに変換する。具体的には、各グリッドに分類された複数の輝度情報ｑ（ｘ，ｙ，ｚ、ｑ）を、そのグリッド内にある複数の輝度情報ｑ（ｘ，ｙ，ｚ，ｑ）の平均μ_ｑ（ｘ_ａｖｅ，ｙ_ａｖｅ，ｚ_ａｖｅ，ｑ_ａｖｅ）と分散Σ_ｑ（σ_ｘ，σ_ｙ，σ_ｚ，σ_ｑ）により表される多次元正規分布データに変換する。多次元正規分布データへの変換には、ステップＳ１４で用いた式と同様の式が用いられる。なお、本実施例においては、位置情報を処理（Ｓ１４）した後に輝度情報を処理（Ｓ１６）したが、輝度情報処理の後に位置情報を処理してもよいし、位置情報処理と輝度情報処理を同時に実行してもよい。

【0027】

次に、コンピュータ２２は、ステップＳ１４で作成された位置情報ｐ（ｘ，ｙ，ｚ）の多次元正規分布データ（μ_ｐ，Σ_ｐ）と、ステップＳ１６で作成された輝度情報ｑの多次元正規分布データ（μ_ｑ，Σ_ｑ）を組み合わせて、多次元正規分布データによる環境地図を作成する（Ｓ１８）。すなわち、ステップＳ１４，１６によって、移動体１０が移動する移動領域を分割した３次元のグリッドのそれぞれについて、位置情報ｐの多次元正規分布データ（μ_ｐ，Σ_ｐ）と、輝度情報ｑの多次元正規分布データ（μ_ｑ，Σ_ｑ）が計算されている。本実施例では、各グリッドの物体の位置分布を位置情報ｐの多次元正規分布データ（μ_ｐ，Σ_ｐ）で表し、各グリッドの物体から反射される輝度の強度分布を多次元正規分布データ（μ_ｑ，Σ_ｑ）で表す。ＬＲＦ１８及び輝度センサ２０で取得されるデータ群を多次元正規分布データ（μ_ｐ，Σ_ｐ），（μ_ｑ，Σ_ｑ）に変換することで、環境地図を構成するデータ数を大幅に低減することができる。また、グリッド内が１つの物体によって占有されていると仮定する環境地図と比較して、グリッド内の物体を多次元正規分布データ（μ_ｐ，Σ_ｐ）で表すため、グリッドを大きくしても環境地図の精度低下を抑制することができる。ステップＳ１８で環境地図を作成すると、コンピュータ２２は、作成した環境地図を環境地図記憶装置１６に保存する（Ｓ２０）。

【0028】

図３に、正規分布データの一例を示す。図３（ａ）は、１次元正規分布データの一例であり、図３（ｂ）は、２次元正規分布データの一例であり、図３（ｃ）は、３次元正規分布データの一例である。黒点が、例えばＬＲＦ１８で計測された位置情報となる。図３（ａ）の１次元正規分布データでは、位置情報の平均は正規分布曲線の中心位置となり、その分散は正規分布曲線の広がりで表現される。図３（ｂ）の２次元正規分布データでは、位置情報の平均は楕円の中心位置となり、その分散は楕円の大きさで表現される。図３（ｃ）の３次元正規分布データでは、位置情報の平均は楕円体の中心位置で表現され、その分散は楕円体の大きさで表現される。本実施例では、図３（ｃ）の３次元正規分布データを用いて、移動領域（詳細には各グリッド）内の物体を表現している。

【0029】

図４に、移動体１０の移動領域を、ＬＲＦ１８で取得された位置情報ｐ（ｘ，ｙ，ｚ）による環境地図（同図（ａ））と、その位置情報ｐ（ｘ，ｙ，ｚ）を多次元正規分布データ（μ_ｐ，Σ_ｐ）に変換した後の環境地図（同図（ｂ））の一例を示す。なお、図４では、得られた環境地図を平面視して示している。図４（ａ）のＬＲＦ１８で取得された位置情報ｐ（ｘ，ｙ，ｚ）は、千箇所以上存在するが、多次元正規分布データに変換した後では、数十点の多次元正規分布データ（μ_ｐ，Σ_ｐ）で同一の環境を表現している。図４（ｂ）において、壁面（直線に見える箇所）部分は、ある方向に分散が大きい複数の楕円で表現されている。一方、物体の構造的変化がある領域（例えば、壁の端面（壁と空間との境界）、２つの壁面が交差する領域等）では、楕円の大きさが複数の方向に広がっている。この楕円の形状の違いの比較により、解像度が低い地図であってもある程度の位置推定精度を確保することができる。

【0030】

図５に、屋外環境に対して、上記の環境地図作成処理（ステップＳ１２〜ステップＳ１８）により作成した環境地図の一例を示す。図５の環境地図は、０．５ｍのグリッド（ボクセル）に分割されている。各グリッド内の物体の位置の平均は、楕円体の中心位置で示されており、各グリッド内の物体の位置の分散は、楕円体の大きさにより表されている。また、各グリッド内の物体の輝度分布は濃淡で表されている。環境地図に輝度分布を導入することで、芝生とアスファルト路面の境界が明確になっていることが分かる。これにより、構造的変化が少ない場所においても、材質の変化による輝度情報の違いを利用することができる。

【0031】

次に、図６を参照して、コンピュータ２２で行われる自己位置推定処理を説明する。本実施例の自己位置推定処理では、モンテカルロ位置推定手法が用いられる。なお、コンピュータ２２は、図６に示す自己位置推定処理を周期的に繰り返し実行する。

【0032】

まず、コンピュータ２２は、ＬＲＦ１８による距離計測と輝度センサ２０による輝度測定を実行する（Ｓ２４）。ＬＲＦ１８による距離計測結果及び輝度センサ２０による輝度計測結果は、コンピュータ２２のＲＡＭに格納される。次に、コンピュータ２２は、ＬＲＦ１８で計測された距離データと輝度センサ２０で計測された輝度データを処理し、物体の位置ｐ（ｘ，ｙ，ｚ）に関する多次元正規分布データ（μ_ｐ，Σ_ｐ）と、物体の輝度情報ｑ（ｘ，ｙ，ｚ，ｑ）に関する多次元正規分布データ（μ_ｑ，Σ_ｑ）を算出する（Ｓ２６）。多次元正規分布データ（μ_ｐ，Σ_ｐ），（μ_ｑ，Σ_ｑ）の算出は、上述した環境地図作成処理におけるステップＳ１４，Ｓ１６の処理と同様に行うことができる。ただし、ステップＳ２６においては、移動体１０（すなわち、ＬＲＦ１８）を基準として設定された３次元のローカルグリッドに基づいて算出される。すなわち、移動体１０を基準原点とすると共に移動体１０の移動方向を基準方向として３次元のローカルグリッドが設定され、そのローカルグリッドの各グリッドについて、ＬＲＦ１８で計測された距離データと輝度センサ２０で計測された輝度データを分類し、分類した距離データと輝度データを処理することで多次元正規分布データ（μ_ｐ，Σ_ｐ），（μ_ｑ，Σ_ｑ）を算出する。これによって、移動体１０の周辺の環境地図（移動体１０に設定されたローカル座標を用いた環境地図）が作成される。なお、ステップＳ２６で用いられるローカルグリッドのグリッドの大きさは、環境地図記憶装置１６に保存される環境地図（グローバル環境地図）のグリッドと同一の大きさとしてもよいし、異なる大きさのグリッド（例えば、小さなグリッド）としてもよい。

【0033】

次に、コンピュータ２２は、公知のモンテカルロ法を用いて、直前の周期（時刻ｔ−１）におけるパーティクル（すなわち、移動体１０）の位置（平均値）ｐ_{ｉ，ｔ−１}（ｉ＝１〜ｋ）と、パーティクルのモーションモデルＤ_ｕから、現在の時刻ｔにおける各パーティクルの位置（平均値）ｐ_ｉ，ｔ（ｉ＝１〜ｋ）を予測する（Ｓ２８）。ステップＳ２８で予測されるパーティクルの位置には、パーティクル（移動体１０）の位置と方位が含まれる。

【0034】

次に、コンピュータ２２は、環境地図記憶装置１６が保持する環境地図（グローバル座標の環境地図）と、ステップＳ２４で作成された移動体１０の周辺の環境地図（ローカル座標を用いたローカル環境地図）を用いて、各パーティクルｐ_ｉ，ｔの重みｗ_ｉ，ｔを算出する（Ｓ３０）。例えば、コンピュータ２２は、複数のパーティクルｐ_ｉ，ｔから１つのパーティクルｐを選択し、その選択したパーティクルの位置データを用いて、ローカル環境地図をグローバル座標系へ変換する。次いで、コンピュータ２２は、グローバル座標系に変換された移動体１０の周辺の環境地図（グローバル座標系）と、環境地図記憶装置１６が保持する環境地図（グローバル環境地図）の位置情報の差分（μ_ｉｊ）及び輝度情報の差分（φ）を用いて、そのパーティクルの重みを算出する。パーティクルの重みは、下記の式で算出される。

【0035】

【数2】

【0036】

上記の式における各記号の意味は、次のとおりである。
ｍ：環境地図記憶装置１６が保持する環境地図の多次元分布データの数
ｎ：ステップＳ２４で作成された多次元分布データの数
μ_ｉｊ：位置情報の差分
ｄ_１、ｄ_２：スケーリングパラメータ
φ：輝度情報の差分
Ｒ_ｋ：回転行列
Σ_ｉ：共分散行列

【0037】

ステップＳ２８及びステップＳ３０の処理によって、パーティクルｐ_ｉ，ｔ（ｉ＝１〜ｍ）のそれぞれについて最終的な重みｗ_ｉ，ｔが算出されると、コンピュータ２２は、算出された重みｗ_ｉ，ｔに従って、新たなパーティクルｐ_ｉ，ｔ（ｉ＝１〜ｋ）をリサンプリングする（Ｓ３２）。すなわち、上述した処理によって、各パーティクルｐ_ｉ，ｔ（ｉ＝１〜ｋ）の重みｗ_ｉ，ｔが算出されている。このため、コンピュータ２２は、重みｗ_ｉ，ｔの高いパーティクルを多く選択する一方で、重みｗ_ｉ，ｔの低いパーティクルを少なく選択して、新たなパーティクルｐ_ｉ，ｔ（ｉ＝１〜ｋ）を作成する。例えば、パーティクルが４個であり、パーティクル１の重みが０．５、パーティクル２の重みが０．３、パーティクル３の重みが０．１、パーティクル４の重みが０．１となる場合は、パーティクル１を新たなパーティクル１，２として２個作成し、パーティクル２を新たなパーティクル３として１個作成し、パーティクル３又はパーティクル４を新たなパーティクル４として１個作成する。そして、あらたに作成したパーティクル１〜４を用いて、以後の処理を行う。

【0038】

ステップＳ３２の処理が終了すると、コンピュータ２２は、次の周期の処理が開始されるまで待機する。以下、ステップＳ２４からステップＳ３２までの処理が繰り返し実行されることで、各周期における移動体１０の位置が推定される。上記の説明から明らかなように、ステップＳ２４からステップＳ３２までの処理を繰り返すことで、重みの高いパーティクルが多く選択されて残り、重みの低いパーティクルが順次消滅してゆくこととなる。これによって、移動体最終的に１０の位置が最終的に推定される。

【0039】

上述した説明から明らかなように、本実施例の移動体１０の位置推定装置では、ＬＲＦ１８及び輝度センサ２０で計測された位置情報と輝度情報を多次元分布データに変換し、多次元分布データで表された環境地図を用いて、各パーティクルの重みを算出する。図５に例示するように、本実施例で用いられる環境地図では、構造的変化が少ない場所においても材質の変化により輝度情報には相違が生じる。本実施例では、環境地図に導入した輝度情報を利用することで、位置推定精度を高めることができる。

【0040】

図７に本実施例の位置推定手法を用いた位置推定精度と、公知のROS amcl（占有格子地図とパーティクルフィルタを用いた位置推定手法）の位置推定精度とを併せて示す。図７の「（i）」は、本実施例の解像度と位置推定精度の関係を示している。図７の「（ii）」は、ROS amclの解像度と位置推定精度の関係を示している。図７から明らかなように、ROS amclでは、環境地図の解像度を下げると（グリッドを大きくすると）、位置推定精度が低下している。しかしながら、本実施例においては、環境地図の解像度を下げても（グリッドを大きくしても）、位置推定精度の低下は見られない。また、位置推定精度のバラつき幅も、本実施例の方が小さくすることができる。これにより、本実施例は、環境地図の解像度を下げながら、位置推定精度を高く維持することができる。その結果、所望の位置推定精度を得ながら、環境地図の容量を小さくすることができる。

【0041】

図８に、輝度情報を用いることが有用な環境の一例を示す。図８に示す横断歩道は構造的変化が少ないため、物体の位置情報だけで横断歩道の有無を判別することは困難である。しかしながら、物体（路面）から反射される光の輝度情報を用いることで、横断歩道の白線と、横断歩道の白線と白線との間と、を明確に判別することができる。これにより、移動体１０に横断歩道を移動させる場合の移動体１０の位置推定精度を向上することができる。これによって、移動体１０の位置制御を精度良く行うことができる。

【0042】

図９に、輝度情報を用いることが有用な環境の他の例を示す。図９に示す車道と車線は、構造的変化が少ないため、物体の位置情報だけで、車道と車線の境界を判別することは困難である。しかしながら、物体（路面）から反射される光の輝度情報を用いることで、車道と車線とを明確に判別することができる。車道を移動する移動体の位置推定精度を向上することができる。

【0043】

図１０に、輝度情報を用いることが有用な環境の他の例を示す。図１０に示すアスファルト面と芝生面は、構造的な変化が少ないため、物体の位置情報だけで、両者の境界を判別することは困難である。しかしながら、物体（アスファルト面又は芝生面）から反射される光の輝度情報を用いることで、アスファルト面と芝生面とを明確に区別することができる。これにより、移動体の位置推定精度を向上することができる。

【0044】

図１１に、輝度情報を用いることが有用な環境の他の例を示す。図１１に示すアスファルト面とコンクリート面は、構造的な変化が少ないため、物体の位置情報だけで両者を判別することは困難である。しかしながら、物体（アスファルト面又はコンクリート面）から反射される光の輝度情報を用いることで、アスファルト面とコンクリート面とを明確に区別することができる。これにより、移動体の位置推定精度を向上することができる。

【0045】

以上、本明細書が開示する技術の実施例について詳細に説明したが、これは例示に過ぎず、本明細書が開示する技術は、上記の実施例を様々に変形、変更したものが含まれる。また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

【符号の説明】

【0046】

１０：移動体
１２ａ，１２ｂ：モータ
１４ａ，１４ｂ：エンコーダ
１６：環境地図記憶装置
１８：ＬＲＦ
２０：輝度センサ
２２：コンピュータ
２４：環境地図作成部
２６：自己位置推定部
２８：走行制御部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】