JP6493140B2 - 方位推定装置 - Google Patents

Description

本発明は、自車両の方位を推定する方位推定装置に関する。

上記の方位推定装置として、以下の構成のものが知られている。すなわち、まず、衛星情報に基づく自車両の位置の変化に基づく方位角の検知精度を求める。そして、この方位角の検知精度が高い場合にはこの方位角を自車両の方位角として採用し、この方位角の検知精度が低い場合には方位角を検知するセンサにて得られた方位角を自車両の方位角として採用する（例えば、特許文献１参照）。

特開２０１２−０９８１８５号公報

ところで、上記の方位推定装置においては、車両運動や車両環境を加味してより高精度に自車両の方位を検知することについての要求がある。そこで、本発明においては、自車両の方位を推定する方位推定装置において、より高精度に自車両の方位を検知できるようにすることを目的とする。

本発明の一側面の方位角推定装置において、絶対方位取得手段は、自車両の位置の変化に基づく自車両の絶対的な方位を表す絶対方位を取得し、相対方位取得手段は、自車両のヨーレートの積算に基づく自車両の相対的な方位を表す相対方位を取得する。そして、周波数特性取得手段は、自車両の旋回についての周波数特性を取得し、採用設定手段は、周波数特性に応じて絶対方位および相対方位のうちの少なくとも一方を自車両の方位として採用する。

このような方位角推定装置によれば、車両運動や車両環境に応じて変化する自車両の旋回についての周波数特性に応じて、絶対方位および相対方位のうちの少なくとも一方を自車両の方位として採用するので、より高精度に自車両の方位を検知することができる。

なお、各請求項の記載は、可能な限りにおいて任意に組み合わせることができる。この際、一部構成を除外してもよい。

本発明が適用された方位角推定装置１の概略構成を示すブロック図である。 制御部１０が実行する絶対方位角推定処理を示すフローチャートである。 制御部１０が実行する相対方位角推定処理を示すフローチャートである。 制御部１０が実行する方位角統合処理を示すフローチャートである。 方位角統合処理のうちのカットオフ周波数設定処理を示すフローチャートである。 操舵系運動周波数と車両の応答特性（ヨーレートのＰＳＤ）との関係の一例を示すグラフである。 操舵系運動周波数と道路形状に応じた特性（ヨーレートのＰＳＤ）との関係の一例を示すグラフである。 操舵系運動周波数と各方位角の利用割合との関係の一例を示すマップである。 雪道においてカットオフ周波数に応じた方位角の誤差の大きさを示すグラフである。

以下に本発明にかかる実施の形態を図面と共に説明する。
［本実施形態の構成］
本発明が適用された方位角推定装置１は、乗用車等の車両（以下、自車両という）に搭載されており、自車両の方位（特に方位角）を推定する装置である。詳細には、図１に示すように、方位角推定装置１は、制御部１０と、衛星情報受信機２１と、車速センサ２２と、ヨーレートセンサ２３と、地図データベース（ＤＢ）２４と、ステア角センサ２５と、出力対象装置３１と、を備えている。

衛星情報受信機２１は、複数の衛星から送信された電波を受信することによって自車両の位置を検知する周知の衛星情報受信機として構成されている。衛星情報受信機２１は、例えば、１０００ｍｓ程度の周期で自車両の位置を検知し、自車両の位置を示す衛星情報を制御部１０に送る。

車速センサ２２は、自車両の走行速度を検知する周知の車速センサとして構成されている。車速センサ２２は、得られた速度の情報を制御部１０に送る。
ヨーレートセンサ２３は、自車両の旋回角速度を検知する周知のヨーレートセンサとして構成されている。ヨーレートセンサ２３は、衛星情報受信機２１による自車両の位置の検知周期よりも短い周期（例えば、２０ｍｓ程度の周期）で角速度を検知し、得られた角速度を制御部１０に送る。

地図ＤＢ２４は、緯度経度の情報が対応付けられた地図データが格納された周知のデータベースである。地図ＤＢ２４内のデータは、必要に応じて制御部１０によって読み出される。

