JP6816830B2 - 位置推定システム、および当該位置推定システムを備える移動体 - Google Patents

Description

本開示は、位置推定システム、および当該位置推定システムを備える移動体に関する。

無人搬送車（無人搬送台車）および移動ロボットのように自律移動可能な移動体の開発が進められている。

特開２００８−２５０９０５号公報は、レーザレンジファインダから取得した局所地図と、前もって用意された地図とのマッチングによって自己位置推定を行う移動ロボットを開示している。

特開２００８−２５０９０５号公報

特開２００８−２５０９０５号公報に開示されている移動ロボットは、マッチングを行うとき、環境地図から不要な点を除去して自己位置の推定を行っている。

本開示の実施形態は、地図作成時の演算量を低減できる位置推定システムおよび移動体を提供する。

本開示の位置推定システムは、非限定的で例示的な実施形態において、環境をスキャンしてスキャンデータを周期的に出力する外界センサに接続されて使用される位置推定システムであって、プロセッサと、前記プロセッサを動作させるコンピュータプログラムを記憶するメモリとを備えている。前記プロセッサは、前記コンピュータプログラムの指令にしたがって、前記外界センサから前記スキャンデータを取得し、前記スキャンデータから参照地図を作成すること、前記外界センサから前記スキャンデータを新たに取得したとき、新たに取得した最新スキャンデータと前記参照地図とのマッチングを行うことにより、前記参照地図上における前記外界センサの位置および姿勢を推定し、前記最新スキャンデータを前記参照地図に追加して前記参照地図を更新すること、複数回更新された参照地図から、前記最新スキャンデータを含む一部分以外の部分を削除して、前記参照地図のリセットを行うこと、および、前記リセットを行うとき、前記リセット前の前記複数回更新された参照地図に基づいて環境地図を更新することを実行する。

本開示の移動体は、非限定的で例示的な実施形態において、上記の位置推定システムと、外界センサと、前記位置推定システムが作成した前記環境地図を記憶する記憶装置と、移動のための駆動装置とを備えている。

本開示のコンピュータプログラムは、非限定的で例示的な実施形態において、上記いずれかの位置推定システムに使用されるコンピュータプログラムである。

本開示の実施形態によれば、環境地図を作成するとき、外界センサから周期的に出力される複数のスキャンデータのマッチングを少ない演算量で実行することが可能になる。

図１は、本開示による移動体の実施形態の構成を示す図である。 図２は、移動体が移動する環境の例を模式的に示す平面レイアウト図である。 図３は、図２に示される環境の環境地図を示す図である。 図４Ａは、外界センサが時刻ｔに取得したスキャンデータＳＤ（ｔ）の例を模式的に示す図である。 図４Ｂは、外界センサが時刻ｔ＋Δｔに取得したスキャンデータＳＤ（ｔ＋Δｔ）の例を模式的に示す図である。 図４Ｃは、スキャンデータＳＤ（ｔ）にスキャンデータＳＤ（ｔ＋Δｔ）をマッチングさせた状態を模式的に示す図である。 図５は、スキャンデータを構成する点群が初期の位置から回転および並進して、環境地図の点群に近づく様子を模式的に示す図である。 図６は、スキャンデータの剛体変換後の位置および姿勢を示す図である。 図７Ａは、外界センサからスキャンデータを取得し、スキャンデータから参照地図を作成した後、新たに取得したスキャンデータを参照地図にマッチングさせた状態を模式的に示す図である。 図７Ｂは、新たに取得したスキャンデータを図７Ａの参照地図に追加して更新した参照地図を模式的に示す図である。 図７Ｃは、新たに取得したスキャンデータを図７Ｂの参照地図に追加して更新した参照地図を模式的に示す図である。 図８Ａは、更新前の環境地図を模式的に示す図である。 図８Ｂは、参照地図に基づいて環境地図を更新するときの様子を示す図である。 図８Ｃは、参照地図と環境地図とのマッチングを行い、参照地図を環境地図に整合させた状態を模式的に示す図である。 図９Ａは、外界センサが時刻ｔに取得したスキャンデータＳＤ（ｔ）の例を模式的に示す図である。 図９Ｂは、環境地図Ｍに対するスキャンデータＳＤ（ｔ）のマッチングを開始するときの状態を模式的に示す図である。 図９Ｃは、環境地図Ｍに対するスキャンデータＳＤ（ｔ）のマッチングが完了した状態を模式的に示す図である。 図１０は、過去に得られた移動体の位置および姿勢の履歴と、現在の位置および姿勢の予測値を模式的に示す図である。 図１１は、本開示の実施形態における位置推定装置の動作の一部を示すフローチャートである。 図１２は、本開示の実施形態における位置推定装置の動作の一部を示すフローチャートである。 図１３は、本開示の実施形態における位置推定装置の動作の他の例を示すフローチャートである。 図１４は、本開示による、各ＡＧＶの走行を制御する制御システムの概要を示す図である。 図１５は、ＡＧＶが存在する環境の一例を示す斜視図である。 図１６は、接続される前のＡＧＶおよび牽引台車を示す斜視図である。 図１７は、接続されたＡＧＶおよび牽引台車を示す斜視図である。 図１８は、本実施形態にかかる例示的なＡＧＶの外観図である。 図１９Ａは、ＡＧＶの第１のハードウェア構成例を示す図である。 図１９Ｂは、ＡＧＶの第２のハードウェア構成例を示す図である。 図２０は、運行管理装置のハードウェア構成例を示す図である。

＜用語＞
「無人搬送車」（ＡＧＶ）とは、本体に人手または自動で荷物を積み込み、指示された場所まで自動走行し、人手または自動で荷卸しをする無軌道車両を意味する。「無人搬送車」は、無人牽引車および無人フォークリフトを含む。

「無人」の用語は、車両の操舵に人を必要としないことを意味しており、無人搬送車が「人（たとえば荷物の積み下ろしを行う者）」を搬送することは除外しない。

「無人牽引車」とは、人手または自動で荷物の積み込み荷卸しをする台車を牽引して、指示された場所まで自動走行する無軌道車両である。

「無人フォークリフト」とは、荷物移載用のフォークなどを上下させるマストを備え、フォークなどに荷物を自動移載し指示された場所まで自動走行し、自動荷役作業をする無軌道車両である。

「無軌道車両」とは、車輪と、車輪を回転させる電気モータまたはエンジンを備える移動体（vehicle）である。

「移動体」とは、人または荷物を載せて移動する装置であり、移動のための駆動力（traction）を発生させる車輪、二足または多足歩行装置、プロペラなどの駆動装置を備える。本開示における「移動体」の用語は、狭義の無人搬送車のみならず、モバイルロボット、サービスロボット、およびドローンを含む。

「自動走行」は、無人搬送車が通信によって接続されるコンピュータの運行管理システムの指令に基づく走行と、無人搬送車が備える制御装置による自律的走行とを含む。自律的走行には、無人搬送車が所定の経路に沿って目的地に向かう走行のみならず、追尾目標に追従する走行も含まれる。また、無人搬送車は、一時的に作業者の指示に基づくマニュアル走行を行ってもよい。「自動走行」は、一般には「ガイド式」の走行および「ガイドレス式」の走行の両方を含むが、本開示では「ガイドレス式」の走行を意味する。

「ガイド式」とは、誘導体を連続的または断続的に設置し、誘導体を利用して無人搬送車を誘導する方式である。

「ガイドレス式」とは、誘導体を設置せずに誘導する方式である。本開示の実施形態における無人搬送車は、位置推定装置を備え、ガイドレス式で走行することができる。

「位置推定装置」は、レーザレンジファインダなどの外界センサによって取得されたセンサデータに基づいて環境地図上における自己位置を推定する装置である。

「外界センサ」は、移動体の外部の状態をセンシングするセンサである。外界センサには、たとえば、レーザレンジファインダ（測域センサともいう）、カメラ（またはイメージセンサ）、ＬＩＤＡＲ（Light Detection and Ranging）、ミリ波レーダ、超音波センサ、および磁気センサがある。

「内界センサ」は、移動体の内部の状態をセンシングするセンサである。内界センサには、たとえばロータリエンコーダ（以下、単に「エンコーダ」と称することがある）、加速度センサ、および角加速度センサ（たとえばジャイロセンサ）がある。

