JP6987973B2

JP6987973B2 - 車両制御装置

Info

Publication number: JP6987973B2
Application number: JP2020510392A
Authority: JP
Inventors: 知靖坂口; 隆筒井; 義幸吉田
Original assignee: Hitachi Astemo Ltd
Current assignee: Hitachi Astemo Ltd
Priority date: 2018-03-28
Filing date: 2019-02-15
Publication date: 2022-01-05
Anticipated expiration: 2039-02-15
Also published as: CN111886170B; US20210078586A1; WO2019187750A1; CN111886170A; DE112019000597T5; DE112019000597B4; JPWO2019187750A1; US11472419B2

Description

本発明は、車両制御装置に関し、特に車両の位置制御を行うために自車位置を推定する車両制御装置に関する。

自動運転や運転アシスト、自動駐車では、システムが外界の状況に合わせて車両を制御するために、システム自体が自車の位置を知ることが重要である。自車の位置を知る方法はシステムの構成によって様々であるが、GPSやレーダー、カメラ等の外界認識センサに依らずに、車両の自車位置を推定する方法として、車輪速センサや舵角センサ、加速度センサ、ジャイロセンサ等の自車センサ情報を用いて自車位置を推定するデッドレコニングという方法が知られている。

車輪速センサは、車輪に取り付けられたカムコーダが回転することでその凹凸の通過によってパルス波形を発生させ、車輪速センサと結線されたコントローラ側でパルス波形の立ち上がりと立ち下がりを検出し、その回数をカウントする。車輪が１回転する間のパルス波形の立ち上がりと立ち下がりの合計回数はカムコーダの歯数によって決まり、例えば、車輪１回転当り９０回カウントされる。そのカウント（数）から車輪の回転角度を計測することができ、車輪の周長情報と合わせて車輪（つまり、車両）の走行距離を知ることができる。

特に、自動駐車システムでは、車両が低速でしか走行しないため、車輪の滑りがほぼ無視できる。そのため、車輪速センサのみを使用したデッドレコニング、もしくは車輪速センサとそれ以外のセンサを組み合わせたデッドレコニングで、ある程度の精度が得られることが知られている。

下記特許文献１では、車輪速センサとジャイロセンサを使用し、車輪速センサを使用して走行距離を求め、ジャイロセンサを使用して車両のヨー角を求めるデッドレコニングの方法が示されている。

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

しかしながら、車輪速センサのみで自車位置を推定する方法では、ヨー角推定誤差、特に、発進時のヨー角誤差に課題がある。例えば、車輪速パルスのみを用いたデッドレコニングでは、車両ヨー角は離散値として算出されるため、車両軌跡は大まかな多角形近似となり、精度が低い。また、車両ヨー角の離散値の量子サイズ（分解能）をより細かくするために、車両ヨー角の時間平均フィルタによる処理や、車輪速から算出した角速度を利用することも可能であるが、時間平均フィルタによる処理の場合、加減速領域での精度に課題がある。また、角速度を利用した場合、発進直後と停車間際に車輪速が不安定となるため、発進停車ごとに誤差が累積するという課題がある。そして、従来のヨー角推定方法、及び発進時ヨー角誤差補正方法では、発進時ヨー角誤差を補正できる場面が、直進発進に限定され、旋回発進に対応していなかった。

本発明は、上記課題に鑑みてなされたものであり、その目的とするところは、発進時の左右輪のパルス波形の初期位相ズレによる、発進時のヨー角誤差を抑制し、自車位置を精緻に推定することのできる車両制御装置を提供することにある。

また、一時停車中に回転方向の分からない車輪速パルスカウントが発生した場合の、ヨー角誤差を抑制し、自車位置を精緻に推定することのできる車両制御装置を提供することにある。

上記課題を解決する本発明の車両制御装置は、車両に搭載したセンサ情報に基づいてヨー角変位量と走行距離と前後進行方向を算出し、該ヨー角変位量と走行距離と前後進行方向から、車両の進行方向の推定開始状態からの相対位置と相対方向を算出して自車位置を推定することを特徴とする。

本発明によれば、発進時のヨー角誤差を抑制することができ、自車位置を精緻に推定することが可能になる。したがって、車両の進行方向の推定精度を向上させることができ、さらに、発進時の進行方向推定誤差を補正することで、車両の制御精度を向上させることができる。
本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。また、上記した以外の、課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

極低速において非操舵輪である左右の後輪ＲＬ、ＲＲの車輪速センサが生成する波形を整形したパルス波形を示す図。発進直後の極低速における自車位置推定の軌跡を示す図。極低速状態から加速して速度が高まってきたときの左右の後輪ＲＬ、ＲＲのパルス波形を示す図。発進後に速度が高まってきたときの自車位置推定の軌跡を示す図。左右輪のパルスタイミング差による自車位置推定の軌跡の違いを示す図。左右の後輪ＲＬ、ＲＲのパルスタイミング差による自車位置推定の軌跡の誤差範囲を示す図。発進直後の車両の推定位置を直進方向に限定したときの自車位置推定の軌跡を示す図。発進地点でのヨー角を推定する方法を説明する図。停車前と再発進時のヨー角を推定する方法を説明する図。自動駐車システムによる自動駐車の駐車経路を座標で示す図。本発明に係る車両制御装置の一実施形態を備えた自車位置推定システムの一例を示す全体構成図。図１１に示す自動駐車システムの制御構成を示す制御ブロック図。図１２に示す自車位置推定部におけるデッドレコニングによる自車位置推定方法を概略的に示すフローチャート。車速加重平均ヨー角の演算用テーブルを示す図。車速加重平均ヨー角の演算方法を示すフローチャート。自車位置推定情報の補正を実行する方法の一例を示す状態遷移図。自車位置推定情報の補正を実行する方法の他の一例を示す状態遷移図。図１６のＳ３０２からＳ３０３に移行する際に実行する処理のフローチャート。図１６のＳ３０３からＳ３０４、あるいは、図１７のＳ４０２からＳ４０３に移行する際に実行する処理のフローチャート。発進時の座標補正を説明する図。自車位置推定情報の補正を実行する方法の一例を示す状態遷移図。自車位置推定情報の補正を実行する方法の他の一例を示す状態遷移図。図２１のＳ７０４からＳ７０１、或いは、図２２のＳ８０３からＳ８０１に移行する際に実行する停車時ヨー角推定処理のフローチャート。図２１のＳ７０３からＳ７０４、或いは、図２２のＳ８０２からＳ８０３に移行する際に実行する再発進時ヨー角補正処理のフローチャートである。車輪回転方向を判定するための判定表。

