WO2019044498A1

WO2019044498A1 - 移動体、位置推定装置、およびコンピュータプログラム

Info

Publication number: WO2019044498A1
Application number: PCT/JP2018/030306
Authority: WO
Inventors: 慎治鈴木; 佐伯　哲夫; 中谷　政次
Original assignee: 日本電産株式会社
Priority date: 2017-09-04
Filing date: 2018-08-14
Publication date: 2019-03-07
Also published as: US20200363212A1; CN111033425A; JPWO2019044498A1; JP6825712B2

Abstract

本開示の位置推定装置は、外界センサ１０２と、座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む複数の部分環境地図を記憶する記憶装置１０４と、外界センサ１０２からのスキャンデータと部分環境地図とのマッチングを行い、移動体の位置および姿勢を推定する位置推定装置１０６とを備える。位置推定装置１０６は、移動体１０の推定位置が第１部分環境地図上から第２部分環境地図上に移動したとき、第１部分環境地図上における移動体の推定位置および推定姿勢の、第２部分環境地図上における対応位置および対応姿勢を座標変換の関係に基づいて決定し、第２部分環境地図上の移動体の位置および姿勢を推定するためのマッチングを開始するタイミングにおける第２部分環境地図上の対応位置および対応姿勢を、第１部分環境地図上における移動体の推定位置および推定姿勢の履歴に基づいて補正し、補正した対応位置および対応姿勢を初期値としてマッチングを行う。

Description

移動体、位置推定装置、およびコンピュータプログラム

　本開示は、移動体、位置推定装置、およびコンピュータプログラムに関する。

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

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

特開２０１０－０９２１４７号公報

　特開２０１０－０９２１４７号公報に開示されている自律移動装置は、局所地図と部分地図とのマッチングを行うとき、モータまたは駆動輪に取り付けられているロータリエンコーダ（以下、単に「エンコーダ」と称する）の出力を用いて自機の移動量を算出している。

　本開示の実施形態は、エンコーダの出力を用いることなく自己位置推定を行うことが可能な移動体を提供する。

　本開示の移動体は、非限定的で例示的な実施形態において、環境をスキャンしてスキャンデータを周期的に出力する外界センサと、座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む複数の部分環境地図を記憶する記憶装置と、前記スキャンデータと前記記憶装置から読み出された前記複数の部分環境地図のいずれかとのマッチングを行い、移動体の位置および姿勢を推定する位置推定装置とを備える。前記位置推定装置は、前記移動体の推定位置が前記第１部分環境地図上から前記第２部分環境地図上に移動したとき、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の、前記第２部分環境地図上における対応位置および対応姿勢を、前記座標変換の関係に基づいて決定する。そして、前記第２部分環境地図上の前記移動体の位置および姿勢を推定するためのマッチングを開始するタイミングにおける前記第２部分環境地図上の前記対応位置および前記対応姿勢を、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の履歴に基づいて補正する。次に、前記補正した前記対応位置および前記対応姿勢を初期値として、前記第２部分環境地図上の前記移動体の前記位置および前記姿勢を推定するための前記マッチングを行う。

　本開示の位置推定装置は、非限定的で例示的な実施形態において、環境をスキャンしてスキャンデータを周期的に出力する外界センサと、座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む複数の部分環境地図を記憶する記憶装置とに接続される、移動体の位置推定装置であって、プロセッサと、前記プロセッサを動作させるコンピュータプログラムを記憶するメモリとを備える。前記プロセッサは、前記コンピュータプログラムの指令にしたがって、前記移動体の推定位置が前記第１部分環境地図上から前記第２部分環境地図上に移動したとき、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の、前記第２部分環境地図上における対応位置および対応姿勢を、前記座標変換の関係に基づいて決定すること、前記第２部分環境地図上の前記移動体の位置および姿勢を推定するためのマッチングを開始するタイミングにおける前記第２部分環境地図上の前記対応位置および前記対応姿勢を、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の履歴に基づいて補正すること、および、前記補正した前記対応位置および前記対応姿勢を初期値として、前記第２部分環境地図上の前記移動体の前記位置および前記姿勢を推定するための前記マッチングを行うことを実行する。

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

