JP7105819B2 - グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体 - Google Patents

Description

本出願は、グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体に関し、特に非同期センシングデータをグラフ構造に整合させることができる、グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体に関する。

走行位置探索を通じて自律走行を行う自律走行システムは主に船舶、航空機などに適用されており、最近では道路を走行する車両にも適用されている。

自律走行システムは、走行車両および周辺車両などの位置を基に走行行動および走行軌跡を決める。例えば、自律走行システムは走行経路、道路混雑度などのような様々な情報を、モニターを介してユーザに知らせたり、自ら車両を運転したり、走行状態を制御したりする。

自律走行システムにおける位置認識の正確性は走行の安全性につながる重要な要素技術であり、既存の自律走行システムは、自車に取り付けられたローカルセンサを用いて確率的に自車の位置および周辺車両の位置を認識した。

例えば、車両内にスキャニング装置、カメラ、レーダーなどのような全てのセンサ装置と、コンピューティング装置と、車両制御および自律走行用ソフトウェアとを搭載して、既に設定された条件に応じて車両が自動で走行することができた。

本出願は、グラフ構造のノード生成時間とセンサのセンシングデータの生成時間が互いに異なる場合、グラフ構造を修正してセンシングデータをグラフ構造内に整合させることができる、グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体を提供する。

本出願は、センサデータ補間法（ｓｅｎｓｏｒ ｄａｔａ ｉｎｔｅｒｐｏｌａｔｉｏｎ）とグラフノード補間法（ｇｒａｐｈ ｎｏｄｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ）を選択的に適用して、グラフ構造に追加されるノード数を最小化することができる、グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体を提供する。

本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法は、複数のセンサを用いて、移動体の移動に応じたセンシングデータを生成するセンシングステップ、基準周期に応じてノード（ｎｏｄｅ）を生成し、前記基準周期と同期化されたセンシングデータに前記ノード間の拘束条件（ｃｏｎｓｔｒａｉｎｔ）を設定してグラフ構造（ｇｒａｐｈ ｓｔｒｕｃｔｕｒｅ）を生成するグラフ構造の生成ステップ、前記センシングデータのうち、前記基準周期と同期化されていない非同期センシングデータが入力されれば、前記非同期センシングデータを用いて前記グラフ構造を修正するグラフ構造の修正ステップ、および前記グラフ構造を用いて、前記移動体の位置情報を生成する位置推定ステップを含む。

本発明の一実施形態による移動体は、複数のセンサを用いて、移動体の移動に応じた各々のセンシングデータを生成するセンサ部、基準周期に応じてノード（ｎｏｄｅ）を生成し、前記基準周期と同期化されたセンシングデータに前記ノード間の拘束条件（ｃｏｎｓｔｒａｉｎｔ）を設定してグラフ構造（ｇｒａｐｈ ｓｔｒｕｃｔｕｒｅ）を生成するグラフ構造構築部、前記センシングデータのうち、前記基準周期と同期化されていない非同期センシングデータが入力されれば、前記非同期センシングデータを用いて前記グラフ構造を修正するグラフ構造修正部、および前記グラフ構造を用いて、前記移動体の位置情報を生成する位置推定部を含む。

また、上記の課題を解決するための手段の記載は、本発明の特徴を全て列挙したものではない。本発明の様々な特徴とそれに応じた長所および効果は下記の具体的な実施形態を参照してより詳細に理解できるものである。

本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体によれば、グラフ構造のノード生成時間とセンサのセンシングデータの生成時間が互いに異なる場合、グラフ構造を修正してセンシングデータをグラフ構造内に整合させることができる。すなわち、非同期センシングデータをグラフ構造に反映することができるため、移動体の位置を正確に推定することができる。

本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体によれば、センサデータ補間法（ｓｅｎｓｏｒ ｄａｔａ ｉｎｔｅｒｐｏｌａｔｉｏｎ）とグラフノード補間法（ｇｒａｐｈ ｎｏｄｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ）を選択的に適用してグラフ構造を修正することができる。この場合、グラフ構造に追加されるノード数を最小化することができるため、グラフ構造の最適化時にかかる時間や演算量などを減少させることができ、位置推定のリアルタイム性を保持することができる。

但し、本発明の実施形態によるグラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体が達成できる効果は以上で言及したものに制限されず、言及していない他の効果も下記の記載によって本発明が属する技術分野における通常の知識を有する者に明らかに理解できるものである。

本発明の一実施形態による移動体を示すブロック図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造の生成を示す例示図である。 本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法を示すフローチャートである。

以下では添付図面を参照して本明細書に開示された実施形態について詳しく説明するが、図面符号に関係なく同一または類似した構成要素には同一の参照番号を付し、それに対する重複する説明は省略することにする。以下の説明で用いられる構成要素に対する接尾辞「モジュール」および「部」は、明細書の作成の容易さだけを考慮して付与または混用されるものであって、それ自体で互いに区別される意味または役割を有するものではない。すなわち、本発明で用いられる「部」という用語はソフトウェア、ＦＰＧＡまたはＡＳＩＣのようなハードウェア構成要素を意味し、「部」はある役割をする。ところが、「部」はソフトウェアまたはハードウェアに限定される意味ではない。「部」は、アドレッシングできるストレージ媒体にあるように構成されてもよく、一つまたはそれ以上のプロセッサを再生させるように構成されてもよい。よって、一例として「部」は、ソフトウェア構成要素、オブジェクト向けソフトウェア構成要素、クラス構成要素およびタスク構成要素のような構成要素と、プロセス、関数、属性、プロシージャ、サブルーチン、プログラムコードのセグメント、ドライバ、ファームウェア、マイクロコード、回路、データ、データベース、データ構造、テーブル、アレイおよび変数を含む。構成要素および「部」において提供される機能は、さらに小さい数の構成要素および「部」に結合されるか、またはさらなる構成要素と「部」とにさらに分離されてもよい。

