JP2023081734A - 移動体軌跡補正装置 - Google Patents

Abstract

【課題】ＧＮＳＳ測位結果と推測航法による測位結果とを効果的に統合可能な移動体軌跡補正装置を実現する。
【解決手段】全地球航法衛星からの情報に基づいて時系列で移動体の測位を行うＧＮＳＳ測位部４０と、移動体の運動量を時系列で算出する車両運動量算出部４２と、運動量算出部１０２で算出した移動体の運動量に基づいて当該移動体の位置及び姿勢を示す走行軌跡を時系列で推定する推測航法測位部４４と、各々同一時刻における、ＧＮＳＳ測位部４０による測位結果と、推測航法測位部４４による測位結果とを対応付けるＧＮＳＳ・推測航法対応部４６と、推測航法測位部４４による測位結果に基づく走行軌跡を、ＧＮＳＳ測位部４０による測位結果で補正する位置補正部４８と、を含む。
【選択図】図３

Description

本発明は、移動体軌跡補正装置に係り、特に、センサから出力される情報に基づいて推定した自車両の走行軌跡を補正する移動体軌跡補正装置に関する。

地図空間上で自身の位置を確認しながら走行する車両等の移動体では、自身の位置の検出に、衛星から受信した情報に基づいたＧＮＳＳ（全地球航法衛星システム）による測位（以下、「ＧＮＳＳ測位」と略記）の情報と、移動体が搭載しているＩＭＵ（慣性計測装置）等の計測値を積分して自身の位置を推定する推測航法とを併用する場合がある。

一般に、ＧＮＳＳ測位は、絶対位置精度が高く、推測航法は、移動体の相対的な位置精度が高いとされている。従って、ＧＮＳＳ測位の情報と推測航法とを併用することにより移動体の走行軌跡を精度よく推定できる。

特許文献１、特許文献２及び特許文献３の各々には、ＧＮＳＳ測位結果と推測航法による測位結果とをカルマンフィルタを用いて統合する発明が開示されている。

特開２０１１－０８０８３４号公報 特開平０８－０６８６５５号公報 特開平１１－２５７９８２号公報

しかしながら、特許文献１、２、３に記載の発明は、移動体の走行軌跡を分割せずに、ＧＮＳＳ測位結果と推測航法による測位結果と、を統合している。推測航法は、相対的な位置精度が高いものの絶対的な位置を検出しないので、移動体の移動距離が長くなるとＧＮＳＳ測位結果と推測航法による測位結果との乖離が拡大し、移動体の走行軌跡を精度よく補正することが困難になるという問題があった。

本発明は、上記問題に鑑みてなされたものであり、ＧＮＳＳ測位結果と推測航法による測位結果とを効果的に統合可能な移動体軌跡補正装置を実現することを目的とする。

上記目的を達成するために、請求項１に記載の移動体軌跡補正装置は、全地球航法衛星からの情報に基づいて時系列で移動体の測位を行う衛星測位部と、前記移動体の運動量を時系列で検出する運動量検出部と、前記運動量検出部で検出した前記移動体の運動量に基づいて前記移動体の位置及び姿勢を示す前記移動体の走行軌跡を時系列で推定する推測航法部と、各々同一時刻における、前記衛星測位部による測位結果と、前記推測航法部による測位結果とを対応付け、前記推測航法部による測位結果に基づく走行軌跡を前記衛星測位部による測位結果で補正する補正部と、を含んでいる。

また、請求項２に記載の移動体軌跡補正装置は、請求項１に記載の移動体軌跡補正装置において、前記運動量検出部は、前記移動体の、前後速度、方位角、及び操舵角の各々を時系列で検出する。

また、請求項３に記載の移動体軌跡補正装置は、請求項２に記載の移動体軌跡補正装置において、前記補正部は、前記推測航法部が推定した走行軌跡において、前記方位角が所定の閾値以上変化した地点を屈曲点として抽出すると共に、該屈曲点が検出された時間と同時間に検出された前記衛星測位部による測位結果が示す地点を屈曲点に設定し、前記走行軌跡及び前記衛星測位部による測位結果の各々を屈曲点で分割して得た各々の区分において、前記走行軌跡を前記衛星測位部による測位結果で補正する。

