JP5763986B2 - 移動体および移動体の制御方法 - Google Patents

Description

本発明は、周囲の環境を計測することで自身の位置・姿勢を認識しながら移動する移動体および移動体の制御方法に関する。

移動体が環境内を効率的に移動するためには、環境内における位置・姿勢の推定（以下、位置姿勢推定）が不可欠である。移動体自身の位置姿勢推定を行う移動体に関する背景技術として、特許文献１と特許文献２が挙げられる。特許文献１には、環境中に設置された複数の反射体を計測することで、路面の傾斜や凹凸による移動体の傾きを検出し、自身の位置を正確に求める技術ついての開示がある。特許文献２には、環境の地図と車体の移動情報を用いて、センサで実測した場合に得られるセンサデータを予測し、この予測によるセンサデータと実測によるセンサデータの比較によって、位置推定誤りが生じていると判定された場合は、その場での旋回を行い、旋回後にセンサが遮断されてない状態で得られるセンサデータと地図とを用いて位置推定を行うことで、位置推定誤り状態から復帰させる技術について記載されている。

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

特許文献１に示される技術は、再帰反射体と正反射体の既知の反射体を所定の複数場所に設け、移動体のセンサから発射したレーザ光が反射体で反射して戻る正反射光と再帰反光を検出して、移動体の傾きと位置を検出している。しかしながら、通常の環境に事前に再帰反射体と正反射体の専用の反射体を設置することは、準備に手数がかかってコスト的、実用的に困難である。また検出の演算が複雑でありこの点からもコストを要していた。

特許文献２に示される技術は、通常の環境中の一般の物体の幾何形状をセンサによって計測し、計測結果と予め保持する地図とを比較（マッチング）して移動体の地図上での位置・姿勢を推定し、その推定結果に基いて移動体が移動するようにしている。しかし、移動体は、サスペンションなどの移動機構のため、路面の凹凸や加速、減速、停止時に揺れることでセンサが傾くことがある。センサが傾くと、環境中の物体の幾何形状の本来の計測点（場所）を見失う恐れがあり、地図データと物体の計測点とマッチングがとれず、計測誤りが生じて位置姿勢推定が困難となる恐れがある。特許文献２にはこの点について考慮されていない。

本発明は、従来技術の問題点にかんがみ、センサの傾きに因む計測誤りの影響を低減して、より正確な位置・姿勢の推定が行える、移動体および移動体の制御方法を提供することを目的とする。

上記課題を解決するため、本発明は、距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体において、
前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
計測点のセンサからの距離に応じて、前記距離センサ制御部で得られた幾何形状データと地図データの一方のマッチング対象のサイズを設定する距離センサ誤差低減部と、
前記距離センサ誤差低減部で設定されたマッチング対象のサイズで幾何形状データと地図データをマッチング処理して移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、計測点のセンサからの距離が長いほどマッチング対象のサイズを大きく設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、幾何形状データをなす計測点毎に、マッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記距離センサ制御部で得られた幾何形状データのマッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記距離センサ制御部で得られた幾何形状データのマッチング対象の形状をセンサの誤差に合わせて楕円形状に設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記地図データのマッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記地図データのマッチング対象である物体存在画素の範囲を設定することを特徴とする。

また、上記に記載の移動体において、前記距離センサ誤差低減部は、移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を幾何形状データから除去することを特徴とする。

上記課題を解決するため、本発明は、距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体の制御方法において、
距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、
距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点のセンサからの距離に応じて設定し、
位置姿勢推定部により前記で設定されたマッチング対象のサイズで幾何形状データと地図データをマッチング処理して移動体の位置と姿勢を推定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、計測点のセンサからの距離が長いほどマッチング対象のサイズを大きく設定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、幾何形状データをなす計測点毎に、マッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記距離センサ制御部で得られた幾何形状データのマッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記距離センサ制御部で得られた幾何形状データのマッチング対象の形状をセンサの誤差に合わせて楕円形状に設定されることを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記地図データのマッチング対象のサイズを設定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記地図データのマッチング対象である物体存在画素の範囲を設定することを特徴とする。

また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を幾何形状データから除去することを特徴とする。

本発明によれば、センサの傾きに因む計測誤りの影響を低減して、より正確な位置姿勢推定が行える。

本発明の実施例の機能の構成図である。 同じくハードウェア・ソフトウェア構成図である。 同じく距離センサにより環境中の物体を計測する説明図である。 本発明の実施例１の動作フロー図である。 レーザスポット光の例を示す説明図である。 地図データへの幾何形状データのマッチングの例を示す説明図である。 本発明の実施例１のマッチングウインドウサイズの設定の例を示す説明図である。 本発明の実施例２のマッチングの例を示す説明図である。 図７の一部を拡大した説明図である。 図８の一部を拡大した説明図である。 本発明の実施例１の変形例を示す説明図である。

