特許 7365958 測位システム、方法及びプログラム

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-10-12

(45)【発行日】2023-10-20

(54)【発明の名称】測位システム、方法及びプログラム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20231013BHJP

   G01S 17/86 20200101ALI20231013BHJP

   G01B 11/00 20060101ALI20231013BHJP

【ＦＩ】

G01C21/28

G01S17/86

G01B11/00 H

G01B11/00 Z

【請求項の数】 10

(21)【出願番号】P 2020074108

(22)【出願日】2020-04-17

(65)【公開番号】P2021169990

(43)【公開日】2021-10-28

【審査請求日】2022-06-07

(73)【特許権者】

【識別番号】000208891

【氏名又は名称】ＫＤＤＩ株式会社

(74)【代理人】

【識別番号】100092772

【弁理士】

【氏名又は名称】阪本 清孝

(74)【代理人】

【識別番号】100119688

【弁理士】

【氏名又は名称】田邉 壽二

(72)【発明者】

【氏名】小森田 賢史

(72)【発明者】

【氏名】田坂 和之

【審査官】白石 剛史

(56)【参考文献】

【文献】国際公開第２０１９／２０２８０６（ＷＯ，Ａ１）

【文献】特開２０１４－２２８４９５（ＪＰ，Ａ）

【文献】特開２０１６－０１７７９６（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／２８

Ｇ０１Ｓ １７／８６

Ｇ０１Ｂ １１／００

(57)【特許請求の範囲】

【請求項1】

ローカル座標系において、測位誤差が累積する第１測位手法により対象の測位を行うことで、位置及び向きの時系列データとして第１測位情報を取得する第１測位部と、
グローバル座標系において、測位誤差が累積しない第２測位手法により前記対象の測位を行うことで、位置及び向きの時系列データとして第２測位情報を取得する第２測位部と、
補完部と、を備え、
前記補完部は、
前記第１測位情報における向きの時系列データと前記第２測位情報における向きの時系列データとの差分である差分回転成分時系列データに対して外れ値を許容した第１回帰直線を計算し、
前記差分回転成分時系列データのうち現時刻に近い側のデータに、前記第１回帰直線からの外れ値に該当するデータの個数が閾値以上連続して存在するか否かを判定し、
前記存在すると判定された場合は、前記閾値以上連続して存在するデータに対して第２回帰直線を計算して当該第２回帰直線上での現時刻データを現時刻差分回転として定め、
前記存在しないと判定された場合は、前記第１回帰直線上での現時刻のデータを現時刻差分回転として定め、
前記第１測位部で取得された現時刻の向きに対して前記現時刻差分回転を適用したものを、前記第２測位部で取得された現時刻の前記グローバル座標系での向きを補完した結果として得ることを特徴とする測位システム。

【請求項2】

前記第１測位情報における時系列データとしての第１誤差は第１挙動を示し、当該第１挙動は、誤差の値がゼロから累積し続ける第１１挙動と、当該第１１挙動の途中で突発的に発生する、累積し続けている誤差の値が急激に変化する第１２挙動と、を含むものであり、
前記第２測位情報における時系列データとしての第２誤差は第２挙動を示し、当該第２挙動は、誤差の値がゼロの周辺で揺らぎ続ける第２１挙動と、当該第２１挙動の途中で突発的に発生する、誤差の値が揺らぎの範囲から逸脱した外れ値を示す第２２挙動と、を含むものであることを特徴とする請求項１に記載の測位システム。

【請求項3】

前記ローカル座標系はグローバルな座標情報を有しない座標系であり、前記グローバル座標系はグローバルな座標情報を有する座標系であることを特徴とする請求項１または２に記載の測位システム。

【請求項4】

前記補完部では、RANSAC（ランダムサンプルコンセンサス）を用いることにより、前記外れ値を許容した第１回帰直線を計算することを特徴とする請求項１ないし３のいずれかに記載の測位システム。

【請求項5】

前記補完部では、前記差分回転成分時系列データのうち現時刻に近い側のデータに、前記第１回帰直線からの外れ値に該当するデータの個数が閾値以上連続して存在すると判定された場合に、当該外れ値に該当する現時刻に近い側のデータを計算対象集合として設定し、
前記補完部では、前記差分回転成分時系列データのうち現時刻に近い側のデータに、前記第１回帰直線からの外れ値に該当するデータの個数が閾値以上連続して存在しないと判定された場合に、前記差分回転成分時系列データの中から、前記第１回帰直線からの外れ値に該当するデータを除外したデータを計算対象集合として設定し、
当該計算対象集合に属する要素に対応する時刻における前記第１測位情報の各時刻での前記ローカル座標系における位置（LP _k ）と、当該計算対象集合に属する要素に対応する時刻における前記第２測位情報の各時刻での前記グローバル座標系における位置（GP _k ）と、を少なくとも用いることにより、前記ローカル座標系における前記第１測位情報の位置座標の長さ尺度に対する、前記グローバル座標系における前記第２測位情報の位置座標の長さ尺度の比率を表すスケール（C）を推定することを特徴とする請求項１ないし４のいずれかに記載の測位システム。

