JP6113333B1 - 駐車支援装置 - Google Patents

Abstract

【課題】ヨー角算出精度を向上させ、自己位置推定精度が高く、悪路面でもロバスト性の高い駐車支援装置を得る。【解決手段】駐車支援が開始された時の自車の位置を基準位置として、自車の位置及び姿勢角を算出する自己位置推定部２が、ヨーレートに基づき、自車の第一のヨー角を算出する第一ヨー角算出部１２と、前輪または後輪の左右車輪移動距離に基づき、自車の第二のヨー角を算出する第二ヨー角算出部１３と、操舵角及び車速に基づき、自車の第三のヨー角を算出する第三ヨー角算出部１４とを有し、合成ヨー角算出部１５により、車両状況に基づき、第一のヨー角、第二のヨー角及び第三のヨー角の合成比率を調整して合成ヨー角を算出し、自車位置姿勢角算出部１６が、この算出した合成ヨー角及び車速に基づき、自車の位置及び姿勢角を算出するようにした。【選択図】図２

Description

この発明は、ある目標駐車位置まで車両を自動的に導く駐車支援装置に関するもので、特に自己位置推定に用いるヨー角の算出方法に関するものである。

ヨー角の算出装置としては、例えば、特許文献１に開示されている駐車支援装置がある。
この駐車支援装置では、車両に搭載されたヨーレートセンサの出力値を積算することでヨー角を算出する。バッファメモリに記憶された過去のヨーレートセンサ出力値に基づいて、過去のヨーレートセンサ出力値の平均を算出し、ヨー角基準位置が設定された時点から所定時間にわたるヨーレートセンサ出力値の平均値を初期値とし、その後、初期値とヨーレートセンサ出力値との差分を時間積分することでヨー角を算出する。
また、一般的に、自車位置及び自車姿勢角を推定する手法として、デッドレコニングという手法が知られている。デッドレコニングでは、自車の移動距離とヨー角から自己位置推定を行う。この自己位置推定では、ヨーレートセンサで取得したヨーレートからヨー角を算出し、車速センサで取得した車速から車両の移動距離を算出し、算出したヨー角と移動距離に基づいて、自車位置と自車姿勢角を推定する。

特許第３５９９０００号公報（第３〜７頁、第１図）

しかしながら、特許文献１に記載された駐車支援装置では、ヨーレートセンサ出力値を積分してヨー角を算出するため、悪路など路面からの振動を受けやすい路面状況の場合、ヨーレートセンサ出力にノイズが重畳すると、センサ出力の精度信頼性が低下する。
このように、センサ出力精度の低下したヨーレートセンサ出力値を用いて、ヨー角を算出すると、悪路のように路面が凹凸している環境では、特に誤差が蓄積しやすく、その結果、自車の位置及び姿勢を含めた駐車経路が実際の経路とは異なってしまう。
最終的には、目標とする駐車位置とは異なる位置に自車が駐車してしまうという問題があった。

この発明は、上述のような課題を解決するためになされたものであり、ヨー角算出精度を向上させ、自己位置推定精度が高く、悪路面でもロバスト性の高い駐車支援装置を得ることを目的とする。

この発明に係わる駐車支援装置においては、駐車支援を行なうように構成された駐車支援装置であって、自車の操舵角を検出する操舵角検出部、及び駐車支援が開始された時の自車の位置を基準位置として、操舵角検出部により検出された操舵角を用いて自車の位置及び姿勢角を算出する自己位置推定部を備え、自己位置推定部は、自車のヨーレートを検出するヨーレート検出部と、自車の前輪または後輪の左右車輪速及び左右車輪移動距離を検出する車輪速検出部と、車輪速検出部によって検出された前輪または後輪の左右車輪速に基づき、車速を算出する車速算出部と、駐車支援中の車両状況を検出する車両状況検出部と、ヨーレート検出部により検出されたヨーレートに基づき、自車の第一のヨー角を算出する第一ヨー角算出部と、車輪速検出部により検出された前輪または後輪の左右車輪移
動距離に基づき、自車の第二のヨー角を算出する第二ヨー角算出部と、操舵角検出部により検出された操舵角及び車速算出部によって算出された車速に基づき、自車の第三のヨー角を算出する第三ヨー角算出部と、車両状況検出部により検出された車両状況に基づき、第一のヨー角、第二のヨー角及び第三のヨー角の合成比率を調整し、合成ヨー角を算出する合成ヨー角算出部と、車速及び合成ヨー角算出部により算出された合成ヨー角に基づき、自車の位置及び姿勢角を算出する自車位置姿勢角算出部とを有するものである。

この発明によれば、駐車支援を行なうように構成された駐車支援装置であって、自車の操舵角を検出する操舵角検出部、及び駐車支援が開始された時の自車の位置を基準位置として、操舵角検出部により検出された操舵角を用いて自車の位置及び姿勢角を算出する自己位置推定部を備え、自己位置推定部は、自車のヨーレートを検出するヨーレート検出部と、自車の前輪または後輪の左右車輪速及び左右車輪移動距離を検出する車輪速検出部と、車輪速検出部によって検出された前輪または後輪の左右車輪速に基づき、車速を算出する車速算出部と、駐車支援中の車両状況を検出する車両状況検出部と、ヨーレート検出部により検出されたヨーレートに基づき、自車の第一のヨー角を算出する第一ヨー角算出部と、車輪速検出部により検出された前輪または後輪の左右車輪移動距離に基づき、自車の第二のヨー角を算出する第二ヨー角算出部と、操舵角検出部により検出された操舵角及び車速算出部によって算出された車速に基づき、自車の第三のヨー角を算出する第三ヨー角算出部と、車両状況検出部により検出された車両状況に基づき、第一のヨー角、第二のヨー角及び第三のヨー角の合成比率を調整し、合成ヨー角を算出する合成ヨー角算出部と、車速及び合成ヨー角算出部により算出された合成ヨー角に基づき、自車の位置及び姿勢角を算出する自車位置姿勢角算出部とを有するので、ヨー角算出精度を向上させ、自己位置推定の精度がよく、悪路面でもロバスト性の高い駐車支援装置とすることができる。

