JP2011027598A - 環境認識装置及びそれを備えた移動体 - Google Patents

Abstract

【課題】移動体の移動可能領域を適切に検出する。
【解決手段】移動体は、構造物までの距離を計測する距離計測センサ３８と、距離計測センサの３次元位置を計測するセンサ位置計測装置１８ａ，１８ｂを備えている。制御装置２０は、距離計測センサ３８の計測結果とセンサ位置計測装置１８ａ，１８ｂの計測結果を用いて、計測された構造物の３次元位置を算出する。そして、算出された構造物の３次元位置と尤度マップを用いて、その算出された３次元位置に対応するマップ上の計測点における構造物の情報と尤度をマップに登録又は更新すると共に、その計測点の近傍の非計測点における構造物の情報と尤度をマップに登録又は更新する。
【選択図】図２

Description

本発明は、移動体が移動する移動領域内にある構造物（例えば、床面、障害物等）の３次元構造を認識するための技術に関する。

特許文献１には、移動体が移動する移動領域内にある障害物を認識する技術が開示されている。この技術では、距離計測センサが移動体に装備される。距離計測センサは、移動体の走行方向前方の床面に向けてレーザ光を照射し、そのレーザ光の反射光を検出する。レーザ光を照射してから反射光を検出するまでの時間によって、距離計測センサから反射物までの距離を計測する。そして、計測された距離から、レーザ光を照射した方向の床面に障害物があるか否かを検出している。

特開２００７−１９３４９５号公報

特許文献１の技術では、距離計測センサで計測されていない領域（非計測点）については、その領域にある構造物を認識できない。このため、距離計測センサで計測されていない領域を通過可能領域と判定するようプログラムした場合、その領域に障害物があると、移動体と障害物とが衝突してしまうこととなる。一方、距離計測センサで計測されていない領域を通過不可能領域と判定するようプログラムした場合、その領域に障害物がなく通過可能な領域であっても、その領域を移動体が通過できないこととなってしまう。

本発明は、上記した実情に鑑みてなされたものであり、距離計測センサで計測された計測点の構造物からその計測点近傍の非計測点の構造物を推定することで、移動体の移動可能領域を適切に検出することが可能となる技術を提供する。

本発明の環境認識装置は、移動体が移動する移動領域内にある構造物の３次元構造を認識する。この環境認識装置は、距離計測センサと、センサ位置計測装置と、尤度マップと、構造物マップと、位置算出手段と、マップ更新手段を備えている。距離計測センサは、移動体に取付けられており、構造物までの距離を計測する。センサ位置計測装置は、距離計測センサの３次元位置を計測する。尤度マップは、距離計測センサで計測された計測点における構造物の情報から、その計測点の近傍の非計測点における構造物を推定するための尤度分布を記憶する。構造物マップは、移動領域内の各点における構造物の情報及びその尤度を記憶する。位置算出手段は、距離計測センサの計測結果及び計測方向とセンサ位置計測装置の計測結果を用いて、距離計測センサで計測された構造物の３次元位置を算出する。マップ更新手段は、位置算出手段で算出された構造物の３次元位置と記憶されている尤度マップを用いて、その算出された３次元位置に対応する構造物マップ上の対応点における構造物の情報と尤度を更新又は登録すると共に、その対応点の近傍の近傍点における構造物の情報と尤度を更新又は登録する。

この環境認識装置では、距離計測センサで構造物までの距離を計測し、センサ位置計測装置で距離計測センサの３次元位置を計測する。移動体への距離計測センサの取付け位置及び取付け角度等から距離計測センサの計測方向は分かるため、前記の各計測値と距離計測時における距離計測センサの計測方向から、計測された構造物の３次元位置が算出される。
この環境認識装置では、計測点の構造物の情報から、その計測点近傍の非計測点の構造物を推定するための尤度マップが設定されており、また、移動領域内の各点における構造物の情報とその尤度を記憶する構造物マップを備えている。このため、距離計測センサで計測された構造物の３次元位置が算出されると、その情報と尤度マップを用いて、計測した点の構造物の情報とその尤度を構造物マップに更新又は登録し、さらに、その計測した点の近傍の点の構造物の情報とその尤度を構造物マップに更新又は登録する。従って、非計測点についても構造物の情報とその尤度が記憶されることとなるため、移動体の移動可能領域を適切に検出することが可能となる。

上記の環境認識装置においては、センサ位置計測装置は、移動体の位置を計測する移動体位置計測センサと、移動体の姿勢を計測する移動体姿勢計測センサを有することができる。この場合に、位置算出手段は、移動体位置計測センサの検出結果と移動体姿勢計測センサの検出結果を用いて、距離計測時における距離計測センサの３次元位置及び計測方向を特定し、これら特定された「距離計測センサの３次元位置及び計測方向」と「距離計測センサで計測された構造物までの距離」とから構造物の３次元位置を算出することができる。このような構成によると、距離計測センサの３次元位置及び計測方向が正確に特定できるため、より正確に構造物の３次元位置を算出することができる。

また、上記の環境認識装置は、構造物マップに記憶されている各点の構造物の情報と尤度に基づいて、移動体の移動可能領域を検出する移動可能領域検出手段をさらに備えていることが好ましい。このような構成によると、構造物の情報とその尤度を記憶する構造物マップに基づいて移動体の移動可能領域を検出するため、移動体の移動経路として適切な移動経路を選択することができる。

移動可能領域検出手段が移動可能領域を検出するアルゴリズムとしては、種々のものを採用することができる。例えば、移動可能領域検出手段は、構造物マップに記憶されている構造物の情報から障害物がないと判断され、かつ、尤度が所定の閾値以上となる領域を、移動可能領域として検出することができる。このような構成によると、障害物がなく、かつ、尤度の高い領域が移動可能領域とされるため、移動体が障害物に衝突する可能性が低められ、安全に移動することができる。

また、上記の環境認識装置では、予め設定された既知のキャリブレーション面を利用して、距離計測センサの移動体への取付け位置及び取付け方向をキャリブレーションすることができる。このような環境認識装置としては、例えば、キャリブレーション面データと、キャリブレーション手段を備えることができる。キャリブレーション面データは、予め設定されたキャリブレーション面内の各点における絶対位置（絶対座標系の位置）を記憶する。キャリブレーション手段は、キャリブレーションデータを用いて、距離計測センサの移動体への取付け位置及び取付け姿勢をキャリブレーションする。そして、キャリブレーション手段は、キャリブレーション面内の複数の点のそれぞれについて得られた（１）距離計測センサにより計測される計測距離と（２）距離計測時の移動体の絶対位置及び絶対姿勢（絶対座標系における位置と姿勢）とさらに用いて、距離計測センサの移動体への取付け位置及び取付け姿勢をキャリブレーションする。
このような構成によると、キャリブレーション面内の複数の点を計測した計測結果を用いてキャリブレーションを行うため、精度よくキャリブレーションを行うことができる。また、単独のターゲット（点）ではなく面をターゲットとするため、距離計測センサによってターゲットを容易に計測することができる。

