特開 2024-4892 自己位置推定装置、自己位置推定方法、およびプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024004892

(43)【公開日】2024-01-17

(54)【発明の名称】自己位置推定装置、自己位置推定方法、およびプログラム

(51)【国際特許分類】

   G01C 21/20 20060101AFI20240110BHJP

   G05D 1/43 20240101ALI20240110BHJP

【ＦＩ】

G01C21/20

G05D1/02 J

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022104777

(22)【出願日】2022-06-29

(71)【出願人】

【識別番号】000006208

【氏名又は名称】三菱重工業株式会社

(74)【代理人】

【識別番号】100149548

【弁理士】

【氏名又は名称】松沼 泰史

(74)【代理人】

【識別番号】100162868

【弁理士】

【氏名又は名称】伊藤 英輔

(74)【代理人】

【識別番号】100161702

【弁理士】

【氏名又は名称】橋本 宏之

(74)【代理人】

【識別番号】100189348

【弁理士】

【氏名又は名称】古都 智

(74)【代理人】

【識別番号】100196689

【弁理士】

【氏名又は名称】鎌田 康一郎

(72)【発明者】

【氏名】中尾 健太

(72)【発明者】

【氏名】杉本 喜一

(72)【発明者】

【氏名】竹内 瑞穂

(72)【発明者】

【氏名】木内 裕介

(72)【発明者】

【氏名】▲高▼尾 健司

(72)【発明者】

【氏名】筈井 祐介

【テーマコード（参考）】

2F129

5H301

【Ｆターム（参考）】

2F129AA03

2F129BB15

2F129BB22

2F129BB26

2F129BB33

2F129BB66

2F129CC33

5H301BB07

5H301CC04

5H301CC07

5H301CC08

5H301GG09

5H301GG16

(57)【要約】

【課題】自律移動体の位置および姿勢を精度よく推定することができる自己位置推定装置を提供する。
【解決手段】自己位置推定装置は、自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得する第１取得部と、前記自律移動体の加速度および角速度を含む第２センサデータを取得する第２取得部と、複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータの一部である部分パラメータを推定する第１推定部と、前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記自律移動体の自己位置を推定する第２推定部と、を備える。
【選択図】図２

【特許請求の範囲】

【請求項1】

自律移動体の走行エリアにおける自己位置を推定する自己位置推定装置であって、
自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得する第１取得部と、
前記自律移動体の加速度および角速度を含む第２センサデータを取得する第２取得部と、
複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータの一部である部分パラメータを推定する第１推定部と、
前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定する第２推定部と、
を備える自己位置推定装置。

【請求項2】

前記第２推定部は、前記６自由度のパラメータのうち、前記部分パラメータ以外のパラメータを推定する、
請求項１に記載の自己位置推定装置。

【請求項3】

前記第２推定部は、前記部分パラメータに基づく制約を加えた最適化計算を行って、前記６自由度のパラメータ全てを推定する、
請求項１に記載の自己位置推定装置。

【請求項4】

前記第２推定部は、前記自律移動体の運動制約をさらに加えた最適化計算を行って、前記６自由度のパラメータ全てを推定する、
請求項３に記載の自己位置推定装置。

【請求項5】

前記目印は、前記走行エリアに施されたペイントおよび前記走行エリアに設置された標識の少なくとも一方であり、
前記第１推定部は、前記第１センサデータから抽出した前記ペイントおよび前記標識の少なくとも一方と、前記既知情報とに基づいて、前記６自由度のパラメータのうち、水平方向に延びる第１軸方向および第２軸方向の少なくとも一方における位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを前記部分パラメータとして推定する、
請求項１から４の何れか一項に記載の自己位置推定装置。

【請求項6】

前記目印は前記固定物の一部に取り付けられたランドマークであり、
前記第１推定部は、前記第１センサデータから抽出した前記ランドマークと、前記既知情報とに基づいて、前記６自由度のパラメータのうち、水平方向に延びる第１軸方向および第２軸方向それぞれにおける位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを前記部分パラメータとして推定する、
請求項１から４の何れか一項に記載の自己位置推定装置。

【請求項7】

前記第１推定部は、前記第１センサデータから抽出した複数の前記ランドマークを繋いで得た形状と、前記既知情報に記録されている前記ランドマークの配置から得られる形状とに基づいて、前記６自由度のパラメータのうち、水平方向に延びる第１軸方向における位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを前記部分パラメータとして推定する、
請求項６に記載の自己位置推定装置。

【請求項8】

自律移動体の走行エリアにおける自己位置を推定する自己位置推定方法であって、
自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、
前記自律移動体の加速度および角速度を含む第２センサデータを取得するステップと、
複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、
前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定するステップと、
を有する自己位置推定方法。

【請求項9】

自律移動体の走行エリアにおける自己位置を推定する自己位置推定装置に、
自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、
前記自律移動体の加速度および角速度を含む第２センサデータを取得するステップと、
複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、
前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定するステップと、
を実行させるプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、自律移動体の自己位置推定装置、自己位置推定方法、およびプログラムに関する。

【背景技術】

【0002】

物流やプラント点検などで使用される自律移動体は、例えば、レーザセンサやカメラ（画像センサ）で取得したセンサデータに基づいて予め作成した地図データと、現在のセンサデータとを照合して、自律移動体の自己位置（３次元座標系の各軸に対する位置および姿勢）を推定する。しかしながら、例えば物流倉庫のような多段積みされた荷物で通路が構成されている施設で自律移動体を運用する場合、これら荷物は一時的に存在する非固定物のため、事前に地図データに反映させることができない。そうすると、一時的に荷物が置かれているエリアでは、自律移動体の自己位置の推定精度が低下する可能性がある。

【0003】

このため、レーザセンサやカメラによる自己位置の推定が困難なエリアでは、他のセンサを代用して自己位置の推定を行う技術が考えられている。例えば、特許文献１には、エンコーダ、慣性計測ユニット（ＩＭＵ；Inertial Measurement Unit）、ＧＮＳＳ（Global Navigation Satellite System）受信機、カメラ、レーザセンサ等の複数の異なるセンサそれぞれで計測したセンサデータを取得し、個々のセンサデータから自律移動体の自己位置候補をそれぞれ推定し、最も信頼度の高い自己位置候補を自律移動体の自己位置として決定する方法が記載されている。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０２０－８７３０７号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかしながら、特許文献１の技術は、個々のセンサデータから独立に推定される自己位置候補から一つを選択して自己位置として決定するものであるため、自己位置の精度はセンサによる自己位置推定結果に依存する。

【0006】

また、複数のセンサを組み合わせた自己位置推定技術が考えられている。カメラとＩＭＵを組み合わせた自己位置推定技術のＶＩＯ（Visual Inertial Odometry）では、カメラ画像から得られる特徴点の移動量と、ＩＭＵから得られる加速度および角速度とが一致するような自己位置を最適化計算（例えば、非線形最小二乗法）によって算出する。

【0007】

しかしながら、このようなオドメトリ法は、時間軸で見た局所的な最適化問題を解くものであるため、誤差の累積により、推定位置が実際の位置から徐々にずれていくドリフトが生じる場合がある。このような累積誤差の影響などを低減し、自己位置を精度よく推定できる技術が望まれている。