この発明の実施の形態１による駐車支援装置の概略構成を示すブロック図である。 この発明の実施の形態１による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。 この発明の実施の形態１による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。 この発明の実施の形態２による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。 この発明の実施の形態３による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。 この発明の実施の形態４による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。 この発明の実施の形態５による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。 この発明の実施の形態６による駐車支援装置の概略構成を示すブロック図である。 この発明の実施の形態６による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。 この発明の実施の形態６による駐車支援装置における縦列駐車時の自車位置と走行経路を説明するための図である。 この発明の実施の形態７による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。 この発明の実施の形態７による駐車支援装置におけるヨー角ノイズ除去部の概略構成を示すブロック図である。

実施の形態１．
以下に、実施の形態１を図に基づいて説明する。
図１は、この発明の実施の形態１による駐車支援装置の概略構成を示すブロック図である。
図１において、駐車支援装置は、次のように構成されている。
駐車支援開始判定部１は、自車の駐車支援を開始する際に駐車支援開始信号を出力する。自己位置推定部２は、駐車支援開始信号が出力された時の位置を基準位置として、自車位置及び自車姿勢角を算出する。駐車スペース検出部３は、自車が駐車可能な駐車スペースを検出し、基準位置に対する駐車スペースの位置を算出する。
誘導経路算出部４は、自己位置推定部２により推定された自車位置及び自車姿勢角から、駐車スペースの位置へ自車を誘導可能な誘導経路を算出する。目標操舵角算出部５は、誘導経路より、自車のステアリングが操舵すべき目標操舵角を算出する。操舵角検出部６は、ステアリングの現在の操舵角を測定する。
電動パワーステアリング７は、操舵角が目標操舵角へ一致するような制御が可能になっている。

図２は、この発明の実施の形態１による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。
図２において、自己位置推定部２は、次のように構成されている。
ヨーレート検出部８は、自車のヨーレートを検出する。車輪速検出部９は、複数あり、それぞれ、少なくとも自車の前輪または後輪の左右車輪速及び左右車輪移動距離を検出する。車速算出部１０は、前輪または後輪の左右車輪速より車速を算出する。操舵角検出部６は、ステアリングの現在の操舵角を測定する。車両状況検出部１１は、駐車支援中の車両状況を検出する。
第一ヨー角算出部１２は、ヨーレートより自車の第一のヨー角を算出する。第二ヨー角算出部１３は、前輪または後輪の左右車輪移動距離より、自車の第二のヨー角を算出する。第三ヨー角算出部１４は、操舵角と車速より自車の第三のヨー角を算出する。
合成ヨー角算出部１５は、車両状況として、車体の振動のピーク値により、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を調整し、合成ヨー角を算出する。自車位置姿勢角算出部１６は、車速と合成ヨー角より、自車の位置及び姿勢角を算出する。

図３は、この発明の実施の形態１による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。
図３において、符号１１〜１５は図２におけるものと同一のものである。図３は、車両状況検出部１１の構成を示し、車体振動検出部１７は、車体の振動を検出する。車体振動ピーク値算出部１８は、車体振動検出部１７の出力から、車体振動のピーク値を算出する。

次に、動作について説明する。
駐車支援開始判定部１により、駐車支援を開始するタイミングの判定を行う。この駐車支援の開始タイミングの判定基準としては、運転者が駐車開始時に押すスイッチを用いる手法、あるいは車速が一定車速以下になったタイミングを、駐車支援開始と判断する手法がある。
これらは一例であり、駐車開始のタイミングが判定できるものであれば、他の手法でも良い。

次いで、自己位置推定部２が、駐車支援開始判定部１より駐車支援開始信号を受け取った位置を基準位置として、駐車支援開始以降の自車の位置、姿勢角の推定を行う。
ここで、自車の位置と姿勢角を推定する手法として、車輪速センサとヨーレートセンサから推定するデッドレコニングと呼ばれる手法を用いる。
デッドレコニングの一例としては、車輪速センサから移動距離を検出し、さらにヨーレートセンサを１サンプリング時間ごとに積分することで、ヨー角を算出する。
なお、ここでは、ヨー角の算出方法としては、ヨーレートセンサの積分を一例に挙げたが、他にも車速と操舵角から求まるヨーレートを積分して算出する方法、あるいは左右車輪の移動距離の差から算出する方法もある。
以上の方法によって、ヨー角を算出したのち、自車の位置を求める。
例えば、自車進行方向をｘ軸、横方向をｙ軸とすると、基準位置からの自車位置（ｘ、ｙ）は、次のように示される。
ｘ＝Ｌ・ｓｉｎ（γ）、ｙ＝Ｌ・ｃｏｓ（γ）。ここで、Ｌは移動距離、γはヨー角である。このような手法を用いることで、自車の位置と姿勢を推定することができる。

