(57)【要約】【課題】ＧＰＳアンテナを増設することなく、既存の車載用ナビゲーションシステムを用いて停車中の車両の指向方向を検出することができるようにする。【解決手段】自動操縦により駐車スペース２１に後進で駐車させる際の基準となる切返し点Ｐの位置座標、及びそのときの基準車両１前方を撮像したステレオ画像に基づき基準車両１から前方立体物の縦方向エッジｗ，ｕまでの距離Ｐｗ，Ｐｕを算出して記憶する。その後対象車両１’を切返し点Ｐを目標として停車させたときの実際の切返し点Ｑにおける位置座標、及びそのときの対象車両１’前方を撮像したステレオ画像に基づき縦方向エッジｗ，ｕまでの距離Ｑｗ，Ｑｕを算出し、各距離Ｐｗ，Ｐｕ，Ｑｗ，Ｑｕと両車両１，１’間の距離ＰＱに基づき余弦定理及びベクトルの内積を用いて、車両１の指向方向に対する対象車両１’の指向方向の角度差θｘを算出する。【選択図】図１０
2007278898.tif 000002
【特許請求の範囲】
【請求項１】
車両の位置情報を取得する位置情報取得手段と、
取得した位置情報を記憶する位置情報記憶手段と、
自車両の周辺を撮像する撮像手段と、
撮像画像から立体物を検出し当該立体物の位置情報を求める画像処理手段と、
前記位置情報を記憶する画像記憶手段と、
前記自車両の向きを算出する向き算出手段と
を備え、
前記向き算出手段は、前記位置情報記憶手段と前記画像記憶手段に記憶している自車両の位置情報及び前記立体物の位置情報と、新たに取得した自車両の位置情報及び前記立体物の位置情報とから、新たに取得した車両位置における車両向きを算出する
ことを特徴とする車両の向き検出装置。
【請求項２】
前記向き算出手段は、前記車両の位置情報を車両が停止した状態で検出する
ことを特長とする請求項１記載の車両の向き検出装置。
【請求項３】
前記向き算出手段は、前記自車両が前記位置情報取得手段からの位置情報に基づき停車位置或いは該停車位置近傍に到達した後、少なくとも車速がゼロで且つブレーキ動作状態のとき停車と判定する
ことを特徴とする請求項２記載の車両の向き検出装置。
【請求項４】
前記向き算出手段は、前記画像処理手段で検出した前記立体物の輝度変化量から抽出した縦方向エッジの位置情報と前記自車位置情報とに基づいて前記車両の向きを算出する
ことを特徴とする請求項２記載の車両の向き検出装置。
【請求項５】
前記縦方向エッジとして、固定物の縦方向エッジを検出するため、車両が停止した状態において、設定した時間間隔を有して撮像した画像間において縦方向エッジの位置情報が変化しないものとする
ことを特徴とする請求項４記載の車両の向き検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自車両の位置情報と撮像手段で撮像した自車両周辺の撮像画像とに基づいて自車両の向きを算出する車両の向き検出装置に関する。
【背景技術】
【０００２】
近年、人工衛星から得られる位置データに基づいて車両の位置を検出するＧＰＳ（Global Positioning System）が、車載用ナビゲーションシステム（位置情報取得手段）において広く用いられており、このＧＰＳで検出した自車位置情報や、カメラ等により検出した前方の道路情報を基に走行制御する様々な技術が提案され、実用化されている。
【０００３】
例えば、特許文献１（特開２００１−２５５９３７号公報）には、車載用ナビゲーションシステムで、運転者が目的地を設定し、この車載用ナビゲーションシステムにより生成される目的地までの走行軌跡を目標誘導路として自動操舵や自動加減速により自車を自動操縦制御する技術が開示されている。
【０００４】
ところで、走行中の自車の向きは、ＧＰＳで検出した自車位置情報に基づいて算出した自車進行方向から推定することができる。従って、停車中の車両は、自車位置を検出することはできるが、自車の向き（以下「指向方向」）は、停車中の車両を発進、或いは後進に際し僅かに移動させて、少なくとも２つの自車位置情報が車載用ナビゲーションシステムに入力されるまでは推定することができないという問題がある。
【０００５】
これに対処するに、例えば特許文献２（特開２００５−７０９８３号公報）に開示されているように、車両の前後に２つのＧＰＳアンテナを配設すれば、それぞれのＧＰＳアンテナで検出した自車位置座標を結ぶことで自車両の向き（指向方向）を検出することができる。
【特許文献１】特開２００１−２５５９３７号公報
【特許文献２】特開２００５−７０９８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、１台の車両に複数のＧＰＳアンテナを設置する場合、コスト高となる問題がある。
【０００７】
又、２つのＧＰＳアンテナを車両に取付ける場合、なるべく離れた位置に設置することが、検出誤差をより小さくする上で好ましいが、車両全長が比較的短い、自家用車等の車両では２つのＧＰＳアンテナを充分に離した位置に設置することができず誤差が生じやすくなり、自車の指向方向を高い精度で検出することができない問題がある。
【０００８】
本発明は、前記事情に鑑み、アンテナ等を増設することなく、既存の位置情報取得手段を用いて停車中の車両の指向方向を、低コストで、しかも高精度に検出することのできる車両の向き検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
前記目的を達成するため本発明による車両の向き検出装置は、車両の位置情報を取得する位置情報取得手段と、取得した位置情報を記憶する位置情報記憶手段と、自車両の周辺を撮像する撮像手段と、撮像画像から立体物を検出し当該立体物の位置情報を求める画像処理手段と、前記位置情報を記憶する画像記憶手段と、前記自車両の向きを算出する向き算出手段とを備え、前記向き算出手段は、前記位置情報記憶手段と前記画像記憶手段に記憶している自車両の位置情報及び前記立体物の位置情報と、新たに取得した自車両の位置情報及び前記立体物の位置情報とから、新たに取得した車両位置における車両向きを算出することを特徴とする。
【発明の効果】
【００１０】
本発明によれば、自車両の位置情報と撮像手段で撮像した自車両周辺の撮像画像とに基づいて自車両の向きを算出するようにしたので、アンテナ等を増設することなく、既存の車両位置検出手段を用いて停車中の車両の指向方向を、低コストで、しかも高精度に検出することができる。
【発明を実施するための最良の形態】
【００１１】
以下、図面に基づいて本発明の一形態を説明する。図１に車両の駐車支援装置の全体構成図を示す。
【００１２】
同図の符号１は自動車等の車両で、この車両（自車）１にＲＴＫ（Real-Time Kinematic）−ＧＰＳを用いて制御を行う移動局としての走行制御装置２が搭載されている。本形態におけるＲＴＫ−ＧＰＳでは、地球を周回する人工衛星（ＧＰＳ衛星）３からの情報（測位計算等に必要な衛星の軌道情報をはじめとするデータ等）が、基準局４と車両１に設けた走行制御装置２との双方で受信される。
【００１３】
基準局４は、ＧＰＳアンテナ４ａ、ＧＰＳ受信機４ｂ、無線機４ｃ等を備えて、例えば車両１を駐車する駐車スペース２１（図９参照）に隣接する位置に設置されている。この基準局４の設置位置は予め正確に特定されており、この設置位置を基準として、自己が観測したＧＰＳ衛星３からの電波の位相情報、疑似距離、基準局４の位置情報（自己位置情報）である位置座標、及び誤差補正量、疑似距離補正量、座標値等の補正情報を作成し、この情報を車両１に搭載されている走行制御装置２に無線機４ｃを介して送信する。
【００１４】
無線機４ｃは、例えば、ＩＥＥＥ８０２．１１ａ／ｂ／ｇ等の規格による無線ＬＡＮ（Local Area Network）に基づき送受信するアクセスポイントであり、通信のセキュリティを維持するためＳＳＩＤ（Service Set ID）、ＷＥＰ（Wired Equivalent Privacy）キー、ＭＡＣ（Media Access Control）アドレス認証の設定が特有になされている。図９に示すように、本形態では、基準局４を中心として駐車エリアが設定されている。この駐車エリアは基準局４の受信可能範囲であれば、車両１に搭載されている車両制御手段としての制御装置８（後述する）で任意に設定することができる。
【００１５】
車両１には、ＧＰＳアンテナ５ａ、車載用ナビゲーションシステムを構成するＧＰＳ受信機５ｂ、基準局４との通信を行う無線機５ｃが搭載されている。ＧＰＳ受信機５ｂは位置情報取得手段としての機能を有し、車両１が駐車エリアに入り、 基準局４との通信が確立されると、基準局４から送信される誤差補正量、疑似距離補正量、座標値等のデータ（無線機５ｃで受信されるデータ）や、車両１で受信したＧＰＳ衛星３からの情報を比較解析し、自車位置（座標値）を、例えば、１〜５cm程度の誤差で検出する。
【００１６】
走行制御装置２に設けられている制御装置８に、上述したＧＰＳ受信機５ｂと画像処理手段としての画像処理部７とが接続されている。更に、この画像処理部７に車載カメラ６が接続されている。この車載カメラ６は左右一対のカメラで構成され、各カメラは車室内前部のルームミラー付近に、車幅方向に所定間隔を開けて並列に配設されている。従って、車載カメラ６では車両１前方の同一風景が異なる視点で各々撮像される。そして、この車載カメラ６で撮像したステレオ画像が画像処理部７に入力される。
【００１７】
画像処理部７は、入力されたステレオ画像に三角測量の原理に基づく画像処理を行うことで、画像中の同一対象物の３次元の距離分布を算出する。そして、算出した距離分布情報から前方立体物等を検出し、車両１から前方立体物までの３次元位置データを求める。
【００１８】
制御装置８には、ＧＰＳ受信機５ｂや画像処理部７の他、車速Ｖを検出する車速センサ９、ハンドル角θHを検出するハンドル角センサ１０、シフトポジションを検出するポジションセンサ１９等のセンサ類、及び自動操縦制御用メインスイッチ１１、ブレーキペダルの踏込みでＯＮ動作するブレーキスイッチ１２、ＯＮ操作により切返し点を登録する切返し点登録スイッチ（図示せず）等のスイッチ類が接続されている。
【００１９】
制御装置８には、ＨＤＤ、フラッシュメモリ、ＣＤ、ＤＶＤ等の読み書き自在な記憶手段（本発明の位置情報記憶手段と画像記憶手段との機能を備えている）が設けられており、基準局４との通信が可能なエリアで生成された学習走行軌跡データκを構成するノード点（後述する）の集合（以下、「ノード列」と称する）等の地図情報、及び切返し点で撮像したステレオ画像を処理して得られた前方立体物の特定部位を示す位置データとしての縦方向エッジ（後述する）を示す３次元位置データが格納される。
【００２０】
記憶手段から読込まれた地図情報は、例えばダッシュボード上に設けられた表示手段としての液晶ディスプレイ１４に適宜表示され、基準局４の位置と、車両１の現在位置と、自動操縦により誘導される目標誘導路、過去に生成した学習走行軌跡データ（ノード列）、或いは、現在生成している走行軌跡データ（ノード列）が表示される。
【００２１】
制御装置８には、自動操縦制御を実行するアクチュエータとして、電動スロットル弁制御装置１５、ブレーキ制御装置１６、及び、パワーステアリング制御装置１７が接続されている。
【００２２】
制御装置８は、車両１が通信可能エリアに進入して基準局４との通信が確立されると、車両１の現在位置が過去に作成された地図データ上の走行軌跡と略一致している場合、この一致している走行路を目標誘導路として自動操縦制御を行う。すなわち、電動スロットル弁制御装置１５からの駆動信号によりスロットル弁１８の開度を制御して目標車速を維持する車速制御を行い、又、目標車速が大きく減速された場合は、ブレーキ制御装置１６からの駆動信号によりブレーキを作動させて減速制御を行う。更に、パワーステアリング制御装置１７を駆動させて、車両１が目標とする誘導路に沿って走行するように操舵制御を行う。
【００２３】
この目標誘導路は自車両の走行軌跡を学習することで生成される。ここで、走行軌跡を学習する手順について説明する。この走行軌跡の学習は自車両１が帰路において駐車エリア内に進入し、同一路を複数回走行することで行われ、具体的には、図２に示す走行軌跡ルーチンに従って実行される。
【００２４】
すなわち、車両１が駐車エリアに進入すると、先ず、ステップＳ１１で、基準となる停車位置である切返し点を検索する。切返し点とは、図８に示すように、車両１を駐車スペース２１に所定の姿勢で駐車させるために車両１の進行方向を前進から後進に切換えるための特定停車位置を示すポイントである。この切返し点の検索は、図３に示す切返し点検索サブルーチンに従って行われる。
【００２５】
ここで、切返し点検索サブルーチンにおける処理について説明する。
【００２６】
このサブルーチンでは、先ず、ステップＳ２１〜Ｓ２３で、車両１が切返し点で停車したか否かを判定する。本形態では、切返し点での停車判定条件を、車速センサ９で検出した車速Ｖ（ステップＳ２１）とブレーキスイッチ１２（ステップＳ２２）とポジションセンサ１９で検出したシフトポジション（ステップＳ２３）とで判定する。
【００２７】
そして、車速Ｖが０の停車状態にあり、且つブレーキスイッチ１２がＯＮのブレーキ動作状態で、且つシフトポジションがＲ（リバース）レンジにセットされているとき、切返し点での停車判定条件が成立、すなわち、車両１が切返し点に到達して停車したと判定し、ステップＳ２４へ進む。尚、この切返し点は、ＧＰＳで検出した位置座標を示す位置であり、具体的にはＧＰＳアンテナ５ａの設置場所となる。
【００２８】
一方、切返し点での停車判定条件の１つでも満足されなかった場合、すなわち例えば車両１が走行中の場合は、切返し点での停車判定条件が不成立と判定し、そのままルーチンを抜ける。
【００２９】
切返し点での停車判定条件が成立し、すなわち車両１が切返し点で停車し、且つ後進しようとしていると判定されてステップＳ２４へ進むと、切返し点登録スイッチ（図示せず）がＯＮか否かを調べる。そして、ＯＦＦのときは、ステップＳ２５へ分岐し、切返し点の登録を促すメッセージを注意表示する。メッセージを表示する手段としては、液晶ディスプレイ１４に切返し点の登録を促すメッセージを表示し、或いは図示しないスピーカから切返し点の登録を促すメッセージを音声により報知するなどが考えられ、或いは液晶ディスプレイ１４とスピーカとの双方から登録を促すメッセージを出力するようにしても良い。
【００３０】
次いで、ステップＳ２６へ進み、操作待機時間カウンタのカウント値Ｃ１をインクリメントし（Ｃ１←Ｃ１＋１）、ステップＳ２７へ進み、当該カウント値Ｃ１と設定値Ｔ１（例えば１０[sec]）とを比較し、設定値Ｔ１に相当する時間が経過したか否かを調べる。そして、Ｃ１≦Ｔ１の設定時間に達していないときは、ステップＳ２４へ戻る。又、Ｃ１＞Ｔ１の設定時間に達したときは、今回の切返し点を登録することなく処理を終了する。
【００３１】
一方、運転者が切返し点登録スイッチを既にＯＮしており、或いは切返し点登録スイッチを設定値Ｔ１以内にＯＮすると、ステップＳ２４からステップＳ２８へ進み、切返し点Ｐ（図９参照）の停車位置情報である位置座標、及びＧＰＳ受信機５ｂに備えられているジャイロスコープ（図示せず）で検出した、車両１の方角データを走行軌跡データ（ノード列）の対応するノード点に記録する。
【００３２】
次いで、ステップＳ２９で、画像処理部７で処理した車両１の前方立体物の３次元位置データに基づき縦方向エッジ抽出処理を実行して、図３のステップＳ１２へ進む。
【００３３】
上述したステップＳ２９で実行される縦方向エッジ抽出処理は、図５に示す縦方向エッジ抽出処理サブルーチンで実行される。
【００３４】
ここで、縦方向エッジ抽出処理サブルーチンで実行される処理について説明する。
【００３５】
このサブルーチンでは、先ず、ステップＳ３１で、画像処理部７で処理した前方立体物の３次元位置データを読込み、ステップＳ３２で、読込んだ前方立体物の３次元位置データに基づいて縦方向に連続するエッジ（以下「縦方向エッジ」と称する）を抽出する。
【００３６】
前方立体物の３次元位置データから縦方向エッジを抽出する方法としては種々のものが考えられる。例えば、前方立体物の輝度変化量を検出し、この輝度変化量が予め設定したしきい値を越えており、且つ縦方向へ連続している部分を縦方向エッジとして抽出することも可能である。尚、図９には、同図に太線で示すように、車載カメラ６で撮像したステレオ画像に基づいて８個の縦方向エッジｅ１〜ｅ８を抽出した状態が示されている。
【００３７】
そして、ステップＳ３３へ進み、抽出した縦方向エッジの３次元位置データを、記憶手段に記憶されている切返し点Ｐの位置座標に関連づけて記憶し、ルーチンを抜け、図３に示す走行軌跡取得サブルーチンのステップＳ１２へ進む。
【００３８】
以下、走行軌跡取得サブルーチンのステップＳ１２からの処理について説明する。
【００３９】
上述したように、車両１の切返し点Ｐにおける位置データ、及びジャイロスコープによる方角データ、及び縦方向エッジの３次元位置データを所定に記憶した後、ステップＳ１２へ進むと、後進走行時の車両１の走行データが取得される。この走行データは、ＧＰＳ衛星３から一定時間毎に送信される位置情報（緯度、経度）に従い、車両１の現在地を地図データ上でのＸ−Ｙ座標軸形式に変換してノード点を設定し、このノード点に、当該位置情報を受信したときの車両情報（車速Ｖ、ハンドル角θH等）をリンクさせて記憶する。
【００４０】
次いで、ステップＳ１３へ進み、ポジションセンサ１９で検出したシフトポジションから、シフトレバーがＰ（パーキング）レンジにセットされているか否かを調べ、Ｐレンジにセットされていないときは、車庫入れ操作が継続されていると判断し、ステップＳ１２へ戻り走行データの取得を継続して行う。
【００４１】
一方、シフトポジションがＰレンジにセットされている場合は、車庫入れが完了したと判定し、ステップＳ１４へ進み、今回の走行軌跡データを記憶手段に記憶してルーチンを抜ける。
【００４２】
そして、記憶手段に記憶されている走行軌跡データに基づいて車両１を駐車させる際の目標誘導路を求める。この場合、複数の走行軌跡データに基づき公知の手法により走行軌跡データを学習する構成であっても良く、又、１回の運転操作で車両１を駐車スペースに駐車させる際の理想的な走行軌跡データを生成する構成としても良い。
【００４３】
次に、図５〜図７に示すフローチャートに従い、制御装置８で実行される、自車を切返し点Ｐから駐車スペース２１まで自動操縦により誘導させる際の制御について説明する。尚、以下においては、便宜的に、目標誘導路を生成する際に走行させた車両（自車）を基準車両１とし、実際に走行する車両を対象車両１’として説明する。
【００４４】
図５には駐車支援制御ルーチンが示されている。このルーチンは自動操縦制御用メインスイッチ１１がＯＮ状態にあるとき、設定演算周期毎に実行され、自動操縦制御用メインスイッチ１１がＯＦＦ動作されたとき終了する。
【００４５】
先ず、ステップＳ４１で学習終了フラグＦ１の値を参照する。Ｆ１＝０の場合は、未だ走行軌跡データの学習が終了していないため、ルーチンを抜ける。
【００４６】
一方、Ｆ１＝１のときは走行軌跡データの学習が終了しているため、ステップＳ４２へ進み、駐車エリアに進入したか否かを調べ、未進入のときはルーチンを抜け、対象車両１’が駐車エリアに進入するまで待機する。
【００４７】
そして、対象車両１’が駐車エリアに進入すると、ステップＳ４３へ進み、切返し点に到達したか否かを調べ、切返し点に到達するまで待機する。切返し点に到達したか否かは、例えば、自動操縦制御により対象車両１’が停車（Ｖ＝０）で、且つ運転者がブレーキペダルを踏み込んでブレーキスイッチ１２をＯＮさせ、且つシフトレバーをＤレンジからＲレンジ側へセットしたことが、ポジションセンサ１９で検出されたとき、切返し点に到達と推定する。尚、この場合、車速Ｖが０[Km/h]で、且つブレーキスイッチ１２がＯＮのとき切返し点に到達したと判定するようにしても良い。
【００４８】
以下においては、学習走行軌跡データに記憶されている切返し点をＰとし、この切返し点Ｐを目標として停車させたときの実際の切返し点をＱと表して説明する。
【００４９】
そして、対象車両１’が切返し点Ｑに到達したとき、ステップＳ４４へ進み、自動操縦制御を実行して、ルーチンを抜ける。このステップＳ４５で実行される自動操縦制御は、図７に示す自動操縦制御サブルーチンに従って行われる。
【００５０】
このサブルーチンでは、先ず、ステップＳ５１で、対象車両１’の到達した切返し点Ｑの位置座標と方角とを検出する。この切返し点Ｑの位置座標は、ＧＰＳ衛星３から一定時間毎に送信される位置情報（緯度、経度）に基づいて設定される。又、方角はＧＰＳ受信機５ｂに備えられているジャイロスコープ（図示せず）で検出する。
【００５１】
次いで、ステップＳ５２へ進み、縦方向エッジ抽出処理を実行する。この縦方向エッジ抽出処理は、前述した図５に示す縦方向エッジ抽出処理サブルーチンと同様の処理で行われる。すなわち、先ず、切返し点Ｑにおいて、対象車両１’の前方を車載カメラ６で撮像し、画像処理部７で入力されたステレオ画像に基づいて前方立体物の３次元位置データを求め、この３次元位置データに基づき、縦方向エッジの３次元位置データを抽出する。
【００５２】
そして、ステップＳ５３へ進み、切返し点Ｑ、或いは当該切返し点Ｑ近傍の地図データ上に、既に学習されている走行軌跡データ（ノード列）が記憶されているか否かを検索する。この走行軌跡データの検索は、例えば対象車両１’が停車している切返し点Ｑの位置座標を中心とする設定範囲（例えば半径0.5[m]）以内に、既に学習されている走行軌跡データ（学習走行軌跡データ）の、切返し点Ｐを示すノード点があるかどうかを検索する。
【００５３】
そして、学習走行軌跡データがある場合は、ステップＳ５５へ進む。又、走行軌跡データが無い場合はステップＳ５６へ分岐し、自動操縦ができない旨のエラー表示を音声、或いは音声と液晶ディスプレイ１４上に表示して、ルーチンを終了する。従って、この場合、自動操縦制御は中止される。
【００５４】
又、ステップＳ５５へ進むと、対象車両１’が停車している切返し点において撮像したステレオ画像と、学習走行軌跡データの、切返し点Ｐを示すノード点に関連付けして記憶されている縦方向エッジを撮像したステレオ画像とが、比較可能な範囲にまでオーバラップしているか否かを調べる。
【００５５】
すなわち、対象車両１’を走行軌跡データに記憶されている切返し点Ｐに、毎回正確に停車させることは困難である。又、例えば、駐車エリアから切返し点Ｐまで自動操縦により対象車両１’を導いたとしても、雨天等で路面μが低下していたり、タイヤの空気圧が変化していたり、ブレーキの制動具合の変化等の影響で、毎回対象車両１’を切返し点Ｐに正確に停車させることは困難である。更に、切返し点Ｑで停車した対象車両１’の指向方向も、走行軌跡データを学習した際に切返し点Ｐで停車させた基準車両１と同じ方向であるとは限らない。
【００５６】
対象車両１’の切返し点Ｑが、学習走行軌跡データの切返し点Ｐに対して、大きくずれている場合、或いは基準車両１と対象車両１’との指向方向に大きなずれが生じている場合、そのまま学習走行軌跡データに沿って基準車両１を駐車スペース２１まで自動操縦により誘導しようとした場合、後進直後の位置誤差が大きいため、駐車スペース２１にスムーズに導くことが困難となる場合がある。
【００５７】
本形態では、基準車両１と対象車両１’とに搭載されている車載カメラ６で撮像したステレオ画像が、比較可能な程度までオーバラップしているか否かを、基準車両１と対象車両１’との方角の差の絶対値が設定角度（例えば３０°）以内に収まっているか否かで大まかに判定する。
【００５８】
そして、基準車両１と対象車両１’との方角の差の絶対値が設定角度以内の場合は、ステレオ画像は比較可能な範囲までオーバラップしていると判定してステップＳ５７へ進む。又、基準車両１と対象車両１’との方角の差の絶対値が設定角度以上の場合は、ステレオ画像は比較できないと判定し、ステップＳ５６へ分岐し、自動操縦ができない旨のエラー表示を音声、或いは音声と液晶ディスプレイ１４上に表示して、ルーチンを終了する。従って、この場合、自動操縦制御は中止される。
【００５９】
又、ステップＳ５７へ進むと、学習走行軌跡データ（ノード列）を読込み、続く、ステップＳ５８で、切返し点Ｑに停車している対象車両１’の指向方向と、走行軌跡データを学習した際に切返し点Ｐで停車させた基準車両１の指向方向（以下「基準指向方向」と称する）との角度差θｘを演算する。
【００６０】
本形態では、各学習走行軌跡データに関連付けて記憶されている縦方向エッジの３次元位置データと、対象車両１’の車載カメラ６で撮像したステレオ画像に基づいて抽出した縦方向エッジの３次元位置データとを比較して、共通する２つの縦方向エッジ（例えば図９の縦方向エッジｅ２，ｅ７）の３次元位置データを抽出し、この２つの各縦方向エッジの３次元位置データに基づき、余弦定理、及びベクトルの内積の定義を用いて基準車両１の基準指向方向に対する対象車両１’の指向方向の角度差θｘを算出する。
【００６１】
以下、図１２に示す模式図を参照して、基準車両１と対象車両１’との指向方向の角度差θｘの算出方法について説明する。尚、以下においては、共通する２つの縦方向エッジの特定点を示す３次元位置データを仮にｕ，ｗとし、基準車両１と対象車両１’との指向方向の交点をＲとして説明する。
【００６２】
基準車両１と対象車両１’との切返し点Ｐ，Ｑ間の距離ＰＱは、各切返し点Ｐ，Ｑの位置座標が予め計測されているため、この位置座標に基づき算出する。又、距離Ｐｕ，Ｐｗ，Ｑｕ，Ｑｗは、各縦方向エッジの特定点を示す３次元位置データｕ，ｗが車載カメラ６で撮像したステレオ画像に基づいて計測されているため、この各３次元位置データに基づいて算出することができる。
【００６３】
次に、△ＲＰＱの∠ＰＲＱの角度差θｘを求める。尚、以下においては、図面上の説明を簡単にするため、各角∠ＲＰＱ，∠ＲＱＰ，∠ｗＰＱ，∠ｗＰＲ，∠ｕＱＰ，∠ｕＱＲの角度を、当該記号でそのまま表示する。
【００６４】
∠ＰＲＱ（＝角度差θｘ）は、
∠ＰＲＱ＝１８０−（∠ＲＰＱ＋∠ＲＱＰ）  …（１）
であり、∠ＲＰＱは、
∠ＲＰＱ＝∠ｗＰＱ−∠ｗＰＲ  …（２）
又、∠ＲＱＰは、
∠ＲＱＰ＝∠ｕＱＰ−∠ｕＱＲ  …（３）
である。ここで、∠ｗＰＱ、及び∠ｕＱＰは余弦定理から、
COS（∠ｗＰＱ）＝（ＰＱ２＋Ｐｗ２−Ｑｗ２）／（２・ＰＱ・Ｐｗ）
従って、
∠ｗＰＱ＝cos−１（（ＰＱ２＋Ｐｗ２−Ｑｗ２）／（２・ＰＱ・Ｐｗ））…（４）
となる。又、
COS（∠ｕＱＰ）＝（ＰＱ２＋Ｑｕ２−Ｐｕ２）／（２・ＰＱ・Ｑｕ）
より、
∠ｕＱＰ＝cos−１（（ＰＱ２＋Ｑｕ２−Ｐｕ２）／（２・ＰＱ・Ｑｕ））…（５）
となる。
【００６５】
従って、∠ｗＰＲ，∠ｕＱＲの角度を算出することで、∠ＰＲＱ（＝角度差θｘ）を算出することができる。∠ｗＰＲは、ベクトルＰｗとベクトルＰＲとのなす角であり、又、∠ｕＱＲは、ベクトルＱｕとベクトルＱＲとのなす角である。
【００６６】
ベクトルの内積の定義を用いれば、∠ｗＰＲ，∠ｕＱＲは次式から算出することができる。尚、図１０に示すように、点ｗ、ｕの位置座標は、基準車両１と対象車両１’との切返し点Ｐ，Ｑの地表面を原点とし、車幅方向にＸ軸、上下方向にＹ軸、前後方向にＺ軸を取り、ｗ（Ｘｗ，Ｙｗ，Ｚｗ），ｕ（Ｘｕ，Ｙｕ，Ｚｕ）として表す。
【００６７】
∠ｗＰＲ，∠ｕＱＲの角度は、地表上のベクトルＰｗとベクトルＰＲとのなす角、及び同じく地表上のベクトルＱｕとベクトルＱＲとのなす角で表せばよいため、縦方向エッジのＹ軸方向の位置座標は０とすることができる。従って、点ｗの位置座標（Ｘｗ，Ｙｗ，Ｚｗ）、点ｕの位置座標（Ｘｕ，Ｙｕ，Ｚｕ）の、Ｘ軸に平行で各点ｗ，ｕを通るＺ軸上の点の位置座標（０，０，Ｚｗ），（０，０，Ｚｕ）と、各点ｗ，ｕから地表面に降ろした垂線の位置座標（Ｘｗ，０，Ｚｗ），（Ｘｕ，０，Ｚｕ）とに基づき、ベクトルの内積の定義を用いて∠ｗＰＲ，∠ｕＱＲの角度を算出する。
【００６８】
すなわち、∠ｗＰＲの角度は、
∠ｗＰＲ＝cos−１｛Ｚｗ／（Ｘｗ２＋Ｚｗ２）｝−２  …（６）
から算出する。又、∠ｕＱＲの角度は、
∠ｕＱＲ＝cos−１｛Ｚｕ／（Ｘｕ２＋Ｚｕ２）｝−２  …（７）
から算出する。
【００６９】
そして、この∠ｗＰＲ，∠ｕＱＲを、上述した（２），（３）式に代入して、（１）式から、∠ＰＲＱ（＝角度差θｘ）を算出する。尚、基準車両１と対象車両１’が、その両指向方向を互いに拡開する状態で停車している場合は、この基準車両１と対象車両１’との指向方向の延長線は車体後方で交差することになるが、この場合は、エラーとなり演算は停止される。
【００７０】
その後、ステップＳ６１へ進み、ステップＳ５８で算出した角度差θｘに基づき、対象車両１’を学習走行軌跡データ上へ収束させるための初期ハンドル角を設定する。
【００７１】
図９に示すように、対象車両１’を学習走行軌跡データに収束させるための走行路は、初期ハンドル角を大きく設定して、学習走行軌跡データに早い時期に収束させる最小リスク軌跡と、初期ハンドル角を小さく設定して学習走行軌跡データへ滑らかに収束させる滑らか軌跡とが設定されている。運転者は、最小リスク軌跡と滑らか軌跡とで囲まれた、図のハッチングで示す領域内で、自己の好みに応じた初期ハンドル角を設定することができる。尚、図示しないが、ハッチングで示す領域内には最短ルートも設定されており、運転者は最短ルートに対応する初期ハンドル角を設定することも可能である。
【００７２】
そして、ステップＳ６２へ進み、ブレーキスイッチ１２がＯＦＦか否かを調べ、ＯＮのときは、ＯＦＦ動作するまで、すなわち、ブレーキ圧が開放されるまで待機する。そして、ブレーキスイッチ１２がＯＦＦのブレーキ圧が開放されたとき、ステップＳ６３へ進み、ステアリング制御を実行する。このステアリング制御は、先ず、学習走行軌跡データのノード点と、対象車両１’のＧＰＳ受信機５ｂで計測した位置情報との横方向のずれ量ΔＤを算出し、このずれ量ΔＤを０にするような目標ハンドル角δｈを、
δｈ＝Ｇｐ・ΔＤ＋Ｇｄ・（ｄ（ΔＤ）／ｄｔ）
から算出する。尚、ここで、Ｇｐは比例項ゲイン、Ｇｄは微分項ゲインであり、最小リスク軌跡が選択されている場合、比例項ゲインＧｐ、微分項ゲインＧｄは大きな値に設定され、走行路が大きく変化する。一方、滑らか軌跡が選択されている場合、比例項ゲインＧｐ、微分項ゲインＧｄは小さな値に設定され、走行路が比較的大きな軌跡を描いて変化するようになる。
【００７３】
そして、目標ハンドル角δｈに対応する制御信号でパワーステアリング制御装置１７を動作させる。
【００７４】
次いで、ステップＳ６４へ進み、ブレーキ制御装置１６、電動スロットル弁制御装置１５を動作させて車速制御を行う。尚、後進走行をトルクコンバータのクリープ現象を利用して行う場合の車速制御はブレーキ制御のみとなる。
【００７５】
このステップＳ６３，Ｓ６４におけるステアリング制御、及び車速制御により対象車両１’の走行路が決定され、この走行路に沿って対象車両１’が後進走行される。
【００７６】
その後、ステップＳ６５へ進み、対象車両１’の位置情報から駐車点までの距離Ｄｓを算出し、この距離Ｄｓが予め設定した設定値Ｄｓｏに達したか否かを調べる。
【００７７】
そして、Ｄｓ＞ＤｓｏのときはステップＳ６３へ戻り、ステアリング制御、車速制御を繰り返し実行する。その後、対象車両１’と駐車点との間の距離Ｄｓが設定値Ｄｓｏに達すると（Ｄｓ≦Ｄｓｏ）、ステップＳ６６へ進み、停車制御が実行される。
【００７８】
停車制御は、対象車両１’と駐車点との間の距離Ｄｓに基づいて、駐車点において車速Ｖが０[Km/h]となる目標車速を設定し、実際の車速が目標車速に収束するように車速制御を行い、同時に対象車両１’が駐車点で適正な姿勢で停止するように操舵制御が行われる。尚、この停車制御は、周知のファジー制御を用いて行うようにしても良い。
【００７９】
その後、ステップＳ６７へ進み、車速Ｖが０[Km/h]か否かを調べ、Ｖ＞０のときはステップＳ６６へ戻り、停車制御を継続して行う。又、対象車両１’が停車した場合（Ｖ＝０）、ステップＳ６８へ進み、シフトレバーをＰレンジへセットするように促す旨の情報を、液晶ディスプレイ１４、或いはスピーカ等の報知手段を介して運転者に報知する。尚、車速Ｖが０[Km/h]のときはブレーキ制御装置１６を介してブレーキ動作状態が維持されている。
【００８０】
その後、ステップＳ６８へ進み、シフトレバーがＲレンジからＰレンジに切換えられたか否かを調べ、Ｐレンジに切換えられたときは、ステップＳ７０へ進み、自動操縦制御を終了して、ルーチンを抜ける。又、シフトレバーがＰレンジにセットされていないときは、ステップＳ６８へ戻る。
【００８１】
このように、本形態では、対象車両１’が切返し点で停車したときの指向方向が、学習走行軌跡データに記憶されている基準車両１の基準指向方向に対して角度差θｘだけずれて停車したとしても、当該角度差θｘに応じて後進走行時の初期ハンドル角を設定するようにしたので、切返し点における対象車両１’の指向方向が学習走行軌跡データに記憶されている位置とずれていたとしても、後進走行の際に学習走行軌跡データにスムーズに収束させることができる。
【００８２】
尚、上述した説明では、共通する２つの縦方向エッジの特定点を示す３次元位置データｕ，ｗを抽出して、基準車両１に対する対象車両１’の角度差θｘを算出するようにしたが、３つ以上の共通する縦方向エッジを抽出し、それに基づいて基準車両１に対する対象車両１’の角度差θｘを算出することも可能である。
【００８３】
又、本形態では、車両に搭載されている既存の設備（車載カメラ６、ＧＰＳ受信機５ｂ）を利用して対象車両１’の指向方向を検出するようにしているので、対象車両１’の指向方向を、従来のようにＧＰＳアンテナを増設することなく、低コストで、しかも高精度に検出することができ、高い汎用性を得ることができる。
【００８４】
更に、車載カメラ６は、前方認識カメラ以外に後方を認識するリヤビューカメラや側方を認識するサイドビューカメラであっても良い。或いはこの車載カメラ６が、前方認識カメラとリヤビューカメラとサイドビューカメラとの中の２つ以上の組み合わせで構成されていても良い。
【００８５】
又、撮像画像から抽出する縦方向エッジの信頼性を確保するためには、移動体の縦方向エッジの検出を除く必要がある。このため、基準車両において抽出する縦方向エッジは、所定の時間間隔を設けて撮像した画像を比較し、縦方向エッジに変化がないものとし、移動体における縦方向エッジを除外したものとすることが好ましい。
【図面の簡単な説明】
【００８６】
【図１】車両の駐車支援装置の全体構成図
【図２】走行軌跡取得ルーチンを示すフローチャート
【図３】切返し点検索サブルーチンを示すフローチャート
【図４】縦方向エッジ抽出処理サブルーチンを示すフローチャート
【図５】駐車支援制御ルーチンを示すフローチャート
【図６】自動操縦制御サブルーチンを示すフローチャート（その１）
【図７】自動操縦制御サブルーチンを示すフローチャート（その２）
【図８】自車を切返し点から自動操縦制御により後進させて駐車スペースに駐車させる状態を示す説明図
【図９】切返し点での自車の指向方向の角度差に応じて自車を学習走行軌跡上に収束させながら駐車させる状態を示す説明図
【図１０】基準車両に対する自車の切返し点における指向方向の角度差を算出する方法を示す模式図
【符号の説明】
【００８７】
１…基準車両、
１’…対象車両、
２…走行制御装置、
５ａ…ＧＰＳアンテナ
５ｂ…ＧＰＳ受信機、
６…車載カメラ、
７…画像処理部、
８…制御装置、
９…車速センサ、
１２…ブレーキスイッチ、
２１…駐車スペース、
ｅ１〜ｅ８…縦方向エッジ、
Ｐｕ，Ｐｗ，Ｑｕ，Ｑｗ…距離、
Ｐ，Ｑ…切返し点、
ｕ，ｗ…縦方向エッジの特定点を示す３次元位置データ、
θｘ…角度差
【図１】
2007278898.tif 000003
【図２】
2007278898.tif 000004
【図３】
2007278898.tif 000005
【図４】
2007278898.tif 000006
【図５】
2007278898.tif 000007
【図６】
2007278898.tif 000008
【図７】
2007278898.tif 000009
【図８】
2007278898.tif 000010
【図９】
2007278898.tif 000011
【図１０】
2007278898.tif 000012