また、本明細書に開示された実施形態を説明するにおいて、関連の公知技術に関する具体的な説明が本明細書に開示された実施形態の要旨を不要にあいまいにする恐れがあると判断される場合には、その詳細な説明は省略する。また、添付された図面は本明細書に開示された実施形態を容易に理解できるようにするためのものに過ぎず、添付された図面によって本明細書に開示された技術的思想が制限されるものではなく、本発明の思想および技術範囲に含まれる全ての変更、均等物乃至代替物を含むものとして理解しなければならない。

移動体（ｖｅｈｉｃｌｅ）は、自律走行する車両やロボット（ｒｏｂｏｔ）、ドローン（ｄｒｏｎｅ）などであってもよい。移動体は、現在位置を認識するための位置推定（ｌｏｃａｌｉｚａｔｉｏｎ）を行うことができ、位置推定により現在移動体の位置情報とポーズ情報などを確認した後、それに基づいて自律走行のための制御などを行うことができる。

移動体は、位置推定のために、複数のセンサからセンシングデータを収集することができ、入力を受けたセンシングデータを用いてグラフ構造（Ｇｒａｐｈ Ｓｔｒｕｃｔｕｒｅ）を生成することができる。ここで、グラフ構造は複数のセンシングデータを統合して示すものであり、グラフ構造を活用すれば、移動体の位置情報を迅速で且つ正確に抽出することができる。

位置推定により抽出する移動体の位置情報には、移動体の回転情報（ｒｏｔａｔｉｏｎ）、移動情報（ｔｒａｎｓｌａｔｉｏｎ）および速度情報（ｖｅｌｏｃｉｔｙ）を含むことができる。回転情報はピッチ（ｐｉｔｃｈ）、ロール（ｒｏｌｌ）、ヨー（ｙａｗ）の３軸回転量を含むことができ、移動情報は移動体が位置するマップ上の座標であってもよい。また、速度情報は、移動体の移動方向と速力を示す速度ベクトルであってもよい。すなわち、位置情報の回転情報（ｒｏｔａｔｉｏｎ）、移動情報（ｔｒａｎｓｌａｔｉｏｎ）および速度情報（ｖｅｌｏｃｉｔｙ）を用いて、移動体の現在位置とポーズを容易に把握することができる。

但し、移動体のセンサが測定するセンシングデータは各々の生成周期が互いに異なりうるし、非同期センシングデータが入力される場合、それに対する処理が問題になる。すなわち、グラフ構造の生成時、非同期センシングデータを除外する場合には、正確度に問題がありうるし、非同期センシングデータを適用する場合には、グラフ構造の修正などに応じた遅延によって位置情報のリアルタイム性に問題がありうる。

一方、本発明の一実施形態による移動体によれば、非同期センシングデータを反映してグラフ構造を修正することができ、それにより、迅速で且つ正確に移動体の位置推定を行うことができる。以下では、図１を参照して、本発明の一実施形態による移動体について説明する。

図１は、本発明の一実施形態による移動体を示すブロック図である。

図１を参照すれば、本発明の一実施形態による移動体１００は、センサ部１１０、グラフ構造構築部１２０、グラフ構造修正部１３０および位置推定部１４０を含むことができる。

センサ部１１０は、複数のセンサを含み、複数のセンサを用いて移動体１００の移動に応じた各々のセンシングデータを生成することができる。具体的には、図１に示すように、センサ部１１０には、ＩＭＵセンサ１１１、ＧＰＳセンサ１１２、ホイールエンコーダセンサ１１３、ライダーセンサ１１４などが含まれる。但し、センサ部１１０に含まれるセンサはこれらに限定されず、この他にも種々のセンサがセンサ部１１０に含まれてもよい。また、センサ部１１０に含まれるセンサは移動体１００の内部または外部に位置してもよく、移動体１００に位置するセンサの種類および個数は特に限定されない。

慣性測定ユニット（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）センサ１１１は、慣性測定手段として加速度センサおよびジャイロセンサ（ジャイロスコープ）を含むことができる。例えば、ＩＭＵセンサ１１１は、移動体１００の回転角として前進方向（Ｒｏｌｌ軸方向）、前進方向の右側方向（ｐｉｔｃｈ軸方向）、重力方向（ｙａｗ軸方向）の各々の角速度増分値と移動体１００の速度増分値などのセンシングデータを生成することができる。また、ＩＭＵセンサ１１１は、センシングデータを用いて、ＩＭＵオドメトリ（Ｏｄｏｍｅｔｒｙ）を生成することができる。すなわち、初期条件から加速度と角速度の変化量を測定して、移動体１００の移動方向および移動距離などのＩＭＵオドメトリを提供することもできる。

ＧＰＳセンサ１１２は、衛星と通信して移動体１００の地表上の座標をセンシングデータとして提供することができる。例えば、ＧＰＳセンサ１１２は、ＷＧＳ８４座標系の測地座標として、緯度、経度および楕円体高などを提供することができる。

ホイールエンコーダセンサ１１３は移動体１００に備えられたホイール（ｗｈｅｅｌ）の回転数を測定して提供するものであり、ホイールエンコーダセンサ１１３を利用すれば、移動体１００の移動距離と移動速度などを測定することができる。