また、請求項４に記載の移動体軌跡補正装置は、請求項３に記載の移動体軌跡補正装置において、前記補正部は、各々隣接する区分の端点同士の中間点を各々隣接する区分同士の接続位置とし、該接続位置で各々隣接する区分同士が連続するように、区分の各々を補正する。

本発明によれば、推測航法で得た移動体の走行軌跡を分割して得た各々の区分毎にＧＮＳＳ測位結果を対応付けて推測航法で得た移動体の走行軌跡を補正することにより、推測航法により取得した走行軌跡の誤差の蓄積の影響を低減し、ＧＮＳＳ測位結果と推測航法による測位結果とを効果的に統合できるという効果を奏する。

本発明の第１実施形態に係る移動体軌跡補正装置を含むシステム全体の概略図である。 本発明の第１実施形態に係る移動体軌跡補正装置の一例を示したブロック図である。 本発明の第１実施形態に係る移動体軌跡補正装置の演算装置の具体的な構成の一例を示すブロック図である。 本発明の第１実施形態に係る移動体軌跡補正装置の処理の一例を示したフローチャートである。 地図情報にプロットされたＧＮＳＳ測位結果による走行軌跡と、移動体の実際の走行軌跡である真値とを比較した説明図である。 地図情報にプロットされた推測航法の測位結果による走行軌跡と、移動体の実際の走行軌跡である真値とを比較した説明図である。 ＧＮＳＳ測位結果と推測航法の測位結果とを対応付け、推測航法の測位結果の位置補正を行った場合を示した説明図である。 本発明の第２実施形態に係る移動体軌跡補正装置の処理の一例を示したフローチャートである。 屈曲点の検出手順を示した説明図である。 屈曲点の検出例を示した概略図である。 推測航法による走行軌跡に分割点を設定した場合の概略図である。 ＧＮＳＳ測位結果と推測航法の測位結果との対応付けの一例を示した説明図である。 補正後の走行軌跡の一例を示した概略図である。 本発明の第３実施形態に係る移動体軌跡補正装置の処理の一例を示したフローチャートである。 補正前の分割点の状態を示した概略図である。 （Ａ）は分割点が***している場合の概略図であり、（Ｂ）は２つに***した分割点の間に中間点を設定した場合の概略図であり、（Ｃ）は各区分を相似変換によって位置合わせをした場合の概略図である。 分割点の位置合わせをして得られた走行軌跡の概略図である。 推測航法の測位結果による走行軌跡、走行軌跡の全体をＧＮＳＳ測位結果で補正した第１実施形態の走行軌跡、及び走行軌跡を分割点で分割した区分毎にＧＮＳＳ測位結果で補正した第３実施形態の走行軌跡の各々を真値と比較した概略図である。

［第１実施形態］
以下、図面を参照して本発明の第１実施形態を詳細に説明する。図１に示すように、本実施形態に係る移動体軌跡補正装置１０は、移動体２００に搭載され、衛星４００から受信した情報に基づいた測位が可能なＧＮＳＳ機器３０、サーバ３００等との通信が可能なＶ２Ｘ通信部３６、移動体２００の主に前方の画像を取得する撮像装置２２、移動体２００の前後方向の速度を検出する車速センサ２４、３軸のジャイロと３方向の加速度計によって、３次元の角速度と加速度とが検出可能なＩＭＵ２６、移動体２００の操舵角を検出する操舵角センサ２８、並びに撮像装置２２、車速センサ２４、ＩＭＵ２６及び操舵角センサ２８の各々で取得した情報に基づいて移動体２００の推測航法を行うと共に、ＧＮＳＳ機器３０による測位結果と推測航法による測位結果とを統合する演算装置１４を含む。

図１に示した移動体軌跡補正装置１０は、移動体２００の現在位置を推定するのみならず、ＧＮＳＳ及び推測航法の各々の測位結果の蓄積と統合とにより、地図を作成する用途にも供される。例えば、移動体２００に搭載された移動体軌跡補正装置１０の演算装置１４による演算結果は、後述する記憶装置１８に記憶されると共に、Ｖ２Ｘ通信部３６を介してサーバ３００にアップロードされる。