なお、キャリブレーション手段は、距離計測センサによりキャリブレーション面を１次元的又は２次元的にスキャンすることで得られた複数の計測点の計測距離と、それらの計測点を計測した時の移動体の絶対位置及び絶対姿勢とを用いてキャリブレーションを行うことが好ましい。ここで、２次元的にスキャンするとは、距離計測センサにより距離計測が行われる点群がキャリブレーション面内で直線状に位置するように計測を行うことを意味する。また、３次元的にスキャンするとは、距離計測センサにより距離計測が行われる点群がキャリブレーション面内で平面状に位置するように計測を行うことを意味する。このような構成によると、距離計測を行う計測点の数を容易に増やすことができる。

また、キャリブレーション面は、互いに交差する複数の平面を有していることが好ましい。複数の面をキャリブレーションに利用することで、キャリブレーションを精度よく行うことができる。

また、前記のセンサ位置計測装置は、移動体の位置を計測する移動体位置計測センサと、移動体の姿勢を計測する移動体姿勢計測センサを有することができる。この場合に、キャリブレーション手段は、移動体位置計測センサの出力と移動体姿勢計測センサの出力を用いて、距離計測時の移動体の絶対位置及び絶対姿勢を取得することができる。
このような構成によると、移動体の位置制御及び／又は姿勢制御に用いることができる移動体位置計測センサと移動体姿勢計測センサを利用して「距離計測時の移動体の絶対位置及び絶対姿勢」を取得する。このため、「距離計測時の移動体の絶対位置及び絶対姿勢」を取得するためだけに、別途計測装置を設ける必要はない。

また、本発明の移動体は、上述した環境認識装置のいずれかが装備されている。このような環境認識装置を装備することで、移動体は自律的に適切な経路を選択して移動することができる。

本発明の一実施例に係る移動体の側面図。 図１に示す移動体の平面図。 移動体の制御装置の構成を示すブロック図。 構造物マップの構成を説明するための図。 尤度マップの一例を示す図。 尤度マップを用いて構造物マップを更新する手順を説明するための図。 移動体の動作手順を示すフローチャート。 マップ更新登録処理の手順を示すフローチャート。 本実施例の変形例により計測された床面の構成を模式的に示す図。 図９の床面を計測することで得られた２次元エレベーションマップ。 図９の床面を計測することで得られた尤度のみを表示するマップ。 図１０，１１に示すマップから得られた移動可能領域マップ。 本発明の他の変形例に係る移動体の正面図。 第２実施例の移動体のキャリブレーションの手順を示すフローチャート。 キャリブレーションを実施している時の移動体の状態を示す図。 第２実施例の変形例を説明するための図。

下記に詳細に説明する実施例の主要な特徴を最初に列記する。
（形態１）距離計測センサは、２次元スキャン型の距離計測センサである。
（形態２）移動領域の２次元マップを備える。この２次元マップは、マップ内の各点について高さ情報と尤度を記憶する。

（第１実施例） 本発明の一実施例に係る移動体について、図面を参照しながら説明する。図１，２に示すように、移動体１０は、４つの車輪１２ａ，１２ｂ，１４ａ，１４ｂと、これら４つの車輪１２ａ，１２ｂ，１４ａ，１４ｂによって支持される車体４０を備えている。

前輪１４ａ，１４ｂは、キャスタ輪であり、車体４０に取付けられている。後輪１２ａは、モータ１６ａと接続されており、モータ１６ａによって駆動される。後輪１２ｂは、モータ１６ｂと接続されており、モータ１６ｂによって駆動される。すなわち、後輪１２ａ、１２ｂはそれぞれ独立して駆動される。後輪１２ａ，１２ｂを駆動することで、移動体１０は床面Ｆを走行する。また、後輪１２ａの回転駆動量と後輪１２ｂの回転駆動量を変えることで、移動体１０はその進行方向を変える。

車体４０には、車輪１２ａ、１２ｂを駆動するモータ１６ａ、１６ｂと、車体４０の姿勢（ロール角，ピッチ角，ヨー角）を検出する３軸のジャイロセンサ３６と、進行方向の床面Ｆまでの距離を計測する距離計測センサ３８と、モータ１６ａ、１６ｂに制御指令値を出力する制御装置２０が備えられている。

モータ１６ａ、１６ｂは、制御装置２０と電気的に接続されている。モータ１６ａ、１６ｂは、制御装置２０から入力される制御指令値にしたがって、後輪１２ａ、１２ｂを駆動する。モータ１６ａにはエンコーダ１８ａが設けられている。エンコーダ１８ａは、制御装置２０と電気的に接続されている。エンコーダ１８ａは、モータ１６ａの回転角速度（すなわち、後輪１２ａの回転角速度）を検出する。エンコーダ１８ａが検出した後輪１２ａの回転角速度は、制御装置２０に入力される。また、モータ１６ｂにもエンコーダ１８ｂが設けられている。エンコーダ１８ｂは、後輪１２ｂの回転角速度を検出する。エンコーダ１８ｂが検出した後車１２ｂの回転角速度も、制御装置２０に入力される。

ジャイロセンサ３６は、車体４０の内部に配設されている。ジャイロセンサ３６は制御装置２０と電気的に接続されている。ジャイロセンサ３６は、車体４０の姿勢（ロール角，ピッチ角，ヨー角）を検出する。検出した車体４０の姿勢（ロール角，ピッチ角，ヨー角）は、制御装置２０に入力される。

距離計測センサ３８は、２次元スキャン型の距離計測センサである。距離計測センサ３８は、照射したレーザ光の反射光を検出し、レーザ光を照射してから反射光を検出するまでの時間で距離を計測する。レーザ光を照射する面Ｓ（以下、計測面Ｓという）は一定とされる一方で、レーザ光を照射する方向は、設定された角度範囲内を一定の角速度で変化する。従って、距離計測センサ３８は、計測面Ｓ内の設定された角度範囲を所定の角度間隔で距離計測を行うこととなる。このため、距離計測センサ３８からの距離が近いと計測点間の間隔は短く（計測密度が高く）、距離計測センサ３８からの距離が遠くなると計測点間の間隔は長くなる（計測密度が低くなる）。このような距離計測センサ３８としては、例えば、ＳＩＣＫ社製のＬＭＳ２００を用いることができる。
なお、本実施例では、距離計測センサ３８に２次元スキャン型の距離計測センサを用いたが、距離計測センサ２８にはその他の公知の距離計測センサ（例えば、３次元スキャン型の距離計測センサ）を用いることができる。また、距離計測方式も、レーザ光に限られず、公知の種々の方式（例えば、電磁波等）を用いることができる。

上述した距離計測センサ３８は、車体４０の進行方向前側の端部で、かつ、左側面の近傍に設置されている。また、距離計測センサ３８の設置角度は、ロール軸（Ｘ軸）周りに所定角度（例えば、３０°）だけ傾けられると共に、ピッチ軸（Ｙ軸）周りに所定角度（例えば、３０°）だけ傾けられている。このため、移動体１０の進行方向前方で計測面Ｓと床面Ｆとが交差し（図１参照）、移動体１０の進行方向前方の車幅の範囲の全域が距離計測センサ３８で計測されることとなる。すなわち、本実施例では、距離計測センサ３８によって、移動体１０の進行方向前方の床面の３次元構造が計測される。