「SLAM（スラム）」とは、Simultaneous Localization and Mappingの略語であり、自己位置推定と環境地図作成を同時に行うことを意味する。

＜本開示における移動体の基本構成＞
図１を参照する。本開示の移動体１０は、図１に示される例示的な実施形態において、環境をスキャンしてスキャンデータを周期的に出力する外界センサ１０２を備える。外界センサ１０２の典型例は、レーザレンジファインダ（ＬＲＦ）である。ＬＲＦは周期的にたとえば赤外線または可視光のレーザビームを周囲に放射して周囲の環境をスキャンする。レーザビームは、たとえば、壁、柱等の構造物、床の上に置かれた物体などの表面で反射される。ＬＲＦは、レーザビームの反射光を受けて各反射点までの距離を計算し、各反射点の位置が示された測定結果のデータを出力する。各反射点の位置には、反射光の到来方向および距離が反映されている。測定結果のデータ（スキャンデータ）は、「環境計測データ」または「センサデータ」と呼ばれることがある。

外界センサ１０２による環境のスキャンは、たとえば、外界センサ１０２の正面を基準として左右１３５度（合計２７０度）の範囲の環境に対して行われる。具体的には、水平面内において所定のステップ角度ごとに方向を変化させながらパルス状のレーザビームを放射し、各レーザビームの反射光を検出して距離を計測する。ステップ角度が０．３度であれば、合計９０１ステップ分の角度で決まる方向における反射点までの距離の測定データを得ることができる。この例において、外界センサ１０２が行う周囲の空間のスキャンは実質的に床面に平行であり、平面的（二次元的）である。しかし、外界センサは、三次元的なスキャンを行ってもよい。

スキャンデータの典型例は、スキャンごとに取得される点群（point cloud）を構成する各点の位置座標によって表現され得る。点の位置座標は、移動体１０とともに移動するローカル座標系によって規定される。このようなローカル座標系は、移動体座標系またはセンサ座標系と呼ばれ得る。本開示においては、移動体１０に固定されたローカル座標系の原点を移動体１０の「位置」と定義し、ローカル座標系の向き（orientation）を移動体１０の「姿勢」と定義する。以下、位置および姿勢を合わせて「ポーズ」と称することがある。

スキャンデータは、極座標系で表示される場合、各点の位置をローカル座標系における原点からの「方向」および「距離」で示す数値セットから構成され得る。極座標系の表示は、直交座標系の表示に変換され得る。以下の説明では、簡単のため、外界センサから出力されたスキャンデータは、直交座標系で表示されているとする。

移動体１０は、環境地図を記憶する記憶装置１０４と、位置推定システム１１５とを備える。

位置推定システム１１５は、外界センサ１０２に接続されて使用され、プロセッサ１０６と、プロセッサの動作を制御するコンピュータプログラムを記憶したメモリ１０７とを有している。

位置推定システム１１５は、外界センサ１０２から取得したスキャンデータと、記憶装置１０４から読み出された環境地図とのマッチングを行い、移動体１０の位置および姿勢、すなわちポーズを推定する。このマッチングは、パターンマッチングまたはスキャンマッチングと呼ばれ、種々のアルゴリズムにしたがって実行され得る。マッチングアルゴリズムの典型例は、Iterative Closest Point （ＩＣＰ：反復最近接点）アルゴリズムである。

位置推定システム１１５は、後述するように、外界センサ１０２から出力される複数のスキャンデータをマッチングによって整合させ、連結することにより、環境地図を作成する。

本開示の実施形態にける位置推定システム１１５は、プロセッサ１０６と、プロセッサ１０６を動作させるコンピュータプログラムを記憶するメモリ１０７とによって実現される。プロセッサ１０６は、コンピュータプログラムの指令にしたがって、以下の動作を実行する。

（１）外界センサ１０２からスキャンデータを取得し、スキャンデータから参照地図を作成する。

（２）外界センサ１０２からスキャンデータを新たに取得したとき、新たに取得した最新スキャンデータと参照地図とのマッチングを行うことにより、参照地図上における外界センサ１０２の位置および姿勢（すなわち移動体１０の位置および姿勢）を推定し、最新スキャンデータを参照地図に追加して参照地図を更新する。

（３）複数回更新された参照地図から、最新スキャンデータを含む一部分以外の部分を削除して、参照地図のリセットを行う。

（４）リセットを行うとき、リセット前の複数回更新された参照地図に基づいて環境地図を更新する。

上記の動作の詳細は後述する。

図示されている例において、移動体１０は、さらに、駆動装置１０８、自動走行制御装置１１０、および通信回路１１２を備えている。駆動装置１０８は、移動体１０が移動するための駆動力を発生する装置である。駆動装置１０８の例は、電気モータまたはエンジンによって回転する車輪（駆動輪）、モータまたは他のアクチュエータによって動作する二足または多足歩行装置を含む。車輪は、メカナムホイールなどの全方位ホイールであってもよい。また、移動体１０が空中または水中を移動する移動体、あるいはホバークラフトであってもよく、その場合の駆動装置１０８は、モータによって回転するプロペラを含む。

自動走行制御装置１１０は、駆動装置１０８を操作して移動体１０の移動条件（速度、加速度、移動方向など）を制御する。自動走行制御装置１１０は、所定の走行経路に沿って移動体１０を移動させてもよく、外部から与えられる指令にしたがって移動させてもよい。位置推定システム１１５は、移動体１０の移動中または停止中において、移動体１０の位置および姿勢の推定値を算出する。自動走行制御装置１１０は、この推定値を参照して移動体１０の走行を制御する。

位置推定システム１１５および自動走行制御装置１１０を、全体として、走行制御装置１２０と呼んでもよい。自動走行制御装置１１０も、位置推定システム１１５とともに、上述したプロセッサ１０６と、プロセッサ１０６の動作を制御するコンピュータプログラムを格納したメモリ１０７によって構成され得る。このようなプロセッサ１０６およびメモリ１０７は、１個または複数の半導体集積回路によって実現され得る。

通信回路１１２は、移動体１０が外部の管理装置、他の移動体、または操作者のモバイル端末機器などを含む通信ネットワークに接続してデータおよび／または指令のやりとりを行う回路である。

＜環境地図＞
図２は、移動体１０が移動する環境２００の例を模式的に示す平面レイアウト図である。環境２００は、より広い環境の一部である。図２において、太い直線は、たとえば建造物の固定壁２０２を示している。

図３は、図２に示される環境２００の地図（環境地図Ｍ）を示す図である。図中の各ドット２０４は、環境地図Ｍを構成する点群の各点に相当する。本開示において、環境地図Ｍの点群を「参照点群」と称し、スキャンデータの点群を「データ点群」または「ソース点群」と称する場合がある。マッチングは、たとえば、位置が固定された環境地図（参照点群）に対して、スキャンデータ（データ点群）の位置合わせを行うことである。ＩＣＰアルゴリズムによるマッチングを行う場合、具体的には、参照点群とデータ点群との間で対応する点のペアが選択され、各ペアを構成する点同士の距離（誤差）を最小化するようにデータ点群の位置および向きが調整される。

図３において、ドット２０４は、簡単のため、複数の線分上に等間隔で配列されている。現実の環境地図Ｍにおける点群は、より複雑な配置パターンを有していてもよい。環境地図Ｍは、点群地図に限定されず、直線または曲線を構成要素とする地図であってもよいし、占有格子地図であってもよい。すなわちスキャンデータと環境地図Ｍとの間でマッチングを行うことが可能な構造を環境地図Ｍが備えていればよい。

移動体１０が図３に示される位置ＰＡ、位置ＰＢ、および位置ＰＣのそれぞれにあるときに、移動体１０の外界センサ１０２が取得するスキャンデータは、それぞれ、異なる点群の配列を有する。移動体１０が位置ＰＡから、位置ＰＢを経て位置ＰＣに達するまでの移動時間が、外界センサ１０２によるスキャンの周期に比べて十分に長い場合、すなわち、移動体１０の移動が遅い場合、時間軸上で隣接する２つのスキャンデータは極めて類似している。しかし、移動体１０の移動が著しく速い場合は、時間軸上で隣接する２つのスキャンデータは大きく異なってしまう可能性がある。