図２は、本実施形態に係る移動体軌跡補正装置１０の構成の一例を示したブロック図である。移動体軌跡補正装置１０は、演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報から走路と車両との相対横位置、及び車両の方位角を算出する画像情報処理部２０と、画像情報処理部２０が算出した横位置偏差、方位角偏差及び曲率、車速センサ２４が検出した移動体２００の前後速度、ＩＭＵ２６が検出した移動体２００の方位角の偏差及び加速度、操舵角センサ２８が検出した移動体２００の操舵角、衛星４００から受信した情報に基づいて移動体２００の位置を検出する衛星測位システムであるＧＮＳＳ機器３０で検出した移動体２００の現在位置及び現在の方位角、並びに地図情報データベース３４に格納された地図情報が各々入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両の自己位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、サーバ３００等との通信が可能なＶ２Ｘ通信部３６と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

本実施形態に係る外界センサである撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して走路の白線等を検出する。又は、画像と地図情報データベース３４に格納された高精度地図とのマッチングから現在位置の座標及び車両の方位角を算出してもよい。ＩＭＵ２６は、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能な慣性計測装置である。

図３は、演算装置１４の具体的な構成の一例を示すブロック図である。演算装置１４は、一種のコンピュータであり、ＣＰＵ（Central Processing Unit）１４Ｂ、ＲＯＭ（Read Only Memory）１４Ａ、ＲＡＭ（Random Access Memory）１４Ｃ、及び入出力ポート１４Ｄを備える。

演算装置１４では、ＣＰＵ１４Ｂ、ＲＯＭ１４Ａ、ＲＡＭ１４Ｃ、及び入出力ポート１４Ｄがアドレスバス、データバス、及び制御バス等の各種バスを介して互いに接続されている。入出力ポート１４Ｄには、各種の入出力機器として、入力装置１２、ハードディスク（ＨＤＤ）等である記憶装置１８、表示装置１６、及びＶ２Ｘ通信部３６等が各々接続されている。

記憶装置１８には、自己位置推定のための自己位置推定プログラムがインストールされている。本実施形態では、ＣＰＵ１４Ｂが自己位置推定プログラムを実行することにより、自己位置推定を実行する。また、ＣＰＵ１４Ｂは、自己位置推定プログラムによる処理結果を表示装置１６に表示させる。なお、本実施形態の自己位置推定プログラムを演算装置１４にインストールするには、幾つかの方法があるが、例えば、自己位置推定プログラムをセットアッププログラムと共にＣＤ－ＲＯＭやＤＶＤ等に記憶しておき、入出力装置であるディスクドライブ等にディスクをセットし、ＣＰＵ１４Ｂに対してセットアッププログラムを実行することにより記憶装置１８に自己位置推定プログラムをインストールする。または、公衆電話回線又はネットワークを介して演算装置１４と接続される他の情報処理機器と通信することで、記憶装置１８に自己位置推定プログラムをインストールするようにしてもよい。

次に、演算装置１４のＣＰＵ１４Ｂが自己位置推定プログラムを実行することで実現される各種機能について説明する。自己位置推定プログラムは、ＣＰＵ１４Ｂを、ＧＮＳＳ機器３０による測位結果を地図情報データベース３４に格納された地図情報にプロットすると共に、当該測位結果をプロットした地図情報を記憶装置１８に蓄積するＧＮＳＳ測位機能、ＩＭＵ２６等の検出結果から移動体２００の運動量を算出する車両運動量算出機能、算出した移動体２００の運動量に基づいた推測航法の測位結果を算出する推測航法測位機能、ＧＮＳＳ測位結果と推測航法測位結果とを対応付けるＧＮＳＳ・推測航法対応機能、及びＧＮＳＳ測位結果と推測航法測位結果とを対応付けた結果から移動体２００の位置を補正する位置補正機能として機能させる。ＣＰＵ１４Ｂは、自己位置推定プログラムを実行することにより、ＧＮＳＳ測位部４０、車両運動量算出部４２、推測航法測位部４４、ＧＮＳＳ・推測航法対応部４６及び位置補正部４８として機能する。