次に、駐車スペース検出部３の処理について説明する。
駐車スペース検出部３では、車載センサを用いて自車周辺の駐車可能なスペースを検出し、自己位置推定部２で決定した基準位置に対する駐車スペースの位置を算出する。
車載センサの一例として、超音波ソナーを用いる。超音波ソナーは、超音波を送信した時間から超音波が障害物に反射し、この反射波を超音波ソナーが受信するまでにかかる時間を基に、障害物との距離を測定することができる。
この超音波ソナーを自車側面に取り付け、自車側面から障害物までの距離を取得することで、自車が走行中に自車の側方の駐車可能なスペースを検出し、障害物距離と自車位置及び自車姿勢角から駐車スペースの位置を算出する。
なお、上述の説明では、車載センサの一例として超音波ソナーを用いたが、自車周辺を映すカメラなど、駐車スペース検出が可能であれば、他の車載センサを用いても良い。

このように、障害物車両の位置は、自車位置と超音波ソナーからの距離に基づいて算出される。
ここで、路面の凹凸が大きい悪路の場合では、ヨーレートにノイズが重畳されることで、自車位置に誤差が生じるため、障害物車両までの距離が正しく検出できていても、結果的には、障害物車両の推定位置が実際の位置とは異なってしまう。このため、ヨーレートにノイズが重畳されることの影響は大きい。

次に、誘導経路算出部４の処理について説明する。
誘導経路算出部４は、自己位置推定部２により推定された自車位置姿勢角（自車位置及び自車姿勢角）から、駐車スペース検出部３で検出された駐車スペースの位置へ、自車を誘導可能な誘導経路を算出する。
自車位置姿勢角と駐車スペースの位置関係に基づいて、自車位置から障害物車両内に設定される目標駐車位置に対し、自車は障害物車両を回避する制約条件の下、目標駐車位置に達する誘導経路を算出する。
このため、誘導経路の算出のためには、少なくとも自車位置姿勢角と障害物車両の位置及び姿勢角を要しており、すなわちヨーレートを内部変数とする誘導経路を算出していると換言できる。

次に、目標操舵角算出部５の処理について説明する。
目標操舵角算出部５では、誘導経路算出部４で算出された駐車誘導経路から、この誘導経路に沿って走行するために必要な目標曲率を算出し、この目標曲率から自車のステアリングが操舵すべき目標操舵角を算出する。
ここで、目標曲率とは、自車の旋回半径の逆数を示す。目標曲率と目標操舵角の関係を定義する理論式がある（数式（１）、（２））。（出典：安部正人著 自動車の運動と制御 第１版、ＩＳＢＮ ９７８−４−５０１−４１７００−０ Ｃ３０５３）

ここで、ρは目標曲率、Ｖは自車走行速度、Ａはスタビリティファクタ、ｌは車両ホイールベース、δは前輪実操舵角、ｎはステアリングギア比（前輪実舵角／操舵角）、θは操舵角である。上述の理論式（数式（１）、（２））を用いることで、目標曲率から目標操舵角を算出する。

次に、操舵角検出部６の処理について説明する。
操舵角検出部６では、自車のステアリングにおける現在の操舵角を測定する。検出方法としては、舵角センサを用いて直接ステアリングの操舵角を測定する。
また、電動パワーステアリング７では、操舵角が目標操舵角算出部５で算出された目標操舵角へ一致するように自車のステアリングを制御し、自動操舵する。電動パワーステアリング７は、運転者がステアリングを操舵した時の操舵トルクをトルクセンサ（図示せず）で測定し、その操舵トルクに応じて、操舵トルクを補助するアシストトルクを発生させることを主な機能とする。
一方、駐車支援では、運転者の操舵を補助する必要はなく、駐車支援装置がステアリングを自動で操舵する。つまり、電動パワーステアリング７は、目標操舵角に操舵角が追従するような舵角によるフィードバック制御がなされている。

次に、図２に示す自己位置推定部２について、詳しく説明する。
ヨーレート検出部８では、ヨーレートセンサを用いて、自車のヨーレートを検出する。車輪速検出部９では、車輪速センサを用いて、自車の従属輪である後輪の左右車輪速及び左右車輪移動距離を検出する。車速算出部１０では、左右車輪速の平均値を求め、この値を自車の車速とする。

第一ヨー角算出部１２では、ヨーレート検出部８で検出されたヨーレートより自車のヨー角を算出し、このヨー角を第一のヨー角とする。第一のヨー角は、ヨーレートセンサを用いて検出したヨーレートを１サンプリング時間ごとに積分することで算出する。
ヨーレート検出部８で検出されたヨーレートをω_１とした場合の第一のヨー角γ_１は、数式（３）にて求まる。

第一のヨー角は、ヨー角算出時に必要な信号がヨーレートセンサの出力のみであるため、容易に算出することができる。
しかし、ヨーレートセンサの出力を積分することで、ヨー角を算出するため、ヨーレートにノイズが含まれる場合、算出したヨー角に誤差が蓄積する。そのため、ヨー角算出の信頼性が低下するという特徴がある。
例えば、悪路を高い車速で走行する場合、ヨーレートセンサの出力にノイズが重畳するため、このヨーレートを積分して算出したヨー角には誤差が蓄積する。

