JP2010061484A

JP2010061484A - 移動体および移動体の位置推定誤り状態からの復帰方法

Info

Publication number: JP2010061484A
Application number: JP2008227744A
Authority: JP
Inventors: Kosai Matsumoto; 高斉松本; Toshio Moriya; 俊夫守屋; Ryozo Masaki; 良三正木
Original assignee: Hitachi Industrial Equipment Systems Co Ltd
Current assignee: Hitachi Industrial Equipment Systems Co Ltd
Priority date: 2008-09-05
Filing date: 2008-09-05
Publication date: 2010-03-18
Anticipated expiration: 2028-09-05
Also published as: JP5276931B2

Abstract

【課題】
位置推定誤りが生じる状態に陥った場合に、自ら位置推定可能な状態に復帰する移動体を構築することを目的とする。
【解決手段】
環境の地図と車体の移動情報を用いて、センサで実測した場合に得られるセンサデータを予測し、この予測によるセンサデータと実測によるセンサデータの比較によって、位置推定誤りが生じていると判定された場合は、その場での旋回を行い、旋回後にセンサが遮蔽されてない状態で得られるセンサデータと地図とを用いて位置推定を行うことで、位置推定誤り状態から復帰させる。
【選択図】図３

Description

本発明は、位置推定誤りの状態に陥った際に、自身で位置推定可能な状態に復帰する移動体およびその位置推定誤り状態からの復帰方法に関する。

特許文献１では、位置推定誤り状態に陥った場合に、環境に設置されたRFIDタグを読み取り、予め登録しておいたRFIDタグのIDと位置とを関連付けの情報を用いておよその位置を認識し、さらに周辺のランドマークをステレオカメラで認識することによって、位置推定誤りの状態から位置推定が可能な状態に復帰するロボットが提案されている。

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

特許文献１の手法は、環境中に多数のRFIDタグを予め設置し、また、その座標を計測するコストが必要となる。

本発明の目的は、RFIDタグの設置など無しに、移動体を位置推定誤り状態から位置推定可能な状態に復帰させることが可能な装置および方法を提供することである。

本発明は、移動体によって、位置推定誤りが検出された際には、移動体にその場で旋回させ、旋回後にセンサを用いた環境の計測を行わせ、この計測によって得られるセンサデータと地図を用いての位置推定を行わせることで、移動体を位置推定誤り状態から位置推定可能な状態に復帰させる。

本発明は、位置推定誤りが検出された場合には、その場で旋回する。

本発明は、位置推定に誤りがあると判定された場合には、位置推定に誤りが生じる前の推定位置と地図の情報を用いて、センサから周囲の環境に存在する障害物までの距離が大きくなる方向に、その場で旋回する。

本発明は、マッチング処理結果が予め定めた範囲内でない場合に、前回のタイミングで推定された移動体の地図上での位置および姿勢から今回の移動体の位置および姿勢を推定し、今回の計測結果が今回の移動体の位置および姿勢にて得られたものとして、計測結果と地図との差分をとることによって両者で一致しない領域を検出し、一致しない領域の領域間の距離に基づいてクラスタリング処理を行い、求めたクラスタのうち最大のクラスタの姿勢をセンサのセンシング範囲に含まれない姿勢として検出し、前記センサのセンシング範囲に含まれない姿勢のうち、今回の移動体の位置および姿勢にて旋回した場合に得られるセンサデータのシミュレーションに基づいて移動体から環境に存在する障害物までの距離が大きいセンサデータが得られるときの姿勢を旋回後の移動体の姿勢として検出する。

本発明により、RFIDタグのように、位置推定誤りからの復帰を補助する機器を環境中に予め設置すること無しに、位置推定誤り状態にある移動体を位置推定可能な状態に復帰させることが可能となる。

ここでは、センサによる計測が遮られることによって、位置推定誤りが生じても、自ら位置推定が可能な状態に復帰して、自律移動を行う移動体の実施例について述べる。

