JP7174676B2 - 位置推定装置、位置推定方法および航走体 - Google Patents

Description

本開示は、航走体の位置推定技術に関する。

慣性航法装置（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）は、慣性力の積分に基づいて自己位置を推定するが、慣性航法装置のみでの推定位置は、積分誤差のために真の位置から次第にずれていく。このため、従来から、慣性航法装置およびＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を用いて現在の自己位置を推定する方法が知られている（例えば特許文献１～２）。しかし、ＧＰＳを用いる場合には、ＧＰＳ信号が届かない例えば水中など領域を航走する場合や、ＧＰＳ信号の受信が妨害されるような場合など、ＧＰＳが利用できない場合もある。

このような場合には、慣性航法装置およびマップマッチング技術を用いて自己位置を推定する手法の適用が考えられる（例えば特許文献３）。マップマッチング技術では、走領域の各位置（座標）における高度や重力ポテンシャル、風景等の外部環境情報を予め記録した（関連付けた）地図を用意しておく。そして、実際の位置において高度計、重力計、カメラ等のセンサ（計測器）を使用して検出した情報と地図情報とを照合（マッチング）し、自己位置を推定する。この地図は、事前に利用可能に用意しておくので、ＧＰＳ等の外部信号が使用できない場合でも、現地においてセンサを用いて検出した検出情報と地図情報との照合結果によって、慣性航法装置で算出された位置（ＩＮＳ位置）の算出結果（推定結果）を補正し、自己位置の推定精度を高めることが可能となる。

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

上述したように、マップマッチング技術では地図上の情報（参照情報）と、現地での自己推定時におけるセンサによる計測値等（検出情報）とを照合して自己位置を推定するが、参照情報あるいは検出情報の変化が乏しい領域（平坦な地形等）を航走する場合には、その近傍領域のどの位置にいるかの判別は困難である。そればかりか、マップマッチングによる結果が、上記の平坦な地形を航走しているなどの理由でずれた位置を示す場合もあり、このような場合に慣性航法装置による推定位置をマップマッチングの結果で補正すると、逆に誤差の拡大の要因にもなり得る。このような課題に対して、分解能の高いセンサを用いることで、微小な差異も判別してマップマッチングを行うことも考えられるが、コストが多大となる。

上述の事情に鑑みて、本発明の少なくとも一実施形態は、ＧＰＳを用いることなく、平坦な地形を航走する場合であっても位置の推定精度を高めることが可能な位置推定装置を提供することを目的とする。

（１）本発明の少なくとも一実施形態に係る位置推定装置は、
航走体の現在位置を推定する位置推定装置であって、
前記航走体が有するセンサおよび慣性航法装置からそれぞれ出力される規定タイミングの到来時毎の実センサ情報および位置算出結果を記憶するよう構成される第１記憶部と、
前記実センサ情報との照合により位置特定を行うための参照センサ情報が位置座標に関連付けられた地図情報を記憶するよう構成される第２記憶部と、
前記現在位置の推定時を含む複数回の前記規定タイミングの到来時の各々における前記位置算出結果に基づく推定位置の時系列をそれぞれ示す複数の候補経路を生成するよう構成される候補経路生成部と、
前記複数の候補経路がそれぞれ有する複数の前記推定位置の各々における、前記地図情報に基づいて得られる前記参照センサ情報の時系列および前記実センサ情報の時系列に基づいて、前記複数の候補経路の各々の確からしさの評価値を算出するよう構成される評価値算出部と、
前記複数の候補経路の各々の前記評価値に基づいて前記現在位置を特定するよう構成される位置特定部と、を備える。

上記（１）の構成によれば、航走体が通過した可能性のある複数の候補経路を、慣性航法による位置算出結果から得られる規定タイミングの到来時毎の複数の候補位置のいずれかを選択して時系列で並べるなどして生成する。こうして生成した各候補経路を地図情報と照合することで、各候補経路を航走体が通過した場合に得られるはずのセンサ情報（参照センサ情報）の時系列を得ると共に、この参照センサ情報の時系列と推定時刻などの規定タイミングを同じくする実センサ情報の時系列とを比較することで、誤差列を算出するなどして、各候補経路の確からしさを評価する。そして、この評価値が最も高い候補経路に含まれる最新の位置を現在位置とするなどして、現在位置を特定（推定）する。

換言すれば、航走体の現在位置を、実際に測定等により検出された複数の規定タイミングの到来時における実センサ情報を期待値として、現在位置の推定時の参照センサ情報の一致度のみならず、さらに１回以上遡った規定タイミングの到来時おける参照センサ情報との一致度を考慮して、実センサ情報の時系列と一致度の高い候補経路に基づいて航走体７の現在位置を推定する。このように、慣性航法に基づいて生成した複数の候補経路の各位置に対応した実センサ情報と参照センサ情報との比較を通して、候補経路の全体と地図情報とのマッチングを行うことで、ＧＰＳを用いることなく、航走体の現在位置を適切に推定することができる。

また、慣性航法による位置算出結果（後述するＩＮＳ位置）をマップマッチング技術により補正する場合には、地図情報（参照センサ情報）とセンサによる検出情報とを照合して自己位置を推定するが、参照センサ情報の変化が乏しい（平坦な地形等）領域を航走する場合、その照合による位置の絞り込みは困難であるが、上述したように候補経路の生成を通して推定を行うことで、平坦な地形等の領域に途中から航走体が進入したとしても、わずかな起伏を検出可能な高精度の計測器を用いることなく、より高い精度で現在位置を推定することができる。

（２）幾つかの実施形態では、上記（１）の構成において、
前記評価値算出部は、
前記候補経路の前記参照センサ情報の時系列を取得する第１取得部と、
前記候補経路の前記実センサ情報の時系列を取得する第２取得部と、
取得された前記参照センサ情報の時系列と前記実センサ情報の時系列との比較に基づいて算出される誤差の時系列である誤差列を算出する誤差列算出部と、
前記誤差列に基づいて前記評価値を算出する評価部と、を有する。