【請求項6】

前記計算対象集合に属する要素に対応する時刻のデータで構成される、前記第１測位情報における位置の時系列データを各時刻で列挙した行列LP _k 及びこの行列の平均値の行列LP _ave 並びに前記第２測位情報における位置の時系列データを各時刻で列挙した行列GP _k 及びこの行列の平均値の行列GP _ave を用いて、以下の式(2)による特異値分解SVDを適用した結果として得られる行列Σの対角成分から、前記スケール（C）を推定することを特徴とする請求項５に記載の測位システム。

【数2】

【請求項7】

前記計算対象集合を集合Wとし、前記行列Σの対角成分で構成される配列Sを用いて以下の式(3)により前記スケール（C）を推定することを特徴とする請求項６に記載の測位システム。

【数3】

【請求項8】

前記補完部では、前記現時刻差分回転（RDiff(t)）と前記スケール（C）と前記平均値の行列LP _ave と前記平均値の行列GP _ave とを用いて、前記ローカル座標系（LPk）と前記グローバル座標系（GPk）との間の並進成分（T）を算出し、
前記スケール（C）、前記並進成分（T）及び前記現時刻差分回転（RDiff(t)）を用いて、前記ローカル座標系（LPk）と前記グローバル座標系（GPk）との間の射影行列（M(t)）を算出し、
前記第１測位部で取得された現時刻での前記第１測位情報における位置である（LPx,LPy,LPz）に対して前記射影行列（M(t)）を以下の式(6)による斉次座標表現での乗算の形で適用することで、前記第２測位部で取得された現時刻での前記第２測位情報における位置を補完したものである（GPx,GPy,GPz）を得ることを特徴とする請求項６または７に記載の測位システム。

【数4】

【請求項9】

ローカル座標系において、測位誤差が累積する第１測位手法により対象の測位を行うことで、位置及び向きの時系列データとして第１測位情報を取得する第１測位段階と、
グローバル座標系において、測位誤差が累積しない第２測位手法により前記対象の測位を行うことで、位置及び向きの時系列データとして第２測位情報を取得する第２測位段階と、
補完段階と、を備え、
前記補完段階は、
前記第１測位情報における向きの時系列データと前記第２測位情報における向きの時系列データとの差分である差分回転成分時系列データに対して外れ値を許容した第１回帰直線を計算し、
前記差分回転成分時系列データのうち現時刻に近い側のデータに、前記第１回帰直線からの外れ値に該当するデータの個数が閾値以上連続して存在するか否かを判定し、
前記存在すると判定された場合は、前記閾値以上連続して存在するデータに対して第２回帰直線を計算して当該第２回帰直線上での現時刻データを現時刻差分回転として定め、
前記存在しないと判定された場合は、前記第１回帰直線上での現時刻のデータを現時刻差分回転として定め、
前記第１測位段階で取得された現時刻の向きに対して前記現時刻差分回転を適用したものを、前記第２測位段階で取得された現時刻の前記グローバル座標系での向きを補完した結果として得ることを特徴とする測位方法。

【請求項10】

コンピュータを請求項１ないし８のいずれかに記載の測位システムとして機能させることを特徴とするプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は対象の測位を行う測位システム、方法及びプログラムに関する。

【背景技術】

【0002】

測位技術として様々な従来手法が存在し、測位精度の向上が図られている。

【0003】

特許文献１（「SLAMマップからのワイドエリア位置推定」）では、ローカルでSLAM（Simultaneous Localization and Mapping, 自己位置推定とマップ作成の同時実行）を開始する際に、その初期位置をグローバル座標を推定するサーバに問い合わせて決定する。特許文献２（「画像処理を用いたグローバル座標取得装置」）では、GPS（全地球測位システム）が利用できない悪条件下においても精度よくグローバル座標を測量すべく、GPSを用いたグローバル座標と、センサや画像で求めたローカル座標の変換式を計算することにより、グローバル座標が無い場合も当該座標を得られるようにしている。

【0004】

特許文献３（「移動体の位置推定装置・方法」）では、走行中の振動の影響やカメラキャリブレーション実施時の誤差の影響があっても移動体の測位精度を向上させるものとして、走行中のSLAM座標を補正している。特許文献４（「位置推定装置・方法」）では、移動体に搭載されるカメラで撮像した画像より移動体の位置を推定する際に、周辺環境条件の変化による位置推定精度の低下を抑制すべく、異なる環境条件に対応した複数の地図から算出される位置を補正計算に用いて精度向上させている。

【0005】