制御装置２０は、ＣＰＵ，ＲＯＭ，ＲＡＭを備えたマイクロプロセッサによって構成されている。制御装置２０は、車体４０内に配されている。制御装置２０は、モータ１６ａ、１６ｂと、エンコーダ１８ａ、１８ｂと、ジャイロセンサ３６と、距離計測センサ３８に電気的に接続されている。図３は制御装置２０の機能を示すブロック図である。図３に示すように、制御装置２０は、センサ位置算出部２２、構造物位置算出部２３、マップ登録更新部２４、移動可能領域検出部２６、移動経路決定部２８、尤度マップ記憶部２９、構造物マップ記憶部３０、目標位置記憶部３２、モータ駆動部３４を備えている。

構造物マップ記憶部３０は、移動体１０が移動する移動領域の２次元マップを記憶する。この２次元マップには、移動領域内の各点［グローバル座標系の点（ｘ，ｙ）］における構造物（例えば、床面Ｆ）の高さμとその尤度（尤もらしさ）σが記憶されている。例えば、移動領域内の点（ｘ１，ｙ１）に高さμ１の構造物が尤度（確率）σ１で存在する場合、２次元マップの座標（ｘ１，ｙ１）には（μ１，σ１）が記憶される（図４参照）。本実施例では、３次元マップに代えて２次元マップに高さ情報μを記憶することで、マップ記憶部３０に要求されるメモリ容量を少なくしている。なお、構造物マップを作成する領域は予め決められていてもよいし、距離計測センサ３８で計測する点が増加するのに応じて適宜拡張していくようにしてもよい。

尤度マップ記憶部２９は、距離計測センサ３８で計測される計測点における構造物の情報（高さμ）から、その計測点の近傍の非計測点における構造物の情報（高さ）を推定するための尤度分布を記憶する。図５に尤度マップの一例を示している。図５に示すように、尤度マップは、計測点Ｐ1から非計測点Ｐ’1，Ｐ’’1までの距離に応じて尤度σが設定されている。具体的には、計測点Ｐ1には尤度σ1（＝１．０）が設定され、計測点Ｐ1に隣接する非計測点Ｐ’1には尤度σ’1（＜σ1）が設定され、非計測点Ｐ’1にさらに隣接する非計測点Ｐ’’1には尤度σ’’1（＜σ’1）が設定される。すなわち、計測点Ｐ1の高さがμである場合、非計測点Ｐ’1の高さがμである尤度（確率）はσ’1となり、非計測点Ｐ’’1の高さがμである尤度（確率）はσ’’1となる。

目標位置記憶部３２は、移動体１０が移動する目的地（ｘｆ，ｙｆ）［グローバル座標系の点（ｘｆ，ｙｆ）］を記憶する。目的地（ｘｆ，ｙｆ）は、外部装置（例えば、遠隔操縦装置等）を用いて制御装置２０に入力される。外部装置から入力された目的地（ｘｆ，ｙｆ）は、目標位置記憶部３２に記憶される。

センサ位置算出部２２は、エンコーダ１８ａ，１８ｂの出力とジャイロセンサ３６の出力から、距離計測時における距離計測センサ３８の３次元位置［（ｘ、ｙ、ｚ）；グローバル座標系］を算出する。すなわち、エンコーダ１８ａ，１８ｂの出力を積分すると、移動体１０の移動距離（Δｘ，Δｙ）を算出することができる。また、移動体１０の基準点の初期位置（ｘｓ，ｙｓ）は既知である。このため、移動体１０の移動距離（Δｘ，Δｙ）を算出すれば、移動体１０の基準点の位置（ｘｓ＋Δｘ，ｙｓ＋Δｙ）を算出することができる。また、移動体１０の基準点と距離計測センサ３８の幾何学的関係は既知であり、車体４０の姿勢（ロール角，ピッチ角，ヨー角）はジャイロセンサ３６により検出されている。このため、センサ位置算出部２２は、エンコーダ１８ａ，１８ｂの出力から移動体１０（基準点）の２次元位置（ｘｓ＋Δｘ，ｙｓ＋Δｙ）を算出し、その算出された２次元位置（ｘｓ＋Δｘ，ｙｓ＋Δｙ）とジャイロセンサ３６の出力より、移動体１０の３次元位置及び姿勢を算出し、算出した移動体の現在の位置及び姿勢、並びに、距離計測センサ３８の位置姿勢関係（幾何学的に既知）から距離計測センサ３８の３次元位置（ｘ，ｙ，ｚ）を算出する。なお、車体４０に対する距離計測センサ３８の取付け位置及び取付け角度は一定である。このため、センサ位置算出部２２は、ジャイロセンサ３６で検出された車体４０の姿勢から、距離計測時における距離計測センサ３８の距離計測方向をさらに算出する。

構造物位置算出部２３は、センサ位置算出部２２で算出された距離計測センサ３８の３次元位置（ｘ，ｙ，ｚ）及び距離計測方向と、距離計測センサ３８で計測された構造物（床面等）までの距離から、構造物の３次元位置［（ｘ0，ｙ0，ｚ0）；グローバル座標系（絶対座標系）］を算出する。すなわち、距離計測センサ３８の距離計測方向と距離計測センサ３８から構造物までの距離が分かると、距離計測センサ３８に対する構造物の相対位置を算出できる。このため、距離計測センサ３８の３次元位置が分かると、構造物の３次元位置（ｘ0，ｙ0，ｚ0）を算出できる。従って、構造物位置算出部２３は、センサ位置算出部２２の算出結果と距離計測センサ３８の計測距離から、構造物の３次元位置（ｘ0，ｙ0，ｚ0）を算出する。

マップ登録更新部２４は、構造物位置算出部２３で算出された構造物の３次元位置（ｘ0，ｙ0，ｚ0）から、構造物マップ記憶部３０に記憶されている２次元マップを更新する。すなわち、構造物の３次元位置（ｘ0，ｙ0，ｚ0）が算出されると、２次元マップ上の点（ｘ0，ｙ0）（すなわち、計測点）の構造物の高さが算出されたこととなる。このため、マップ登録更新部２４は、まず、尤度マップ記憶部２９に記憶されている尤度分布（図５参照）を用いて、計測点（ｘ0，ｙ0）と、その計測点の近傍の非計測点における構造物の高さと尤度を特定する。具体的には、計測点（ｘ0，ｙ0）における高さと尤度を（ｚ0，σ１）とし、計測点に隣接する非計測点（ｘ0＋１，ｙ０），（ｘ0−１，ｙ０），（ｘ0，ｙ0＋１），（ｘ0，ｙ0−１）における高さと尤度を（ｚ0，σ１’）とし、さらにその外側の非計測点（ｘ0＋２，ｙ０），（ｘ0＋１，ｙ０＋１），（ｘ0＋１，ｙ０−１），（ｘ0，ｙ０＋２），（ｘ0，ｙ０−２），（ｘ0−１，ｙ0＋１），（ｘ0−１，ｙ0−１），（ｘ0−２，ｙ０）における高さと尤度を（ｚ0，σ１’’）とする。