上記（２）の構成によれば、複数の候補経路について、それぞれ、候補経路を航走体が通った場合の参照センサ情報の時系列を地図情報に基づいて取得し、実際の計測等の検出により得られた実センサ情報の時系列と比較することで誤差列を算出する。そして、各候補経路の評価値を、航走体の航走時に実際に検出された実センサ情報の時系列との比較により得られる誤差列に基づいて算出する。これによって、誤差列における誤差が小さいほど評価値が高くなるなど、実センサ情報の時系列との比較に基づく誤差に応じて、評価値を適切に算出することができる。よって、実際の実センサ情報の時系列に合うような確度の高い候補経路を選択することができ、現在位置を適切に推定することができる。

（３）幾つかの実施形態では、上記（１）～（２）の構成において、
前記候補経路生成部は、
前記現在位置の候補となる複数の候補位置を取得する候補位置取得部と、
前記複数の候補位置の各々から、規定回数前までの前記規定タイミングの到来時の各々における前記推定位置をそれぞれ算出することにより、前記複数の候補経路を算出する候補経路算出部と、を有する。

上記（３）の構成によれば、例えば慣性航法装置により得られる推定位置の範囲にある複数の候補位置の各々について、それぞれ、例えば状態遷移行列などを用いて、過去の規定タイミングの到来時の位置を遡るようにすることにより、複数の候補位置の各々に対応した複数の候補経路を算出する。これによって、複数の適切な候補経路を生成することができる。

（４）幾つかの実施形態では、上記（１）～（２）の構成において、
前記候補経路生成部は、
前記候補経路を取得する候補経路取得部と、
取得された前記候補経路に基づいて、新たな前記候補経路を算出する生成部と、を有する。

上記（４）の構成によれば、入力されるなどして取得した候補経路に基づいて、例えば誤差が小さくなるような新たな候補経路を算出する。これによって、複数の候補経路を適切に生成することができる。

（５）幾つかの実施形態では、上記（１）～（２）の構成において、
前記候補経路生成部は、
前記複数回の前記規定タイミングの到来時の各々において、それぞれ、所定数の前記推定位置を抽出する候推定位置抽出部と、
抽出された各々の前記規定タイミングの到来時毎の前記所定数の推定位置を総当たりで組み合わせることで、前記複数の候補経路を生成する組合せ部と、を有する。

上記（５）の構成によれば、例えば複数の規定タイミングの到来時の各々における航走体の位置として可能性のある全ての推定位置を抽出すると共に、抽出した各規定タイミングの到来時毎の複数の推定位置同士の総当たりにより候補経路Ｒｓを生成することで、想定される全ての候補経路Ｒｓを網羅的に生成することもできる。

（６）本発明の少なくとも一実施形態に係る航走体は、
慣性航法により航走体の位置を算出する慣性航法装置と、
少なくとも１つのセンサと、
前記慣性航走装置による位置算出結果、および前記少なくとも１つのセンサにより検出された検出情報である実センサ情報に基づいて、前記航走体の現在位置を推定する上記（１）～（５）のいずれか１項に記載の位置推定装置と、
前記位置推定装置によって推定された前記現在位置に基づいて前記航走体の航走を制御する航走制御装置と、を備える。
上記（６）の構成によれば、上記（１）と同様の効果を奏する。

（７）本発明の少なくとも一実施形態に係る位置推定方法は、
航走体の現在位置を推定する位置推定方法であって、
前記航走体が有するセンサおよび慣性航法装置からそれぞれ出力される規定タイミングの到来時毎の実センサ情報および位置算出結果を記憶する第１記憶ステップと、
前記実センサ情報との照合により位置特定を行うための参照センサ情報が位置座標に関連付けられた地図情報を記憶する第２記憶ステップと、
前記現在位置の推定時を含む複数回の前記規定タイミングの到来時の各々における前記位置算出結果に基づく推定位置の時系列をそれぞれ示す複数の候補経路を生成する候補経路生成ステップと、
前記複数の候補経路がそれぞれ有する複数の前記推定位置の各々における、前記地図情報に基づいて得られる前記参照センサ情報の時系列および前記実センサ情報の時系列に基づいて、前記複数の候補経路の各々の確からしさの評価値を算出する評価値算出ステップと、
前記複数の候補経路の各々の前記評価値に基づいて前記現在位置を特定する位置特定ステップと、を備える。
上記（７）の構成によれば、上記（１）と同様の効果を奏する。

本発明の少なくとも一実施形態によれば、ＧＰＳを用いることなく、平坦な地形を航走する場合であっても位置の推定精度を高めることが可能な位置推定装置が提供される。

本発明の一実施形態に係る位置推定装置の機能を概略的に示す図である。 本発明の一実施形態に係る位置推定装置を備える航走体の航走時の状況を示す図である。 本発明の一実施形態に係る候補経路生成部の機能を示すブロック図であり、候補位置に基づいて候補経路を生成する。 本発明の一実施形態に係る候補経路生成部の機能を示すブロック図であり、候補経路に基づいて新たな候補経路を生成する。 本発明の一実施形態に係る候補経路生成部の機能を示すブロック図であり、推定位置の総当たりにより新たな候補経路を生成する。 本発明の一実施形態に係る位置推定方法を示す図である。

以下、添付図面を参照して本発明の幾つかの実施形態について説明する。ただし、実施形態として記載されている又は図面に示されている構成部品の寸法、材質、形状、その相対的配置等は、本発明の範囲をこれに限定する趣旨ではなく、単なる説明例にすぎない。
例えば、「ある方向に」、「ある方向に沿って」、「平行」、「直交」、「中心」、「同心」或いは「同軸」等の相対的或いは絶対的な配置を表す表現は、厳密にそのような配置を表すのみならず、公差、若しくは、同じ機能が得られる程度の角度や距離をもって相対的に変位している状態も表すものとする。
例えば、「同一」、「等しい」及び「均質」等の物事が等しい状態であることを表す表現は、厳密に等しい状態を表すのみならず、公差、若しくは、同じ機能が得られる程度の差が存在している状態も表すものとする。
例えば、四角形状や円筒形状等の形状を表す表現は、幾何学的に厳密な意味での四角形状や円筒形状等の形状を表すのみならず、同じ効果が得られる範囲で、凹凸部や面取り部等を含む形状も表すものとする。
一方、一の構成要素を「備える」、「具える」、「具備する」、「含む」、又は、「有する」という表現は、他の構成要素の存在を除外する排他的な表現ではない。