第二ヨー角算出部１３では、車輪速検出部９で検出された左右車輪移動距離より、自車のヨー角を算出し、このヨー角を第二のヨー角とする。第二のヨー角は、自車左右の左右車輪移動距離の差から算出する。
ここで、車輪速センサは、一定距離を車輪が回転するごとにパルスを検出し、このパルスをカウントすることで、車輪の移動距離及び車輪速を算出する。自車の従属輪を後輪とし、後輪の車輪速センサで検出した右車輪の移動距離をｌ_ｗｒ、左車輪の移動距離をｌ_ｗｌ、左右車輪間距離をｄとした場合の第二のヨー角γ_２は、数式（４）にて求まる。

第二のヨー角は、積分を用いずに直接ヨー角を算出できるため、第一のヨー角のようにヨーレート積分による誤差が蓄積しない。そのため、車速が速い場合に第一のヨー角よりヨー角算出精度が低下しにくいという特徴がある。
しかし、急な加減速時は、車輪速センサ値と実際の車輪移動距離との間に誤差が生じ、ヨー角算出の信頼性が低下する可能性がある。

第三ヨー角算出部１４では、操舵角検出部６で検出された操舵角と車速算出部１０で算出された車速より、自車のヨー角を算出し、このヨー角を第三のヨー角とする。第三のヨー角は、操舵角から算出した前輪タイヤの実舵角と自車左右の車輪速の平均値から算出された車速を用いてヨーレートを算出し、この算出したヨーレートを１サンプリング時間ごとに積分することで算出する。
操舵角θから算出された前輪実舵角をδ、自車後輪の左右車輪速の平均から算出された車速をＶ、ホイールベースをｌとした場合の第三のヨー角γ_３は、数式（５）にて求まる。

第三のヨー角は、ステアリングの操舵角からヨー角を算出するため、直進走行時など操舵角が小さい場合にはヨー角が安定する。そのため、精度の高いヨー角を算出することができるという特徴がある。
しかし、自動操舵中においては、パワーステアリングの制御の影響で、操舵角にノイズが生じ、ヨー角算出精度が低下する可能性がある。

以上のように、実施の形態１では、数式（３）〜数式（５）で示される３種類のヨー角算出方法があり、それぞれの特徴を説明した。
前述したヨー角精度が低下する場合を除外することができれば、常に高精度にヨー角を算出できるようになる。
以下に、車両状況検出部１１を用いて、ヨー角精度が低下しないように、あるいは精度低下の影響を低減できる手法について説明する。

車両状況検出部１１では、駐車支援中の車両状況を検出する。車両状況として例えば、車体振動のピーク値および周波数、車速および加速度の大きさ、操舵角の大きさが挙げられる。これらの詳細については後述する。

車両状況を基準として、合成ヨー角算出部１５では、第一、第二、第三のヨー角の合成比率を調整し、合成ヨー角を算出する。
第一のヨー角に対する合成比率をＫ_１、第二のヨー角に対する合成比率をＫ_２、第三のヨー角に対する合成比率をＫ_３とした場合の合成ヨー角γ_ｓｕｍは、数式（６）にて求まる。

ただし、各ヨー角の合成比率（Ｋ_１、Ｋ_２、Ｋ_３）には、数式（７）に示す条件が成り立つ。

第一のヨー角と第二のヨー角と第三のヨー角の合成比率を車両状況に合わせて調整することで、例えば、ヨー角算出における悪路での車体振動の影響を抑制し、より精度の高い合成ヨー角を算出することができる。
これにより、自己位置推定精度が高まり、より正確に目標駐車スペースへの駐車支援を行うことができる。

車両状況の実例として、車体振動のピークをヨー角合成の判断基準とする。
悪路走行時には、車体振動により、ヨーレートセンサで取得するヨーレートに誤差が生じ、ヨー角算出精度が低下するという課題がある。
そこで、車体振動のピークを検出し、これをヨー角合成の判断基準とすることで、車体振動による合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めることができる。

車体振動は、車両に搭載された振動センサによって検出する。振動センサは車体に取り付けることで、振動のピークや周波数を検出できる。
振動センサを用いて、自車の車体振動のピーク値を検出し、この値をヨー角合成比率の判断基準にすることで、悪路走行時に大きな車体振動が生じた場合に誤差が生じるヨー角の合成比率を小さくすることで、合成ヨー角算出における車体振動の影響を抑制することができる。
例えば、（Ｋ_１＝０、Ｋ_２＝０．５、Ｋ_３＝０．５）という合成比率にすることで、ヨーレートセンサ出力値に重畳されるノイズによる誤差を含まないヨー角を合成することが
できる。

実施の形態１によれば、悪路で生じる車体振動といった車両状況の影響によってセンサ出力の精度が低下する場合であっても、ヨーレート、操舵角、左右車輪移動距離と３つの異なる検出値を基に算出したヨー角の合成比率を、路面状況や車体状態に合わせて変更することで、常に高いヨー角算出精度を保つことができる。
この算出精度の高い合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

実施の形態２．
以下に、実施の形態２を図に基づいて説明する。
実施の形態１では、車両状況検出部１１において、車体振動のピーク値を基準として合成ヨー角の合成比率の調整を行った。
しかしながら、車体振動には自車走行速度または路面の凹凸によって車体振動の周波数に高低が生じ、車体振動の周波数によって、第一、第二、第三のヨー角の算出に与える影響が異なる。
そこで、実施の形態２は、車両状況として、車体振動の周波数を検出し、これをヨー角合成の判断基準とすることで、上述の問題を解決し、車体振動による合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図４は、この発明の実施の形態２による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。
図４において、符号１１〜１５、１７は図３におけるものと同一のものである。図４では、車両状況検出部１１に、車体振動の周波数を算出する車体振動周波数算出部１９を設けている。