本実施形態の車両制御装置は、車両に搭載したセンサ情報に基づいてヨー角変位量と走行距離と前後進行方向を算出し、そのヨー角変位量と走行距離と前後進行方向から、車両の相対位置と相対方向を算出して自車位置を推定する構成を有している。車両制御装置は、車輪速パルスカウント部と、ギアポジション検出部と、自車位置推定部を備えており、自車位置推定部は、一対の非操舵輪の車輪速パルスとギアポジションから、車両の相対位置と相対方向を算出する。自車位置推定部は、車両の相対方向であるヨー角を、一対の非操舵輪の走行距離差分の車速加重平均によって算出する。そして、この相対方向を、車両発進直後の複数の移動地点で測定することによって、発進時の相対方向の誤差を推定し、相対位置と相対方向を補正する。

したがって、車両の相対位置と相対方向の推定精度が向上し、ヨー角推定値を使用する制御は高精度な制御が可能になる。特に、発進時の進行方向推定誤差を補正することで、車両の制御精度が向上する。そして、本実施形態の車両制御装置を自動駐車システムに適用した場合には、駐車開始位置から目標駐車位置まで車両を誘導する制度が向上し、目標駐車位置への到達精度が向上する。

次に、本実施形態の車両制御装置の詳細な構成について説明する。
例えば、車両の車輪のうち２輪を結ぶ直線が常に旋回中心近くを通る場合（その２輪は多くの車両では非操舵輪すなわち左右の後輪である）、ある程度の精度であれば、２輪の車輪速センサだけで車両の走行距離とヨー角を求めることも可能である。この車輪速センサが生成するパルス波形からカウントされた車輪速パルスのみを使用するデッドレコニング手法の概要は、次の通りである。