本実施例で用いる移動体の構成を図１に示す。移動体１００１は、プロセッサ１００２、メモリ１００３、キーボード１００４、ビデオカード１００５、ディスプレイ１００６、通信線１００７、レーザー距離センサ１００８、移動機構１００９、エンコーダ１０１０、カメラ１０１１、記憶装置１０１２から構成される。本実施例では、移動機構として、2つの駆動輪とキャスターを備え、駆動輪の回転角速度と回転方向を制御することで、移動体のその場での旋回を可能とする移動機構を想定しているが、同様の効果が得られるのであれば、無限軌道や脚、船舶、航空機、各種ロボットなど、移動機構の形態は問わない。また、本実施例では、環境の幾何形状を計測するセンサとして、水平方向にレーザーを走査しながら、照射したレーザー光が障害物から反射して戻る時間の計測によって障害物までの距離を算出し、2次元水平面における物体の幾何形状を計測するレーザー距離センサを想定している。しかし、この他のセンサとして、例えば、水平方向以外も走査することで物体の3次元の幾何形状を計測するレーザー距離センサ、画像特徴の三角測量によって物体の3次元の幾何形状を計測するステレオカメラ、あるいは、同様に幾何形状の計測が可能な超音波センサなど、幾何形状が計測可能であればそのセンサの形態は問わない。また、本実施例では、それぞれの構成要素が有線の通信線にて接続されていることを想定しているが、通信が可能であれば無線であってもよい。また、本実施例における移動体の構成要素のうち、特定の要素のみが物理的に遠隔にあってもよい。センサによるセンシング範囲は、移動体の周囲３６０°ではなく、限られた角度（例えば、１８０°）であってもよい。

記憶装置１０１２には、ＯＳ１０１３、全体制御部１０１４、レーザー距離センサ１００８を用いて幾何データを取得するレーザー距離センサ値取得部１０１５、移動機構に取り付けられたエンコーダ１０１０による車輪の回転データを取得するエンコーダ値取得部１０１６、経路追従や旋回動作のための車輪の回転制御を行う移動機構制御部１０１７、ある時刻における車輪の回転速度より次の時刻に到達する移動体の位置と姿勢（向き）を推定する到達予定位置推定部１０１８、レーザー距離センサ１００８による幾何データを用いて移動体の位置と姿勢を求める現在位置推定部１０１９、現在の位置・姿勢と地図とを用いることで目的地までの経路を算出する経路計画部１０２０、センサが人などによって遮られて位置推定誤りが生じているかどうかを判定する位置推定誤り判定部１０２１、位置推定結果を用いて環境の地図を更新する地図更新部１０２３、ビデオカード１００５を介して地図や移動体の位置の画像をディスプレイ１００６に表示する表示部１０２４、位置推定に用いる地図データ１０２５が格納されている。なお、本実施例では、プロセッサ１００２が、記憶装置１０１２のプログラムをメモリ１００３にロードして処理として実行することを想定しているが、これと同等の働きをするのであれば、FPGA（Field Programmable Grid Array）やCPLD（Complex Programmable Logic Device）などのプログラマブルなハードウェアで実現してもよい。また、前述のプログラムやデータは、CD-ROM等の記憶媒体から移してもよいし、ネットワーク経由で他の装置からダウンロードしてもよい。また、以上のハードウェアやソフトウェアは、実施形態に応じて、取捨選択してもよい。

以上のような構成の移動体が、環境内を自律移動するものとする。図２は、移動体が移動する環境とその移動経路を示す図である。移動体２００９は、レーザー距離センサ２０１０を用いて、周辺の棚や壁などの障害物２００１を計測して位置推定を行い、初期位置２００９から目的地２００２までの自律移動を行うものとする。この移動の際に、環境中に居合わせた人間２００３によって、移動体のレーザー距離センサ２０１０による計測が遮られ、位置推定に誤りが生じた場合に、位置推定が可能な状態に復帰し、自律移動を続ける際に移動体２００９が行う一連の処理を図３に示す。