実施の形態２は、車両状況検出部１１において、車体の振動を検出する車体振動検出部１７と、車体振動の周波数を算出する車体振動周波数算出部１９を備え、合成ヨー角算出部１５において、車体の振動の周波数により、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を変更するようにした。

車体振動は、車両に搭載した振動センサによって検出する。車体振動の周波数を算出し、車体振動周波数をヨー角合成比率の判断基準にすることで、車両振動の周波数域に応じてヨー角の比率を変更することができ、悪路の影響を抑制したヨー角を取得することができる。
特に、車体振動の周波数が高い場合には、ヨーレートセンサ、車輪速センサの出力に高周波ノイズが生じるため、例えば、（Ｋ_１＝０．２、Ｋ_２＝０．２、Ｋ_３＝０．６）という合成比率にすることで、ヨーレートセンサや車輪速センサにおいて生じる悪路によるノイズの影響を抑制したヨー角を合成することができる。

実施の形態２によれば、悪路で生じる車体振動、特に車体振動の周波数を基準とすることで、高周波ノイズによる誤差の蓄積を抑制したヨー角を合成することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

実施の形態３．
以下に、実施の形態３を図に基づいて説明する。
実施の形態２では、車両状況検出部１１において車体振動の周波数を基準として、合成ヨー角の合成比率の調整を行った。
しかしながら、悪路走行時には、車速によって第一、第二、第三のヨー角の算出精度に違いが生じるという問題がある。
例えば、車速が低い時には、路面の凹凸の影響で車輪が回転し、平面を走行する場合と比較して、車輪移動距離に誤差が生じやすい。そのため、車輪移動距離に基づいて算出される第二のヨー角の算出精度は低下してしまう。
そこで、実施の形態３においては、上述の問題を解決し、車速による合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図５は、この発明の実施の形態３による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。
図５において、符号１１〜１５は図３におけるものと同一のものである。図５では、車両状況検出部１１に、図２と同一の車速算出部１０を設け、合成ヨー角算出部１５において、車速により、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を変更するようにした。

車速は、車輪速検出部９で検出された自車後輪の左右車輪速を平均することで算出する。
この車速をヨー角合成比率の判断基準にすると、車速が高い時には、算出時の精度が高い車輪速に基づく第二のヨー角の比率を大きくし（例えば、Ｋ_１＝０．２、Ｋ_２＝０．６、Ｋ_３＝０．２）、車速が低い時には、路面の凹凸などの影響を受けやすい第二のヨー角の比率を小さくする（例えば、Ｋ_１＝０．４、Ｋ_２＝０．２、Ｋ_３＝０．４）ことで、悪路走行時の車速によるヨー角算出精度の低下を抑制することができる。

実施の形態３によれば、ヨー角合成比率の調整の基準として車速を用いることで、車速が高い場合、低い場合それぞれにおいて、算出精度の高い合成ヨー角を算出することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

実施の形態４．
以下に、実施の形態４を図に基づいて説明する。
実施の形態３では、車両状況検出部１１において、車速を基準として、合成ヨー角の合成比率の調整を行う。
しかしながら、自車が加減速を行う時、第一、第二、第三のヨー角の算出精度に違いが生じるという問題がある。
例えば、急な加減速時には、車輪速センサ値と実際の車輪移動距離との間に誤差が生じ、第二のヨー角の算出精度が低下する可能性がある。
そこで、実施の形態４においては、上述の問題を解決し、車体加減速による合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図６は、この発明の実施の形態４による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。
図６において、符号１１〜１５は図５におけるものと同一のものである。図６では、車両状況検出部１１に、車体の加速度を算出する車体加速度算出部２０を設けたもので、合成ヨー角算出部１５において、車体加速度により、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を変更するようにした。

車体加速度は、車輪速検出部９で検出された自車後輪の左右車輪速の平均を微分することで算出することができる。
この加速度をヨー角合成比率の判断基準にすることで、加減速時のヨー角算出精度の低
下を抑制できる。
例えば、急なアクセル踏み増しによる加速あるいは急なブレーキによる減速時には、車輪速に基づく第二のヨー角の精度は低下する。そのため、第二のヨー角の比率を小さくする（例えば、Ｋ_１＝０．４、Ｋ_２＝０．２、Ｋ_３＝０．４）ことで、加減速による悪影響を抑制することができる。
なお、ここでは、車体加速度を車輪速から算出したが、車体加速度を算出できれば、加速度センサなど他の方法で算出しても良い。

実施の形態４によれば、ヨー角合成比率の調整の基準として、車体加速度を用いることで、加減速時における算出精度の低下を抑制した合成ヨー角を算出することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

実施の形態５．
以下に、実施の形態５を図に基づいて説明する。
実施の形態４では、車両状況検出部１１において、車体加速度を基準として、合成ヨー角の合成比率の調整を行った。
しかしながら、自車の操舵角によって、第一、第二、第三のヨー角の算出精度に違いが生じるという問題がある。例えば、直進走行時など、操舵角が小さい場合には、ヨー角が安定するため、第三のヨー角の算出精度が高くなる。
そこで、実施の形態５においては、上述の問題を解決し、操舵角による合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図７は、この発明の実施の形態５による駐車支援装置における車両状況検出部の概略構成を示すブロック図である。
図７において、符号１１〜１５は図６におけるものと同一のものである。図７では、車両状況検出部１１に、図２と同一の操舵角検出部６を設けている。合成ヨー角算出部１５において、操舵角により、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を変更するようにした。