【0008】

本開示の目的は、自律移動体の位置および姿勢を精度よく推定することができる自己位置推定装置、自己位置推定方法、およびプログラムを提供することにある。

【課題を解決するための手段】

【0009】

本開示の一態様によれば、自律移動体の走行エリアにおける自己位置を推定する自己位置推定装置は、自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得する第１取得部と、前記自律移動体の加速度および角速度を含む第２センサデータを取得する第２取得部と、複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータの一部である部分パラメータを推定する第１推定部と、前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定する第２推定部と、を備える。

【0010】

本開示の一態様によれば、自律移動体の走行エリアにおける自己位置を推定する自己位置推定方法は、自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、前記自律移動体の加速度および角速度を含む第２センサデータを取得するステップと、複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定するステップと、を有する。

【0011】

本開示の一態様によれば、プログラムは、自律移動体の走行エリアにおける自己位置を推定する自己位置推定装置に、自律移動体の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、前記自律移動体の加速度および角速度を含む第２センサデータを取得するステップと、複数の前記固定物から予め選択した一の固定物である目印の前記走行エリアにおける位置を含む情報を記録した既知情報と、前記第１センサデータとに基づいて、３次元座標系における前記自律移動体の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、前記第１センサデータと、前記第２センサデータと、前記部分パラメータとに基づいて、前記６自由度のパラメータで表される前記自律移動体の自己位置を推定するステップと、を実行させる。

【発明の効果】

【0012】

上記態様によれば、自律移動体の位置および姿勢を精度よく推定することができる。

【図面の簡単な説明】

【0013】

【図1】第１の実施形態に係る自律移動体の全体構成を示す概略図である。

【図2】第１の実施形態に係る自己位置推定装置の機能構成を示すブロック図である。

【図3】第１の実施形態に係る自己位置推定装置の処理の一例を示すフローチャートである。

【図4】第１の実施形態に係る第１推定部の処理の一例を示すフローチャートである。

【図5】第１の実施形態に係る自己位置推定装置の機能を説明する図である。

【図6】第１の実施形態に係る第２推定部の処理の一例を示すフローチャートである。

【図7】第２の実施形態に係る自己位置推定装置の機能を説明する図である。

【図8】第２の実施形態に係る第１推定部の処理の一例を示すフローチャートである。

【図9】第３の実施形態に係る自己位置推定装置の機能を説明する第１の図である。

【図10】第３の実施形態に係る自己位置推定装置の機能を説明する第２の図である。

【図11】第３の実施形態に係る第１推定部の処理の一例を示すフローチャートである。

【図12】第３の実施形態に係る自己位置推定装置の機能を説明する第３の図である。

【図13】第３の実施形態に係る自己位置推定装置の機能を説明する第４の図である。

【図14】第４の実施形態に係る自己位置推定装置の機能を説明する図である。

【図15】第４の実施形態に係る第１推定部の処理の一例を示すフローチャートである。

【図16】第５の実施形態に係る第２推定部の処理の一例を示すフローチャートである。

【図17】第６の実施形態に係る第２推定部の処理の一例を示すフローチャートである。

【発明を実施するための形態】

【0014】

＜第１の実施形態＞
以下、図１～図６を参照しながら第１の実施形態について詳しく説明する。

【0015】

（自律移動体の全体構成）
図１は、第１の実施形態に係る自律移動体の全体構成を示す概略図である。
図１に示すように、本実施形態に係る自律移動体１は、例えば、物流倉庫などの走行エリア内においてあらかじめ定められた走行ルートに沿って目標地点まで自律走行する無人フォークリフトである。なお、他の実施形態では、自律移動体１は、プラント点検などに用いられる点検用ロボットなどであってもよい。

【0016】

自律移動体１は、自己位置推定装置１０と、第１センサ２０と、第２センサ２１とを備える。

【0017】

第１センサ２０は、自律移動体１の周囲を撮影した画像データ（映像データの各フレーム）を取得するカメラである。

【0018】

第２センサ２１は、自律移動体１の加速度および角速度を計測する慣性計測装置（以下、「ＩＭＵ」とも記載する。）である。

【0019】

自己位置推定装置１０は、第１センサ２０が取得した画像データ（第１センサデータ）と、第２センサ２１が計測した加速度および角速度（第２センサデータ）とに基づいて、自律移動体１の自己位置を推定する。

【0020】

（自己位置推定装置の機能構成）
図２は、第１の実施形態に係る自己位置推定装置の機能構成を示すブロック図である。
図２に示すように、自己位置推定装置１０は、プロセッサ１１と、メモリ１２と、ストレージ１３と、インタフェース１４とを備える。

【0021】

メモリ１２は、プロセッサ１１の動作に必要なメモリ領域を有する。

【0022】

ストレージ１３は、いわゆる補助記憶装置であって、例えば、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）等である。

【0023】

インタフェース１４は、外部機器（例えば、第１センサ２０および第２センサ２１）との間で各種情報の送受信を行うためのインタフェースである。

【0024】

プロセッサ１１は、所定のプログラムに従って動作することにより、第１取得部１１０、第２取得部１１１、地図照合部１１２、第１推定部１１３、第２推定部１１４、出力処理部１１５としての機能を発揮する。

【0025】

第１取得部１１０は、第１センサ２０から、自律移動体１の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な画像データ（第１センサデータ）を取得する。例えば、固定物は、走行エリア内の天井、床、棚、看板、ペイントなどの配置や形状が変化しない構造物である。また、非固定物は、床や棚などに一時的に載置される荷物やパレットなどである。

【0026】

第２取得部１１１は、第２センサ２１から、自律移動体１の加速度および角速度（第２センサデータ）を取得する。

【0027】

地図照合部１１２は、第１センサデータと、予めストレージ１３に記録された地図データＤ１とを照合して、自律移動体１の自己位置を推定する。なお、自律移動体１の自己位置は、ワールド座標系の座標軸（Ｘｗ，Ｙｗ，Ｚｗ）方向それぞれにおける位置と、各座標軸回りの回転角度（姿勢）とからなる６自由度のパラメータで表される。

【0028】

地図データＤ１は、予め走行エリア内で自律移動体１をテスト走行させて第１センサデータを収集し、各第１センサデータを取得した時点における自律移動体１の自己位置を、各第１センサデータに付したサンプルデータの集合である。地図照合部１１２は、第１取得部１１０が取得した第１センサデータと地図データＤ１の各サンプルデータとを照合して、第１センサデータと一致するサンプルデータに付された自己位置を、自律移動体１の自己位置であると推定する。

【0029】

第１推定部１１３は、予めストレージ１３に記録された既知情報Ｄ２と、第１センサデータとに基づいて、３次元座標系における位置および姿勢を表す６自由度のパラメータのうち少なくとも１つである部分パラメータを推定する。既知情報Ｄ２は、複数の固定物から予め選択した一の固定物である目印について、自律移動体１の走行エリアにおける位置や角度、形状などを予め記録したものである。例えば、目印は、走行エリアの床に施されたペイント（白線など）や、走行エリア内に設置された標識（看板など）である。目印は、非固定物があるときも少なくとも一部が観測可能となる固定物であることが望ましい。