このように外界センサ１０２から順次出力されるスキャンデータのうち、最新のスキャンデータがその直前のスキャンデータに類似している場合は、マッチングは相対的に容易であり、短時間で信頼度の高いマッチングが期待される。しかし、移動体１０の移動速度が相対的に高い場合は、最新のスキャンデータがその直前のスキャンデータに類似していない可能性があり、マッチングに要する時間が長くなったり、所定時間内にマッチングが完了しないことがあり得る。

＜地図作成時のマッチング＞
図４Ａは、外界センサ１０２が時刻ｔに取得したスキャンデータＳＤ（ｔ）の例を模式的に示す図である。スキャンデータＳＤ（ｔ）は、移動体１０とともに位置および姿勢が変わるセンサ座標系で表示されている。スキャンデータＳＤ（ｔ）は、外界センサ１０２の真正面をＶ軸として、Ｖ軸から時計周りに９０°回転した方向をＵ軸とするＵＶ座標系によって表現される。ＵＶ座標系の原点に移動体１０、より正確には外界センサ１０２が位置している。本開示では、移動体１０が前進するとき、移動体１０は、外界センサ１０２の真正面、すなわちＶ軸の方向に進む。わかりやすさのため、スキャンデータＳＤ（ｔ）を構成する点は黒丸で記載されている。

本明細書において位置推定システム１１５が外界センサ１０２からスキャンデータを取得する周期をΔｔとする。Δｔはたとえば２００ミリ秒である。移動体１０が移動しているとき、外界センサ１０２から周期的に取得されるスキャンデータの内容は変化し得る。

図４Ｂは、外界センサ１０２が時刻ｔ＋Δｔに取得したスキャンデータＳＤ（ｔ＋Δｔ）の例を模式的に示す図である。わかりやすさのため、スキャンデータＳＤ（ｔ＋Δｔ）を構成する点は白丸で記載されている。

Δｔがたとえば２００ミリ秒である場合、移動体１０が毎秒１メールの速さで移動していると、Δｔの間に移動体１０は２０センチメートル程度移動する。通常、２０センチメートル程度の移動によって、移動体１０の環境は大きく変化しないため、外界センサ１０２が時刻ｔ＋Δｔにスキャンした環境と、時刻ｔにスキャンした環境との間には広い範囲で重複した部分が含まれる。したがって、スキャンデータＳＤ（ｔ）の点群とスキャンデータＳＤ（ｔ＋Δｔ）の点群との間には多くの対応点が含まれることになる。

図４Ｃは、スキャンデータＳＤ（ｔ）とスキャンデータＳＤ（ｔ＋Δｔ）とのマッチングが完了した状態を模式的に示している。この例では、スキャンデータＳＤ（ｔ）に対してスキャンデータＳＤ（ｔ＋Δｔ）が整合するように位置合わせが行われている。図４ＣのＵＶ座標系の原点には時刻ｔにおける移動体１０が位置し、時刻ｔ＋Δｔにおける移動体１０は、ＵＶ座標系の原点から移動した位置にある。２枚のスキャンデータのマッチング行うことにより、一方のローカル座標系に対する他方ローカル座標系の配置関係が求められる。

こうして、周期的に取得される複数のスキャンデータＳＤ（ｔ）、ＳＤ（ｔ＋Δｔ）、・・・、ＳＤ（ｔ＋Ｎ×Δｔ）を連結することにより、局所的な環境地図（参照地図）を作成することができる。ここで、Ｎは１以上の整数である。

図５は、時刻ｔにおけるスキャンデータを構成する点群が初期の位置から回転および並進して、参照地図の点群に近づく様子を模式的に示す図である。時刻ｔにおけるスキャンデータの点群を構成するＫ個の点のうちのｋ番目（ｋ＝１、２、・・・、Ｋ−１、Ｋ）の点の座標値をＺ_t,k、この点に対応する参照地図上の点の座標値をｍ_kとする。このとき、２つの点群における対応点の誤差は、Ｋ個の対応点について計算した誤差の二乗和であるΣ（Ｚ_t,k-ｍ_k）²をコスト関数として評価することができる。Σ（Ｚ_t,k-ｍ_k）²を小さくするように回転および並進の剛体変換を決定する。剛体変換は、回転の角度および並進のベクトルをパラメータとして含む変換行列（同次変換行列）によって規定される。

図６は、スキャンデータの剛体変換後の位置および姿勢を示す図である。図６に示される例において、スキャンデータと参照地図とのマッチングは完了しておらず、２つの点群の間には、まだ大きな誤差（位置ずれ）が存在している。この位置ずれを縮小するため、剛体変換をさらに行う。こうして、誤差が所定値を下回る大きさになったとき、マッチングは完了する。

＜参照地図の作成＞
図７Ａは、新たに取得した最新のスキャンデータＳＤ（ｂ）と、前回に取得したスキャンデータＳＤ（ａ）とのマッチングが完了した状態を模式的に示す図である。図７Ａにおいて、黒円の点群は前回のスキャンデータを表し、白丸の点群は最新のスキャンデータを表している。図７Ａには、前回のスキャンデータを取得したときの移動体１０の位置ａ、および、最新のスキャンデータを取得したときの移動体１０の位置ｂが示されている。

この例において、前回に取得したスキャンデータＳＤ（ａ）は「参照地図ＲＭ」を構成している。参照地図ＲＭは、作成されつつある環境地図の一部である。最新のスキャンデータＳＤ（ｂ）の位置および向きは、前回に取得したスキャンデータＳＤ（ａ）の位置および向きに対して整合するようにマッチングが実行される。

このようなマッチングを行うことにより、参照地図ＲＭ上における移動体１０ｂの位置および姿勢を知ることができる。マッチングが完了した後、スキャンデータＳＤ（ｂ）を参照地図ＲＭに追加して参照地図ＲＭを更新する。

スキャンデータＳＤ（ｂ）の座標系は、スキャンデータＳＤ（ａ）の座標系に連結される。この連結は、２つの座標系の回転および並進の変換（剛体変換）を規定する行列に表される。このような変換の行列によれば、スキャンデータＳＤ（ｂ）上の各点の座標値を、スキャンデータＳＤ（ａ）の座標系における座標値に変換することができる。

図７Ｂは、次に取得したスキャンデータを図７Ａの参照地図ＲＭに追加して更新した参照地図ＲＭを示している。図７Ｂにおいて、黒円の点群は更新前の参照地図ＲＭを表し、白丸の点群は最新のスキャンデータＳＤ（ｃ）を表している。図７Ｂには、前々回、前回、および、最新のスキャンデータを取得したときの移動体１０の位置ａ、ｂ、ｃが示されている。図７Ｂの白丸の点群および黒円の点群の全体は、更新された参照地図ＲＭを構成している。

図７Ｃは、新たに取得したスキャンデータＳＤ（ｄ）を図７Ｂの参照地図ＲＭに追加して更新した参照地図ＲＭを示している。図７Ｃにおいて、黒円の点群は更新前の参照地図ＲＭを表し、白丸の点群は最新のスキャンデータＳＤ（ｄ）を表している。図７Ｃには、過去の推定位置にある移動体１０の位置ａ、ｂ、ｃに加えて、最新のスキャンデータＳＤ（ｄ）のマッチングによって推定した位置にある移動体１０の位置ｄが示されている。図７Ｃの白丸の点群および黒円の点群の全体は、更新された参照地図ＲＭを構成している。

このようにして、参照地図ＲＭは次々と更新されるため、参照地図ＲＭ内の点の個数が外界センサ１０２のスキャンごとに増大していく。このことは、最新スキャンデータと参照地図ＲＭとのマッチングを行うときの演算量の増加を引き起こす。たとえば１枚のスキャンデータが最大約１０００個の点を含む場合、２０００枚のスキャンデータをつなぎ合わせて１枚の参照地図ＲＭを作成すると、その参照地図ＲＭ内の点の個数は、最大で約２００万個にも達する。対応する点を見つけてマッチングのための演算を反復するとき、参照地図ＲＭの点群が大きすぎると、スキャン周期であるΔｔの期間内にマッチングが完了しない可能性がある。

