JP6191103B2 - 移動状態算出方法及び移動状態算出装置 - Google Patents

Description

本発明は、移動体の移動状態を算出するための方法等に関する。

移動空間を移動する移動体の位置や速度、移動方向といった諸量を含む移動状態を、状態空間における状態推定の手法を用いて推定する技術が考案されている。この状態推定の手法の中でも広く知られているのは、カルマンフィルターである。例えば特許文献１には、カルマンフィルターを利用して移動体の位置を推定する手法が記載されている。

特開２０１０−２６６４６８号公報

カルマンフィルターを利用した移動状態の推定では、誤差を含む観測情報を利用して、時々刻々と変化する移動体の移動状態を逐次的に推定する。観測情報としては、例えば外部のセンサー（例えばＧＰＳ（Global Positioning System）センサーや慣性センサー、地磁気センサー等のセンサー、以下包括的に「参照用センサー」と称す。）によって検出された参照情報が用いられる。

しかし、上記の手法では、移動状態の推定精度が、もっぱら参照用センサーの検出精度に依存してしまうという問題がある。つまり、参照用センサーの検出精度が低いと、正確でない参照情報が観測情報としてカルマンフィルターの演算で用いられることになるため、この観測情報に引きずられて、推定される移動状態の正確性が低下するという問題があった。

本発明は上述した課題に鑑みてなされたものであり、その目的とするところは、移動体の移動状態を正しく算出するための新しい手法を提案することにある。

以上の課題を解決するための第１の形態は、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定することと、前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定することと、前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出することと、を含む移動状態算出方法である。

また、他の形態として、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する推定部と、前記第１の移動状態と、過去に算出した移動空間を移動する前記移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する設定部と、前記拘束条件を用いて前記第１の移動状態を修正することによって、新たな第２の移動状態を算出する修正部と、を備えた移動状態算出装置を構成することとしてもよい。

この第１の形態等によれば、移動空間を移動する移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する。そして、この第１の移動状態と、過去に算出した移動空間を移動する当該移動体の位置、速度及び移動方向の何れかを含む第２の移動状態とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定し、当該拘束条件を用いて第１の移動状態を修正することによって、新たな第２の移動状態を算出する。第１の移動状態の推定は、公知の状態推定の手法（例えばカルマンフィルター）によって実現可能である。単純に第１の移動状態の推定を行うばかりでなく、所定の拘束条件を用いて第１の移動状態を修正することによって新たな第２の移動状態を算出することで、移動体の移動状態を従来よりも正しく算出することが可能となる。

また、第２の形態として、第１の形態の移動状態算出方法における前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を前記拘束条件として設定することを含む、移動状態算出方法を構成することとしてもよい。

この第２の形態によれば、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に沿うように修正することに用いる条件を拘束条件として設定する。

この場合の拘束条件としては、例えば第３の形態として、過去に算出した第２の移動状態の状態変化が直進傾向を示している場合には、継続して直進するように第１の移動状態を修正することに用いる条件を拘束条件として設定することができる。

この第３の形態によれば、直進傾向を示すように第１の移動状態を修正することによって、新たな第２の移動状態を算出することが可能となる。

また、例えば第４の形態として、過去に算出した第２の移動状態の状態変化が旋回傾向を示している場合には、継続して旋回するように第１の移動状態を修正することに用いる条件を拘束条件として設定するようにすることも可能である。

この第４の形態によれば、旋回傾向を示すように第１の移動状態を修正することによって、新たな第２の移動状態を算出することが可能となる。

また、第５の形態として、第２〜第４の何れかの形態における前記設定することは、前記第１の移動状態を過去に算出した前記第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターを算定することを含み、前記修正することは、前記パラメーターを用いて前記第１の移動状態を前記拘束条件を満たす状態に近づけることを含む、移動状態算出方法を構成することとしてもよい。

この第５の形態によれば、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターを算定する。そして、当該パラメーターを用いて第１の移動状態を拘束条件を満たす状態に近づける。これにより、第１の移動状態を適切に修正することによって、新たな第２の移動状態を算出することが可能となる。

また、第６の形態として、第５の形態の移動状態算出方法における前記設定することは、過去に算出した前記第２の移動状態を用いて基準移動方向を定めることを含み、前記算定することは、前記第１の移動状態に含まれる移動方向と前記基準移動方向との差違に基づいて前記パラメーターを算定することを含む、移動状態算出方法を構成することとしてもよい。