ライダー（ＬｉＤＡＲ）センサ１１４は、レーザパルスなどを周辺に放射した後に反射して戻ってくる時間を測定して、移動体１００の周辺に対する３次元深さ情報を生成することができる。ここで、ライダーセンサ１１４を用いて移動体１００の回転量を測定するかまたはビジュアルオドメトリ（Ｖｉｓｕａｌ Ｏｄｏｍｅｔｒｙ）を生成することができる。したがって、ライダーセンサ１１４を利用すれば、移動体１００の移動方向、移動距離などを測定することができる。実施形態によっては、ライダーセンサ１１４としてＶｅｌｏｄｙｎｅ ＶＬＰ－１６の３Ｄ ＬｉＤＡＲを用いることができるが、これに限定されるものではない。

さらに、センサ部１１０にカメラなどをさらに含むことができ、カメラを用いて移動体１００の移動に対応する画像を撮影することもできる。この場合、カメラから移動体１００のビジュアルオドメトリを生成することができる。

一方、センサ部１１０が生成するセンシングデータは、単項データ（ｕｎａｒｙ ｄａｔａ）と２項データ（ｂｉｎａｒｙ ｄａｔａ）に区別することができる。ここで、単項データは、センシングデータの生成時点における測定結果を示すものであり、ＧＰＳセンサ１１２により測定された座標値などが該当する。すなわち、ＧＰＳセンサ１１２によれば、特定時点に移動体１００の位置に対応する緯度および経度座標値が生成されるため、単項データに該当する。

その反面、２項データは、センシングデータの測定開始時点と測定終了時点との間の関係情報を測定結果として示すものであり、ＩＭＵセンサ１１１のＩＭＵオドメトリなどが該当する。すなわち、特定時点における加速度値ではなく、測定開始時点と測定終了時点との間の加速度変化量を用いた移動方向と移動距離などの情報を抽出するものであるため、ＩＭＵオドメトリは２項データに該当する。

センサ部１１０により生成されたセンシングデータはキュー（Ｑｕｅｕｅ）に入力され、入力されたセンシングデータはグラフ構造構築部１２０に順次入力されることができる。但し、センサ部１１０に含まれるセンサの生成周期は互いに異なりうるし、センシングデータの生成後にキューに入力される時点も各々互いに異なりうる。

グラフ構造構築部１２０は、基準周期に応じてノード（ｎｏｄｅ）を生成することができ、基準周期と同期化されたセンシングデータにノード間の拘束条件（ｃｏｎｓｔｒａｉｎｔ）を設定して、グラフ構造（ｇｒａｐｈ ｓｔｒｕｃｔｕｒｅ）を生成することができる。ここで、グラフ構造構築部１２０は、センシングデータのうちいずれか一つを基準データに設定することができ、基準データの生成周期を基準周期に設定することができる。すなわち、基準データの生成周期に応じて、ノードを生成してグラフ構造を生成することができる。この時、基準周期と同期化されたセンシングデータが共に入力され、入力されたセンシングデータを該ノードの拘束条件に設定することができる。実施形態によっては、ＩＭＵセンサ１１１のＩＭＵオドメトリ（Ｏｄｏｍｅｔｒｙ）を基準データに設定することができる。

具体的には、図２（ａ）に示すように、グラフ構造構築部１２０は、ｔ＝０の時点に最初に第１ノードを生成することができ、第１ノードの初期条件（ｉ１）を拘束条件に設定することができる。その後、ｔ＝５の時点に第２ノードを生成することができ、第１ノードと第２ノードとの間を連結する拘束条件（ａ１）を設定することができる。すなわち、基準周期は５であり、ｔ＝０とｔ＝５との間のＩＭＵオドメトリを拘束条件（ａ１）に設定することができる。このように、基準周期ごとに移動体１００の位置に対応するノードを新規生成し、各々のノードにセンシングデータに対応する拘束条件を設定して、移動体１００のグラフ構造を生成することができる。

グラフ構造修正部１３０は、センシングデータのうち、基準周期と同期化されていない非同期センシングデータが入力されれば、非同期センシングデータを用いてグラフ構造を修正することができる。

図１に示すように、センサ部１１０により生成されたセンシングデータは、キュー（Ｑｕｅｕｅ）を通じてグラフ構造構築部１２０に順次入力されることができる。この時、各々のセンシングデータは生成された順に応じて順次入力されることができるが、実施形態によっては、センサがセンシングデータを生成する周期が一致しなくてもよい。また、各々のセンサごとにセンシングデータを生成するのにかかる時間が互いに異なり、同一の時点に対するセンシングデータにもかかわらず、相対的に遅くキューに入力される場合も発生しうる。

つまり、センサ部１１０のセンサが生成するセンシングデータは各々の生成周期が互いに異なりうるし、ノードの生成周期と同期化されていない非同期センシングデータが入力されることができる。この時、単に非同期センシングデータを除外する場合には、位置推定などの正確度が問題になるため、非同期センシングデータをグラフ構造に整合するようにグラフ構造を修正する必要がある。したがって、グラフ構造修正部１３０は、グラフ構造を修正して非同期センシングデータをグラフ構造内に整合させることができる。

具体的には、グラフ構造修正部１３０は、先ず受信した非同期センシングデータの生成時点を確認することができる。すなわち、非同期センシングデータの生成時点とグラフ構造の中で最近生成された第１ノードの生成時点とを比較することができる。