　本開示の移動体の実施形態によれば、環境地図が複数の部分環境地図から構成され、移動中に部分環境地図を切り替えるときにも、エンコーダの出力を用いることなく自己位置推定を行うことが可能である。

図１は、本開示による移動体の実施形態の構成例を示す図である。図２は、移動体が移動する環境の例を模式的に示す平面レイアウト図である。図３は、図２に示される環境の第１部分環境地図Ｍ１および第２部分環境地図Ｍ２を示す図である。図４は、第１部分環境地図Ｍ１および第２部分環境地図Ｍ２の配置関係の例を示す図である。図５は、第１部分環境地図Ｍ１の座標系（Ｘ１Ｙ１座標系）および第２部分環境地図Ｍ２（Ｘ２Ｙ２座標系）との関係を示す図である。図６Ａは、時刻＝ｔにおける第１部分環境地図上の移動体の位置および姿勢を示す図である。図６Ｂは、時刻＝ｔおよびｔ＋Δtにおける第２部分環境地図上の移動体の位置および姿勢を示す図である。図７Ａは、第１部分環境地図Ｍ１を示す図である。図７Ｂは、外界センサが時刻ｔに取得したスキャンデータＳＤ（ｔ）の例を模式的に示す図である。図７Ｃは、第１部分環境地図Ｍ１に対するスキャンデータＳＤ（ｔ）のマッチングが完了した状態を模式的に示す図である。図８Ａは、スキャンデータを構成する点群が初期の位置から回転および並進して、環境地図の点群に近づく様子を模式的に示す図である。図８Ｂは、スキャンデータの剛体変換後の位置および姿勢を示す図である。図９は、時刻ｔにおける移動体が記載された第１部分環境地図を示す図である。図１０Ａは、時刻ｔにおける第２部分環境地図Ｍ２上の移動体１０の位置および姿勢を示す図である。図１０Ｂは、時刻ｔ＋Δtにおける第２部分環境地図Ｍ２上の移動体１０の位置および姿勢を示す図である。図１１は、時刻ｔ＋Δtに外界センサから取得したスキャンデータＳＤ（ｔ＋Δt）の例を模式的に示す図である。図１２Ａは、スキャンデータＳＤ（ｔ＋Δt）の点群と第２部分環境地図Ｍ２の点群との間の相対的に大きな位置ずれを示す図である。図１２Ｂは、スキャンデータＳＤ（ｔ＋Δt）の点群と第２部分環境地図Ｍ２の点群との間の相対的に小さな位置ずれを示す図である。図１３は、時刻ｔ－Δｓ、ｔ、ｔ＋Δｔにおける移動体１０の位置および姿勢を模式的に示す平面図である。図１４は、本開示の実施形態における位置推定装置の動作の一部を示すフローチャートである。図１５は、本開示の実施形態における位置推定装置の動作の一部を示すフローチャートである。図１６は、本開示の実施形態における位置推定装置の動作の一部を示すフローチャートである。図１７は、本開示による移動体の他の構成例を示す図である。図１８は、本開示による移動体を含む移動体制御システムの概要を示す図である。図１９は、ＡＧＶが存在する環境の一例を示す斜視図である。図２０は、接続される前のＡＧＶおよび牽引台車を示す斜視図である。図２１は、接続されたＡＧＶおよび牽引台車を示す斜視図である。図２２は、本実施形態にかかる例示的なＡＧＶの外観図である。図２３Ａは、ＡＧＶの第１のハードウェア構成例を示す図である。図２３Ｂは、ＡＧＶの第２のハードウェア構成例を示す図である。図２４は、運行管理装置のハードウェア構成例を示す図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　移動体１０は、複数の部分環境地図を記憶する記憶装置１０４と、位置推定装置（自己位置推定装置）１０６とを備える。複数の部分環境地図は、座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む。複数の部分環境地図は、第１部分環境地図および第２部分環境地図に対して直接または間接的に連結された他の部分環境地図を含んでいても良い。

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

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

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

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

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

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

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

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