この第６の形態によれば、過去に算出した第２の移動状態を用いて基準移動方向を定める。そして、第１の移動状態に含まれる移動方向と基準移動方向との差違に基づくことで、第１の移動状態を過去に算出した第２の移動状態の状態変化の傾向に適応させる程度を示すパラメーターとして適切な値を設定することができる。

移動状態推定に係る手順を示すフローチャート。 （１）直進性条件の説明図。（２）適応度合パラメーターの設定方法の説明図。 （１）平滑化条件の説明図。（２）適応度合パラメーターの設定方法の説明図。 位置算出装置の機能構成の一例を示す図。 位置算出処理の流れを示すフローチャート。 （１）直進性条件を適用した場合の位置算出結果の一例を示す図。（２）平滑化条件を適用した場合の位置算出結果の一例を示す図。

１．原理
１−１．総論
最初に、変数等の定義を行う。本実施形態では、移動体の移動空間を状態空間とする。移動空間における移動状態を「ｘ」を用いて表記し、時刻「ｋ」における移動状態を「ｘ_k」と表記する。また、観測値を「ｙ」を用いて表記し、時刻「ｋ」において取得された観測値を「ｙ_k」と表記する。また、時刻ｋまでの観測値でなる観測値の集合を観測信号と称し、「Ｙ_k＝｛ｙ₀，ｙ₁，・・・，ｙ_k｝」と表記する。

従来から知られている状態推定の手法を移動状態の推定に適用する場合、移動状態の予測と移動状態の補正とを繰り返すことによって逐次的に移動状態を推定する。移動状態の予測は、１時刻前に推定された移動状態から現在の移動状態を予測する。これは、過去の観測から未来を推論することを意味する。時刻ｋにおける移動状態ｘ_kの予測は、時刻ｋ−１までの観測信号Ｙ_k-1＝｛ｙ₀，ｙ₁，・・・，ｙ_k-1｝が与えられたときの移動状態ｘ_kの条件付き確率密度関数を用いて、次式（１）のように表現することができる。

移動状態の補正は、移動状態の予測で得られた予測値を、新たに取得した観測値を用いて補正する。具体的には、時刻ｋにおいて新たに取得した観測値ｙ_kを用いて移動状態ｘ_kを補正する。観測信号Ｙ_k-1＝｛ｙ₀，ｙ₁，・・・，ｙ_k-1｝が既に与えられている状態で新たに観測値ｙ_kが取得されるため、観測信号はＹ_k＝｛ｙ₀，ｙ₁，・・・，ｙ_k｝となる。この場合、移動状態ｘ_kの補正は、観測信号Ｙ_kが与えられたときの移動状態ｘ_kの条件付き確率密度関数を用いて、次式（２）のように表現することができる。

ここまでの移動状態の推定演算は、従来公知の状態推定手法を適用したに過ぎない。本実施形態では、上記の移動状態の推定演算を行うことで推定された移動状態（以下、「第１の移動状態」と称す。）と、過去に算出した移動状態（以下、「第２の移動状態」と称す。）とを用いて、新たに第２の移動状態を算出する際の拘束条件を設定する。そして、設定した拘束条件を用いて第１の移動状態を修正することによって、新たな第２の移動状態を算出する。これが本実施形態の大きな特徴の１つである。

拘束条件を導入するため、移動状態ｘ_kの条件付き確率密度関数の変数として、拘束条件を表す変数を導入する。拘束条件については詳細に後述するが、ここでは、拘束条件を規定するために必要なパラメーターとして、拘束条件規定値「ｄ」と称するパラメーターを導入する。時刻「ｋ」における拘束条件規定値を「ｄ_k」と表記する。また、時刻ｋまでの拘束条件規定値でなる拘束条件規定値の集合を拘束条件信号と称し、「Ｄ_k＝｛ｄ₀，ｄ₁，・・・，ｄ_k｝」と表記する。

図１は、本実施形態における移動状態算出に係る手順（移動状態算出方法）を示すフローチャートである。
最初に、移動状態推定処理を行う（ステップＡ１）。移動状態推定処理では、前述したように、移動状態の予測（ステップＡ３）と、移動状態の補正（ステップＡ５）との２つを行う。

ステップＡ３では、１時刻前に算出された第２の移動状態を用いて、現在の時刻における移動状態ｘを予測する。具体的には、時刻ｋにおける移動状態ｘ_kの予測は、時刻ｋ−１までの観測信号Ｙ_k-1＝｛ｙ₀，ｙ₁，・・・，ｙ_k-1｝と、時刻ｋ−１までの拘束条件信号Ｄ_k-1＝｛ｄ₀，ｄ₁，・・・，ｄ_k-1｝とが与えられたときの移動状態ｘ_kの条件付き確率密度関数を用いて、次式（３）のように表現することができる。