以下、図面を用いて本発明の実施例について説明する。移動体の具体例としてはロボットを用いている。

また、前述の説明では、各部の間で出力、入力、受信、送信されたり、参照されたり、記憶されたり、設定される情報、データ、信号等を説明によっては、異なる表現としているが、その文言によって、限定されるものではない。例えば、「値（設定値、検出値、補正値、出力値など）」として、説明していても、データにのみ限定されるものではなく、情報、信号として扱い、表現しても同様の意味を表すものとする。これは、以下の説明においても、同様とする。

また、本明細書において、「計測」として、説明をしている。しかし、この「計測」に限定されるものではなく、一般に「測定」として、説明されること、「測定」を行う構成としても、若しくは、表現しても同様の意味を表すものとする。

ここでは、まず本実施例で想定するロボットの機能の構成・主な処理内容について述べ、続いて、ロボットのより具体的なハードウェア・ソフトウェア構成と、これらの動作について述べる。

ロボットの機能の構成を図１に示す。ロボット（移動体）０１０１は、コントローラ部０１０２、距離センサ部０１０３、移動機構部０１０９を有している。コントローラ部０１０２は、距離センサ制御部０１０４、距離センサ誤差低減部０１０５、位置姿勢推定部０１０６、経路計画部０１０７、移動機構制御部０１０８、距離センサ設置データ０１１０、距離センサスポット光サイズデータ０１１１、地図データ０１１２、経路データ０１１３から構成される。各データは後述するように記憶装置に保持されている。また、ここでは図示していないが、各部を支持する筐体、電源・配線など、各部が連携し、動作するために必要なものは備わっているものとする。

まず、ロボット０１０１が自動走行する際に行われる処理について述べる。ロボット０１０１は、距離センサ制御部０１０４により距離センサ部０１０３を制御し、ロボット０１０１から周囲の環境中の物体までの距離と方向からなる距離データを、計測する。ここでは距離センサ部０１０３として、レーザ距離センサを用いるものとする。

このレーザ距離センサは、レーザを物体に照射してから、照射したレーザが物体によって反射してセンサに返ってくるまでの時間を計測することで、センサから環境中の物体までの距離の計測を行うレーザ照射部（図示せず）を備えている。このレーザ照射部を一定の回転角毎に回転させながら計測することで、一定の回転角度範囲内にある物体までの距離の計測（以下、スキャン）が可能であるとする。このスキャンにより、スキャンによってレーザがなす平面（以下、スキャン面）上にある物体のまでの距離と方向が得られる。

これらのデータの組を距離データと呼ぶものとする。この距離データの１つ１つは距離と方向のデータから位置のデータに変換することができる。このようにして距離データを位置のデータに変換したものを幾何形状データと呼ぶものとする。また、幾何形状データをなす１つ１つの位置のデータは、レーザスポット光が環境中の物体に到達した位置、つまりは点の位置を表すがこの点を計測点と呼ぶものとする。ここで、このレーザ距離センサのスキャン面が床面に平行となるようにロボットに取り付けられているものとすると、レーザ距離センサのスキャン面の高さでの幾何形状がデータとして得られることとなる。

上記のようにして、距離センサ制御部０１０４によって距離データから得られた幾何形状データは、距離センサ誤差低減部０１０５に送られる。距離センサ誤差低減部０１０５には、予め距離センサ設置データ０１１０と距離センサスポット光サイズデータ０１１１が読み込まれているものとする。

距離センサ設置データ０１１０とは、ロボットに対する距離センサ部０１０３の位置・姿勢と床面からの高さを示すデータである。また、距離センサスポット光サイズデータ０１１１とは、距離センサから環境中の物体に向けて照射されるレーザなどのスポット光が距離に応じてどのように広がるかを示すデータである。より具体的には、ここでは、センサからの計測距離毎のスポット光の直径が記録されているものとする。これらのデータを用いて、距離センサ誤差低減部０１０５では、まずスポット光割れセンサデータの除去が行われ、続いて、距離データをなす距離値毎に後述の位置姿勢推定のマッチング判定基準となるマッチングウインドウサイズの設定が行われる。

まず、スポット光割れセンサデータの除去について述べる。

スポット光割れセンサデータの除去は、前述の距離センサ設置データ０１１０と距離センサスポット光サイズデータ０１１１を用いて行われる。より具体的には、センサが傾いてないとの仮定のもと、距離センサ部０１０３より照射されたレーザスポット光が距離に応じて広がって床面と交わる際の距離を求め、幾何形状データをなす多数の計測点のうち、この距離よりも遠い位置にある計測点の幾何形状データについては、床面などを誤計測しているものとみなして、幾何形状データから除去する。