【0030】

第２推定部１１４は、第１センサデータと、第２センサデータと、第１推定部１１３が推定した部分パラメータとに基づいて、６自由度のパラメータで表される自律移動体の自己位置を推定する。

【0031】

出力処理部１１５は、地図照合部１１２または第２推定部１１４が推定した自律移動体１の自己位置を、自律移動体１の動作を制御する制御装置（不図示）などに出力する。

【0032】

（自己位置推定装置の処理フロー）
図３は、第１の実施形態に係る自己位置推定装置の処理の一例を示すフローチャートである。
以下、図３を参照しながら、自己位置推定装置１０が自律移動体１の自己位置を推定する処理の流れについて説明する。

【0033】

まず、第１取得部１１０は、第１センサ２０から第１センサデータを取得する。また、第２取得部１１１は、第２センサ２１から第２センサデータを取得する（ステップＳ１００）。本実施形態では、第１センサデータは、カメラが撮影した映像の最新フレームを表す画像データであり、第２センサデータはＩＭＵが計測した自律移動体１の加速度および角速度である。

【0034】

次に、地図照合部１１２は、第１センサデータと地図データＤ１とを照合して、自律移動体１の自己位置を推定する（ステップＳ１０１）。例えば、地図照合部１１２は、既知のパターンマッチング処理を用いて、第１取得部１１０が取得した第１センサデータおよび地図データＤ１の各サンプルデータを照合し、所定の一致度以上かつ最も一致度の高いサンプルデータを、第１センサデータに一致するデータとして抽出する。地図照合部１１２は、抽出したサンプルデータに付された自己位置を、第１センサデータ取得時点における自律移動体１の自己位置であると推定する。

【0035】

なお、例えば棚や床に荷物が一時的に置かれるなどして、地図データＤ１生成時における走行エリアの風景と、実際に自律移動体１が走行したときの走行エリアの風景とが異なる場合がある。そうすると、第１センサデータと一致するサンプルデータが地図データＤ１内に存在せず、自律移動体１の自己位置を推定できない。このため、自己位置推定装置１０は、地図照合により自律移動体１の自己位置の推定が完了したか判定する（ステップＳ１０２）。

【0036】

地図照合部１１２が地図照合により自己位置の推定が完了した場合（ステップＳ１０２；ＹＥＳ）、出力処理部１１５は、地図照合部１１２が推定した自己位置を自律移動体１の制御装置（不図示）に出力する（ステップＳ１０５）。また、自己位置推定装置１０は、ステップＳ１００に戻り、図３の一連の処理を再度実行する。

【0037】

一方、地図照合部１１２が地図照合により自己位置の推定ができなかった場合（ステップＳ１０２；ＮＯ）、第１推定部１１３および第２推定部１１４による自己位置推定処理に進む。

【0038】

まず、第１推定部１１３は、第１センサデータに含まれる目印と、既知情報Ｄ２とに基づいて、自律移動体１の自己位置を表す６自由度のパラメータのうち少なくとも１つのパラメータを推定する処理を行う（ステップＳ１０３）。本実施形態では、目印が走行エリアの床に施された白線である態様を例に説明する。なお、他の実施形態では、走行エリアの壁や天井に施されたペイントを目印として用いてもよい。

【0039】

図４は、第１の実施形態に係る第１推定部の処理の一例を示すフローチャートである。
第１推定部１１３の処理について、図４を参照しながら説明する。まず、第１推定部１１３は、第１センサデータに所定の画像変換処理を施す（ステップＳ１１０）。この画像変換処理は、例えば、一般的な歪み補正、鳥観視点への変換などの処理である。

【0040】

次に、第１推定部１１３は、画像変換処理後の第１センサデータから白線を抽出する処理を行う（ステップＳ１１１）。例えば、第１推定部１１３は、第１センサデータにさらに２値化処理を施すことにより、第１センサデータに含まれる白線を抽出する。

【0041】

また、第１推定部１１３は、第１センサデータから抽出した白線および既知情報Ｄ２に基づいて、自律移動体１のＸｗ軸における位置およびＺｗ軸回りの回転角度（姿勢）を推定する処理を行う（ステップＳ１１２）。

【0042】

図５は、第１の実施形態に係る自己位置推定装置の機能を説明する図である。
図５に示すように、走行エリアＲの床には白線Ｍ１が設けられている。また、走行エリアＲの床や棚には荷物Ｂ１が一時的に置かれている場合であっても、少なくとも何れかの白線Ｍ１（図５の例では、自律移動体１の右側の白線Ｍ１）は、荷物Ｂ１で隠れることなく第１センサ２０で観測可能となる位置に白線Ｍ１が設けられる。図５の例では、自律移動体１（車両座標系）の左右方向をＸｖ軸、前後方向をＹｖ軸、鉛直方向をＺｖ軸で表す。また、走行エリアＲ（ワールド座標系）の水平方向（例えば、東西方向および南北方向）をＸｗ軸およびＹｗ軸、鉛直方向をＺｗ軸で表す。

【0043】

既知情報Ｄ２には、白線Ｍ１が走行エリアＲ内のどの位置にどの角度で設けられているかを示す情報や、白線Ｍ１の幅などを示す情報が予め記録されている。例えば、第１推定部１１３は、前回の地図照合部１１２の自己位置推定結果や、予め定められた走行ルートの情報などから、自律移動体１が走行エリアＲ内のどの走行路に進入するかを特定し、既知情報Ｄ２から進入先の走行路に引かれた白線Ｍ１に関する情報を抽出する。また、白線Ｍ１上に走行路の識別情報（走行路番号やバーコードなど）が印字されている場合、第１推定部１１３は、第１センサデータから既知の文字認識処理やバーコード読取処理などでこの識別情報を読み取り、識別情報で特定される白線Ｍ１に関する情報を既知情報Ｄ２から抽出してもよい。

【0044】

第１推定部１１３は、既知情報Ｄ２から抽出した白線Ｍ１の情報と、第１センサデータ（画像データ）における白線Ｍ１の見え方（大きさ、傾きなど）とに基づいて、自律移動体１のワールド座標系におけるＸｗ軸方向の位置Ｘｗ１と、Ｚｗ軸回りの回転角度θＺｗ１の２自由度のパラメータ（部分パラメータ）を推定することができる。

【0045】

次に、図３に戻り、第２推定部１１４は最適化計算による自己位置推定処理を実行する（ステップＳ１０４）。

【0046】

図６は、第１の実施形態に係る第２推定部の処理の一例を示すフローチャートである。
第２推定部１１４の処理について、図６を参照しながら説明する。

【0047】

まず、第２推定部１１４は、第１推定部１１３が既知情報Ｄ２で求めた自己位置（部分パラメータ）をセットする（ステップＳ１２０）。なお、自己位置推定における最適化計算は、１つ前（前回フレーム）の状態からの相対的な位置姿勢の変化（オドメトリ）を用いて計算するものである。このため、本実施形態では、第１推定部１１３が推定したＸｗ軸における位置Ｘｗ１およびＺｗ軸回りの回転角度θＺｗ１を、前回フレームの自己位置推定結果のＸｗおよびθＺｗから引いた相対値に変換してセットする。