図１は、本発明の一実施形態に係る位置推定装置１の機能を概略的に示す図である。また、図２は、本発明の一実施形態に係る位置推定装置１を備える航走体７の航走時の状況を示す図である。

位置推定装置１は、所望の航路を無人または有人で航走可能な航走体７の現在位置Ｐｃを推定するための装置である。航走体７は、例えば潜水艦、自律型無人潜水機（ＡＵＶ：Ａｕｔｏｎｏｍｏｕ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅ）、空中を飛翔する飛翔体などである。図１に示すように、航走体７は、上記の位置推定装置１と、慣性航法により航走体７の位置を算出する慣性航法装置８（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）と、例えば高度計や重力計、周辺の風景を撮影可能なカメラが有する撮像センサ（例えばＣＭＯＳセンサ）など、マップマッチングによる現在位置Ｐｃの推定（補正）に利用可能な、例えば高度や重力、周辺の風景などの少なくとも１種類の外部環境情報を検出するための少なくとも１種類（図１では複数種類）のセンサ７ｓと、航走体７の航走を制御する航走制御装置９と、を備える。

この航走体７においては、図２に示すように、例えば周期的などの規定タイミングが到来した際の慣性航法装置８による位置算出結果（以下、ＩＮＳ位置Ｐｂ）、および航走体７が備える１以上のセンサ７ｓの少なくとも一部（図１では全部）によるセンシング対象（高度、重量など）の検出結果が蓄積される。そして、位置推定装置１は、規定タイミングが到来する度に、上記の蓄積された慣性航法装置８による位置算出結果、および少なくとも１つのセンサ７ｓによる検出結果を用いて、その際の航走体７の位置である現在位置Ｐｃの推定を実行するように構成される。また、航走制御装置９は、位置推定装置１から得られる現在位置Ｐｃを確認しながら、予め定められた航路（既定航路）などを航走体７が航走するように航走を制御する。

図１～図２に示す実施形態では、航走体７はＡＵＶであり、水中を無人で潜水航走する。なお、航走体７は人が操縦しても良く、この場合には既定航路を航走しても良いし、現在位置Ｐｃを確認しながら任意の航路を航走しても良い。また、慣性航法装置８は、上記のセンサ７ｓの検出結果を利用して位置の算出を行っても良い。

以下、位置推定装置１について、詳細に説明する。
図１に示すように、位置推定装置１は、第１記憶部２ａと、第２記憶部２ｂと、候補経路生成部３と、評価値算出部４と、位置特定部５と、を備える。位置推定装置１が備える上記の機能部について、それぞれ説明する。

なお、位置推定装置１は、コンピュータで構成されても良く、図示しないＣＰＵ（プロセッサ）や、ＲＯＭやＲＡＭといったメモリや外部記憶装置などとなる記憶装置ｍを備えている。そして、メモリ（主記憶装置）にロードされたプログラム（位置推定プログラム）の命令に従ってＣＰＵが動作（データの演算など）することで、位置推定装置１が備える上記の各機能部を実現する。換言すれば、上記の位置推定プログラムは、コンピュータに後述する各機能部を実現させるためのソフトウェアであり、コンピュータによる読み込みが可能な記憶媒体に記憶されても良い。

第１記憶部２ａは、航走体７が有する１以上のセンサ７ｓおよび慣性航法装置８からそれぞれ出力される、規定タイミングの到来時毎のセンサ７ｓによる検出情報（以下、実センサ情報Ｓｔ）および慣性航法装置８による航走体７の位置の算出結果を記憶するよう構成される記憶部である。規定タイミングは例えば周期的なタイミングであり、図２に示すように、規定タイミングが到来する度に、その到来時におけるセンサ７ｓによるセンシング対象の検出結果（検出値など）である検出情報が実センサ情報Ｓｔとして第１記憶部２ａに記憶される。例えば、航走体７が複数種類のセンサ７ｓを備える場合には、規定タイミングの到来時毎の実センサ情報Ｓｔは、航走体７が備える全てのセンサ７ｓによる複数種類の検出情報を含んでも良いし、全てのセンサ７ｓの少なくとも一部のセンサ７ｓによる１種類以上の検出情報を含んでも良い。同様に、規定タイミングが到来する度に、その到来時における慣性航法装置８によるＩＮＳ位置Ｐｂ（位置算出結果）が第１記憶部２ａに記憶される。

また、第１記憶部２ａには、最新のものから規定回数Ｎの前までの各規定タイミングの到来時における実センサ情報ＳｔおよびＩＮＳ位置Ｐｂが、時系列が分かるような状態で記憶される。具体的には、各実センサ情報Ｓｔが、検出時刻、あるいは規定タイミングの到来時を識別するための序数などの識別情報が時系列情報Ｔと共に記憶されても良い。また、上記の規定回数Ｎは、航走体７が自己位置の推定を開始してから到来した規定タイミングの到来数よりも少ない数である。図１に示す実施形態では、第１記憶部２ａは、位置推定装置１が備える記憶装置ｍの既定の記憶領域に形成されており、外部情報取得部６が、規定タイミングが到来する度に、最新の実センサ情報ＳｔおよびＩＮＳ位置Ｐｂを取得して、時系列情報Ｔと共に第１記憶部２ａに記憶するようになっている。

第２記憶部２ｂは、上述した各実センサ情報Ｓｔとの照合により位置特定を行うための参照センサ情報Ｓｒが位置座標に関連付けられた地図情報Ｍを記憶するよう構成される記憶部である。参照センサ情報Ｓｒは、１種類以上の外部環境情報を事前に検出した検出情報である。また、地図情報Ｍは、航走体７の航路を含む所定地域に仮想的に設定した座標系（２次元または３次元）の各座標に参照センサ情報Ｓｒを関連付けた情報である。つまり、地図情報Ｍの任意の座標を参照すれば、その座標で得られるべき高度や重力といった外部環境情報がわかる。よって、航走体７のセンサ７ｓで検出するセンシング対象と同種類の外部環境情報が参照センサ情報Ｓｒに含まれるのを前提に、実センサ情報Ｓｔと地図情報Ｍの参照センサ情報Ｓｒとを照合する（マップマッチング）ことで、自己位置の特定（推定）が可能である。すなわち、実センサ情報Ｓｔと一致すると言えるような参照センサ情報Ｓｒを有する地図情報Ｍ上の座標が自己位置である可能性が高い。図１に示す実施形態では、第２記憶部２ｂは、位置推定装置１が備える記憶装置ｍの既定の記憶領域に形成されている。