続いて、幾何形状データをなす計測点毎にマッチングウインドウサイズ（マッチング対象のサイズ）の設定が行われる。まず、ここでのマッチングとしては、画像として表現された幾何形状データと、予めスキャン面の高さでの環境の幾何形状を画像として扱えるようにした地図データ０１１２を重ね合せたときに、物体が存在することを示す画素（以下、物体存在画素）同士が最も重なり合うときの地図データ上での幾何形状データの位置・姿勢を探索によって求める方式を想定する。また、マッチングウインドウ（マッチング対象）とは、幾何形状データの画像における物体存在画素（幾何形状データをなす計測点に相当）を中心に設けられる枠を指す。このマッチングウインドウは、幾何形状データと地図データとを重ね合わせたときのマッチングの判定基準として用いられる。

具体的には、幾何形状データをなす計測点に設けたマッチングウインドウと重なる範囲にある地図データの画素に、物体存在画素が含まれる場合、その計測点は地図データに対してマッチしたとみなすというふうに用いられる。よって、ウインドウサイズが大きくなるほど、幾何形状データをなす計測点と地図データとを重ね合わせたときのずれが許容されることとなる。距離センサ誤差低減部０１０５では、幾何形状データをなす計測点のセンサからの距離が大きい（長い）ほど、マッチングウインドウサイズも大きく設定する。

距離センサ部０１０３が傾いた状態（スキャン面が床面に対して平行でない状態）で計測を行うと、センサが傾いてない状態（スキャン面が床面に対して平行な状態）での計測に比べて大きな距離値が得られる。今、センサが傾いてない状態で得られる幾何形状データのマッチングを前提として、床面に平行かつ傾いてないセンサのスキャン面に対応するように地図データが作られているとする。この状況において、マッチングウインドウサイズが距離によらず一定の大きさだとすると、センサが傾いてないときには地図データに重なり合う計測点であっても、センサが傾いているときには、地図データに重なりにくく、つまりはマッチしなくなる。これは、計測点のセンサからの距離が大きいほど計測点のずれが大きくなるため、マッチングしづらくなる。

これに対して、本実施例１は前述の通り、幾何形状データをなす計測点のセンサからの距離が大きいほど、マッチングウインドウサイズが大きくなるように設定すると、本来、地図データと重なり合うべき計測点が、地図データ上で本来重なり合うべき点に対して少々ずれても、ウインドウサイズの範囲内に収まり、つまりはマッチする可能性が上がり、センサの傾きに伴うマッチング誤りが低減されることとなる。以上のようにして、マッチングウインドウサイズの大きさの変更によって、マッチングの判定基準を緩める設定を行い、距離センサの傾きにより計測に誤差があってもマッチング誤差の低減、つまりは位置姿勢推定の誤りを低減することができる。

距離に応じてマッチングウインドウのサイズが設定された幾何形状データは、位置姿勢推定部０１０６に送られる。位置姿勢推定部０１０６では、予め地図データ０１１２が読み込まれており、幾何形状データと地図データ０１１２とのマッチングが行われ、幾何形状データと地図データ０１１２とが重なる割合が最大のときの幾何形状データの位置・姿勢が求まる。地図データ０１１２上に定められた座標系における幾何形状データの始点の位置と姿勢つまりは距離センサ部０１０３の始点の位置と姿勢が求められる。ロボットの位置・姿勢を表す際にはロボットの筐体のどこを基準としてもよいため、ここでは求められた距離センサ部０１０３の位置・姿勢をもってロボット０１０１の位置・姿勢とする。

経路計画部０１０７は、ロボットの通りうる通路の長さや方向が記録された経路グラフデータ０１１３を予め読み込んでおき、位置姿勢推定部０１０６で得られた現在のロボットの位置・姿勢をもとに目的地までの経路、および経路と現在位置とのずれを求める。

そして、移動機構制御部０１０８では、ロボットと追従しようとしている経路とのずれを小さくするように移動機構部０１０９の制御する。すなわち、車輪の回転速度やステアリングの切れ角などを求めてモータなどへの指示が行われる。これらにより、ロボットの経路への追従、ひいては目的地までの自動走行が実現される。以上が、ロボットを構成する機能毎に行われる処理の概要となる。

続いて、まず、ロボットのハードウェアとソフトウェアの構成について述べ、ロボットの距離センサが傾いた状態で計測することで得られた幾何形状データを用いて、位置姿勢推定を行う例を通して、ハードウェアとソフトウェア全体の処理の流れについて述べる。

ロボットのハードウェアとこれに格納されるソフトウェアの構成を図２に示す。ロボット０２０１（０１０１に相当）は、位置姿勢推定コントローラ０２０２、レーザ距離センサ０２０７（０１０３に相当）、移動機構０２０８（０１０９に相当）、ディスプレイ０２０９、入力機器０２１０、これらの機器同士が通信するための通信線０２０５より構成される。なお、図２では処理の流れに直接関わる要素のみを表記しており、各要素の動作に必要な電源等は当然備わっているものとする。