　移動体１０が図３に示される位置ＰＡにあるとき、位置ＰＡで外界センサが取得したスキャンデータと第１部分環境地図Ｍ１とのマッチングが行われる。その結果、移動体１０の位置および姿勢の推定値が求められる。移動体１０の位置および姿勢は、第１部分環境地図Ｍ１の座標系によって表現される。また、移動体１０が、位置ＰＢを通過して位置ＰＣに移動したときは、位置ＰＣで外界センサが取得したスキャンデータと第２部分環境地図Ｍ２とのマッチングが行われる。その結果、移動体１０の位置および姿勢の推定値が求められる。このときの移動体１０の位置および姿勢は、第１部分環境地図Ｍ１の座標系ではなく、第２部分環境地図Ｍ２の座標系によって表現される。このように、移動体１０の位置および姿勢の「値」は、移動体１０の位置に応じて選択された部分環境地図の座標系で表現され得る。一方、位置ＰＢは、第１部分環境地図Ｍ１と第２部分環境地図Ｍ２との重複領域内である。このため、移動体１０が図３の位置ＰＢにあるとき、移動体１０の位置および姿勢は、第１部分環境地図Ｍ１の座標系による値、および、第２部分環境地図Ｍ２の座標系による値の両方を有し得る。

　移動体１０が第１部分環境地図Ｍ１と第２部分環境地図Ｍ２との重複領域内に位置していないときでも、第１部分環境地図Ｍ１の座標系と第２部分環境地図Ｍ２の座標系との関係が既知であれば、一方の座標系における値から他方の座標系における値に変換することができる。

　図４に示されるように、第１部分環境地図Ｍ１および第２部分環境地図Ｍ２を含む複数の部分環境地図の配置は、平行であるとは限らない。このような場合、第１部分環境地図Ｍ１の座標系と第２部分環境地図Ｍ２の座標系との間の変換には、単純な並進移動だけではなく、回転移動も必要になる。

　図５は、第１部分環境地図Ｍ１の座標系（Ｘ１Ｙ１座標系）および第２部分環境地図Ｍ２の座標系（Ｘ２Ｙ２座標系）との関係を模式的に示す図である。図５に示される例において、移動体１０の位置は、Ｘ１Ｙ１座標系ではＰ１＝（ｘ１，ｙ１）の値を有し、Ｘ２Ｙ２座標系ではＰ２＝（ｘ２，ｙ２）の値を有する。移動体１０の姿勢（向き）は、Ｘ１Ｙ１座標系ではθ１の値を持ち、Ｘ２Ｙ２座標系ではθ２の値を持つ。ここで、θ２－θ１＝βである。すなわち、Ｘ２Ｙ２座標系は、Ｘ１Ｙ１座標系を角度βだけ反時計回りに回転させ、Ｑ＝（Δｘ，Δｙ）だけ並進させた座標系である。

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

　Ｘ１Ｙ１座標系における移動体１０の状態を¹Ｐ＝（ｘ１，ｙ１，θ１，１）、Ｘ２Ｙ２座標系における移動体１０の状態を²Ｐ＝（ｘ２，ｙ２，θ２，１）で表す場合、以下の関係が成立する。

ここで、数１の右辺における変換行列を下記の数２に示すように、¹Ｔ₂で表すことができる。このとき、数１の式は下記の数３の式で簡単に表される。

　また、数１の式から数４の式が導かれる。

　以下の数５に示すように、¹Ｔ₂の逆行列を²Ｔ₁で表すと、数６の式が得られる。

　上記の数１および数４の関係式は、第１部分環境地図Ｍ１と第２部分環境地図Ｍ２との関係を規定している。このような関係が定まることにより、Ｘ１Ｙ１座標系における移動体１０の位置および姿勢を示す（ｘ１，ｙ１，θ１）と、Ｘ２Ｙ２座標系における移動体１０の位置および姿勢を示す（ｘ２，ｙ２，θ２）との間で、相互変換が可能になる。本開示では、ある部分環境地図の座標系と他の部分環境地図の座標系との配置関係が既知であるとき、これらの「部分環境地図は連結されている」と定義する。

　本開示の実施形態では、第１部分環境地図Ｍ１と第２部分環境地図Ｍ２とが連結されているため、たとえば図３の位置ＰＢに移動体１０があるとき、数４の式を用いることにより、第１部分環境地図Ｍ１における移動体１０の位置および姿勢（ｘ１，ｙ１，θ１）から、第２部分環境地図Ｍ２における移動体１０の位置および姿勢（ｘ２，ｙ２，θ２）を算出することができる。