非特許文献１では、ノイズの乗った点群のペアから最もうまく変換する式を導き出し、全体平均として精度よく動きを推定している。非特許文献２では、GPSのグローバル座標とセンサによるローカル座標の誤差を補正している。

【先行技術文献】

【特許文献】

【0006】

【文献】特表2016-528476号公報

【文献】特開2005-091298号公報

【文献】特開2019-070983号公報

【文献】特開2018-028489号公報

【非特許文献】

【0007】

【文献】Sorkine-Hornung, Olga, and Michael Rabinovich. "Least-squares rigid motion using svd." Computing 1.1 (2017).

【文献】Accurate Global Localization Using Visual Odometry and Digital Maps on Urban Environments。: IEEE Transactions on Intelligent Transportation Systems ( Volume: 13 , Issue: 4 , Dec. 2012 )

【発明の概要】

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

【0008】

しかしながら、以上のような従来手法においては、２つ以上の測位手法の結果を相互に補正する等により精度を向上させる手法が採用されているが、何らかの制約が存在しており、必ずしも簡素に精度を向上させることができなかった。

【0009】

特許文献１は、端末側でSLAMを開始する際に、グローバル座標を参照することで、その対応付けを得ることができる。しかし、前提としてグローバル座標の推論システムが正しいこととしているが、実際にはノイズが大きく、一度ずれるとズレたままとなってしまう。特許文献２では、グローバル座標とローカル座標の変換式を用いて、グローバル座標が得られないときもローカル座標から変換して得られるようにしているが、あくまで１つのペアを使っており、これに誤差が含まれることを考慮しておらず計算結果が安定しない。

【0010】

特許文献３では、あくまでローカル座標内でのノイズを考慮して、精度を向上させる技術であって、グローバル座標は加味されていない。特許文献４では、グローバル位置推定の精度が低い場合に他のペアから補完計算を行うが、SLAMの誤差やグローバル位置推定の誤認を判定する方法が無い。また複数の地図を用いて最も精度が高いものを使うために、環境条件に応じた複数の地図を用意しておくという手間が発生する。

【0011】

非特許文献１などでは、ノイズの乗った点群のペアから最もうまく変換する式を導き出しているが、SLAM等の測位手法に固有の誤差を考慮しておらず、単純に全体平均としてよい結果を求める。しかし、SLAMにおける誤差の挙動のように、急にまとまってずれる場合など、正しい結果が得られない。非特許文献2では、GPSのグローバル座標とセンサによるローカル座標の誤差を補正しているが、車を想定し走行する道からずれないように補正するという手段をとっている。しかし車以外の移動体では、車の場合における走行可能な道に対応するものを定義して設定することが困難であるため、車以外の移動体の測位には不向きである。

【0012】

上記従来技術の課題に鑑み、本発明は、２つの測位手法を補完的に使用する際に、互いの測位座標の関係を正しく得て、簡素に測位精度を向上させることができる測位システム、方法及びプログラムを提供することを目的とする。

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

【0013】

上記目的を達成するため、本発明は測位システムであって、対象の測位をローカルに行うことで、位置及び向きの時系列データとして第１測位情報を取得する第１測位部と、前記対象の測位をグローバルに行うことで、位置及び向きの時系列データとして第２測位情報を取得する第２測位部と、前記第１測位情報は誤差として連続的な累積誤差と突発的な変動とを伴うものであり、前記第２測位情報は誤差として非連続の揺らぎと突発的な外れ値とを伴うものであることに基づき、前記第１測位情報及び前記第２測位情報の誤差影響を除外したものとして、同一測位結果としての前記第１測位情報と前記第２測位情報との対応関係を推定し、当該対応関係に基づいて前記対象の測位情報を得る補完部と、を備えることを特徴とする。また、当該測位システムに対応する方法及びプログラムであることを特徴とする。

【発明の効果】

【0014】

本発明によれば、同一の対象を測位した第１測位情報及び第２測位情報の互いに異なる誤差の挙動を踏まえることで、前記第１測位情報及び前記第２測位情報の誤差影響を除外したものとして、簡素に前記対象の第１測位情報と第２測位情報との対応関係を得ることができる。この対応関係を用いて、一方の測位情報からもう一方の測位情報を正しく補完することができる。

【図面の簡単な説明】

【0015】

【図1】一実施形態に係る測位システムの構成図である。

【図2】一実施形態に係る測位システムの機能ブロック図である。

【図3】一実施形態に係る測位システムによる測位の動作のフローチャートである。

【図4】第１測位情報及び第２測位情報の挙動を模式的に示す図である。

【図5】本実施形態による誤差低減の効果を説明するための模式例として、ステップS3での差分の回転成分時系列データの模式例を示す図である。

【図6】一般的なコンピュータ装置におけるハードウェア構成の例を示す図である。

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

【0016】