レーザ距離センサ０２０７には、前述の距離センサ部０１０４で挙げたレーザ距離と同じ方式のセンサを用いるものとする。ここでは、例として、レーザ距離センサ０２０７がスキャンする角度範囲を１８０度とし、この角度範囲において０．５度毎にレーザを照射し、物体までの距離を計測することを想定するが、スキャンする角度範囲やレーザを照射する角度の刻み幅、距離の最大計測範囲などは異なっていてもよい。

図３にロボットに取り付けられたレーザ距離センサにより、環境中の物体を計測する様子を示す。同図は環境とロボット０３０５（図２のロボット０２０１に相当）を上から見下ろした様子を表す平面図である。また、環境中の物体０３０１の壁面は床面に対して垂直であり、凹凸がないものとする。例として図中の位置・姿勢にあるロボット０３０５がレーザ距離センサ０３０６（図２のレーザ距離センサ０２０７に相当）により１８０度の角度範囲０３０９をスキャンしたとすると、幾何形状データ０３０８（ドットで示す）、赤外線を面状に物体に照射することで画素毎の物体までの距離計測が可能なデプスカメラなどであってもよい。

移動機構部０２０８としては、図３に示されるロボット０３０５を想定する。同ロボット０３０５には、前部にキャスタが、また後部に駆動輪が備えられており、駆動輪の回転角速度の差を制御することで直進・旋回を行えるようになっているものとする。本実施例ではこのような移動機構の使用を想定しているが、同様に環境内を移動する効果が得られるのであれば移動機構の方式は異なっていてもよい。例えば、無限軌道を備える車両、脚を備えるロボット、船舶、航空機、飛行船など他の移動機構であってもよい。また、本実施例においては、ロボットが自動走行するようになっているが、人が搭乗してロボットを操縦するようになっていてもよいし、また、搭乗せずに遠隔からの通信によって人が操縦するようになっていてもよい。

また、位置姿勢推定コントローラ０２０２は、プロセッサ０２０３、メモリ０２０４、記憶装置０２０６、ＯＳ０２１１、ＢＩＯＳ読み込みやＯＳの起動を行うコントローラ初期化プログラム０２１２、レーザ距離センサ０２０７から距離データを取得するレーザ距離センサ制御プログラム０２１３、幾何形状データの中から無効なデータを除去すると共に、マッチングウインドウの大きさを設定する、レーザ距離センサ誤差低減プログラム０２１４、幾何形状データと地図データ０２２０とのマッチングによって、位置・姿勢を算出する位置姿勢推定プログラム０２１５、経路グラフデータ０２２１をもとに目的地に辿り着くための経路を算出する経路計画プログラム０２１６、経路に沿って車体が移動するように車輪の回転速度などを算出する移動機構制御プログラム０２１７から構成される。

また、記憶装置０２０６には、ロボットに対する距離センサの設置位置・姿勢や床面からの高さを表すレーザ距離センサ設置データ０２１８、レーザ距離センサより照射されるレーザスポット光が距離に応じて広がる際の距離毎の直径を表すレーザ距離センサスポット光サイズデータ０２１９、位置姿勢推定プログラム０２１５がマッチングに用いる地図データ０２２０、環境内でロボットが通る経路の長さや方向を表す経路グラフデータ０２２１が記録されている。

なお、図２のプログラムやデータは、メモリにロードされた上で、プロセッサにより処理されることを想定しているが、これと同様の効果が得られるのであれば、実装は異なっていてもよい。例えば、ＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇｒｉｄ Ａｒｒａｙ）やＣＰＬＤ（Ｃｏｍｐｌｅｘ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）などのプログラマブルなハードウェアで以上の処理を実現してもよい。なお、プログラムやデータは、ＣＤ−ＲＯＭ等の記憶媒体から移してもよいし、ネットワーク経由で他の装置からダウンロードしてもよい。

また、プロセッサや記憶装置、移動機構など、ロボット０２０１を構成する各デバイスは、ここでは有線の通信線０２０５により互いに通信することを想定しているが、無線であってもよく、また、通信が可能であれば、コントローラ０２０２、ディスプレイ０２０８、入力装置０２１０の各デバイスが物理的に遠隔にあってもよい。また、以上のハードウェアやソフトウェアは、実施形態に応じて、取捨選択してもよい。

続いて、ロボット０２０１で行われる処理の流れについて述べる。起動されたコントローラ０２０２の処理の流れを図４に示す。

コントローラ０２０２が起動されると（０４０１）、コントローラ初期化プログラム０２１２により、ＯＳ０２１１の読み込み、各プログラム０２１２〜０２１７の起動が行われる（０４０２）。次いで、目的地の設定を終了するかどうかの確認画面がディスプレイ０２０９に表示される。ロボット０２０１に自動走行を行わせる場合、操作者は目的地の設定を行うことを入力装置０２１０により選択する（０４０３）。もし、ロボット０２０１の自動走行を行わないのであれば終了を選択する。この場合、直ちにプログラムは終了となる（０４０４）。今、ロボットに自動走行を行わせるため、操作者が目的地の設定を行う方を選択したとすると、処理０４０５に処理が進む。