　第１部分環境地図Ｍ１と第２部分環境地図Ｍ２とを「連結」するには、地図作成時において、第１部分環境地図Ｍ１と第２部分環境地図Ｍ２との境界領域または重複領域内の物理的に同一の位置および姿勢を有する移動体１０について、第１部分環境地図Ｍ１および第２部分環境地図Ｍ２のそれぞれの座標系で値を取得すればよい。具体的には、その位置にある移動体１０の位置推定装置に、第１部分環境地図Ｍ１を用いた自己位置推定と第２部分環境地図Ｍ２を用いた自己位置推定を実行させ、（ｘ１，ｙ１，θ１）および（ｘ２，ｙ２，θ２）をそれぞれ出力させればよい。位置推定装置の動作は後述する。（ｘ１，ｙ１，θ１）および（ｘ２，ｙ２，θ２）が既知であれば、数１または数４の式から、未知数であるΔｘ、Δｙ、およびβを算出することができる。その結果、並進および回転によって規定される変換行列のパラメータ（Δｘ，Δｙ，β）が決定され、「連結」が確立する。

　図６Ａは、相互に連結された第１部分環境地図Ｍ１から第２部分環境地図Ｍ２に切り替わる時刻＝ｔにおける、移動体１０の位置および姿勢、すなわちポーズ（ｘ１，ｙ１，θ１）を模式的に示す図である。図６Ａには、第１部分環境地図Ｍ１のＸ１Ｙ１座標系が記載されている。一方、図６Ｂは、時刻＝ｔにおける移動体１０の位置および姿勢（ｘ２，ｙ２，θ２）、および時刻＝ｔ＋Δtにおける移動体１０の位置および姿勢（ｘ２’，ｙ２’，θ２’）を模式的に示す図である。図６Ｂには、第２部分環境地図Ｍ２のＸ２Ｙ２座標系が記載されている。

　時刻＝ｔにおいて、第１部分環境地図Ｍ１から第２部分環境地図Ｍ２への切り替えを行う場合、第２部分環境地図Ｍ２を記憶装置１０４（図１）から読み出し、マッチングを開始するには、たとえばΔtの時間が必要になる。このため、時間Δtの間に移動体１０の位置および姿勢は、（ｘ２，ｙ２，θ２）から（ｘ２’，ｙ２’，θ２’）に変化する。

　たとえば移動体１０の駆動輪にエンコーダが取り付けられている場合は、移動体１０の位置および姿勢の変化量（Δｘ２，Δｙ２，Δθ２）＝（ｘ２’－ｘ２，ｙ２’－ｙ２，θ２’－θ２）を計測によって取得することができる。そして、後述するマッチングを行うときには、エンコーダ出力に基づいて（ｘ２，ｙ２，θ２）から補正した値、すなわち（ｘ２’，ｙ２’，θ２’）を初期値として用いることが可能である。

　しかし、本開示の実施形態では、エンコーダなどの内界センサの出力は用いることなく、部分環境地図の切り替え後のマッチングを行う。この点の詳細は、後述する。

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

　図７Ｃは、第１部分環境地図Ｍ１に対するスキャンデータＳＤ（ｔ）のマッチングが完了した状態を模式的に示す図である。マッチングが完了した状態では、外界センサ１０２がスキャンデータＳＤ（ｔ）を取得したときのセンサ座標系の位置および姿勢と第１部分環境地図Ｍ１の座標系の位置および姿勢との関係が確定する。こうして、時刻ｔにおける移動体１０の位置（センサ座標系の原点）および姿勢（センサ座標系の向き）の推定値が決定される（位置同定）。

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

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

　図９は、マッチングによって求められた時刻ｔにおける推定位置および推定姿勢を示す移動体１０が記載された第１部分環境地図を示す図である。