キーボード１００４からの入力によりプログラムが開始されると（３００１）、まず、プログラム全体の処理を統括する全体制御部１０１４によって、地図データ１０２４が読み込まれる（３００２）。この地図データ１０２４は、図４の４００１のように、環境の幾何形状を表すものとなっている。

続いて、レーザー距離センサ制御部１０１５により、初期位置２０１１におけるレーザー距離センサデータの取得が行われる（３００３）。レーザー距離センサデータには、レーザーを走査した際に、レーザースポット光が障害物に当たった点の座標が記録されている。例えば、位置２００４でレーザーの走査２０１４を行ったとするとスポット光の反射した点群２０１３のセンサ座標系における座標値が得られる。

移動体２００９は、初期位置２０１１でのレーザー距離センサデータを地図データ４００１にマッチングすることによって、自身の初期位置２０１１の推定を行う（３００４）。ここでのマッチングとは、レーザー距離センサデータと地図データ４００１の幾何的な特徴が最もよく重なる位置と姿勢を算出することを指す。本実施例では、マッチングの手法として、ICP（Iterative Closest Point）の利用を想定するが、同様の効果が得られるならばマッチングの手法は問わない。このマッチングにより、初期位置２０１１における移動体２００９の位置・姿勢が算出される。この移動体の位置と姿勢の算出を、以下では単に位置推定と呼ぶ。なお、本実施例での位置推定としては、２次元の位置推定を想定しているが、３次元計測が可能なセンサによるデータを用いて３次元での位置推定を行い、これを位置推定値として用いてもよい。

続いて、求めた初期位置２０１１と地図データ４００１を用いて、目的地２００２までの経路計画を行う（３００５）。この時点では、人間２００３は環境内にいない状況であり、経路２００８が求められたものとする。

経路２００８に沿って移動するため、移動体２００９は、移動機構制御部１０１５によって、車輪を回転制御して、移動する（３００６）。このときの移動機構の制御情報をもとに、移動体は、レーザー距離センサデータを次回取得する際に自身が到達する位置と姿勢を推定する（３００７）。以下では、この移動体の到達予定の位置と姿勢を合わせて、単に到達予定位置と呼ぶ。

次に、推定した到達予定位置と地図データ４００１から、到達予定位置において移動体がレーザー距離センサによる計測を行った場合に得られるセンサデータを算出する（３００８）。

次に、移動体２００９が移動して実際に到達した位置にて、レーザー距離センサ２０１０による計測を行う（３００９）。このとき、移動機構に取り付けられたエンコーダ１０１０での計測にもとづくオドメトリデータもレーザー距離センサデータと同時に取得される。オドメトリとは、車輪移動機構を備える移動体やロボットにおいて、左右の車輪の回転速度から、移動体の並進速度と移動体本体の角速度を求め、それを積分しロボットの位置と姿勢を求める方法である。なお、ここでのオドメトリは、移動体の初期値からの絶対位置・姿勢ではなく、前回レーザー距離センサデータを用いて位置推定を行ったときからの移動量（相対的な位置・姿勢）をさすものとする。

次に、移動体のセンサが人などに遮蔽されるなどして位置推定誤りが生じているかどうかの判定を行う（３０１２）。この判定は、処理３００８によって算出した到達予定位置におけるレーザー距離センサデータと、処理３００９によって実際の到達位置で取得したレーザー距離センサデータのマッチングによって行われる。マッチングの手法は問わないが、ここでは、前述のICPを用いてマッチングを行うことを想定する。ICPを用いたマッチングにより、幾何的特徴が最も重なり合うときのセンサデータの相対位置と姿勢とマッチング評価値が得られる。ここでのマッチング評価値とは、センサデータ同士の幾何的特徴の重なり合いの程度を表す値である。ここでは、ICPで定義されるところの最近傍の点群の点間距離の総和をマッチング評価値とし、評価値が小さいほどマッチングがよいとみなす。今、人間２００３によってセンサが遮蔽された状態で、移動体が位置２００４で、環境を実測することで得られるセンサデータ５００２と、処理３００９で予測したセンサデータ５００１があるとき、ICPを用いたマッチング５００３によって、マッチング結果５００４が得られる。このマッチングにより、センサデータ同士が重なってない部分がどの程度かを示す量として、マッチング評価値が得られる。このマッチング評価値が予め設定した閾値よりも小さい（位置推定誤りがない範囲内）場合は、位置推定誤りが生じていないものと判定して、位置推定処理３０１３に処理を進め、閾値よりも大きい（位置推定誤りがない範囲外）場合は位置推定誤りが生じているものと判定して、復帰動作３０１８に処理を進める（３０１１）。閾値よりも大きい場合は移動体の移動を停止してから復帰動作３０１８の処理を行ってもよい。