ステア角センサ２５は、自車両の運転者が操作するステアリングの角度を検知する周知のステアリング角度センサである。ステア角センサ２５は、衛星情報受信機２１による自車両の位置の検知周期よりも短い周期（例えば、５０ｍｓ程度の周期）でステアリングの角度を検知し、得られたステアリングの角度を制御部１０に送る。

制御部１０は、ＣＰＵ１１とＲＯＭ、ＲＡＭ等のメモリ１２とを備えたコンピュータとして構成されている。ＣＰＵ１１は、メモリ１２に格納されたプログラムに従って、自車両の方位角（自車両の正面方向が向けられた方位角）を推定し、この方位角を出力対象装置３１に出力する。

出力対象装置３１は、制御部１０にて推定された自車両の方位角を得て、自車両の運転支援を行う。例えば、出力対象装置３１は、自車両の方位角に基づいて自車両の進行方向を正確に認識することによって、自車両を正確に目的地に誘導する。また、例えば、出力対象装置３１は、自車両の方位角に基づいて自車両が走行する道路が延びる方向に対する斜行を認識し、警告等を行う。

［本実施形態の処理］
このように構成された方位角推定装置１において、制御部１０は、図２以下に示す各種処理を実施する。各種処理の１つとして、図２に示す絶対方位角推定処理を実施する。絶対方位角推定処理は、複数の自車両の位置を用いて、その位置の変化から自車両が向く方位角（絶対方位角）を推定する処理である。

このように絶対方位角を求める手法では、誤差が蓄積されることがないため、時間の経過と共に誤差が大きくなるということはなく、自車両が直線道路を安定走行しているような場合（後述する操舵系運動周波数が低い場合）に精度がよいという特徴がある。ただし、絶対方位角を求める手法では、自車両が山道等の曲線道路を走行する場合等、自車両の舵角が頻繁に変化する状況（後述する操舵系運動周波数が高い場合）には、精度が低下するという特徴がある。

絶対方位角推定処理は、例えば自車両の電源（イグニッションやアクセサリ等）が投入されると開始される処理であり、その後、一定周期毎（例えば１０００ｍｓ程度毎）に繰り返し実施される処理である。絶対方位角推定処理では、図２に示すように、まず、今回得られた最新の衛星情報を衛星情報受信機２１から取得し、メモリ１２に格納された前回の衛星情報を取得する（Ｓ１１０）。なおこの際、最新の衛星情報をメモリ１２に記録させる。

続いて、取得したこれらの衛星情報に基づいて、自車両の速度ベクトルを算出する（Ｓ１２０）。速度ベクトルは、位置の差異を時間の差異で除することで得られる。続いて、速度ベクトルについての子午線に対する角度を求め、この角度に従って方位角を求める（Ｓ１３０）。この方位角を絶対方位角［Ａ］としてメモリ１２に記録させる。

続いて、各種センサ（車速センサ２２、ヨーレートセンサ２３、ステア角センサ２５等）によるセンサ値に基づいて、地図ＤＢ２４から得られる地図上においてマッピングを行うことによって方位角を推定する（Ｓ１４０）。すなわち、センサ値に基づいて自車両における地図上の位置（現在地）を特定する。

そして、この位置における道路が延びる方向のうち、自車両の進行方向側の方向における子午線に対する角度を求め、この角度に従って方位角を求める。この方位角を絶対方位角［Ｂ］としてメモリ１２に記録させる。

続いて、絶対方位角［Ａ］および絶対方位角［Ｂ］を任意に組み合わせることによって絶対方位角［Ｃ］を求める（Ｓ１５０）。絶対方位角を組み合わせる際には、例えば、絶対方位角［Ａ］および絶対方位角［Ｂ］の何れの精度が高いかを地図データの精度や衛星の捕捉数等に応じて求め、これらに応じて絶対方位角［Ａ］および絶対方位角［Ｂ］のうちの何れかを採用するとよい。

また或いは、加重平均値や平均値を採用してもよい。続いて、絶対方位角［Ｃ］をメモリ１２に記録（出力）し（Ｓ１６０）、絶対方位角推定処理を終了する。
ところで、制御部１０は、各種処理の１つとして、図３に示す相対方位角推定処理を実施する。相対方位角推定処理は、ヨーレートセンサ２３からのセンサ値を用いて、以前に検知された自車両の方位角に対する自車両が向く方向の変化を検知し、これによって自車両の方位角（相対方位角）を推定する処理である。