操舵角検出部６で測定される操舵角の大きさを、ヨー角合成比率の判断基準にすることで、自車のステアリングの操舵に合わせたヨー角算出を行うことができる。
例えば、操舵角度の大きさが小さい時、車両はほぼ直進している状態にあり、操舵角が安定するため、操舵角に基づく第三ヨー角の比率を大きくする（例えば、Ｋ_１＝０．２、Ｋ_２＝０．２、Ｋ_３＝０．６）ことで、直進走行時におけるヨー角算出精度を高めることができる。

実施の形態５によれば、ヨー角合成比率の調整の基準として、操舵角の大きさを用いることで、操舵角度の大きさが小さい直進走行時の算出精度を高めた合成ヨー角を算出することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

実施の形態６．
以下に、実施の形態６を図に基づいて説明する。
実施の形態５では、車両状況検出部１１において、操舵角を基準として合成ヨー角の合成比率の調整を行った。
しかしながら、駐車支援中の駐車シーケンスを表す駐車モードによって、車速および操舵角の大きさにそれぞれ特徴があるため、第一、第二、第三のヨー角の算出精度に違いが生じるという問題がある。
例えば、電動パワーステアリング７によって自動操舵を行っているときには、車速が低いため、第二のヨー角の算出精度が低下してしまう。また、駐車スペース検出部３で、駐車スペースを検出している間は、直進走行を行うため、操舵角が小さくなる。そのため、第三のヨー角の算出精度は高くなる。
このように、駐車モードによって、車速、操舵角が異なるため、第一、第二、第三のヨー角の算出精度に違いが生じる。
そこで、実施の形態６においては、上述の問題を解決し、駐車モードによる合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図８は、この発明の実施の形態６による駐車支援装置の概略構成を示すブロック図である。
図８において、符号１〜７は図１におけるものと同一のものである。図８の駐車支援装置は、駐車支援中の駐車シーケンスを表す駐車モードを管理する駐車モード管理部２１を有する。

図９は、この発明の実施の形態６による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。
図９において、符号２、６、８〜１６は図２におけるものと同一のものである。図９では、駐車モード管理部２１の出力が合成ヨー角算出部１５に入力されている。

図１０は、この発明の実施の形態６による駐車支援装置における縦列駐車時の自車位置と走行経路を説明するための図である。
図１０において、障害物車両１００は、すでに駐車している２台の障害物となる車両である。２台の障害物車両１００の間の駐車スペースに、自車１０１を駐車支援によって縦列駐車させる際の走行経路を示す。
駐車支援開始時の自車位置Ｐ１から自動操舵開始時の自車位置Ｐ２までの走行経路をＬ１とする。自動操舵開始時の自車位置Ｐ２から駐車完了時の自車位置Ｐ３までの走行経路をＬ２とする。

実施の形態６は、実施の形態１の駐車支援装置に加えて、駐車支援中の駐車シーケンスを表す駐車モードを管理する駐車モード管理部２１を有する。
そして、図９に示すように、合成ヨー角算出部１５において、駐車モードにより、第一のヨー角、第二のヨー角、第三のヨー角の合成比率を変更するようにしている。

駐車モードをヨー角合成比率の判断基準にすることで、各モードにおける操舵角や車速の条件に合う、高精度な合成ヨー角を取得することができる。
まず、縦列駐車時の走行経路を示す図１０を基に、駐車モードについて説明する。

図１０には、すでに駐車している２台の障害物車両１００の間の駐車スペースに、自車１０１を駐車支援によって縦列駐車させる際の走行経路を示している。
駐車支援開始時の自車位置Ｐ１から自動操舵開始時の自車位置Ｐ２までの走行経路をＬ１とし、この走行経路Ｌ１では、駐車スペース検出部３によって駐車スペースの検出を行う。この時、自車１０１は直進走行する。
また、自動操舵開始時の自車位置Ｐ２から駐車完了時の自車位置Ｐ３までの走行経路をＬ２とする。この走行経路Ｌ２では、電動パワーステアリング７によって自動操舵を行う。
ここで、走行経路Ｌ１における駐車モードを駐車スペース検出シーケンス、走行経路Ｌ２における駐車モードを自動操舵シーケンスとする。

駐車モードにおける駐車スペース検出シーケンスでは、直進走行を行うため、駐車支援中の速度の中で比較的車速が高く、操舵角は小さい。そのため、車輪速に基づく第二のヨー角、および操舵角に基づく第三のヨー角の合成比率を大きくする（例えば、Ｋ_１＝０．２、Ｋ_２＝０．４、Ｋ_３＝０．４）ことで、精度の高い合成ヨー角を取得することができる。
また、自動操舵シーケンスでは、直進走行時と比較して車速は低くなり、操舵角は大きくなる。そのため、ヨーレートに基づく第一のヨー角の合成比率を大きくする（例えば、Ｋ_１＝０．６、Ｋ_２＝０．２、Ｋ_３＝０．２）ことで、精度の高い合成ヨー角を取得することができる。

実施の形態６によれば、駐車モードを基準として、ヨー角合成比率を調整することで、駐車支援中において、常に精度の高い合成ヨー角を取得することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

なお、ここでは、縦列駐車について説明を行ったが、並列駐車の場合も同様に、駐車モードを基準として、合成比率を調整することで、ヨー角算出精度を向上させることができる。