計測点と非計測点における高さと尤度が特定されると、次に、マップ登録更新部２４は、その特定された高さと尤度を２次元マップに記憶する。この際、構造物マップ記憶部３０に記憶される２次元マップの該当する座標に高さと尤度が記憶されていない場合、上述した手順で特定した高さと尤度をそのまま登録する。すなわち、計測点については（ｚ0，σ１）が登録され、計測点に隣接する非計測点については（ｚ0，σ１’）が登録され、さらにその外側の非計測点については（ｚ0，σ１’’）が登録される。一方、構造物マップ記憶部３０に記憶される２次元マップの該当する座標に既に高さと尤度が記憶されている場合、次の式でその点の高さと尤度を更新する。なお、下記の式において、μ１，σ１は２次元マップに記憶されている高さと尤度であり、μ２とσ２は計測された高さと尤度であり、μ３，σ３は新しく登録される高さと尤度である。

図６を用いて具体的に説明する。図６において、時刻ｔ１の計測によって計測点Ｐt1の高さが算出され、時刻ｔ２の計測によって計測点Ｐt2の高さが算出されたものとする。この場合、時刻ｔ１の計測によって計測点Ｐt1とその近傍の非計測点Ｐ’t1，Ｐ’’t1，Ｐ’’t1t2の高さと尤度が登録される。次いで、時刻ｔ２の計測によって、計測点Ｐt2とその近傍の非計測点Ｐ’t2，Ｐ’’t2，Ｐ’’t1t2の高さと尤度が計算される。ここで、非計測点Ｐ’’t1t2については、既に時刻t1の計測によって高さと尤度が記憶されている。このため、非計測点Ｐ’’t1t2については、上記の式で高さと尤度を計算する。一方、計測点Ｐt2とその近傍の非計測点Ｐ’t2，Ｐ’’t2については、２次元マップに高さと尤度が記憶されていないため、計算された高さと尤度をそのまま記憶する。

移動可能領域検出部２６は、構造物マップ記憶部３０に記憶されている２次元マップから、移動体１０が移動することができる領域を検出する。具体的には、２次元マップに記憶されている高さ情報μが所定の閾値範囲内の領域（すなわち、床面Ｆの凹凸が小さい領域）であり、かつ、尤度が所定の閾値以上の領域（すなわち、計測がある程度行われた領域）を、移動可能領域として検出する。なお、移動可能領域か否かを判断する基準は、移動体１０の床面Ｆの走行速度に応じて適宜変更することができる。

移動経路決定部２８は、移動可能領域検出部２６で検出された移動可能領域と、目標位置記憶部３２に記憶されている目的地（ｘｆ，ｙｆ）とから、移動体１０の移動経路を決定する。例えば、現在位置と目的地とを結ぶ直線上に移動可能領域があれば、現在位置と目的地を結ぶ直線を移動経路とする。一方、現在位置と目的地とを結ぶ直線上に移動可能領域がなければ、現在位置と目的地とを結ぶ迂回経路を探索する。迂回経路を探索するアルゴリズムには、公知の種々の方法を採ることができる。

モータ駆動部３４は、移動経路決定部２８で決定された経路上を移動体１０が移動するようにモータ１６ａ、１６ｂの駆動量を決定し、その駆動量でモータ１６ａ、１６ｂを駆動する。

次に、上述のように構成される制御装置２０によって行われる処理について説明する。図７は制御装置２０の処理手順を示すフローチャートである。図７に示すように、制御装置２０は、まず、エンコーダ１８ａ、１８ｂとジャイロセンサ３６と距離計測センサ３８を動作させてマップ更新登録処理を行う（Ｓ１０）。

図８はマップ更新登録処理の処理手順を示すフローチャートである。図８に示すように、制御装置２０は、まず、距離計測センサ３８により距離計測を行う（Ｓ２２）。距離計測センサ３８で計測された距離は制御装置２０に入力される。また、距離計測時における距離計測センサ３８の距離計測角度（レーザ光を出射した角度θ（図２））も制御装置２０に入力される。
次に、制御装置２０は、ジャイロセンサ３６により車体４０の姿勢を計測し（Ｓ２４）、エンコーダ１６ａ，１６ｂの出力から車体４０（基準点）のグローバル座標系（絶対座標系）の位置を算出（計測）する（Ｓ２６）。これらの処理によって、ステップＳ２２で距離計測を行った時の車体４０の姿勢（グローバル座標系の姿勢）と車体４０（基準点）の座標（グローバル座標系の座標値）が得られる。
次に、制御装置２０は、ステップＳ２４で得られた車体４０の姿勢と、ステップＳ２６で得られた車体４０（基準点）の座標から、距離計測センサ３８の３次元位置（グローバル座標系の座標値）を算出する（Ｓ２８）。すなわち、車体４０に対する距離計測センサ３８の取付け位置及び取付け角度は既知であるため、ステップＳ２４とＳ２６の計測結果から距離計測センサ３８の３次元位置及び計測方向を求めることができる。
ステップＳ３０に進むと、制御装置２０は、ステップＳ２２で得られた距離計測センサ３８から構造物（床面等）までの距離及び距離計測角度（相対座標系）と、ステップＳ２８で算出された距離計測センサ３８の３次元位置（絶対座標系）及び姿勢（絶対座標系）から、距離計測センサ３８で計測された構造物の３次元位置（グローバル座標系）を算出する。この際、ステップＳ２４で検出された車体４０の姿勢より、距離計測センサ３８の計測角度（相対座標系）が補正され、補正された距離計測角度が用いられる。これによって、計測された構造物の３次元位置が精度良く算出される。
次に、ステップＳ３０で算出された構造物の３次元位置と、尤度マップ記憶部２９に記憶されている尤度マップを用いて、構造物マップ記憶部３０に記憶されている２次元マップを更新及び／又は登録する（Ｓ３２）。これによって、構造物マップ記憶部３０の対応する点及びその近傍の点に高さ情報μと尤度σが格納される。ステップＳ３２が終わると、マップ更新登録処理を終了し、図７のステップＳ１２に戻る。

図７のステップＳ１２に戻ると、制御装置２０は、構造物マップ記憶部３０に記憶されている２次元マップから移動経路が存在するか否かを判定する（Ｓ１２）。すなわち、制御装置２０は、まず、構造物マップ記憶部３０に記憶されている２次元マップの各点のうち、高さが所定の閾値範囲内となり、かつ、尤度が所定の閾値より大きくなる領域を、移動可能領域として検出する。次に、移動体１０の現在位置と目標位置記憶部３２に記憶されている目標位置を結び、その結んだ直線上に移動可能領域が存在する場合は、その直線上の経路を移動経路として選択する。直線上の経路に移動可能領域が存在しない場合は、迂回経路が存在するか否かを探索する。迂回経路が存在する場合は迂回経路を選択し、迂回経路が見つけられない場合は移動経路が存在しないと判断する。

移動経路が存在しない場合（ステップＳ１２でＮＯ）、制御装置２０は、距離計測センサ３８によって距離計測する測定領域を変更し（Ｓ２０）、ステップＳ１０のマップ更新登録処理を再び行う。すなわち、構造物マップ記憶部３０に記憶されている２次元マップのうち尤度の低い領域は十分な計測が行われていない。このため、モータ１６ａ，１６ｂを駆動して車体４０の向きを変更し、尤度の低い領域を再計測する。これによって制御装置２０は、尤度の低い領域に移動経路が存在するか否かを確認する。