このように相対方位角を求める手法では、自車両の舵角が頻繁に変化する状況（後述する操舵系運動周波数が高い場合）において精度よく自車両の方位角を推定できるという特徴がある。ただし、過去において検知された方位角からの変化を積算する手法であるため、誤差が蓄積され、時間の経過と共に誤差が大きくなるという特徴がある。このため、自車両が直線道路を安定走行しているような場合（後述する操舵系運動周波数が低い場合）には、結果的に前述の絶対方位角を用いた方が精度よくなる可能性が高い。

相対方位角推定処理は、例えば自車両の電源（イグニッションやアクセサリ等）が投入されると開始される処理であり、その後、一定周期毎（例えば２０ｍｓ程度毎）に繰り返し実施される処理である。また、本処理は他の処理とは独立して実行される。相対方位角推定処理では、図３に示すように、まず、後述する方位角統合処理にて得られた最新の方位角（統合方位角）を取得する（Ｓ２１０）。

続いて、ヨーレートセンサ２３からヨーレートを取得し（Ｓ２２０）、ヨーレート積分を行う（Ｓ２３０）。ヨーレート積分とは、最新の方位角が得られた時刻以降に得られた１または複数のヨーレートを積算することを示す。ヨーレート積分によって最新の方位角が得られてからの方位の変化量が得られる。

続いて、相対方位角を推定する（Ｓ２４０）。この処理では、最新の方位角にヨーレート積分によって得られた積算値を加算（または減算）することによって相対的な方位角を得る。この方位角を相対方位角［Ｄ］としてメモリ１２に記録（出力）する（Ｓ２５０）。

このような処理が終了すると相対方位角推定処理を終了する。
また、制御部１０は、各種処理の１つとして、図４に示す方位角統合処理を実施する。方位角統合処理は、絶対方位角［Ｃ］および相対方位角［Ｄ］を用いて最も確からしい方位角を推定する処理である。

方位角統合処理は、例えば自車両の電源（イグニッションやアクセサリ等）が投入されると開始される処理であり、その後、一定周期毎（例えば５０ｍｓ程度毎）に繰り返し実施される処理である。また、本処理は他の処理とは独立して実行される。方位角統合処理では、図４に示すように、まず、カットオフ周波数設定処理を実施する（Ｓ３１０）。

カットオフ周波数設定処理は、絶対方位角［Ｃ］および相対方位角［Ｄ］をどのような割合で利用するかを決定づける際に用いるカットオフ周波数（境界周波数）を設定する処理である。カットオフ周波数は、操舵系運動周波数において設定される値である。

ここで、操舵系運動周波数（自車両の旋回についての周波数特性）とは、操舵に関するパラメータ（例えば、ステア角センサ２５やヨーレートセンサ２３によるセンサ値）が変化する際の周波数を示す。すなわち、荒れた路面を有する道路を走行中である場合等、振動が多い状況や、ステアリングが細かく操作される山道等の状況では、操舵系運動周波数が高くなると言える。また、滑らかな路面を有する道路を走行中である場合等、振動が少ない状況や、ほとんどステアリングが操作されない直線道路等の状況では、操舵系運動周波数が低くなると言える。

カットオフ周波数設定処理では、図５に示すように、まず、車両の応答特性を取得する（Ｓ４１０）。車両の応答特性とは、例えば図６に示すように、操舵系運動周波数に対するヨーレートのＰＳＤ（パワースペクトル関数）等を用いて求めることができる。すなわち、操舵に関する入力（ステアリング操作等）があったときに、車両運動（車両の旋回）にどの程度反映されるかによって求められる。操舵に関する入力に対する車両運動の追従性は、例えば、路面の滑りやすさや、自車両の操舵系の応答性等によって変化し、車両（車種）固有の特性として求められるものである。