【0048】

次に、第２推定部１１４は、第１推定部１１３が推定した２自由度のパラメータＸｗ１，θＺｗ１を除く、Ｙｗ軸およびＺｗ軸方向における位置Ｙｗ１，Ｚｗ１と、Ｘｗ軸回りおよびＹｗ軸回りの回転角度θＸｗ１，θＹｗ１の４自由度のパラメータについて、前回フレームとの相対値を最適化計算により推定する（ステップＳ１２１）。具体的には、第２推定部１１４は、Ｘｗ１，θＺｗ１の相対値を固定として以下の式（１）を解くことにより、残りの４自由度のパラメータの相対値を求める。

【0049】

【数1】

【0050】

式（１）において、χは前回フレームｆから最新フレームｆ＋１における自己位置の変化量（前回フレームとの相対値）の推定値である。また、式（１）の波括弧内の第１項は前回フレームｆから最新フレームｆ＋１における位置姿勢変化とその間のＩＭＵ積分値との残差（ＩＭＵ残差）、第２項は前回フレームｆから最新フレームｆ＋１における位置姿勢変化の画像上での再投影誤差（画像残差）を表す。

【0051】

次に、第２推定部１１４は、ステップＳ１２０およびＳ１２１で求めた各パラメータの前回フレームとの相対値と、前回フレームの自己位置推定結果とに基づいて、今回フレームの自律移動体１のワールド座標系における自己位置を算出する（ステップＳ１２２）。

【0052】

また、図３に戻り、出力処理部１１５は、第１推定部１１３および第２推定部１１４が推定した自己位置（６自由度のパラメータ）を自律移動体１の制御装置（不図示）に出力する（ステップＳ１０５）。また、自己位置推定装置１０は、ステップＳ１００に戻り、図３の一連の処理を再度実行する。

【0053】

（作用、効果）
以上のように、本実施形態に係る自己位置推定装置１０は、第１センサデータを取得する第１取得部１１０と、第２センサデータを取得する第２取得部１１１と、第１センサデータに含まれる目印と既知情報Ｄ２とに基づいて、自律移動体１の自己位置を表す６自由度のパラメータのうち一部である部分パラメータを推定する第１推定部１１３と、第１センサデータと、第２センサデータと、第１推定部１１３が推定した部分パラメータとに基づいて、残りのパラメータを推定する第２推定部１１４と、を備える。

【0054】

このように、自己位置推定装置１０は、第１センサデータから間接的に観測できる既知情報Ｄ２を活用して一部のパラメータを推定することにより、最適化計算における累積誤差を抑制して、自律移動体１の自己位置を精度よく推定することができる。

【0055】

また、第２推定部１１４は、６自由度のパラメータのうち、第１推定部１１３が推定した部分パラメータ以外のパラメータを推定する。

【0056】

このようにすることで、自己位置推定装置１０は、一部パラメータが既知となり、他のパラメータのみを求めればよいので、累積誤差の影響をより効果的に抑制することができる。また、自己位置推定装置１０は、最適化計算を高速かつ低負荷で行うことが可能となる。

【0057】

また、目印は、走行エリアＲに予め施された白線Ｍ１（ペイント）であり、第１推定部１１３は、第１センサデータから抽出した白線Ｍ１と既知情報Ｄ２とに基づいて、６自由度のパラメータのうち、Ｘｗ軸方向における位置Ｘｗ１と、Ｚｗ軸回りの回転で表される姿勢θＺｗ１とを部分パラメータとして推定する。

【0058】

このようにすることで、自己位置推定装置１０は、走行エリアＲの白線Ｍ１から、６自由度のうち２自由度のパラメータを精度よく推定することができる。これにより、自己位置推定装置１０は、他の４自由度のパラメータの推定において、累積誤差の影響をより効果的に抑制することができる。

【0059】

＜第２の実施形態＞
以下、図７～図８を参照しながら第２の実施形態について詳しく説明する。なお、第２の実施形態の構成のうち、第１の実施形態と同じ構成であるものについては、第１の実施形態と同じ符号を用いて説明する。

【0060】

図７は、第２の実施形態に係る自己位置推定装置の機能を説明する図である。
上述の第１の実施形態では、自己位置推定装置１０の第１推定部１１３が走行エリアＲの白線Ｍ１を目印として、６自由度のうち２つのパラメータを推定する例について説明した。これに対し、本実施形態に係る自己位置推定装置１０の第１推定部１１３は、図８の例のように、走行エリアＲに設置された標識Ｍ２（看板など）を目印として、６自由度のうち２つのパラメータを推定する。標識Ｍ２は、荷物Ｂ１などで隠れることなく第１センサ２０で観測可能となる位置に標識Ｍ２が設置される。

【0061】

（自己位置推定装置の処理フロー）
図８は、第２の実施形態に係る第１推定部の処理の一例を示すフローチャートである。
本実施形態例に係る第１推定部１１３は、第１の実施形態（図４）の処理に代えて、図８の処理を実行する。

【0062】

まず、図８に示すように、第１推定部１１３は、第１センサデータに所定の画像変換処理を施す（ステップＳ１３０）。当該処理は図４のステップＳ１１０と同じである。

【0063】

次に、第１推定部１１３は、画像変換処理後の第１センサデータに対し、既知のエッジ検出処理やパターン認識処理などを施して、標識Ｍ２を検出する処理を行う（ステップＳ１３１）。

【0064】

また、第１推定部１１３は、第１センサデータから抽出した白線および既知情報Ｄ２に基づいて、自律移動体１のＹｗ軸における位置およびＺｗ軸回りの回転角度を推定する処理を行う（ステップＳ１３２）。

【0065】

既知情報Ｄ２には、標識Ｍ２が走行エリアＲ内のどの位置にどの角度で設置されているかを示す情報や、標識Ｍ２のサイズなどを示す情報が予め記録されている。第１推定部１１３は、第１の実施形態と同様に、前回の地図照合部１１２の自己位置推定結果や、予め定められた走行ルートの情報などから、自律移動体１が走行エリアＲ内のどの走行路に進入するかを特定し、既知情報Ｄ２から進入先の走行路に設置された標識Ｍ２に関する情報を抽出する。また、標識Ｍ２上に走行路の識別情報（走行路番号やバーコードなど）が印字されている場合、第１推定部１１３は、第１センサデータから既知の文字認識処理やバーコード読取処理などでこの識別情報を読み取り、識別情報で特定される標識Ｍ２に関する情報を既知情報Ｄ２から抽出してもよい。

【0066】

第１推定部１１３は、既知情報Ｄ２から抽出した標識Ｍ２の情報と、第１センサデータ（画像データ）における標識Ｍ２の見え方（大きさ、傾きなど）とに基づいて、自律移動体１のワールド座標系におけるＹｗ軸方向の位置Ｙｗ１と、Ｚｗ軸回りの回転角度θＺｗ１）の２自由度のパラメータ（部分パラメータ）を推定することができる。

