(57)【要約】【課題】簡易且つ高精度に位置推定を行うこと。【解決手段】本実施形態に係る位置推定システム１は、発生源、移動体、複数の物理量検出器及び推定部を有する。発生源は、物理量が変化するエネルギーを発生する。移動体は、位置推定対象である。複数の物理量検出器は、移動体に設けられ、発生源から発生されたエネルギーの物理量を検出する。推定部は、発生源の位置と発生源の姿勢角と複数の物理量検出器によりそれぞれ検出された複数の物理量検出値とに基づいて移動体の推定位置を推定する。【選択図】      図１
2018155639.tif 000002
【特許請求の範囲】
【請求項１】
物理量が変化するエネルギーを発生する発生源と、
位置推定対象の移動体と、
前記移動体に設けられ、前記発生源から発生された前記エネルギーの物理量を検出する複数の物理量検出器と、
前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出器によりそれぞれ検出された複数の物理量検出値とに基づいて前記移動体の推定位置を推定する推定部と、
を具備する移動体位置推定システム。
【請求項２】
前記発生源の姿勢角を可変自在に支持する支持体と、
前記支持体を駆動して前記エネルギーが前記推定位置に照射されるように前記発生源の姿勢角を調節する駆動部と、を更に備える、
請求項１記載の移動体位置推定システム。
【請求項３】
前記駆動部は、前記発生源の姿勢角を、鉛直角を含む禁止角度範囲以外の許容角度範囲に制限する、請求項２記載の移動体位置推定システム。
【請求項４】
前記発生源は、前記エネルギーを個別に発生する複数の発生源を有し、
前記複数の発生源のうちの一の発生源の前記禁止角度範囲に対応する前記エネルギーの照射範囲が、他の一の発生源の前記許容角度範囲に対応する前記エネルギーの照射範囲に含まれるように前記複数の発生源が配置される、
請求項３記載の移動体位置推定システム。
【請求項５】
前記推定部は、前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出値と前記発生源が発生するエネルギーの空間分布とにアンセンテッドカルマンフィルタを適用して前記推定位置として事後状態推定値と事後誤差共分散行列とを算出する、請求項２記載の移動体位置推定システム。
【請求項６】
前記推定部は、前記事後誤差共分散行列を閾値に対して比較し、前記事後誤差共分散行列が前記閾値よりも高い場合、前記発生源の姿勢角を保持する旨の信号を前記駆動部に出力し、前記事後誤差共分散行列が前記閾値よりも低い場合、前記発生源の姿勢角を前記事後状態推定値に向ける旨の信号を前記駆動部に出力する、請求項５記載の移動体位置推定システム。
【請求項７】
前記複数の物理量検出器は、３個以上である、請求項１記載の移動体位置推定システム。
【請求項８】
前記複数の物理量検出器は、前記移動体の異なる高度で取り付けられている、請求項１記載の移動体位置推定システム。
【請求項９】
前記複数の物理量検出器は、前記移動体の中心から異なる距離に取り付けられている、請求項１記載の移動体位置推定システム。
【請求項１０】
前記移動体に取り付けられた方位検出器を更に備え、
前記推定部は、前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出値とに加え、前記方位検出器から出力される方位検出値に基づいて前記推定位置を推定する、
請求項１記載の移動体位置推定システム。
【請求項１１】
前記推定部は、前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出値とに加え、前記移動体の運動モデルと前記移動体の回転角、加速度及び角速度の少なくとも一つの内部情報とに基づき、カルマンフィルタ、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ及びパーティクルフィルタの少なくとも一つのアルゴリズムを用いて前記推定位置を推定する、請求項１記載の移動体位置推定システム。
【請求項１２】
前記発生源は、前記エネルギーとして光を発生する光源又は音波を発生するパラメトリックスピーカである、請求項１記載の移動体位置推定システム。
【請求項１３】
前記発生源は、照度分布を波長毎に変化させるために分光フィルムが貼り付けられた光源である、請求項１記載の移動体位置推定システム。
【請求項１４】
前記発生源は、前記エネルギーとして互いに個別に光を発生する複数の光源を有し、
前記複数の光源は、互いが発生する光を区別するために異なる分光フィルムが取り付けられている、
請求項１記載の移動体位置推定システム。
【請求項１５】
前記発生源は、前記エネルギーとして互いに個別に音波を発生する複数のパラメトリックスピーカを有し、
前記複数のパラメトリックスピーカは、互いが発生する音波を区別するために異なる周波数帯に属する音波を発生する、
請求項１記載の移動体位置推定システム。
【請求項１６】
位置推定対象の移動体に設けられ、発生源から発生されたエネルギーの物理量を検出する複数の物理量検出器によりそれぞれ検出された複数の物理量検出値を入力する入力部と、
前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出値とに基づいて前記移動体の推定位置を推定する推定部と、
を具備する移動体位置推定装置。
【請求項１７】
物理量が変化するエネルギーを発生源から発生し、
位置推定対象の移動体に設けられ、前記発生源から発生された前記エネルギーの物理量を複数の物理量検出器により検出し、
前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出器によりそれぞれ検出された複数の物理量検出値とに基づいて前記移動体の推定位置を推定する、
ことを具備する移動体位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、移動体位置推定システム、装置及び方法に関する。
【背景技術】
【０００２】
移動体屋内位置推定では、Ｗｉ−Ｆｉ電波強度や屋内ＧＰＳ信号、超音波センサを用いた測位技術、カメラやＲＦＩＤ、マーカ等を用いた位置認識技術、内部センサを活用したデッドレコニングを利用した技術が検討されている。現在最も屋内位置推定精度が高い超音波利用の推定技術では、天井などに、超音波センサを１〜２ｍ単位で設置する必要があり、既存の建設構造物に取り付けるには工事が必要であり、コストがかかる。Ｗｉ−Ｆｉ電波強度や屋内ＧＰＳ信号は、上記に比べ設置コストが下がるが、電波の屋内マルチパスの影響により位置推定精度が下がる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】国際公開２００８／０１０２６９号
【非特許文献】
【０００４】
【非特許文献１】橋本、「レーザ位置計測とデッドレコニングの統合による車輪型移動ロボットの位置推定法」、１９９３年
【非特許文献２】田中、「パラメトリックスピーカを制御音源として使用した騒音制御」、Ｄ＆Ｄ、２００７年
【非特許文献３】田中、岩本、「遅延駆動パラメトリックアレイスピーカによる広域能動騒音制御、日本機械学会論文集(Ｃ編)」、Ｖｏｌ．７９、Ｎｏ．７９９、２０１３年
【発明の概要】
【発明が解決しようとする課題】
【０００５】
実施形態の目的は、簡易且つ高精度に位置推定可能な移動体位置推定システム、装置及び方法を提供することにある。
【課題を解決するための手段】
【０００６】
本実施形態に係る移動体位置推定システムは、物理量が変化するエネルギーを発生する発生源と、位置推定対象の移動体と、前記移動体に設けられ、前記発生源から発生された前記エネルギーの物理量を検出する複数の物理量検出器と、前記発生源の位置と前記発生源の姿勢角と前記複数の物理量検出器によりそれぞれ検出された複数の物理量検出値とに基づいて前記移動体の推定位置を推定する推定部と、を具備する。
【図面の簡単な説明】
【０００７】
【図１】図１は、本実施形態に係る移動体位置推定システムの構成を示す図である。
【図２】図２は、図１のエネルギー発生源の姿勢角を示す図である。
【図３】図３は、図１の移動体位置推定システムの動作例の概要を示す図である。
【図４】図４は、図１の演算装置の制御に従い行われる移動体の位置推定に関する典型的な流れを示す図である。
【図５】図５は、本実施形態に係る第１シミュレーション条件を模式的に示す図である。
【図６】図６は、第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定軌道のシミュレーション結果を示す図である。
【図７】図７は、図６の第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定誤差のシミュレーション結果を示す図である。
【図８】図８は、図６の第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた事後誤差共分散行列Ｐｓの対角成分ｘ、ｙ、ψ及びｒの時系列推移を示す図である。
【図９】図９は、図６の第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各照度センサ４１−ｎ（ｎ＝１〜５）の実測値と推定値を示す図である。
【図１０】図１０は、図６の第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られたオドメトリでの推定結果を示す。
【図１１】図１１は、第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定軌道のシミュレーション結果を示す図である。
【図１２】図１２は、図１１の第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定誤差のシミュレーション結果を示す図である。
【図１３】図１３は、図１１の第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各照度センサの実測値と推定値を示す図である。
【図１４】図１４は、図１１の第２シミュレーション条件のもとにエネルギー発生源３１の姿勢角を真下に固定したい場合の移動体の軌道の推定結果を示す図である。
【図１５】図１５は、図１４の第２シミュレーション条件のもとでの各照度センサの真の実測値と推定値とを示す図である。
【図１６】図１６は、分光フィルムが貼り付けられた光源を模式的に示す図である。
【図１７】図１７は、図１６の分光フィルムが貼り付けられた光源から照射された光の配光分布を模式的に示す図である。
【図１８】図１８は、図１６とは異なる態様で分光フィルムが貼り付けられた光源を模式的に示す図である。
【図１９】図１９は、本実施形態に係る二つのエネルギー発生源の配置を示す図である。
【図２０】図２０は、エンドファイアレイを考慮した、出力周波数１ｋＨｚのパラメトリックスピーカから発生される音圧分布を示す図である。
【図２１】図２１は、第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定軌道のシミュレーション結果を示す図である。
【図２２】図２２は、図２１の第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定誤差のシミュレーション結果を示す図である。
【図２３】図２３は、図２１の第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各パラメトリックスピーカのパン角及びチルト角の時間推移を示す図である。
【図２４】図２４は、図２１の第３シミュレーション条件のもとでの各照度センサの実測値と推定値とを示す図である。
【図２５】図２５は、第４シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定軌道のシミュレーション結果を示す図である。
【図２６】図２６は、図２５の第４シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車の推定誤差のシミュレーション結果を示す図である。
【図２７】図２７は、図２５の第４シミュレーション条件のもとでの各照度センサの実測値と推定値とを示す図である。
【発明を実施するための形態】
【０００８】
以下、図面を参照しながら本実施形態に係わる移動体位置推定システム、装置及び方法を説明する。
【０００９】
図１は、本実施形態に係る移動体位置推定システム１の構成を示す図である。図１に示すように、移動体位置推定システム１は、移動体位置推定装置１０、移動体２０、エネルギー照射器３０、物理量検出部４０、表示機器５０及び操作機器６０を有する。移動体位置推定システム１は、屋内を移動する移動体２０の位置を、エネルギー照射器３０から照射され物理量検出部４０により検出されたエネルギーの空間分布物理量に基づいて移動体位置推定装置１０により推定する。
【００１０】
移動体２０は、屋内を移動する装置である。移動体２０としては、屋内の床面や壁等を走行する走行車や屋内の空間を飛翔する飛翔体、床面等を歩行するロボット等が適宜利用可能である。移動体２０は、移動体位置推定装置１０に通信可能に接続されている。移動体２０は、モータやエンジン等の原動機を搭載する。移動体２０は、移動体位置推定装置１０からの移動指令に従い原動機の動力により各可動軸を駆動して移動する。移動体２０には内部センサが設けられている。内部センサは、移動体２０の可動軸に取り付けられたエンコーダや移動体２０の任意箇所に取り付けられた方位センサから適宜選択される。エンコーダは可動軸の回転角を示す信号を出力する。方位センサは、移動体２０の方位を検出し、方位を示す出力値（方位検出値）を示す信号を出力する。方位センサとしては、例えば、加速度センサやジャイロセンサ等が用いられると良い。加速度センサは、移動体２０の加速度を検出し、加速度検出を示す信号を出力する。ジャイロセンサは、移動体２０の角速度を検出し、角速度検出を示す信号を出力する。これら内部センサの出力情報（以下、内部センサ情報と呼ぶ）は、移動体位置推定装置１０に送信される。
【００１１】
エネルギー照射器３０は、分布物理量を有するエネルギーを任意方向に照射する。具体的には、エネルギー照射器３０は、エネルギー発生源３１、支持体３２及び駆動部３３を有する。エネルギー発生源３１は、分布物理量を有するエネルギーを発生する。分布物理量を有するエネルギーとは、エネルギー発生源３１からの伝播距離及び指向性に応じて変化する物理量を有するエネルギーである。例えば、エネルギー発生源３１は、光を発生する光源、音を有する音源等が挙げられる。光の指向性は、照度等の物理量の空間分布を示す配光分布により表される。なお、本実施形態に係る音波は、可聴音でも良いし、超音波でも良い。音の指向性は、音圧等の物理量の空間分布を示す音圧分布により表される。エネルギー照射器３０から照射されるエネルギーは、移動体２０の位置推定のための外部情報として利用される。
【００１２】
支持体３２は、エネルギー発生源３１の姿勢角を可変自在に支持する。支持体３２は、姿勢角を調節するための所定の回転軸と、当該回転軸を回転可能に支持する軸受けと、当該軸受けを構造的に支持するフレームとを有する。
【００１３】
図２は、本実施形態に係るエネルギー発生源３１の姿勢角を示す図である。図２に示すように、エネルギー発生源３１は、図示しない支持体３２を介して屋内の天井や壁面、構造体に取り付けられている。本実施形態においてｚ軸は、鉛直方向に規定され、ｙ軸はｚ軸に水平に直交し、ｘ軸はｚ軸及びｙ軸に直交するものとする。ｘｙｚ軸は３軸直交座標系を形成する。本実施形態に係る可変可能な姿勢角としては、例えば、チルト角φとパン角θとが挙げられる。チルト角φは、例えば、ｚ軸から、エネルギー発生源３１により照射されたエネルギーの中心軸Ａ１への角度に規定され、パン角θは、例えば、ｘｙ平面でのｘ軸から中心軸Ａ１への角度に規定される。
【００１４】
駆動部３３は、移動体位置推定装置１０からの角度変更指令に従い支持体３２を駆動してエネルギー発生源３１の姿勢角、すなわち、チルト角及びパン角を変更する。例えば、駆動部３３は、移動体２０の推定位置にエネルギーが照射されるようにエネルギー発生源３１の姿勢角を変更する。駆動部３３としては、例えば、サーボモータ等の任意のモータが用いられる。なお、駆動部３３としては、チルト角用のモータとパン角用のモータとが設けられても良い。
【００１５】
物理量検出部４０は、移動体２０に取り付けられる。物理量検出部４０は、エネルギー照射器３０から照射されたエネルギーの物理量の空間分布を検出する。物理量検出部４０は、物理量を検出する複数の物理量検出器４１−ｎ（ｎは物理量検出器の個数を示す整数）を有する。複数の物理量検出器４１−ｎは、移動体２０に設けられている。物理量検出器４１−ｎの個数は３個以上であれば幾つでも良い。物理量検出器４１−ｎは、エネルギー照射器３０から照射されたエネルギーの物理量を検出し、検出した物理量に応じたデジタル値を有するデータ信号を出力する。以下、検出した物理量に応じたデジタル値を物理量検出値と呼び、物理量検出値を有するデータ信号を物理量検出情報と呼ぶことにする。物理量検出情報は、移動体位置推定装置１０に供給される。例えば、エネルギー発生源３１が光源である場合、物理量検出器４１−１としては、光の物理量である照度を検出する照度センサが用いられると良い。例えば、エネルギー発生源３１が音源である場合、物理量検出器４１−１としては、音の物理量である音圧を検出するマイクロフォンが用いられると良い。
【００１６】
移動体位置推定装置１０は、屋内を移動する移動体２０の位置を、エネルギー照射器３０から照射され物理量検出部４０により検出されたエネルギーの分布物理量に基づいて移動体位置推定装置１０により推定する。移動体位置推定装置１０は、ハードウェアとして、第１通信ＩＦ（interface）１１、第２通信ＩＦ１２、第３通信ＩＦ１３、記憶装置１４及び演算装置１５を有する。第１通信ＩＦ１１、第２通信ＩＦ１２、第３通信ＩＦ１３及び記憶装置１４はバス（bus）を介して演算装置１５に接続されている。
【００１７】
第１通信ＩＦ１１は、物理量検出部４０との通信のためのインタフェースである。例えば、第１通信ＩＦ１１は、物理量検出部４０から送信された物理量検出値に関する物理量検出情報を受信する。受信した物理量検出情報は演算装置１５に転送される。
【００１８】
第２通信ＩＦ１２は、エネルギー照射器３０との通信のためのインタフェースである。例えば、第２通信ＩＦ１２は、演算装置１５から出力された角度変更指令をエネルギー照射器３０に送信する。角度変更指令は、具体的には、パン角に関する角度変更指令とチルト角に関する角度変更指令とを含む。
【００１９】
第３通信ＩＦ１３は、移動体２０との通信のためのインタフェースである。例えば、第３通信ＩＦ１３は、演算装置１５から出力された移動指令を移動体２０に送信する。移動指令は、移動体の進む方向及び距離に関する情報を含む。また、第３通信ＩＦ１３は、移動体２０から内部センサ情報を受信する。受信した内部センサ情報は演算装置１５に転送される。
【００２０】
なお、物理量検出部４０のための第１通信ＩＦ１１、エネルギー照射器３０のための第２通信ＩＦ１２及び移動体２０のための第３通信ＩＦ１３が設けられるとしたが、一又は二の通信ＩＦにより物理量検出部４０、エネルギー照射器３０及び移動体２０と通信しても良い。また、移動体２０に設けられる内部センサ等との通信のための通信ＩＦが設けられても良い。
【００２１】
記憶装置１４は、ＲＯＭ(Read Only Memory)やＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、集積回路記憶装置等を含む。記憶装置１４は、例えば、通信ＩＦ１１，１２，１３により受信された情報、演算装置１５による種々の演算結果、演算装置１５が実行する種々のプログラムを記憶する。
【００２２】
演算装置１５は、ＣＰＵ（Central Processing Unit）とＲＡＭ（Random Access Memory）とを有する。演算装置１５は、記憶装置１４に記憶されているプログラムを実行することにより位置推定部７１、目標軌道設定部７２、移動体制御部７３及び支持体制御部７４を実現する。
【００２３】
位置推定部７１は、エネルギー発生源３１の位置とエネルギー発生源３１の姿勢角と複数の物理量検出器４０−ｎによりそれぞれ検出された複数の物理量検出値とに基づいて移動体２０の位置を繰り返し推定する。位置推定は、カルマンフィルタ、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ及びパーティクルフィルタ等の少なくとも一つのアルゴリズムが用いられる。なお、位置推定部７１は、推定精度を高めるため、内部センサ情報等の他の情報を加味して位置推定を行っても良い。位置推定部７１は、エネルギー発生源３１の位置とエネルギー発生源３１の姿勢角と複数の物理量検出値とに加え、移動体２０の運動モデルと上記内部センサにより検出された移動体２０の回転角、加速度及び角速度の少なくとも一つの内部センサ情報とに基づき、カルマンフィルタ、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ及びパーティクルフィルタの少なくとも一つのアルゴリズムを用いて移動体２０の位置を推定しても良い。
【００２４】
目標軌道設定部７２は、操作機器６０による指示に従い移動体２０の目標軌道を設定する。なお、目標軌道設定部７２は、他のコンピュータにより予め設定された軌道を目標軌道に設定しても良い。
【００２５】
移動体制御部７３は、目標軌道設定部７２により設定された目標軌道に沿って移動するように移動体２０を制御する。具体的には、移動体制御部７３は、移動体２０の推定位置の目標軌道からのズレを算出し、当該ズレを埋めるための移動体２０の各可動軸の駆動量を算出し、各可動軸の駆動量に応じた移動指令を生成する。
【００２６】
支持体制御部７４は、位置推定部７１により推定された移動体２０の推定位置に向けてエネルギー発生源３１がエネルギーを照射するように、エネルギー発生源３１の姿勢角を制御する。具体的には、支持体制御部７４は、移動体２０の推定位置にエネルギーを照射するためのエネルギー発生源３１の目標の姿勢角を算出し、現在の姿勢角から目標の姿勢角へ変更するための支持体３２の各可動軸の駆動量を算出し、各可動軸の駆動量に応じた角度変更指令を生成する。
【００２７】
なお、位置推定部７１、目標軌道設定部７２、移動体制御部７３及び支持体制御部７４は、単一のＣＰＵにより実現されても良いし、複数のＣＰＵにより分散して実現されても良い。
【００２８】
図１に示すように、移動体位置推定装置１０には表示機器５０と操作機器６０とが接続される。表示機器５０は、演算装置１５により推定された推定位置用の種々の情報を表示する。表示機器５０としては、例えば、ＣＲＴ（Cathode-Ray Tube）ディスプレイや液晶ディスプレイ、有機ＥＬ（Electro Luminescence）ディスプレイ、ＬＥＤ（Light-Emitting Diode）ディスプレイ、プラズマディスプレイ又は当技術分野で知られている他の任意のディスプレイが適宜利用可能である。
【００２９】
操作機器６０は、ユーザからの各種指令を入力する。具体的には、操作機器６０としては、キーボードやマウス、各種スイッチ、タッチパッド、タッチパネルディスプレイ等から適宜選択可能である。操作機器６０からの出力信号は演算装置１５に供給される。なお、操作機器６０としては、演算装置１５に有線又は無線を介して接続された、コンピュータであっても良い。
【００３０】
なお、移動体位置推定装置１０は、如何なる場所に設けられても良い。例えば、移動体位置推定装置１０は、移動体２０が移動する屋内又は屋外に設けられたコンピュータ等に内蔵されても良いし、移動体２０又はエネルギー照射器３０等に設けられても良い。また、位置推定部７１、目標軌道設定部７２、移動体制御部７３及び支持体制御部７４が同一の装置に実装されている必要はなく、例えば、屋内又は屋外に設けられた上記コンピュータ、移動体２０及びエネルギー照射器３０等に分散して実装されても良い。
【００３１】
次に、本実施形態に係る移動体位置推定システム１の動作例について具体的に説明する。図３は、本実施形態に係る移動体位置推定システム１の動作例の概要を示す図である。図３に示すように、以下の動作例においては、移動体２０として二輪台車を用い、エネルギー照射器３０として光源を用い、物理量検出器４１―ｎとして照度センサを用いるものとする。
【００３２】
以下、具体的に、演算装置１５の位置推定部７１による二輪台車２０の位置推定の具体例を説明する。二輪台車２０の運動モデルを記述するシステム方程式として、非特許文献１を参照し、ダイナミクスを考慮に入れない幾何的関係式（１）、（２）及び（３）を用いるものとする。
【００３３】
【数１】
2018155639.tif 000003
【００３４】
Ｘ（ｋ）は、時刻ｋにおける二輪台車２０の位置を示す状態ベクトルである。Ｘ（ｋ）は、（２）式に示すように、ｘ（ｋ）、ｙ（ｋ）、ψ（ｋ）及びｒ（ｋ）の４個の状態変数により構成されるものとする。ｘ（ｋ）は時刻ｋにおける二輪台車２０のＸ座標位置を示し、ｙ（ｋ）は時刻ｋにおける二輪台車２０のＹ座標位置を示し、ψ（ｋ）は時刻ｋにおける二輪台車２０の姿勢を示し、ｒ（ｋ）は時刻ｋにおける二輪台車２０の車輪の半径を示す。（１）式に示すように、時刻ｋ＋１における二輪台車２０の位置を示す状態ベクトルＸ（ｋ＋１）は、状態ベクトルＸ（ｋ）と観測ベクトルｕ（ｋ）との関数により表される。ｕ（ｋ）は時刻ｋにおける二輪台車２０の車輪のサンプリング間隔でのエンコーダ変化量を示す。（３）式に示すように、ｕ（ｋ）は、時刻ｋにおける二輪台車２０の右車輪のサンプリング間隔でのエンコーダ変化量を示すとｕr（ｋ）と、左車輪のサンプリング間隔でのエンコーダ変化量を示すｕl（ｋ）とにより構成される。なお、二輪台車２０であるため高さは考慮せず、二輪台車２０はｚ＝０のｘｙ平面上を移動するものとする。（１）式のＡは、Ａ＝（ｕl＋ｕr）ｒにより規定され、Ｂ＝（ｕl−ｕr）ｒにより規定される。また、ｄは車輪の幅を示す。Ｔは転置を示す。ｕr（ｋ）とｕl（ｋ）とは上記システム方程式の入力となる。なお、システム方程式に、方位センサにより検出された移動体２０の加速度及び角速度等の方位検出値を組み込んでも良い。これにより、移動体２０の位置推定精度が更に向上する。
【００３５】
位置推定処理は、予測ステップとフィルタリングステップとに分けられる。予測ステップにおいては、時刻ｋにおける状態ベクトルＸ（ｋ）とシステムノイズとに基づいて時刻ｋ＋１における状態ベクトルＸ（ｋ＋１）が算出される。システムノイズは、エンコーダ計測誤差であり、エンコーダ分解能を接点とする三角分布として与え真の軌道が求められる。位置推定時の予測ステップは、ノイズ項が分離できないためアンセテッドカルマンフィルタ（ＵＫＦ：Unscented Kalman Filter）でなく拡張カルマンフィルタ（ＥＫＦ：Extended Kalman Filter）が用いられる。
【００３６】
また、予測ステップにおいて、（１）式の関数ｆを下記（４）式のように状態ベクトルＸで偏微分することにより行列Ｆ（ｋ）が算出され、関数ｆを下記（５）式のように状態ベクトルｕで偏微分することにより行列Ｇ（ｋ）が算出される。
【００３７】
【数２】
2018155639.tif 000004
【００３８】
（４）式及び（５）式においてＸs（ｋ）は時刻ｋにおける事後状態推定値を示す。そして、下記（６）式に示すように、時刻ｋ＋１における事前状態推定値Ｘｐ（ｋ＋１）が時刻ｋにおける事後状態推定値と観測ベクトルｕ（ｋ）とに基づき算出される。また、下記（７）式に示すように、時刻ｋ＋１における事前誤差共分散行列Ｐｐ（ｋ＋１）が時刻ｋにおける行列Ｆ（ｋ）、事後誤差共分散行列Ｐｓ（ｋ）、行列Ｇ（ｋ）及びシステム雑音共分散行列Ｑに基づき算出される。システム雑音共分散行列Ｑは、エンコーダの三角分布ノイズをガウス分布で近似する。なお、（８）式のｃｏは、エンコーダ１回転でのパルス数を示す。
【００３９】
【数３】
2018155639.tif 000005
【００４０】
以上により、本実施形態に係る予測ステップの説明を終了する。次に、照度センサ４１の観測値について示す。二輪台車２０の中心を原点とした物体座標での各照度センサ４１の配置位置をｓｉとする。なおｉは、照度センサ４１の番号を示す。ワールド座標での光源３１と二輪台車２０と各照度センサ４１との幾何的関係から、時刻ｋにおける光源３１の位置Ｌｘから配置位置ｓｉまでのベクトルｒｉ（ｋ）は下記（９）式により表現される。
【００４１】
【数４】
2018155639.tif 000006
【００４２】
光源３１の視線ベクトルＬａは、パン角θとチルト角φとに基づき下記の（１０）式により表現される。
【００４３】
【数５】
2018155639.tif 000007
【００４４】
光源３１の視線ベクトルＬａと各照度センサ４１の位置ベクトルｒｉとのなす角θａｉは、（１１）式により表現される。
【００４５】
【数６】
2018155639.tif 000008
【００４６】
光源３１の配光分布が関数Ｉ（θａｉ（ｋ））により決定される場合、時刻ｋにおける各照度センサ４１の観測値ｓｌｉ（ｋ）は、下記（１２）式により表現される。ここで、αは、（光源３１からの光の光速）／（配光分布での基準光速）であり、ｎｉは観測ノイズである。観測ノイズｎｉは、標準偏差σｉ、平均０のガウス分布に従うノイズであるとする。
【００４７】
【数７】
2018155639.tif 000009
【００４８】
上記（１２）式は、照度センサ４１に関する観測方程式をなす。以降、（１２）式の観測方程式を、（１３）式のようにＺ（ｋ）を表現する。すなわち、照度センサ４１に関する観測方程式Ｚ（ｋ）は、二輪台車２０の位置を示す状態ベクトルＸ（ｋ）を変数とする、照度センサ４１に関する観測関数ｈ（Ｘ（ｋ））と観測ノイズｎｉの転置との和により表現される。なお、照度センサ４１の個数はｍ個とする。以降、観測ノイズ共分散行列Ｒは、下記の（１４）式により表されるものとする。
【００４９】
【数８】
2018155639.tif 000010
【００５０】
次に、フィルタリングステップを示す。上記（１２）式及び（１３）式に示すように、観測方程式が非線形式になるため、フィルタリングステップには拡張カルマンフィルタやアンセンテッドカルマンフィルタを用いる必要がある。但し、状態から実測値までの関係が煩雑なため、拡張カルマンフィルタは適用しにくい。よって本実施形態に係るフィルタリングステップにおいては、アンセンテッドカルマンフィルタを用いる。
【００５１】
フィルタリングステップにおいては、上記（６）式の事前状態推定値Ｘｐ（ｋ）と上記（１３）の観測方程式Ｚ（ｋ）とに基づいて、アンセンテッドカルマンフィルタによるフィルタリングを行い、事前状態推定値Ｘｐ（ｋ）を修正して、時刻ｋにおける事後状態推定値Ｘｓ（ｋ）を推定する。また、上記（６）式の事前誤差共分散行列Ｐｐ（ｋ）と上記（１３）の観測方程式Ｚ（ｋ）とに基づいて、アンセンテッドカルマンフィルタによるフィルタリングを行い、事前誤差共分散行列Ｐｐ（ｋ）を修正して、時刻ｋにおける事後状態推定値Ｐｓ（ｋ）を推定する。
【００５２】
具体的には、まず、事前状態推定値Ｘｐ（ｋ）の２ｎ＋１個のσポイントを作成する。σポイントは、１個の平均値と当該平均値回りの２ｎ個の標準偏差とを含む。平均値に対応するσポイントΧ０−は下記（１５）式により表現され、１からｎ番目までの標準偏差に対応するσポイントΧｉ−は下記（１６）式により表現され、ｎ＋１から２ｎ番目までの標準偏差に対応するσポイントΧｎ＋ｉ−は下記（１７）式により表現される。なお、上記の通り、状態変数が４個であるため、ｎ＝４であり、σ＝９である。変数κは調整パラメータである。
【００５３】
【数９】
2018155639.tif 000011
【００５４】
以下、Уｉ−（ｋ）＝ｈ（Χｉ−（ｋ））とする。Уｉ−（ｋ）は、ｉ番目のσポイントに対するｍ個の照度センサ４１の物理量検出値（観測値）の事前出力推定値である。事前出力推定値у＾−（ｋ）は、下記の（１８）式の通り、Уｉ−（ｋ）と重みＷｉとに基づくアンセンテッド変換により算出される。
【００５５】
【数１０】
2018155639.tif 000012
【００５６】
同様に、時刻ｋにおける事前出力誤差共分散行列Ｐｙｙ−（ｋ）は、下記の（１９）式の通り、Уｉ−（ｋ）と事前出力推定値у＾−（ｋ）と重みＷｉとに基づくアンセンテッド変換により算出される。
【００５７】
【数１１】
2018155639.tif 000013
【００５８】
更に、時刻ｋにおける事前状態・出力誤差共分散行列Ｐｘｙ−（ｋ）は、下記の（２０）式の通り、σポイントΧｉ−と事前状態推定値Ｘｐ（ｋ）とУｉ−（ｋ）と事前出力推定値у＾−（ｋ）と重みＷｉとに基づくアンセンテッド変換により算出される。
【００５９】
【数１２】
2018155639.tif 000014
【００６０】
次に、下記の（２１）式の通り、事前状態・出力誤差共分散行列Ｐｘｙ−（ｋ）と事前出力誤差共分散行列Ｐｙｙ−（ｋ）と観測ノイズ共分散行列Ｒとに基づいてカルマンゲインＧ（ｋ）が算出される。
【００６１】
【数１３】
2018155639.tif 000015
【００６２】
そして、下記の（２２）式に従い、事前状態推定値Ｘｐ（ｋ）とカルマンゲインＧ（ｋ）と照度センサ４１に関する観測方程式Ｚ（ｋ）と事前出力推定値у＾−（ｋ）とに基づいて事後状態推定値Ｘｓ（ｋ）を算出する。事後状態推定値Ｘｓ（ｋ）は、二輪台車２０の時刻ｋにおける推定位置として出力される。また、下記の（２３）式に従い、事前誤差共分散行列Ｐｐ（ｋ）とカルマンゲインＧ（ｋ）と事前状態・出力誤差共分散行列Ｐｘｙ−（ｋ）とに基づいて事後誤差共分散行列Ｐｓ（ｋ）を算出する。
【００６３】
【数１４】
2018155639.tif 000016
【００６４】
事後状態推定値Ｘｓ（ｋ）と事後誤差共分散行列Ｐｓ（ｋ）とが算出されるとフィルタリングステップが終了する。二輪台車２０の推定位置は、予測ステップとフィルタリングステップが繰り返されることにより更新される。
【００６５】
以上により、本実施形態に係る位置推定部７１による二輪台車２０の位置推定の一例の説明を終了する。
【００６６】
次に、本実施形態に係る移動体位置推定システム１による位置推定処理の流れを説明する。図４は、本実施形態に係る演算装置１５の制御に従い行われる二輪台車２０の位置推定に関する典型的な流れを示す図である。
【００６７】
図４に示すように、まず、演算装置１５の位置推定部７１は、時刻ｋを初期値０に設定する（ステップＳ１）。
【００６８】
ステップＳ１が行われると演算装置１５の移動体制御部７３は、二輪台車２０への入力指令を行う（ステップＳ２）。初期値ｋ＝０において移動体制御部７３は、目標軌道設定部７２により設定された目標軌道に基づく移動指令を、第３通信ＩＦ１３を介して移動体（二輪台車）２０に供給する。二輪台車２０は、移動指令に従い屋内を移動する。二輪台車２０の移動中、照度センサ４１−ｎは光源３１から発せられる光の照度を検出し、検出された照度値を第１通信ＩＦ１１を介して演算装置１５に供給される。
【００６９】
ステップＳ２が行われると演算装置１５の支持体制御部７４は、ステップＳ３を行う。なお、時刻ｋが初期値ｋ＝０の場合、事後誤差共分散行列Ｐｓ（ｋ−１）が算出されていないので、ステップＳ３及びステップＳ４は行われない。すなわち、光源３１の姿勢角は初期角度に固定される。
【００７０】
ステップＳ５において演算装置１５の位置推定部７１は、位置推定処理（ステップＳ５及びＳ６）を行う。まず位置推定部７１は、（１５）式−（２３）式を用いたアンセンテッドカルマンフィルタに基づくフィルタリングステップを実行する（ステップＳ５）。ステップＳ５において位置推定部７１は、光源３１の位置と光源３１の姿勢角と複数の照度センサからの複数の照度値と光源３１の配光分布とにアンセンテッドカルマンフィルタを適用してｋ＝０の事後状態推定値とｋ＝０の事後誤差共分散行列とを推定する。具体的には、位置推定部７１は、（１２）−（１４）式に従う、ｋ＝０における光源３１の位置と光源３１の姿勢角と光源３１の配光分布とに基づく観測方程式に対応する観測値Ｚ（０）を物理量検出部４０から取得する。また、位置推定部７１は、（１５）−（１７）式に従い事前状態推定値Ｘｐ（０）の複数のσポイントを算出する。（１５）式に用いられる事前状態推定値Ｘｐ（０）としては所定の初期値等が用いられると良い。
【００７１】
次に位置推定部７１は、事前状態推定値Ｘｐ（０）のσポイントに基づいてУｉ−（ｋ）を算出し、（１８）式に従い各σポイントに対応する事前出力推定値Уｉ−（０）と重みＷｉとに基づくアンセンテッド変換により事前出力推定値を算出し、各σポイントに対応する事前出力推定値Уｉ−（０）と事前出力推定値у＾−（０）と重みＷｉとに基づくアンセンテッド変換により事前出力誤差共分散行列Ｐｙｙ−（０）を算出し、（２０）式に従い、σポイントΧｉ−と事前状態推定値Ｘｐ（０）と各σポイントに対応する事前出力推定値Уｉ−（０）と事前出力推定値у＾−（０）と重みＷｉとに基づくアンセンテッド変換により事前状態・出力誤差共分散行列Ｐｘｙ−（０）を算出する。そして位置推定部７１は、（２１）式に従い、事前状態・出力誤差共分散行列Ｐｘｙ−（ｋ）と事前出力誤差共分散行列Ｐｙｙ−（０）と観測ノイズ共分散行列Ｒとに基づいてカルマンゲインＧ（０）を算出し、下記の（２２）式に従い、事前状態推定値Ｘｐ（０）とカルマンゲインＧ（０）と照度センサ４１から得られる観測値Ｚ（０）と事前出力推定値у＾−（０）とに基づいて事後状態推定値Ｘｓ（０）を算出する。事後状態推定値Ｘｓ（０）は、二輪台車２０の時刻ｋ＝０における推定位置として出力される。また、位置推定部７１は、（２３）式に従い、事前誤差共分散行列Ｐｐ（０）とカルマンゲインＧ（０）と事前状態・出力誤差共分散行列Ｐｘｙ−（０）とに基づいて事後誤差共分散行列Ｐｓ（０）を算出する。
【００７２】
ステップＳ５が行われると位置推定部７１は、（４）式−（７）式を用いた拡張カルマンフィルタに基づく予測ステップを行う（ステップＳ６）。ステップＳ６において位置推定部７１は、ステップＳ５において算出されたｋ＝０の事後状態推定値と事後誤差共分散行列とに拡張カルマンフィルタを適用して、移動体２０の推定位置である時刻ｋ＝１の事前状態推定値と事前誤差共分散行列とを推定する。具体的には、位置推定部７１は、（６）式に従いｋ＝０の事後状態推定値Ｘｓ（０）と内部センサ情報ｕ（０）とに基づいて、時刻ｋ＝１における事前状態推定値Ｘｐ（１）を算出する。また、位置推定部７１は、（４）式に従い（１）式の関数ｆを状態ベクトルＸで偏微分することにより行列Ｆ（ｋ）を算出し、（５）式に従い関数ｆを状態ベクトルｕで偏微分することにより行列Ｇ（ｋ）が算出し、（７）式に従い行列Ｆ（０）と事後誤差共分散行列Ｐｓ（０）と行列Ｇ（０）とシステム雑音共分散行列Ｑとに基づいて、時刻ｋ＝１における事前誤差共分散行列Ｐｐ（１）を算出する。
【００７３】
ステップＳ６において位置推定部７１は、位置推定処理を終了するか否かを判定する（ステップＳ７）。例えば、操作機器６０等を介してユーザから終了指示が入力された場合、あるいは、所定の時刻が経過した場合、位置推定部７１は、位置推定処理を終了すると判定する。終了するか否かの判定結果は、表示機器５０に表示されても良い。
【００７４】
ステップＳ７において位置推定処理を終了しないと判定された場合（ステップＳ７：ＮＯ）、位置推定部７１は、時刻ｋを次の時刻ｋ＋１に設定する（ステップＳ８）。そして、再び時刻ｋ＋１についてステップＳ２からステップＳ７が繰り返される。
【００７５】
時刻ｋ（ｋ＝１以上）の場合、ステップＳ２において移動体制御部７３は、二輪台車２０への入力指令を行う（ステップＳ２）。時刻ｋ（ｋ＝１以上）の場合、移動体制御部７３は、目標軌道における時刻ｋ（ｋ＝１以上）での移動体２０の位置と時刻ｋ−１での移動体２０の推定位置との差分を算出し、算出された差分を埋めるための移動体２０の各可動軸の駆動量を算出し、各可動軸の駆動量に応じた移動指令を生成する。生成された移動指令は、第３通信ＩＦ１３を介して移動体（二輪台車）２０に供給される。二輪台車２０は、移動指令に従い屋内を移動する。
【００７６】
時刻ｋ（ｋ＝１以上）のステップＳ２が行われると支持体制御部７４は、時刻ｋ−１の事後誤差共分散行列Ｐｓが閾値よりも大きいか否かを判定する（ステップＳ３）。時刻ｋ−１の事後誤差共分散行列Ｐｓが閾値よりも大きい場合、時刻ｋ−１の事後状態推定値Ｘｓ（ｋ−１）、すなわち、推定位置の精度が良くない事を意味する。反対に、事後誤差共分散行列Ｐｓが閾値よりも小さい場合、時刻ｋ−１の事後状態推定値Ｘｓ（ｋ−１）の精度が良い事を意味する。なお、閾値はユーザにより操作機器６０等を介して任意の値に設定可能である。事後誤差共分散行列Ｐｓが閾値よりも大きいか否かの判定結果は、表示機器５０に表示されても良い。
【００７７】
従って事後誤差共分散行列Ｐｓが閾値よりも小さいと判定された場合（ステップＳ３：ＮＯ）、支持体制御部７４は、光源の姿勢角を、時刻ｋ−１における推定位置、すなわち、事後状態推定値Ｘｓ（ｋ−１）に向ける（ステップＳ４）。ステップＳ４において支持体制御部７４は、二輪台車２０の推定位置Ｘｓ（ｋ−１）に、光を照射するための光源３１の目標の姿勢角を算出し、現在の姿勢角から目標の姿勢角へ変更するための支持体３２の各可動軸の駆動量を算出し、各可動軸の駆動量に応じた角度変更指令を生成する。生成された角度変更指令は、第２通信ＩＦ１２を介して駆動部３３に供給される。そして駆動部３３は、角度変更指令に従い駆動して支持体３２を動かして光源３１を目標の姿勢角に移動させる。これにより光源３１からの光は二輪台車２０の推定位置に照射される。換言すれば、光源３１からの光が二輪台車２０を追従する。これにより、二輪台車２０が光源３１から離れた場合であっても、光源３１からの光を常に二輪台車２０に取り付けられた照度センサ４１−ｎにより良好に検出させることができる。従って、位置推定誤差を低減させるこができる。
【００７８】
一方、時刻ｋ（ｋ＝１以上）の事後誤差共分散行列Ｐｓが閾値よりも大きいと判定された場合（ステップＳ３：ＹＥＳ）、支持体制御部７４は、最後に姿勢角を変更した時刻の姿勢角を維持する。これにより、位置推定精度が良好でない場合に、位置推定精度が良好であった時刻の姿勢角が維持されるので、位置推定誤差の劣化を防止することができる。例えば、光源３１が鉛直下方（φ＝−π／２）を向いている場合、光源３１から発生される光の照度の空間分布が、ｘｙ平面において同心円状に対称になるので、二輪台車２０の位置の判別能が低下する。上記の通り姿勢角を維持することにより、推定誤差が劣化する姿勢角に変更されるのを防止することができる。
【００７９】
時刻ｋ（ｋ＝１以上）のステップＳ３において事後誤差共分散行列Ｐｓ（ｋ−１）が閾値よりも大きいと判定された場合（ステップＳ３：ＹＥＳ）又はステップＳ４が行われた場合、位置推定部７１は、１５）式−（２３）式を用いたアンセンテッドカルマンフィルタに基づくフィルタリングステップを実行（ステップＳ５）し、時刻ｋにおける事後状態推定値Ｘｓ（ｋ）と事後誤差共分散行列Ｐｓ（ｋ）とを算出する。時刻ｋにおける事後状態推定値Ｘｓ（ｋ）と事後誤差共分散行列Ｐｓ（ｋ）との算出方法は、時刻ｋ＝０のステップＳ５と同様の方法であるのでここでの説明は省略する。事後状態推定値Ｘｓ（ｋ）と事後誤差共分散行列Ｐｓ（ｋ）とは、表示機器５０に表示されても良い。
【００８０】
ステップＳ５が行われると位置推定部７１は、（４）式−（７）式を用いた拡張カルマンフィルタに基づく予測ステップを実行し（ステップＳ６）、時刻ｋ＋１における事前状態推定値Ｘｐ（ｋ＋１）と事前誤差共分散行列Ｐｐ（ｋ＋１）とを算出する。時刻ｋ＋１における事前状態推定値Ｘｐ（ｋ＋１）と事前誤差共分散行列Ｐｐ（ｋ＋１）との算出方法は、時刻ｋ＝０のステップＳ６と同様の方法であるのでここでの説明は省略する。事前状態推定値Ｘｐ（ｋ＋１）と事前誤差共分散行列Ｐｐ（ｋ＋１）は、表示機器５０に表示されても良い。
【００８１】
ステップＳ６において位置推定部７１は、位置推定処理を終了するか否かを判定する（ステップＳ７）。
【００８２】
ステップＳ７において位置推定処理を終了しないと判定された場合（ステップＳ７：ＮＯ）、位置推定部７１は、時刻ｋを次の時刻ｋ＋１に設定する（ステップＳ８）。そして、再び時刻ｋ＋１についてステップＳ２からステップＳ７が繰り返される。
【００８３】
そしてステップＳ７において位置推定処理を終了すると判定された場合（ステップＳ７：ＹＥＳ）、演算装置１５は、二輪台車２０の位置推定の処理を終了する。
【００８４】
なお、上記の処理の流れは一例であって、種々の変更が可能である。例えば、エネルギー発生源３１の姿勢角は、下記に示す通り、必ずしも変更されなくても良い。また、初期時刻においてステップＳ３及びＳ４が実行されないとしたが、初期時刻から数時刻に亘り実行されなくても良い。
【００８５】
次に、本実施形態に係る位置推定処理の妥当性をシミュレーションにより検証する。図５は、第１シミュレーション条件を模式的に示す図である。また、当該第１シミュレーション条件を以下に示す。移動体２０をベース基地に置いた場合、多少位置、姿勢ズレが生じることを想定し、移動体２０の位置の初期値と推定アルゴリズムの初期値とがずれているものとする。
【００８６】
＜第１シミュレーション条件＞
・二輪台車２０のタイヤ径ｒ＝０．２ｍ
・二輪台車２０の幅ｄ＝０．５ｍ
・光源３１の位置（ｘ、ｙ、ｚ）＝（−０．３、−０．３、３）
・光源３１の配光分布：適当な分布
・光源３１の光束＝１０００ルクス
・二輪台車２０上の照度センサ４１の個数＝５個
・二輪台車２０上の照度センサ４１の位置ｓｉ：ｓ０＝（０、０、０）、Ｓ１＝（０．２５、０、０．１）、ｓ２＝（０、０．２５、０．１５）、ｓ３＝（−０．２５、０、０．２）、ｓ４＝(０、−０．２５、０．０５)
・照度センサ４１の誤差＝５ルクス標準偏差
・二輪台車２０のエンコーダ分解能＝２０００パルス
・エンコーダ誤差標準偏差＝分解能による三角分布ガウス分布近似
・二輪台車２０の初期値(ｘ、ｙ、φ、ｒ)＝（０．０１、０．０１、０．０５、０．２）
・事前状態推定値の初期値(ｘ、ｙ、φ、ｒ)＝（０、０、０、０．２＋０．０３）
・二輪台車２０の右車輪のサンプリング間隔でのエンコーダ変化量ｕr（ｋ）と左車輪のサンプリング間隔でのエンコーダ変化量ｕl（ｋ）との入力は、時刻ｔが０＜ｔ＜４の場合は下記の（２４）式であり、時刻ｔが４＜ｔ＜８の場合は下記の（２５）式である。
【００８７】
【数１５】
2018155639.tif 000017
【００８８】
図６は、上記第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定軌道のシミュレーション結果を示す図である。図６の（ａ）は二輪台車２０のｘ軸座標の時間推移を示し、（ｂ）は二輪台車２０のｙ軸座標の時間推移を示し、（ｃ）は二輪台車２０のｘｙ平面の軌跡を示し、（ｄ）は二輪台車２０の姿勢ψの時間推移を示す。図７は、上記第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定誤差のシミュレーション結果を示す図である。図７の（ａ）は二輪台車２０のｘ軸座標及びｙ軸座標の推定誤差の時間推移を示し、（ｂ）は二輪台車２０の姿勢ψの推定誤差の時間推移を示し、（ｃ）は二輪台車２０のタイヤ半径ｒの推定誤差の時間推移を示し、（ｄ）は二輪台車２０のパン角及びチルト角の時間推移を示す。図７に示すように、十分時間が経過した２ｓ以降では、ｘ軸及びｙ軸に関する位置誤差が０．０２ｍ以下に抑えられており、また、姿勢及びタイヤ半径も十分な精度で推定できていることがわかる。更に、光源３１のパン角及びチルト角も適切に稼働していることがわかる。
【００８９】
図８は、上記第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた事後誤差共分散行列Ｐｓの対角成分ｘ、ｙ、ψ及びｒの時系列推移を示す図である。図８の縦軸は対角成分の値をであり０に近いほど精度が高く、１に近いほど低いことを示す。図８の横軸は時間に規定される。図８に示すように、事後共分散行列の対角成分は、十分小さな値となり推定が良好に進んでいることがわかる。図９は上記第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各照度センサ４１−ｎ（ｎ＝１〜５）の実測値と推定値を示す図である。図９の縦軸は照度［ｌｘ］に規定され、横軸は時間「ｓ」に規定される。図９の（ａ）は実測値の時間推移を示し、図９の（ｂ）は推定値の時間推移を示す。図９に示すように、ノイズを良好に除去できていることがわかる。図１０は、上記第１シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られたオドメトリでの推定結果を示す。図１０の縦軸はｙ軸に規定され、横軸はｘ軸に規定される。図１０に示すように、推定誤差が時間経過とともに蓄積され推定精度が劣化してくことがわかる。
【００９０】
図６−図１０に示すように、本実施形態に係る位置推定処理が妥当であることが検証される。
【００９１】
次に、本実施形態に係る移動体２０の最小個数について検証する。三次元空間を移動する移動体２０に対し本実施形態に係る位置推定処理を適用する場合、エネルギー発生源３１の取付け位置と移動体２０との相対位置を考慮すると、空間上にて位置を把握するためには少なくとも３個の物理量検出器４１が必要である。以下、照度センサ４１を３個にした場合のシミュレーション結果を示す。当該シミュレーションは、第２シミュレーション条件のもとに行われた。第２シミュレーション条件において移動体２０上の照度センサ４１の位置ｓｉは、ｓ０＝（０、０、０）、Ｓ１＝（０．２５、０、０）、ｓ２＝（０、０．２５、０）である。第２シミュレーション条件のその他の条件は、第１シミュレーション条件と同一である。
【００９２】
図１１は、上記第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定軌道のシミュレーション結果を示す図である。図１２は、上記第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定誤差のシミュレーション結果を示す図である。図１１及び図１２を図６及び図７に対して比較すれば分かるように、第２シミュレーションの推定誤差は、第１シミュレーションに比して推定誤差がｙ座標で０．０７ｍ程度増加するが、十分な推定が行われている。図１３は、上記第２シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各照度センサ４１の実測値と推定値を示す図である。図１３を図９に対して比較すれば分かるように、第２シミュレーションは、第１シミュレーションと同様にノイズを良好に除去できている。
【００９３】
次に、本実施形態に係る位置推定処理の精度向上方法について記載する。
【００９４】
図１４は、上記第２シミュレーション条件のもとにエネルギー発生源３１の姿勢角を真下に固定したい場合の移動体２０の軌道の推定結果を示す図である。図１５は、図１４の第２シミュレーション条件のもとでの各照度センサの真の実測値と推定値とを示す図である。図１４に示すように、移動体２０の真の軌道と推定軌道との間に乖離が生じる。頻繁にこのような現象が生じるわけではないが、推定ミスが生じる可能性があることを示す。図１４に示す真の移動体軌道と推定移動体軌道とでは、ともに図１５に示すように各照度センサの実測値が一致してしまう。これは、真下に光源を向けている場合、光源分布が同心円状で同一となり判別不能となるためである。つまり、実測値のみから位置を特定できていないことを意味している。逆に、光源の姿勢角が斜めとなる角度範囲においては、光源の配光分布が楕円分布となるため、図４の処理の流れにおける姿勢角の稼働（ステップＳ２)が省略されても良い。
【００９５】
上記の推定ミスは、光源が光を真下に照射することにより生じた。よって、本提案手法においても、移動体２０がエネルギー発生源の真下近傍領域に入った場合、推定ミスが生じる可能性がある。以下に解決方法を示す。
【００９６】
第１の解決策：エネルギー発生源３１の姿勢角を、鉛直を含む角度範囲（以下、禁止角度範囲と呼ぶ）以外の角度範囲（以下、許容角度範囲と呼ぶ）に制限する。換言すれば、エネルギー発生源３１を真下近傍に向かせない。鉛直方向に寄与する姿勢角の成分はチルト角φである。具体的には、支持体制御部７４は、ステップＳ２において事後誤差共分散行列Ｐｓ（ｋ−１）が閾値より大きくないと判定された場合（ステップＳ２：ＮＯ）、事後状態推定値Χｅ（ｋ−１）、すなわち、移動体２０の推定位置へのチルト角φが鉛直角度範囲に含まれるか許容角度範囲に含まれるかを判定する。推定位置へのチルト角φが禁止角度範囲に含まれると判定された場合、支持体制御部７４は、時刻ｋ−１のチルト角φを維持し、ステップＳ４に移行する。推定位置へのチルト角φが許容角度範囲に含まれると判定された場合、支持体制御部７４は、ステップＳ３に移行し、推定位置へのチルト角φにエネルギー発生源３１の姿勢角を向けるために角度変更指令を駆動部３３に供給する。禁止角度範囲は、物理量検出器４１−ｎの分散特性とエネルギー発生源３１から照射されるエネルギーの空間分布とに基づく決定される。なお、禁止角度範囲は、エネルギー発生源３１から照射される光の中心軸Ａ１が−Ｚ軸に平行する１８０°を含むのであれば、任意の角度範囲に設定可能である。
【００９７】
第１の解決策は、エネルギー発生源３１の真下近傍に移動体２０が存在しないことを前提とすれば簡便でよい対策である。倉庫での利用などを考えると、壁面に光源３１を設置した場合、壁面近傍が真下となり、壁面近傍に移動体２０を誘導するニーズは少ないため合理的解決策と考えられる。
【００９８】
第２の解決策：物理量検出器４１−ｎを移動体２０において異なる高度に取り付ける。エネルギー発生源３１から発せられるエネルギーは伝播距離に応じて減衰するので、移動体２０がエネルギー発生源３１の真下に位置する場合においても、各物理量検出器４１−ｎが検出する物理量の値が異なることとなる。これにより、空間情報をより効率的に利用することができる。
【００９９】
第３の解決策：エネルギー発生源３１として光源を用いる場合、光源に分光フィルム（グラディエーションフィルム）を貼る。図１６は、分光フィルムが貼り付けられた光源を模式的に示す図である。図１７は、図１６の分光フィルムが貼り付けられた光源から照射された光の配光分布を模式的に示す図である。図１６及び図１７に示すように、パン角θ回りに異なる赤色のフィルム、青色のフィルム、緑色のフィルムを順番に貼り付けることにより、パン角θ方向に関する照度分布を波長ごとに変えることができる。この場合、移動体２０に取り付ける照度センサ４１−ｎに、例えば、光源に入り付けた分光フィルムに対応するカラーセンサを用いる。各カラーセンサは、当該色に対応する波長の光強度を計測する。例えば、青色、緑色及び赤色のカラーセンサは、それぞれ青色、緑色及び赤色に対応する波長の光強度を計測することができる。エネルギー発生源３１が真下に光を照射している場合においても、波長の違いにより、パン角θ方向を識別することができる。これにより、位置推定精度が向上する。
【０１００】
図１８は、図１６とは異なる態様で分光フィルムが貼り付けられた光源を模式的に示す図である。図１８に示すように、パン角θ方向に関して異なる角度間隔で光源に分光フィルムが貼り付けられる。例えば、パン角θが０°−１２０°の範囲は赤色に割り当てられ、１２０°−２４０°の範囲は緑色に割り当てられ、２４０°−０°の範囲は青色に割り当てられる。各色のパン角範囲全体に分光フィルムを貼り付けるのではなく、図１８に示すように、分光フィルムを貼り付けない領域（透明）が設けられると良い。透明の領域の角度範囲はランダムに設けられると良い。このように、透明の領域をランダムに設けることにより、パン角θ方向に関する位置識別能が更に向上し、エネルギー発生源３１が真下に向いている状態における移動体２０の位置推定精度を更に改善する。小型の移動体２０やエネルギー発生源３１の取付け高さが高い場合、青フィルム、緑フィルム、赤フィルムの貼り付け間隔を狭めると良い。これにより、移動体２０に取り付けられた複数のカラーセンサが同一の分光フィルムの角度範囲に含まれることを防止し、位置推定精度を更に向上させることができる。
【０１０１】
第４の解決策：２つ以上のエネルギー発生源３１を用いる。この場合、一のエネルギー発生源３１の禁止角度範囲に対応するエネルギーの照射範囲が、他の一のエネルギー発生源３１の許容角度範囲に対応するエネルギーの照射範囲に含まれるように複数のエネルギー発生源３１が配置される。
【０１０２】
図１９は、二つのエネルギー発生源３１の配置を示す図である。図１９のφａはチルト角上限であり、φｂはチルト角下限を示す。高さｈは、二つのエネルギー発生源３１の床面からの設置高さを示す。図１９に示すように、二つのエネルギー発生源３１を設けることにより、一のエネルギー発生源３１のチルト制約角φｂを、他方のエネルギー発生源３１で補うことができる。第１のエネルギー発生源３１の禁止角度範囲Ｒ１１が第２のエネルギー発生源３１の許容角度範囲Ｒ２２に包含され且つ第２のエネルギー発生源３１の禁止角度範囲Ｒ１２が第１のエネルギー発生源３１の許容角度範囲Ｒ２１に包含されるように第１のエネルギー発生源３１と第２のエネルギー発生源３１との間の距離が設定される。具体的には、二つのエネルギー発生源３１の間隔は、ｈ／（ｔａｎ（φａ））−ｈ・ｔａｎ（φｂ）以下に設定される。この方法により、エネルギー発生源３１の数は増えるが、推定ミスエリアをなくすことが可能となる。この場合、移動体２０には二つ以上のエネルギー発生源３１からエネルギーが照射されるため、各エネルギー発生源３１からのエネルギーを分離して計測できることが望ましい。
【０１０３】
エネルギー発生源３１として光源を用いる場合、分光フィルムなどを用い光源ごとに波長を変える。例えば、第１の光源３１には赤色フィルムを貼り付け、第２の光源には青色フィルムが貼り付けられる。移動体２０に取り付ける照度センサは、波長ごとに照度を検出可能なセンサ、もしくは、周波数ごとに複数の照度センサが用意されると良い。
【０１０４】
エネルギー発生源３１としてパラメトリックスピーカを用いる場合、音源ごとに出力周波数を変える。音源毎に周波数を変えることにより、互いの音源が発生する音波を区別することができる。物理量検出器としては、音圧を電気信号に変換するマイクロフォンが用いられる。パラメトリックスピーカに関しては、非特許文献２及び３に詳述されている。図２０は、エンドファイアレイを考慮した、出力周波数１ｋＨｚのパラメトリックスピーカから発生される音圧分布を示す図である。図２０に示すように、パラメトリックスピーカから発生される音波は、指向性が鋭く、音圧勾配が光源に比べ高く、本実施形態に係る位置推定に適している。また、環境反射の影響も一次反射以外寄与率が低く、また一次反射も全反射するため、部屋環境にて環境反射の影響が低い。よって、残響成分をマイクロフォンが拾わない点でも本実施形態に係る位置推定への適用に適している。
【０１０５】
次に、エネルギー発生源３１として二台のパラメトリックスピーカを用いた場合の第４の解決策の妥当性検証のための第３のシミュレーションについて示す。第３のシミュレーション条件は、以下の通りである。１ｋＨｚを出力するパラメトリックスピーカ（ＰＡＬ１）と２ｋＨｚを出力するパラメトリックスピーカ（ＰＡＬ２）とが用いられる。パラメトリックスピーカ設置位置は、ＰＡＬ１＝(０．３、−０．３、３．０)、ＰＡＬ２＝（−０．３、−１．７、３．０）である。前半移動時ではＰＡＬ１が移動体２０の真上に近く、後半移動時ではＰＡＬ２が移動体２０の真上に近い設定とする。すなわち、ＰＡＬ一台では推定ミスが生じ得る設定である。マイクノイズ標準偏差は０．５ｄＢとする。観測方程式は、下記の（２６）式に示される。
【０１０６】
【数１６】
2018155639.tif 000018
【０１０７】
光源の時と同様、拡張カルマンフィルタで推定する（ここでＰｉはＰＡＬ(ｉ=１、２)の音圧分布を示し、ｊはマイクロフォン（ｍｉｃ）の番号を示し、ｒｉｊはＰＡＬ（ｉ）からｍｉｃ（ｊ）までのベクトル，θａｉｊはＰＡＬの向きとｒｉｊとのなす角）。移動体２０に取付けられるマイクロフォンは３個とし、マイクロフォンの取付け位置はｓ０＝（０、０、０．２）、ｓ１＝（０．２５、０、０．２３）、ｓ２＝（０、０．２５、０．２５）である。ここで、第２の解決策に基づきマイクロフォンは、異なる高度に取り付けられている。各マイクロフォンの実測値は１ｋＨｚ及び２ｋＨｚの２種類の周波数があるため、合計６系統の実測値となる。上記以外の第３のシミュレーション条件は、上記第２のシミュレーション条件と同じである。
【０１０８】
図２１は、上記第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定軌道のシミュレーション結果を示す図である。図２２は、図２１の第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定誤差のシミュレーション結果を示す図である。図２３は、図２１の第３シミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた各パラメトリックスピーカのパン角及びチルト角の時間推移を示す図である。図２４は、図２１のシミュレーション条件のもとでの各照度センサの実測値と推定値とを示す図である。図２４の「１Ｋ」及び「２Ｋ」は、それぞれ第１周波数の系統及び第２周波数の系統を示す。図２１及び図２２に示すように、第３のシミュレーションによれば、位置誤差０．０２ｍ以下の十分な位置推定が実行される。図２３に示すように、光源３１が真下を向くエリア付近でも高い推定が行われていることである。よって、第３の解決策の妥当性が確かめられた。しかしながら、図２４に示すように、実測値がＰＡＬ１に対しては４ｓ以前でｍｉｃ２及びｍｉｃ３が類似し、６ｓ以降でｍｉｃ１及びｍｉｃ３が類似し、ｍｉｃ２に対しては４ｓ以降でｍｉｃ２及びｍｉｃ３で類似したものとなるため、推定精度に悪影響を与えている。実際６ｓ以降では観測値が実質４データとなり、推定精度が多少劣化していることがわかる。これは、パラメトリックスピーカの鋭い直進性によるものである。図２０に示すＰＡＬの音圧分布からわかるように、音波進行方向と垂直な面では音圧勾配は急峻だが、進行方向では直進性より勾配が緩やかである。よって、ＰＡＬの姿勢が斜めであっても、移動体２０の中央からｘｙ平面上にて同距離に設置したマイクロフォン２及び３は、音源付近では類似の音圧値を計測しやすくなる。
【０１０９】
次に、マイクロフォン２及び３の配置の対称性に起因する上記問題の解決策について説明する。この問題は、ｘｙ平面上における移動体２０の中央からマイクロフォン２までの距離とマイクロフォン３までの距離とを異ならせ、対称性を崩すことにより改善する。第３のシミュレーション条件からマイクロフォンの位置を変更して第４のシミュレーションを実行した。第４のシミュレーション条件におけるマイクロフォンの位置は、ｓ０＝（０、０、０．２）、ｓ１＝（０．２５、０、０．２３）、ｓ２＝（０、０．２５／２、０．２５）である。他の条件は、第３のシミュレーション条件と同様である。
【０１１０】
図２５は、第４のシミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定軌道のシミュレーション結果を示す図である。図２６は、図２５の第４のシミュレーション条件のもとに本実施形態に係る位置推定処理を実行して得られた二輪台車２０の推定誤差のシミュレーション結果を示す図である。図２７は、図２５の第４のシミュレーション条件のもとでの各照度センサの実測値と推定値とを示す図である。図２５、図２６及び図２７に示すように、推定誤差が０．０１ｍ以下であり、第４のシミュレーションは、第３のシミュレーションに比して推定精度が向上していることがわかる。また、図２５と図２２とを比較すればわかるように、マイクロフォンの観測値の類似現象が減り、各観測値を位置推定に効果的に反映できることがわかる。以上から、物理量検出器４１をエネルギーの空間分布を考慮し、移動体２０の中央から異なる距離に非対称に各物理量検出器４１を配置することにより、位置推定精度が向上することがわかる。
【０１１１】
なお、エネルギー発生源３１の位置は固定で個数は２つ以下の場合を示してきたが、本実施形態はこれに限定されない。３つ以上のエネルギー発生源３１を配置しても良いし、エネルギー発生源３１の位置も可変としても良い。この場合、エネルギー発生源３１の正確な位置を取得する内部センサが設けられると良い。当該内部センサはエネルギー発生源３１の位置情報を取得して演算装置１５に送信し、演算装置１５は、当該位置情報を上記位置推定処理に利用することにより、移動体２０の位置推定精度を高める。
【０１１２】
上記の説明の通り、本実施形態に係る移動体位置推定システム１は、エネルギー発生源３１、移動体２０、複数の物理量検出器４１−ｎ及び位置推定部７１を有する。エネルギー発生源３１は、伝播距離及び指向性に応じて物理量が変化するエネルギーを発生する。移動体２０は、位置推定対象である。複数の物理量検出器４１−ｎは、移動体２０に設けられ、エネルギー発生源３１から発生されたエネルギーの物理量を繰り返し検出する。位置推定部７１は、エネルギー発生源３１の位置とエネルギー発生源３１の姿勢角と複数の物理量検出器４１−ｎからそれぞれ出力された複数の物理量検出値とに基づいて移動体２０の推定位置を推定する。
【０１１３】
上記の構成により、本実施形態に係る移動体位置推定システム１は、伝播距離及び指向性に応じて物理量が変化するエネルギーを発生するエネルギー発生源３１を有するので、移動体２０が存在する空間全体に、エネルギーという位置情報を分布させることができる。複数の物理量検出器４１−ｎは、エネルギー発生源３１から発生されたエネルギーを検出するので、複数の物理量検出器４１−ｎ各々の物理量検出値は当該物理量検出器４１−ｎの位置に応じて異なる値を有することとなる。位置推定部７１は、エネルギー発生源３１の位置とエネルギー発生源３１の姿勢角と複数の物理量検出値とを利用することにより、移動体２０の推定位置を推定することが可能になる。
【０１１４】
従来例のような天井等に複数の超音波センサを１〜２ｍ単位で設置する必要ないので、本実施形態に係る移動体位置推定システム１は、低コストで高精度の位置推定が可能になる。また、本実施形態に係る移動体位置推定システム１は、従来例のようにＷｉ−Ｆｉ電波や屋内ＧＰＳを用いないので、電波の屋内マルチパスの影響により位置推定精度が下がることもない。また、超音波の到達時刻を利用した位置推定方法も知られているが、この方法によれば、超音波発生源と超音波検出器とを同期させる必要があり、コストが嵩む傾向にある。本実施形態に係る移動体位置推定システム１は、エネルギー発生源３１から発生されたエネルギーの物理量を用いるので、エネルギー発生源３１と物理量検出器４１−ｎとを同期する必要がない。また、本実施形態に係る移動体位置推定システム１は、デッドレコニングに加え、エネルギー発生源３１からのエネルギーという外部情報を利用するので、デッドレコニングに比して各段に推定精度を向上することができる。
【０１１５】
以上述べた実施形態によれば、簡易且つ高精度に位置推定可能な移動体位置推定システム、装置及び方法を提供することにある。
【０１１６】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。
【符号の説明】
【０１１７】
１…移動体位置推定システム、１０…移動体位置推定装置、１１…第１通信ＩＦ、１２…第２通信ＩＦ、１３…第３通信ＩＦ、１４…記憶装置、１５…演算装置、２０…移動体、３０…エネルギー照射器、３１…エネルギー発生源、３２…支持体、３３…駆動部、４０…物理量検出部、４０−ｎ…物理量検出器、５０…表示機器、６０…操作機器、７１…位置推定部、７２…目標軌道設定部、７３…移動体制御部、７４…支持体制御部。
【図１】
2018155639.tif 000019
【図２】
2018155639.tif 000020
【図３】
2018155639.tif 000021
【図４】
2018155639.tif 000022
【図５】
2018155639.tif 000023
【図６】
2018155639.tif 000024
【図７】
2018155639.tif 000025
【図８】
2018155639.tif 000026
【図９】
2018155639.tif 000027
【図１０】
2018155639.tif 000028
【図１１】
2018155639.tif 000029
【図１２】
2018155639.tif 000030
【図１３】
2018155639.tif 000031
【図１４】
2018155639.tif 000032
【図１５】
2018155639.tif 000033
【図１６】
2018155639.tif 000034
【図１７】
2018155639.tif 000035
【図１８】
2018155639.tif 000036
【図１９】
2018155639.tif 000037
【図２０】
2018155639.tif 000038
【図２１】
2018155639.tif 000039
【図２２】
2018155639.tif 000040
【図２３】
2018155639.tif 000041
【図２４】
2018155639.tif 000042
【図２５】
2018155639.tif 000043
【図２６】
2018155639.tif 000044
【図２７】
2018155639.tif 000045