図１は、一実施形態に係る測位システムの構成図であり、測位システム10はインターネット等のネットワークNWを介して相互に通信可能とされる端末1及びサーバ2を備える。端末1は、スマートフォン等のモバイルデバイス又は車載装置等として構成することができる移動体であり、測位システム10において測位する対象となるものである。一実施形態では測位システム10には端末1のみが含まれサーバ2が含まれない構成により、端末1が単独で自身の測位を行うようにしてもよい。一実施形態では測位システム10は端末1及びサーバ2を含み、端末1の測位を行う際に必要となる処理の一部をサーバ2が担うようにしてもよい。

【0017】

図２は、一実施形態に係る測位システム10の機能ブロック図であり、測位システム10は、計測部11、第１測位部12、第１DB（データベース）13、撮影部21、第２測位部22、第２DB23及び補完部30を備える。補完部30はさらに差分計算部31、フィッティング部32、回転成分計算部33、スケール計算部34、並進成分計算部35及び変換部36を備える。

【0018】

図２に示される各機能部は、測位システム10の端末1又はサーバ2のいずれかに備わっていればよいが、測位システム10による測位の対象となる端末1の測位を可能とするために、端末1には少なくとも計測部11及び撮影部21が設置され、端末1の存在する環境においてそれぞれ計測及び撮影を行うようにすればよい。図２に示される各機能部のうち計測部11及び撮影部21以外のものは、端末1ではなくサーバ2に備わるものであってもよい。

【0019】

図３は、一実施形態に係る測位システム10による測位の動作のフローチャートである。以下、図３の各ステップを説明する。

【0020】

ステップS1では、リアルタイムの現時刻t(t=1,2,3…)において、第１測位部12が端末1の測位を第１測位方式によって行うことで第１測位情報P1(t)を取得し、且つ、同時刻tにおいて第２測位部22が端末1の測位を第２測位方式によって行うことで第２測位情報P2(t)を取得し、これら測位情報P1(t),P2(t)を補完部30へと出力して補完部30において記録してから、ステップS2へと進む。

【0021】

ステップS1にて第１測位部12は第１測位方式として例えばSLAMや、オドメーター、加速度・ジャイロセンサ等を用いることにより、端末1の位置及び向きをローカルなものとして取得して、第１測位情報P1(t)を得ることができる。SLAMの場合、計測部11は例えばLiDAR（レーザ画像検出と測距）センサとして構成し、このLiDARセンサの現時刻tでの計測結果を第１測位部12で参照して第１測位情報P1(t)を得るようにすればよい。また画像を用いたSLAMの場合、計測部11は例えばステレオカメラとして構成し、このステレオカメラの現時刻tでのステレオ画像を第１測位部12で参照して、点対応を求めて三角測量等により第１測位情報P1(t)を得るようにすればよい。オドメーターや加速度・ジャイロセンサの場合は、それらをセンサとして構成し、このセンサから得られる位置や角度の増減成分を累積計算して、第１測位情報P1(t)を得るようにすればよい。第１測位部12において第１測位情報P1(t)を求める際に必要となる情報（カメラパラメータや、リファレンスとなる特徴点等の情報）は第１DB13に保存しておき、この情報を参照して第１測位部12は第１測位情報P1(t)を算出するようにすればよい。

【0022】

ステップS1にて第２測位部22は第２測位方式として例えばVPS(Visual Positioning System)やマーカ検出等を用いることにより、端末1の位置及び向きをグローバルなものとして取得して、第２測位情報P2(t)を得ることができる。VPSの場合、撮影部21は通常の単眼カメラとして構成し、当該カメラよって現時刻tにおいて撮影された画像を第２測位部22で参照し、特徴点及び特徴量（例えばSIFT特徴などによる特徴点及び特徴量）を抽出し、第２DB23に予めグローバル座標（３次元世界座標）が紐付けられて保存されているリファレンスの特徴点及び特徴量との間で照合を行い、照合結果として、第２測位情報P2(t)を出力することができる。マーカ検出の場合、撮影部21は通常の単眼カメラとして構成し、当該カメラによって現時刻tにおいて撮影された画像を第2測位部22で参照し、画像内に撮影されているマーカ（拡張現実で用いる正方マーカ等）を検出することにより、第２DB23に予めマーカのグローバル座標（３次元世界座標）が紐付けられて保存されているリファレンス情報と照合し、第2測位情報P2(t)を得るようにすればよい。

【0023】

ステップS2では、現時刻tまでにおいて、ステップS1により記録され蓄積済みとなっている測位情報{P1(k),P2(k)}（k=t,t-1,t-2,…）が一定期間に渡って存在するか否かを判定し、一定期間分の記録が存在していればステップS3へと進み、存在していなければステップS1へと戻り、次のタイミングの現時刻t+1について同様に測位を継続する。

【0024】