【0067】

以降の処理（図３のステップＳ１０４～Ｓ１０５）については、第１の実施形態と同じである。

【0068】

（作用、効果）
以上のように、本実施形態において、目印は、走行エリアＲに予め設置された標識Ｍ２であり、第１推定部１１３は、第１センサデータから抽出した標識Ｍ２と既知情報Ｄ２とに基づいて、６自由度のパラメータのうち、Ｙｗ軸方向における位置Ｙｗ１と、Ｚｗ軸回りの回転で表される姿勢θＺｗ１とを部分パラメータとして推定する。

【0069】

このようにすることで、自己位置推定装置１０は、走行エリアＲの標識Ｍ２から、６自由度のうち２自由度のパラメータを精度よく推定することができる。これにより、自己位置推定装置１０は、他の４自由度のパラメータの推定において、累積誤差の影響をより効果的に抑制することができる。

【0070】

また、他の実施形態では、自己位置推定装置１０の第１推定部１１３は、白線Ｍ１および標識Ｍ２の両方を目印として抽出し、６自由度のパラメータのうち、２自由度または３自由度のパラメータを推定してもよい。例えば、第１推定部１１３は、第１センサデータから白線Ｍ１のみを検出した場合、第１の実施形態と同様に、Ｘｗ１およびθＺｗ１の２自由度のパラメータを推定する。また、第１推定部１１３は、第１センサデータから標識Ｍ２のみを検出した場合、第２の実施形態と同様に、Ｙｗ１およびθＺｗ１の２自由度のパラメータを推定する。さらに、第１推定部１１３は、第１センサデータから白線Ｍ１および標識Ｍ２の両方を検出した場合、これらに基づいて、Ｘｗ１、Ｙｗ１およびθＺｗ１の３自由度のパラメータを推定する。このようにすることで、走行エリアＲ内の棚などの配置にあわせて白線Ｍ１および標識Ｍ２の少なくとも一方を設置すればよいので、さまざまなレイアウトの走行エリアＲに柔軟に対応することができる。また、第１推定部１１３が白線Ｍ１および標識Ｍ２の両方から３自由度のパラメータを推定できる場合は、第２推定部１１４の最適化計算において、累積誤差の影響をより効果的に抑制することができる。

【0071】

＜第３の実施形態＞
以下、図９～図１３を参照しながら第３の実施形態について詳しく説明する。なお、第３の実施形態の構成のうち、第１～第２の実施形態と同じ構成であるものについては、第１～第２の実施形態と同じ符号を用いて説明する。

【0072】

図９は、第３の実施形態に係る自己位置推定装置の機能を説明する第１の図である。
図１０は、第３の実施形態に係る自己位置推定装置の機能を説明する第２の図である。
上述の第１～第２の実施形態では、自己位置推定装置１０の第１推定部１１３が走行エリアＲの白線Ｍ１または標識Ｍ２を目印として、６自由度のうち２つのパラメータを推定する例について説明した。これに対し、本実施形態に係る自己位置推定装置１０の第１推定部１１３は、図９～図１０に示すように、走行エリアＲの固定物に取り付けられたランドマークＭ３を目印として、６自由度のうち３つのパラメータを推定する。

【0073】

図９は走行エリアＲを鉛直方向上方（＋Ｚｗ）から見た図、図１０は棚Ｂ２を水平方向一方（＋Ｘｗ）から見た図である。図９および図１０に示すように、ランドマークＭ３は、荷物Ｂ１などで隠れることなく第１センサ２０で観測可能となるように、荷物Ｂ１を載置する棚Ｂ２の柱の下部（足元）などに取り付けられる。

【0074】

また、本実施形態において、自律移動体１の第１センサ２０は、ＬｉＤＡＲ（Light Detection and Ranging）であり、自律移動体１の周辺の固定物および非固定物の点群データＰを取得する。また、自己位置推定装置１０の第１取得部１１０は、図３のステップＳ１００において、第１センサ２０から点群データである第１センサデータを取得する。

【0075】

（自己位置推定装置の処理フロー）
図１１は、第３の実施形態に係る第１推定部の処理の一例を示すフローチャートである。
本実施形態例に係る第１推定部１１３は、第１の実施形態（図４）または第２の実施形態（図８）の処理に代えて、図１１の処理を実行する。

【0076】

まず、図１１に示すように、第１推定部１１３は、第１センサデータ（点群データＰ）に所定の変換処理を施す（ステップＳ１４０）。この変換処理は、例えば、一般的な歪み補正や、ワールド座標系への変換処理などである。

【0077】

次に、第１推定部１１３は、変換処理後の第１センサデータからランドマークＭ３を抽出する処理を行う（ステップＳ１４１）。

【0078】

例えば、既知情報Ｄ２には、各ランドマークＭ３が走行エリアＲ内のどの位置にどの角度で設置されているかを示す情報や、各ランドマークＭ３の形状やサイズなどを示す情報が予め記録されている。また、既知情報Ｄ２には、各ランドマークＭ３が含まれる領域（存在領域Ａ）の位置が記録されている。存在領域Ａは、例えば図９に示すように、各ランドマークＭ３を中心とする矩形の仮想領域であり、ランドマークＭ３の水平方向サイズよりも所定マージン分大きくなるように設定される。また、複数のランドマークＭ３が隣接して設けられている場合、存在領域Ａはこれら隣接する複数のランドマークＭ３を含むように設定されてもよい。

【0079】

第１推定部１１３は、前回の自己位置推定結果と既知情報Ｄ２とに基づいて、第１センサデータから、ランドマークＭ３の存在領域Ａに対応する領域を特定し、この存在領域Ａ内に含まれる点群データＰを、ランドマークＭ３の点群データＰとして抽出する。

【0080】

また、第１推定部１１３は、抽出した点群データに基づいて、ランドマークＭ３の代表点ＲＰを算出する処理を行う（ステップＳ１４２）。

【0081】

図１２は、第３の実施形態に係る自己位置推定装置の機能を説明する第３の図である。
図１２には、ランドマークＭ３の形状が直方体である例が示されている。この場合、第１推定部１１３は、点群に対して以下の式（２）および（３）で表される２つの直線をフィッティングさせ、これら直線の交点（角）をランドマークＭ３の代表点ＲＰとして抽出する。

【0082】

【数2】

【0083】

【数3】

【0084】

図１３は、第３の実施形態に係る自己位置推定装置の機能を説明する第３の図である。
図１３には、ランドマークＭ３の形状が円柱である例が示されている。この場合、第１推定部１１３は、以下の式（４）で表される円の方程式と点群とをフィッティングして、誤差が最小となる円の中心（ａ，ｂ）をランドマークＭ３の代表点ＲＰとして抽出する。

【0085】

【数4】

【0086】

次に、第１推定部１１３は、第１センサデータから抽出した個々のランドマークＭ３について、ステップＳ１４３で算出した車両座標系における代表点ＲＰの位置座標と、既知情報Ｄ２に記録されたランドマークＭ３のワールド座標系における代表点の位置座標とに基づいて、自律移動体１のＸｗ軸方向およびＹｗ軸方向における位置Ｘｗ１，Ｙｗ１と、Ｚｗ軸回りの回転角度θＺｗ１との３自由度のパラメータを推定する（ステップＳ１４３）。