図４は、本実施形態に係る移動体軌跡補正装置１０の処理の一例を示したフローチャートである。ステップ１００では、ＧＮＳＳ機器３０による測位結果を地図情報データベース３４に格納された地図情報にプロットすると共に、当該測位結果をプロットした地図情報を記憶装置１８に蓄積する。

図５は、地図情報にプロットされたＧＮＳＳ測位結果による走行軌跡１０２と、移動体２００の実際の走行軌跡である真値１００とを比較した説明図である。前述のように、ＧＮＳＳ測位結果は、絶対位置の検出に優れるが、衛星４００からの電波の受信に難がある等の障害があると、図５に示したような位置情報の欠損１０４、又は誤差１０６を生じる場合がある。後述するように、本実施形態では、ＧＮＳＳ測位結果と推測航法の測位結果とを対応付けることにより、図５に示したような欠損１０４及び誤差１０６を補正する。

ステップ１０２では、撮像装置２２、車速センサ２４、ＩＭＵ２６及び操舵角センサ２８等の検出結果から移動体２００の運動量を算出する。具体的には、車載カメラ等の撮像装置２２が取得した移動体２００の周辺の画像情報を解析して検出した走路の区画線等に基づいて、走路上の移動体２００の位置を推定すると共に、車速センサ２４の検出結果を積分することにより、移動体２００の移動距離を算出し、ＩＭＵ２６の検出結果の積分値と操舵角センサ２８の検出結果とに基づいて、移動体２００の進行方向を推定する。

ステップ１０４では、撮像装置２２、車速センサ２４、ＩＭＵ２６、操舵角センサ２８、及びＧＰＳ３０等の検出結果から得た移動体２００の運動量に基づいて、移動体２００の位置及び姿勢を推定する推測航法を行い、推測航法による測位結果を地図情報データベース３４に格納された地図情報にプロットすると共に、当該測位結果をプロットした地図情報を記憶装置１８に蓄積する。

図６は、地図情報にプロットされた推測航法の測位結果による走行軌跡１１０と、移動体２００の実際の走行軌跡である真値１００とを比較した説明図である。前述のように、推測航法による測位結果は、相対的な位置精度が高いものの絶対的な位置を検出しないので、図６に示したように、移動体の移動距離が長くなると推測航法の測位結果による走行軌跡１１０と真値１００との乖離が拡大していく。

ステップ１０６では、ＧＮＳＳ測位結果と推測航法の測位結果とを対応付ける。具体的には、推測航法の測位結果に含まれる地点を検出した時間と、同一時間を示すＧＮＳＳ測位結果の地点とを対応付ける。

ＧＮＳＳ測位結果と推測航法の測位結果との対応付けは、一例として、下記のように行う。推測航法の測位結果を３次元の点群Ｐ：{ｐ_i：ｉ＝１、…、Ｎ}とし、ＧＮＳＳ測位結果を３次元の点群Ｑ：{ｑ_i：ｉ＝１、…、Ｎ}とする。ｉの値が同じ点ｐ_iと点ｑ_iは、時系列的に同じ時間で検出されたことを示す。

かかる点群Ｐ（点ｐ_i）を座標変換して点群Ｑ（点ｑ_i）と位置合わせを行う。点ｐ_i、ｑ_iの座標を各々同次座標ｐ_i＝（ｐ_i,x、ｐ_i,y、ｐ_i,z、１）、ｑ_i＝（ｑ_i,x、ｑ_i,y、ｑ_i,z、１）で表し、４×４の座標変換行列Ｔを用いて点ｐ_iを点ｑ_iに近づけるようにすると、座標変換行列Ｔによる座標変換後の点ｐ_iは、Ｔｐ_iとなる。

座標変換行列Ｔは、下記の式（１）のように、座標変換後の点ｐ_i（Ｔｐ_i）と点ｑ_iとの距離の二乗和が最小となるように決定する。