移動経路がある場合（ステップＳ１２でＹＥＳ）、制御装置２０は、モータ１６ａ，１６ｂを駆動してステップＳ１２で選択した移動経路を移動する（Ｓ１４）。次いで、制御装置２０は、移動体１０が目標位置（目的地）に到着したか否かを判定する（Ｓ１８）。目標位置に到着している場合（ステップＳ１８でＹＥＳ）は、そのまま処理を終了する。目標位置に到着していない場合（ステップＳ１８でＮＯ）は、ステップＳ１０に戻ってステップＳ１０からの処理を繰り返す。これによって、移動体１０は、自律的に移動経路を選択し、目標位置まで移動することができる。

上述した説明から明らかなように、本実施例の移動体１０では、尤度マップ記憶部２９に記憶されている尤度分布によって、計測点の構造物（床面）の情報（高さμ）から、その計測点近傍の非計測点の構造物（床面）の情報（高さμ）を推定する。このため、非計測点における構造物（床面）を予測して移動経路を決定することができる。例えば、非計測点であっても、その非計測点の周囲に多くの計測点がある場合は、その非計測点の尤度は高くなっている。このため、その非計測点の予測される高さμが所定の閾値範囲内であれば、その非計測点は通過可能領域として検出され、この領域を通過することができる。一方、計測密度が低く尤度の低い領域（非計測点）は通過可能領域として検出されない。このため、移動体１０が検出されていない障害物と衝突してしまうといった事態を避けることができる。
また、本実施例では、距離計測センサ３８として２次元スキャン型の距離計測センサを用いている。２次元スキャン型の距離計測センサは、計測対象が遠くなるほど、計測点間の間隔が長くなる。本実施例では、計測点の構造物の情報から計測点間の構造物の情報を推定するため、計測対象を遠くに設定して計測点間の間隔を長くすることができる。このため、移動体１０の移動速度を高めることができる。

なお、上述した実施例では、距離計測センサ３８の計測面Ｓが一定であったが、計測面ＳをＺ軸方向に揺動可能な構成とすることができる。計測面ＳをＺ軸方向に揺動させることで、移動体１０を停止した状態のまま、移動体１０に近い床面から遠い床面までを計測することができる。これによって、移動体１０の移動経路をより適切に選択することができる。
図９〜１２は、距離計測センサ３８をＺ軸方向に揺動させたときに得られる構造物マップの一例を示している。図９は距離計測センサ３８によって計測した床面の状況を模式的に示している。図９に示すように、床面には複数の凹所Ｈが形成されている。図１０は、尤度を用いてマップを更新することなく得られた、高さ方向の情報のみを持つ構造物マップ（いわゆる、２次元エレベーションマップ）を示している。図１１は、尤度を用いてマップを更新することで得られたマップの尤度（σ）を示している。図１２は、尤度を用いてマップを更新することで得られた構造物マップから移動可能領域を検出した状態を示している。図１２に示すように、床面のうち凹所Ｈが形成された領域と、尤度の低い領域（移動体１０の右前方）が移動不可能領域となっている。
このように距離計測センサ３８をＺ軸方向に揺動させると、移動体１０から離れた位置の床面まで計測することができる。このため、移動体１０から離れた床面の障害物の状況を加味して移動経路を選択できるので、より適切な移動経路を選択することができる。

また、上述した実施例では、車体４０の左前端部の１箇所にのみ距離計測センサ３８を設置したが、移動体１０には複数の距離計測センサを設置することができる。例えば、図１３に示すように、車体４０の左右の前端部にそれぞれ距離計測センサ４２ａ，４２ｂを設置するようにしてもよい。図１３に示すように、距離計測センサ４２ａ，４２ｂは、ロール軸（Ｘ軸）及びピッチ軸（Ｙ軸）周りに傾けられ、移動体の進行方向前方を密に計測できるようになっている。このように複数の距離計測センサを装備することで、床面の状況をより正確に観測することができる。

また、上述した実施例では、尤度を用いた構造物マップのみから移動可能領域を検出したが、本発明はこのような実施例には限られない。例えば、尤度マップを用いていない２次元エレベーションマップ（高さ方向の情報のみを持つ構造物マップ）と、尤度マップを用いた構造物マップ（高さ情報μと尤度σを持つ構造物マップ）の両者を用いて、移動可能領域を検出するようにしてもよい。例えば、２次元エレベーションマップから障害物（床面の突起や凹所等）がある領域を検出し、その領域を移動不可能領域とする。また、尤度マップが用いられた構造物マップから尤度の低い領域を検出し、その領域を移動不可能領域とする。その後、上記２つのマップで移動不可能領域として検出されていない領域を、移動可能領域として検出する。尤度を用いていない２次元エレベーションマップは、計測点のみの情報から作成されているため、床面の障害物（凹所や突起等）を正確に反映したものとなっている。このため、２次元エレベーションマップを用いることで、観測されている障害物（凹所や突起等）を確実に回避することができる。また、尤度の高い領域が移動不可能領域として検出されるため、観測されていない障害物に衝突してしまうことを回避することができる。

また、上述した実施例では、移動体１０の位置と姿勢を計測することで、距離計測センサ３８の３次元位置を特定したが、本発明はこのような例に限らない。例えば、距離計測センサ自体にＧＰＳセンサを取付け、距離計測センサの位置を直接計測するようにしてもよい。また、移動体１０が移動する領域が屋内環境等で床面の傾斜の影響が無視でき、また、移動体の移動速度による影響が無視できるようであれば、移動体１０の姿勢を計測するジャイロセンサを省略することができる。

また、上述した実施例では、移動体１０が自律的に移動経路を選択し、その選択した移動経路を移動するものであったが、本発明は、操作者が操作する移動体（例えば、パワーアシスト型の移動車両）にも適用することができる。このような半自律的な移動体に本発明の技術を適用することで、適切な経路選択をアシストすることができる。

また、上述した実施例では、２次元マップを用いたが、３次元マップを用いることもできる。この場合には、尤度マップは３次元で与えられる。

（第２実施例） 次に、本願の第２実施例の移動体について説明する。第２実施例の移動体は、構造物の計測精度を向上するために、距離計測センサの取付け位置及び取付け姿勢をキャリブレーションする機能を備えていることを特徴としている。
既に説明したように、この移動体では、距離計測センサで計測された構造物までの距離と、距離計測時の移動体の位置及び姿勢と、移動体に対する距離計測センサの取付け位置及び取付け姿勢を用いて、距離計測センサで計測された構造物の絶対位置を算出する。このため、構造物の絶対位置を精度よく算出するためには、距離計測センサの取付け位置及び取付け姿勢を正確に取得する必要がある。特に、距離計測センサで床面の状態を計測しようとすると、移動体に対して距離計測センサが斜めに取付けられる。このため、距離計測センサの取付け姿勢の変化が、構造物の絶対位置算出精度に大きな影響を与える。

ここで、移動体及び距離計測センサの設計値と、移動体に取付ける際の加工精度や組付け精度だけを用いて、距離計測センサの取付け位置及び取付け姿勢を決定すると、構造物の絶対位置を精度よく算出できない。一方、距離計測センサの取付け位置及び取付け姿勢をキャリブレーションにより決定しようとすると、従来の技術では、キャリブレーションのためだけに専用の装置を製作しなければならなかった。