ここで、非同期センシングデータが第１ノードの生成時点以後に生成された場合には、非同期センシングデータの生成時点に対応する第２ノードを新規生成してグラフ構造に追加することができる。すなわち、基準周期が到来する前に非同期センシングデータが先に入力された場合に該当するため、該非同期センシングデータの生成時点に対応するノードを追加して、非同期センシングデータをグラフ構造上に反映することができる。この場合、第２ノードに非同期センシングデータに対応する第１拘束条件を設定し、第１ノードと第２ノードとの間の第２拘束条件は基準データを用いて設定することができる。

具体的には、図４（ａ）に示すように、既存のグラフ構造は、ｔ＝０とｔ＝５に各々ノードを生成した後、初期条件（ｉ１）をｔ＝０の拘束条件に設定し、ｔ＝０とｔ＝５のノードの間にＩＭＵオドメトリに応じた拘束条件（ａ１）を設定したものである。ここで、ｔ＝５までノードが生成されたため、その後基準周期に応じてｔ＝１０にノードが生成される予定であるが、現在ｔ＝７の時点に非同期センシングデータが入力された場合に該当する。

この場合、図４（ｂ）に示すように、ｔ＝７は現在グラフ構造と比較すると未来データに該当するため、ｔ＝７に該当するノードを追加することができる。その後、最も近接するｔ＝５のノードと基準データを用いて拘束条件（ａ２）を設定することができ、その後ｔ＝７に対応する非同期センシングデータを適用してｔ＝７における拘束条件（ｕ１）を設定することができる。すなわち、グラフ構造を基準に、未だ生成されていない未来データが入力される場合には、該非同期センシングデータに対応するノードを追加するようにグラフ構造を修正することができる。

一方、グラフ構造において直近に生成された第１ノードの生成時点を基準に、非同期センシングデータが第１ノードの生成時点以前に生成された場合には、グラフノード補間法（Ｇｒａｐｈ ｎｏｄｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ）またはセンサデータ補間法（Ｓｅｎｓｏｒ ｄａｔａ ｉｎｔｅｒｐｏｌａｔｉｏｎ）を利用してグラフ構造を修正することができる。

具体的には、非同期センシングデータが単項データ（ｕｎａｒｙ ｄａｔａ）であり、非同期センシングデータの生成時点が、グラフ構造内に含まれる全体ノードの生成時点と設定時間間隔以上の差がある場合には、グラフノード補間法を利用することができる。すなわち、非同期センシングデータの生成時点が、グラフ構造内に含まれるノードの生成時点とは近接しない場合には、グラフ構造内に追加のノードをさらに含む方式で非同期センシングデータを整合させることができる。

グラフノード補間法を利用する場合には、グラフ構造修正部１３０が非同期センシングデータの生成時点に第２ノードを新規生成することができ、第２ノードに非同期センシングデータに対応する第１拘束条件を設定することができる。また、第２ノードと既存のノードとの間には、連携要素（ｂｅｔｗｅｅｎ ｆａｃｔｏｒ）を用いて第２拘束条件を設定することができる。

具体的には、図２に示すように、ｔ＝０とｔ＝５に対応するノードを各々生成した後に、ｔ＝４の時点に対応する非同期センシングデータが入力される場合がありうる。ここで、グラフ構造は既にｔ＝５の時点まで生成された状態であるため、ｔ＝４の時点に対応する非同期センシングデータを反映するためにはグラフ構造を修正する必要がある。ここで、非同期センシングデータはｔ＝４の時点に生成されたものであるため、既存のグラフ構造に含まれるノードと設定時間間隔以上（例えば、ｔ＝１）の差がある。したがって、図２の場合は、グラフノード補間法を適用することができる。

この場合、図２（ｂ）に示すように、非同期センシングデータが生成されたｔ＝４に対応するノードを新規生成することができ、ｔ＝４に対応するノードに非同期センシングデータに対応する拘束条件（ｕ１）を設定することができる。ここで、ｔ＝４のノードと、ｔ＝５のノードとの間を連結する拘束条件を追加する必要があり、このために追加的に連携要素を設定して拘束条件（ｂ１）を設定することができる。例えば、ｔ＝４に生成された非同期センシングデータがＧＰＳ座標値である場合、ｔ＝４のＧＰＳ座標値と、ｔ＝５における位置情報を比較した後、ｔ＝４のＧＰＳ座標からｔ＝５の位置まで進行するためのホイールエンコーダ値などに設定することができる。但し、連携要素は、これに限定されず、実施形態に応じて多様に設定可能である。

また、非同期センシングデータが単項データであり、グラフ構造内に含まれる全体ノードのうち、非同期センシングデータの生成時点と設定時間間隔（例えば、ｔ＝１）未満である対象ノードが存在する場合には、センサデータ補間法を利用することができる。すなわち、図３（ａ）に示すように、非同期センシングデータの生成時点がｔ＝４．９であり、既存のグラフ構造内に生成時点がｔ＝５である対象ノードが存在する場合には、設定時間間隔未満である対象ノードが存在する場合に該当する。したがって、図３（ｂ）に示すように、非同期センシングデータに対応するノードを別途に生成せず、既存のグラフ構造に含まれる対象ノード（ｔ＝５）に非同期センシングデータに対応する拘束条件（ｕ１）を追加するようにグラフ構造を修正することができる。

つまり、グラフ構造修正部１３０は、非同期センシングデータの生成時点が既存のグラフ構造のノードと設定時間間隔未満の差である場合には、センサデータ補間法を適用して、速かに非同期センシングデータをグラフ構造上に反映するようにすることができ、設定時間間隔以上の差がある場合には、グラフノード補間法を適用して、非同期センシングデータを正確にグラフ構造上に反映するようにすることができる。したがって、本発明の一実施形態によれば、非同期センシングデータをグラフ構造上に速かに反映すると共に正確に反映することができる。