ステップS2では、現時刻tまでの記録がしきい値のN個（例えばN=8）の時刻に関して蓄積済みであれば、（すなわち、N個の記録済みの測位情報{P1(k),P2(k)}（k=t,t-1,t-2,…,t-N+1）が存在していれば、）肯定判定を得てステップS3へ進み、そうではない場合、すなわち、現時刻tを含めN-1個以下の時刻についてのみ測位情報{P1(k),P2(k)}が得られている場合には否定判定を得てステップS1へ戻り、N個の蓄積が得られるまで記録を継続すればよい。

【0025】

ステップS2よりも後にあるステップS3からS10までの処理は、補完部30による処理であり、これらの処理によってN個の記録済みの第１及び第２の測位情報{P1(k),P2(k)}（k=t,t-1,t-2,…,t-N+1）を相互の誤差の挙動を踏まえて誤差が低減されるように統合した結果として、最終的にステップS10において、現時刻tの端末1の測位結果としての位置及び向きが出力されることとなる。

【0026】

ここで、ステップS3以降の補完部30による処理の詳細を説明する前に、本実施形態において第１、第２の測位情報を統合して端末1の測位結果（位置及び向き）を得る原理に関して概略的に説明する。図４は、当該原理を説明するための前提となる、第１測位情報及び第２測位情報の挙動を模式的に示す図である。

【0027】

図４にてグラフG1は第１測位情報を４つの連続する時刻について取得した結果のデータd1,d2,d3,d4を示しており、３次元xyz座標空間内において位置を白丸（○）で表現し、向きを当該白丸を始点とするベクトル矢印で表現することにより、位置及び向きで構成される第１測位情報の時間変化の挙動を模式的に示している。既に説明したように第１測位情報はローカルなものとして取得され、第２測位情報のようにグローバルな座標情報は有せず、第１測位情報の取得を開始した時点での位置及び姿勢を基準とした相対的な測位情報となる。グラフG1のデータd1,d2,d3,d4にも模式的に示されるように、第１測位情報は第２測位情報と比べてノイズ等による影響で非連続に揺らぐことは少ないが、誤差が連続的に累積し、また、SLAM等のロストにより、まれにではあるが座標系が急にずれる挙動を示すものである。（この急にずれる挙動は図４のグラフG1内では示されていない。）

【0028】

図４にてグラフG2は第２測位情報を３つの連続する時刻について取得したデータd11,d12,d13を示しており、グラフG1と同様に白丸で位置を、向きをベクトル矢印で表現することで、位置及び向きで構成される第２測位情報の時間変化の挙動を模式的に示している。既に説明したように、第２測位情報はグローバル座標で表現されるグローバルなものとして取得される。グラフG2のデータd11,d12,d13にも模式的に示されるように、グローバル座標で表現される第２測位情報は、ノイズ等による影響で非連続の揺らぎが発生しやすく、また、まれに外れ値が混ざるという挙動を示すものである。

【0029】

以上のように、第１測位情報及び第２測位情報は、誤差等が全く存在しない理想的な状況においては、同じ時刻の端末1についての同一の測位結果を表すものとなるべきであるところ、実際に取得される時間変化の挙動には以下のような特徴C1がある。
＜特徴C1＞
第１測位情報…ローカル座標で定義され、誤差が連続的に累積し、稀に急に大幅なずれを生じる（突発的な変動を生じる）。
第２測位情報…グローバル座標で定義され、非連続の揺らぎがあり、稀に外れ値が出る（突発的な外れ値を有する）。

【0030】

上記の特徴C1は、第１測位方式と第２測位方式との相違によるものである。すなわち、根本的な違いとして、第１測位方式は動きの差分を累積する方法であり、例えば加速度成分や、連続する画像間の相対位置を積分し、現在の位置を推定するものである。そのため、急な動きや画像のブレが発生し、前情報との比較ができなくなると非連続的に大きくずれ、そこからまた累積計算していく動きが続くこととなる。これが誤差の累積、突発的な変動の要因となる。

【0031】

一方、第２測位方式は、逆に基準となる情報があり、その基準に対してどのような位置になるかを推定するものである。そのため、誤差が累積することはないが、RANSAC（ランダムサンプルコンセンサス）などに起因するランダムなノイズ成分の除外計算を行うことで、全く同じ情報を与えても、毎回計算結果がわずかに異なったり、場合によっては全く違う値を出してしまうという特徴がある。これが、非連続な揺らぎ、突発的な外れ値の要因である。

【0032】

補完部30ではステップS3～S10において、本来であれば同一内容であるべき第１測位情報及び第２測位情報を、上記のような実際の値における挙動の相違（統計的な相違）を考慮して統合することにより、誤差等の影響を低減した安定的な測位結果を得ることができる。

【0033】