本願の第２実施例は、上記の課題を解決することができるキャリブレーション方法を提供する。このキャリブレーション方法は、移動体に取付けられた距離計測センサの取付け位置及び取付け姿勢のキャリブレーションを行う方法に関する。この方法では、位置決め工程と距離計測工程と回転角計測工程を有している。位置決め工程では、絶対位置が既知のキャリブレーション面に対して予め設定された位置関係となる基準軸に移動体を位置決めする。距離計測工程では、基準軸に対して位置決めされた移動体を基準軸周りに回転させながら距離計測センサでキャリブレーション面までの距離を計測する。回転角計測工程では、距離計測工程において距離を計測した時の移動体の基準軸周りの回転角を計測する。そして、キャリブレーション面内の複数の点について、距離計測工程と回転角計測工程とを行うことで得られた計測結果から、距離計測センサの移動体への取付け位置及び取付け姿勢をキャリブレーションする。
この方法では、絶対位置が既知の基準軸に移動体を位置決めし、その位置から絶対位置が既知のキャリブレーション面までの距離を計測する。このため、計測された「キャリブレーション面までの距離」と「距離計測時の移動体の基準軸周りの回転角」とを用いて、距離計測センサの取付け位置及び取付け姿勢のキャリブレーションを行うことができる。また、キャリブレーション面内の複数の点について計測された結果に基づいてキャリブレーションを行うため、精度よくキャリブレーションを行うことができる。また、移動体を基準軸周りに回転させながらキャリブレーション面までの距離を計測するため、簡易な方法でキャリブレーション面内の多くの点を計測することができる。さらに、単独のターゲット（点）ではなく面をターゲットとするため、距離計測センサによってターゲット（キャリブレーション面）を容易に計測することができる。

なお、上記の回転角計測工程では、移動体の姿勢を計測する移動体姿勢計測センサによって、移動体の基準軸周りの回転角を計測することができる。

また、上記のキャリブレーション面は、互いに交差する複数の平面を有していることが好ましい。この場合、それら複数の平面のなす角が１８０°未満となる側を計測することが好ましい（すなわち、凹となる側の平面を計測することが好ましい）。凹となる側の平面に対して距離計測を行うことで、多くの点を計測することができる。

以下、図面を参照して第２実施例の移動体を詳細に説明する。第２実施例の移動体は、距離計測センサの取付け位置及び取付け姿勢をキャリブレーションする機能を備えている点で第１実施例と相違し、その他の構成（ハードウェア構成及びソフトウェア構成）については第１実施例の移動体１０と同一である。このため、以下の説明では、第１実施例と同一の部分については同一の符号を用いて、その説明を省略する。図１４は本実施例のキャリブレーションの手順を示すフローチャートであり、図１５はキャリブレーションを実施している時の移動体１０の様子を示す図である。

図１４に示すように、制御装置２０は、まず、モータ１６ａ，１６ｂにより車輪１２ａ，１２ｂを駆動して、キャリブレーション面Ｗｘ，Ｗｙ，Ｆに対して予め設定された所定の位置及び姿勢となるように移動体１０を位置決めする（Ｓ４０）。これによって、移動体１０は、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの近傍の予め設定された位置に予め設定された姿勢で位置決めされる（図１５に示す状態）。図１５に示すように、キャリブレーションに使用する面（キャリブレーション面）は、ｘ方向に伸びる壁面Ｗｘと、ｙ方向に伸びる壁面Ｗｙと、床面Ｆによって構成されている。本実施例では、壁面Ｗｘと壁面Ｗｙと床面Ｆのそれぞれは９０°の角度で交差し、壁面Ｗｘと壁面Ｗｙと床面Ｆの凹となる側の面がキャリブレーションに使用される。なお、壁面Ｗｘと壁面Ｗｙと床面Ｆの絶対位置（グローバル座標系（絶対座標系Ｃａ）の形状情報）は既知であり、それらの値はキャリブレーションデータとして制御装置２０内に格納されている。また、移動体１０を位置決めする絶対位置及び絶対姿勢（絶対座標系Ｃａの位置及び姿勢）も、制御装置２０内に格納されている。

次に、制御装置２０は、距離計測センサ３８を作動させて、移動体１０からキャリブレーション面Ｗｘ，Ｗｙ，Ｆまでの距離の計測を開始する（Ｓ４２）。既に説明したように、距離計測センサ３８は２次元スキャン型の距離計測センサである。このため、レーザ光を照射する計測面Ｓが一定となる一方で、レーザ光を照射する方向は設定された角度範囲内を一定の角速度で変化する。従って、距離計測センサ３８は、キャリブレーション面Ｗｘ，Ｗｙ，Ｆと計測面Ｓとの交線Ｌ上の点Ｐ_Ｌ１１，Ｐ_Ｌ１２，Ｐ_Ｌ１３・・の距離を計測することとなる。
なお、距離計測センサ３８は、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの凹側の面を計測するため、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの凸側の面を計測する場合と比較して、隣接する計測点Ｐ_Ｌ１１，Ｐ_Ｌ１２，Ｐ_Ｌ１３・・間の距離が短くなる（すなわち、計測密度が高くなる）。このため、距離計測センサ３８は、キャリブレーション面のＷｘ，Ｗｙ，Ｆの多くの点を計測することができる。
また、距離計測センサ３８は、車体４０に対して、ロール軸（Ｘ軸）周りに所定角度（例えば、３０°）だけ傾けられると共に、ピッチ軸（Ｙ軸）周りに所定角度（例えば、３０°）だけ傾けて取付けられている。このため、計測点Ｐ_Ｌ１１，Ｐ_Ｌ１２，Ｐ_Ｌ１３・・を結んだ直線（すなわち、走査線）Ｌ１，Ｌ２・・は、図１５に示すように、壁面Ｗｘ，Ｗｙ，Ｆを斜めに伸びることとなる。このため、計測点を結ぶ直線Ｌは、複数の面Ｗｘ，Ｗｙ，Ｆに跨る場合が生じる。ただし、図１５では、計測点を結ぶ直線Ｌは複数の壁面Ｗｘ，Ｗｙ，Ｆには跨っていない。

次に、制御装置２０は、モータ１６ａ，１６ｂを駆動して、移動体１０をステップＳ４０で位置決めした位置で自転させる（Ｓ４４）。すなわち、移動体１０では、前輪１４ａ，１４ｂにキャスタ輪が用いられている。このため、右後輪１２ｂと左後輪１２ａを逆方向に等角速度で駆動することで、移動体１０はステップＳ４０で位置決めした位置で自転することとなる。この場合、移動体１０は、右後輪１２ｂと床面Ｆとの接触点と、左後輪１２ａと床面Ｆとの接触点とを結ぶ線分の中点を通る「床面Ｆに垂直な軸線（基準軸）」の周りに回転することとなる。

次に、制御装置２０は、距離計測センサ３８から出力される１走査線分の計測結果を取込む（ステップＳ４６）。なお、距離計測センサ３８が１走査線分の計測を行うのに要する時間は短いため、ステップＳ４６で計測される計測点群はキャリブレーション面Ｗｘ，Ｗｙ，Ｆ上を直線状に位置するとみなすことができる。