そして、ステップ１０８では、推測航法の測位結果の位置補正を行って処理を終了する。ステップ１０８での位置補正は、並進及び回転からなる座標変換である合同変換による。座標変換が合同変換の場合、座標変換行列Ｔは下記の式に表されるような４×４の行列になる。下記式の右辺において、Ｒは３×３の正規直交行列であり、ｔは３×１の列ベクトルである。

図７は、ＧＮＳＳ測位結果と推測航法の測位結果とを対応付け、推測航法の測位結果の位置補正を行った場合を示した説明図である。推測航法は、一般に相対的な位置精度が高いので、ＧＮＳＳ測位結果によって絶対的な位置を補正することにより、真値１００に近い走行軌跡１６０を得ることができる。ＸＹ平面上における座標変換の場合は、各々同次座標ｐ_i＝（ｐ_i,x、ｐ_i,y、１）、ｑ_i＝（ｑ_i,x、ｑ_i,y、１）で表し、行列Ｔを３×３、Ｒを２×２の行列、ｔを２×１の列ベクトルとして、同様の位置補正を行う。

以上説明したように、本実施形態によれば、相対的な位置精度が高いものの絶対的な位置を推定できない推測航法の測位結果を、衛星４００からの受信状態等に影響されて、上方の欠損１０４又は誤差１０６が生じ得るものの、絶対位置の検出精度が高いＧＮＳＳ測位結果に対応付けることにより、移動体２００の移動の走行軌跡１１０の形状精度を保持したまま、走行軌跡１１０の絶対位置の誤差を精度よく補正することができる。

［第２実施形態］
続いて、本発明の第２実施形態について説明する。本実施形態に係る移動体軌跡補正装置の構成は第１実施形態に係る移動体軌跡補正装置１０と同じなので、詳細な説明は省略する。

図８は、本実施形態に係る移動体軌跡補正装置の処理の一例を示したフローチャートである。しかしながら、図８のステップ２００～２０４の手順は、図４のステップ１００～２０４と同一なので、詳細な説明は省略する。

ステップ２０６では、移動体２００の移動経路における屈曲点を検出する。図９は、屈曲点の検出手順を示した説明図である。屈曲点は、移動体２００の位置２１０の前後での方位角の変化量、換言すれば、移動体２００の方位角の時系列での変化量に基づいて検出する。

移動体２００の方位角は、ＩＭＵ２６等で検出するが、移動体２００が直進している場合、移動体２００の位置２１０の前方方位角２２０Ｆと後方方位角２２０Ｒとは同一となる。しかしながら、移動体２００の位置２１０が屈曲点にさしかかると、前方方位角２２０Ｆと後方方位角２２０Ｒとに差異が生じる。言うなれば、移動体２００が屈曲点を通過する直前にＩＭＵ２６で検出した方位角と、屈曲点を通過した直後にＩＭＵ２６で検出した方位角とが異なる。かかる方位角の変化量が所定の閾値以上の場合に、移動体２００の走行軌跡１１０上に屈曲点が存在すると判断する。所定の閾値は、例えば、２０度から９０度まで任意の値を設定し得るが、あまり小さくすると、走行軌跡１１０上に屈曲点が多数生じることになり、後段における走行軌跡１１０の補正の演算負荷が大きくなるので、実走試験等を通じて妥当な値を決定する。

図１０は、屈曲点２３０Ａ、２３０Ｂ、２３０Ｃの検出例を示した概略図である。図１０に示したように、屈曲点２３０Ａ、２３０Ｂ、２３０Ｃでは、前方方位角２２０Ｆと後方方位角２２０Ｒとが大きく異なっている。

ステップ２０８では、屈曲点２３０Ａ、２３０Ｂ、２３０Ｃを走行軌跡１１０の分割点に設定し、走行軌跡１１０を分割点で分割する。