(１）ヨー角変位量θは、左右の非操舵輪の走行距離の差分と、左右の非操舵輪の間の中心から左右の非操舵輪までの左右間隔とから算出できる。例えば、以下の式(1)を用いて、非操舵輪左右2輪に設けられた車輪速センサの車輪速パルスカウントからヨー角変位量θを算出する（ヨー角変位量算出手段）。
θ=(左右車輪速パルスカウント差分)×(1パルス当りの走行距離)／(後輪トレッド長)・・・式(1)

前進においては、左の車輪のカウントの方が多ければ、車両が(上から見て)時計回りに旋回する方向のヨー角変位量となる。一方、右の車輪のカウントの方が多ければ、車両が(上から見て)反時計周りに旋回する方向のヨー角変位量となる。後退であれば、それぞれ逆方向となる。

(２）走行距離は、左右の非操舵輪の走行距離の平均から算出することができ、左右の非操舵輪の走行距離は、車輪速センサで車輪の回転によって発生するパルスをカウントすることで算出できる。例えば、以下の式(2)を用いて、車輪速パルスカウントから左右の非操舵輪車軸中心の走行距離Uを算出する。
走行距離U=(左右車輪速パルスカウントの平均)×(1パルス当たりの走行距離)・・・式(2)

(３）車両のギアポジションから前後進行方向を検出する。ギアポジションが前進ギアであれば、進行方向は＋１とし、ギアポジションが後退ギアであれば、進行方向は−１とする。車輪速センサに車輪の回転方向を検出する機構が付いている場合は、このギアポジションからの検出処理は不要となる。

(４）前記の車両のヨー角変位量θと走行距離Lと前後進行方向から、以下の式(3)〜(5)を用いて、車両の左右の非操舵輪車軸中心と進行方向の推定開始状態からの相対位置（Ｘ、Ｙ）と相対方向（ヨー角）を出力する。
ヨー角=(前回ヨー角)+(進行方向)×(ヨー角変位量θ)・・・式(3)X座標=(前回X座標)+(進行方向)×sin(ヨー角)×(走行距離U)・・・式(4)Y座標=(前回Y座標)+(進行方向)×cos(ヨー角)×(走行距離U)・・・式(5)

上記(1)〜(4)のシーケンスを制御周期毎に実行することによって、ある程度の精度において、推定開始地点(例えば、発進場所)からの相対位置(自車位置)を推定することができる。

しかしながら、上述したように車輪速センサによって、XY座標、ヨー角を算出する場合、カウント数という量子化された値に基づいて自車位置推定するため、走行距離とヨー角の両方が量子化の影響を受けることになる。それらは、算出結果に量子化誤差を与える程度のものと考えられがちであるが、発進時の左右のパルス波形の初期位相ズレは、自車位置推定精度に大きな影響を与える。この初期位相ズレによって、主に発進時の推定方向誤差(以後、発進時のヨー角誤差と呼ぶものとする)が引き起こされる。発進時のヨー角誤差は、発進後の自車位置推定結果の全てに影響を及ぼし、発進場所からの距離に比例して推定位置誤差が大きくなる。

前記の発進時のヨー角誤差が発生するメカニズムについてより具体的に説明する。図１は、極低速において非操舵輪である左右の後輪ＲＬ、ＲＲの車輪速センサが生成する波形を整形したパルス波形である。後輪ＲＬ、ＲＲのパルスの位相が完全に同期するケースは少ないため、極低速では、後輪ＲＬ、ＲＲの行路長が殆ど同じ場合は、後輪ＲＬ、ＲＲに設けられた車輪速センサの車輪速パルスカウントは延々と交互にＯＮとＯＦＦ、つまり、左右左右、もしくは右左右左を繰り返す。

図２は、発進直後の極低速における自車位置推定の軌跡である。前記のデッドレコニングのアルゴリズムでは、軌跡Ｌ１〜５は、最初のパルスでヨー角変化した方向にジグザグ斜行する。全体として、その斜行方向Ｆは、(本来、車両が進行すべき基準方向に対して)1パルス分のヨー角変位量の1/2の方向となる。

図３は、極低速状態から加速して速度が高まってきたときの後輪ＲＬ、ＲＲのパルス波形である。後輪ＲＬ、ＲＲのパルスは、互いに別個独立して出力されるものであり、車両の速度が高まると、後輪ＲＬ、ＲＲのパルス波形におけるパルスが同じ制御間隔にカウント（以下、左右同時カウントもしくは左右同時パルス検出と呼ぶことがある)される確率が高まる。後輪ＲＬ、ＲＲのパルスタイミング差が小さい場合は、左右同時カウントの確率が更に高くなる。

図４は、発進後に速度が高まってきたときの自車位置推定の軌跡Ｌ１〜Ｌ８である。全体として、出発時の進行方向Ｆは、後輪ＲＬ、ＲＲのパルス波形の位相差に依存する。軌跡Ｌ５、Ｌ６は、後輪ＲＬ、ＲＲのパルスが左右同時カウントされたときのものであり、軌跡Ｌ４からヨー角変位量が０のままで真っ直ぐに伸びる。

図５は、左右輪のパルスタイミング差による自車位置推定の軌跡の違いを示している。図５の上段に示す例は、左右輪のパルスタイミング差が小さく、初回カウントの時間差も小さいケースであり、カウント周期の１／２以下のタイミング差となっている。図５の中段に示す例は、左右輪のパルスタイミング差が大きいケースであり、カウント周期の約１／２程のタイミング差となっている。図５の下段に示す例は、左右輪のパルスタイミング差は小さいが、初回カウントの時間差が大きいケースであり、カウント周期の１／２以上のタイミング差となっている。

図５の上段に示すように、後輪ＲＬ、ＲＲのパルスタイミング差がパルスカウント周期と比較して小さいときは、速度が高まってくると、ヨー角誤差が少ない状態でジグザグ斜行が早い段階で収まり、自車位置推定の軌跡が前方方向(本来、進行すべき基準方向)に進み始める。

図５の中段に示すように、後輪ＲＬ、ＲＲのパルスタイミング差がパルスカウント周期の半分程度のときはなかなか同時カウントにならず、ジグザグ斜行が長く続くことになる。

図５の下段に示すように、左右の後輪ＲＬ、ＲＲのパルスタイミング差は小さいが、初回カウントの時間差が大きいときは、速度が高まって来ると、ジグザグ斜行が早い段階で止み、自車位置推定の軌跡が斜行方向(本来、進行すべき基準方向に対して傾いた方向)に進み始める。つまり、ヨー角誤差が付いた状態でジグザグ斜行が収まり、誤差が大きいケースとなる。このように、発進時の進行方向は、左右輪のパルス波形の位相差に依存する。

図６は、後輪ＲＬ、ＲＲのパルスタイミング差による自車位置推定の軌跡の誤差範囲を示している。後輪ＲＬ、ＲＲのパルスタイミング差(パルス位相差)が全くない場合には、推定軌跡の進行方向は図の真上方向になるとする。また、左右の実線矢印は、進行方向に対して1パルスの左右車輪速パルスカウント差分によるヨー角変位量δ(1パルス当たりの走行距離/後輪トレッド長)分、左右それぞれに回転した方向である。

右輪ＲＲのパルスタイミング差がパルスカウント周期の半分よりも大きい場合には、車両の進行方向の推定軌跡は図６のAの領域に進み、右輪ＲＲのパルスタイミング差がパルスカウント周期の半分よりも小さい場合には、車両の進行方向の推定軌跡は図６のBの領域に進むことになる。また、左輪ＲＬのパルスタイミング差がパルスカウント周期の半分よりも小さい場合には、車両の進行方向の推定軌跡はCの領域に進み、左輪ＲＬのパルスタイミング差がパルスカウント周期の半分よりも大きい場合には、車両の進行方向の推定軌跡はDの領域に進むことになる。このように、左右輪のパルスタイミング差によって、自車位置推定の発進時のヨー角誤差は、前記の各範囲内に拡がっている。

このような状況に対応するため、簡易的な対策として、図７に示すように、発進時の左右輪ＲＬ、ＲＲのパルスタイミング差を無視して、発進直後の車両の推定位置を直進方向Ｆ０に限定する方法が考えられる。

しかし、上記した方法では、発進直後に車両が旋回を始めた場合に、その旋回を検出できないことになる。また、前記の、旋回できるように発進直後のごくわずかな距離だけ直進方向Ｆ０に限定した場合には、単純に誤差発生地点を直進方向Ｆ０に限定する距離の分だけ先送りするだけに過ぎない。

発進時のヨー角誤差と同様の誤差は、発進後に一時停車したときにも発生する可能性がある。一時停車中に車両の車輪の状態に変化がなかった場合には、ヨー角の断絶はなく発進時と同様の誤差は発生しない。一時停車中に車両の揺れや操舵輪の舵角を変化させることなどによって車両の状態に変化があったとしても、その際にカウントが発生しなかった場合や、カウントが発生したとしてもカウントを正しくヨー角値に反映した場合にも、発進時と同様の誤差は発生しない。

しかし、一時停車中では車両の進行方向が特定できない場合があり、車輪速センサがカウントアップしても、車輪の回転方向が想定と反対の可能性がある。従って、一時停車中にカウントが発生した時にカウントを逆方向にヨー角値に反映してしまう場合がある。このような場合には、一時停車時にも大きなヨー角誤差が発生する。

このような状況に対応するため、簡易的な対策として、一時停車中に発生する回転方向の分からない車輪速パルスカウントを無視する方法が考えられる。これにより、車輪速パルスカウントを逆方向にヨー角値に反映してしまうことはなくなるが、本来正しく反映すべきカウントを無視することによるヨー角誤差が発生する。

本実施形態の車両制御装置では、車両発進時の左右輪のパルスタイミング差によって生じたヨー角誤差を推定し、推定後その誤差分を補正する処理を行う。ヨー角誤差は、1カウント当りのヨー角変位量θよりも小さいため、一定距離もしくは一定時間の間のヨー角値の平均値演算で算出する。

発進地点の近傍では、車速が大きく変化するため、従来のようなヨー角値の単純時間平均では、正確なヨー角値は得られない。そのため、本実施形態の車両制御装置では、平均値演算において、ヨー角値の継続時間を継続距離に変換するために、ヨー角値に車速を加重し、距離平均を算出している(式(6))。
精密ヨー角値=Σ(ヨー角値×車速)／Σ車速・・・式(6)

上記式(6)のヨー角値は、左右輪の走行距離差分によって求められるので、上記式(6)の精密ヨー角値は、左右輪の走行距離差分の車速加重平均により求めることもできる(式(6)’)。
精密ヨー角値=Σ(左右輪の走行距離差分／後輪トレッド長×車速)／Σ車速・・・式(6)’

上記式(6)の車速加重平均値演算によれば、1カウント当りのヨー角変位量θよりも小さい分解能の精密ヨー角値を求めることができる。この車速加重平均値は、一定距離もしくは一定時間の間の平均ヨー角値であり、その走行区間の中間地点での精密ヨー角値である。したがって、発進時の精密ヨー角値とは異なる。また、車速も発進時には精度よく取得できない場合があり、車速加重平均値演算の対象とする走行区間にも発進時を含むことができないことも多い。

しかし、発進地点と測定地点の距離が近い時など、発進地点から測定地点までのヨー角変化が小さい場合には、測定地点のヨー角値を近似的に発進地点でのヨー角値と見なせる場合がある。その場合は、発進地点になるべく近い測定地点の精密ヨー角値から発進時のヨー角誤差を算出してよい。つまり、補正時点までの走行区間の中間地点での方向を推定し、その推定方向に基づき、相対位置を補正することもできる。

そして、発進地点から旋回を開始するなどにより、発進地点から測定地点までのヨー角変化が大きい場合には、発進時の精密ヨー角値を推定する必要がある。その場合、発進地点近傍での曲率変化を一定（定常円或いはクロソイド曲線）と仮定すると、円旋回ではヨー角変化が距離に比例するので、図８に示すように、２箇所の測定地点で算出した精密ヨー角（ヨー角の車速加重平均）ＹａｗＡ、ＹａｗＢを示す２点の延長線から発進地点での精密ヨー角Ｙａｗｓｔａｒｔ（発進時ヨー角ずれ）を推定することができる。

本実施形態の車両制御装置では、発進地点から円旋回を開始した場合に、発進地点近傍での曲率変化を一定（定常円或いはクロソイド曲線）と仮定し、発進した後の２箇所の時点で得られるヨー角変位量の車速加重平均を用いて、発進地点（推定開始位置）における精密ヨー角（Ｙａｗｓｔａｒｔ）を算出する。

測定地点は1点または2点に限らず、複数地点とすること、また、延長線を直線に限らず、二次曲線などの曲線にすることで、曲率変化がある場合にも対応が可能な推定が可能になる。発進地点でのあるべきヨー角と精密ヨー角との差分により発進時のヨー角誤差を算出することができる。したがって、発進時のヨー角誤差に基づいて、発進位置からの相対方向を補正することができる。そして、発進時のヨー角誤差を補正した後に、推定した走行距離とヨー角の関係に基づいて、相対位置を補正することができる。本実施形態の車両制御装置によれば、発進地点から旋回(定常円或いはクロソイド曲線の旋回)を開始していた場合でも対応が可能であり、発進時における車両の進行方向の推定精度を向上させることができる。

また、本実施形態の車両制御装置では、誤った精密ヨー角が算出されるのを防ぐためのフェールセーフ機能を付加している。例えば、発進地点における精密ヨー角Ｙａｗｓｔａｒｔ（発進時ヨー角ずれ）をθ０とし、発進地点から距離Ｌ１だけ離れた第１の地点Ａにおける精密ヨー角（ヨー角の車速加重平均）ＹａｗＡをθ１とし、発進地点から距離Ｌ１の２倍だけ離れた第２の地点Ｂにおける精密ヨー角（ヨー角の車速加重平均）ＹａｗＢをθ２とする。この場合、第１の地点Ａにおける精密ヨー角ＹａｗＡの誤差は、発進地点における精密ヨー角Ｙａｗｓｔａｒｔにおいて２倍になる（θ０＝２×(θ１−θ２)）。したがって、精密ヨー角ＹａｗＡの異常値は除去または補正することが望ましい。

ここで、図８に示すように、距離とヨー角との傾きは、車両性能（最小旋回半径等）によって限界があり、θ１とθ２の差分の大きさも上限が存在するはずである。したがって、距離とヨー角との関係による制限を設けて、下記の式の要件を満たさない精密ヨー角ＹａｗＡの値は、異常値として除去または補正する処理を行う。
｜θ１−θ２｜＜Ｋ１・・・式(7）
（Ｋ１：車両性能から算出される）

つまり、発進した後の複数の時点で得られるヨー角値の車速加重平均どうしの差分が、車両の車両性能から算出される上限値Ｋ１以上の場合に、第１の地点Ａにおける精密ヨー角θ１は異常値として抽出され、除去または補正される。

一時停車においては、停車中に車輪の微小な動きによって発生する車輪速パルスカウントの回転方向が分からないことがあり、その場合にはそれによる推定ヨー角、推定位置への反映を保留にする。反映を保留する一方で、反映を保留した左右輪の車輪速パルスカウント数を記憶する。次に、図９に示すように、停車前と再発進時の精密ヨー角を算出する。

再発進時の精密ヨー角の算出方法は、発進時と同様の方法である。停車時の精密ヨー角も、発進時、再発進時と類似の方法で算出することができる。停車前の1点もしくは複数の地点の精密ヨー角から停車時の精密ヨー角を推定することができる。

停車時の精密ヨー角と再発進時の精密ヨー角の差分には、停車中の挙動と車輪速パルスカウントの反映の保留分が現れる。差分は、式(8)で表すことができる。
(再発進時の精密ヨー角)- (停車時の精密ヨー角)=(停車中の挙動分)+(車輪速パルスカウントの反映保留分)+(測定誤差)・・・式(8)

ここで、停車中の挙動分は補正する必要がなく、測定誤差分は補正できない。ここでは、停車時と再発進時の精密ヨー角の差分情報(ヨー角ギャップと呼ぶものとする)を元に、停車中にカウントされた左右の車輪速パルスカウントの保留分を正しく推定ヨー角及び推定位置に反映することを考える。

停車中に車輪が十分に制動されている場合、停車中に車輪が車輪速パルスカウント数パルス分も同一方向に転がることは考えにくいため、せいぜい1カウント分の距離を転がったものであると限定する仮定を置くことができる。停車中に車両を外部から揺さぶったりした場合に、車両は元の場所に停車したままであっても複数カウント検出されることがあるが、これは車輪の1カウント分の距離の中で往復運動的に転がったものと考えることができ、1回の往復運動で2カウント発生するとした場合、偶数カウントの発生は無視することができる。従って、停車中にカウントされた左右の車輪速パルスカウントは、左右それぞれ偶数か奇数かを記憶しておけばよい。偶数か奇数か、もしくは0か1かでもよい。

停車中にカウントされる左右の車輪速パルスカウントのパターンは、偶数と偶数、偶数と奇数、奇数と偶数、奇数と奇数の４パターンとなる。偶数と偶数のパターンでは、ヨー角ギャップは、(停車中の挙動分)+(測定誤差分)となり、補正の必要はない。

偶数と奇数、奇数と偶数のパターンでは、ヨー角ギャップは、(停車中の挙動分)+(車輪速パルスカウントの反映保留分)+(測定誤差分) となるが、(停車中の挙動分)と(測定誤差分)の和が(車輪速パルスカウントの反映保留分)よりも小さい場合には、ヨー角ギャップの符号は、(車輪速パルスカウントの反映保留分)の符号と一致する。ここから、車輪速パルスカウントの反映保留分の符号を求めることができ、左輪もしくは右輪でカウントされた奇数カウント、実効的には１カウントはその車輪が前後どちらの方向に回転して発生したものか判別することができる。

奇数と奇数のパターンでは、(車輪速パルスカウントの反映保留分)は、左右輪とも前方回転、左右輪とも後方回転の場合にはゼロとなり、左輪が前方回転で右輪が後方回転の場合には時計回り方向、左輪が後方回転で右輪が前方回転の場合には反時計回り方向となる。2カウント分のヨー角変化は(停車中の挙動分)と(測定誤差分)の和と比較して大きい場合には、この三種類の(車輪速パルスカウントの反映保留分)を2つの閾値で識別することが可能である。これによって、左右輪が相互に逆方向に回転した場合は左右輪それぞれの回転方向を推定することができる。一方で左右輪が同一方向に回転した場合は左右輪が前後どちらに回転したかはこの方法で判別することはできない。その場合、例えば、その他の情報、例えば加速度センサによる加速度変動情報や勾配情報、駆動力方向の変更情報、などによって判別することができる。他に判別するための情報がない場合は、これらのパルスカウントの反映保留分は反映しないまま破棄する。破棄した場合には、ヨー角の誤差は発生しないが、前後方向の位置変位情報の誤差が生じる。

このようにして、一時停車時に進行方向が分からない車輪速パルスカウントがあった場合でも、その進行方向を推定することで、自車位置を精緻に推定することが可能になる。

以下、本実施形態の具体例について図面に基づき説明する。なお、各図中、同一の作用又は機能を有する部材又は要素には同一の符号を付し、重複した説明を適宜省略する。

以下で説明する本実施形態は、自動駐車システムに本発明による車両制御装置を備えた自車位置推定システムを適用した例である。自動駐車システムは、車両を比較的低速で制御するので、車輪速センサを使用した自車位置推定の誤差原因となる車輪の滑りが殆ど発生しない。したがって、自動駐車システムに対して本発明による車両制御装置を備えた自車位置推定システムを適用するのに好適である。本実施形態における自動駐車システムは、カメラ、レーダー等の外界認識手段によって、車両と外界のさまざまな物体との相対位置関係を検出し、自動駐車開始地点、自動駐車目標地点、自動駐車経路などを計画することができる。

自動駐車システムは、図１０に示すように、本発明による車両制御装置を備えた自車位置推定システムによる自車位置推定によって、自車位置Ｐｖと自動駐車経路Ｋを比較しながらフィードバック制御することで、自車両Ｖｏを自動駐車開始地点Ｐｓ(開始位置)から切返し地点Ｐｍを経由して自動駐車目標地点（目標位置）Ｐｅまで到達させる。しかし、以下の本実施形態の説明では、説明の簡素化のために、自動駐車システムにおける自車位置推定システムの説明に絞るものとする。

図１１は、本発明に係る車両制御装置の一実施形態を備えた自車位置推定システムの一例を示す全体構成図である。図１１に示すように、自車位置推定システム１は、主に、左右の前輪からなる左右操舵輪５１、５２及び左右の後輪からなる左右非操舵輪５３、５４に設けられた車輪速センサ６１〜６４と、車輪速センサ６１〜６４と接続された制動力制御装置としてのABS/ESC_ECU(Electronic Control Unit)１１と、変速機５５に設けられたギアポジション検出センサ６５（図１２参照）と、ギアポジション検出センサ６５と接続された変速機ECU１２と、車両制御装置としての自動駐車コントローラ１０とを備え、ABS/ESC_ECU１１と変速機ECU１２と自動駐車コントローラ１０とは、車内ネットワーク(CAN)１５を介して相互に通信可能に接続されている。

車輪速センサ６１〜６４は、車輪の回転に応じてパルス波形を生成し、ABS/ESC_ECU１１側でパルス波形の立ち上がりと立ち下がりを検出し、その回数(パルス数)をカウントし、自動駐車コントローラ１０に常時通知する。ギアポジション検出センサ６５は、変速機５５の前進ギアと後退ギアを判別するためのセンサで、変速機ECU１２側で現在のギアポジションを検出し、自動駐車コントローラ１０に常時通知する。

図１２は、図１１に示す自動駐車システムの制御構成を示す制御ブロック図である。図３に示すように、ABS/ESC_ECU１１は、車輪速パルスカウント部２１を有し、変速機ECU１２は、ギアポジション検出部２２を有し、自動駐車コントローラ１０は、自車位置推定部２０を有する。

車輪速パルスカウント部２１は、ABS/ESC_ECU１１内に実装される制御ブロックであり、車輪速センサ６１〜６４から発生する波形をパルス波形に整形して、パルス波形の立ち上がりエッジと立ち下がりエッジの両方をカウントする。そのカウント値(車輪速パルスカウント)は、例えば０〜２５５の整数値で表され、２５５を超えると０に戻るという循環値となっている。車輪速パルスカウント部２１は、カウント値を含む情報を車内ネットワーク(CAN)１５に送信しており、自動駐車コントローラ１０を含む他のコントローラは、この情報を車内ネットワーク(CAN)１５から受信することによって、車輪速センサ６１〜６４のカウンタ値を取得することができる。

ギアポジション検出部２２は、変速機ECU１２内に実装される制御ブロックであり、ギアポジション検出センサ６５によって検出したギアポジション情報を含む情報を車内ネットワーク(CAN)１５に送信しており、自動駐車コントローラ１０を含む他のコントローラは、この情報を車内ネットワーク(CAN)１５から受信することによって、ギアポジション情報を取得することができる。

自車位置推定部２０は、自動駐車コントローラ１０内に実装される制御ブロックであり、車輪速パルスカウント部２１で生成される各車輪の車輪速センサ６１〜６４のカウンタ値と、ギアポジション検出部２２で生成されるギアポジション情報とから、推定開始状態からの車両の相対位置と相対方向とを出力して当該車両の自車位置を推定し、この推定自車位置情報を自動駐車コントローラ１０内の他の機能に提供する。

図１３は、図１２に示す自車位置推定部におけるデッドレコニングによる自車位置推定方法を概略的に示すフローチャートである。本フローチャートで示すシーケンスは、自車位置推定の推定開始後に制御周期毎に動作する。

自車位置推定部２０による処理を開始すると、最初に、左右後輪(非操舵輪)でカウントアップがあるか否かを判定する(ステップＳ１０１)。カウントアップが無ければ、その制御周期での処理は終了となる。一方、カウントアップが有る場合は、前記の式(2)にて走行距離Uを計算する(ステップＳ１０２)。次に、ギアポジション検出部２２で生成されるギアポジション情報から進行方向を検出する(ステップＳ１０３)。進行方向の検出は、ギアポジションが前進ギアであれば、進行方向は+1とし、ギアポジションが後退ギアであれば、進行方向は-1とする。次に、前記の式(1)にてヨー角変位量θを計算する(ステップＳ１０４)。次に、前記の式(3)にてヨー角更新を計算する(ステップＳ１０５)。次に、後述する車速加重平均ヨー角の演算をおこなう(ステップＳ１０６)。次に、前記の式(4)にてX座標更新を計算し、前記の式(5)にてY座標更新を計算する(ステップＳ１０７)。

なお、前記したように、車輪速センサに車輪の回転方向を検出する機構が付いている場合は、ギアポジション情報から前後進行方向を検出する代わりに、車輪速センサから得られる情報を基に前後進行方向を検出することができる。

図１４と図１５で、図１３における車速加重平均ヨー角の演算方法を説明する。図１４は、図１５の車速加重平均ヨー角の演算において使用する一時記憶用メモリを示しており、車速加重平均ヨー角の演算用テーブルが示されている。図１５は車速加重平均ヨー角の演算方法を示すフローチャートである。

自車位置推定部２０では、図１４に示す一時記憶用メモリに、制御周期毎に、格納済みの内容を一段下方にシフトし、最上段に最新の車速とヨー角を格納する。ここでメモリ使用量削減のために、変化時のみ車速とヨー角を格納する方法もある。例えば、車速とヨー角に加えて当該車速とヨー角が変化せずに継続した時間を追加で格納するものとし、車速かヨー角が変化した時だけ最新の車速とヨー角を格納し、変化しない場合は継続時間を1周期分インクリメントするような格納方法がある。

図１５において、車速加重平均ヨー角の演算処理は、引数Kを必要とする。引数Kは一時記憶用メモリの演算開始インデックスを示し、指定したインデックスを基点としてそれより過去の情報が演算の対象となる。一時記憶用メモリの最新の情報まで使用する場合は、引数Kは0を指定する。

演算処理は、まず、引数Kをkに代入する(ステップＳ２０１)。次に、初期化として距離に関する変数LをL=0、ヨー角に関する変数YをY=0とする(ステップＳ２０２)。次に、車速ヨー角テーブルを参照して、L=L+VSPk×Tu, Y=Y+Yawk×VSPk×Tuを算出する(ステップＳ２０３)。ここでTuは制御周期時間を示す。次に、kをカウントアップする(ステップＳ２０４)。次に、Lが演算区間長さLmax以上になったか否かを調べる(ステップＳ２０５)。Lmax以上になっていなければ、ステップＳ２０３に戻り加算演算を繰り返す。Lmax以上になっていれば、ステップＳ２０３に戻り加算演算を終了し、Yawave=Y÷Lで車速加重平均ヨー角を算出する(ステップＳ２０６)。演算の戻り値として、インデックスに使用したk（戻り値１）と車速加重平均ヨー角（Yawave：戻り値２）を返す。

図１６は、図１２に示す自車位置推定部におけるデッドレコニングによる自車位置推定情報を推定開始時点としての発進の発進直後に、必要かつ十分なタイミングで自車位置推定情報の補正を実行する方法を示す状態遷移図である。

自車位置推定開始時の状態は"発進前"である(Ｓ３０１)。"発進前"の状態において発進を検出すると"発進直後A"に遷移する(Ｓ３０２)。発進の検出は、例えば、ある一定時間内に全車輪で車輪速パルスカウントのカウントアップが検出されたことをもって行う。"発進直後A"の状態において累積走行距離がLaを越えたら、フロー(A)を実行し、"発進直後B"の状態に遷移する(Ｓ３０３)。"発進直後B"の状態において累積走行距離がLbを越えたら、フロー(B)を実行し、"走行中"の状態に遷移する(Ｓ３０４)。"走行中"の状態は自車位置推定終了まで継続する。

図１７は、図１６に示した状態遷移図において、処理の簡素化を行った処理を示す状態遷移図である。

自車位置推定開始時の状態は"発進前"である(Ｓ４０１)。"発進前"の状態において発進を検出すると"発進直後B"に遷移する(Ｓ４０２)。発進の検出は、例えば、ある一定時間内に全車輪で車輪速パルスカウントのカウントアップが検出されたことをもって行う。"発進直後B"の状態において累積走行距離がLbを越えたら、フロー(B)を実行し、"走行中"の状態に遷移する(Ｓ４０３)。"走行中"の状態は自車位置推定終了まで継続する。

図１８は、図１６において、”発進直後A"から"発進直後B"に遷移する際に実行する処理のフローチャートである。処理を開始すると、その時点での車速加重平均ヨー角YawaveをYawAとして取得する(ステップＳ５０１)。

図１９は、図１６、図１７において、”発進直後B"から"走行中"に遷移する際に実行する処理のフローチャートである。処理を開始すると、その時点での車速加重平均ヨー角YawaveをYawBとして取得する(ステップＳ６０１)。

次に、発進時ヨー角算出を式(9)で算出する(ステップＳ６０２)。簡素化処理では、式(9)'で算出する。
発進時ヨー角Yawstart=(Lb-Lmax/2)/(Lb-La)×YawA-(La-Lmax/2)/(Lb-La)×YawB・・・式(9)
発進時ヨー角Yawstart=YawB・・・式(9)'

次に、ヨー角補正は式(10)、(11)で実施する。
推定開始時ヨー角誤差Yawerror=（Yawstart）-(推定開始時基準ヨー角)・・・式(10)
補正後推定ヨー角=（補正前推定ヨー角）-（Yawerror）・・・式(11)

次に、座標補正を実行する。座標補正の方法は次の図２０で説明する。座標補正後、本フローを終了する。

図２０は、発進時の座標補正を説明する図である。座標補正では、発進地点から距離Lbだけ走行した時点での位置は、距離Lb/2だけ走行した時点でのヨー角方向に距離Lbだけ移動した地点で近似するものとする。

座標補正では、まず発進地点から距離Lb/2だけ走行した地点でのヨー角(中間地点のヨー角、中間ヨー角Yawmidと呼ぶものとする)を式(12)で求める。
中間ヨー角Yawmid=(Lb-Lmax)/(Lb-La)×YawA/2+(Lb-2La+Lmax)/(Lb-La)×YawB/2-Yawerror・・・式(12)

次に式(13)、式(14)で発進地点から中間ヨー角方向に距離Lb/2だけ移動した座標変位を座標補正分とする。
座標補正分X=Lb/2×sin(Yawmid)・・・式(13)
座標補正分Y=Lb/2×cos(Yawmid)・・・式(14)

発進地点の座標に座標補正分を式(15)、式(16)で足し込むことで、補正後の座標とする。
補正後の座標X=発進地点座標X+座標補正分X・・・式(15)
補正後の座標Y=発進地点座標Y+座標補正分Y・・・式(16)

発進地点座標は、例えば推定開始地点を発進地点座標とし、推定開始地点を座標原点とする場合は(0, 0)である。

図２１は、図１２に示す自車位置推定部におけるデッドレコニングによる自車位置推定情報を一時停車後の発進直後に、必要かつ十分なタイミングで自車位置推定情報の補正を実行する方法を示す状態遷移図である。

本状態遷移は循環するが、"走行中"状態（Ｓ７０４）から説明する。"走行中"の状態（Ｓ７０４）において停車を検出するとフロー(C)を実行し、"停車中"に遷移する（Ｓ７０１）。停車の検出は、例えば、ある一定時間内に全車輪で車輪速パルスのカウントアップが検出されないことをもって行う。"停車中"状態（Ｓ７０１）においては、停車中カウントとして、非操舵輪左右輪の停車中の車輪速パルスのカウントアップ回数を数えている。"停車中"の状態において発進を検出すると"発進直後A"に遷移する（Ｓ７０２）。発進の検出は、例えば、ある一定時間内に全車輪で車輪速パルスカウントのカウントアップが検出されたことをもって行う。ここで、発進の検出の対象となった車輪速パルスのカウントアップは、進行方向が判明していればその進行方向に従って推定ヨー角、推定位置に反映させ、停車中カウントからは除外する。"発進直後A"の状態（Ｓ７０２）において累積走行距離がLaを越えたら、フロー(A)を実行し、"発進直後B"の状態（Ｓ７０３）に遷移する。"発進直後B"の状態（Ｓ７０３）において累積走行距離がLbを越えたら、フロー(D)を実行し、"走行中"の状態（Ｓ７０４）に遷移する。

図２２は、図２１に示した状態遷移図において、処理の簡素化を行った処理を示す状態遷移図である。

本状態遷移は循環するが、"走行中"状態（Ｓ８０３）から説明する。"走行中"の状態（Ｓ８０３）において停車を検出するとフロー(C)を実行し、"停車中"に遷移する（Ｓ８０１）。停車の検出は、例えば、ある一定時間内に全車輪で車輪速パルスのカウントアップが検出されないことをもって行う。"停車中"状態（Ｓ８０１）においては、停車中カウントとして、非操舵輪左右輪の停車中の車輪速パルスのカウントアップ回数を数えている。"発進前"の状態において発進を検出すると"発進直後Ｂ"（Ｓ８０２）に遷移する。発進の検出は、例えば、ある一定時間内に全車輪で車輪速パルスカウントのカウントアップが検出されたことをもって行う。ここで、発進の検出の対象となった車輪速パルスのカウントアップは、進行方向が判明していればその進行方向に従って推定ヨー角、推定位置に反映させ、停車中カウントからは除外する。"発進直後Ｂ"の状態（Ｓ８０２）において累積走行距離がLbを越えたら、フロー(Ｄ)を実行し、"走行中"の状態（Ｓ８０３）に遷移する。

図２３は、図２１、図２２において、"走行中"から"停車中"に遷移する際に実行する処理のフローチャートである。

処理を開始すると、車速加重平均ヨー角演算用テーブルにおいて最後に車輪速パルスのカウントアップインデックスを取得し、X0とする（ステップＳ９０１）。次に、引数K=X0として、車速加重平均ヨー角Yawaveを演算し、YawCとして取得する（ステップＳ９０２）。このときの戻り値kをX1とする。

次に、引数K=X1として、車速加重平均ヨー角Yawaveを演算し、YawDとして取得する（ステップＳ９０３）。次に、停車時ヨー角Yawstopを式(17)で求める（ステップＳ９０４）。
停車時ヨー角Yawstop=(3/2)×YawC-(1/2)×YawD・・・式(17)
フロー(C)は以上である。

図２４は、図２１、図２２において、”発進直後B"から"走行中"に遷移する際に実行する処理のフロー(Ｄ)である。そして、図２５は、本処理で使用する判定表である。フロー(Ｄ)では、処理を開始すると、その時点での車速加重平均ヨー角YawaveをYawBとして取得する(ステップＳ１００１)。

次に、発進時ヨー角Yawstartを前記式(8)で算出する(ステップＳ１００２)。簡素化処理では、前記式(8)'で算出する。

次に、発進時ヨー角Yawstartと停車時ヨー角Yawstopから式(18)で停車時発進時ヨー角差分Yawdiffを求める(ステップＳ１００３)。
停車時発進時ヨー角差分Yawdiff=Yawstart-Yawstop・・・式(18)

次に、停車中カウントと停車時発進時ヨー角差分Yawdiffから、図２５の判定表に従って、左右輪の停車中パルスの回転方向とヨー角誤差量Yawerror、走行距離補正量Lerrorを求める（ステップＳ１００４）。停車中カウントが左右輪とも奇数でYawdiffが0近傍の場合は、車両がヨー角の変動なく前後に動いた可能性が考えられるが、ここでは例えば、停車中に車両の駆動力方向を変更したのであれば、変更後の方向に車両が動いた可能性が高いため、駆動力方向変更情報に基づいて、左右両輪の回転方向を判定する方法がある。

次に、ヨー角補正を実行する（ステップＳ１００５）。ヨー角補正は、式(19)で実施する。
補正後推定ヨー角=補正前推定ヨー角-Yawerror・・・式(19)

次に、座標補正を実行する（ステップＳ１００６）。座標補正の方法は図２０で説明した方法と同様である。ただし、距離に関して、停車中パルスによる走行距離補正量分Zを反映する。発進後の進行方向が前進であれば、ZはLerrorを加算し、後退であれば、ZはLerrorの符号を反転して加算する。
座標補正分X=(Lb/2+Z)×sin(Yawmid)・・・式(20)
座標補正分Y=(Lb/2+Z)×cos(Yawmid)・・・式(21)

次に、座標補正分を停車地点の座標に加算する。
補正後の座標X=停車地点座標X+座標補正分X・・・式(22)
補正後の座標Y=停車地点座標Y+座標補正分Y・・・式(23)
座標補正後、本フローを終了する。

以上、本発明の実施形態について詳述したが、本発明は、前記の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の精神を逸脱しない範囲で、種々の設計変更を行うことができるものである。例えば、前記した実施の形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることが可能であり、また、ある実施形態の構成に他の実施形態の構成を加えることも可能である。さらに、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

１…自車位置推定システム、１０…自動駐車コントローラ(車両制御装置)、１１…ABS/ESC-ECU、１２…変速機ECU、１５…車内ネットワーク(CAN)、２０…自車位置推定部、２１…車輪速パルスカウント部、２２…ギアポジション検出部、５１…右前輪(右操舵輪)、５２…左前輪(左操舵輪)、５３…右後輪(右非操舵輪)、５４…左後輪(左非操舵輪)、５５…変速機、６１〜６４…車輪速センサ、６５…ギアポジション検出センサ

Claims

車両に搭載したセンサの情報に基づいてヨー角変位量と走行距離と前後進行方向を算出し、該ヨー角変位量と走行距離と前後進行方向から車両位置の推定開始状態からの相対位置と相対方向を算出して自車位置を推定し、
前記相対方向は、一定距離もしくは一定時間の間のヨー角値の車速加重平均を演算することによって得られ、
前記車両が推定開始地点から発進した直後に得られる前記ヨー角値の車速加重平均から、前記推定開始地点におけるヨー角誤差を算出することを特徴とする車両制御装置。
車両に搭載したセンサの情報に基づいてヨー角変位量と走行距離と前後進行方向を算出し、該ヨー角変位量と走行距離と前後進行方向から車両位置の推定開始状態からの相対位置と相対方向を算出して自車位置を推定し、
前記相対方向は、一定距離もしくは一定時間の間のヨー角値の車速加重平均を演算することによって得られ、
前記車両が推定開始地点から発進した後の複数の地点で得られる前記ヨー角値の車速加重平均から、前記推定開始地点におけるヨー角誤差を算出することを特徴とする車両制御装置。
前記ヨー角変位量は、左右の非操舵輪の走行距離の差分と、前記左右の非操舵輪の間の中心から前記左右の非操舵輪までの左右間隔と、から算出することを特徴とする請求項１または２に記載の車両制御装置。
前記走行距離は、左右の非操舵輪の走行距離の平均から算出することを特徴とする請求項１または２に記載の車両制御装置。
前記左右の非操舵輪の走行距離は、車輪速センサで車輪の回転によって発生するパルスをカウントすることで、算出することを特徴とする請求項４に記載の車両制御装置。
前記推定開始地点における前記ヨー角誤差に基づき、前記相対方向を補正することを特徴とする請求項２に記載の車両制御装置。
前記推定開始地点における前記ヨー角誤差に基づき、前記推定開始地点を中心として前記相対位置を回転移動することで、前記相対位置を補正することを特徴とする請求項２に記載の車両制御装置。
補正時点までの走行区間の中間地点での方向を推定し、該中間地点での推定方向に基づき、前記相対位置を補正することを特徴とする請求項２に記載の車両制御装置。
前記発進した後の複数の時点で得られる前記ヨー角値の車速加重平均の中から異常値を抽出し、該異常値を補正または除去することを特徴とする請求項２に記載の車両制御装置。