まず、処理３０１１で、マッチング評価値が閾値より小さいと判定された場合に行われる位置推定３０１２について述べる。ここでは、初期位置の推定３００４と同様にレーザー距離センサデータと地図データ４００１とのマッチングが行われ、移動体の位置・姿勢が求められる（３０１３）。また、この移動体の位置・姿勢にレーザー距離センサデータを座標変換、すなわちセンサ座標系のデータを地図の座標系に変換し、地図データ４００１に書き込むことで、地図の更新を行う（３０１４）。更新した地図と移動体の位置、計画した経路、目的地などは、表示部１０２４により画像として生成され、ビデオカード１００８を介してディスプレイ１００９に表示される。続いて、移動体の位置より、目的地に移動体が到着したかどうかの判定がなされ（３０１５）、到着と判定されればプログラムは終了となり（３０１６）、到着していないと判定されれば、現在位置から目的地までの経路計画が行われ（３００５）、目的地に到着するまでの間、移動が繰り返される。

次に、処理３０１１で、マッチング評価値が閾値より大きいと判定された場合、すなわち位置推定に誤りが生じていると判定された場合に、移動体が位置推定可能な状態に復帰するために行う処理３０１７について述べる。処理３０１７は、復帰動作計画部１０２２によって行われる。以下では、その一連の処理について述べる。マッチング評価値が閾値より大きい場合、ここでは、センサが人などの地図にない障害物によって遮蔽されるなどして、位置推定誤りが生じていると判定し、この位置推定誤り状態から位置推定可能な状態へと復帰する動作を行う。このため、まず、周期的なタイミングにおける前回のタイミング（所定時間前）でのレーザー距離センサデータを用いた推定位置に処理３００９によるオドメトリデータによる相対移動量を加算することで、オドメトリデータにもとづくおよその現在位置と姿勢を推定する（３０１８）。続いて、推定された位置・姿勢をもとに、実測のレーザー距離センサデータを地図の座標系に変換し、レーザー距離センサデータから地図と重なり合う部分を引くことにより、差分をとる（つまりセンサデータと地図との不一致箇所を特定する）（３０１９）。差分は、両者が一致していない位置の座標で表しても良い。続いて、レーザー距離センサデータと地図との差分によって残った領域（つまりセンサデータと地図とが一致していない領域）について、領域間の距離にもとづいたクラスタリングを行う（３０２０）。クラスタリング手法としては、自己組織化マップやK平均など、何を用いても良い。求めたクラスタのうち、分布が最大のクラスタ、すなわちセンサを遮蔽している人間が存在していると思われる領域がセンシング範囲に入らないように、処理３０１８で求めた現在位置での旋回と旋回時に得られるレーザー距離センサデータのシミュレーションを行う。シミュレーションによって得られる、旋回時の姿勢と姿勢毎に得られるセンサデータを用い、センサから障害物までの距離の平均値が最大となるときのセンサデータを求め、これに対応する搬送車２００９の姿勢２００５を求める。処理３０１８で求められている搬送車の姿勢から、姿勢２００５に旋回する際の旋回角度が小さくなる方の旋回方向２００６と旋回角度を求め（３０２１）、この旋回角度と旋回方向となるように車体をその場で旋回させる（３０２２）。