続いて、操作者は、ディスプレイ０２０９に表示される搬送先の候補のリスト上で目的地を確認し、入力装置０２１０により目的地を設定する（０４０５）。
次に、レーザ距離センサ誤差低減プログラム０２１４により、レーザ距離センサ設置データ０２１８が読み込まれる（０４０６）。このデータには前述のとおり、ロボット０２０１に対するレーザ距離センサ０２０７の設置位置・姿勢や床面からの高さが記録されている。

次に、同じくレーザ距離センサ誤差低減プログラム０２１４により、レーザ距離センサスポット光サイズデータ０２１９が読み込まれる（０４０７）。このデータには前述のとおり、距離センサから環境中の物体に向けて照射されるレーザなどのスポット光が、距離に応じてどのように広がるかが記録されている。ここでは、具体的には、センサからの計測距離毎のスポット光の直径が記録されている。

次に、位置姿勢推定プログラム０２１５により、地図データ０２２０が読み込まれる（０４０８）。なお、ここでの地図データ０２２０は画像データとなっており、画素毎に環境中の物体の有無が記録されているものとする。

次に、経路計画プログラム０２１６により、経路グラフデータ０２２１が読み込まれる（０４０９）。なお、ここでの経路グラフデータ０２２１には、環境内でロボットが通る経路の長さや方向が記録されているものとする。

続いて、レーザ距離センサ制御プログラム０２１３によりレーザ距離センサ０２０７から距離データを取得する（０４１０）。距離データをなす距離と方向より位置が求められ、物体の存在の有無を画素に記録して画像とした幾何形状データに変換される。

レーザ距離センサ制御プログラム０２１３により変換された幾何形状データは、距離センサ誤差低減プログラム０２１４に送られる。レーザ距離センサ誤差低減プログラム０２１４には前述の通り、レーザ距離センサ設置データ０２１８とレーザ距離センサスポット光サイズデータ０２１９が読み込まれている。これらのデータを用いることで、レーザ距離センサ誤差低減プログラム０２１４では、まずスポット光割れセンサデータの除去が行われる（０４１１）。

ここでは、レーザ距離センサ０２０７が傾いてないとの仮定のもと、レーザ距離センサ０２０７より照射されたレーザスポット光が距離に応じて広がって床面と交わる際の距離（以下、床面交差距離）を求め、幾何形状データをなす各計測点のうち、センサから計測点までの距離が床面交差距離よりも大きい計測点については、床面などを誤計測しているものとみなして、幾何形状データから除去する。

具体的には、今、レーザ距離センサ設置データ０２１８として、ロボット０２０１に対するレーザ距離センサ０２０７の設置位置・姿勢と、レーザ距離センサ０２０７の床面からの設置高さが、図５のような状態にあることが記録されており、かつ、レーザスポット光が距離に応じて図５のように広がることが記録されているとする。この場合、レーザスポット光０５０４は、図中の点０５０６で床面０５０５と交わることがわかる。このレーザスポット光０５０４と、床面０５０５とが交わる点０５０６までのセンサからの距離０５０７が床面交差距離である。

図５のレーザスポット光０５０４によって環境中の物体０５０８までの距離を計測する場合、このレーザスポット光０５０４は図５のとおり、床面０５０５と交差するため、計測値には一定の誤差が含まれることとなる。このため、この計測点のデータは除去するものとする。

続いて、計測点毎に幾何形状データへのマッチングウインドウサイズの設定が行われる（０４１２）。処理０４１２において、幾何形状データをなす計測点のセンサからの距離が大きいほど、幾何形状データのマッチングウインドウ（マッチング対象）のサイズを大きく設定しておくことで、この後の処理０４１４でのマッチング誤りを抑制し、正確な位置・姿勢の算出（推定）が行われる。

ここでのマッチングウインドウの前提として、次の処理０４１４の位置姿勢推定で行われるマッチングについてまず述べる。ここでのマッチングとは、幾何形状データと地図データ０２２０を重ね合せて、物体存在画素同士が最も重なり合うときの地図データ０２２０上での幾何形状データの位置・姿勢を探索によって求めることである。地図データ０２２０への幾何形状データのマッチングの様子を図６に示す。

図６の地図データ０６０１（図２の地図データ０２２０に相当）は、図３の環境に対応している。ここで、地図データの座標系０６０８上でのロボットの位置・姿勢を求める処理について述べる。ロボットが図６の位置・姿勢にて計測した際に得られる幾何形状データ０６０２が、前回の位置姿勢推定結果０６０６を中心とする図中の点線による一定の領域内において取りうるすべての位置・姿勢で、地図データ０６０１に対して重ね合わせ、物体存在画素が重なる割合を算出する処理を行っていく。これにより、例えば、幾何形状データ０６０３の始点の位置・姿勢（上方にずれている）０６０７では、地図データ０６０１と重なる割合は低いため、解の候補から外され、重なる割合がより高くなる幾何形状データ０６０２の始点の位置・姿勢０６０４解として求められることとなる。求められた座標系０６０８上での幾何形状データの位置・姿勢をロボットの位置・姿勢とする。