　図１０Ａは、時刻ｔにおける第２部分環境地図Ｍ２上の移動体１０の位置および姿勢を示す図である。図１０Ｂは、時刻ｔ＋Δtにおける第２部分環境地図Ｍ２上の移動体１０の位置および姿勢を示す図である。時刻ｔでは、第２部分環境地図Ｍ２の読み出しは完了しておらず、時刻ｔ＋Δtにおいて、第２部分環境地図Ｍ２に対するマッチングの開始が可能になる。

　図１１は、時刻ｔ＋Δtに外界センサから取得したスキャンデータＳＤ（ｔ＋Δt）の例を模式的に示す図である。このスキャンデータＳＤを第２部分環境地図Ｍ２にマッチングさせるとき、移動体１０の初期値として、図１０Ａに示されている移動体１０の位置および姿勢を用いると、図１２Ａに示されるように、スキャンデータＳＤ（ｔ＋Δt）の点群と第２部分環境地図Ｍ２の点群との間の位置ずれが大きい。このため、マッチングに要する時間が増加したり、あるいは、マッチングが所定時間内に完了せず、位置同定に失敗するという可能性がある。このような可能性がある場合、部分環境地図の切り替え時に、移動体を停止させたり、減速させたりする措置が必要になることもある。しかし、移動体１０の初期値として、図１０Ｂに示されている位置および姿勢を用いると、図１２Ｂに示されるように、スキャンデータＳＤ（ｔ＋Δt）の点群と第２部分環境地図Ｍ２の点群との間の位置ずれが小さくなる。その結果、マッチングに要する時間が短縮し、マッチングの信頼度も向上する。

　このように部分環境地図の切り替え時に移動体１０が移動するという課題は、従来、移動体１０の移動量をエンコーダなどの内界センサの出力に基づいて計測することによって解決していた。しかし、本開示の実施形態では、以下の方法により、内界センサによる移動量の測定を行うことなく、上記の課題を解決する。

　まず、部分環境地図の切り替え開始時刻をｔ、部分環境地図の切り替えに要する時間をΔｔとする。また、部分環境地図の切り替え前における移動体１０の位置および姿勢の最新の推定値が出力される時刻をｔ－Δｓで表す。具体的には、移動体１０の位置および姿勢の推定値は、周期的、たとえば２０～４０ミリ秒ごとに出力され得る。この例において、Δｓはたとえば２５ミリ秒である。

　図１３は、時刻ｔ－Δｓ、ｔ、ｔ＋Δｔにおける移動体１０の位置を模式的に示す平面図である。時刻ｔ－Δｓにおける移動体１０の位置の推定値（出力値）と、時刻ｔにおける移動体１０の位置の推定値（出力値）とから、移動体１０の時間Δｓあたりにおける位置および姿勢の変化率が算出される。この変化率を用いることにより、時刻ｔ＋Δｔにおける移動体１０の位置の予測値を決定することができる。位置の変化率（移動速度）は、時刻ｔ－Δｓにおける位置の代わりに、あるいは、時刻ｔ－Δｓにおける位置に加えて、時刻ｔ－２×Δｓ、時刻ｔ－３×Δｓなどの、過去の複数の位置を参照して決定してもよい。

　一方、部分環境地図の切り替えに要する時間Δｔは、５０ミリ秒程度である。一般に、ΔｔはΔｓよりも大きい。ここでは、部分環境地図の切り替えが行われる場所で移動体１０の姿勢（向き）はほとんど変化しないと仮定している。このため、Δｔの間に姿勢の変化は無視することが可能である。しかし、部分環境地図の切り替えが行われる場所で移動体１０の姿勢の変化を考慮してもよい。その場合、過去の複数の位置で取得した姿勢の推定値に基づいて、時刻ｔ＋Δｔにおける姿勢を予測することができる。

　ここで留意すべき点は、時間Δｓあたりにおける移動体１０の「位置の変化率（移動速度）」は、ベクトルであるため、用いる地図の座標系に依存する。このため、第１部分環境地図Ｍ１に基づいて算出された「位置の変化率」は、前述の連結関係を規定する座標変換を行うことにより、第２部分環境地図Ｍ２の座標系における「位置の変化率」に換算される。すなわち、第１部分環境地図Ｍ１上における移動体１０の推定位置および推定姿勢の履歴に基づいて、第１部分環境地図Ｍ１上における移動体１０の移動速度を決定した後、移動速度に基づいて、第２部分環境地図Ｍ１上の移動体１０の位置および姿勢を予測することができる。