以下、この詳細としてのステップS3～S10を説明する。説明の前提として、第１及び第２の測位情報{P1(k),P2(k)}（k=t,t-1,t-2,…,t-N+1）における位置及び向きを次のように表記するものとする。すなわち、時刻kの第１測位情報P1(k)における位置（ローカル座標）をLP_kとし、向きをLR_kとする。また、時刻kの第２測位情報P2(k)における位置（グローバル座標）をGP_kとし、向きをGR_kとする。これら位置LP_k, GP_kはxyz座標の値として要素数3のベクトルで与えられ、向きLR_k, GR_kはサイズ3×3の回転行列として与えることができるが、以下に説明する各計算の際には、コンピュータグラフィックス等の分野で周知のように、斉次座標表現によりこれら座標（ベクトル）LP_k, GP_kや回転行列LR_k, GR_kを扱うようにしてもよい。

【0034】

ステップS3では、差分計算部31が、第１及び第２の測位情報{P1(k),P2(k)}（k=t,t-1,t-2,…,t-N+1）におけるN個の同時刻の向きに関して、その差分としての回転成分（以下、「差分回転成分」や「差分の回転成分」と表現する場合もある）RDiff_kを以下の式(1)のように計算してから、ステップS4へと進む。式(1)においてinv()は逆行列を表す（inv(LR_k)=LR_k ^-1）ものである。
RDiff_k=GR_k・inv(LR_k) …(1)

【0035】

原理に関して既に説明した通り、式(1)による差分としてのN個の回転成分RDiff_k（k=t,t-1,t-2,…,t-N+1）は、誤差等が全く存在しないと仮定した理想的な状況においては、N個の値が全て等しくなるべきものであるが、実際には前述した特徴C1が反映された誤差によるばらつきが発生する。

【0036】

ステップS4では、ステップS3で得たN個の時系列データとしての回転成分｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）に対して、フィッティング部32がRANSAC（ランダムサンプルコンセンサス）等のロバスト推定により外れ値を許容した回帰直線を計算したものとしてフィッティング結果を得てからステップS5へと進む。

【0037】

ステップS5では、フィッティング部32がさらに、ステップS4で得た回帰直線と、フィッティング対象としてのN個の時系列データである回転成分｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）と、を比較し、回転成分｛RDiff_k｝の値が回帰直線に対して外れ値となっていると閾値判定されるような要素が、後ろ側（時刻kが現時刻tに近い側）に指定数TH以上連続しているか否かを判定し、否定の場合（外れ値が後ろ側に指定数TH以上連続していない場合）にはステップS51へと進み、肯定の場合（外れ値が後ろ側に指定数TH以上連続している場合）にはステップS52へと進む。

【0038】

なお、指定数THは、回転成分の個数Nよりも小さい所定の閾値として設定しておけばよく、例えばN=8の場合にTH=2等として設定してよい。また、ステップS5では以下の手順により当該判定を行えばよい。
（手順１）N個の各回転成分｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）につき、当該時刻kにおける回帰直線の値と比較し、差（絶対値）が閾値以上であれば外れ値であるものと判断し、閾値以上でなければ外れ値ではないものと判断する。
（手順２）上記の手順１で外れ値と判断されたものが１個以上ある場合に、当該外れ値が、N個の時系列データのうちの後ろ側所定範囲（最も後ろ側である現時刻tを含む所定範囲）内に属しており、且つ、当該属しているものが指定数TH以上連続して存在する場合に、肯定判定を得る。これ以外の場合に関しては、否定判定を得る。

【0039】

ステップS51では、フィッティング部32がさらに、N個の各回転成分｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）のうち、ステップS5で外れ値とみなされたもの（手順１で外れ値とされたもの）を、第２測位情報による非連続な揺らぎに起因するものであると推定して除外したものを、以降での計算処理の対象集合Wとして設定してからステップS6へと進む。ここで、RANSACの結果をより安定させるために２回以上繰り返してもよい。すなわち、外れ値を除外した要素のみを用いて再度、RANSACの回帰直線を計算し、除外される外れ値の個数がしきい値以下となった際のものを計算処理の対象集合Wとして設定してからステップS6へと進むようにしてもよい。

【0040】

ステップS52では、フィッティング部32がさらに、N個の各回転成分｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）においては、後ろ側（現時刻tに近い側）の要素が、第１測位情報における大幅なずれが発生した後の状態になっているものと推定し、N個の全データのうち当該後ろ側の要素のみを用いて再度、回帰直線を計算し、当該後ろ側の要素を以降での計算処理の対象集合Wとして設定してからステップS6へと進む。

【0041】

ステップS52で後ろ側の要素について回帰直線を計算する際は、RANSACを用いてもよいし、用いなくともよい。用いる場合、ステップS51と同様に、RANSACの結果がより安定するように２回以上繰り返してもよい。また、後ろ側の要素の個数については、現時刻tを含むTH個又はTH個以上（N個未満）の所定数のサンプルを対象に、回帰直線を計算すればよい。

【0042】