図１１は、推測航法による走行軌跡１１０に分割点２４０Ａ、２４０Ｂ、２４０Ｃ、２４０Ｄ、２４０Ｅ、２４０Ｆ、２４０Ｇ、２４０Ｈ、２４０Ｉ、２４０Ｊ、２４０Ｋを設定した場合の概略図である。分割点は、ＧＮＳＳ測位結果による走行軌跡１０２にも設定する。具体的には、推測航法による走行軌跡１１０で分割点２４０Ａ～２４０Ｋを検出した時間と同一時間のＧＮＳＳ測位結果による走行軌跡１０２上の位置をＧＮＳＳ測位結果による走行軌跡１０２における分割点とする。

ステップ２１０では、ＧＮＳＳ測位結果と推測航法の測位結果とを対応付ける。ステップ２１０では、第１実施形態と同様に、推測航法の測位結果に含まれる地点を検出した時間と、同一時間を示すＧＮＳＳ測位結果の地点とを対応付けるが、対応付けが分割点で区切られた区分毎に行う点で第１実施形態と相違する。

図１２は、ＧＮＳＳ測位結果と推測航法の測位結果との対応付けの一例を示した説明図である。本実施形態では、例えば、分割点で区切られた１の区分１３０のように、推測航法の測位結果に含まれる地点を検出した時間と同一時間を示すＧＮＳＳ測位結果の地点とを対応付ける。

ステップ２１２では、推測航法の測位結果の位置補正を行って処理を終了する。ステップ２１２での位置補正は、第1実施形態で説明した合同変換である。

図１３は、補正後の走行軌跡１４０の一例を示した概略図である。推測航法の測位結果による走行軌跡１１０は、屈曲点では誤差が大きくなりやすいが、屈曲点以外の区分では、相対的な位置精度が高いという特徴がある。本実施形態では、分割した走行軌跡１１０の区分毎に、推測航法の測位結果による走行軌跡１１０とＧＮＳＳ測位結果による走行軌跡１０２との対応付けをすることにより、移動体２００の移動距離が長くなると増大するＧＮＳＳ測位結果による走行軌跡１０２と推測航法の測位結果による走行軌跡１１０との乖離を抑制すると共に、相対的な位置精度が高い推測航法の測位結果による区分を、絶対位置の精度が高いＧＮＳＳ測位結果による区分と対応付けることにより、真値１００に近い走行軌跡１４０を得ることができる。

以上説明したように、本実施形態では、推測航法による走行軌跡１１０を分割することで推測航法により取得した走行軌跡の誤差の蓄積の影響を低減する。しかしながら、図１３の分割点２４０Ｄと分割点２４０Ｅとで区切られた区分を拡大してみると、分割点２４０Ｄは、分割点２４０Ｄ１と分割点２４０Ｄ２とに、分割点２４０Ｅは、分割点２４０Ｅ１と分割点２４０Ｅ２とに、各々***している。***した分割点の位置補正は、後述する第３実施形態で行う。

［第３実施形態］
続いて、本発明の第３実施形態について説明する。本実施形態に係る移動体軌跡補正装置の構成は第１実施形態に係る移動体軌跡補正装置１０と同じなので、詳細な説明は省略する。

図１４は、本実施形態に係る移動体軌跡補正装置の処理の一例を示したフローチャートである。しかしながら、図１４のステップ３００～２０４の手順は、図４のステップ１００～１０４と同一であり、図１４のステップ３０６～３１２の手順は、図８のステップ２０６～２１２と同一なので、詳細な説明は省略する。

ステップ３１４では、分割点の位置補正を行う。図１５は、補正前の分割点の状態を示した概略図である。図１３でも示したように、図１５では、分割点２４０Ｄは、分割点２４０Ｄ１と分割点２４０Ｄ２とに、分割点２４０Ｅは、分割点２４０Ｅ１と分割点２４０Ｅ２とに、各々***している。ステップ３１４では、これらの***した分割点が同一の分割点となるように分列した分割点の各々の位置を補正する。