続いて、車両の応答特性に応じたカットオフ周波数（ｆ１）を設定する（Ｓ４２０）。
図６に示す例では、ドライ路面を自車両が走行する場合の車両の応答特性と、雪道を自車両が走行する場合の車両の応答特性とをグラフにて示している。ドライ路面でのＰＳＤのピークは、雪道でのＰＳＤのピークよりも高い周波数となっている。すなわち、ドライ路面では、雪道の場合よりも高い周波数においてヨーレートセンサ２３からの出力を利用するとよいと言える。したがって、Ｓ４２０の処理では、自車両が雪道を走行している場合には、相対的に低めのカットオフ周波数を設定し、自車両がドライ路面を走行している場合には、相対的に高めのカットオフ周波数を設定する。

具体的に、この処理では、操舵系運動周波数（例えばステア角センサ２５によるセンサ値）に対するヨーレートのＰＳＤをモニタリングすることによって車両の応答特性（ＰＳＤのピーク等）を求め、応答特性に応じてカットオフ周波数を設定すればよい。また或いは、カメラ等によって路面状況（ドライ、ウェット、雪道等）を認識し、路面状況に応じて予め設定されたカットオフ周波数を設定するようにしてもよい。

続いて、道路形状に基づいてカットオフ周波数（ｆ２）を設定する（Ｓ４３０）。この処理では、例えば、道路形状が実際の道路に頻繁に用いられるクロソロイド曲線であるものとして、車両の応答特性を推定する。なお、道路形状とは、道路が延びる方向である延伸方向と直交する幅方向における任意の位置（例えば、中央や左右の端部）が、延伸方向に沿って移動したと仮定したときに描く軌跡を示す。

上記式［１］を用いると、自車両におけるヨーレートは次のように求めることができる。

この際、操舵系運動周波数とヨーレートのＰＳＤとの関係は、図７に示すように表すことができる。すなわち、操舵系運動周波数が増加するに従ってヨーレートのＰＳＤが単調に減少するという特性が得られる。このような関係において、例えば、ヨーレートのＰＳＤが０となる点の操舵系運動周波数をカットオフ周波数（ｆ２）として得る。なお、クロソロイドパラメータＣ０の設定に応じてカットオフ周波数（ｆ２）は変動する。

続いて、カットオフ周波数ｆ１およびｆ２を合成することによって、設定に利用するカットオフ周波数（ｆｃ）を得る（Ｓ４４０）。この処理では、例えば次式を用いてカットオフ周波数の合成を行う。

上記式［３］において、αは任意の定数であり動的に変更可能である。例えば、近い将来の道路形状から求めたカットオフ周波数（ｆ２）が、車両応答特性から求めたカットオフ周波数（ｆ１）より小さくなることが予想されるときは、段階的にαの値を小さくするなどすることができる。

このような処理が終了すると、カットオフ周波数設定処理を終了し、図４に戻る。続いて、操舵系運動周波数を各種センサ（例えば、ヨーレートセンサ２３またはステア角センサ２５）から取得する（Ｓ３２０）。そして、絶対方位角［Ｃ］および相対方位角［Ｄ］をメモリ１２から取得する（Ｓ３３０）。

続いて、予め準備されたマップにおいて、カットオフ周波数（ｆｃ）を設定し（Ｓ３４０）、このマップに基づいて統合方位角を算出する（Ｓ３５０）。ここで、マップとは、例えば図８に示すように、操舵系運動周波数に応じて各方位角の利用割合が対応付けられたものである。また、統合方位角とは本処理において最終的に推定される自車両の方位角を示す。

このマップにおいては、図８に示すように、βを任意の値として、操舵系運動周波数がｆｃ−β未満である場合には、絶対方位角［Ｃ］がそのまま統合方位角として設定される。また、操舵系運動周波数がｆｃ＋β以上である場合には、相対方位角［Ｄ］がそのまま統合方位角として設定される。

また、操舵系運動周波数がｆｃ−βからｆｃ＋βの範囲内では、絶対方位角［Ｃ］と相対方位角［Ｄ］との両方が利用されるクロスオーバー領域となる。クロスオーバー領域では、操舵系運動周波数がｆｃ−βからｆｃ＋βに近づくにつれて、絶対方位角［Ｃ］の利用割合が減少し、相対方位角［Ｄ］の利用割合は絶対方位角［Ｃ］の利用割合が減少した分だけ増加する。なお、操舵系運動周波数がカットオフ周波数ｆｃと一致したときには、絶対方位角［Ｃ］および相対方位角［Ｄ］の利用割合が共に５０％に設定される。