ステップS6では、回転成分計算部33が、回帰直線上での値として、現時刻tでの回転成分RDiff(t)を計算してから、ステップS7へと進む。

【0043】

なお、ステップS6で当該計算するために用いる回帰直線は、ステップS51からステップS6へと至った場合にはステップS4で計算した回帰直線（外れ値を除外しながらRANSACを2回以上繰り返した場合はその最後の回帰直線）であり、ステップS52からステップS6へと至った場合には、ステップS52で後ろ側の要素のみについて再度計算された回帰直線（外れ値を除外しながらRANSACを2回以上繰り返した場合はその最後の回帰直線）である。

【0044】

ステップS7では、スケール計算部34が、計算対象集合W（ステップS51又はS52において設定されている集合W）に属する（k∈W）第１測位情報のローカル座標LP_k及び第２測位情報のグローバル座標GP_kを用いて、集合W内でのローカル座標LP_kの平均値LP_ave及び集合W内でのグローバル座標GP_kの平均値GP_aveを計算し、以下の式(2)の特異値分解を行う。
U・Σ・V^T=SVD((LP_k-LP_ave)^T・(GP_k-GP_ave)) …(2)

【0045】

式(2)において、右辺のSVD()は括弧内の引数を対象として特異値分解を行う旨を表しており、当該特異値分解された結果が左辺となり、行列Σの対角成分に集合W内の各要素についてのスケール計算結果が現れることとなる。説明のため、この行列Σの対角成分を配列Sとする。また、上付きTは転置演算であり、以下同様とする。

【0046】

ステップS7ではさらに、スケール計算部34が上記の配列Sを用いて以下の式(3)により、ローカル座標（第１測位情報）に対するグローバル座標（第２測位情報）のスケールCを計算してから、ステップS8へと進む。式(3)において、sumは、配列S内での各要素の総和であり、count(W)は集合Wの要素数であり、average()は集合Wでの平均値演算である。

【0047】

【数1】

【0048】

ステップS8では、並進成分計算部35が、ステップS6で得られている回転成分RDiff(t)と、ステップS7で得られているスケールCと、を用いることによりさらに、ローカル座標・グローバル座標間での並進成分としての並進ベクトルT（転置演算を表す上付きTとは異なる）を以下の式(4)により求めてから、ステップS9へと進む。
T=-C・RDiff(t)・(LP_ave)^T+(GP_ave)^T …(4)

【0049】

ステップS9では、ステップS6で得られている回転成分RDiff(t)と、ステップS7で得られているスケールCと、ステップS8で得られている並進ベクトルTと、を用いて、変換部36が以下の式(5)の結合処理により、ローカル座標・グローバル座標間での射影行列M(t)を求める。回転成分RDiff(t)はサイズ3×3の行列であり、並進ベクトルTはサイズ1×3の行列（ベクトル）であるため、以下の式(5)の結合によりサイズ3×4の射影行列M(t)が得られることとなる。
M(t)=( C・RDiff(t), T) …(5)

【0050】

ステップS9ではさらに、変換部36が上記求めた射影行列Mを用いて、以下の式(6)により現時刻tのローカル座標（右辺の斉次座標表現）をグローバル座標（左辺の斉次座標表現）に変換したものとして、位置に関する測位結果を得てから、また、以下の式(7)により現時刻tのローカル座標での向きLRをグローバル座標での向きGRに変換したものとして、向きに関する測位結果を得てから、ステップS10へと進む。式(6),(7)でのローカル座標(LP_x,LP_y,LP_z)及びローカル座標での向きLRは、第１測位部12で現時刻tについて取得したデータである。
(GP_x,GP_y,GP_z,1)^T=M(t)・(LP_x,LP_y,LP_z,1)^T …(6)
GR = RDiff(t)・LR …(7)

【0051】

ステップS10では、現時刻tについてのグローバル座標での補完された測位結果として、ステップS9で得たグローバル座標（式(6),(7)の位置及び向き）の値を補完部30より出力してから、ステップS1へと戻る。当該戻ったステップS1では次の現時刻t+1について第１測位部12及び第２測位部22による第１、第２の測位情報の取得が行われ、同様にしてさらにステップS10に至った際に、次の現時刻t+1についての測位結果が出力される。この補正されたグローバル座標は、既存手法によりグローバル座標で得られた測位情報に対して、既存手法における非連続な揺らぎや外れ値が補正されたものとなっている。また補正元のローカル座標に生じる累積誤差もグローバル座標に基づいて随時対応関係が再計算されるため、累積誤差の影響を小さく抑えられている。

【0052】