【0087】

なお、走行エリアＲの走行路や棚Ｂ２ごとに異なる形状のランドマークＭ３や、反射率が異なる材質のランドマークＭ３を配置してもよい。この場合、第１推定部１１３は、既知情報Ｄ２から、点群データＰから検出した形状や反射率と一致するランドマークＭ３の情報を読み出して、パラメータの推定を行う。これにより、第１推定部１１３は、自律移動体１の走行路や、どの棚Ｂ２の付近を走行しているかを絞り込んだ上で、３自由度のパラメータを精度よく推定することができる。

【0088】

以降の処理（図３のステップＳ１０４～Ｓ１０５）については、第１の実施形態と同じである。

【0089】

（作用、効果）
以上のように、本実施形態に係る自己位置推定装置１０において、第１取得部１１０は、第１センサ２０から点群データＰである第１センサデータを取得する。また、目印は、固定物（棚Ｂ２の下部）に取り付けられたランドマークＭ３であり、第１推定部１１３は、第１センサデータから抽出したランドマークＭ３と既知情報Ｄ２とに基づいて、６自由度のパラメータのうち、Ｘｗ軸方向およびＹｗ軸方向における位置Ｘｗ１，Ｙｗ１と、Ｚｗ軸回りの回転で表される姿勢θＺｗ１とを部分パラメータとして推定する。

【0090】

このようにすることで、自己位置推定装置１０は、第１センサ２０としてＬｉＤＡＲが搭載されている自律移動体１についても、点群データＰと既知情報Ｄ２とから、６自由度のパラメータのうち一部のパラメータを推定することができる。また、第１推定部１１３は、第１センサデータから検出した複数のランドマークＭ３との位置関係および角度に基づいて、より正確に３自由度のパラメータを推定することができる。これにより、自己位置推定装置１０は、自律移動体１の自己位置をより精度よく推定することができる。

【0091】

＜第４の実施形態＞
以下、図１４～図１５を参照しながら第４の実施形態について詳しく説明する。なお、第４の実施形態の構成のうち、第１～第３の実施形態と同じ構成であるものについては、第１～第３の実施形態と同じ符号を用いて説明する。

【0092】

図１４は、第４の実施形態に係る自己位置推定装置の機能を説明する図である。
第３の実施形態では、自己位置推定装置１０の第１推定部１１３が、個々のランドマークＭ３の代表点ＲＰに基づいて、自律移動体１の３自由度のパラメータ（Ｘｗ１，Ｙｗ１，θＺｗ１）を求める例について説明した。これに対し、本実施形態では、第１推定部１１３は、図１４に示すように、複数のランドマークＭ３を例えば直線上に一列に配置し、複数のランドマークＭ３の点群データを仮想線で繋いで得られる形状（直線Ｌ）と、実際のランドマークＭ３の配置から得られる形状とを比較して、自律移動体１の２自由度のパラメータ（Ｘｗ１，θＺｗ１）を推定する。

【0093】

（自己位置推定装置の処理フロー）
図１５は、第４の実施形態に係る第１推定部の処理の一例を示すフローチャートである。
本実施形態例に係る第１推定部１１３は、第３の実施形態（図１１）の処理に代えて、図１５の処理を実行する。

【0094】

まず、図１５に示すように、第１推定部１１３は、第１センサデータ（点群データＰ）に所定の変換処理を施す（ステップＳ１５０）。また、第１推定部１１３は、変換処理後の第１センサデータからランドマークＭ３を抽出する処理を行う（ステップＳ１５１）。これら処理は、図１１のステップＳ１４０～Ｓ１４１と同じである。

【0095】

次に、第１推定部１１３は、抽出したランドマークＭ３の点群データＰを直線フィッティングして、この直線Ｌに対する距離および角度と、既知情報Ｄ２に記録されたランドマークＭ３の配置から得られる直線とに基づいて、自律移動体１のＸｗ軸における位置Ｘｗ１およびＺｗ軸回りの回転角度θＺｗ１を推定する（ステップＳ１５２）。

【0096】

以降の処理（図３のステップＳ１０４～Ｓ１０５）については、第１の実施形態と同じである。

【0097】

なお、図１４には、複数のランドマークＭ３が直線上に一列に配置される例が示されているが、これに限られることはない。他の実施形態では、ランドマークＭ３をＬ字形状など、他の形状となるように配置してもよい。

【0098】

（作用、効果）
以上のように、本実施形態に係る自己位置推定装置１０において、第１取得部１１０は、第１センサデータから抽出した複数のランドマークＭ３を繋いで得た形状と、既知情報Ｄ２に記録されているランドマークＭ３の配置から得られる形状とに基づいて、Ｘｗ軸方向における位置Ｘｗ１およびＺｗ軸回りの回転角度θＺｗ１の２自由どのパラメータを推定する。

【0099】

このようにすることで、自己位置推定装置１０は、複数のランドマークＭ３の組み合わせから、よりロバストに自己位置の推定を行うことができる。

【0100】

＜第５の実施形態＞
以下、図１６を参照しながら第５の実施形態について詳しく説明する。なお、第５の実施形態の構成のうち、第１～第４の実施形態と同じ構成であるものについては、第１～第４の実施形態と同じ符号を用いて説明する。

【0101】

上述の各実施形態において、自己位置推定装置１０の第２推定部１１４は、６自由度のパラメータのうち、第１推定部１１３が推定した部分パラメータ以外のパラメータのみを推定する例について説明した。これに対し、本実施形態に係る第２推定部１１４は、第１推定部１１３が推定した部分パラメータに基づく制約を加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0102】

（自己位置推定装置の処理フロー）
図１６は、第５の実施形態に係る第２推定部の処理の一例を示すフローチャートである。
本実施形態例に係る第２推定部１１４は、第１の実施形態（図６）の処理に代えて、図１６の処理を実行する。なお、第１推定部１１３が部分パラメータを推定する方法は、第１～第４の実施形態のうち何れの方法であってもよい。

【0103】

まず、第２推定部１１４は、第１推定部１１３が既知情報Ｄ２で求めた部分パラメータを制約項にセットする（ステップＳ１６０）。ここでは、第１の実施形態と同様に、前回フレームｆと今回フレームｆ＋１との相対値がセットされる。

【0104】

次に、第２推定部１１４は、第１推定部１１３が推定した部分パラメータの制約項を加えた最適化計算を行って、６自由度のパラメータ全てについて、前回フレームとの相対値を推定する（ステップＳ１６１）。具体的には、第２推定部１１４は、第１の実施形態の式（１）に代えて、以下の式（５）を解くことにより、６自由度のパラメータ全ての相対値を求める。

【0105】

【数5】

【0106】

式（５）において、波括弧内の第３項は、以下の式（６）のように、最適化計算で求まる自己位置χと、既知情報Ｄ２から第１推定部１１３が推定した自己位置ｍ（部分パラメータ）との差を表す。式（６）において、ａは重み係数、ｐは位置座標（ｘ、ｙ、ｚ）、ｒは姿勢（θｘ，θｙ，θｚ）を表す。重み係数ａは、自律移動体１の運用前に、パラメータチューニングを行い決めた値である。