候補経路生成部３は、現在位置Ｐｃの推定時を含む複数回（２以上）の規定タイミングの到来時の各々における、慣性航法装置８の位置算出結果に基づく推定位置Ｐｅの時系列をそれぞれ示す複数の候補経路Ｒｓを生成するよう構成される機能部である。候補経路Ｒｓは、現在位置Ｐｃの候補位置Ｐｓおよび、現在位置Ｐｃの推定時から規定回数Ｎだけ前の各規定タイミングの到来時における航走体７の推定位置Ｐｅを含む。つまり、候補経路Ｒｓは、上記の複数回（Ｎ＋１回）の規定タイミングの到来時の各々における推定位置Ｐｅの時系列が分かる情報である。

慣性航法装置８（慣性航法）により算出される位置には積分誤差があるため、航走体７の実際の位置は、ＩＮＳ位置Ｐｂおよびその積分誤差を考慮した範囲内のいずれかの座標である可能性がある。つまり、慣性航法装置８による位置算出結果に基づいて、ＩＮＳ位置Ｐｂに加えて、ＩＮＳ位置Ｐｂを中心とした誤差半径の円（図２の破線円）で囲われるような誤差範囲Ａｅの内側にある複数の推定位置Ｐｅが得られる。よって、各候補経路Ｒｓは、複数の規定タイミングの到来時の各々における誤差範囲Ａｅから１つずつ選択された位置座標を時系列で並べることで、候補経路Ｒｓが生成可能である。

そして、後述するように、複数の候補経路Ｒｓの中から１つの候補経路Ｒｓが最も確からしいものとして選択されると共に、選択された候補経路Ｒｓにおける候補位置Ｐｓ（時系列で最新の位置）が現在位置Ｐｃとして推定される。この複数の候補経路Ｒｓの具体的な生成手法については後述する。なお、上記の誤差半径は、慣性航法装置８の仕様（カタログスペックなど）に基づいて定めても良い。また、図２のｔｉは、規定タイミングのｉ番目の到来時（推定時刻など）を示すが、上記の積分誤差は推定回数が増えるほど大きくなり、誤差範囲Ａｅ（破線円の面積）は時間の経過（ｔ_１＜ｔ_２＜ｔ_３）と共に大きくなる。

評価値算出部４は、複数の候補経路Ｒｓがそれぞれ有する複数の推定位置Ｐｅの各々における、上記の地図情報Ｍに基づく参照センサ情報Ｓｒの時系列（以下、参照センサ時系列Ｃｒ）および実センサ情報Ｓｔの時系列（以下、実センサ時系列Ｃｔ）に基づいて、複数の候補経路Ｒｓの各々の確からしさの評価値Ｖを算出するよう構成される機能部である。より具体的には、幾つかの実施形態では、図１に示すように、評価値算出部４は、入力などされた候補経路Ｒｓの参照センサ時系列Ｃｒを取得する第１取得部４１と、この候補経路Ｒｓの実センサ時系列Ｃｔを取得する第２取得部４２と、第１取得部４１および第２取得部４２によってそれぞれ取得された参照センサ時系列Ｃｒと実センサ時系列Ｃｔとの比較に基づいて算出される誤差Ｅの時系列である誤差列Ｃｅを算出する誤差列算出部４３と、この誤差列算出部４３によって算出された誤差列Ｃｅに基づいて評価値Ｖを算出する評価部４４と、を有しても良い。

既に説明した通り、各候補経路Ｒｓは、複数回の規定タイミングの到来時の各々における推定位置Ｐｅの時系列の情報を含む。よって、任意の候補経路Ｒｓが有する複数の推定位置Ｐｅの各々における参照センサ情報Ｓｒを地図情報Ｍから取得して時系列に並べることで、その候補経路Ｒｓに対応した参照センサ時系列Ｃｒが得られる。同様に、任意の候補経路Ｒｓが有する複数の推定位置Ｐｅの各々の推定時刻などが一致する実センサ情報Ｓｔを第１記憶部２ａから取得して時系列に並べることで、その候補経路Ｒｓに対応した実センサ時系列Ｃｔが得られる。そして、各候補経路Ｒｓで得られる参照センサ時系列Ｃｒと実センサ時系列Ｃｔとを比較することで、各推定位置Ｐｅ（各規定タイミング）における誤差Ｅを算出し、その誤差Ｅを時系列に並べることで誤差列Ｃｅが得られる。

この各推定位置Ｐｅでの誤差Ｅは、実センサ情報Ｓｔが１種類の検出情報のみを含む場合には、候補経路Ｒｓに含まれる各推定位置Ｐｅにおける実センサ情報Ｓｔと参照センサ情報Ｓｒとの差などの差異の演算値であっても良い。他方、実センサ情報Ｓｔが複数種類の検出情報を含む場合には、各推定位置Ｐｅでの誤差Ｅは、各推定位置Ｐｅにおける実センサ情報Ｓｔと参照センサ情報Ｓｒとのセンシング対象の種類毎の差異を、その種類の違いを調整するための係数で調整するなどした上で加算などした演算値であっても良い。この係数は、センシング対象の種類毎に全て同じであっても良い。

そして、評価値算出部４は、この誤差列Ｃｅに基づいて、各候補経路Ｒｓの評価値Ｖを算出する。具体的には、この評価値Ｖは、誤差列Ｃｅに含まれる各推定位置Ｐｅの誤差Ｅを合計するなどして、算出しても良い。各推定位置Ｐｅの誤差に重み付け係数を乗算して合計しても良い。この場合、評価値Ｖは誤差が大きいほど大きくなるので、誤差列Ｃｅにおける誤差Ｅの合計の逆数を算出するなどすることで、評価値Ｖの値は確からしさが高いほど大きくなるように算出されても良い。なお、逆に、評価値Ｖの値は確からしさが高いほど小さくなるように算出されても良い。