以上のように、図３のフローはリアルタイムで実行することができ、各時刻tにおいてステップS1での測位結果をもとに、当該時刻tでの補完された測位情報がステップS10において出力されることとなる。（なお、測位情報を時系列データとして予め取得しておき、オフラインで同様の処理を行うようにしてもよい。）以上においては、第１測位部12と第２測位部22とでの測位結果が同一時刻に取得されることを前提として説明してきたが、第１測位部12の測位タイミング（測位頻度）と第２測位部22の測位タイミング（測位頻度）とを異ならせてもよい。例えば、第２測位部22の測位の方がシステム全体的として高負荷である場合には、第２測位部22の測位頻度を第１測位部12の測位頻度よりも低くすることにより、システム全体の負荷を抑制することができる。この場合、時刻tに得られた、M(t)やRDiff(t)をt以降の補完計算に用いてもよい。例えば第1測位部12の測位処理を30fps（フレーム毎秒）で動作させ、第2測位部22の測位処理を1fpsで動作させた場合などでは、M(t)やRDiff(t)の計算は1fpsで行い、それ以外は直前のMやRDiffを再利用してよい。

【0053】

図５は、本実施形態による誤差低減の効果を説明するための模式例として、ステップS3での差分の回転成分時系列データ｛RDiff_k｝（k=t,t-1,t-2,…,t-N+1）の模式例を示す図である。既に説明したように、この時系列データは誤差などのない理想的な状況では一定値となるべきものであるが、実際には図５の例のように、誤差影響によってばらつく挙動を示す。

【0054】

図５のウィンドウWは現時刻t=8におけるものであり、サンプル数N=8である当該ウィンドウW(k=1,2,3,…,8)内を対象として、ステップS4でのフィッティング結果が得られることとなる。時刻t=1～4のデータ群DAや時刻t=5,7,8のデータ群DCは、特徴Cのうち、第２測位情報における揺らぎの誤差と、第１測位情報における時間進行に沿った累積的な誤差（フィッティング直線LCで表現される）と、の影響が現れたものに相当する。時刻t=6のデータDBは、第２測位情報における外れ値の影響が現れたものに相当するが、本実施形態によれば、RANSAC等のロバスト推定によりこのようなデータを除外し、その誤差影響を低減することができる。図５の例では、フィッティング直線LCにおける現時刻t=8の値をRDiff(8)の値として利用することで、諸々の誤差影響を低減することができる。

【0055】

図５ではまた、時刻t=8からt=9へと移った際に、特徴Cのうち、第１測位情報における大幅なずれ（SLAM等のロストによる突発的な変動）が発生したことによる影響が現れている。このように、第１測位情報における座標値のロスト後は、ロスト前の値は廃棄してロスト後の時系列のみを使う必要があるが、本実施形態によればステップS52の処理により、このようにロスト後の時系列のみを利用すること可能となり、誤差を低減することができる。なお、図５においてロスト後の時刻t=9～16のデータ群DDは、データ群DA,DBと同様に、特徴Cのうち、第１測位情報における時間進行に沿った累積的な誤差（フィッティング直線LDで表現される）と、第２測位情報における揺らぎの誤差と、の影響が現れたものに相当する。

【0056】

図６は、一般的なコンピュータ装置70におけるハードウェア構成の例を示す図である。測位システム10における端末1及びサーバ2はそれぞれ、このような構成を有する１台以上のコンピュータ装置70として実現可能である。なお、２台以上のコンピュータ装置70で測位システム10を実現する場合、ネットワーク経由で処理に必要な情報の送受を行うようにしてよい。コンピュータ装置70は、所定命令を実行するCPU（中央演算装置）71、CPU71の実行命令の一部又は全部をCPU71に代わって又はCPU71と連携して実行する専用プロセッサとしてのGPU（グラフィックス演算装置）72、CPU71（及びGPU72）にワークエリアを提供する主記憶装置としてのRAM73、補助記憶装置としてのROM74、通信インタフェース75、ディスプレイ76、マウス、キーボード、タッチパネル等によりユーザ入力を受け付ける入力インタフェース77、測位のためのセンシングハードウェアとしての計測部11及び撮影部21と、これらの間でデータを授受するためのバスBSと、を備える。

【0057】

測位システム10における端末1及びサーバ2の各機能部は、各部の機能に対応する所定のプログラムをROM74から読み込んで実行するCPU71及び／又はGPU72によって実現することができる。なお、CPU71及びGPU72は共に、演算装置（プロセッサ）の一種である。ここで、表示関連の処理が行われる場合にはさらに、ディスプレイ76が連動して動作し、データ送受信に関する通信関連の処理が行われる場合にはさらに通信インタフェース75が連動して動作する。測位システム10による測位結果等はディスプレイ76で表示して出力してよい。

【符号の説明】

【0058】

10…測位システム、1…端末、2…サーバ
11…計測部、12…第１測位部、13…第１DB、21…撮影部、22…第２測位部、23…第２DB、30…補完部、31…差分計算部、32…フィッティング部、33…回転成分計算部、34…スケール計算部、35…並進成分計算部、36…変換部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】