さらに、グラフ構造修正部１３０は、入力される非同期センシングデータが２項データである場合にも、グラフ構造を修正して反映するようにすることができる。具体的には、非同期センシングデータが２項データである場合には、グラフノード補間法を利用することができる。

すなわち、非同期センシングデータの測定開始時点および測定終了時点が各々第１ノードの生成時点以前であれば、非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の第１拘束条件を設定することができる。また、第２ノードおよび第３ノードに最も隣接する対象ノードとの間には、各々連携要素を用いて第２拘束条件および第３拘束条件を設定することができる。

例えば、図５に示すように、ｔ＝０、ｔ＝５、ｔ＝１０の時点に各々ノードを生成してグラフ構造を構築した状態で、測定開始時点がｔ＝４であり、測定終了時点がｔ＝７である２項データが入力されることができる。この場合、ｔ＝４とｔ＝７に対応するノードを各々生成することができ、非同期センシングデータを用いて各ノードを連結する拘束条件（ｖ１）を設定することができる。また、ｔ＝４とｔ＝７と隣接するｔ＝５のノードの間に各々連携要素を用いて拘束条件（ｂ１、ｂ２）を設定することができる。ここで、連携要素は、実施形態に応じて多様に設定可能である。

また、実施形態によっては、非同期センシングデータの測定開始時点が第１ノードの生成時点以前であり、非同期センシングデータの測定終了時点が第１ノードの生成時点以後である場合もありうる。この時、非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の第１拘束条件を設定することができる。ここで、第２ノードと第１ノードとの間の第１拘束条件は連携要素を用いて設定し、第３ノードと第１ノードとの間の第２拘束条件は基準データを用いて設定することができる。

すなわち、図６に示すように、ｔ＝０、ｔ＝５の時点に各々ノードを生成してグラフ構造を構築した状態で、測定開始時点がｔ＝４であり、測定終了時点がｔ＝７である２項データが入力されることができる。この場合、ｔ＝４とｔ＝７に対応するノードを各々生成することができ、非同期センシングデータを用いて各ノードを連結する拘束条件（ｖ１）を設定することができる。また、ｔ＝４とｔ＝７と隣接するｔ＝５のノードに対して、各々連携要素と基準データを用いて拘束条件（ｂ１、ａ２）を設定することができる。

さらに、図７に示すように、非同期センシングデータの測定開始時点（ｔ＝７）と測定終了時点（ｔ＝１３）が第１ノードの生成時点（ｔ＝５）以後である場合もありうる。この時、グラフ構造修正部１３０は、非同期センシングデータの測定開始時点（ｔ＝７）と測定終了時点（ｔ＝１３）に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の拘束条件（ｖ１）を設定することができる。その後、第２ノード（ｔ＝７）と第１ノード（ｔ＝５）との間の拘束条件（ａ２）と、第２ノード（ｔ＝７）と第３ノード（ｔ＝１３）との間の拘束条件（ａ３）は各々基準データを用いて設定することができる。

位置推定部１４０は、グラフ構造を用いて移動体１００の位置情報を生成することができる。具体的には、位置推定部１４０は、構築されるグラフ構造にグラフ最適化技法（Ｇｒａｐｈ ｏｐｔｉｍｉｚａｔｉｏｎ）を適用することができ、最適化されたグラフ構造を用いて、移動体１００の移動経路および時間に応じた最終位置を推定することができる。すなわち、位置推定部１４０は、グラフ構造から、移動体１００の回転情報、移動情報および速度情報などを含む位置情報を最終的に抽出することができる。その後、位置推定部１４０から抽出した位置情報に基づいて、自律走行のための移動体１００の制御を行うようにすることができる。

図８は、本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法を示すフローチャートである。

図８を参照すれば、本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法は、センシングステップ（Ｓ１１０）、グラフ構造の生成ステップ（Ｓ１２０）、グラフ構造の修正ステップ（Ｓ１３０）および位置推定ステップ（Ｓ１４０）を含むことができる。ここで、各ステップは、移動体によって行われる。

以下では、図８を参照して、本発明の一実施形態によるグラフ構造ベースの移動体の位置推定方法について説明する。

センシングステップ（Ｓ１１０）では、複数のセンサを用いて移動体の移動に応じたセンシングデータを生成することができる。具体的には、移動体に取り付けられるＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）センサ、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）センサ、ホイールエンコーダ（Ｗｈｅｅｌ Ｅｎｃｏｄｅｒ）センサ、ライダー（ＬｉＤＡＲ）センサなどを用いて、移動体の移動に応じたセンシングデータを生成することができる。但し、移動体に含まれるセンサは、これらに限定されず、この他にも種々のセンサを用いてセンシングデータを生成することができる。

一方、センサが生成するセンシングデータは、単項データ（ｕｎａｒｙ ｄａｔａ）と２項データ（ｂｉｎａｒｙ ｄａｔａ）に区別することができる。すなわち、センシングデータの生成時点における測定結果を示す単項データと、センシングデータの測定開始時点と測定終了時点との間の関係情報を測定結果として示す２項データに各々区別することができる。