位置特定部５は、複数の候補経路Ｒｓの各々の評価値Ｖに基づいて現在位置Ｐｃを特定するよう構成される機能部である。本実施形態では、評価値Ｖが高い候補経路Ｒｓほど確からしいので、複数の候補経路Ｒｓのうちから例えば評価値Ｖが最も高い候補経路Ｒｓを選択し、選択した候補経路Ｒｓの最新の推定位置Ｐｅを現在位置Ｐｃとして特定する。

図１に示す実施形態では、候補経路生成部３と評価値算出部４とが接続されており、候補経路生成部３が推定した複数の候補経路Ｒｓが評価値算出部４に入力されるようになっている。評価値算出部４において、第１取得部４１が、候補経路生成部３から入力される各候補経路Ｒｓの有する複数の推定位置Ｐｅの各々に対応する参照センサ情報Ｓｒを第２記憶部２ｂから取得し、これらの参照センサ情報Ｓｒの時系列が分かるように記憶することで、参照センサ時系列Ｃｒを取得する。同様に、第２取得部４２が、候補経路生成部３から入力される各候補経路Ｒｓの有する複数の推定位置Ｐｅの各々に対応する実センサ情報Ｓｔを第１記憶部２ａから取得し、これらの実センサ情報Ｓｔの時系列が分かるように記憶することで、実センサ時系列Ｃｔを取得する。こうして得られた、１つの候補経路Ｒｓについて参照センサ時系列Ｃｒおよび実センサ時系列Ｃｔが、第１取得部４１および第２取得部４２に接続された誤差列算出部４３に入力されて誤差列Ｃｅが算出される。この誤差列算出部４３は評価部４４に接続されており、評価部４４において、誤差列算出部４３から入力された誤差列Ｃｅの評価値Ｖが、確からしさが高いほど大きくなるように算出される。これによって、各候補経路Ｒｓの評価値Ｖを適切に算出することが可能となる。

上記の構成によれば、航走体７が通過した可能性のある複数の候補経路Ｒｓを、慣性航法による位置算出結果から得られる規定タイミングの到来時毎の複数の候補位置のいずれかを選択して時系列で並べるなどして生成する。こうして生成した各候補経路Ｒｓを地図情報と照合することで、各候補経路Ｒｓを航走体７が通過した場合に得られるはずのセンサ情報の時系列（参照センサ時系列Ｃｒ）を得ると共に、この参照センサ時系列Ｃｒと推定時刻などの規定タイミングを同じくする実センサ時系列Ｃｔとを比較することで、誤差列Ｃｅを算出するなどして、各候補経路Ｒｓの確からしさを評価する。そして、この評価値Ｖが最も高い候補経路Ｒｓに含まれる最新の位置を現在位置とするなどして、現在位置を特定（推定）する。

換言すれば、航走体７の現在位置Ｐｃを、実際に測定等により検出された複数の規定タイミングの到来時における実センサ情報Ｓｔを期待値として、現在位置の推定時の参照センサ情報Ｓｒの一致度のみならず、さらに１回以上遡った規定タイミングの到来時おける参照センサ情報Ｓｒとの一致度を考慮して、実センサ情報Ｓｔの時系列と一致度の高い候補経路Ｒｓに基づいて航走体７の現在位置Ｐｃを推定する。このように、慣性航法に基づいて生成した複数の候補経路の各位置に対応した実センサ情報Ｓｔと参照センサ情報Ｓｒとの比較を通して、候補経路Ｒｓの全体と地図情報Ｍとのマッチングを行うことで、ＧＰＳを用いることなく、航走体７の現在位置Ｐｃを適切に推定することができる。

また、慣性航法による位置算出結果（ＩＮＳ位置Ｐｂ）をマップマッチング技術により補正する場合には、地図情報Ｍ（参照センサ情報Ｓｒ）とセンサ７ｓによる検出情報とを照合して自己位置を推定するが、参照センサ情報Ｓｒの変化が乏しい（平坦な地形等）領域を航走する場合、その照合による位置の絞り込みは困難であるが、上述したように候補経路Ｒｓの生成を通して推定を行うことで、平坦な地形等の領域に途中から航走体７が進入したとしても、わずかな起伏を検出可能な高精度の計測器を用いることなく、より高い精度で現在位置を推定することができる。

次に、上述した候補経路生成部３による複数の候補経路Ｒｓの生成手法に関する幾つかの実施形態について、図３～図５を用いて説明する。図３は、本発明の一実施形態に係る候補経路生成部３の機能を示すブロック図であり、候補位置Ｐｓに基づいて候補経路を生成する。図４は、本発明の一実施形態に係る候補経路生成部３の機能を示すブロック図であり、候補経路Ｒｓに基づいて新たな候補経路Ｒｓを生成する。また、図５は、本発明の一実施形態に係る候補経路生成部３の機能を示すブロック図であり、推定位置Ｐｅの総当たりにより新たな候補経路Ｒｓを生成する。

幾つかの実施形態では、図３に示すように、候補経路生成部３は、現在位置Ｐｃの候補となる複数の候補位置Ｐｓを取得する候補位置取得部３１と、複数の候補位置Ｐｓの各々から、規定回数Ｎ前までの規定タイミングの到来時の各々における推定位置Ｐｅをそれぞれ算出することにより、複数の候補経路Ｒｓを算出する候補経路算出部３２と、を有しても良い。既に説明したように、慣性航法装置８による位置算出結果から、ＩＮＳ位置Ｐｂを中心とした誤差範囲Ａｅに航走体７が存在する可能性が見いだせる。よって、候補位置取得部３１は、現在位置Ｐｃの推定時における慣性航法装置８の位置算出結果の誤差範囲Ａｅ内に含まれる少なくとも一部の位置座標をそれぞれ候補位置Ｐｓとする。すなわち、候補位置Ｐｓは、慣性航法装置８から出力されたＩＮＳ位置Ｐｂと、記憶装置ｍなどに記憶された誤差の仕様に基づく誤差半径と、地図情報Ｍとを重ね合わせことにより得られる全ての位置座標のうちから抽出される。