ここで、前述の物体存在画素が重なっているかどうかの判定基準について、図７および図７の一部を拡大して示す図９を用いて説明する。今、レーザ距離センサ０７２０（図２のレーザ距離センサ０２０７に相当）が、傾いてない状態で計測して得られる幾何形状データ０７０２を、地図データ０７０１にマッチングする場合を考える。マッチングにおける探索時の位置・姿勢の解の候補として、幾何形状データ０７０２が０７０３の位置・姿勢で計測されたものとして地図データ０７０１と重ね合わせられたとする。このとき、幾何形状データ０７０２と地図データ０７０１とが重ね合わせられた範囲０７０４を拡大した図を０７０５に示す。

拡大図０７０５において、ハッチングがかかっている画素０７０７は、幾何形状データ０７０２をなす計測点である。つまりは、ハッチングがかかっている画素０７０７が、幾何形状データ０７０２の物体存在画素である。また、白い画素０７０８と黒い画素０７０６は、地図データ０７０１の画素をなす部分であり、白い画素０７０８は物体が存在していないことを示しており、黒い画素０７０６は物体が存在することを示している。つまりは、黒い画素０７０６が地図データ０７０１の物体存在画素である。

幾何形状データ０７０２が図７の位置・姿勢にあるとき、例えば計測点の上記物体存在画素０７０７は、地図データ０７０１の物体存在画素０７０６重なっているため、マッチしたと判定される。マッチングウインドウサイズとは、幾何形状データ０７０２の物体存在画素（ハッチング部分）を中心に一定の大きさを持つ枠である。幾何形状データ０７０２の物体存在画素０７０７では、マッチングウインドウサイズはこの計測点０７０７の画素と同じ大きさとしている。

今、レーザ距離センサ０７２０が傾いた状態で計測を行ったとすると、センサからの距離が短い計測点よりも、距離が長い計測点ほど、センサが傾いてないときの距離との差が大きくなる。このセンサの傾きを考慮せずに、幾何形状データを地図データにマッチングしようとすると、センサからの距離が長い計測点、つまりは幾何形状データの物体存在画素のうち、センサからの距離が長い物体存在画素ほど地図データにマッチしなくなる。図７を例とすると、幾何形状データ０７０２の物体存在画素０７０７の周辺の拡大図０７０５は、センサが傾いた状態で計測が行われたとしても、センサ０７２０から距離が短い計測点部分０７０４であるため、傾いてないときの計測点との距離の差は小さいため、マッチングウインドウサイズが小さくても、幾何形状データ０７０２の物体存在画素０７０７は地図データ０７０１の物体存在画素０７０６にマッチする。

これに対して、センサが傾いた状態で計測が行われた場合、センサから遠い計測点ほど、傾いてないときの計測点との距離の差は大きくなるため、マッチングウインドウサイズが小さいままだと、幾何形状データ０７０２の物体存在画素０７０７は地図データ０７０１の物体存在画素０７０６にマッチしなくなる。

例えば、計測点がセンサ０７２０から遠く離れ位置にある場合について説明する。センサが傾いてない状態では、幾何形状データ０７０２と地図データ０７０１とが重なり合う範囲０７１１において、センサが傾いてないときは、０７１０のように幾何形状データ０７０２が地図データ０７０１にマッチする。しかし、センサが傾いているときは０７０９のように、幾何形状データ０７０２が実際のセンサから物体までの水平での距離（センサが傾いてないとき）よりも大きくでるため、地図データ０７０１にマッチしなくなる。このマッチしない部分を拡大したものを０７１５に示す。０７１５では、マッチングウインドウサイズが画素単位の１×１で小さく設定されている。幾何形状データ０７０２の物体存在画素０７１３（ハッチング部分）が、地図データ０７０１の物体存在画素０７１４（黒い画素）に重なっておらず、マッチしていないことがわかる。

このマッチング誤りに対しては、マッチングウインドウサイズの拡大で対応する。具体的には、拡大部分０７１６における、幾何形状データ０７０２の物体存在画素０７１９（ハッチング部分）の周囲に設けたマッチングウインドウを、破線の０７１７で示したようにサイズを３×３に大きく設定する。そして、大きく設定されたサイズのマッチングウインドウ０７１７の範囲に、地図データ０７０１の物体存在画素０７１９がある場合はマッチしたと判定するようにする。