　図６Ａおよび図６Ｂを再び参照する。まず、時刻ｔにおける第１部分環境地図Ｍ１の座標系上の位置および姿勢（ｘ１，ｙ１，θ１）に対応する、時刻ｔにおける第２部分環境地図Ｍ２の座標系上の対応位置および対応姿勢（ｘ２，ｙ２，θ２）を変換行列によって算出する。すなわち、第２部分環境地図Ｍ２上における対応位置および対応姿勢を、座標変換の関係に基づいて決定する。さらに、「位置の変化率」に基づいて、時刻ｔにおける第２部分環境地図Ｍ２上の位置および姿勢の値（ｘ２，ｙ２，θ２）を、時刻ｔ＋Δｔにおける第２部分環境地図Ｍ２上の位置および姿勢の値（ｘ２’，ｙ２’，θ２’）に補正する。ここで、θ２’＝θ２の近似式を用いている。

　このように本開示の実施形態では、まず、第１部分環境地図Ｍ１上における移動体１０の推定位置および推定姿勢の、第２部分環境地図Ｍ２上における対応位置および対応姿勢を部分環境地図の連結関係に基づいて算出する。そして、これらの対応位置および対応姿勢を、第１部分環境地図Ｍ１上における移動体１０の推定位置および推定姿勢の履歴に基づいて補正する。補正の計算に必要な時間Δｔは、第２部分環境地図Ｍ２上の移動体１０の位置および姿勢は、推定するためのマッチングを開始するタイミング（時刻ｔ＋Δ）に依存して決定される。さらに、このようにして補正された対応位置および対応姿勢を初期値として、第２部分環境地図Ｍ２上の移動体１０の位置および姿勢を推定するためのマッチングが行われる。

　位置同定によって取得された時刻ｔ－Δｓおよび時刻ｔにおける移動体１０の位置および姿勢の推定値から、時刻ｔ＋Δｔにおける移動体１０の位置および姿勢の予測値を決定する方法は、上記の例に限定されない。

　最初に、時刻ｔ－Δｓおよび時刻ｔにおける第１部分環境地図Ｍ１上の「位置の変化率」に基づいて、時刻ｔ＋Δｔにおける第１部分環境地図Ｍ１の座標系上の位置および姿勢の予測値を算出してもよい。その後、時刻ｔ＋Δｔにおける第１部分環境地図Ｍ１の座標系上の位置および姿勢の予測値を、第２部分環境地図Ｍ２の座標系上の値に変換行列によって変換してもよい。

　あるいは、最初に、位置同定によって取得された時刻ｔ－Δｓおよび時刻ｔにおける移動体１０の位置および姿勢の推定値を、それぞれ、第２部分環境地図Ｍ２上の値に変換行列によって変換してもよい。その後、第２部分環境地図Ｍ２の座標系における「位置の変化率」を算出し、時刻ｔ＋Δｔにおける移動体１０の位置および姿勢の予測値を決定しても良い。

　上記の方法により、本開示の実施形態では、第１部分環境地図Ｍ１上における移動体１０の推定位置および推定姿勢の履歴に基づいて、第１部分環境地図Ｍ１上における移動体１０の移動速度を決定した後、この移動速度に基づいて、時刻ｔ＋Δｔにおける第２部分環境地図Ｍ１上の移動体１０の位置および姿勢を予測することができる。そのため、地図切り替え後の時刻ｔ＋Δｔに開始するマッチングの初期値として、内界センサの出力を用いることなく、適切な値を取得することができる。

＜位置推定装置の動作フロー＞
　図１および図１４から図１６を参照しながら、本開示の実施形態における位置推定装置の動作フローを説明する。

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

　ステップＳ１０において、位置推定装置１０６は、移動体１０の位置が部分環境地図の切り替え位置にあるか否かを判定する。Ｎｏであれば、ステップＳ２０に進む。ステップＳ２０については後述する。Ｙｅｓであれば、ステップＳ１２に進む。