図１６（Ａ）は、分割点２４０Ｄが分割点２４０Ｄ１と分割点２４０Ｄ２とに、分割点２４０Ｅが分割点２４０Ｅ１と分割点２４０Ｅ２とに、分割点２４０Ｆは、分割点２４０Ｆ１と分割点２４０Ｆ２とに、各々***している場合の概略図である。本実施形態では、***した分割点同士は、同一の点であるとして、***した分割点同士の中間点を補正後の分割点として設定する。図１６（Ｂ）は、分割点２４０Ｄ１と分割点２４０Ｄ２との間に中間点２４０ＤＣを、分割点２４０Ｅ１と分割点２４０Ｅ２との間に中間点２４０ＥＣを、分割点２４０Ｆ１と分割点２４０Ｆ２との間に中間点２４０ＦＣを、各々設定した場合の概略図である。本実施形態では、これら中間点を補正後の分割点とし、ＧＮＳＳ測位結果で補正した走行軌跡１４０の各々の区分に対し、並進、回転及び拡大縮小からなる変換である相似変換を実施して、区分同士の位置合わせをする。

座標変換が相似変換の場合、座標変換行列Ｔは下記の式に表されるような４×４の行列になる。下記式の右辺において、Ｒは３×３の正規直交行列であり、ｓは拡大縮小の倍率を示すスケールであり、ｔは３×１の列ベクトルである。

図１６（Ｃ）は、図１６（Ｂ）に示した各区分を相似変換によって位置合わせをした場合の概略図である。上記の座標変換行列Ｔを用いた変換により、走行軌跡１４０の区分の各々は、中間点２４０ＤＣ、２４０ＥＣ、２４０ＦＣで連続した走行軌跡１５０となる。

図１７は、分割点の位置合わせをして得られた走行軌跡１５０の概略図である。走行軌跡１５０は、中間点２４０ＡＣ、２４０ＢＣ、２４０ＣＣ、２４０ＤＣ、２４０ＥＣ、２４０ＦＣ、２４０ＧＣ、２４０ＨＣ、２４０ＩＣ、２４０ＪＣ、２４０ＫＣの各々で連続した態様となる。

図１８は、推測航法の測位結果による走行軌跡１１０、走行軌跡１１０の全体をＧＮＳＳ測位結果で補正した第１実施形態の走行軌跡１６０、及び走行軌跡１１０を分割点で分割した区分毎にＧＮＳＳ測位結果で補正した本実施形態の走行軌跡１５０の各々を真値１００と比較した概略図である。全体を補正した走行軌跡１６０は、真値１００との乖離が目立つが、区分毎に補正した走行軌跡１５０は、真値１００に近く、推測航法の測位結果による走行軌跡１１０の絶対位置の誤差を精度よく補正している。

以上説明したように、本実施形態によれば、屈曲点では誤差が大きくなりやすいが、屈曲点以外の区分では、相対的な位置精度が高い推測航法の測位結果による走行軌跡１１０を屈曲点で分割し、分割した走行軌跡１１０の区分毎に、推測航法の測位結果による走行軌跡１１０とＧＮＳＳ測位結果による走行軌跡１０２との対応付けをして補正している。さらにＧＮＳＳ測位結果で補正した各々隣接する区分の端点同士が一致するように区分の各々を相似変換によって補正することにより、真値１００に近い連続した補正済みの走行軌跡１５０を得ることができる。

第１実施形態、第２実施形態、及び第３実施形態において、移動体２００の演算装置１４でＧＮＳＳ機器３０、及びＩＭＵ２６等で得たデータからＧＮＳＳ測位結果による走行軌跡１０２、及び推測航法の測位結果による走行軌跡１１０を算出すると共に、得られたＧＮＳＳ測位結果による走行軌跡１０２、推測航法の測位結果による走行軌跡１１０、ＧＮＳＳ測位結果で補正した推測航法の走行軌跡１４０、１５０等を、移動体２００の記憶装置１８に蓄積することを想定しているが、これに限定されない。例えば、サーバ３００にＧＮＳＳ機器３０、及びＩＭＵ２６等で得たデータを送信し、サーバにおいてＧＮＳＳ測位結果による走行軌跡１０２、及び推測航法の測位結果による走行軌跡１１０を算出すると共に、得られたＧＮＳＳ測位結果による走行軌跡１０２、推測航法の測位結果による走行軌跡１１０、ＧＮＳＳ測位結果で補正した推測航法の走行軌跡１４０、１５０等を、サーバ３００に蓄積してもよい。