【0107】

【数6】

【0108】

式（６）によれば、最適化計算で推定した自己位置χと、第１推定部１１３が推定した自己位置ｍとの差が大きいほど式（５）の第３項の値が大きくなる。

【0109】

このように、第１推定部１１３が推定したパラメータをそのまま採用するのではなく、最適化計算時の制約項として追加することにより、第１センサデータの観測誤差を緩和しつつ全体の累積誤差を抑制することができる。

【0110】

また、例えば、第１の実施形態のように、第１推定部１１３が白線Ｍ１に基づいて自己位置ｍ（部分パラメータ）を推定した場合、この自己位置ｍには画像から検出した白線Ｍ１の誤差が含まれる可能性がある。このため、第２推定部１１４は、第１推定部１１３が画像処理により白線Ｍ１を検出したときの信頼度を用いて、重み係数αの値を変更してもよい。例えば、白線Ｍ１の検出信頼度が高ければ、第１推定部１１３による自己位置ｍの推定結果の精度も高くなるはずである。したがって、第２推定部１１４は、白線Ｍ１検出の信頼度が高いほど、重み係数αの値を大きくする。このように重み係数αを変更することで、第１推定部１１３の自己位置ｍの推定精度が高いほど自己位置ｍからの差が少なくなるように、一方で第１推定部１１３の自己位置ｍの推定精度が低いほど自己位置ｍの誤差の影響を受けにくくなるように、最適化計算を行うことができる。

【0111】

次に、第２推定部１１４は、ステップＳ１６１で求めた各パラメータの前回フレームとの相対値と、前回フレームの自己位置推定結果とに基づいて、今回フレームの自律移動体１のワールド座標系における自己位置を算出する（ステップＳ１６２）。

【0112】

以降の処理（図３のステップＳ１０５）については、第１の実施形態と同じである。

【0113】

（作用、効果）
以上のように、本実施形態に係る自己位置推定装置１０において、第２推定部１１４は、第１推定部１１３が推定した部分パラメータに基づく制約を加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0114】

このようにすることで、自己位置推定装置１０は、第１推定部１１３が推定したパラメータをベースにしながらも、最適化計算で他のパラメータとも整合した推定値を求めることで、より高精度な自己位置の推定が可能となる。また、自己位置推定装置１０は、一部のパラメータについて制約を加えるため、最適化計算の処理速度を向上することができる。

【0115】

＜第６の実施形態＞
以下、図１７を参照しながら第６の実施形態について詳しく説明する。なお、第６の実施形態の構成のうち、第１～第５の実施形態と同じ構成であるものについては、第１～第５の実施形態と同じ符号を用いて説明する。

【0116】

本実施形態に係る自己位置推定装置１０において、第２推定部１１４は、自律移動体１の運動制約に基づく制約を加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0117】

（自己位置推定装置の処理フロー）
図１７は、第６の実施形態に係る第２推定部の処理の一例を示すフローチャートである。
本実施形態例に係る第２推定部１１４は、第５の実施形態（図１６）の処理に代えて、図１７の処理を実行する。なお、第１推定部１１３が部分パラメータを推定する方法は、第１～第４の実施形態のうち何れの方法であってもよい。

【0118】

まず、第２推定部１１４は、第１推定部１１３が既知情報Ｄ２で求めた部分パラメータを最適化計算の制約項にセットする（ステップＳ１７０）。この処理は図１６のステップＳ１６０と同じである。

【0119】

また、第２推定部１１４は、自律移動体１のストレージ１３に予め記録された車両運動制約Ｄ３を最適化計算の制約項にセットする（ステップＳ１７１）。

【0120】

例えば、自律移動体１が物流倉庫で運用する無人フォークリフトである場合、車体が前後方向（Ｘｗ軸回りの回転）または左右方向（Ｙｗ軸回りの回転）に所定角度以上に回転するような動作（車体の片側が床にめり込む、宙に浮くような動作など）を行うことができない。車両運動制約Ｄ３は、このような自律移動体１の機体特性や、自律移動体１の運用条件、自律移動体１が移動不可な位置（例えば棚のある領域）などに基づいて、自律移動体１が取り得る位置および姿勢の範囲を定義したものである。

【0121】

次に、第２推定部１１４は、第１推定部１１３が推定した部分パラメータの制約項と、車両運動制約Ｄ３の制約項とを加えた最適化計算を行って、６自由度のパラメータ全てについて、前回フレームとの相対値を推定する（ステップＳ１７２）。具体的には、第２推定部１１４は、上式（５）を解くことにより、６自由度のパラメータ全ての相対値を求める。また、本実施形態では、式（５）の第３項は、上式（６）に代えて、以下の式（７）で表される。

【0122】

【数7】

【0123】

式（７）において、ａは重み係数、ｍ１は第１推定部１１３が推定した自己位置（部分パラメータ）、ｍ２は車両運動制約Ｄ３から定める自己位置、ｐは位置座標（ｘ、ｙ、ｚ）、ｒは姿勢（θｘ，θｙ，θｚ）を表す。

【0124】

また、第２推定部１１４は、自律移動体１の車輪回転数を計測するエンコーダ（不図示）の計測データに基づいて、車両進行方向の移動量を推定し、これを制約に加えてもよい。

【0125】

式（７）によれば、最適化計算で推定した自己位置χと、車両運動制約Ｄ３から定まる自己位置ｍ２との差が大きいほど式（５）の第３項の値が大きくなる。

【0126】

次に、第２推定部１１４は、ステップＳ１７２で求めた各パラメータの前回フレームとの相対値と、前回フレームの自己位置推定結果とに基づいて、今回フレームの自律移動体１のワールド座標系における自己位置を算出する（ステップＳ１７３）。

【0127】

以降の処理（図３のステップＳ１０５）については、第１の実施形態と同じである。

【0128】

（作用、効果）
以上のように、本実施形態に係る自己位置推定装置１０において、第２推定部１１４は、自律移動体１の運動制約をさらに加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0129】

このようにすることで、自己位置推定装置１０は、自律移動体１として起こり得ない位置や姿勢を排除することができるので、さらに累積誤差を抑制することができる。

【0130】

以上、図面を参照して一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、様々な設計変更等をすることが可能である。すなわち、他の実施形態においては、上述の処理の順序が適宜変更されてもよい。また、一部の処理が並列に実行されてもよい。

【0131】

上述した実施形態に係る自己位置推定装置１０は、単独のコンピュータによって構成されるものであってもよいし、自己位置推定装置１０の構成を複数のコンピュータに分けて配置し、複数のコンピュータが互いに協働することで自己位置推定装置１０として機能するものであってもよい。このとき、自己位置推定装置１０を構成する一部のコンピュータが自律移動体１の内部に搭載され、他のコンピュータが自律移動体１の外部に設けられてもよい。

【0132】