これにより、センサの傾きに因む、幾何形状データの地図データに対するずれが許容され、マッチング誤りが低減される。以上のように処理０４１２において、レーザ距離センサ誤差低減プログラム０２１４によって、幾何形状データをなす計測点のセンサからの距離が大きくなるほど、幾何形状データのマッチングウインドウのサイズを大きく設定する処理が行われる。例えば、計測点のセンサからの距離が大きくなるに従って、サイズが（１×１）→（２×２）→（３×３）のように設定される。なお、このマッチングウインドウサイズは、幾何形状データをなす計測点のセンサからの距離に単純に比例させてもよいが、予め設定された距離の閾値を用いて、この距離の閾値を越えたらマッチングウインドウサイズを一定としてもよい。これにより、マッチングウインドウサイズが大きくなりすぎることでのマッチング誤りを低減するようにしてもよい。

また、マッチングウインドウの形状は、ここでは単純に縦横比が同じ正方形としているが、形状は問わない。例えば、丸や長方形でもよい。また、レーザ距離センサによる計測の誤差の特性を考慮してもよい。例えば図１１のように、レーザ距離センサのレーザスポット光の始点から幾何形状データをなす計測点とを結ぶ直線１１０１に沿い、かつ計測点１１０４の誤差の範囲を表す楕円１１０３をマッチングウインドウとして設けるものとし、レーザ距離センサの奥行き方向の計測誤差より楕円の軸１１０２方向の大きさ１１０６を設定し、また、レーザスポット光の広がりやレーザスポット光を投光する方位の制御に因む計測誤差より楕円の軸１１０７方向の大きさ１１０５を設定し、マッチングウインドウとしてもよい。もちろん、楕円でなく、レーザ距離センサのレーザスポット光の始点から幾何形状データをなす計測点とを結ぶ直線１１０１に沿う四角形などであってもよい。

このように、レーザ距離センサ誤差低減プログラム０２１４により、処理０４１１と０４１２でマッチング誤りの低減処理が行われる。

続いて、位置姿勢推定プログラム０２１５によって、幾何形状データと地図データ０２２０とのマッチングにもとづく位置・姿勢の推定が行われる（０４１４）。ここでは前述の通り、画像状に表現された幾何形状データと、同じく画像状に表現された地図データ０２２０を重ね合わせ、幾何形状データの中で物体存在画素が、地図データ０２２０上で物体存在画素と重なる割合が最大となるときの、地図データ０２２０上での幾何形状データの位置・姿勢、つまりはレーザ距離センサ０２０７の位置・姿勢が求められる。ここではこのレーザ距離センサ０２０７の位置・姿勢をロボット０２０１の位置・姿勢とする。なお、マッチングの手法としては、同様に地図データに対する幾何形状データの位置・姿勢を求める効果が得られるのであればマッチングの方式は異なっていてもよい。

次にロボットが目的地に到着しているか否かの判定を行う（０４１５）。これは、前述の位置姿勢推定で得られた位置が、目的地の座標から一定の距離内に入っているかどうかの判定によって行う。位置姿勢推定で得られた位置と目的地との距離が閾値以下の場合は、目的地に到着したものと判定され、処理０４０５に戻り、次の目的地までの自動走行まで指示待ちとなる。位置姿勢推定で得られた位置と目的地との距離が閾値より大きい場合は、目的地に到着していないものと判定され処理０４１７に処理が進む。

ロボットが目的地にまだ到着してない場合、経路計画プログラム０２１６によって、現在位置・姿勢をもとに、目的地までの移動経路が求められる。図３の環境において、移動開始位置０３０２にあるロボット０２０１の目的地として、０３０７が設定されたとすると、経路計画プログラム０２１６は、起動時に予め読み込んだ経路グラフデータ０２２１を用いて経路探索を行い、局所的移動経路０３０４が求められる（０４１７）。

続いて、移動機構制御プログラム０２１７により、経路計画で得られた経路に車体を移動させるための車輪の回転速度が算出され、また、車輪の回転速度が所定の値となるように移動機構０２０８のモータに対する電流値の指示が出される（０４１６）。

ロボットが移動していき、目的地に到着したと判定されたならば（０４１５）、前述の通り、目的地の設定確認の処理０４０３に戻る。以上が、ロボットが通常動作を行っている際の処理の流れとなる。

実施例１では、処理０４１２において、幾何形状データをなす計測点のセンサからの距離が大きいほど、幾何形状データへのマッチングウインドウ（マッチング対象）のサイズを大きく設定しておくことで、処理０４１３でのマッチング誤りを抑制し、センサの傾きの位置姿勢推定への影響を低減する方式について述べた。ここでは同様の効果を得るための別の方式について述べる。

本実施例２では、実施例１のように幾何形状データへのマッチングウインドウサイズ（マッチング対象のサイズ）を大きくすることはせず、代わりに地図データのマッチング対象である物体存在画素の範囲（サイズ）を大きくする。つまり、低解像度化して物体存在画素を大きくする（膨張処理あるいは太線化処理する）ことで、計測点がずれた幾何形状データでも、地図データの物体存在画素にマッチし易くすることで、マッチング誤りを低減している。より具体的には、解像度の異なる地図データを複数準備しておき、幾何形状データをなす計測点のセンサからの距離が大きいほど、低解像度な（大きな）地図データを選択して、選択された地図データと幾何形状データのマッチングを行うようにする。