なお、特許請求の範囲における「衛星測位部」は、明細書の発明の詳細な説明における「ＧＮＳＳ測位部４０」に、特許請求の範囲における「運動量検出部」は同「車速センサ２４」、「ＩＭＵ２６」、操舵角センサ２８」及び「撮像装置２２」に、特許請求の範囲に記載の「推測航法部」は、同「推測航法測位部４４」に、特許請求の範囲における「補正部」は、同「ＧＮＳＳ・推測航法対応部４６」及び「位置補正部４８」に、各々相当する。

１０ 移動体軌跡補正装置
１２ 入力装置
１４ 演算装置
１４Ａ ＲＯＭ
１４Ｂ ＣＰＵ
１４Ｃ ＲＡＭ
１８ 記憶装置
２０ 画像情報処理部
２２ 撮像装置
２４ 車速センサ
２６ ＩＭＵ
２８ 操舵角センサ
３０ ＧＮＳＳ機器
３４ 地図情報データベース
４０ ＧＮＳＳ測位部
４２ 車両運動量算出部
４４ 推測航法測位部
４６ ＧＮＳＳ・推測航法対応部
４８ 位置補正部
１００ 真値
１０２ 走行軌跡
１０４ 欠損
１０６ 誤差
１１０、１４０、１５０、１６０ 走行軌跡
１３０ 区分
２００ 移動体
２１０ 位置
２２０Ｆ 前方方位角
２２０Ｒ 後方方位角
２３０Ａ、２３０Ｂ、２３０Ｃ 屈曲点
２４０Ａ、２４０Ｂ、２４０Ｃ、２４０Ｄ、２４０Ｅ、２４０Ｆ、２４０Ｇ、２４０Ｈ、２４０Ｉ、２４０Ｊ、２４０Ｋ 分割点
２４０ＡＣ、２４０ＢＣ、２４０ＣＣ、２４０ＤＣ、２４０ＥＣ、２４０ＦＣ、２４０ＧＣ、２４０ＨＣ、２４０ＩＣ、２４０ＪＣ、２４０ＫＣ 中間点
２４０Ｄ、２４０Ｄ１、２４０Ｄ２、２４０Ｅ、２４０Ｅ１、２４０Ｅ２、２４０Ｆ、２３０Ｆ１、２４０Ｆ２ 分割点
３００ サーバ
４００ 衛星

Claims (4)

1. 全地球航法衛星からの情報に基づいて時系列で移動体の測位を行う衛星測位部と、
   前記移動体の運動量を時系列で検出する運動量検出部と、
   前記運動量検出部で検出した前記移動体の運動量に基づいて前記移動体の位置及び姿勢を示す前記移動体の走行軌跡を時系列で推定する推測航法部と、
   各々同一時刻における、前記衛星測位部による測位結果と、前記推測航法部による測位結果とを対応付け、前記推測航法部による測位結果に基づく走行軌跡を前記衛星測位部による測位結果で補正する補正部と、
   を含む移動体軌跡補正装置。
2. 前記運動量検出部は、前記移動体の、前後速度、方位角、及び操舵角の各々を時系列で検出する請求項１に記載の移動体軌跡補正装置。
3. 前記補正部は、前記推測航法部が推定した走行軌跡において、前記方位角が所定の閾値以上変化した地点を屈曲点として抽出すると共に、該屈曲点が検出された時間と同時間に検出された前記衛星測位部による測位結果が示す地点を屈曲点に設定し、前記走行軌跡及び前記衛星測位部による測位結果の各々を屈曲点で分割して得た各々の区分において、前記走行軌跡を前記衛星測位部による測位結果で補正する請求項２に記載の移動体軌跡補正装置。
4. 前記補正部は、各々隣接する区分の端点同士の中間点を各々隣接する区分同士の接続位置とし、該接続位置で各々隣接する区分同士が連続するように、区分の各々を補正する請求項３に記載の移動体軌跡補正装置。

Images