グラフ構造の生成ステップ（Ｓ１２０）では、基準周期に応じてノード（ｎｏｄｅ）を生成し、基準周期と同期化されたセンシングデータにノード間の拘束条件（ｃｏｎｓｔｒａｉｎｔ）を設定してグラフ構造（ｇｒａｐｈ ｓｔｒｕｃｔｕｒｅ）を生成することができる。ここで、センシングデータのうちいずれか一つを基準データに設定することができ、基準データの生成周期を基準周期に設定することができる。すなわち、基準データの生成周期に応じて、ノードを生成してグラフ構造を生成することができる。この時、基準周期と同期化された他のセンシングデータが共に入力され、入力されたセンシングデータは該ノードの拘束条件に設定することができる。実施形態によっては、ＩＭＵセンサのＩＭＵオドメトリ（Ｏｄｏｍｅｔｒｙ）を基準データに設定することができる。

グラフ構造の修正ステップ（Ｓ１３０）では、センシングデータのうち、基準周期と同期化されていない非同期センシングデータが入力されれば、非同期センシングデータを用いてグラフ構造を修正することができる。

各々のセンサが生成したセンシングデータは、キュー（Ｑｕｅｕｅ）を通じて順次入力されることができる。この時、各々のセンシングデータは生成された順に応じて順次入力されることができるが、実施形態によっては、センサがセンシングデータを生成する周期が一致しなくてもよい。また、各々のセンサごとにセンシングデータを生成するのにかかる時間が互いに異なり、同一の時点に対するセンシングデータにもかかわらず、相対的に遅くキューに入力される場合も発生しうる。

つまり、センシングデータは各々の生成周期が互いに異なりうるし、ノードの生成周期と同期化されていない非同期センシングデータが入力されることができる。この時、単に非同期センシングデータを除外する場合には、位置推定などの正確度が問題になるため、非同期センシングデータをグラフ構造に整合するようにグラフ構造を修正する必要がある。したがって、グラフ構造の修正ステップ（Ｓ１３０）では、グラフ構造を修正して非同期センシングデータをグラフ構造内に整合させることができる。

この場合、先ず受信した非同期センシングデータの生成時点を確認することができる。すなわち、非同期センシングデータの生成時点とグラフ構造の中で最近生成された第１ノードの生成時点とを比較することができる。

ここで、非同期センシングデータが第１ノードの生成時点以後に生成された場合には、非同期センシングデータの生成時点に対応する第２ノードを新規生成してグラフ構造に追加することができる。すなわち、基準周期が到来する前に非同期センシングデータが先に入力された場合に該当するため、該非同期センシングデータの生成時点に対応するノードを追加して、非同期センシングデータをグラフ構造上に反映することができる。この場合、第２ノードに非同期センシングデータに対応する第１拘束条件を設定し、第１ノードと第２ノードとの間の第２拘束条件は基準データを用いて設定することができる。すなわち、グラフ構造を基準に、未だ生成されていない未来データが入力される場合には、該非同期センシングデータに対応するノードを追加するようにグラフ構造を修正することができる。

一方、グラフ構造において直近に生成された第１ノードの生成時点を基準に、非同期センシングデータが第１ノードの生成時点以前に生成された場合には、グラフノード補間法（Ｇｒａｐｈ ｎｏｄｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ）またはセンサデータ補間法（Ｓｅｎｓｏｒ ｄａｔａ ｉｎｔｅｒｐｏｌａｔｉｏｎ）を利用してグラフ構造を修正することができる。

具体的には、非同期センシングデータが単項データ（ｕｎａｒｙ ｄａｔａ）であり、非同期センシングデータの生成時点が、グラフ構造内に含まれる全体ノードの生成時点と設定時間間隔以上の差がある場合には、グラフノード補間法を利用することができる。

グラフノード補間法を利用する場合には、非同期センシングデータの生成時点に第２ノードを新規生成することができ、第２ノードに非同期センシングデータに対応する第１拘束条件を設定することができる。また、第２ノードと既存のノードとの間には連携要素（ｂｅｔｗｅｅｎ ｆａｃｔｏｒ）を用いて第２拘束条件を設定することができる。

また、非同期センシングデータが単項データであり、グラフ構造内に含まれる全体ノードのうち、非同期センシングデータの生成時点と設定時間間隔（例えば、ｔ＝１）未満である対象ノードが存在する場合には、センサデータ補間法を利用することができる。すなわち、非同期センシングデータに対応するノードを別途に生成せず、既存のグラフ構造に含まれる対象ノードに非同期センシングデータに対応する拘束条件を追加するようにグラフ構造を修正することができる。

さらに、入力される非同期センシングデータが２項データである場合にも、グラフ構造を修正して反映するようにすることができる。具体的には、非同期センシングデータが２項データである場合には、グラフノード補間法を利用することができる。

すなわち、非同期センシングデータの測定開始時点および測定終了時点が各々第１ノードの生成時点以前であれば、非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の第１拘束条件を設定することができる。また、第２ノードおよび第３ノードに最も隣接する対象ノードとの間には、各々連携要素を用いて第２拘束条件および第３拘束条件を設定することができる。

また、実施形態によっては、非同期センシングデータの測定開始時点が第１ノードの生成時点以前であり、非同期センシングデータの測定終了時点が第１ノードの生成時点以後である場合もありうる。この時、非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の第１拘束条件を設定することができる。ここで、第２ノードと第１ノードとの間の第１拘束条件は連携要素を用いて設定し、第３ノードと第１ノードとの間の第２拘束条件は基準データを用いて設定することができる。

この他にも、非同期センシングデータの測定開始時点と測定終了時点が第１ノードの生成時点以後である場合もありうる。この場合、非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、非同期センシングデータを用いて第２ノードおよび第３ノードの間の拘束条件を設定することができる。その後、第２ノードと第１ノードとの間の拘束条件と、第２ノードと第３ノードとの間の拘束条件を、各々基準データを用いて設定することができる。