また、候補経路算出部３２は、候補位置取得部３１によって取得された複数の候補位置Ｐｓの各々を終点とする１以上の候補経路Ｒｓを算出する。
より具体的には、幾つかの実施形態では、候補経路Ｒｓの生成にあたって、任意の推定位置Ｐｅから１つ前の規定タイミングの到来時における推定位置Ｐｅを算出可能な状態遷移行列Ｍｔを用いても良い。この場合、任意の候補位置Ｐｓを初期位置として、この初期位置から状態遷移行列Ｍｔにより規定タイミングの到来時における位置（推定位置Ｐｅ）を過去に順番に遡るようにして算出することで、候補経路Ｒｓを生成しても良い。これによって、初期位置である候補位置Ｐｓを終点とする候補経路Ｒｓを生成することが可能である。

この状態遷移行列Ｍｔは、航走体７の位置と、その位置における速度や進行方向、航走体７の重量などの航走時の条件から次はどこにいるか（状態遷移）を算出することが可能な行列である。状態遷移行列Ｍｔはカルマンフィルタや、ＭＨＥ（Ｍｏｖｉｎｇ Ｈｏｒｉｚｏｎ Ｅｓｔｉｍａｔｉｏｎ）であっても良い。例えば、状態遷移行列Ｍｔの逆行列と推定位置Ｐｅとを行列演算（Ｍｔ^－１×Ｐｅ）することで、その推定位置Ｐｅの１つ前の規定タイミングの到来時における位置座標を求めることが可能である。

よって、複数の候補位置Ｐｓ（初期位置）について、それぞれ、候補位置Ｐｓおよび状態遷移行列Ｍｔを用いて、候補位置Ｐｓから１つ前の規定タイミングの到来時における推定位置Ｐｅを求める。さらに、求めた推定位置Ｐｅおよび状態遷移行列Ｍｔを用いて、さらに１つ前の規定タイミングの到来時における推定位置Ｐｅを求める。このような状態遷移行列Ｍｔを用いた行列演算を規定回数Ｎだけ繰り返すことで、規定回数Ｎの前までの規定タイミングの到来時の各々における推定位置Ｐｅを求めることで、任意の候補位置Ｐｓに対応する候補経路Ｒｓを得ることが可能となる。

この実施形態においては、最初に複数の候補位置Ｐｓを決めてから、候補位置Ｐｓ毎に状態遷移行列Ｍｔを用いて候補経路Ｒｓを生成しても良い。あるいは、慣性航法装置８による現在位置Ｐｃの推定時における例えばＩＮＳ位置Ｐｂなど、１つの候補位置Ｐｓのみを決めて、この候補位置Ｐｓに対応する候補経路Ｒｓを作成した後、次の候補位置Ｐｓを決めるということを終了条件が満たされるまで繰り返すことで、複数の候補経路Ｒｓを生成しても良い。この終了条件が満たされる場合は、例えば、生成した候補経路Ｒｓの誤差列Ｃｅを算出し、誤差列Ｃｅの評価値Ｖが閾値を下回った場合や、所定数Ｌの候補経路Ｒｓを生成した場合であっても良い。また、次の候補位置Ｐｓを決定するにあたっては、後述するような所定のアルゴリズムを用いても良い。これによって、複数の適切な候補経路Ｒｓを生成することができる。

他の幾つかの実施形態では、図４に示すように、任意の候補経路Ｒｓを初期経路から候補経路Ｒｓの生成を開始し、上述したような終了条件が満たされるまで、所定のアルゴリズムにより次の候補経路Ｒｓを作成し、これを繰り返すことで複数の候補経路Ｒｓを生成しても良い。例えば、現在位置Ｐｃの推定時のＩＮＳ位置Ｐｂ、およびそこから規定回数Ｎだけ前の規定タイミングの到来時の各々におけるＩＮＳ位置Ｐｂの時系列を候補経路Ｒｓの初期経路として開始しても良い。また、上記の所定のアルゴリズムとして遺伝的アルゴリズムや内点法、ニュートン法等の最適化アルゴリズムなどを用いることにより、評価値Ｖが最大となるように候補経路Ｒｓを作成することが可能となる。

図４に示す実施形態では、候補経路生成部３は、任意の候補経路Ｒｓを取得する候補経路取得部３３と、候補経路取得部３３によって取得された候補経路Ｒｓに基づいて、新たな候補経路Ｒｓ´を算出する生成部３４と、を有する。つまり、本実施形態の候補経路生成部３は、１つの候補経路Ｒｓが入力されると、この入力された１つの候補経路Ｒｓに対応した参照センサ時系列Ｃｒおよび実センサ時系列Ｃｔを求めて誤差列Ｃｅを算出すると共に、入力された１つの候補経路Ｒｓから上記の所定のアルゴリズムを用いて、求めた誤差列Ｃｅに基づく評価値Ｖが最小となるように、新たな候補経路Ｒｓ´を生成する。

このような候補経路生成部３に対して初期値となる候補経路Ｒｓを入力すれば新たな候補経路Ｒｓ´が生成できるので、生成された新たな候補経路Ｒｓ´を候補経路生成部３に再度入力するといったことを、上述したような終了条件が満たされるまで繰り返せば、複数の候補経路Ｒｓが得られる。図４に示す実施形態では、初期経路となる候補経路ＲｓはＩＮＳ位置Ｐｂの時系列であり、この初期経路を候補経路生成部３および評価値算出部４にそれぞれ入力し、その結果得られた新たな候補経路Ｒｓ´および評価値Ｖが記憶される。この際、終了条件を満たさない場合には、この生成された新たな候補経路Ｒｓ´が候補経路取得部３３に入力されて、次の新たな候補経路Ｒｓが生成された後、評価値算出部４による評価値Ｖが行われ、記憶される。これを終了条件が満たされるまで繰り返すことで、複数の候補経路Ｒｓが生成される。これによって、複数の適切な候補経路Ｒｓを生成することができる。

