(57)【要約】【課題】移動体の移動状態を正しく算出するための新しい手法の提案。【解決手段】移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する。そして、第１の移動状態と、過去に算出した移動空間を移動する当該移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する。そして、拘束条件を用いて第１の移動状態を修正することによって、新たな第２の移動状態を算出する。【選択図】図１
2014048228.tif 000002
【特許請求の範囲】
【請求項１】
移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定することと、
前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定することと、
前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出することと、
を含む移動状態算出方法。
【請求項２】
前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を前記拘束条件として設定することを含む、
請求項１に記載の移動状態算出方法。
【請求項３】
前記設定することは、過去に算出した前記第２の移動状態の状態変化が直進傾向を示している場合には、継続して直進するように前記第１の移動状態を修正することに用いる条件を前記拘束条件として設定することを含む、
請求項２に記載の移動状態算出方法。
【請求項４】
前記設定することは、過去に算出した前記第２の移動状態の状態変化が旋回傾向を示している場合には、継続して旋回するように前記第１の移動状態を修正することに用いる条件を前記拘束条件として設定することを含む、
請求項２又は３に記載の移動状態算出方法。
【請求項５】
前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターを算定することを含み、
前記修正することは、前記パラメーターを用いて前記第１の移動状態を前記拘束条件を満たす状態に近づけることを含む、
請求項２〜４の何れか一項に記載の移動状態算出方法。
【請求項６】
前記設定することは、過去に算出した前記第２の移動状態を用いて基準移動方向を定めることを含み、
前記算定することは、前記第１の移動状態に含まれる移動方向と前記基準移動方向との差違に基づいて前記パラメーターを算定することを含む、
請求項５に記載の移動状態算出方法。
【請求項７】
移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する推定部と、
前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する設定部と、
前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出する修正部と、
を備えた移動状態算出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の移動状態を算出するための方法等に関する。
【背景技術】
【０００２】
移動空間を移動する移動体の位置や速度、移動方向といった諸量を含む移動状態を、状態空間における状態推定の手法を用いて推定する技術が考案されている。この状態推定の手法の中でも広く知られているのは、カルマンフィルターである。例えば特許文献１には、カルマンフィルターを利用して移動体の位置を推定する手法が記載されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−２６６４６８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
カルマンフィルターを利用した移動状態の推定では、誤差を含む観測情報を利用して、時々刻々と変化する移動体の移動状態を逐次的に推定する。観測情報としては、例えば外部のセンサー（例えばＧＰＳ（Global Positioning System）センサーや慣性センサー、地磁気センサー等のセンサー、以下包括的に「参照用センサー」と称す。）によって検出された参照情報が用いられる。
【０００５】
しかし、上記の手法では、移動状態の推定精度が、もっぱら参照用センサーの検出精度に依存してしまうという問題がある。つまり、参照用センサーの検出精度が低いと、正確でない参照情報が観測情報としてカルマンフィルターの演算で用いられることになるため、この観測情報に引きずられて、推定される移動状態の正確性が低下するという問題があった。
【０００６】
本発明は上述した課題に鑑みてなされたものであり、その目的とするところは、移動体の移動状態を正しく算出するための新しい手法を提案することにある。
【課題を解決するための手段】
【０００７】
以上の課題を解決するための第１の形態は、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定することと、前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定することと、前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出することと、を含む移動状態算出方法である。
【０００８】
また、他の形態として、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する推定部と、前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する設定部と、前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出する修正部と、を備えた移動状態算出装置を構成することとしてもよい。
【０００９】
この第１の形態等によれば、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する。そして、この第１の移動状態と、過去に算出した移動空間を移動する当該移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定し、当該拘束条件を用いて第１の移動状態を修正することによって、新たな第２の移動状態を算出する。第１の移動状態の推定は、公知の状態推定の手法（例えばカルマンフィルター）によって実現可能である。単純に第１の移動状態の推定を行うばかりでなく、所定の拘束条件を用いて第１の移動状態を修正することによって新たな第２の移動状態を算出することで、移動体の移動状態を従来よりも正しく算出することが可能となる。
【００１０】
また、第２の形態として、第１の形態の移動状態算出方法における前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を前記拘束条件として設定することを含む、移動状態算出方法を構成することとしてもよい。
【００１１】
この第２の形態によれば、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を拘束条件として設定する。
【００１２】
この場合の拘束条件としては、例えば第３の形態として、過去に算出した第２の移動状態の状態変化が直進傾向を示している場合には、継続して直進するように第１の移動状態を修正することに用いる条件を拘束条件として設定することができる。
【００１３】
この第３の形態によれば、直進傾向を示すように第１の移動状態を修正することによって、新たな第２の移動状態を算出することが可能となる。
【００１４】
また、例えば第４の形態として、過去に算出した第２の移動状態の状態変化が旋回傾向を示している場合には、継続して旋回するように第１の移動状態を修正することに用いる条件を拘束条件として設定するようにすることも可能である。
【００１５】
この第４の形態によれば、旋回傾向を示すように第１の移動状態を修正することによって、新たな第２の移動状態を算出することが可能となる。
【００１６】
また、第５の形態として、第２〜第４の何れかの形態における前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターを算定することを含み、前記修正することは、前記パラメーターを用いて前記第１の移動状態を前記拘束条件を満たす状態に近づけることを含む、移動状態算出方法を構成することとしてもよい。
【００１７】
この第５の形態によれば、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターを算定する。そして、当該パラメーターを用いて第１の移動状態を拘束条件を満たす状態に近づける。これにより、第１の移動状態を適切に修正することによって、新たな第２の移動状態を算出することが可能となる。
【００１８】
また、第６の形態として、第５の形態の移動状態算出方法における前記設定することは、過去に算出した前記第２の移動状態を用いて基準移動方向を定めることを含み、前記算定することは、前記第１の移動状態に含まれる移動方向と前記基準移動方向との差違に基づいて前記パラメーターを算定することを含む、移動状態算出方法を構成することとしてもよい。
【００１９】
この第６の形態によれば、過去に算出した第２の移動状態を用いて基準移動方向を定める。そして、第１の移動状態に含まれる移動方向と基準移動方向との差違に基づくことで、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターとして適切な値を設定することができる。
【図面の簡単な説明】
【００２０】
【図１】移動状態推定に係る手順を示すフローチャート。
【図２】（１）直進性条件の説明図。（２）適応度合パラメーターの設定方法の説明図。
【図３】（１）平滑化条件の説明図。（２）適応度合パラメーターの設定方法の説明図。
【図４】位置算出装置の機能構成の一例を示す図。
【図５】位置算出処理の流れを示すフローチャート。
【図６】（１）直進性条件を適用した場合の位置算出結果の一例を示す図。（２）平滑化条件を適用した場合の位置算出結果の一例を示す図。
【発明を実施するための形態】
【００２１】
１．原理
１−１．総論
最初に、変数等の定義を行う。本実施形態では、移動体の移動空間を状態空間とする。移動空間における移動状態を「ｘ」を用いて表記し、時刻「ｋ」における移動状態を「ｘk」と表記する。また、観測値を「ｙ」を用いて表記し、時刻「ｋ」において取得された観測値を「ｙk」と表記する。また、時刻ｋまでの観測値でなる観測値の集合を観測信号と称し、「Ｙk＝｛ｙ0，ｙ1，・・・，ｙk｝」と表記する。
【００２２】
従来から知られている状態推定の手法を移動状態の推定に適用する場合、移動状態の予測と移動状態の補正とを繰り返すことによって逐次的に移動状態を推定する。移動状態の予測は、１時刻前に推定された移動状態から現在の移動状態を予測する。これは、過去の観測から未来を推論することを意味する。時刻ｋにおける移動状態ｘkの予測は、時刻ｋ−１までの観測信号Ｙk-1＝｛ｙ0，ｙ1，・・・，ｙk-1｝が与えられたときの移動状態ｘkの条件付き確率密度関数を用いて、次式（１）のように表現することができる。
【数１】
2014048228.tif 000003
【００２３】
移動状態の補正は、移動状態の予測で得られた予測値を、新たに取得した観測値を用いて補正する。具体的には、時刻ｋにおいて新たに取得した観測値ｙkを用いて移動状態ｘkを補正する。観測信号Ｙk-1＝｛ｙ0，ｙ1，・・・，ｙk-1｝が既に与えられている状態で新たに観測値ｙkが取得されるため、観測信号はＹk＝｛ｙ0，ｙ1，・・・，ｙk｝となる。この場合、移動状態ｘkの補正は、観測信号Ｙkが与えられたときの移動状態ｘkの条件付き確率密度関数を用いて、次式（２）のように表現することができる。
【数２】
2014048228.tif 000004
【００２４】
ここまでの移動状態の推定演算は、従来公知の状態推定手法を適用したに過ぎない。本実施形態では、上記の移動状態の推定演算を行うことで推定された移動状態（以下、「第１の移動状態」と称す。）と、過去に算出した移動状態（以下、「第２の移動状態」と称す。）とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する。そして、設定した拘束条件を用いて第１の移動状態を修正することによって、新たな第２の移動状態を算出する。これが本実施形態の大きな特徴の１つである。
【００２５】
拘束条件を導入するため、移動状態ｘkの条件付き確率密度関数の変数として、拘束条件を表す変数を導入する。拘束条件については詳細に後述するが、ここでは、拘束条件を規定するために必要なパラメーターとして、拘束条件規定値「ｄ」と称するパラメーターを導入する。時刻「ｋ」における拘束条件規定値を「ｄk」と表記する。また、時刻ｋまでの拘束条件規定値でなる拘束条件規定値の集合を拘束条件信号と称し、「Ｄk＝｛ｄ0，ｄ1，・・・，ｄk｝」と表記する。
【００２６】
図１は、本実施形態における移動状態算出に係る手順（移動状態算出方法）を示すフローチャートである。
最初に、移動状態推定処理を行う（ステップＡ１）。移動状態推定処理では、前述したように、移動状態の予測（ステップＡ３）と、移動状態の補正（ステップＡ５）との２つを行う。
【００２７】
ステップＡ３では、１時刻前に算出された第２の移動状態を用いて、現在の時刻における移動状態ｘを予測する。具体的には、時刻ｋにおける移動状態ｘkの予測は、時刻ｋ−１までの観測信号Ｙk-1＝｛ｙ0，ｙ1，・・・，ｙk-1｝と、時刻ｋ−１までの拘束条件信号Ｄk-1＝｛ｄ0，ｄ1，・・・，ｄk-1｝とが与えられたときの移動状態ｘkの条件付き確率密度関数を用いて、次式（３）のように表現することができる。
【数３】
2014048228.tif 000005
【００２８】
ステップＡ５では、ステップＡ３で予測した移動状態ｘkを、新たに取得した観測値ｙkを用いて補正する。時刻ｋ−１までの観測信号Ｙk-1＝｛ｙ0，ｙ1，・・・，ｙk-1｝が与えられている状態で新たに観測値ｙkが取得されることで、観測信号はＹk＝｛ｙ0，ｙ1，・・・，ｙk｝となる。この場合、移動状態ｘkの補正は、観測信号Ｙkと、時刻ｋ−１までの拘束条件信号Ｄk-1とが与えられたときの移動状態ｘkの条件付き確率密度関数を用いて、次式（４）のように表現することができる。
【数４】
2014048228.tif 000006
【００２９】
ステップＡ１の移動状態推定処理の後、拘束条件を適用するか否かを判定する（ステップＡ７）。拘束条件を適用しないと判定した場合は（ステップＡ７；Ｎｏ）、移動状態推定処理で推定した移動状態（第１の移動状態）を、新たな移動状態（第２の移動状態）として算出する（ステップＡ１７）。そして、ステップＡ１９へと移行する。
【００３０】
一方、拘束条件を適用すると判定した場合は（ステップＡ７；Ｙｅｓ）、移動状態修正処理を行う（ステップＡ９）。移動状態修正処理では、最初に拘束条件を設定する（ステップＡ１１）。時刻ｋ−１までの拘束条件信号Ｄk-1＝｛ｄ0，ｄ1，・・・，ｄk-1｝が与えられている状態で時刻ｋにおいて新たに拘束条件が設定されることで、拘束条件信号はＤk＝｛ｄ0，ｄ1，・・・，ｄk｝となる。
【００３１】
次いで、ステップＡ１１で設定した拘束条件を用いて、移動状態推定処理で推定した移動状態（第１の移動状態）を修正する（ステップＡ１３）。この第１の移動状態の修正は、観測信号Ｙkと、拘束条件信号Ｄkとが与えられたときの移動状態ｘkの条件付き確率密度関数を用いて、次式（５）のように表現することができる。
【数５】
2014048228.tif 000007
【００３２】
移動状態修正処理を行ったならば、修正した移動状態（第１の移動状態）を、新たな移動状態（第２の移動状態）として算出する（ステップＡ１５）。
【００３３】
ステップＡ１５又はＡ１７の後、移動状態の算出を終了するか否かを判定し（ステップＡ１９）、移動状態の算出を継続すると判定した場合は（ステップＡ１９；Ｎｏ）、ステップＡ１に戻る。また、移動状態の算出を終了すると判定した場合は（ステップＡ１９；Ｙｅｓ）、移動状態の算出に係る処理を終了する。
【００３４】
１−２．各論
次に、図１の各ステップで行われる処理について詳細に説明する。
【００３５】
１−２−１．移動状態推定処理
図１では、ステップＡ１の移動状態推定処理における移動状態の予測及び補正を、それぞれ移動状態ｘkの条件付き確率密度関数を用いて表現した。移動状態の予測を表す式（３）と移動状態の補正を表す式（４）とにそれぞれガウス性の仮定を導入することで、移動状態推定処理を、カルマンフィルターを用いた移動状態推定処理として実現することができる。
【００３６】
カルマンフィルターにおける状態方程式は、次式（６）のように表現される。
【数６】
2014048228.tif 000008
【００３７】
式（６）において、「Ｆk」は移動状態ｘの時間遷移に関する線形モデルである。「ｗk」はシステムのノイズであり、共分散Ｑk且つ零平均の多変数正規分布に従う。つまり、「ｗk〜Ｎ（０，Ｑk）」である。
【００３８】
また、カルマンフィルターにおける観測方程式は、次式（７）のように表現される。
【数７】
2014048228.tif 000009
【００３９】
式（７）において、「Ｈk」は状態空間を観測空間に線形写像する観測モデルである。「ｖk」は観測値のノイズであり、共分散Ｒk且つ零平均の多変数正規分布に従う。つまり、「ｖk〜Ｎ（０，Ｒk）」である。
【００４０】
カルマンフィルターを用いた移動状態推定処理では、式（６）で表される状態方程式と式（７）で表される観測方程式とに基づいて、移動状態ｘの予測及び補正を行う。なお、カルマンフィルターの予測及び補正に係る演算式は従来公知であるため、ここでは説明を省略する。
【００４１】
１−２−２．移動状態修正処理
移動状態修正処理では、拘束条件を用いて第１の移動状態を修正する。本実施形態では、拘束条件を、次式（８）に示すような拘束方程式によって記述する。
【数８】
2014048228.tif 000010
【００４２】
式（８）において、「ｄk」は拘束条件規定値であり、「ｇk（・）」は拘束条件を与えるモデル関数（以下、「拘束条件モデル関数」と称す。）である。「εk」は拘束条件の誤差項である。本実施形態では、この拘束条件の誤差項εkを、共分散Ｓk且つ零平均の多変数正規分布に従うと仮定する。つまり、「εk〜Ｎ（０，Ｓk）」と仮定する。
【００４３】
拘束条件モデル関数ｇk（・）は、設定する拘束条件の種類に応じて個別に設定される。この拘束条件モデル関数ｇk（・）には、線形の関数のみならず、非線形の関数を適用することができる。非線形の関数をモデル関数として設定する場合、そのままの形では計算が困難であるため、モデル関数を線形化する。
【００４４】
時刻ｋにおいて移動状態推定処理で推定された移動状態を「ｘ+k」と表記する。このとき、この既知の移動状態ｘ+kの近傍で拘束条件モデル関数ｇk（・）を１次の項までテイラー展開すると、次式（９）が導出される。
【数９】
2014048228.tif 000011
但し、「Ｇk」はｇk（ｘk）をｘkで偏微分することで得られる行列（以下、「拘束行列」と称す。）である。
【００４５】
このとき、式（９）を式（８）に代入して式を整理すると、次式（１０）が導かれる。
【数１０】
2014048228.tif 000012
【００４６】
式（１０）は、カルマンフィルターにおいて外部入力ｕkを受ける状態方程式と同じ形となっている。そこで、カルマンフィルターに基づき、式（１０）から移動状態ｘkを修正するための修正演算式を導出する。
【００４７】
その結果、次式（１１）〜（１３）に示すような修正演算式が導出される。
【数１１】
2014048228.tif 000013
【数１２】
2014048228.tif 000014
【数１３】
2014048228.tif 000015
【００４８】
但し、「Ｌk」は移動状態ｘkを修正するための補正係数（ゲイン）であり、「Ｐk」は移動状態ｘkの誤差共分散である。「Ｉ」は単位行列である。上付きの添え字の「＋」は、移動状態推定処理で推定された値を意味する。つまり、「ｘ+k」は移動状態推定処理で推定された移動状態を意味し、「Ｐ+k」は移動状態推定処理で推定された誤差共分散を意味する。
【００４９】
図１の移動状態修正処理のステップＡ１３では、式（１１）〜式（１３）で表される修正演算式に従って、ステップＡ１の移動状態推定処理で推定された第１の移動状態を修正する。
【００５０】
１−２−３．拘束条件の設定
本実施形態では、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を拘束条件として設定する。式（１１）〜式（１３）に従って第１の移動状態を修正するためには、拘束条件モデル関数ｇk（ｘk）と、拘束条件規定値ｄkと、共分散Ｓkとを定める必要がある。
【００５１】
拘束条件モデル関数ｇk（ｘk）と拘束条件規定値ｄkとによって、拘束条件を表す拘束方程式（式（８））が定められる。また、共分散Ｓkによって、移動状態推定処理で推定された第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度が決定付けられる。
【００５２】
共分散Ｓkを大きくすると、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を低くする作用が働く。なぜならば、式（１１）において共分散Ｓkを“＋∞”に近付けると（Ｓk→＋∞）、補正係数Ｌkは“０”に近づく。すると、式（１２）は「ｘk＝ｘ+k」となる。これは、移動状態推定処理で推定された第１の移動状態をそのまま維持することを表しており、拘束条件の作用をゼロにすることを意味する。これとは逆に、共分散Ｓkを小さくすると、移動状態推定処理で推定された第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を高める作用が働く。つまり、拘束条件の作用を強めて移動状態を修正することが可能となる。
【００５３】
このことから、共分散Ｓkは、移動状態推定処理で推定された第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターであると言える。そこで、以下では、共分散Ｓkのことを「適応度合パラメーター」と称して説明する。
【００５４】
ここで、拘束条件の具体例を挙げる。移動状態ｘkを移動体（例えば自動車）の速度ベクトルｖkとし、速度ベクトルｖkに対する拘束条件を設定する場合を例示する。速度ベクトルｖkは移動体の速度及び移動方向を表しているため、この例では移動体の速度及び移動方向を移動状態ｘkとして定めることになる。
【００５５】
図２は、拘束条件の一例である直進性条件の説明図である。直進性条件は、過去に算出した第２の移動状態の状態変化が直進傾向を示している場合に、継続して直進するように第１の移動状態を修正することに用いる条件である。例えば、自動車が真っ直ぐな道路（高速道路等）を走行する場面では、自動車の移動方向は進行方向に沿った真っ直ぐな方向となる。従って、移動状態ｘkとして算出される速度ベクトルｖkは直進傾向を示すことが望まれる。そこで、直進傾向を示す速度ベクトルｖkが算出されるように、移動方向に対する制約を与えるのである。
【００５６】
図２（１）には、時刻ｋ−４〜時刻ｋにおける自動車の速度ベクトルを模式的に図示している。始点が黒丸である速度ベクトルは、過去に算出された速度ベクトルを示している。始点が白丸である速度ベクトルは、移動状態推定処理によって推定された最新の速度ベクトルを示している。
【００５７】
移動状態推定処理によって時刻ｋにおける速度ベクトルｖkが推定された状態を考える。このとき、例えば、時刻ｋから遡って過去所定期間分（例えば過去３時刻分）の速度ベクトルを平均した平均速度ベクトルｖaveを算出する。そして、時刻ｋにおいて推定された速度ベクトルの単位ベクトルｖk／||ｖk||と、平均速度ベクトルの単位ベクトルｖave／||ｖave||との内積を求める関数を、拘束条件モデル関数ｇk（・）として設定する。
【００５８】
具体的には、例えば次式（１４）に示すような拘束条件モデル関数ｇk（ｖk）を設定する。
【数１４】
2014048228.tif 000016
但し、「θk」は速度ベクトルｖkと平均速度ベクトルｖaveとの成す角度である。
【００５９】
速度ベクトルｖkの向きと平均速度ベクトルｖaveの向きとを揃えるためには、速度ベクトルｖkと平均速度ベクトルｖaveとの成す角度θkを０度とするような拘束、つまり、式（１４）に従って計算される内積を“１”とするような拘束を与えればよい。従って、式（８）の拘束方程式における拘束条件規定値ｄkを“１”とする（ｄk＝１）。
【００６０】
図２（２）は、この場合における適応度合パラメーターＳkの設定方法の説明図である。図２（２）において、横軸は式（１４）に従って計算される内積ｃｏｓθkであり、縦軸は適応度合パラメーターＳkの設定値である。速度ベクトルｖkの向きと平均速度ベクトルｖaveの向きとが一致している場合は「ｃｏｓθk＝１」となる。この場合は、速度ベクトルｖkに拘束を与える必要がない。そこで、拘束の作用を最小とするために、適応度合パラメーターＳkの設定値を所与の規定最大値Ｓmaxとする。
【００６１】
これとは逆に、速度ベクトルｖkの向きと平均速度ベクトルｖaveの向きとが逆向きである場合は「ｃｏｓθk＝−１」となる。この場合は、速度ベクトルｖkの向きを平均速度ベクトルｖaveの向きに合わせるために、拘束の強さを最大限とする必要がある。そこで、拘束の作用を最大とするために、適応度合パラメーターＳkの設定値を所与の規定最小値Ｓminとする。
【００６２】
「−１＜ｃｏｓθk＜１」の範囲では、θkが小さくなるにつれて拘束の作用を徐々に弱めていけばよい。このため、例えば図２（２）に示すように、規定最小値Ｓminと規定最大値Ｓmaxとを結ぶ線形の増加関数に従って適応度合パラメーターＳkを算定する。
なお、この場合における適応度合パラメーターＳkの算定に係る関数は、必ずしも図２（２）のような関数としなければならないわけではなく、例えば非線形の増加関数としてもよい。
【００６３】
この適応度合パラメーターＳkの設定方法では、過去に算出した速度ベクトルを用いて平均速度ベクトルを定め、この平均速度ベクトルと、推定した最新の速度ベクトルとの内積によって、適応度合パラメーターＳkの値を算定している。ベクトルの内積は、ベクトル同士の成す角度と等価である。従って、上記の手法では、平均速度ベクトルの向きを基準移動方向として定め、推定した最新の移動方向と基準移動方向との差違に基づいて、適応度合パラメーターＳkを算定していることになる。
【００６４】
なお、上記の例では、平均速度ベクトルｖaveと最新の速度ベクトルｖkとの内積を求める関数を拘束条件モデル関数として定義したが、これに代えて、推定した最新の速度ベクトルｖkと、１時刻前に算出された速度ベクトルｖk-1との内積を求める関数を拘束条件モデル関数として定義することとしてもよい。
【００６５】
図３は、拘束条件の別例である平滑化条件の説明図である。平滑化条件は、過去に算出した第２の移動状態の状態変化が旋回傾向を示している場合に、継続して旋回するように第１の移動状態を修正することに用いる条件である。例えば、自動車がカーブに沿って旋回する場合に、カーブの曲線に沿った速度ベクトルが算出されるように、速度ベクトルに拘束を与える条件である。
【００６６】
図３（１）には、時刻ｋ−３〜時刻ｋにおける自動車の速度ベクトルを模式的に示している。図の見方は図２（１）と同じである。平滑化条件では、時系列に得られる速度ベクトルについて、隣り合う速度ベクトルの成す角度θを同じとする条件を拘束条件として与える。
【００６７】
時刻ｋにおける速度ベクトルｖkと、時刻ｋ−１における速度ベクトルｖk-1との成す角度をθkと表記する。そして、角度θkを、速度ベクトルｖkと速度ベクトルｖk-1との内積及び外積を用いて計算する。内積を用いて角度θkの大きさを求め、外積を用いて隣り合う速度ベクトルの相対的な回転方向を求めることになる。隣り合う速度ベクトルの相対的な回転方向に応じて、角度θkの正負の符号が変化する。
【００６８】
このとき、隣り合う速度ベクトルの成す角度を過去所定期間分（例えば過去３時刻分）平均した平均角度θaveを計算する。そして、拘束条件モデル関数ｇk（・）として、時刻ｋにおける角度θkと平均角度θaveとの角度差Δθkを求める次式（１５）に示すようなモデル関数を設定する。
【数１５】
2014048228.tif 000017
【００６９】
平滑化条件では、隣り合う速度ベクトルの成す角度を同じとするために、式（１５）に従って計算される角度差Δθkをゼロとする制約を与えればよい。従って、式（８）の拘束方程式における拘束条件規定値ｄkを“０”とする（ｄk＝０）。
【００７０】
図３（２）は、この場合における適応度合パラメーターＳkの設定方法の説明図である。図３（２）において、横軸は式（１５）に従って計算される角度差Δθkの絶対値｜Δθk｜であり、縦軸は適応度合パラメーターＳkの設定値である。上記のように、平滑化条件では、角度差Δθkがゼロとなるのが理想的な状態である。従って、「Δθk＝０」の場合の適応度合パラメーターＳkの設定値を、拘束の作用を最小とするために、所与の規定最大値Ｓmaxとする。
【００７１】
そして、｜Δθk｜が大きくなるにつれて拘束の作用を強めるように、適応度合パラメーターＳkの設定値を小さくする。具体的には、例えば図３（２）に示すように、｜Δθk｜の増加に伴い、規定最大値Ｓmaxから所与の規定最小値Ｓminに漸次近付くような非線形の減少関数に従って、適応度合パラメーターＳkを算定する。
なお、この場合における適応度合パラメーターＳkの算定に係る関数は、必ずしも図３（２）のような関数としなければならないわけではなく、例えば線形の減少関数としてもよい。
【００７２】
また、上記の例では、最新の角度θkと過去所定期間分の平均角度θaveとの角度差を求める関数を拘束条件モデル関数として定義したが、これに代えて、最新の角度θkと１時刻前に算出した角度θk-1との角度差を求める関数を拘束条件モデル関数として設定することとしてもよい。
【００７３】
上記の直進性条件及び平滑化条件は、拘束条件の一例である。これらの拘束条件は、移動体の速度ベクトルｖkを算出対象とする場合のみならず、移動体の位置ｐkを算出対象とする場合についても実質的に同一に適用可能である。単位時間当たりの位置の変化量は速度に相当するため、速度ベクトルｖkの代わりに位置ベクトルの差分ｐk−ｐk-1を用いることで、式（１４）や式（１５）に示した直進性条件や平滑化条件に係るモデル関数を同様に定式化することができる。このようにして定式化したモデル関数を用いることで、移動体の位置ｐkを状態変数として、拘束条件を用いた位置ｐkの修正演算を行うことができる。
【００７４】
２．実施例
次に、移動状態算出装置の実施例について説明する。本実施例では、移動状態算出装置の一例として、移動体の位置を状態変数とし、移動体の位置を算出する位置算出装置の実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけでないことは勿論である。
【００７５】
２−１．構成
図４は、本実施例における位置算出装置１の機能構成の一例を示すブロック図である。位置算出装置１は、処理部１０と、衛星信号受信部２０と、センサー部３０と、操作部４０と、表示部５０と、音出力部６０と、通信部７０と、時計部８０と、記憶部９０とを備えて構成される。
【００７６】
処理部１０は、記憶部９０に記憶されているシステムプログラム等の各種プログラムに従って位置算出装置１の各部を統括的に制御するプロセッサーであり、ＣＰＵ（Central Processing Unit）やＤＳＰ（Digital Signal Processor）等のプロセッサーを有して構成される。
【００７７】
本実施例において、処理部１０は、主要な機能部として、位置推定部１１と、拘束条件設定部１３と、位置修正部１５とを有する。但し、これらの機能部は一実施例として記載したものに過ぎず、必ずしもこれら全ての機能部を必須構成要素としなければならないわけではない。また、これら以外の機能部を必須構成要素としてもよい。
【００７８】
位置推定部１１は、例えばカルマンフィルターを利用した位置推定処理を行って、位置算出装置１が設置された移動体の位置を推定する。
拘束条件設定部１３は、位置推定部１１によって推定された位置と、過去に算出した位置とを用いて、新たに位置を算出する際の拘束条件を設定する。
位置修正部１５は、拘束条件設定部１３によって設定された拘束条件を用いて位置推定部１１によって推定された位置を修正することによって、新たな位置を算出する。
【００７９】
衛星信号受信部２０は、測位用衛星の一種であるＧＰＳ衛星から発信されているＧＰＳ衛星信号を受信する受信装置である。衛星信号受信部２０は、不図示のＧＰＳアンテナで受信されたＲＦ（Radio Frequency）信号に対する信号処理を行ってＧＰＳ衛星信号を捕捉し、捕捉したＧＰＳ衛星信号に係る諸量（以下、「メジャメント情報」と称す。）を演算・取得する。メジャメント情報には、ＧＰＳ衛星信号を受信した信号のコード位相やドップラー周波数といった諸量や、位置算出装置１とＧＰＳ衛星との間の擬似距離や擬似距離変化率といった諸量が含まれる。衛星信号受信部２０は、取得したメジャメント情報を処理部１０に出力する。
【００８０】
センサー部３０は、例えば加速度センサーやジャイロセンサーといった慣性センサーを有して構成されるセンサーユニットである。センサー部３０は、その検出結果を処理部１０に出力する。
【００８１】
操作部４０は、例えばタッチパネルやボタンスイッチ等を有して構成される入力装置であり、押下されたキーやボタンの信号を処理部１０に出力する。この操作部４０の操作により、位置算出要求等の各種指示入力がなされる。
【００８２】
表示部５０は、ＬＣＤ（Liquid Crystal Display）等を有して構成される表示装置であり、処理部１０から出力される表示信号に基づいた各種表示を行う。表示部５０には、位置の算出結果や時刻情報等が表示される。
【００８３】
音出力部６０は、スピーカー等を有して構成される音出力装置であり、処理部１０から出力される音出力信号に基づいた各種音出力を行う。音出力部６０からは、各種アプリケーションに係る音声や音楽等が音出力される。
【００８４】
時計部８０は、位置算出装置１の内部時計であり、水晶振動子及び発振回路でなる水晶発振器等を有して構成される。時計部８０の計時時刻は処理部１０に随時出力される。
【００８５】
記憶部９０は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置を有して構成され、処理部１０が位置算出装置１を制御するためのシステムプログラムや、各種アプリケーション処理を実行するための各種プログラムやデータ等を記憶する。
【００８６】
記憶部９０には、処理部１０によって読み出され、位置算出処理（図５参照）として実行される位置算出プログラム９１が記憶されている。位置算出プログラム９１は、位置推定処理として実行される位置推定プログラム９１１と、位置修正処理として実行される位置修正プログラム９１３とをサブルーチンとして含む。
【００８７】
また、記憶部９０には、適応度合パラメーター設定用データ９３と、メジャメントデータ９５と、算出位置データ９７とが記憶される。
【００８８】
適応度合パラメーター設定用データ９３は、適応度合パラメーターＳkを設定するために用いるデータである。例えば、図２（２）に示したような直進性条件での適応度合パラメーターＳkの算定に係る関数を定めたデータや、図３（２）に示したような平滑化条件での適応度合パラメーターＳkの算定に係る関数を定めたデータがこれに含まれる。
【００８９】
メジャメントデータ９５は、衛星信号受信部２０から出力されるメジャメント情報が記憶されたデータである。このメジャメント情報は、位置推定処理において位置の補正演算を行うために使用される。
【００９０】
算出位置データ９７は、位置修正部１５によって最終的な算出位置として算出された位置が記憶されたデータである。
【００９１】
２−２．処理の流れ
図５は、処理部１０が、記憶部９０に記憶されている位置算出プログラム９１に従って実行する位置算出処理の流れを示すフローチャートである。この処理においては、衛星信号受信部２０においてＧＰＳ衛星信号の捕捉及びメジャメント情報の取得が随時行われ、処理部１０がメジャメント情報を随時取得可能な状態にあるものとして説明する。
【００９２】
最初に、処理部１０は、初期設定を行う（ステップＢ１）。具体的には、移動体の位置ｐkを移動状態ｘkと定義し（ｘk＝ｐk）、その初期値ｘ0として所与の初期位置ｐ0を設定する。例えば、位置算出装置１の概略位置を操作部４０からユーザーに入力させ、その概略位置を初期位置ｐ0として設定するなどすればよい。
【００９３】
次いで、位置推定部１１は、記憶部９０に記憶されている位置推定プログラム９１１に従って位置推定処理を行う（ステップＢ３）。位置推定処理では、位置推定部１１は、ＫＦ予測演算を実行する（ステップＢ５）。具体的には、カルマンフィルターの公知の予測演算式に従って、１時刻前の位置ｐk-1から現在の位置ｐkを予測する予測演算を行う。
【００９４】
その後、位置推定部１１は、衛星信号受信部２０からメジャメント情報（例えばコード位相や擬似距離）を観測値として取得し、記憶部９０のメジャメントデータ９５に記憶させる（ステップＢ７）。
【００９５】
次いで、位置推定部１１は、ＫＦ補正演算を実行する（ステップＢ９）。具体的には、ステップＢ７で取得した観測値を用いて、カルマンフィルターの公知の補正演算式に従って、ステップＢ５で予測した位置ｐkを補正する補正演算を行う。
【００９６】
その後、処理部１０は、移動傾向判定処理を行う（ステップＢ１１）。具体的には、例えば、センサー部３０を構成する加速度センサーの検出加速度やジャイロセンサーの検出角速度を利用して、移動体が直進傾向にあるか、旋回傾向にあるか、それ以外であるかを判定する。
【００９７】
次いで、処理部１０は、記憶部９０に記憶されている位置修正プログラム９１３に従って位置修正処理を行う（ステップＢ１３）。位置修正処理では、拘束条件設定部１３が、移動傾向判定処理の判定結果に基づいて（ステップＢ１５）、拘束条件を設定する。
【００９８】
すなわち、移動傾向が直進傾向であった場合は（ステップＢ１５；直進傾向）、拘束条件設定部１３は、直進性条件を設定する（ステップＢ１７）。この場合の直進性条件の設定では、前述したように、速度ベクトルｖkの代わりに位置の差分ｐk−ｐk-1を用いて式（１４）の拘束条件モデル関数ｇk（・）を定式化する。拘束条件規定値ｄkや適応度合パラメーターＳkの設定方法は、図２を用いて説明した通りである。
【００９９】
次いで、位置修正部１５は、ステップＢ１７で設定された直進性条件を利用して、式（１１）〜式（１３）に従って、位置推定処理で推定された位置ｐkを修正する（ステップＢ１９）。
【０１００】
移動傾向が旋回傾向であった場合は（ステップＢ１５；旋回傾向）、拘束条件設定部１３は、平滑化条件を設定する（ステップＢ２１）。この場合の平滑化条件の設定では、前述したように、速度ベクトルｖkの代わりに位置の差分ｐk−ｐk-1を用いて式（１５）の角度θkを定めて拘束条件モデル関数ｇk（・）を定式化する。拘束条件規定値ｄkや適応度合パラメーターＳkの設定方法は、図３を用いて説明した通りである。
【０１０１】
次いで、位置修正部１５は、ステップＢ２１で設定された平滑化条件を利用して、式（１１）〜式（１３）に従って、位置推定処理で推定された位置ｐkを修正する（ステップＢ２３）。
【０１０２】
なお、移動傾向が直進傾向でも旋回傾向でもなかった場合には（ステップＢ１５；それ以外）、位置修正部１５は、位置ｐkの修正を行うことなく、ステップＢ２５へと移行する。
【０１０３】
次いで、処理部１０は、算出位置を出力する（ステップＢ２５）。拘束条件を用いて位置ｐkを修正した場合は、その修正位置を算出位置として出力する。また、拘束条件を用いて位置ｐkを修正しなかった場合は、位置推定処理で推定した推定位置を算出位置として出力する。
【０１０４】
次いで、処理部１０は、処理を終了するか否かを判定し（ステップＢ２７）、処理を継続すると判定した場合は（ステップＢ２７；Ｎｏ）、ステップＢ３に戻る。また、処理を終了すると判定した場合は（ステップＢ２７；Ｙｅｓ）、位置算出処理を終了する。
【０１０５】
２−３．実験結果
図６は、位置算出装置１を用いて移動体の位置を実際に算出する実験を行った実験結果の一例を示す図である。図６（１）は、直進性条件を利用して位置算出を行った実験結果の一例を示し、図６（２）は、平滑化条件を適用して位置算出を行った実験結果の一例を示す。
【０１０６】
図６（１）の実験では、位置算出装置１を設置した自動車を直線道路に沿って走行させ、その場合に位置算出装置１によって算出される位置を時系列にプロットした。黒丸のプロットは直進性条件を適用した場合の算出位置を示しており、白丸のプロットは直進性条件を適用しない場合の算出位置を示している。この実験結果を見ると、直進性条件を適用しない場合は、算出位置は蛇行した軌跡を描いているが、直進性条件を適用した場合は、道路に沿ったほぼ真っ直ぐな軌跡を描いており、直進傾向を反映した算出位置が得られていることがわかる。
【０１０７】
図６（２）の実験では、位置算出装置１を設置した自動車をカーブに沿って走行させ、その場合に位置算出装置１によって算出される位置を時系列にプロットした。図の見方は図６（１）と同じである。この実験結果を見ると、平滑化条件を適用しない場合は、算出位置は滑らかな軌跡を描いていないが、平滑化条件を適用した場合は、カーブに沿った滑らかな軌跡を描いており、旋回傾向を反映した算出位置が得られていることがわかる。
【０１０８】
２−４．作用効果
位置算出装置１において、位置推定部１１は、移動空間を移動する移動体の位置を推定する。そして、拘束条件設定部１３は、位置推定部１１によって推定された移動体の位置と、過去に算出された移動体の位置とを用いて、新たに位置を算出する際の拘束条件を設定する。そして、位置修正部１５が、拘束条件設定部１３によって設定された拘束条件を用いて位置を修正することによって、新たな位置を算出する。位置の推定処理は、例えばカルマンフィルターを用いた推定処理として実現可能である。位置推定処理で推定した位置を最終的な算出位置とするのではなく、所定の拘束条件を用いて推定位置を修正することによって新たな位置を算出する手法により、移動体の位置を正しく算出することが可能となる。
【０１０９】
拘束条件には、推定位置を過去に算出した位置の状態変化の傾向に沿うように修正することに用いる条件を設定する。例えば、過去に算出した位置の位置変化が直進傾向を示している場合には、継続して直進するように位置を修正することに用いる条件（直進性条件）を拘束条件として設定する。また、過去に算出した位置の位置変化が旋回傾向を示している場合には、継続して旋回するように位置を修正することに用いる条件（平滑化条件）を拘束条件として設定する。これにより、過去に算出した位置の状態変化の傾向に沿った適切な位置を算出することが可能となる。
【０１１０】
３．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明する。
【０１１１】
３−１．移動状態推定処理
移動状態推定処理は、通常のカルマンフィルターを適用した推定処理に限らず、状態空間における状態推定の手法を用いて処理であれば、任意の処理を適用可能である。例えば、拡張カルマンフィルターやアンセンテッドカルマンフィルター、パーティクルフィルター、Ｈ∞フィルターといったフィルターを適用した処理としてもよい。
【０１１２】
３−２．移動状態
算出対象とする移動状態の成分は、移動体の位置、速度及び移動方向の何れかを含んでいればよい。この場合、位置、速度及び移動方向の要素をそれぞれ単体で移動状態の成分としてもよいし、複数の要素を組み合わせて移動状態の成分としてもよい。
【０１１３】
例えば、上記の実施例では、移動体の位置を移動状態の成分として、移動体の位置を算出する位置算出装置の実施例を一例として説明したが、速度及び移動方向（つまり速度ベクトル）を移動状態の成分として、移動体の速度ベクトルを算出する速度ベクトル算出装置の実施例としてもよい。
【０１１４】
３−３．移動体
また、移動状態算出装置は、種々の移動体に設置して利用することが可能である。具体的には、人間や自転車、自動車、電車、船舶、飛行機といった各種の移動体に移動状態算出装置を設置して移動状態の算出を行うことが可能である。
【０１１５】
３−４．修正演算
上記の実施例では、移動体の移動傾向を判定する移動傾向判定処理を行い、その判定結果に応じた拘束条件を設定して、移動状態の修正演算を行うこととして説明した。しかし、移動傾向判定処理を省略し、同時に複数種類の拘束条件を設定して、移動状態の修正演算を行う構成とすることも可能である。
【０１１６】
この場合は、拘束条件別に設定する拘束条件モデル関数ｇk（・）、拘束条件規定値ｄk及び適応度合パラメーターＳkのそれぞれについて、複数の拘束条件に係る関数や規定値、パラメーターを一纏めにした行列を定義し、これらの行列を用いて、式（１１）〜式（１３）の修正演算式に従って修正演算を行うようにすればよい。
【０１１７】
具体的には、複数の拘束条件に係るモデル関数ｇk（・）を含む拘束条件モデル関数行列ｇk（・）として、次式（１６）に示すような行列を設定する。
【数１６】
2014048228.tif 000018
但し、上付きの括弧書きで示した数字は、拘束条件の番号を示す。つまり、ｇ(1)k（・）は第１の拘束条件（例えば直進性条件）に係る拘束条件モデル関数を示し、ｇ(2)k（・）は第２の拘束条件（例えば平滑化条件）に係る拘束条件モデル関数を示す。
【０１１８】
また、複数の拘束条件に係る拘束条件規定値ｄkを成分とする拘束条件規定値行列として、次式（１７）に示すような行列を設定する。
【数１７】
2014048228.tif 000019
但し、ｄ(1)kは第１の拘束条件に係る拘束条件規定値を示し、ｄ(2)kは第２の拘束条件に係る拘束条件規定値を示す。
【０１１９】
また、複数の拘束条件に係る適応度合パラメーターＳkを成分とする適応度合パラメーター行列として、次式（１８）に示すような行列を設定する。
【数１８】
2014048228.tif 000020
但し、Ｓ(1)kは第１の拘束条件に係る適応度合パラメーターを示し、Ｓ(2)kは第２の拘束条件に係る適応度合パラメーターを示す。
【０１２０】
３−５．拘束条件の種類
上記の実施形態では、拘束条件として直進性条件及び平滑化条件を例に挙げて説明したが、拘束条件はこれらに限られるわけではない。例えば、移動体が停止している場合の拘束条件として、速度ベクトル（或いは位置ベクトルの差分）をゼロとする拘束条件を設定するなどしてもよい。
【０１２１】
３−６．移動体の移動傾向の判定
上記の実施例では、センサー部３０の検出結果に基づいて、移動体が直進傾向にあるか、旋回傾向にあるか、それ以外であるかを判定しているが、センサー部３０を用いないで、過去に算出した位置の変化や速度の変化に基づいて、移動体が直進傾向にあるか、旋回傾向にあるか、それ以外であるかを判定してもよい。
【０１２２】
３−７．衛星測位システム
上記の実施例では、位置算出装置が備える衛星信号受信装置を、ＧＰＳを適用したＧＰＳ衛星信号受信装置として説明したが、ＧＰＳ以外の衛星測位システムであるＷＡＡＳ（Wide Area Augmentation System）やＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の衛星測位システムを適用した衛星信号受信装置としてもよい。
【符号の説明】
【０１２３】
１  位置算出装置、  １０  処理部、  ２０  衛星信号受信部、  ３０  センサー部、  ４０  操作部、  ５０  表示部、  ６０  音出力部、  ７０  通信部、  ８０  時計部、  ９０  記憶部
【図１】
2014048228.tif 000021
【図２】
2014048228.tif 000022
【図３】
2014048228.tif 000023
【図４】
2014048228.tif 000024
【図５】
2014048228.tif 000025
【図６】
2014048228.tif 000026