次に、制御装置２０は、ステップＳ４６の計測を行っている時の移動体１０の軸線（基準軸）周りの回転角を計測する（Ｓ４８）。具体的には、ステップＳ４６の計測を行っているときのエンコーダ１８ａ，１８ｂの出力を取込む。エンコーダ１８ａ，１８ｂの出力から後輪１２ａ，１２ｂの回転駆動量を求めることができるため、これによって移動体１０の軸線周りの回転角を算出することができる。なお、上述したように、距離計測センサ３８が１走査線分の計測を行うのに要する時間は短いため、本実施例では、１走査線分の距離計測に対して１つの回転角のみが計測される。ただし、計測点毎に移動体１０の軸線周りの回転角を計測してもよいことはいうまでもない。

次いで、制御装置２０は、移動体１０が軸線（基準軸）周りに所定角度だけ回転したか否かを判断する（Ｓ５０）。移動体１０が基準軸周りに所定角度だけ回転していないときは（ステップＳ５０でＮＯ）、ステップＳ４６に戻って、ステップＳ４６からの処理を繰り返す。これによって、制御装置２０には、移動体１０の軸線周りの複数の回転角のそれぞれについて、１走査線分の距離計測結果が格納されることとなる。すなわち、図１５の走査線Ｌ１に対して計測点Ｐ_Ｌ１１，Ｐ_Ｌ１２，Ｐ_Ｌ１３・・の距離計測結果と基準軸周りの回転角が格納され、走査線Ｌ２に対して計測点Ｐ_Ｌ２１，Ｐ_Ｌ２２，Ｐ_Ｌ２３・・の距離計測結果と基準軸周りの回転角が格納され、以下、同様に各走査線について計測点群の距離計測結果と基準軸周りの回転角が格納される。これによって、距離計測センサ３８は、キャリブレーション面Ｗｘ，Ｗｙ，Ｆを３次元的にスキャンすることとなる。

移動体１０が基準軸周りに所定角度だけ回転しているときは（ステップＳ５０でＹＥＳ）、距離計測センサ３８による距離計測を停止すると共に、モータ１６ａ，１６ｂの駆動を停止する。次いで、ステップＳ４６で取込んだ距離計測結果と、ステップＳ４８で取込んだ基準軸周りの回転角とを用いて、距離計測センサ３８の車体４０への取付け位置及び取付け姿勢のキャリブレーションを実施する。
すなわち、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置は、移動体１０の基準位置（基準軸の位置）と、この基準位置から距離計測センサ３８までの距離及び方向と、距離計測センサから各計測点までの距離及び計測方向によって算出することができる。ここで、移動体１０の基準位置は既知である。このため、距離計測センサ３８の取付け位置及び取付け姿勢を仮決定すれば、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置を算出することができる。具体的には、床面Ｆの傾斜角は移動体１０のジャイロセンサ３６で計測できるため、移動体１０の基準位置（基準軸の位置（既知））の絶対座標と、ジャイロセンサ３６で計測された床面Ｆの傾きと、仮決定した距離計測センサ３８の取付け位置及び取付け姿勢とから、距離計測センサ３８の絶対位置及び絶対姿勢を算出する。次いで、算出した距離計測センサ３８の絶対位置及び絶対姿勢と、距離計測センサで計測した距離から、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置を算出する。このように算出されたキャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置は、既知のキャリブレーション面Ｗｘ，Ｗｙ，Ｆの上に位置しなければならない。このため、上記のように算出されるキャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置が、既知のキャリブレーション面Ｗｘ，Ｗｙ，Ｆ上に位置するように、距離計測センサ３８の取付け位置及び取付け姿勢を修正する。これによって、距離計測センサ３８の取付け位置及び取付け姿勢がキャリブレーションされることとなる。上記のキャリブレーションを行う際には、例えば、次の式を用いることができる。

ここで、^{ｗｏｒｌｄ}Ｐijは、キャリブレーション面Ｗｘ，Ｗｙ，Ｆの各計測点の絶対位置である。^{ｗｏｒｌｄ}Ｔrobotは、移動体１０を基準とする座標系を絶対座標系に変換するための行列である。すなわち、床面Ｆの傾きや移動体１０の基準軸周りの回転角を用いて、移動体１０を基準とする座標系を絶対座標系に変換する。^{ｒｏｂｏｔ}Ｔsensorは、距離計測センサ３８を基準とする座標系を、移動体１０を基準とする座標系に変換するための行列である。キャリブレーションにより推定する、移動体１０に対する距離計測センサ３８の取付け位置及び姿勢は^{ｒｏｂｏｔ}Ｔsensor内に格納される。なお、キャリブレーション前においては、^{ｒｏｂｏｔ}Ｔsensorは設計値等に基づいて仮決定された誤差を有する行列である。ΔＴは、距離計測センサ３８の取付け位置及び取付け姿勢の誤差を表す行列であり、^{ｒｏｂｏｔ}Ｔsensorが正しいか否かを評価するための評価行列である。^{ｓｅｎｓｏｒ}Ｐijは、距離計測センサ３８で計測された計測結果からなる行列である。上記の式によって算出される^{ｗｏｒｌｄ}Ｐijが既知のキャリブレーション面Ｗｘ，Ｗｙ，Ｆ上の点となるように、^{ｒｏｂｏｔ}Ｔsensorを最適化する。具体的には、初期値として与えられた^{ｒｏｂｏｔ}Ｔsensorを用いてΔＴを最小二乗法で求める。次いで、^{ｒｏｂｏｔ}Ｔsensorを更新し、更新した^{ｒｏｂｏｔ}Ｔsensorを用いてΔＴを最小二乗法で求める。以下、このような計算を繰り返すことで^{ｒｏｂｏｔ}Ｔsensorが収束する。そして、収束した^{ｒｏｂｏｔ}Ｔsensorがキャリブレーション後の変換行列となる。すなわち、キャリブレーションされた距離計測センサ３８の取付け位置及び取付け姿勢が取得される。

上述した説明から明らかなように、第２実施例の移動体１０では、距離計測センサ３８の取付け位置及び取付け姿勢がキャリブレーションされるため、距離計測センサ３８で計測された構造物の絶対位置を精度よく算出することができる。

また、第２実施例のキャリブレーション方法では、距離計測センサ３８で距離計測するターゲットとして平面Ｗｘ，Ｗｙ，Ｆを用いる。このため、単独のポイント（点）をターゲットとする場合と比較して、距離計測センサ３８によってターゲットまでの距離を容易に計測することができる。特に、移動体１０では、距離計測センサ３８が車体４０に対して傾けて取付けられている。このため、単独のポイントをターゲットとした場合、移動体１０に対してターゲットをどのような位置に配置すればよいかが即座に決定できないことがある。しかしながら、本実施例では、ターゲットとして平面Ｗｘ，Ｗｙ，Ｆを用いるため、このような問題が生じない。
また、本実施例では、ターゲットとして用いる複数の平面Ｗｘ，Ｗｙ，Ｆの凹となる側の面（例えば、部屋の角部）を距離計測センサ３８で計測する。このため、凸となる側の面を計測する場合と比較して、距離計測センサ３８の計測方向が計測対象となる壁面に対して垂直に近い角度となり、また、隣接する計測点の間隔も略等間隔となる。このため、キャリブレーションに用いることができる精度の高い計測結果を多く取得することができる。また、オクルージョンも発生し難くなる。