　ステップＳ１２において、位置推定装置１０６は、部分環境地図を切り替える。このとき、図１の記憶装置１０４から移動体１０の位置に応じて選択された部分環境地図を読み出す。読み出された部分環境地図は、位置推定装置１０６のメモリ（不図示）に記憶される。このとき、切り替え前の部分環境地図がメモリから消去される必要はない。このメモリの記憶容量は、記憶装置１０４の記憶容量よりも小さく、不要な部分環境地図はメモリから消去され得る。

　ステップＳ１４において、記憶装置１０４から新たに読み出す次の部分環境地図と前の部分環境地図との間の連結関係から、次の部分環境地図上における移動体の位置および姿勢を算出して取得する。このとき、位置推定装置１０６は、数４の変換行列を用いた変換を実行する。

　ステップＳ１６において、位置推定装置１０６は、次の部分環境地図上のマッチングを開始するタイミングでの移動体の位置および姿勢の予測値を算出する。

　ステップＳ２０において、位置推定装置１０６は、部分環境地図を切り替えた直後は、上記の予測値を初期値としてマッチングを開始し、位置同定計算を実行する。

　以下、図１５を参照して、ステップＳ２０における位置同定計算を、より詳細に説明する。

　まず、ステップＳ２２において、位置推定装置１０６は、外界センサ１０２からスキャンデータを取得する。

　ステップＳ２４において、位置推定装置１０６は、ステップＳ１６で算出した予測値を用いて初期位置合わせを行う。部分環境地図を切り替えていないとき、すなわちステップＳ１０の判定が「Ｎｏ」だったときは、時刻ｔにおける位置および姿勢を初期値としてマッチングを行うことができる。しかし、図１３を参照しながら説明した方法により、最新のスキャンデータを取得した時刻における位置および姿勢の予測値を算出し、予測値を初期値としてマッチングを行ってもよい。

　ステップＳ３０において、本実施形態における位置推定装置１０６は、ＩＣＰアルゴリズムによる位置ずれ補正を行う。

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

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

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

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

　ステップＳ３８では、座標変換行列を用いてスキャンデータの座標値をセンサ座標系の値から部分環境地図の座標系の値に変換する。

　本開示の移動体によれば、部分環境地図の切り替え後において、マッチングに要する時間が大きく増加したり、あるいは、マッチングが所定時間内に完了せず、位置同定に失敗するという可能性が低減する。このため、部分環境地図の切り替え時に、移動体を停止させたり、減速させりする措置が不要になる。

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

　本開示の移動体は、駆動装置を備えている必要がない。たとえばユーザによって駆動される手押し車であってもよい。このような移動体は、位置推定装置から取得した移動体の位置、または、位置および姿勢をユーザのために表示装置の地図上に表示したり、音声で伝えてもよい。

　図１７は、本開示による移動体１０の他の構成例を示す図である。この図に示される移動体１０は、位置推定装置１０６から出力された移動体の位置、または、位置および姿勢の情報を用いてユーザを案内するナビゲーション装置１１４を備えている。このナビゲーション装置１１４は、画像または音声により、ユーザを案内するため表示装置１１６に接続されている。表示装置１１６は、現在位置および目的位置を地図上に表示したり、「停止」または「右に曲がれ」などの音声を発したりすることができる。このため、ユーザは、移動体１０を押して目的地まで移動させることができる。このような移動体１０の例は、手押し車、その他のカートである。

　図１７に示される構成を備えた移動体１０によれば、ナビゲーション装置１１４内のメモリ（不図示）に予め記憶された目的地または経路に基づいて、ルート案内が実行される。

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

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

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

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

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

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

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

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

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

　ＡＧＶ１０は、ユーザの操作によってデータ取得モードに遷移する。データ取得モードにおいて、ＡＧＶ１０はＬＲＦを用いたセンサデータの取得を開始する。

　位置推定装置は、センサデータを記憶装置に蓄積する。環境Ｓ内のセンサデータの取得が完了すると、記憶装置に蓄積されたセンサデータが外部装置に送信される。外部装置は、たとえば信号処理プロセッサを有し、かつ、地図作成コンピュータプログラムがインストールされたコンピュータである。