また、上述の各実施の形態において、第一、第二、第三のヨー角の合成比率（Ｋ_１、Ｋ_２、Ｋ_３）の具体的数値を示しているが、あくまで一例であり、数式（７）の条件を満たす範囲の内、車両状況に合わせて、各実施の形態で示したような第一、第二、第三のヨー角の特徴に合わせた比率設定であれば、別の数値でも良い。

また、上述の各実施の形態においては、第一、第二、第三の３つのヨー角に基づいて、ヨー角の合成を行ったが、（Ｋ_１、Ｋ_２、Ｋ_３）のいずれかの比率を０として、３つの内２つをヨー角合成に用いる方法、あるいは３つの内のいずれか１つを合成ヨー角とするように切り替える方法を用いても良い。

実施の形態７．
以下に、実施の形態７を図に基づいて説明する。
実施の形態１〜実施の形態６の合成ヨー角算出部１５では、車両状況検出部１１で検出される車両状況または駐車モード管理部２１で管理される駐車モードを基準として、第一、第二、第三のヨー角の合成比率を調整し、合成ヨー角を算出していた。
しかしながら、第一、第二、第三のヨー角のいずれかに悪路の影響でノイズが生じている状態で、合成した場合、合成ヨー角においてもノイズが生じ、合成ヨー角の算出精度が低下するという問題がある。
そこで、実施の形態７においては、上述の問題を解決し、ノイズによる合成ヨー角算出精度の低下を抑制し、駐車支援中の自車位置及び姿勢角の信頼性を高めたものである。

図１１は、この発明の実施の形態７による駐車支援装置における自己位置推定部の概略構成を示すブロック図である。
図１１において、符号２、６、８、９、１１〜１６、２１は図９におけるものと同一のものである。図１１では、合成ヨー角算出部１５が、ヨー角合成比率調整部２２及びヨー角ノイズ除去部２３を有する。ヨー角合成比率調整部２２は、車両状況を基準として、ヨー角ノイズ除去部２３でノイズ処理された第一、第二、第三ヨー角の合成比率を調整し、合成ヨー角を算出する。

図１２は、この発明の実施の形態７による駐車支援装置におけるヨー角ノイズ除去部の概略構成を示すブロック図である。
図１２において、符号１１〜１５、２１〜２３は図１１におけるものと同一のものである。図１２は、ヨー角ノイズ除去部２３の構成を示している。
ヨー角ノイズ除去部２３は、第一ヨー角算出部１２で算出された第一のヨー角よりヨーレートを算出する第一ヨーレート算出部２４と、第二ヨー角算出部１３で算出された第二のヨー角よりヨーレートを算出する第二ヨーレート算出部２５と、第三ヨー角算出部１４で算出された第三のヨー角よりヨーレートを算出する第三ヨーレート算出部２６と、第一のヨーレートより第一のヨーレートの周波数を検出する第一ヨーレート周波数検出部２７と、第二のヨーレートより第二のヨーレートの周波数を検出する第二ヨーレート周波数検出部２８と、第三のヨーレートより第三のヨーレートの周波数を検出する第三ヨーレート周波数検出部２９と、第一のヨー角に対するノイズ除去フィルタを算出し、第一のヨー角のノイズを除去する第一ヨー角ノイズ除去部３０と、第二のヨー角に対するノイズ除去フィルタを算出し、第二のヨー角のノイズを除去する第二ヨー角ノイズ除去部３１と、第三のヨー角に対するノイズ除去フィルタを算出し、第三のヨー角のノイズを除去する第三ヨー角ノイズ除去部３２とを有する。

第一ヨーレート算出部２４では、第一ヨー角算出部１２で算出された第一のヨー角を微分し、第一のヨーレートを算出する。第一ヨーレート周波数検出部２７では、第一のヨーレートの周波数を検出する。第一ヨー角ノイズ除去部３０では、第一のヨーレートの周波数に基づいて、ノイズ除去フィルタの遮断周波数を調整し、ノイズ除去フィルタによって第一のヨー角のノイズ除去を行う。
例えば、高周波ノイズ除去を目的とする場合、第一のヨー角の周波数成分において、Ｎ次の周波数を遮断周波数とすることで、遮断周波数より高周波のノイズを除去することができる。ここで、Ｎの値は駐車支援の試験を行い、試験結果に合わせて調整すれば良い。

第二、第三のヨー角においても、同様にしてノイズ除去を行う。このように、第一、第二、第三のヨー角の周波数に合わせて、遮断周波数を算出することで、常にノイズの少ないヨー角を算出できる。

実施の形態７によれば、第一、第二、第三のヨー角を合成する前に、各ヨーレートの周波数を基準としてノイズ除去を行うことで、各ヨー角の元信号を基に合成ヨー角を算出する場合よりも精度の高い合成ヨー角を取得することができる。
この合成ヨー角を自己位置推定に用いることで、自車位置及び自車姿勢角の算出精度を向上させ、駐車精度を向上させることに対して顕著な効果を奏する。

なお、実施の形態１〜実施の形態７において、様々な方法を用いてヨー角検出精度を向上させ、自己位置推定精度を高める手法について説明したが、実施の形態１〜実施の形態７の手法について、優先順位をつけて組み合わせて用いても良い。
また、組み合わせのパターン及び優先順位については、車両出荷時に仕向け地毎にあらかじめ設定しても良いし、車両出荷後でも駐車支援開始時の周辺環境により能動的に変更するようにしても良い。

なお、本発明は、その発明の範囲内において、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略することが可能である。