本開示の位置推定システムでは、複数回更新された参照地図から、最新スキャンデータを含む一部分以外の部分を削除して参照地図のリセットを行う。また、リセットを行うとき、リセット前の複数回更新された参照地図に基づいて環境地図を更新する。このため、環境地図そのものは、スキャンによって得た環境情報を失うことなく保持し得る。

参照地図のリセットは、たとえば、（ｉ）参照地図を更新する回数が所定数に達したとき、（ｉｉ）参照地図のデータ量が所定量に達したとき、または（ｉｉｉ）前回のリセットからの経過時間が所定長さに達したときに行うことができる。（ｉ）の場合の「所定数」は、たとえば１００回であり得る。（ｉｉ）の場合の「所定量」は、たとえば１００００であり得る。（ｉｉｉ）の場合の「所定長さ」は、たとえば５分であり得る。

リセット後の参照地図のデータ量を最小にするには、最新のスキャンデータ、すなわち、リセットを行う時点で最も新しい１回のスキャンによって取得したデータのみを残して他のスキャンデータを削除すればよい。最新のスキャンデータに含まれる点の個数が所定値以下の場合、リセット後のマッチング精度を高めるため、最新のスキャンデータの加えて、現在に近い複数のスキャンデータをリセット後の参照地図に含めてもよい。

複数のスキャンデータから参照地図を作成するとき、点群の単位面積あたり点の密度が所定値を超えて増加することはマッチングにとって無駄になり得る。たとえば、環境内で１０×１０ｃｍ²のサイズを有する矩形の領域に相当する部分に多数の点（測定点）が存在した場合、マッチングに要する演算量が増加する割合に比べてマッチング精度が充分に向上せずに飽和することが起こり得る。このような無駄を抑制するため、スキャンデータおよび／または参照地図を構成する点群の密度が所定密度を超えたときには、点群から幾つかの点を間引き、点群の密度を所定密度以下に低下させる処理を行ってもよい。「所定密度」は、たとえば１個／（１０ｃｍ）²であり得る。

図８Ａは、更新前の環境地図Ｍを模式的に示している。図８Ｂは、参照地図ＲＭに基づいて環境地図Ｍを更新するときの様子を示している。この例において、参照地図ＲＭと環境地図Ｍとの間で配置関係がずれている。図７Ａを参照しながら説明した例では、最初に参照地図ＲＭを構成していた点群は、スキャンデータＳＤ（ａ）であった。その後に取得されたスキャンデータＳＤ（ｂ）は、スキャンデータＳＤ（ａ）に対して整合している。このため、スキャンデータＳＤ（ａ）を基準に連結された参照地図ＲＭの位置および向きは、スキャンデータＳＤ（ａ）の位置および向きに依存する。一方、スキャンデータＳＤ（ａ）の位置および向きは、スキャンデータＳＤ（ａ）を取得したときの移動体１０の位置ａおよび姿勢（向き）の推定値によって規定される。推定値が微小誤差を含む可能性があり、それによって更新後の環境地図が実際の地図(環境)から乖離していくことが懸念される。

図８Ｃは、参照地図ＲＭと環境地図Ｍとのマッチングを行い、参照地図ＲＭを環境地図Ｍに整合させた状態を模式的に示している。このマッチングにより、更新後の環境地図が実際の地図から乖離していくことが抑制される。

このようにして環境地図Ｍの更新が繰り返され、やがて環境地図Ｍは完成する。このようにして作成された環境地図は、その後、移動体１０の移動に際して自己位置推定に利用される。

＜環境地図を用いた位置推定＞
図９Ａは、外界センサが時刻ｔに取得したスキャンデータＳＤ（ｔ）の例を模式的に示す図である。スキャンデータＳＤ（ｔ）は、移動体１０とともに位置および姿勢が変わるセンサ座標系で表示され、スキャンデータＳＤ（ｔ）を構成する点は白丸で記載されている。

図９Ｂは、環境地図Ｍに対するスキャンデータＳＤ（ｔ）のマッチングを開始するときの状態を模式的に示す図である。図１のプロセッサ１０６は、外界センサ１０２からスキャンデータＳＤ（ｔ）を取得すると、スキャンデータＳＤ（ｔ）と記憶装置１０４から読み出した環境地図Ｍとのマッチングを行うことにより、移動体１０の環境地図Ｍ上における位置および姿勢を推定することができる。このようなマッチングを開始するとき、時刻ｔにおける移動体１０の位置および姿勢の初期値を決定する必要がある（図５参照）。初期値が、実際の移動体１０の位置および姿勢に近いほど、マッチングに要する時間は短縮され得る。

図９Ｃは、環境地図Ｍに対するスキャンデータＳＤ（ｔ）のマッチングが完了した状態を模式的に示す図である。

本開示の実施形態では、この初期値の決定に際して２種類の方法を採用することができる。

第１の方法では、前回のマッチングによって推定した位置および姿勢から変化量をオドメトリによって計測することである。たとえば、移動体１０が２個の駆動輪によって移動するとき、それぞれの駆動輪またはモータに取り付けられたエンコーダにより、移動体１０の移動量および移動方向を求めることができる。オドメトリを用いる方法は公知であるため、さらに詳細な説明は特に必要ない。

第２の方法は、移動体１０の位置および姿勢の推定値の履歴に基づいて、現在の位置および姿勢を予測することである。以下、この点を説明する。

＜初期値の予測＞
図１０は、図１の位置推定システム１１５によって過去に得られた移動体１０の位置および姿勢の履歴と、現在の位置および姿勢の予測値を模式的に示す図である。位置および姿勢の履歴は、位置推定システム１１５の内部のメモリ１０７に記憶される。このような履歴の一部または全部は、位置推定装置１０５の外部の記憶装置、たとえば図１の記憶装置１０４に記憶されていても良い。

図１０には、移動体１０のローカル座標系（センサ座標系）であるＵＶ座標系も示されている。スキャンデータは、ＵＶ座標系によって表現される。環境地図Ｍ上における移動体１０の位置は、環境地図Ｍの座標系におけるＵＶ座標系の原点の座標値（ｘｉ，ｙｉ）である。移動体１０の姿勢（向き）は、環境地図Ｍの座標系に対するＵＶ座標系の向き（θｉ）である。θｉは半時計回りを「正」とする。

本開示の実施形態では、位置推定装置によって過去に得られた位置および姿勢の履歴から、現在の位置および姿勢の予測値を算出する。

前回のマッチングによって得られた移動体の位置および姿勢を（ｘ_i-1，ｙ_i-1，θ_i-1）、さらにその前のマッチングによって得られた移動体の位置および姿勢を（ｘ_i-2，ｙ_i-2，θ_i-2）とする。また、現在の移動体の位置および姿勢の予測値を（ｘ_i，ｙ_i，θ_i）とする。このとき、以下の仮定が成立するとする。

仮定１：位置（ｘ_i-1，ｙ_i-1）から位置（ｘ_i，ｙ_i）までの移動に要する時間は、位置（ｘ_i-2，ｙ_i-2）から位置（ｘ_i-1，ｙ_i-1）までの移動に要した時間に等しい。

仮定２：位置（ｘ_i-1，ｙ_i-1）から位置（ｘ_i，ｙ_i）までの移動時の移動速度は、位置（ｘ_i-2，ｙ_i-2）から位置（ｘ_i-1，ｙ_i-1）までの移動時の移動速度に等しい。

仮定３：θ_i−θ_i-1は、Δθ＝θｙ_i−θ_i-1に等しい。

上記の過程のもと、以下の数１の式が設立する。

ここで、Δθは、上述の通り、θｙ_i−θ_i-1である。

移動体の姿勢（向き）については、仮定３から、以下の数２の関係が成立する。
（数２）
θ_i＝θ_i-1＋Δθ

なお、Δθがゼロであるとの近似を行うと、数２の右辺第２項の行列は単位行列として計算が単純化され得る。

上記の仮定１が成立しない場合、位置（ｘ_i-1，ｙ_i-1）から位置（ｘ_i，ｙ_i）までの移動に要する時間をΔｔ、位置（ｘ_i-2，ｙ_i-2）から位置（ｘ_i-1，ｙ_i-1）までの移動に要した時間をΔｓとする。この場合、数１の右辺における（ｘ_i-1−ｘ_i-2）および（ｙ_i-1−ｙ_i-2）を、それぞれ、Δｔ／Δｓ倍する補正と、数１の右辺の行列におけるΔθをΔｔ／Δｓ倍する補正を行えばよい。