なお、上記マップにおけるβの値は、例えば、カットオフ周波数の３０〜５０％の値に設定される。続いて、このようにして設定された統合方位角を出力対象装置３１に対して出力し（Ｓ３６０）、方位角統合処理を終了する。

［本実施形態による効果］
上記の方位角推定装置１において制御部１０は、自車両の位置の変化に基づく自車両の絶対的な方位を表す絶対方位を取得し、自車両のヨーレートの積算に基づく自車両の相対的な方位を表す相対方位を取得する。そして、制御部１０は、自車両の旋回についての周波数特性を取得し、周波数特性に応じて絶対方位および相対方位のうちの少なくとも一方を自車両の方位として採用する。

このような方位角推定装置１によれば、車両運動や車両環境に応じて変化する自車両の旋回についての周波数特性に応じて、絶対方位および相対方位のうちの少なくとも一方を自車両の方位として採用するので、より高精度に自車両の方位を検知することができる。

なお、自車両の方位とは、例えば自車両の前部等、任意の部位が向けられた方位（ある基準となる方向に対する角度）を示す。ただし、上記実施形態では、方位のうちの方位角（子午線に対する角度）を推定している。また、周波数特性としては、例えばステア角や操舵角が変化する際の周波数や、車両の振動周波数等をモニタリングしておき、この値を採用するとよい。

また、絶対的な方位とは、過去において得られた方位を加味することなく求められる方位を示す。これに対して、自車両の相対的な方位とは、過去において得られた方位に対してヨーレート等のセンサ値に応じた変位を積算することで求められる方位を示す。

上記の方位角推定装置１において制御部１０は、周波数特性が予め設定された境界周波数（カットオフ周波数）以上であるとき、絶対方位に対する相対方位の重み付けを大きく設定し、境界周波数未満であるとき、絶対方位に対する相対方位の重み付けを小さく設定する。

このような方位角推定装置１によれば、自車両の旋回についての周波数特性が変化したときに方位を求める精度が高くなる手法を用いて方位を推定することができる。
上記の方位角推定装置１において制御部１０は、自車両の環境または走行状態に応じて境界周波数を設定し、設定された境界周波数を用いて重み付けを設定する。

このような方位角推定装置１によれば、境界周波数を自車両の環境または走行状態に応じて適切に設定するので、自車両の環境または走行状態に応じて車両の応答性能が変化する特性を利用して適切な境界周波数を設定することができる。

上記の方位角推定装置１において制御部１０は、自車両が走行する道路の滑りやすさを加味して境界周波数を設定する。
このような方位角推定装置１によれば、自車両が走行する道路の滑りやすさに応じて車両の応答性能が変化する特性を利用して適切な境界周波数を設定することができる。

例えば、図９には、自車両が雪道を走行する場合において、上記手法によりカットオフ周波数ｆｃ適切に設定した場合（ｆｃ＝０．０５Ｈｚ）と、雪道にも拘わらずドライ用のカットオフ周波数（ｆｃ＝３．０Ｈｚ）に設定した場合とにおいて、時間の経過とともに自車両の方位角が変化したときにおける方位角の誤差の大きさの関係を示す。

上記手法によって雪道用のカットオフ周波数を設定した場合には、自車両の方位角の変化と共に誤差の大きさが変化するが、誤差の大きさが概ね３ｄｅｇ未満に収まっていることが分かる。一方、ドライ用のカットオフ周波数を設定した場合には、誤差の振幅が大きく、その値も相対的に大きくなることが分かる。つまり、上記手法を用いることによって、誤差が小さく安定した方位角が得られることが分かる。

上記の方位角推定装置１において制御部１０は、自車両が走行する道路において想定される道路形状を加味して境界周波数を設定する。
このような方位角推定装置１によれば、自車両が走行する道路形状に応じて車両の応答性能が変化する特性を利用して適切な境界周波数を設定することができる。

なお、自車両が走行する道路の滑りやすさ、および自車両が走行する道路において想定される道路形状の両方を加味して境界周波数を設定する場合には、より適切な境界周波数を設定することができる。