また、第１～第２の実施形態では、第１センサ２０がカメラである例について説明したが、これに限られることはない。第１～第２の実施形態の構成において、第１センサ２０としてＬｉＤＡＲを用いてもよい。同様に、第３～第４の実施形態では、第１センサ２０がＬｉＤＡＲである例について説明したが、これに限られることはない。第３～第４の実施形態の構成において、第１センサ２０としてカメラを用いてもよい。

【0133】

＜付記＞
上述の実施形態は、例えば以下のように把握される。

【0134】

（１）第１の態様によれば、自律移動体１の走行エリアＲにおける自己位置を推定する自己位置推定装置１０は、自律移動体１の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得する第１取得部１１０と、自律移動体１の加速度および角速度を含む第２センサデータを取得する第２取得部１１１と、複数の固定物から予め選択した一の固定物である目印Ｍ１，Ｍ２，Ｍ３の走行エリアＲにおける位置を含む情報を記録した既知情報Ｄ２と、第１センサデータとに基づいて、３次元座標系における自律移動体１の位置および姿勢を表す６自由度のパラメータの一部である部分パラメータを推定する第１推定部１１３と、第１センサデータと、第２センサデータと、部分パラメータとに基づいて、６自由度のパラメータで表される自律移動体１の自己位置を推定する第２推定部１１４と、を備える。

【0135】

このように、自己位置推定装置１０は、第１センサデータから間接的に観測できる既知情報Ｄ２を活用して一部のパラメータを推定することにより、最適化計算における累積誤差を抑制して、自律移動体１の自己位置を精度よく推定することができる。

【0136】

（２）第２の態様によれば、第１の態様に係る自己位置推定装置１０において、第２推定部１１４は、６自由度のパラメータのうち、部分パラメータ以外のパラメータを推定する。

【0137】

このようにすることで、自己位置推定装置１０は、一部パラメータが既知となり、他のパラメータのみを求めればよいので、累積誤差の影響をより効果的に抑制することができる。また、自己位置推定装置１０は、最適化計算を高速かつ低負荷で行うことが可能となる。

【0138】

（３）第３の態様によれば、第１の態様に係る自己位置推定装置１０において、第２推定部１１４は、部分パラメータに基づく制約を加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0139】

このようにすることで、自己位置推定装置１０は、第１推定部１１３が推定したパラメータをベースにしながらも、最適化計算で他のパラメータとも整合した推定値を求めることで、より高精度な自己位置の推定が可能となる。また、自己位置推定装置１０は、一部のパラメータについて制約を加えるため、最適化計算の処理速度を向上することができる。

【0140】

（４）第４の態様によれば、第３の態様に係る自己位置推定装置１０において、第２推定部１１４は、自律移動体１の運動制約をさらに加えた最適化計算を行って、６自由度のパラメータ全てを推定する。

【0141】

このようにすることで、自己位置推定装置１０は、自律移動体１として起こり得ない位置や姿勢を排除することができるので、さらに累積誤差を抑制することができる。

【0142】

（５）第５の態様によれば、第１から第４のいずれか一の態様に係る自己位置推定装置１０において、目印は、走行エリアＲに施されたペイントＭ１および走行エリアＲに設置された標識Ｍ２の少なくとも一方であり、第１推定部１１３は、第１センサデータから抽出したペイントＭ１および標識Ｍ２の少なくとも一方と、既知情報Ｄ２とに基づいて、６自由度のパラメータのうち、水平方向に延びる第１軸方向および第２軸方向の少なくとも一方における位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを部分パラメータとして推定する。

【0143】

このようにすることで、自己位置推定装置１０は、走行エリアＲのペイントＭ１または標識Ｍ２から、６自由度のうち２自由度のパラメータを精度よく推定することができる。
また、自己位置推定装置１０は、ペイントＭ１および標識Ｍ２の組み合わせから、６自由度のうち３自由度のパラメータを精度よく推定することができる。これにより、自己位置推定装置１０は、残りの４自由度または３自由度のパラメータの推定において、累積誤差の影響をより効果的に抑制することができる。

【0144】

（６）第６の態様によれば、第１から第４の何れか一の態様に係る自己位置推定装置１０において、目印は固定物の一部に取り付けられたランドマークＭ３であり、第１推定部１１３は、第１センサデータから抽出したランドマークＭ３と、既知情報Ｄ２とに基づいて、６自由度のパラメータのうち、水平方向に延びる第１軸方向および第２軸方向それぞれにおける位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを部分パラメータとして推定する。

【0145】

このようにすることで、自己位置推定装置１０の第１推定部１１３は、第１センサデータから検出した複数のランドマークＭ３との位置関係および角度に基づいて、より正確に３自由度のパラメータを推定することができる。これにより、自己位置推定装置１０は、自律移動体１の自己位置をより精度よく推定することができる。

【0146】

（７）第７の態様によれば、第６の態様に係る自己位置推定装置１０において、第１推定部１１３は、第１センサデータから抽出した複数のランドマークＭ３を繋いで得た形状と、既知情報Ｄ２に記録されているランドマークＭ３の配置から得られる形状とに基づいて、６自由度のパラメータのうち、水平方向に延びる第１軸方向における位置と、鉛直方向に延びる第３軸回りの回転で表される姿勢とを部分パラメータとして推定する。

【0147】

このようにすることで、自己位置推定装置１０は、複数のランドマークＭ３の組み合わせから、よりロバストに自己位置の推定を行うことができる。

【0148】

（８）第８の態様によれば、自律移動体１の走行エリアＲにおける自己位置を推定する自己位置推定方法は、自律移動体１の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、自律移動体１の加速度および角速度を含む第２センサデータを取得するステップと、複数の固定物から予め選択した一の固定物である目印Ｍ１，Ｍ２，Ｍ３の走行エリアＲにおける位置を含む情報を記録した既知情報と、第１センサデータとに基づいて、３次元座標系における自律移動体１の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、第１センサデータと、第２センサデータと、部分パラメータとに基づいて、６自由度のパラメータで表される自律移動体１の自己位置を推定するステップと、を有する。

【0149】

（９）第９の態様によれば、プログラムは、自律移動体１の走行エリアにおける自己位置を推定する自己位置推定装置１０に、自律移動体１の周囲に常に存在する固定物および一時的に存在する非固定物を検出可能な第１センサデータを取得するステップと、自律移動体１の加速度および角速度を含む第２センサデータを取得するステップと、複数の固定物から予め選択した一の固定物である目印Ｍ１，Ｍ２，Ｍ３の走行エリアＲにおける位置を含む情報を記録した既知情報Ｄ２と、第１センサデータとに基づいて、３次元座標系における自律移動体１の位置および姿勢を表す６自由度のパラメータのうち一部である部分パラメータを推定するステップと、第１センサデータと、第２センサデータと、部分パラメータとに基づいて、６自由度のパラメータで表される自律移動体１の自己位置を推定するステップと、を実行させる。

【符号の説明】

【0150】

１ 自律移動体
１０ 自己位置推定装置
１１ プロセッサ
１１０ 第１取得部
１１１ 第２取得部
１１２ 地図照合部
１１３ 第１推定部
１１４ 第２推定部
１１５ 出力処理部
１２ メモリ
１３ ストレージ
１４ インタフェース
２０ 第１センサ
２１ 第２センサ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】