＜位置推定システムの動作フロー＞
図１、図１１から図１３を参照しながら、本開示の実施形態における位置推定システムの動作フローを説明する。

まず、図１１を参照する。

ステップＳ１０において、位置推定システム１１５のプロセッサ１０６は、外界センサ１０２から最新（現在:current）のスキャンデータを取得する。

ステップＳ１２において、プロセッサ１０６は、オドメトリによって現在の位置および姿勢の値を取得する。

ステップＳ１４において、プロセッサ１０６は、オドメトリから取得した現在の位置および姿勢の値を初期値として、参照地図に対する最新スキャンデータの初期位置合わせを行う。

ステップＳ１６において、プロセッサ１０６は、ＩＣＰアルゴリズムによる位置ずれ補正を行う。

ステップＳ１８において、プロセッサ１０６は、最新スキャンデータを既存の参照地図に追加することにより、参照地図の更新を行う。

ステップＳ２０において、参照地図が更新条件を満たしたか否かを判定する。更新条件は、前述したように、（ｉ）参照地図を更新する回数が所定数に達したとき、（ｉｉ）参照地図のデータ量が所定量に達したとき、または（ｉｉｉ）前回のリセットからの経過時間が所定長さに達したとき、などの条件である。Ｎｏの場合、ステップＳ１０に戻り、次のスキャンデータを取得する。Ｙｅｓの場合は、ステップＳ２２に進む。

ステップＳ２２において、プロセッサ１０６は、複数回更新された参照地図に基づいて環境地図を更新する。

ステップＳ２４において、プロセッサ１０６は、複数回更新された参照地図から、最新スキャンデータを含む一部分以外の部分を削除して、参照地図のリセットを行う。こうして、参照地図を構成する点群内の点の個数および密度を低減することができる。

次に図１２を参照して、ステップＳ１６における位置ずれ補正を説明する。

まず、ステップＳ３２において、プロセッサ１０６は、２組の点群から対応点の探索を行う。具体的には、プロセッサ１０６は、スキャンデータに含まれる点群を構成する各点に対応する、環境地図上の点を選択する。

ステップＳ３４において、プロセッサ１０６は、スキャンデータと環境地図との間にある対応点間距離を縮小するように、スキャンデータの回転および並進の剛体変換（座標変換）を行う。これは、対応点間距離、すなわち、対応点の誤差の総和（二乗和）を小さくするように、座標変換行列のパラメータを最適化することである。この最適化は反復計算によって行われる。

ステップＳ３６において、プロセッサ１０６は、反復計算の結果が収束したか否かを判定する。具体的には、プロセッサ１０６は、座標変換行列のパラメータを変化させても対応点の誤差の総和（二乗和）の減少量が所定値を下回ったとき、収束したと判定する。収束しなかったときは、ステップＳ３２に戻り、プロセッサ１０６は、対応点の探索からの処理を繰りかえす。ステップＳ３６において、収束したと判定されたときは、ステップＳ３８に進む。

ステップＳ３８において、プロセッサ１０６は、座標変換行列を用いてスキャンデータの座標値をセンサ座標系の値から環境地図の座標系の値に変換する。こうして得たスキャンデータの座標値は、環境地図の更新に用いることができる。

次に、図１３を参照して図１１のフローの変形例を説明する。

図１３のフローが図１１のフローと異なる点は、ステップＳ１０とステップＳ１４との間において、プロセッサ１０６がステップＳ１２に代えてステップＳ４０を実行することにある。ステップＳ４０では、プロセッサ１０６は、オドメトリから移動体１０の現在の位置および姿勢の計測値を取得するのではなく、移動体１０（外界センサ１０２）の位置および姿勢の履歴に基づいて、現在の位置および姿勢の予測値を算出する。この予測値の計算は、図１０を参照しながら説明した演算によって実行することができる。こうして得た値を、位置および姿勢の初期値としてマッチングを実行する。他のステップは、前述した通りであるため、説明の繰り返しは行わない。

図１３のフローによれば、ロータリエンコーダなどの内界センサの出力を用いて位置および姿勢を求める必要がなくなる。特にロータリエンコーダは、車輪がスリップしたときには大きな誤差が発生し、その誤差は累積されるため、測定値の信頼度が低い。さらにロータリエンコーダによる測定は、メカナムホイールなどの全方位ホイール、二足または多足歩行装置を用いて移動する移動体、あるいは、バークラフトおよびドローンなどの飛行体には適用できない。これに対して、本開示による位置推定システムは、多用な駆動装置によって移動する種々の移動体に適用可能である。

本開示における位置推定システムは、駆動装置を備えた移動体に搭載されて使用されなくてもよい。たとえばユーザによって駆動される手押し車に載せられて地図作成に用いられてもよい。

＜例示的な実施形態＞
以下、本開示による位置推定システムを備える移動体の実施形態をより詳細に説明する。本実施形態では、移動体の一例として無人搬送車を挙げる。以下の説明では、略語を用いて、無人搬送車を「ＡＧＶ：Automatic Guided Vehicle」と記述する。以下、「ＡＧＶ」についても、移動体１０と同様に参照符号「１０」を付す。

（１）システムの基本構成
図１４は、本開示による例示的な移動体管理システム１００の基本構成例を示している。移動体管理システム１００は、少なくとも１台のＡＧＶ１０と、ＡＧＶ１０の運行管理を行う運行管理装置５０とを含む。図１４には、ユーザ１によって操作される端末装置２０も記載されている。

ＡＧＶ１０は、走行に磁気テープなどの誘導体が不要な「ガイドレス式」走行が可能な無人搬送台車である。ＡＧＶ１０は、自己位置推定を行い、推定の結果を端末装置２０および運行管理装置５０に送信することができる。ＡＧＶ１０は、運行管理装置５０からの指令にしたがって環境Ｓ内を自動走行することが可能である。

運行管理装置５０は各ＡＧＶ１０の位置をトラッキングし、各ＡＧＶ１０の走行を管理するコンピュータシステムである。運行管理装置５０は、デスクトップ型ＰＣ、ノート型ＰＣ、および／または、サーバコンピュータであり得る。運行管理装置５０は、複数のアクセスポイント２を介して、各ＡＧＶ１０と通信する。たとえば、運行管理装置５０は、各ＡＧＶ１０が次に向かうべき位置の座標のデータを各ＡＧＶ１０に送信する。各ＡＧＶ１０は、定期的に、たとえば２５０ミリ秒ごとに自身の位置および姿勢（orientation）を示すデータを運行管理装置５０に送信する。指示した位置にＡＧＶ１０が到達すると、運行管理装置５０は、さらに次に向かうべき位置の座標のデータを送信する。ＡＧＶ１０は、端末装置２０に入力されたユーザ１の操作に応じて環境Ｓ内を走行することも可能である。端末装置２０の一例はタブレットコンピュータである。

図１５は、３台のＡＧＶ１０ａ，１０ｂおよび１０ｃが存在する環境Ｓの一例を示している。いずれのＡＧＶも図中の奥行き方向に走行しているとする。ＡＧＶ１０ａおよび１０ｂは天板に載置された荷物を搬送中である。ＡＧＶ１０ｃは、前方のＡＧＶ１０ｂに追従して走行している。なお、説明の便宜のため、図１５では参照符号１０ａ，１０ｂおよび１０ｃを付したが、以下では、「ＡＧＶ１０」と記述する。

ＡＧＶ１０は、天板に載置された荷物を搬送する方法以外に、自身と接続された牽引台車を利用して荷物を搬送することも可能である。図１６は接続される前のＡＧＶ１０および牽引台車５を示している。牽引台車５の各足にはキャスターが設けられている。ＡＧＶ１０は牽引台車５と機械的に接続される。図１７は、接続されたＡＧＶ１０および牽引台車５を示している。ＡＧＶ１０が走行すると、牽引台車５はＡＧＶ１０に牽引される。牽引台車５を牽引することにより、ＡＧＶ１０は、牽引台車５に載置された荷物を搬送できる。