図８およびその一部を拡大して示す図１０を用いて、解像度の異なる地図データを用いての、マッチング誤りを防ぐ様子を示す。センサが傾いていないならば、幾何形状データは０８０１のように地図データにマッチするが、センサが傾いた状態では、０８０２のように地図データに対してずれる。これは実施例１の説明で、図７を参照して説明したとおりである。ここで、ずれた０８０２の幾何形状データが、低解像度な地図データに対してマッチする様子を０８０４に示す。

０８０４において、白い画素０８０６は地図データの物体が存在しない画素、黒い画素０８０７は地図データの物体存在画素、ハッチングがかかっている画素０８０５は幾何形状データの物体存在画素である。もとの地図データの物体存在画素は、図７の０７１９のように１画素の幅の線であったが、ここでは地図データを低解像度化として３画素の幅の線に変更されている。この画素の変更により、幾何形状データのウインドウサイズが１画素であっても、物体存在画素０８０５は地図データの物体存在画素０８０７に重なり、マッチしたと判定されることとなる。

以上の処理を用いれば、本実施例２は、実施例１と同様にセンサの傾きのマッチングへの影響を低減することが可能となる。本実施例２の動作フローとしては、図４で処理０４０８、０４１２、０４１４を変更すればよい。

まず、処理０４０８において、解像度の異なる複数の地図データの読み込みを行う。解像度の異なる（マッチング対象のサイズの異なる）地図データとしては、実際の画素数を減らしてもよいし、画素数はそのままに、物体存在画素のある領域を膨張させて実質的な解像度を落としてもよい。処理０４１２を図４のフローから省き、処理０４１４の処理を変更する。すなわち、変更した処理０４１４では、センサからの距離が大きい計測点のマッチングには、複数の地図データから低解像度な地図データを選択した上で、マッチングを行うようにする。このように実施例２では、レーザ距離センサ誤差低減プログラム０２１４により、変更された処理０４１４でマッチング誤りの低減処理が行われる。

センサの傾きによって生じうるマッチング誤りに対し、実施例１では幾何形状データのマッチングウインドウサイズ（マッチング対象のサイズ）の変更により対応し、実施例２では低解像度にすることにより、地図データのマッチング対象のサイズ変更により対応する。

両実施例は効果が同じであるが、実施例１では、マッチングの都度、ウインドウサイズが変わり、このウインドウサイズの範囲でマッチしているか否かの判定を行う必要がある。幾何形状データにセンサから遠く離れた計測点が多く含まれるほど、大きなウインドウサイズでのマッチング判定を行うこととなり、計算量が増えることとなるが、実施例２のように複数の地図データを準備する必要がないため、使用するメモリ量は少なくて済む。

一方、実施例２では、解像度の異なる地図データをマッチングの都度生成するのであれば計算量が増えることとなるが、予め複数の解像度の異なる地図データを用意しておけば、計算量は少なくなり、処理速度を速めることができる。

なお、幾何形状データと地図データの両方のマッチング対象のサイズを変更しても良いが、サイズ設定の制御が複雑となるので、一方のみで行うのが好ましい。

０１０１、０２０１…ロボット、移動体、０１０２…コントローラ部、０１０３…距離センサ部、０１０４…距離センサ制御部、０１０５…距離センサ誤差低減部、０１０６…位置姿勢推定部、０１０７…経路計画部、０１０８…移動機構制御部、０１０９…移動機構制御部、０１１０…距離センサ設置データ、０１１１…距離センサスポット光サイズデータ、０１１２…地図データ、０１１３…経路グラフデータ、０７１７、０８０７…マッチング対象。

Claims (6)

1. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体において、
   前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
   計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記計測点の前記距離センサからの距離が長いほど前記マッチング対象のサイズを大きく設定する距離センサ誤差低減部と、
   前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。
2. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体において、
   前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
   計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記距離センサ制御部で得られた前記幾何形状データのマッチング対象の形状を前記距離センサの誤差に合わせて楕円形状に設定する距離センサ誤差低減部と、
   前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。
3. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体において、
   前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
   計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を前記幾何形状データから除去する距離センサ誤差低減部と、
   前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。
4. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体の制御方法において、
   距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、
   距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記計測点の前記距離センサからの距離が長いほど前記マッチング対象のサイズを大きく設定し、
   位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。
5. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体の制御方法において、
   距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、
   距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記距離センサ制御部で得られた前記幾何形状データのマッチング対象の形状を前記距離センサの誤差に合わせて楕円形状に設定し、
   位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。
6. 距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体の制御方法において、
   距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、
   距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を前記幾何形状データから除去し、
   位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。

Images