その他の幾つかの実施形態では、図５に示すように、候補経路生成部３は、複数回の規定タイミングの到来時の各々において、それぞれ、慣性航法装置８の位置算出結果に基づいて得られる全ての推定位置Ｐｅのうちから所定数の推定位置Ｐｅを抽出する候推定位置抽出部３５と、候推定位置抽出部３５によって抽出された各々の規定タイミングの到来時の所定数の推定位置Ｐｅを総当たりで組み合わせることで、複数の候補経路Ｒｓを生成する組合せ部３６と、を有しても良い。なお、複数の候補経路Ｒｓが有する推定位置Ｐｅの全ては慣性航法装置８による推定により求められた推定位置Ｐｅであり、全ての候補経路Ｒｓはいずれも実際に航走体７が通過した可能性のある経路である。

つまり、現在位置Ｐｃの推定時および、そこから規定回数Ｎだけ前の各々の規定タイミングの到来時におけるＩＮＳ位置Ｐｂを中心とする誤差範囲Ａｅ内から１以上の所定数の位置座標をそれぞれ抽出する。ｉ番目（ｉ＝１、２、・・・、Ｎ、Ｎ＋１）の規定タイミングの到来時において抽出された推定位置Ｐｅの数をｎｉで表すと、生成される候補経路Ｒｓの総数は、ｎ_１×ｎ_２×・・・×ｎ_Ｎ＋１となる。この際、例えばｎｉを、慣性航法装置８の位置算出結果に基づいて得られる全ての推定位置Ｐｅとすれば、航走体７の位置として可能性のある全ての候補経路Ｒｓを生成される。

図５に示す実施形態では、候推定位置抽出部３５が、複数回の規定タイミングの到来時の各々から、それぞれ所定数の推定位置Ｐｅを抽出した後、規定タイミング毎の推定位置Ｐｅのグループがどの規定タイミングでのものか判別可能なように時系列情報Ｔと関連付けた状態で、組合せ部３６に入力するようになっている。ただし、本実施形態に本発明は限定されない。他の幾つかの実施形態では、例えば、複数の規定タイミングの各々の到来時におけるＩＮＳ位置Ｐｂの時系列で構成される候補経路Ｒｓを初期経路とし、この初期経路の任意の推定位置Ｐｅを、慣性航法装置８による誤差範囲内において、既に生成した候補経路Ｒｓと全体として一致することがないように任意に変えることで、新たな候補経路Ｒｓを生成しても良い。上述したような所定のアルゴリズムを用いて新たな候補経路を生成しても良い。そして、終了条件がみたされるまで、この候補経路Ｒｓの生成を行っても良い。

これによって、例えば複数の規定タイミングの到来時の各々における航走体７の位置として可能性のある全ての推定位置Ｐｅを抽出ると共に、抽出した各規定タイミングの到来時毎の複数の推定位置Ｐｅ同士の総当たりにより候補経路Ｒｓを生成することで、想定される全ての候補経路Ｒｓを網羅的に生成することもできる。

以下、上述した位置推定装置１に対応した位置推定方法を、図６を用いて説明する。図６は、本発明の一実施形態に係る位置推定方法を示す図である。

位置推定方法は、航走体７の現在位置Ｐｃを推定する方法である。図６に示すように、位置推定方法は、第１記憶ステップ（Ｓ２）と、第２記憶ステップ（Ｓ１）と、候補経路生成ステップ（Ｓ４）と、評価値算出ステップ（Ｓ５）と、位置特定ステップ（Ｓ６）と、を備える。位置推定方法を図６のフローに従って説明する。なお、以下で説明するステップＳ２～Ｓ６は、規定タイミングが到来する度に実行される。

図６のステップＳ１において、第２記憶ステップを実行する。第２記憶ステップは、上述した地図情報Ｍを記憶するステップである。本ステップは、地図情報Ｍを利用可能に準備するステップであり、航走体７の航走開始前など事前に第１記憶部２ａに記憶する。

図６のステップＳ２において、第１記憶ステップを実行する。第１記憶ステップは、規定タイミングの到来時毎の上述した実センサ情報Ｓｔおよび慣性航法装置８による位置算出結果を記憶するステップである。例えば、周期的に到来する規定タイミングの到来時に検出された実センサ情報Ｓｔ、および慣性航法装置８により算出されたＩＮＳ位置Ｐｂを取得し、第２記憶部２ｂに記憶する。

図６のステップＳ４において、候補経路生成ステップを実行する。候補経路生成ステップは、上述した複数の候補経路Ｒｓを生成するステップである。この候補経路生成ステップは、既に説明した候補経路生成部３が実行する処理内容と同様であるため、詳細は省略する。なお、候補経路生成ステップは、規定タイミングが規定回数Ｎよりも多く到来することで、第１記憶ステップの実行により記憶された実センサ情報Ｓｔの数が規定回数Ｎよりも多くなった後（ステップＳ３でＹｅｓ）に実行される。

図６のステップＳ５において、評価値算出ステップを実行する。評価値算出ステップは、上述した複数の候補経路Ｒｓを生成するステップである。この候補経路生成ステップは、複数の候補経路Ｒｓがそれぞれ有する複数の推定位置Ｐｅの各々における、上記の地図情報Ｍに基づく参照センサ時系列Ｃｒおよび実センサ時系列Ｃｔに基づいて、複数の候補経路Ｒｓの各々の確からしさの評価値Ｖを算出するステップである。評価値算出ステップは、既に説明した評価値算出部４が実行する処理内容と同様であるため、詳細は省略する。

図６のステップＳ６において、位置特定ステップを実行する。位置特定ステップは、複数の候補経路Ｒｓの各々の評価値Ｖに基づいて現在位置Ｐｃを特定するステップである。この位置特定ステップは、既に説明した位置特定部５が実行する処理内容と同様であるため、詳細は省略する。

図６に示す実施形態では、航走体７が備える記憶装置ｍに地図情報Ｍを事前に第２記憶部２ｂに記憶しておく。その後、航走体７の航走が開始されると、ステップＳ２において、規定タイミングが到来する度に、その到来時の実センサ情報ＳｔおよびＩＮＳ位置Ｐｂを第１記憶部２ａに記憶する。そして、ステップＳ３において、実センサ情報Ｓｔの数を確認し、その数が規定回数Ｎを超えた場合に、ステップＳ４以降を実行する。