１ 駐車支援開始判定部、２ 自己位置推定部、３ 駐車スペース検出部、４ 誘導経路算出部、５ 目標操舵角算出部、６ 操舵角検出部、７ 電動パワーステアリング、８ ヨーレート検出部、９ 車輪速検出部、１０ 車速算出部、１１ 車両状況検出部、１２ 第一ヨー角算出部、１３ 第二ヨー角算出部、１４ 第三ヨー角算出部、１５ 合成ヨー角算出部、１６ 自車位置姿勢角算出部、１７ 車体振動検出部、１８ 車体振動ピー
ク値算出部、１９ 車体振動周波数算出部、２０ 車体加速度算出部、２１ 駐車モード管理部、２２ ヨー角合成比率調整部、２３ ヨー角ノイズ除去部、２４ 第一ヨーレート算出部、２５ 第二ヨーレート算出部、２６ 第三ヨーレート算出部、２７ 第一ヨーレート周波数検出部、２８ 第二ヨーレート周波数検出部、２９ 第三ヨーレート周波数検出部、３０ 第一ヨー角ノイズ除去部、３１ 第二ヨー角ノイズ除去部、３２ 第三ヨー角ノイズ除去部、１００ 障害物車両、１０１ 自車、Ｐ１ 駐車支援開始時の自車位置、Ｐ２ 自動操舵開始時の自車位置、Ｐ３ 駐車完了時の自車位置、Ｌ１ 駐車スペース検出中の走行経路、Ｌ２ 自動操舵中の走行経路

Claims (9)

1. 駐車支援を行なうように構成された駐車支援装置であって、
   自車の操舵角を検出する操舵角検出部、
   及び駐車支援が開始された時の自車の位置を基準位置として、上記操舵角検出部により検出された操舵角を用いて自車の位置及び姿勢角を算出する自己位置推定部を備え、
   上記自己位置推定部は、
   自車のヨーレートを検出するヨーレート検出部と、
   自車の前輪または後輪の左右車輪速及び左右車輪移動距離を検出する車輪速検出部と、
   上記車輪速検出部によって検出された前輪または後輪の左右車輪速に基づき、車速を算出する車速算出部と、
   駐車支援中の車両状況を検出する車両状況検出部と、
   上記ヨーレート検出部により検出されたヨーレートに基づき、自車の第一のヨー角を算出する第一ヨー角算出部と、
   上記車輪速検出部により検出された前輪または後輪の左右車輪移動距離に基づき、自車の第二のヨー角を算出する第二ヨー角算出部と、
   上記操舵角検出部により検出された操舵角及び上記車速算出部によって算出された車速に基づき、自車の第三のヨー角を算出する第三ヨー角算出部と、
   上記車両状況検出部により検出された車両状況に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整し、合成ヨー角を算出する合成ヨー角算出部と、
   上記車速及び上記合成ヨー角算出部により算出された合成ヨー角に基づき、自車の位置及び姿勢角を算出する自車位置姿勢角算出部とを有することを特徴とする駐車支援装置。
2. 上記車両状況検出部は、
   自車の車体の振動を検出する車体振動検出部と、
   この車体振動検出部により検出された車体の振動のピーク値を算出する車体振動ピーク値算出部とを有し、
   上記合成ヨー角算出部は、上記車体振動ピーク値算出部により算出された上記車体の振動のピーク値に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
3. 上記車両状況検出部は、
   自車の車体の振動を検出する車体振動検出部と、
   この車体振動検出部により検出された車体の振動の周波数を算出する車体振動周波数算出部とを有し、
   上記合成ヨー角算出部は、上記車体振動周波数算出部により算出された上記車体の振動の周波数に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
4. 上記車両状況検出部は、上記車速算出部を含み、
   上記合成ヨー角算出部は、上記車速算出部により算出された車速に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
5. 上記車両状況検出部は、
   車体の加速度を算出する車体加速度算出部を有し、
   上記合成ヨー角算出部は、上記車体加速度算出部により算出された車体加速度に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
6. 上記車両状況検出部は、上記操舵角検出部を含み、
   上記合成ヨー角算出部は、上記操舵角検出部により検出された操舵角に基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
7. 駐車支援中の駐車シーケンスを示す駐車モードを管理する駐車モード管理部を備え、
   上記合成ヨー角算出部は、上記駐車モードに基づき、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整することを特徴とする請求項１に記載の駐車支援装置。
8. 上記合成ヨー角算出部は、
   上記合成ヨー角を算出する前に、上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角のノイズを除去するヨー角ノイズ除去部と、
   上記第一のヨー角、上記第二のヨー角及び上記第三のヨー角の合成比率を調整するヨー角合成比率調整部とを有することを特徴とする請求項１から請求項７のいずれか一項に記載の駐車支援装置。
9. 上記ヨー角ノイズ除去部は、
   上記第一のヨー角から算出されるヨーレートの周波数に応じて設定された遮断周波数をもつノイズ除去フィルタにより、上記第一のヨー角のノイズを除去する第一ヨー角ノイズ除去部と、
   上記第二のヨー角から算出されるヨーレートの周波数に応じて設定された遮断周波数をもつノイズ除去フィルタにより、上記第二のヨー角のノイズを除去する第二ヨー角ノイズ除去部と、
   上記第三のヨー角から算出されるヨーレートの周波数に応じて設定された遮断周波数をもつノイズ除去フィルタにより、上記第三のヨー角のノイズを除去する第三ヨー角ノイズ除去部とを有することを特徴とする請求項８に記載の駐車支援装置。