位置推定ステップ（Ｓ１４０）では、グラフ構造を用いて、移動体の位置情報を生成することができる。具体的には、構築されるグラフ構造にグラフ最適化技法（Ｇｒａｐｈ ｏｐｔｉｍｉｚａｔｉｏｎ）を適用することができ、最適化されたグラフ構造を用いて、移動体の移動経路および時間に応じた最終位置を推定することができる。すなわち、修正されたグラフ構造から、移動体の回転情報、移動情報および速度情報などを含む位置情報を最終的に抽出することができる。その後、抽出した位置情報に基づいて、自律走行のための移動体の制御を行うようにすることができる。

前述した本発明は、プログラムが記録された媒体にコンピュータ読み取り可能なコードとして実現することができる。コンピュータ読み取り可能な媒体は、コンピュータで実行可能なプログラムを持続的に格納するか、または実行またはダウンロードのために臨時格納するものであってもよい。また、媒体は単一または数個のハードウェアが結合された形態の様々な記録手段または格納手段であってもよく、或るコンピュータ・システムに直接接続される媒体に限定されず、ネットワーク上に分散存在するものであってもよい。媒体の例示としては、ハードディスク、フロッピーディスクおよび磁気テープのような磁気媒体、ＣＤ－ＲＯＭおよびＤＶＤのような光記録媒体、フロプティカルディスク（ｆｌｏｐｔｉｃａｌ ｄｉｓｋ）のような磁気－光媒体（ｍａｇｎｅｔｏ－ｏｐｔｉｃａｌ ｍｅｄｉｕｍ）、およびＲＯＭ、ＲＡＭ、フラッシュメモリなどを含めてプログラム命令語が格納されるように構成されたものがある。また、他の媒体の例示として、アプリケーションを流通するアプリストアやその他の様々なソフトウェアを供給乃至流通するサイト、サーバなどが管理する記録媒体乃至ストレージ媒体も挙げられる。したがって、上記の詳細な説明は、全ての面で制限的に解釈されてはならず、例示的なものに考慮されなければならない。本発明の範囲は添付された請求項の合理的な解釈によって決定されなければならず、本発明の等価的な範囲内の全ての変更は本発明の範囲に含まれる。

本発明は、前述した実施形態および添付された図面によって限定されるものではない。本発明が属する技術分野における通常の知識を有した者であれば、本発明の技術的思想を逸脱しない範囲内で本発明に係る構成要素を置換、変形および変更できることは明らかである。

１００ ・・・移動体
１１０ ・・・センサ部
１１１ ・・・ＩＭＵセンサ
１１２ ・・・ＧＰＳセンサ
１１３ ・・・ホイールエンコーダセンサ
１１４ ・・・ＬｉＤＡＲセンサ
１２０ ・・・グラフ構造構築部
１３０ ・・・グラフ構造修正部
１４０ ・・・位置推定部
Ｓ１１０ ・・・センシングステップ
Ｓ１２０ ・・・グラフ構造の生成ステップ
Ｓ１３０ ・・・グラフ構造の修正ステップ
Ｓ１４０ ・・・位置推定ステップ

Claims (18)

1. 複数のセンサを用いて、移動体の移動に応じたセンシングデータを生成するセンシングステップ、
   基準周期に応じてノードを生成し、前記基準周期と同期化されたセンシングデータを用いて前記ノード間の拘束条件を設定してグラフ構造を生成する、グラフ構造の生成ステップ、
   前記センシングデータとして、前記基準周期と同期化されていない非同期センシングデータが入力されると、前記非同期センシングデータを用いて前記グラフ構造を修正する、グラフ構造の修正ステップ、および
   前記ノード間の拘束条件が示す位置情報を、前記グラフ構造における時間順に統合することにより、前記移動体の位置情報を生成する位置推定ステップ
   を含み、前記グラフ構造の修正ステップは、
   前記非同期センシングデータの生成時点と、前記グラフ構造に含まれるノードの生成時点との間の差が、（１）設定時間間隔以上である場合には、グラフノード補間法を利用することにより、（２）前記設定時間間隔未満である場合には、センサデータ補間法を利用することにより、前記グラフ構造を修正する、グラフ構造ベースの移動体の位置推定方法。
2. 前記グラフ構造の修正ステップは、
   前記グラフ構造の中で最近生成された第１ノードの生成時点を基準に、前記非同期センシングデータが前記第１ノードの生成時点以後に生成されたものであれば、前記非同期センシングデータの生成時点に対応する第２ノードを新規生成して前記グラフ構造に追加することを特徴とする、請求項１に記載のグラフ構造ベースの移動体の位置推定方法。
3. 前記グラフ構造の修正ステップは、
   前記非同期センシングデータが２項データであれば、グラフノード補間法を利用して、前記グラフ構造を修正することを特徴とする、請求項２に記載のグラフ構造ベースの移動体の位置推定方法。
4. 前記グラフ構造の修正ステップは、
   前記非同期センシングデータの測定開始時点および測定終了時点が各々前記第１ノードの生成時点以前であれば、前記非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、前記非同期センシングデータを用いて前記第２ノードおよび第３ノードの間の第１拘束条件を設定し、前記第２ノードおよび第３ノードに最も隣接する対象ノードとの間に連携要素を用いて各々第２拘束条件および第３拘束条件を設定することを特徴とする、請求項３に記載のグラフ構造ベースの移動体の位置推定方法。