ＡＧＶ１０と牽引台車５との接続方法は任意である。ここでは一例を説明する。ＡＧＶ１０の天板にはプレート６が固定されている。牽引台車５には、スリットを有するガイド７が設けられている。ＡＧＶ１０は牽引台車５に接近し、プレート６をガイド７のスリットに差し込む。差し込みが完了すると、ＡＧＶ１０は、図示されない電磁ロック式ピンをプレート６およびガイド７に貫通させ、電磁ロックをかける。これにより、ＡＧＶ１０と牽引台車５とが物理的に接続される。

再び図１４を参照する。各ＡＧＶ１０と端末装置２０とは、たとえば１対１で接続されてＢｌｕｅｔｏｏｔｈ（登録商標）規格に準拠した通信を行うことができる。各ＡＧＶ１０と端末装置２０とは、１または複数のアクセスポイント２を利用してＷｉ−Ｆｉ（登録商標）に準拠した通信を行うこともできる。複数のアクセスポイント２は、たとえばスイッチングハブ３を介して互いに接続されている。図１４には２台のアクセスポイント２ａ，２ｂが記載されている。ＡＧＶ１０はアクセスポイント２ａと無線で接続されている。端末装置２０はアクセスポイント２ｂと無線で接続されている。ＡＧＶ１０が送信したデータはアクセスポイント２ａで受信された後、スイッチングハブ３を介してアクセスポイント２ｂに転送され、アクセスポイント２ｂから端末装置２０に送信される。また、端末装置２０が送信したデータは、アクセスポイント２ｂで受信された後、スイッチングハブ３を介してアクセスポイント２ａに転送され、アクセスポイント２ａからＡＧＶ１０に送信される。これにより、ＡＧＶ１０および端末装置２０の間の双方向通信が実現される。複数のアクセスポイント２はスイッチングハブ３を介して運行管理装置５０とも接続されている。これにより、運行管理装置５０と各ＡＧＶ１０との間でも双方向通信が実現される。

（２）環境地図の作成
自己位置を推定しながらＡＧＶ１０が走行できるようにするため、環境Ｓ内の地図が作成される。ＡＧＶ１０には位置推定装置およびＬＲＦが搭載されており、ＬＲＦの出力を利用して地図を作成できる。

ＡＧＶ１０は、ユーザの操作によってデータ取得モードに遷移する。データ取得モードにおいて、ＡＧＶ１０はＬＲＦを用いたセンサデータ(スキャンデータ)の取得を開始する。その後の処理は、前述した通りである。

なお、センサデータを取得するための環境Ｓ内の移動は、ユーザの操作にしたがってＡＧＶ１０が走行することによって実現し得る。たとえば、ＡＧＶ１０は、端末装置２０を介して無線でユーザから前後左右の各方向への移動を指示する走行指令を受け取る。ＡＧＶ１０は走行指令にしたがって環境Ｓ内を前後左右に走行し、地図を作成する。ＡＧＶ１０がジョイスティック等の操縦装置と有線で接続されている場合には、当該操縦装置からの制御信号にしたがって環境Ｓ内を前後左右に走行し、地図を作成してもよい。ＬＲＦを搭載した計測台車を人が押し歩くことによってセンサデータを取得してもよい。

なお、図１４および図１５には複数台のＡＧＶ１０が示されているが、ＡＧＶは１台であってもよい。複数台のＡＧＶ１０が存在する場合、ユーザ１は端末装置２０を利用して、登録された複数のＡＧＶのうちから一台のＡＧＶ１０を選択して、環境Ｓの地図を作成させることができる。

地図が作成されると、以後、各ＡＧＶ１０は当該地図を利用して自己位置を推定しながら自動走行することができる。

（３）ＡＧＶの構成
図１８は、本実施形態にかかる例示的なＡＧＶ１０の外観図である。ＡＧＶ１０は、２つの駆動輪１１ａおよび１１ｂと、４つのキャスター１１ｃ、１１ｄ、１１ｅおよび１１ｆと、フレーム１２と、搬送テーブル１３と、走行制御装置１４と、ＬＲＦ１５とを有する。２つの駆動輪１１ａおよび１１ｂは、ＡＧＶ１０の右側および左側にそれぞれ設けられている。４つのキャスター１１ｃ、１１ｄ、１１ｅおよび１１ｆは、ＡＧＶ１０の４隅に配置されている。なお、ＡＧＶ１０は、２つの駆動輪１１ａおよび１１ｂに接続される複数のモータも有するが、複数のモータは図１８には示されていない。また、図１８には、ＡＧＶ１０の右側に位置する１つの駆動輪１１ａおよび２つのキャスター１１ｃおよび１１ｅと、左後部に位置するキャスター１１ｆとが示されているが、左側の駆動輪１１ｂおよび左前部のキャスター１１ｄはフレーム１２の蔭に隠れているため明示されていない。４つのキャスター１１ｃ、１１ｄ、１１ｅおよび１１ｆは、自由に旋回することができる。以下の説明では、駆動輪１１ａおよび駆動輪１１ｂを、それぞれ車輪１１ａおよび車輪１１ｂとも称する。

走行制御装置１４は、ＡＧＶ１０の動作を制御する装置であり、主としてマイコン（後述）を含む集積回路、電子部品およびそれらが搭載された基板を含む。走行制御装置１４は、上述した、端末装置２０とのデータの送受信、および、前処理演算を行う。

ＬＲＦ１５は、たとえば赤外のレーザビーム１５ａを放射し、当該レーザビーム１５ａの反射光を検出することにより、反射点までの距離を測定する光学機器である。本実施形態では、ＡＧＶ１０のＬＲＦ１５は、たとえばＡＧＶ１０の正面を基準として左右１３５度（合計２７０度）の範囲の空間に、０．２５度ごとに方向を変化させながらパルス状のレーザビーム１５ａを放射し、各レーザビーム１５ａの反射光を検出する。これにより、０．２５度ごと、合計１０８１ステップ分の角度で決まる方向における反射点までの距離のデータを得ることができる。なお、本実施形態では、ＬＲＦ１５が行う周囲の空間のスキャンは実質的に床面に平行であり、平面的（二次元的）である。しかしながら、ＬＲＦ１５は高さ方向のスキャンを行ってもよい。

ＡＧＶ１０の位置および姿勢（向き）と、ＬＲＦ１５のスキャン結果とにより、ＡＧＶ１０は、環境Ｓの地図を作成することができる。地図には、ＡＧＶの周囲の壁、柱等の構造物、床の上に載置された物体の配置が反映され得る。地図のデータは、ＡＧＶ１０内に設けられた記憶装置に格納される。

ＡＧＶ１０の位置および姿勢、すなわちポーズ（ｘ，ｙ，θ）を、以下、単に「位置」と呼ぶことがある。

走行制御装置１４は、前述したようにして、ＬＲＦ１５の測定結果と、自身が保持する地図データとを比較して、自身の現在位置を推定する。地図データは、他のＡＧＶ１０が作成した地図データであってもよい。

図１９Ａは、ＡＧＶ１０の第１のハードウェア構成例を示している。また図１９Ａは、走行制御装置１４の具体的な構成も示している。

ＡＧＶ１０は、走行制御装置１４と、ＬＲＦ１５と、２台のモータ１６ａおよび１６ｂと、駆動装置１７と、車輪１１ａおよび１１ｂとを備えている。

走行制御装置１４は、マイコン１４ａと、メモリ１４ｂと、記憶装置１４ｃと、通信回路１４ｄと、位置推定装置１４ｅとを有している。マイコン１４ａ、メモリ１４ｂ、記憶装置１４ｃ、通信回路１４ｄおよび位置推定装置１４ｅは通信バス１４ｆで接続されており、相互にデータを授受することが可能である。ＬＲＦ１５もまた通信インタフェース（図示せず）を介して通信バス１４ｆに接続されており、計測結果である計測データを、マイコン１４ａ、位置推定装置１４ｅおよび／またはメモリ１４ｂに送信する。