　外部装置の信号処理プロセッサは、スキャンごとに得られたセンサデータ同士を重ね合わせる。信号処理プロセッサが重ね合わせる処理を繰り返し行うことにより、環境Ｓの地図を作成することができる。外部装置は、作成した地図のデータをＡＧＶ１０に送信する。ＡＧＶ１０は、作成した地図のデータを内部の記憶装置に保存する。外部装置は、運行管理装置５０であってもよいし、他の装置であってもよい。

　外部装置ではなくＡＧＶ１０が地図の作成を行ってもよい。上述した外部装置の信号処理プロセッサが行った処理を、ＡＧＶ１０のマイクロコントローラユニット（マイコン）などの回路が行えばよい。ＡＧＶ１０内で地図を作成する場合には、蓄積されたセンサデータを外部装置に送信する必要がなくなる。センサデータのデータ容量は一般には大きいと考えられる。センサデータを外部装置に送信する必要がないため、通信回線の占有を回避できる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims

　移動体であって、
　環境をスキャンしてスキャンデータを周期的に出力する外界センサと、
　座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む複数の部分環境地図を記憶する記憶装置と、
　前記スキャンデータと前記記憶装置から読み出された前記複数の部分環境地図のいずれかとのマッチングを行い、前記移動体の位置および姿勢を推定する位置推定装置と、
を備え、
　前記位置推定装置は、
　前記移動体の推定位置が前記第１部分環境地図上から前記第２部分環境地図上に移動したとき、
　前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の、前記第２部分環境地図上における対応位置および対応姿勢を、前記座標変換の関係に基づいて決定し、
　前記第２部分環境地図上の前記移動体の位置および姿勢を推定するためのマッチングを開始するタイミングにおける前記第２部分環境地図上の前記対応位置および前記対応姿勢を、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の履歴に基づいて補正し、
　前記補正した前記対応位置および前記対応姿勢を初期値として、前記第２部分環境地図上の前記移動体の前記位置および前記姿勢を推定するための前記マッチングを行う、移動体。
　前記位置推定装置は、
　前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の履歴に基づいて、前記第１部分環境地図上における前記移動体の移動速度を決定し、前記移動速度に基づいて、前記タイミングにおける前記第２部分環境地図上の前記移動体の位置および姿勢を予測する、請求項１に記載の移動体。
　前記位置推定装置は、反復最近接点アルゴリズムによって前記マッチングを行う、請求項１または２に記載の移動体。
　全方位ホイール、二足または多足歩行装置のいずれかを備える、請求項１から３のいずれかに記載の移動体。
　前記位置推定装置が推定した前記移動体の前記位置、または、前記位置および前記姿勢を画像および／または音声によって表示する表示装置を備える、請求項１から３のいずれかに記載の移動体。
　目的地と、前記位置推定装置が推定した前記移動体の前記位置および前記姿勢とに基づいて、ユーザに経路をガイドするナビゲーション装置を備える、請求項５に記載の移動体。
　環境をスキャンしてスキャンデータを周期的に出力する外界センサと、座標変換の関係によって連結された第１部分環境地図および第２部分環境地図を含む複数の部分環境地図を記憶する記憶装置とに接続される、移動体の位置推定装置であって、
　プロセッサと、
　前記プロセッサを動作させるコンピュータプログラムを記憶するメモリと、
を備え、
　前記プロセッサは、前記コンピュータプログラムの指令にしたがって、
　前記移動体の推定位置が前記第１部分環境地図上から前記第２部分環境地図上に移動したとき、
　前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の、前記第２部分環境地図上における対応位置および対応姿勢を、前記座標変換の関係に基づいて決定すること、
　前記第２部分環境地図上の前記移動体の位置および姿勢を推定するためのマッチングを開始するタイミングにおける前記第２部分環境地図上の前記対応位置および前記対応姿勢を、前記第１部分環境地図上における前記移動体の推定位置および推定姿勢の履歴に基づいて補正すること、および、
　前記補正した前記対応位置および前記対応姿勢を初期値として、前記第２部分環境地図上の前記移動体の前記位置および前記姿勢を推定するための前記マッチングを行うこと、を実行する、位置推定装置。
　請求項７に記載の位置推定装置に使用されるコンピュータプログラム。