また、本実施例のキャリブレーション方法では、移動体１０を回転させながら距離計測センサ３８でキャリブレーション面Ｗｘ，Ｗｙ，Ｆまでの距離を計測する。このため、数多くの計測点を簡易な方法により取得することができる。また、数多くの計測結果を用いてキャリブレーションを実施するため、キャリブレーションを精度よく実施することができる。

さらに、本実施例のキャリブレーション方法では、移動体１０の軸周りの回転角や床面Ｆの傾きを、エンコーダ１８ａ，１８ｂ及びジャイロセンサ（慣性センサ）３６の出力を用いて算出する。このため、これらを計測するための計測装置を別途必要としない。

なお、上述した第２実施例では、距離計測センサ３８によりキャリブレーション面Ｗｘ，Ｗｙ，Ｆまでの距離を計測する際に、移動体１０がモータ１６ａ，１６ｂを駆動することで自ら回転した。しかしながら、本願のキャリブレーション方法は、このような実施形態に限られない。
例えば、図１６に示すように、床面Ｆに設けられたターンテーブル６０上に移動体１０を固定し、ターンテーブル６０を回転することで移動体１０を回転させるようにしてもよい。ターンテーブル６０の回転はモータを駆動することで行うこともできるし、作業者が人力で回転させることもできる。ターンテーブル６０を用いることで、移動体１０の基準位置７０をより正確に特定することができ、キャリブレーションを精度よく行うことができる。
なお、ターンテーブル６０を用いる場合には、移動体１０の軸周りの回転角及び移動体１０の傾き（ターンテーブル６０の傾き）を、移動体１０に設けられた慣性センサ（ジャイロセンサ３６）により計測することができる。このため、これらを計測するための専用の計測装置を不要とすることができる。
また、ターンテーブル６０を用いる場合には、ターンテーブル６０の回転軸（図示しない）と移動体１０の車軸が交差するように、移動体１０をターンテーブル６０に固定することができる。これによって、ターンテーブル６０に移動体１０を容易に固定することができる。

なお、上述した各実施例のキャリブレーション方法においては、距離計測センサ３８で計測した計測点のうち、キャリブレーション面Ｗｘ，Ｗｙ，Ｆのうちの２つの壁面が交差する辺の近傍の領域を計測したものについては、キャリブレーションに用いないことが好ましい。例えば、図１６に示す壁面Ｗｘについては、領域Ａ内の計測点のみをキャリブレーションに用い、それ以外の領域（壁面Ｗｘと床面Ｆとが交差する辺の近傍）の点はキャリブレーションに用いない。２つの壁面が交差する近傍の領域を計測した計測点は、２つの壁面のうちどちらの壁面を計測したものか判定し難く、また、誤差が含まれ易い。このため、これらの領域を計測した計測結果をキャリブレーションに用いないことで、キャリブレーションを精度よく行うことができる。

以上、本実施例のキャリブレーション方法について詳細に説明したが、本実施例のキャリブレーション方法は、距離計測センサを搭載する種々の装置に用いることができる。このため、第１実施例に開示した「尤度を利用して構造物の３次元構造を構築する機能」を搭載していない移動体にも適用することができる。
また、上述した実施例は、２次元スキャン型の距離計測センサ３８の取付け位置及び取付け姿勢のキャリブレーションを行う例であったが、本実施例のキャリブレーション方法は３次元スキャン型の距離計測センサの取付け位置及び取付け姿勢をキャリブレーションする場合にも適用することができる。

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

１０：移動体
１２ａ，１２ｂ，１４ａ，１４ｂ：車輪
１６：モータ
１８：エンコーダ
２０：制御装置
３６：ジャイロセンサ
３８：距離計測センサ

Claims (9)

1. 移動体が移動する移動領域内にある構造物の３次元構造を認識する環境認識装置であって、
   移動体に取付けられており、構造物までの距離を計測する距離計測センサと、
   距離計測センサの３次元位置を計測するセンサ位置計測装置と、
   距離計測センサで計測された計測点における構造物の情報から、その計測点の近傍の非計測点における構造物を推定するための尤度を記憶する尤度マップと、
   移動領域内の各点における構造物の情報及びその尤度を記憶するための構造物マップと、
   距離計測センサの計測結果及び計測方向とセンサ位置計測装置の計測結果を用いて、距離計測センサで計測された構造物の３次元位置を算出する位置算出手段と、
   位置算出手段で算出された構造物の３次元位置と記憶されている尤度マップを用いて、その算出された３次元位置に対応する構造物マップ上の対応点における構造物の情報と尤度を更新又は登録すると共に、その対応点の近傍の近傍点における構造物の情報と尤度を更新又は登録するマップ更新手段と、
   を備えている環境認識装置。
2. 前記センサ位置計測装置は、移動体の位置を計測する移動体位置計測センサと、移動体の姿勢を計測する移動体姿勢計測センサを有しており、
   前記位置算出手段は、移動体位置計測センサの検出結果と移動体姿勢計測センサの検出結果を用いて、距離計測時における距離計測センサの３次元位置及び計測方向を特定し、これら特定された距離計測センサの３次元位置及び計測方向と距離計測センサで計測された構造物までの距離から構造物の３次元位置を算出することを特徴とする請求項１に記載の環境認識装置。
3. 構造物マップに記憶されている各点の構造物の情報と尤度に基づいて、移動体の移動可能領域を検出する移動可能領域検出手段をさらに備えていることを特徴とする請求項１又は２に記載の環境認識装置。
4. 移動可能領域検出手段は、構造物マップに記憶されている構造物の情報から障害物がないと判断され、かつ、尤度が所定の閾値以上となる領域を、移動可能領域として検出することを特徴とする請求項３に記載の環境認識装置。
5. 予め設定されたキャリブレーション面内の各点における絶対位置を記憶するキャリブレーション面データと、
   そのキャリブレーションデータを用いて、距離計測センサの移動体への取付け位置及び取付け姿勢をキャリブレーションするキャリブレーション手段をさらに有しており、
   前記キャリブレーション手段は、キャリブレーション面内の複数の点のそれぞれについて得られた（１）距離計測センサにより計測される計測距離と（２）距離計測時の移動体の絶対位置及び絶対姿勢とさらに用いて、距離計測センサの移動体への取付け位置及び取付け姿勢をキャリブレーションすることを特徴とする請求項４に記載の環境認識装置。
6. 前記キャリブレーション手段は、距離計測センサによりキャリブレーション面を２次元的又は３次元的にスキャンすることで得られた複数の計測点の計測距離と、それらの計測点を計測した時の移動体の絶対位置及び絶対姿勢とを用いてキャリブレーションを行うことを特徴とする請求項５に記載の環境認識装置。
7. 前記キャリブレーション面が、互いに交差する複数の平面を有していることを特徴とする請求項５又は６に記載の環境認識装置。
8. 前記センサ位置計測装置は、移動体の位置を計測する移動体位置計測センサと、移動体の姿勢を計測する移動体姿勢計測センサを有しており、
   キャリブレーション手段は、移動体位置計測センサの出力と移動体姿勢計測センサの出力を用いて、距離計測時の移動体の絶対位置及び絶対姿勢を取得することを特徴とする請求項５〜７のいずれか一項に記載の環境認識装置。
9. 請求項１〜８のいずれか一項に記載の環境認識装置を備えていることを特徴とする移動体。