5. 前記グラフ構造の修正ステップは、
   前記非同期センシングデータの測定開始時点が前記第１ノードの生成時点以前であり、前記非同期センシングデータの測定終了時点が前記第１ノードの生成時点以後であれば、前記非同期センシングデータの測定開始時点と測定終了時点に各々第２ノードおよび第３ノードを生成し、前記非同期センシングデータを用いて前記第２ノードおよび第３ノードの間の第１拘束条件を設定し、前記第２ノードと前記第１ノードとの間の第１拘束条件を、連携要素を用いて設定し、前記第３ノードと第１ノードとの間の第２拘束条件を、基準データを用いて設定することを特徴とする、請求項３または４に記載のグラフ構造ベースの移動体の位置推定方法。
6. 前記グラフ構造の修正ステップは、
   前記第２ノードに前記非同期センシングデータに対応する第１拘束条件を設定し、基準データを用いて前記第１ノードと前記第２ノードとの間に第２拘束条件を設定することを特徴とする、請求項２に記載のグラフ構造ベースの移動体の位置推定方法。
7. 前記グラフ構造の修正ステップは、
   前記グラフ構造の中で最近生成された第１ノードの生成時点を基準に、前記非同期センシングデータが前記第１ノードの生成時点以前に生成されたものであれば、グラフノード補間法またはセンサデータ補間法を利用して、前記グラフ構造を修正することを特徴とする、請求項１または２に記載のグラフ構造ベースの移動体の位置推定方法。
8. 前記グラフ構造の修正ステップは、
   前記非同期センシングデータが単項データであり、前記非同期センシングデータの生成時点が、前記グラフ構造に含まれるノードの生成時点と設定時間間隔以上の差があれば、前記グラフノード補間法を利用して、ノードを追加することにより前記グラフ構造を修正することを特徴とする、請求項７に記載のグラフ構造ベースの移動体の位置推定方法。
9. 前記グラフ構造の修正ステップは、
   前記グラフノード補間法を利用して、前記非同期センシングデータの生成時点に第２ノードを新規生成し、前記第２ノードに前記非同期センシングデータに対応する第１拘束条件を設定し、前記第２ノードと既存のノードとの間に連携要素を用いて第２拘束条件を設定することを特徴とする、請求項８に記載のグラフ構造ベースの移動体の位置推定方法。
10. 前記グラフ構造の修正ステップは、
    前記非同期センシングデータが単項データであり、前記グラフ構造に含まれるノードのうち、前記非同期センシングデータの生成時点と設定時間間隔未満の対象ノードが存在すれば、前記センサデータ補間法を利用して、前記グラフ構造を修正することを特徴とする、請求項７に記載のグラフ構造ベースの移動体の位置推定方法。
11. 前記グラフ構造の修正ステップは、
    前記対象ノードに前記非同期センシングデータに対応する拘束条件をさらに設定することを特徴とする、請求項１０に記載のグラフ構造ベースの移動体の位置推定方法。
12. 前記センシングステップは、
    前記移動体に取り付けられるＩＭＵセンサ、ＧＰＳセンサ、ホイールエンコーダセンサ、ライダーセンサのうち少なくともいずれか一つを用いて、前記移動体の移動に応じたセンシングデータを生成することを特徴とする、請求項１に記載のグラフ構造ベースの移動体の位置推定方法。
13. 前記センシングデータは、
    前記センシングデータの生成時点における測定結果を示す単項データ、または前記センシングデータの測定開始時点と測定終了時点との間の関係情報を測定結果として示す２項データであることを特徴とする、請求項１に記載のグラフ構造ベースの移動体の位置推定方法。
14. 前記グラフ構造の生成ステップは、
    前記センシングデータのうちいずれか一つを基準データに設定し、前記基準データの生成周期を前記基準周期に設定することを特徴とする、請求項１に記載のグラフ構造ベースの移動体の位置推定方法。
15. 前記グラフ構造の生成ステップは、
    前記移動体に取り付けられるＩＭＵセンサにより測定された加速度と角速度の変化量から導出される前記移動体の移動方向および移動距離を前記基準データに設定することを特徴とする、請求項１４に記載のグラフ構造ベースの移動体の位置推定方法。
16. 前記位置推定ステップは、
    前記グラフ構造にグラフ最適化技法を適用して前記移動体の位置を推定することを特徴とする、請求項１に記載のグラフ構造ベースの移動体の位置推定方法。
17. ハードウェアと結合されて、請求項１～１６のいずれか１項に記載のグラフ構造ベースの移動体の位置推定方法を実行するためのコンピュータプログラム。
18. 複数のセンサを用いて、移動体の移動に応じた各々のセンシングデータを生成するセンサ部、
    基準周期に応じてノードを生成し、前記基準周期と同期化されたセンシングデータを用いて前記ノード間の拘束条件を設定してグラフ構造を生成するグラフ構造構築部、
    前記センシングデータとして、前記基準周期と同期化されていない非同期センシングデータが入力されると、前記非同期センシングデータを用いて前記グラフ構造を修正するグラフ構造修正部、および
    前記ノード間の拘束条件が示す位置情報を、前記グラフ構造における時間順に統合することにより、前記移動体の位置情報を生成する位置推定部
    を含み、前記グラフ構造修正部は、
    前記非同期センシングデータの生成時点と、前記グラフ構造に含まれるノードの生成時点との間の差が、（１）設定時間間隔以上である場合には、グラフノード補間法を利用することにより、（２）前記設定時間間隔未満である場合には、センサデータ補間法を利用することにより、前記グラフ構造を修正する、移動体。

Images