マイコン１４ａは、走行制御装置１４を含むＡＧＶ１０の全体を制御するための演算を行うプロセッサまたは制御回路（コンピュータ）である。典型的にはマイコン１４ａは半導体集積回路である。マイコン１４ａは、制御信号であるＰＷＭ（Ｐｕｌｓｅ Ｗｉｄｔｈ Ｍｏｄｕｌａｔｉｏｎ）信号を駆動装置１７に送信して駆動装置１７を制御し、モータに印加する電圧を調整させる。これによりモータ１６ａおよび１６ｂの各々が所望の回転速度で回転する。

左右のモータ１６ａおよび１６ｂの駆動を制御する１つ以上の制御回路（たとえばマイコン）を、マイコン１４ａとは独立して設けてもよい。たとえば、モータ駆動装置１７が、モータ１６ａおよび１６ｂの駆動をそれぞれ制御する２つのマイコンを備えていてもよい。

メモリ１４ｂは、マイコン１４ａが実行するコンピュータプログラムを記憶する、揮発性の記憶装置である。メモリ１４ｂは、マイコン１４ａおよび位置推定装置１４ｅが演算を行う際のワークメモリとしても利用され得る。

記憶装置１４ｃは、不揮発性の半導体メモリ装置である。ただし、記憶装置１４ｃは、ハードディスクに代表される磁気記録媒体、または、光ディスクに代表される光学式記録媒体であってもよい。さらに、記憶装置１４ｃは、いずれかの記録媒体にデータを書き込みおよび／または読み出すためのヘッド装置および当該ヘッド装置の制御装置を含んでもよい。

記憶装置１４ｃは、走行する環境Ｓの環境地図Ｍ、および、１または複数の走行経路のデータ（走行経路データ）Ｒを記憶する。環境地図Ｍは、ＡＧＶ１０が地図作成モードで動作することによって作成され記憶装置１４ｃに記憶される。走行経路データＲは、環境地図Ｍが作成された後に外部から送信される。本実施形態では、環境地図Ｍおよび走行経路データＲは同じ記憶装置１４ｃに記憶されているが、異なる記憶装置に記憶されてもよい。

走行経路データＲの例を説明する。

端末装置２０がタブレットコンピュータである場合には、ＡＧＶ１０はタブレットコンピュータから走行経路を示す走行経路データＲを受信する。このときの走行経路データＲは、複数のマーカの位置を示すマーカデータを含む。「マーカ」は走行するＡＧＶ１０の通過位置（経由点）を示す。走行経路データＲは、走行開始位置を示す開始マーカおよび走行終了位置を示す終了マーカの位置情報を少なくとも含む。走行経路データＲは、さらに、１以上の中間経由点のマーカの位置情報を含んでもよい。走行経路が１以上の中間経由点を含む場合には、開始マーカから、当該走行経由点を順に経由して終了マーカに至る経路が、走行経路として定義される。各マーカのデータは、そのマーカの座標データに加えて、次のマーカに移動するまでのＡＧＶ１０の向き（角度）および走行速度のデータを含み得る。ＡＧＶ１０が各マーカの位置で一旦停止し、自己位置推定および端末装置２０への通知などを行う場合には、各マーカのデータは、当該走行速度に達するまでの加速に要する加速時間、および／または、当該走行速度から次のマーカの位置で停止するまでの減速に要する減速時間のデータを含み得る。

端末装置２０ではなく運行管理装置５０（たとえば、ＰＣおよび／またはサーバコンピュータ）がＡＧＶ１０の移動を制御してもよい。その場合には、運行管理装置５０は、ＡＧＶ１０がマーカに到達する度に、次のマーカへの移動をＡＧＶ１０に指示してもよい。たとえば、ＡＧＶ１０は、運行管理装置５０から、次に向かうべき目的位置の座標データ、または、当該目的位置までの距離および進むべき角度のデータを、走行経路を示す走行経路データＲとして受信する。

ＡＧＶ１０は、作成された地図と走行中に取得されたＬＲＦ１５が出力したセンサデータとを利用して自己位置を推定しながら、記憶された走行経路に沿って走行することができる。

通信回路１４ｄは、たとえば、Ｂｌｕｅｔｏｏｔｈ（登録商標）および／またはＷｉ−Ｆｉ（登録商標）規格に準拠した無線通信を行う無線通信回路である。いずれの規格も、２．４ＧＨｚ帯の周波数を利用した無線通信規格を含む。たとえばＡＧＶ１０を走行させて地図を作成するモードでは、通信回路１４ｄは、Ｂｌｕｅｔｏｏｔｈ（登録商標）規格に準拠した無線通信を行い、１対１で端末装置２０と通信する。

位置推定装置１４ｅは、地図の作成処理、および、走行時には自己位置の推定処理を行う。位置推定装置１４ｅは、ＡＧＶ１０の位置および姿勢とＬＲＦのスキャン結果とにより、環境Ｓの地図を作成する。走行時には、位置推定装置１４ｅは、ＬＲＦ１５からセンサデータを受け取り、また、記憶装置１４ｃに記憶された環境地図Ｍを読み出す。ＬＲＦ１５のスキャン結果から作成された局所的地図データ（センサデータ）を、より広範囲の環境地図Ｍとのマッチングを行うことにより、環境地図Ｍ上における自己位置（ｘ，ｙ，θ）を同定する。位置推定装置１４ｅは、局所的地図データが環境地図Ｍに一致した程度を表す「信頼度」のデータを生成する。自己位置（ｘ，ｙ，θ）、および、信頼度の各データは、ＡＧＶ１０から端末装置２０または運行管理装置５０に送信され得る。端末装置２０または運行管理装置５０は、自己位置（ｘ，ｙ，θ）、および、信頼度の各データを受信して、内蔵または接続された表示装置に表示することができる。

本実施形態では、マイコン１４ａと位置推定装置１４ｅとは別個の構成要素であるとしているが、これは一例である。マイコン１４ａおよび位置推定装置１４ｅの各動作を独立して行うことが可能な１つのチップ回路または半導体集積回路であってもよい。図１９Ａには、マイコン１４ａおよび位置推定装置１４ｅを包括するチップ回路１４ｇが示されている。以下では、マイコン１４ａおよび位置推定装置１４ｅが別個独立に設けられている例を説明する。

２台のモータ１６ａおよび１６ｂは、それぞれ２つの車輪１１ａおよび１１ｂに取り付けられ、各車輪を回転させる。つまり、２つの車輪１１ａおよび１１ｂはそれぞれ駆動輪である。本明細書では、モータ１６ａおよびモータ１６ｂは、それぞれＡＧＶ１０の右輪および左輪を駆動するモータであるとして説明する。

移動体１０は、さらに、車輪１１ａおよび１１ｂの回転位置または回転速度を測定するロータリエンコーダをさらに備えていてもよい。マイコン１４ａは、位置推定装置１４ｅから受信した信号だけでなく、ロータリエンコーダから受信した信号を利用して移動体１０の位置および姿勢を推定してもよい。

駆動装置１７は、２台のモータ１６ａおよび１６ｂの各々に印加される電圧を調整するためのモータ駆動回路１７ａおよび１７ｂを有する。モータ駆動回路１７ａおよび１７ｂの各々はいわゆるインバータ回路を含む。モータ駆動回路１７ａおよび１７ｂは、マイコン１４ａまたはモータ駆動回路１７ａ内のマイコンから送信されたＰＷＭ信号によって各モータに流れる電流をオンまたはオフし、それによりモータに印加される電圧を調整する。

図１９Ｂは、ＡＧＶ１０の第２のハードウェア構成例を示している。第２のハードウェア構成例は、レーザ測位システム１４ｈを有する点、および、マイコン１４ａが各構成要素と１対１で接続されている点において、第１のハードウェア構成例（図１９Ａ）と相違する。

レーザ測位システム１４ｈは、位置推定装置１４ｅおよびＬＲＦ１５を有する。位置推定装置１４ｅおよびＬＲＦ１５は、たとえばイーサネット（登録商標）ケーブルで接続されている。位置推定装置１４ｅおよびＬＲＦ１５の各動作は上述した通りである。レーザ測位システム１４ｈは、ＡＧＶ１０のポーズ（ｘ，ｙ，θ）を示す情報をマイコン１４ａに出力する。

マイコン１４ａは、種々の汎用Ｉ／Ｏインタフェースまたは汎用入出力ポート（図示せず）を有している。マイコン１４ａは、通信回路１４ｄ、レーザ測位システム１４ｈ等の、走行制御装置１４内の他の構成要素と、当該汎用入出力ポートを介して直接接続されている。