なお、旋回角度が小さくなる方の旋回方向の算出は必須ではない。つまり、時計周りなど旋回方向を予め定めておいてもよい。また、センサを遮蔽している人間が存在していると思われる領域がセンシング範囲に入らないのが好ましいが、人間が存在していると思われる領域がセンシング範囲に入っていても、マッチング評価値が閾値より小さくなる姿勢があればよい。

なお、ここでの車体の旋回としては、超信地旋回、信地旋回や車体が円や弧を描くような旋回など、いずれでもよい。旋回後は、センサが人間によって遮られた状態から脱した状態にあることが期待されるため、レーザー距離センサデータをあらためて取得し、そのデータを用いての位置推定を行い、位置推定誤りの状態から復帰する。位置推定誤りから復帰できた場合は、目的地へ移動する一連の処理に戻る。

以上の実施例では、移動体２００９のレーザー距離センサ２０１０によって、センサが遮蔽されていることを検出しているが、レーザー距離センサ以外のセンサとして、例えばカメラ１０１１により取得される画像から、センサが遮蔽されていることを検出しても良い。

また、以上の実施例では、位置推定誤りの判定によって、その場での旋回を行うかどうかを決定していたが、電源を切るなどして、位置がまったく不明な移動体が電源が入れられるなどして、センサや車体を駆動できる状態となったときには、位置推定が正しいか誤っているかに関わらず、超信地旋回して得られたデータをもとに、初期位置推定を行ってもよい。

また、以上の実施例では、位置推定誤りと判定された場合にのみ、その場での旋回を行っていたが、位置推定誤りの有無に関わらず、環境の複数の地点において旋回するものとして、旋回時に得られるセンサデータ群を1組として、センサの計測範囲を見かけ上広げることで、センサデータの組同士のマッチングの精度を向上とこれに伴う地図作成精度の向上のために旋回を用いてもよい。

本実施例により、人に囲まれるなどして、位置推定が困難となっても、位置推定が可能な状態に自ら復帰し、自律移動を継続する移動体が実現可能となる。また、位置推定が可能な状態に復帰するために移動体が行う動作は、その場での旋回のみであり、移動を伴わないため、人や物体への衝突なしに、安全に位置推定が可能な状態に復帰できるという効果がある。

装置の構成を示す図。環境と自律移動の様子を示す図。処理の流れを示す図。地図データを示す図。予測によるセンサデータと実測によるセンサデータのマッチングを示す図。

符号の説明

１００１…移動体、１００２…プロセッサ、１００３…メモリ、１００４…キーボード、１００５…ビデオカード、１００６…ディスプレイ、１００７…通信線、１００８…レーザー距離センサ、１００９…移動機構、１０１０…エンコーダ、１０１１…カメラ、１０１２…記憶装置、１０１３…ＯＳ、１０１４…全体制御部、１０１５…レーザー距離センサ値取得部、１０１６…エンコーダ値取得部、１０１７…移動機構制御部、１０１８…到達予定位置推定部、１０１９…現在位置推定部、１０２０…経路計画部、１０２１…位置推定誤り判定部、１０２２…復帰動作計画部、１０２３…地図更新部、１０２４…表示部、１０２５…地図データ

Claims