ステップＳ４では、複数の候補経路Ｒｓを生成し、ステップＳ５において、複数の候補経路Ｒｓの各々の評価値Ｖを、各候補経路Ｒｓの参照センサ時系列Ｃｒおよび実センサ時系列Ｃｔに基づいて算出する。具体的には、誤差列Ｃｅを算出し、その誤差列Ｃｅを構成する誤差Ｅの合計を評価値Ｖとして算出しても良い。そして、ステップＳ６において、ステップＳ５で算出された複数の候補経路Ｒｓの各々の評価値Ｖに基づいて現在位置Ｐｃを特定し、航走体７の位置推定結果とする。その後、ステップＳ６で得られた位置推定結果が航走制御装置９に送信される。

本発明は上述した実施形態に限定されることはなく、上述した実施形態に変形を加えた形態や、これらの形態を適宜組み合わせた形態も含む。

１ 位置推定装置
２ａ 第１記憶部
２ｂ 第２記憶部
３ 候補経路生成部
３１ 候補位置取得部
３２ 候補経路算出部
３３ 候補経路取得部
３４ 生成部
３５ 候推定位置抽出部
３６ 組合せ部
４ 評価値算出部
４１ 第１取得部
４２ 第２取得部
４３ 誤差列算出部
４４ 評価部
５ 位置特定部
６ 外部情報取得部
７ 航走体
７ｓ センサ
８ 慣性航法装置
９ 航走制御装置
Ｃｔ 実センサ時系列
Ｃｒ 参照センサ時系列
Ｃｅ 誤差列
Ｅ 誤差
Ｖ 評価値
Ｍ 地図情報
Ｍｔ 状態遷移行列
Ｎ 規定回数
Ｒｓ 候補経路
Ｐｃ 現在位置
Ｐｅ 推定位置
Ｐｓ 候補位置
Ｐｂ ＩＮＳ位置
Ａｅ 誤差範囲（ＩＮＳ）
Ｓ センサ情報
Ｓｒ 参照センサ情報
Ｓｔ 実センサ情報
Ｔ 時系列情報
ｍ 記憶装置

Claims (7)

1. 航走体の現在位置を推定する位置推定装置であって、
   前記航走体が有するセンサおよび慣性航法装置からそれぞれ出力される規定タイミングの到来時毎の実センサ情報および位置算出結果を記憶するよう構成される第１記憶部と、
   前記実センサ情報との照合により位置特定を行うための参照センサ情報が位置座標に関連付けられた地図情報を記憶するよう構成される第２記憶部と、
   前記現在位置の推定時を含む複数回の前記規定タイミングの到来時の各々における前記位置算出結果に基づく推定位置の時系列をそれぞれ示す複数の候補経路を生成するよう構成される候補経路生成部と、
   前記複数の候補経路がそれぞれ有する複数の前記推定位置の各々における、前記地図情報に基づいて得られる前記参照センサ情報の時系列および前記実センサ情報の時系列に基づいて、前記複数の候補経路の各々の確からしさの評価値を算出するよう構成される評価値算出部と、
   前記複数の候補経路の各々の前記評価値に基づいて前記現在位置を特定するよう構成される位置特定部と、を備えることを特徴とする位置推定装置。
2. 前記評価値算出部は、
   前記候補経路の前記参照センサ情報の時系列を取得する第１取得部と、
   前記候補経路の前記実センサ情報の時系列を取得する第２取得部と、
   取得された前記参照センサ情報の時系列と前記実センサ情報の時系列との比較に基づいて算出される誤差の時系列である誤差列を算出する誤差列算出部と、
   前記誤差列に基づいて前記評価値を算出する評価部と、を有することを特徴とする請求項１に記載の位置推定装置。
3. 前記候補経路生成部は、
   前記現在位置の候補となる複数の候補位置を取得する候補位置取得部と、
   前記複数の候補位置の各々から、規定回数前までの前記規定タイミングの到来時の各々における前記推定位置をそれぞれ算出することにより、前記複数の候補経路を算出する候補経路算出部と、を有することを特徴とする請求項１または２に記載の位置推定装置。
4. 前記候補経路生成部は、
   前記候補経路を取得する候補経路取得部と、
   取得された前記候補経路に基づいて、新たな前記候補経路を算出する生成部と、を有することを特徴とする請求項１または２に記載の位置推定装置。
5. 前記候補経路生成部は、
   前記複数回の前記規定タイミングの到来時の各々において、それぞれ、所定数の前記推定位置を抽出する候推定位置抽出部と、
   抽出された各々の前記規定タイミングの到来時毎の前記所定数の推定位置を総当たりで組み合わせることで、前記複数の候補経路を生成する組合せ部と、を有することを特徴とする請求項１または２に記載の位置推定装置。
6. 慣性航法により航走体の位置を算出する慣性航法装置と、
   少なくとも１つのセンサと、
   前記慣性航法装置による位置算出結果、および前記少なくとも１つのセンサにより検出された検出情報である実センサ情報に基づいて、前記航走体の現在位置を推定する請求項１～５のいずれか１項に記載の位置推定装置と、
   前記位置推定装置によって推定された前記現在位置に基づいて前記航走体の航走を制御する航走制御装置と、を備えることを特徴とする航走体。
7. 航走体の現在位置を推定する位置推定方法であって、
   前記航走体が有するセンサおよび慣性航法装置からそれぞれ出力される規定タイミングの到来時毎の実センサ情報および位置算出結果を記憶する第１記憶ステップと、
   前記実センサ情報との照合により位置特定を行うための参照センサ情報が位置座標に関連付けられた地図情報を記憶する第２記憶ステップと、
   前記現在位置の推定時を含む複数回の前記規定タイミングの到来時の各々における前記位置算出結果に基づく推定位置の時系列をそれぞれ示す複数の候補経路を生成する候補経路生成ステップと、
   前記複数の候補経路がそれぞれ有する複数の前記推定位置の各々における、前記地図情報に基づいて得られる前記参照センサ情報の時系列および前記実センサ情報の時系列に基づいて、前記複数の候補経路の各々の確からしさの評価値を算出する評価値算出ステップと、
   前記複数の候補経路の各々の前記評価値に基づいて前記現在位置を特定する位置特定ステップと、を備えることを特徴とする位置推定方法。

Images