ステップＡ５では、ステップＡ３で予測した移動状態ｘ_kを、新たに取得した観測値ｙ_kを用いて補正する。時刻ｋ−１までの観測信号Ｙ_k-1＝｛ｙ₀，ｙ₁，・・・，ｙ_k-1｝が与えられている状態で新たに観測値ｙ_kが取得されることで、観測信号はＹ_k＝｛ｙ₀，ｙ₁，・・・，ｙ_k｝となる。この場合、移動状態ｘ_kの補正は、観測信号Ｙ_kと、時刻ｋ−１までの拘束条件信号Ｄ_k-1とが与えられたときの移動状態ｘ_kの条件付き確率密度関数を用いて、次式（４）のように表現することができる。

ステップＡ１の移動状態推定処理の後、拘束条件を適用するか否かを判定する（ステップＡ７）。拘束条件を適用しないと判定した場合は（ステップＡ７；Ｎｏ）、移動状態推定処理で推定した移動状態（第１の移動状態）を、新たな移動状態（第２の移動状態）として算出する（ステップＡ１７）。そして、ステップＡ１９へと移行する。

一方、拘束条件を適用すると判定した場合は（ステップＡ７；Ｙｅｓ）、移動状態修正処理を行う（ステップＡ９）。移動状態修正処理では、最初に拘束条件を設定する（ステップＡ１１）。時刻ｋ−１までの拘束条件信号Ｄ_k-1＝｛ｄ₀，ｄ₁，・・・，ｄ_k-1｝が与えられている状態で時刻ｋにおいて新たに拘束条件が設定されることで、拘束条件信号はＤ_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で偏微分することで得られる行列（以下、「拘束行列」と称す。）である。

このとき、式（９）を式（８）に代入して式を整理すると、次式（１０）が導かれる。

式（１０）は、カルマンフィルターにおいて外部入力ｕ_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は直進傾向を示すことが望まれる。そこで、直進傾向を示す速度ベクトルｖ_kが算出されるように、移動方向に対する制約を与えるのである。

図２（１）には、時刻ｋ−４〜時刻ｋにおける自動車の速度ベクトルを模式的に図示している。始点が黒丸である速度ベクトルは、過去に算出された速度ベクトルを示している。始点が白丸である速度ベクトルは、移動状態推定処理によって推定された最新の速度ベクトルを示している。

移動状態推定処理によって時刻ｋにおける速度ベクトルｖ_kが推定された状態を考える。このとき、例えば、時刻ｋから遡って過去所定期間分（例えば過去３時刻分）の速度ベクトルを平均した平均速度ベクトルｖ_aveを算出する。そして、時刻ｋにおいて推定された速度ベクトルの単位ベクトルｖ_k／||ｖ_k||と、平均速度ベクトルの単位ベクトルｖ_ave／||ｖ_ave||との内積を求める関数を、拘束条件モデル関数ｇ_k（・）として設定する。

具体的には、例えば次式（１４）に示すような拘束条件モデル関数ｇ_k（ｖ_k）を設定する。

但し、「θ_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を求める次式（１５）に示すようなモデル関数を設定する。

平滑化条件では、隣り合う速度ベクトルの成す角度を同じとするために、式（１５）に従って計算される角度差Δθ_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）、その初期値ｘ₀として所与の初期位置ｐ₀を設定する。例えば、位置算出装置１の概略位置を操作部４０からユーザーに入力させ、その概略位置を初期位置ｐ₀として設定するなどすればよい。

次いで、位置推定部１１は、記憶部９０に記憶されている位置推定プログラム９１１に従って位置推定処理を行う（ステップＢ３）。位置推定処理では、位置推定部１１は、ＫＦ予測演算を実行する（ステップＢ５）。具体的には、カルマンフィルターの公知の予測演算式に従って、１時刻前の位置ｐ_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（・）として、次式（１６）に示すような行列を設定する。

但し、上付きの括弧書きで示した数字は、拘束条件の番号を示す。つまり、ｇ⁽¹⁾ _k（・）は第１の拘束条件（例えば直進性条件）に係る拘束条件モデル関数を示し、ｇ⁽²⁾ _k（・）は第２の拘束条件（例えば平滑化条件）に係る拘束条件モデル関数を示す。

また、複数の拘束条件に係る拘束条件規定値ｄ_kを成分とする拘束条件規定値行列として、次式（１７）に示すような行列を設定する。