［その他の実施形態］
本発明は、上記の実施形態によって何ら限定して解釈されない。また、上記の実施形態の説明で用いる符号を特許請求の範囲にも適宜使用しているが、各請求項に係る発明の理解を容易にする目的で使用しており、各請求項に係る発明の技術的範囲を限定する意図ではない。上記実施形態における１つの構成要素が有する機能を複数の構成要素として分散させたり、複数の構成要素が有する機能を１つの構成要素に統合させたりしてもよい。また、上記実施形態の構成の少なくとも一部を、同様の機能を有する公知の構成に置き換えてもよい。また、上記実施形態の構成の一部を、課題を解決できる限りにおいて省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加又は置換してもよい。なお、特許請求の範囲に記載した文言のみによって特定される技術思想に含まれるあらゆる態様が本発明の実施形態である。

上述した方位角推定装置１の他、当該方位角推定装置１を構成要素とするシステム、当該方位角推定装置１としてコンピュータを機能させるためのプログラム、このプログラムを記録した媒体、方位角推定方法など、種々の形態で本発明を実現することもできる。

例えば、上記実施形態においては、方位角を推定したが、単に方位を推定する構成とすることもできる。
［実施形態の構成と本発明の手段との対応関係］
上記実施形態において制御部１０が実行する処理のうちのＳ１１０〜Ｓ１６０、Ｓ３３０の処理は本発明でいう絶対方位取得手段に相当し、上記実施形態においてＳ２１０〜Ｓ２５０、Ｓ３３０の処理は本発明でいう相対方位取得手段に相当する。また、上記実施形態においてＳ３１０、Ｓ３４０の処理は本発明でいう境界周波数設定手段に相当し、上記実施形態においてＳ３２０の処理は本発明でいう周波数特性取得手段に相当する。また、上記実施形態においてＳ３５０の処理は本発明でいう採用設定手段に相当する。

１…方位角推定装置、１０…制御部、１１…ＣＰＵ、１２…メモリ、２１…衛星情報受信機、２２…車速センサ、２３…ヨーレートセンサ、２４…地図ＤＢ、２５…ステア角センサ、３１…出力対象装置。

Claims (4)

1. 自車両の方位を推定する方位推定装置（１）であって、
   自車両の位置の変化に基づく自車両の絶対的な方位を表す絶対方位を得る絶対方位取得手段（Ｓ３３０：Ｓ１１０〜Ｓ１６０）と、
   自車両のヨーレートの積算に基づく自車両の相対的な方位を表す相対方位を得る相対方位取得手段（Ｓ３３０：Ｓ２１０〜Ｓ２５０）と、
   前記自車両の舵角又はヨーレートが変化する頻度に応じて変化する周波数である操舵系運動周波数を取得する周波数特性取得手段（Ｓ３２０）と、
   前記操舵系運動周波数に応じて前記絶対方位および前記相対方位のうちの少なくとも一方を自車両の方位として採用する採用設定手段（Ｓ３５０）と、
   を備え、
   前記採用設定手段は、前記操舵系運動周波数が予め設定された境界周波数以上であるとき、前記絶対方位に対する前記相対方位の重み付けを大きく設定し、前記境界周波数未満であるとき、前記絶対方位に対する前記相対方位の重み付けを小さく設定する
   ことを特徴とする方位推定装置。
2. 請求項１に記載の方位推定装置において、
   自車両の環境または走行状態に応じて前記境界周波数を設定する境界周波数設定手段（Ｓ３１０、Ｓ３４０）を備え、
   前記採用設定手段は、前記境界周波数設定手段にて設定された境界周波数を用いて前記重み付けを設定すること
   を特徴とする方位推定装置。
3. 請求項２に記載の方位推定装置において、
   前記境界周波数設定手段は、自車両が走行する道路の滑りやすさを加味して前記境界周波数を設定すること
   を特徴とする方位推定装置。
4. 請求項２または請求項３に記載の方位推定装置において、
   前記境界周波数設定手段は、自車両が走行する道路において想定される道路形状を加味して前記境界周波数を設定すること
   を特徴とする方位推定装置。