センサによる移動体の周囲の環境の計測によって移動体の位置と姿勢とを推定し、この位置推定結果をもとに目的地に向かうための車体制御を行う移動体において、
位置推定誤りを検出する検出部を備え、
前記検出部によって、位置推定誤りが検出された場合には、その場で旋回することを特徴とする移動体。
センサによる移動体の周囲の環境の周期的な計測によって移動体の位置と姿勢とを推定し、この位置推定結果をもとに移動する移動体において、
位置推定に誤りがあると判定された場合には、前記位置推定に誤りが生じる前の推定位置と地図の情報を用いて、前記センサから周囲の環境に存在する障害物までの距離が大きくなる方向に、その場で旋回することを特徴とする移動体。
移動体周辺の環境の幾何形状を計測可能なセンサによる幾何形状計測部と、
前記幾何形状計測部より得られる環境の幾何形状データと環境の地図との照合により、移動体の位置と姿勢とを推定する位置推定部と、
前記位置推定誤りを検出する検出部と、
前記位置推定部による位置推定結果と前記幾何形状データをもとに環境の地図を更新する地図更新部と、
複数の車輪の操舵により任意経路への追従の他に、その場での旋回が可能な移動機構部を備えることを特徴とする移動体。
請求項３記載の移動体であって、
前記地図と前記幾何形状データを用いて自律的に移動することを特徴とする移動体。
センサによる移動体の周囲の環境の計測によって移動体の位置と姿勢とを推定し、この位置推定結果をもとに移動する移動体の位置推定誤り状態からの復帰方法において、
位置推定誤りが検出された場合には、その場で前記移動体を旋回することによって、前記位置推定が可能な状態に復帰することを特徴とする移動体の位置推定誤り状態からの復帰方法。
移動体の周囲の環境を周期的に計測し、その計測結果と予め保持する地図とを比較することにより移動体の前記地図上での位置および姿勢を推定し、その推定結果に基づいて目的地へ移動する移動体において、
前記計測結果と前記地図とのマッチング処理を行う処理部と、
前記マッチング処理結果が予め定めた範囲内である場合に、マッチングした前記地図に基づいて前記移動体の前記地図上での位置および姿勢を推定し、前記マッチング処理結果が予め定めた範囲内でない場合に、前回のタイミングで推定された前記移動体の前記地図上での位置および姿勢から今回の移動体の位置および姿勢を推定し、今回の計測結果が前記今回の移動体の位置および姿勢にて得られたものとして、計測結果と地図との差分をとることによって両者で一致しない領域を検出し、一致しない領域の領域間の距離に基づいてクラスタリング処理を行い、求めたクラスタのうち最大のクラスタの姿勢を、センサのセンシング範囲に含まれない姿勢として検出し、前記センサのセンシング範囲に含まれない姿勢のうち、前記今回の移動体の位置および姿勢にて旋回した場合に得られるセンサデータのシミュレーションに基づいて前記移動体から前記環境に存在する障害物までの距離が大きいセンサデータが得られるときの姿勢を旋回後の前記移動体の姿勢として検出する処理部と、
検出された前記移動体の姿勢へ前記移動体が旋回するように指示する処理部とを備えることを特徴とする移動体。
移動体の周囲の環境を周期的に計測し、その計測結果と予め保持する地図とを比較することにより移動体の前記地図上での位置および姿勢を推定し、その推定結果に基づいて目的地へ移動する移動体の位置推定誤り状態からの復帰方法において、
前記計測結果と前記地図とのマッチング処理を行い、
前記マッチング処理結果が予め定めた範囲内である場合に、マッチングした前記地図に基づいて前記移動体の前記地図上での位置および姿勢を推定し、
前記マッチング処理結果が予め定めた範囲内でない場合に、前回のタイミングで推定された前記移動体の前記地図上での位置および姿勢から今回の移動体の位置および姿勢を推定し、今回の計測結果が前記今回の移動体の位置および姿勢にて得られたものとして、計測結果と地図との差分をとることによって両者で一致しない領域を検出し、一致しない領域の領域間の距離に基づいてクラスタリング処理を行い、求めたクラスタのうち最大のクラスタの姿勢をセンサのセンシング範囲に含まれない姿勢として検出し、前記センサのセンシング範囲に含まれない姿勢のうち、前記今回の移動体の位置および姿勢にて旋回した場合に得られるセンサデータのシミュレーションに基づいて前記移動体から前記環境に存在する障害物までの距離が大きいセンサデータが得られるときの姿勢を旋回後の前記移動体の姿勢として検出し、
検出された前記移動体の姿勢へ前記移動体が旋回するように指示することを特徴とする移動体の位置推定誤り状態からの復帰方法。