但し、ｄ⁽¹⁾ _kは第１の拘束条件に係る拘束条件規定値を示し、ｄ⁽²⁾ _kは第２の拘束条件に係る拘束条件規定値を示す。

また、複数の拘束条件に係る適応度合パラメーターＳ_kを成分とする適応度合パラメーター行列として、次式（１８）に示すような行列を設定する。

但し、Ｓ⁽¹⁾ _kは第１の拘束条件に係る適応度合パラメーターを示し、Ｓ⁽²⁾ _kは第２の拘束条件に係る適応度合パラメーターを示す。

３−５．拘束条件の種類
上記の実施形態では、拘束条件として直進性条件及び平滑化条件を例に挙げて説明したが、拘束条件はこれらに限られるわけではない。例えば、移動体が停止している場合の拘束条件として、速度ベクトル（或いは位置ベクトルの差分）をゼロとする拘束条件を設定するなどしてもよい。

３−６．移動体の移動傾向の判定
上記の実施例では、センサー部３０の検出結果に基づいて、移動体が直進傾向にあるか、旋回傾向にあるか、それ以外であるかを判定しているが、センサー部３０を用いないで、過去に算出した位置の変化や速度の変化に基づいて、移動体が直進傾向にあるか、旋回傾向にあるか、それ以外であるかを判定してもよい。

３−７．衛星測位システム
上記の実施例では、位置算出装置が備える衛星信号受信装置を、ＧＰＳを適用したＧＰＳ衛星信号受信装置として説明したが、ＧＰＳ以外の衛星測位システムであるＷＡＡＳ（Wide Area Augmentation System）やＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の衛星測位システムを適用した衛星信号受信装置としてもよい。

１ 位置算出装置、 １０ 処理部、 ２０ 衛星信号受信部、 ３０ センサー部、 ４０ 操作部、 ５０ 表示部、 ６０ 音出力部、 ７０ 通信部、 ８０ 時計部、 ９０ 記憶部

Claims (2)

1. 旋回傾向にある移動体の移動状態を算出する移動状態算出方法であって、
   前記移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定するステップと、
   前記第１の移動状態、及び前記推定がされるまでに算出したＮ個（Ｎ≧２）の第２の移動状態を用いることにより、新たに第２の移動状態を算出する際の拘束条件を設定するステップと、
   前記拘束条件を用いて前記第１の移動状態を修正することにより、前記新たな第２の移動状態を算出するステップと、
   を含み、
   前記設定するステップは、
   前記Ｎ個の第２の移動状態において時間的に隣り合う第２の移動状態に基づく変化角度から、基準変化角度を求めるステップと、
   前記推定の直前の前記第２の移動状態及び前記第１の移動状態に基づく最新変化角度を求めるステップと、
   前記第１の移動状態を前記基準変化角度で移動する移動状態に近づける度合を示すパラメーターを、前記基準変化角度と前記最新変化角度との角度差が大きいほど、前記近づける度合を漸次強くする値に算定することにより前記拘束条件を設定するステップと、
   を含み、
   前記第１の移動状態の前記修正は、
   前記パラメーターに従って、前記第１の移動状態を前記基準変化角度で移動する移動状態に近づけるように修正する、
   移動状態算出方法。
2. 旋回傾向にある移動体の移動状態を算出する移動状態算出装置であって、
   前記移動体の位置、速度及び移動方向の何れかを含む第１の移動状態を推定する推定部と、
   前記第１の移動状態、及び前記推定がされるまでに算出したＮ個（Ｎ≧２）の第２の移動状態を用いることにより、新たに第２の移動状態を算出する際の拘束条件を設定する設定部と、
   前記拘束条件を用いて前記第１の移動状態を修正することにより、前記新たな第２の移動状態を算出する修正部と、
   を備え、
   前記設定部は、
   前記Ｎ個の第２の移動状態において時間的に隣り合う第２の移動状態に基づく変化角度から、基準変化角度を求め、
   且つ、前記推定の直前の前記第２の移動状態及び前記第１の移動状態に基づく最新変化角度を求め、
   且つ、前記第１の移動状態を前記基準変化角度で移動する移動状態に近づける度合を示すパラメーターを、前記基準変化角度と前記最新変化角度との角度差が大きいほど、前記近づける度合を漸次強くする値に算定することにより前記拘束条件を設定し、
   前記修正部は、
   前記パラメーターに従って、前記第１の移動状態を前記基準変化角度で移動する移動状態に近づけるように修正することにより、前記新たな第２の移動状態を算出する、
   移動状態算出装置。

Images