図１９Ｂに関して上述した構成以外は、図１９Ａの構成と共通である。よって共通の構成の説明は省略する。

本開示の実施形態におけるＡＧＶ１０は、図示されていない障害物検知センサおよびバンパースイッチなどのセーフティセンサを備えていてもよい。

（４）運行管理装置の構成例
図２０は、運行管理装置５０のハードウェア構成例を示している。運行管理装置５０は、ＣＰＵ５１と、メモリ５２と、位置データベース（位置ＤＢ）５３と、通信回路５４と、地図データベース（地図ＤＢ）５５と、画像処理回路５６とを有する。

ＣＰＵ５１、メモリ５２、位置ＤＢ５３、通信回路５４、地図ＤＢ５５および画像処理回路５６は通信バス５７で接続されており、相互にデータを授受することが可能である。

ＣＰＵ５１は、運行管理装置５０の動作を制御する信号処理回路（コンピュータ）である。典型的にはＣＰＵ５１は半導体集積回路である。

メモリ５２は、ＣＰＵ５１が実行するコンピュータプログラムを記憶する、揮発性の記憶装置である。メモリ５２は、ＣＰＵ５１が演算を行う際のワークメモリとしても利用され得る。

位置ＤＢ５３は、各ＡＧＶ１０の行き先となり得る各位置を示す位置データを格納する。位置データは、たとえば管理者によって工場内に仮想的に設定された座標によって表され得る。位置データは管理者によって決定される。

通信回路５４は、たとえばイーサネット（登録商標）規格に準拠した有線通信を行う。通信回路５４はアクセスポイント２（図１４）と有線で接続されており、アクセスポイント２を介して、ＡＧＶ１０と通信することができる。通信回路５４は、ＡＧＶ１０に送信すべきデータを、バス５７を介してＣＰＵ５１から受信する。また通信回路５４は、ＡＧＶ１０から受信したデータ（通知）を、バス５７を介してＣＰＵ５１および／またはメモリ５２に送信する。

地図ＤＢ５５は、ＡＧＶ１０が走行する工場等の内部の地図のデータを格納する。各ＡＧＶ１０の位置と１対１で対応関係を有する地図であれば、データの形式は問わない。たとえば地図ＤＢ５５に格納される地図は、ＣＡＤによって作成された地図であってもよい。

位置ＤＢ５３および地図ＤＢ５５は、不揮発性の半導体メモリ上に構築されてもよいし、ハードディスクに代表される磁気記録媒体、または光ディスクに代表される光学式記録媒体上に構築されてもよい。

画像処理回路５６はモニタ５８に表示される映像のデータを生成する回路である。画像処理回路５６は、専ら、管理者が運行管理装置５０を操作する際に動作する。本実施形態では特にこれ以上の詳細な説明は省略する。なお、モニタ５８は運行管理装置５０と一体化されていてもよい。また画像処理回路５６の処理をＣＰＵ５１が行ってもよい。

上述の実施形態の説明では、一例として二次元空間（床面）を走行するＡＧＶを挙げた。しかしながら本開示は三次元空間を移動する移動体、たとえば飛行体（ドローン）、にも適用され得る。ドローンが飛行しながら三次元空間地図を作成する場合には、二次元空間を三次元空間に拡張することができる。

上記の包括的または具体的な態様は、システム、方法、集積回路、コンピュータプログラム、または記録媒体によって実現されてもよい。あるいは、システム、装置、方法、集積回路、コンピュータプログラム、および記録媒体の任意な組み合わせによって実現されてもよい。

本開示の移動体は、工場、倉庫、建設現場、物流、病院などで荷物、部品、完成品などの物の移動および搬送に好適に利用され得る。

１・・・ユーザ、２ａ、２ｂ・・・アクセスポイント、１０・・・ＡＧＶ（移動体）、１１ａ、１１ｂ・・・駆動輪（車輪）、１１ｃ、１１ｄ、１１ｅ、１１ｆ・・・キャスター、１２・・・フレーム、１３・・・搬送テーブル、１４・・・走行制御装置、１４ａ・・・マイコン、１４ｂ・・・メモリ、１４ｃ・・・記憶装置、１４ｄ・・・通信回路、１４ｅ・・・位置推定装置、１６ａ、１６ｂ・・・モータ、１５・・・レーザレンジファインダ、１７ａ、１７ｂ・・・モータ駆動回路、２０・・・端末装置（タブレットコンピュータなどのモバイルコンピュータ）、５０・・・運行管理装置、５１・・・ＣＰＵ、５２・・・メモリ、５３・・・位置データベース（位置ＤＢ）、５４・・・通信回路、５５・・・地図データベース（地図ＤＢ）、５６・・・画像処理回路、１００・・・移動体管理システム

Claims (13)

1. 環境をスキャンしてスキャンデータを周期的に出力する外界センサに接続されて使用される位置推定システムであって、
   プロセッサと、
   前記プロセッサを動作させるコンピュータプログラムを記憶するメモリと、
   を備え、
   前記プロセッサは、前記コンピュータプログラムの指令にしたがって、
   前記外界センサから前記スキャンデータを取得し、前記スキャンデータから参照地図を作成すること、
   前記外界センサから前記スキャンデータを新たに取得したとき、新たに取得した最新スキャンデータと前記参照地図とのマッチングを行うことにより、前記参照地図上における前記外界センサの位置および姿勢を推定し、前記最新スキャンデータを前記参照地図に追加して前記参照地図を更新すること、
   複数回更新された参照地図から、前記最新スキャンデータを含む一部分以外の部分を削除して、前記参照地図のリセットを行うこと、および、
   前記リセットを行うとき、前記リセット前の前記複数回更新された参照地図に基づいて環境地図を更新すること、
   を実行する、位置推定システム。
2. 前記プロセッサは、前記参照地図を更新する回数が所定数に達したとき、前記参照地図のリセットを行う、請求項１に記載の位置推定システム。
3. 前記プロセッサは、前記参照地図のデータ量が所定量に達したとき、前記参照地図のリセットを行う、請求項１に記載の位置推定システム。
4. 前記プロセッサは、前回の前記リセットからの経過時間が所定長さに達したとき、前記参照地図のリセットを行う、請求項１に記載の位置推定システム。
5. 前記環境地図を更新するとき、前記リセット前の前記複数回更新された参照地図と前記環境地図とのマッチングを行い、前記参照地図を前記環境地図に整合させる、請求項１から４のいずれかに記載の位置推定システム。
6. 前記プロセッサは、反復最近接点アルゴリズムによって前記マッチングを行う、請求項１から５のいずれかに記載の位置推定システム。
7. 前記プロセッサは、前記スキャンデータおよび／または前記参照地図を構成する点群の密度を所定密度以下に低下させる処理を行う、請求項１から６のいずれかに記載の位置推定システム。
8. 前記プロセッサは、内界センサの出力に基づいて前記外界センサの移動量を測定し、
   前記マッチングを行うときに用いる前記外界センサの前記位置および前記姿勢の初期値を、前記外界センサの移動量に基づいて決定する、請求項１から７のいずれかに記載の位置推定システム。
9. 前記プロセッサは、前記外界センサの位置および姿勢の履歴に基づいて、前記外界センサの現在の位置および姿勢の予測値を算出し、
   前記マッチングを行うときに用いる前記外界センサの前記位置および前記姿勢の初期値として前記予測値を用いる、請求項１から７のいずれかに記載の位置推定システム。
10. 請求項１から９のいずれかに記載の位置推定システムと、
    前記外界センサと、
    前記位置推定システムが作成した前記環境地図を記憶する記憶装置と、
    移動のための駆動装置と、
    を備える移動体。
11. 内界センサをさらに備える、請求項１０に記載の移動体。
12. 前記プロセッサは、前記外界センサから前記スキャンデータを取得し、前記スキャンデータと前記記憶装置から読み出した前記環境地図とのマッチングを行うことにより、前記移動体の前記環境地図上における位置および姿勢を推定する、請求項１０または１１に記載の移動体。
13. 請求項１から９のいずれかに記載の位置推定システムに使用されるコンピュータプログラム。

Images