JP5699670B2 - 走行経路生成装置、及び走行経路生成方法 - Google Patents

Description

本発明は、カメラで撮像した撮像画像に基づき走行経路を生成する技術に関する。

カメラで撮像した画像に基づき走行経路を生成する技術としては、例えば特許文献１に記載の技術がある。この従来技術では、車両後方に取り付けた後方カメラで車両後方の画像を撮像可能に構成されている。そして、車両が前進する際に、後方カメラで撮像した画像に基づき前進時の走行軌跡を検出して記憶しておき、車両が後退する際に、上記検出した走行軌跡に基づき運転支援を行う。すなわち、予め記憶している走行軌跡を走行経路とする。

特開２００７−２３７９３０号公報

上記従来技術は、前進走行時に取得した画像から算出した走行軌跡を後退時の走行経路として利用するものである。しかし、上記従来技術では、走行軌跡を記憶するときとは逆方向に車両の向きを変えて同じルートを逆向きに走行するような場合には、その走行軌道を走行経路として利用出来ないおそれがある。すなわち、行きに通った通路の走行軌跡の情報を元に、帰りの走行経路生成を行う場合、上記従来技術では、カメラの撮像方向が反対向きになってしまうため、カメラ画像による自己位置の推定が困難となり、経路生成が非常に難しくなってしまう。
本発明は、上記のような点に着目したもので、走行履歴を構成する場合と走行経路を生成する場合とで車両の進行方向に大きな変化があっても、より確実に自己位置を推定して経路生成を可能とすることを目的としている。

上記課題を解決するために、本発明の一態様は、車両位置を把握できる車両に搭載した少なくとも車両前方及び車両後方をそれぞれ個別に車両周囲環境を撮像可能な２以上のカメラで、少なくとも車両前方及び車両後方を個別に撮像する。そして、その各撮像画像内の特徴点を撮像したときの車両位置に基づくカメラ位置及び撮像方向角を関連付けて蓄積画像として蓄積し、撮像方向角及びカメラ位置に基づき上記蓄積画像を照合して、カメラが撮像する撮像画像と画像の重なりが相対的に大きいと推定される蓄積画像を選択する。その選択された蓄積画像内の上記特徴点に基づき車両の走行経路を生成する。

本発明によれば、撮像方向角が全く異なる複数方向の画像を蓄積画像として蓄積できる。このため、カメラの撮像方向角（カメラの向き）も考慮に入れて蓄積画像を選択することが可能となる。進行方向が逆方向に変化するなど進行方向に大きな変化があったとしても、自己位置を推定するための蓄積画像として、微小時間前の蓄積画像以外の蓄積画像も適用することが出来る。また、このようなより適切な蓄積画像で自己位置が推定できるので、経路生成もより確実に可能となる。

また、撮像方向角及びカメラ位置に基づき蓄積画像を選択することで、異なるカメラによる蓄積画像が混在していても、より適切な蓄積画像を選択可能となる。
以上のように、本発明によれば、走行履歴を構成する場合と走行経路を生成する場合とで車両の進行方向に大きな変化があっても、より確実に自己位置を推定して経路生成を可能となる。

本発明に基づく実施形態に係る車両に搭載した走行支援装置システム構成図である。 本発明に基づく実施形態に係る走行支援装置の処理ブロック図である。 複数カメラ画像（蓄積画像）から好適な画像選択を行うことが出来ることを示す概念図である。 走行履歴構成時の処理のフローチャートを示す図である。 走行支援時の処理のフローチャートを示す図である。 本発明の動作等を説明するための図であって、（ａ）は走行履歴構成時を説明する図であり、（ｂ）は走行経路生成時を説明する図である。

「第１実施形態」
次に、本発明の実施形態について図面を参照しつつ説明する。
本実施形態の走行支援装置は、本実施形態の走行経路生成装置を構成要素として備える。
（構成）
まず、本実施形態による走行支援装置１００の構成について、図１及び図２を参照して説明する。図１は、本実施形態の走行支援装置１００のシステム構成図であり、図２は本実施形態の走行支援装置１００で行われる処理内容を模式的に示したブロック図である。

本実施形態の走行支援装置１００は、図１に示すように、車両前後方向前方又は後方を個別に撮像可能な複数のカメラ２と、走行支援ＥＣＵ３と、車両情報ＤＢ４と、走行履歴ＤＢ５と、ディスプレイ６と、タッチパネル７と、車両ＥＣＵ８と、車両センサ群９と、制御ユニット群１０とで構成されている。これらの構成要素は、図１に示すように、車両ＭＭに搭載されている。

上記複数のカメラ２は、車両前後方向前方を撮像可能な前方カメラ２（２ａ）と、車両前後方向後方を撮像可能な後方カメラ２（２ｂ）とからなる。これらのカメラ２は、例えばＣＣＤ等の固体撮像素子を用いたカメラなどを使用すればよい。そして、前方カメラ２（２ａ）は、例えば車両ＭＭの車内ミラー部の裏側に、車両前方を撮像可能な方向に設置される。後方カメラ２（２ｂ）は、例えば車両後部に装着されたナンバープレート上部に設置される。各カメラ２で個別に撮像された画像は、それぞれ走行支援ＥＣＵ３へ送られる。

走行支援ＥＣＵ３は、撮像画像を用いた自己位置推定や走行経路算出などの処理を行う電子制御ユニットである。走行支援ＥＣＵ３は、他の制御に用いるＥＣＵと兼用しても良い。走行支援ＥＣＵ３は、一般的なＥＣＵと同様にＣＰＵ、ＲＯＭ、ＲＡＭ等で構成され、ＲＯＭには後述する各種処理部を実現するプログラムが格納されている。この走行支援ＥＣＵ３は、機能的には、特徴点検出部３１、データ蓄積部３２、自車位置推定部３３、蓄積画像選択部３４、走行経路算出部３５、及び走行支援部３６の各処理ブロックを備える。これらの各処理ブロックの処理内容については後述する。

車両情報ＤＢ４は、車両ＭＭやカメラ２に関する各種パラメータが記憶された記憶部である。車両情報ＤＢ４には、例えば車両ＭＭの最小旋回半径、車幅、トレッド長、タイヤ半径などの車両モデルに関する情報や、カメラ２の内部パラメータや光軸方向や画角などのカメラ２設計値に関する情報が記憶されている。
走行履歴ＤＢ５は、カメラ２で撮像された撮像画像を、撮像時のカメラ位置、撮像方向角、車両位置、車両姿勢などの付加情報と関連づけられて、蓄積画像として記憶する記憶部であり、記憶部としては、例えばハードディスク、メモリーカードなどの記憶媒体を用いればよい。

ディスプレイ６は、例えば車両ＭＭの室内のセンタコンソールに設置される。例えば、走行支援の際に、カメラ画像に対し、自車の予測進路と推奨経路を重畳して表示する表示装置として使用される。なお、ディスプレイ６は、一般にナビゲーション装置に用いるディスプレイと兼用しても良い。
タッチパネル７は、例えばディスプレイ６の表示部に配置され、ユーザが接触した部位の座標位置や接触箇所の移動軌跡を特定できる、一般的なタッチパネルから構成する。本実施形態では、ユーザが、経路保存を開始するかどうかを選択したり、走行支援を開始するかどうかを指示したりする際に利用される。なお、ユーザからの指示入力インタフェースとしては、タッチパネル７の代わりに、ボタン、マウス、キーボード、音声入力などの入力インタフェースを用いてもよい。

車両ＥＣＵ８は、エンジンやブレーキ等の作動を制御する電子制御ユニットである。車両ＥＣＵ８は、エンジン制御ＥＣＵ、ブレーキ制御ＥＣＵ、ステアリング制御用ＥＰＳ等の制御ユニットで構成される制御ユニット群１０に接続している。車両ＥＣＵ８は、走行支援ＥＣＵ３からの出力に基づき、アクセル開度、ブレーキ制御量、ステアリング操舵角等の指令値を算出し、算出した指令値を制御ユニット群１０に伝送する。これによって、走行支援ＥＣＵ３が算出した走行軌跡に沿って車両ＭＭを制御することが可能となる。

車両センサ群９は、アクセルセンサ、ブレーキセンサ、ステアリングセンサ、車速センサ、加速度センサなどの各種車載センサで構成され、走行支援ＥＣＵ３に接続される。走行支援ＥＣＵ３は、車両センサ群９の出力値から、車両ＭＭ２の車速、加速度、操舵角や、運転者のアクセル操作量、ブレーキ操作量などを算出することが可能である。

次に走行支援ＥＣＵ３の各処理ブロックについて説明する。
特徴点検出部３１は、カメラ２の撮像画像もしくは、走行履歴ＤＢ５に蓄積された過去の撮像画像（蓄積画像）に対し、対象とする画像の画面内に存在する特徴点を検出するための処理部である。特徴点は、画面が移動した際に追従可能な点であり、一般的には画像内において縦方向、横方向ともに輝度差がある点が特徴点として抽出される。すなわち、特徴点は、エッジ処理などの画像処理によって検出可能であり、頑健な特徴点検出方法としてさまざまな手法（ＳＩＦＴ， ＳＵＲＦ，ｆａｓｔなど）が公知の処理方法として提案されている。これらの処理方法によって特徴点を抽出することで検出すればよい。

データ蓄積部３２は、各カメラ２が撮像した各撮像画像を、特徴点検出部３１により得られた特徴点と、自車位置推定部３３で推定される自車位置に基づくカメラ位置と、撮像時の撮像方向角等の付加情報とを関連付けて、蓄積画像として走行履歴ＤＢ５に保存する。
ここで、撮像方向角は、車両ＭＭの向きに対するカメラ２の撮像方向（設置方向）で特定できるので、車両ＭＭの向きを、各カメラ２の撮像方向角の情報として記憶しても問題はない。同様に、車両ＭＭに対するカメラ２の設置位置も特定されているので、カメラ位置の情報として、車両位置及び車両ＭＭの向きを使用しても良い。なお、車両位置としては、例えば車両重心点を使用する。なお、撮像方向角は、カメラ２が向いている視軸方向であって、例えば撮像画角の中央位置に設定される。

また、カメラ２の撮像画像の特徴点と、蓄積画像選択部３４により走行履歴ＤＢ５から選択された蓄積画像の特徴点とを用いて照合を行うことで、上記カメラ２の撮像画像の特徴点は、三次元位置計測を行い、さらに世界座標系に変換された上で、つまり世界座標系での座標で特定して保存される。このとき、カメラ２から特徴点までの位置計測結果はカメラ２座標系で表されており、これを世界座標系に変換するためには、世界座標系におけるカメラ２の位置と、特徴点の三次元構成とを、同時に求めておく必要がある。このように、マップの生成と自己位置の推定を同時に行う手法としては、ＳＬＡＭ（Simultaneous Localization And Mapping）を適用すればよい。このＳＬＡＭに関しては様々なアルゴリズムが発表されている。カメラ２を使用したＳＬＡＭとしてはＭｏｎｏ−ＳＬＡＭ， ＰＴＡＭなどがあげられる。

蓄積画像選択部３４は、カメラ２が撮像した撮像画像と照合するために相応しい蓄積画像を、走行履歴ＤＢ５から選択する。
蓄積画像選択部３４は、まず、カメラ２の撮像方向角（撮像画像の撮像方向角）を取得する。カメラ２の撮像方向角は、撮像時の自車位置の情報と、車両情報ＤＢ４に蓄積されたカメラ２の取り付け位置等の情報から演算すれば良い。なお、上記撮像方向角は、上面視における方向角とする。次に、上記取得した撮像方向角と、走行履歴ＤＢ５内の各蓄積画像に対応する撮像方向角とを比較して、カメラ２の撮像方向角からの向きのズレ（差分の角度）が予め設定した設定ズレ角以内となっている蓄積画像群を抽出する。上記設定ズレ角は、例えば、カメラ２の画角の５０％若しくはそれ以下の値の角度に設定する。次に、抽出した蓄積画像群のうち、予め設定した設定接近距離以内の蓄積画像であって、対象とするカメラ２（自車位置で代用しても良い。）に一番近い位置で撮像された蓄積画像を選択する。

具体的な選択例を図３を用いて説明する。現在画像に対して、画像の重なりが大きい候補（３）が好適な蓄積画像の撮像位置とする。撮像方向角の設定ズレ角をカメラ２画角の半分以下とする。カメラ２が反対向きとなっている、つまり候補（１）に対するずれ角θ１は設定ズレ角より大きいため、候補（１）は排除することができる。このため、ズレ角に基づいて候補（２）と候補（３）が残る。次に、残った候補の中から最も近い撮像画像を選ぶので、候補（３）を選ぶことができる。撮像方向角に制限を加えることで、画像の半分程度が重なることが期待できる。また、より近い蓄積画像を選択することで共通する特徴点が多い可能性が大きくなる。この結果、自車位置推定部３３での自己位置推定の精度向上に貢献する。

以上のように、蓄積画像選択部３４は、複数カメラ２での撮像画像が混在する走行履歴ＤＢ５から、現在の撮像画像と照合するのに好適な撮像画像を選択することができる。
自車位置推定部３３は、カメラ２が撮像した撮像画像と、蓄積画像選択部３４により選択された蓄積画像及び蓄積画像内の特徴点と、車両情報ＤＢ４に蓄積された車両情報と、を用いて自車位置を推定する。

自車位置推定部３３は、まず事前知識（前に実施された自車位置推定における自車位置推定結果（例えば直前の処理ステップで求めた自車位置推定結果）や、車両センサ群９から算出される車速、操舵角等の車両情報）を用いて、自車位置の事前推定の処理を行う。その後、自車位置推定部３３は、事前推定により車両ＭＭの位置と姿勢が得られたら、これを用いて、走行履歴ＤＢ５に蓄積されている特徴点の中で、カメラ２の撮像画像に写りうる特徴点を選別する。具体的には蓄積画像選択部３４が選択した蓄積画像中の特徴点とカメラ２が撮像した撮像画像とから特徴点を選別する。その選別した特徴点の撮像画像内での位置を推定して、その画像内位置推定値を中心として、その特徴点を画像内で検索を行う。上記検索によって、事前推定値から想定される画像内位置と実際に検出された画像内位置とのずれを検出する。その後、自車位置推定部３３は、画像内特徴点位置と、自車位置の事前推定値から算出した特徴点位置のずれから、自車位置の事前推定値を補正して自車位置を算出する。これを事後推定と呼ぶ。このように、本実施形態の自車位置推定部３３では、事前位置推定の結果を、特徴点の位置が実際の計測位置に一致するように調整することで事後推定を実施して、最終的な自車位置推定結果を得る。自己位置推定部が推定した最終的な自車位置推定結果（自車位置情報）は、走行履歴ＤＢ５に蓄積される。

走行経路算出部３５は、蓄積画像選択部３４が選択した蓄積画像を使用して求めた現在の自車位置に基づき、走行履歴ＤＢ５に蓄積された過去の自車位置の軌跡を読み込み、走行経路とする。すなわち、走行経路算出部３５は、蓄積画像選択部３４が選択した蓄積画像と走行履歴ＤＢ５に蓄積された過去の自車位置の軌跡とに基づき走行経路を生成する。このとき、走行履歴ＤＢ５に蓄積された複数の撮像画像を照合して得られる三次元特徴点群も同時に算出される。走行経路である過去の自車位置の軌跡は、特徴点算出に用いられる撮像画像の、撮像時のカメラ位置から算出され、三次元特徴点群との相対関係で規定されることになる。

また、走行経路算出部３５は、読み込んだ走行経路の向きを逆向きに利用することも可能である。方向の選択手法としては、例えば、経路上の現在位置が始点に近ければ順方向に、終点に近ければ逆方向に走行する、としても良いし、運転者がタッチパネル７を通じて方向もしくは目的地を指示することで決定しても良い。
走行支援部３６は、走行経路算出部３５が定めた走行経路を用いて、走行支援を行う。具体的には、作成した走行経路情報を、カメラ２（例えば前方カメラ２（２ａ））のカメラ２座標系に変換した上で、ディスプレイ６に、撮像画像に走行経路を重畳して表示する。さらに走行支援部３６は、走行軌跡と車両ＭＭの位置・姿勢のずれ量を車両ＥＣＵ８に送付することで、アクセル、ブレーキ、ステアリングを制御し、自動運転を行うことができる。

次に、上記走行支援装置で実施する処理のうち、走行履歴構成の処理、及びその走行履歴を利用した走行支援時の処理について、図４と図５のフローチャートを参照して説明する。上記走行履歴構成の処理、及び走行支援時の処理は、走行支援ＥＣＵ４において、予め設定した制御周期、例えば１００ｍｓｅｃの間隔で連続的に行われる。
まず、走行履歴構成の処理を、図４のフローチャートを参照して説明する。
なお、以下の説明では、運転者からの開始指示があった場合に作成を開始する場合で説明するが、走行支援ＥＣＵ４が予め設定した条件を満足することで自動的に作成を開始するようにしても良い。

まず、図４を用いて走行履歴構成時の処理フローを説明する。
ステップＳ１１０では、ディスプレイ６に走行履歴保存を開始するか否かのボタンを表示し、タッチパネル７を通じて、運転者の入力を受け付け可能な状態とする。そして、運転者の入力に基づき開始指示があったと判定した場合には、ステップＳ１１１に移行する。開始指示がない場合は、予め設定した制御周期でステップＳ１１０の判定を繰り返すことで、運転者からの入力を待つ状態となる。
ステップＳ１１１では、走行履歴ＤＢ５に過去の走行履歴が存在するかどうかを判定する。走行履歴が存在しないと判定した場合は、ステップＳ１１２へ移行し、走行履歴が存在すると判定した場合はステップＳ１１３へ移行する。

ステップＳ１１２では、走行軌跡の起点となる自己位置の特定など走行履歴初期化処理を行う。まず、特徴点検出部３１が、カメラ２の撮像画像から特徴点を検出する。次に、予め設定した時間経過後（例えばカメラ２の撮像間隔）に再びカメラ２から撮像画像を取り込み、特徴点検出部３１が、その取り込んだ撮像画像から特徴点を算出する。なお、車両ＭＭが移動中など上記２つの画像の取得位置が異なる場合とする。そして、新たな画像中の特徴点について先に取得した各特徴点と比較することで、特徴点の追跡を行う。ここで、特徴点の追跡手法としては、ＫＬＴ Ｔｒａｃｋｅｒなど、一般に知られる画像処理手法を適用すれば良い。上記撮像画像の取り込み及び特徴点検出の処理を継続的に繰り返すことで、継続的に特徴点追跡を行う。さらに、車両ＭＭが一定距離、例えば０．５ｍ移動したところで、一番最初の撮像画像の特徴点と、現在の撮像画像の特徴点とを比較し、公知の８点法などの手法を用いて、マップ作成のための各特徴点の三次元化を行うと共に、特徴点と車両ＭＭの相対関係に基づき、車両ＭＭの初期自車位置と初期自車姿勢を規定する。以上でステップＳ１１２の処理を終了し、ステップＳ１１３へ移行する。

次に示すステップＳ１１３〜ステップＳ１１７の処理は、車両ＭＭの進行とともに繰り返し行われる処理群である。
ステップＳ１１３では、蓄積画像選択部３４が、カメラ２の撮像する撮像方向角と、自車位置に基づくカメラ位置とを用いて、走行履歴ＤＢ５中の蓄積画像群から、現在の撮像画像と照合するのに好適な蓄積画像（過去の撮像画像）を選択する。その後、ステップＳ１１４へ進む。

ステップＳ１１４では、自車位置推定部３３が、カメラ２が撮像した撮像画像の特徴点と、獲得済み三次元特徴点群（走行環境を構成する構築済みのマップの情報）との相対関係で、自車位置を推定する。その後、ステップＳ１１５へ移行する。
本実施形態では、自車位置推定部３３が、現在の撮像画像の特徴点と、ステップＳ１１３で選択された蓄積画像の特徴点との相関関係から自車位置を推定する。

ここで、車両ＭＭの進行方向が大きく変更されない場合には、１制御ステップ前に取得した蓄積画像が選択されることが殆どと思われるが、車両ＭＭの進行方向が大きく変更する場合には、１制御ステップ前に取得した蓄積画像とは別の蓄積画像が選択される場合がある。これは、複数のカメラ２で撮像した蓄積画像が混在して、走行履歴ＤＢ５中に蓄積されているためである。

これに鑑み、ステップＳ１１３において、予め設定した時間内における車両ＭＭの進行方向の変更量が、予め設定した角度、例えば９０度以上変更された場合にだけ、撮像方向角及びカメラ位置に基づき蓄積画像を選択し、それ以外の場合には、直前に撮像した画像を蓄積画像として選択するように処理を行ってもよい。また、走行履歴構成時から経路生成時に切り替わったときにだけ、撮像方向角及びカメラ位置に基づき蓄積画像を選択するようにしても良い。

ステップＳ１１５では、ステップＳ１１４で得られた自車位置情報に基づき、走行環境を構成するマップの情報である、三次元特徴点の更新を行う。具体的には、カメラ２から取得した撮像画像中の特徴点と、ステップＳ１１３で選択された蓄積画像の特徴点とを比較し、一般に知られた８点法などの手法を用いて、特徴点の三次元化を行い、既知の特徴点群に追加する。この処理は、ステップＳ１１４の処理の自己位置推定と共に実施すればよい。すなわち、自己位置推定と共に走行経路生成のためのマップ更新処理をすれば良い。その後、ステップＳ１１６へ進む。

ステップＳ１１６では、データ蓄積部３２が、ステップＳ１１２からステップＳ１１５で取得した、カメラ２の撮像画像、三次元特徴点群、自車位置、撮像方向角の情報群を走行履歴ＤＢ５に保存し、その後、ステップＳ１１７へ進む。なお走行履歴ＤＢ５には、車両センサ群９から算出される車速、操舵角等の各種車両情報を、自車位置と関連付けて保存しても良い。この処理によって、データ蓄積部３２は、処理のためにカメラ画像を取得する度に、カメラ２が撮像した撮像画像を蓄積画像として走行履歴ＤＢ５に蓄積する。具体的には、上述の処理によって、カメラ位置及び撮像方向角の情報と関連付けられた状態で、カメラ２が撮像した撮像画像を蓄積画像として走行履歴ＤＢ５に蓄積する。

ステップＳ１１７では、ディスプレイ６に走行履歴保存を終了するか否かのボタンを表示し、タッチパネル７を通じて、運転者の入力を受け付ける。運転者から終了指示がないと判定した場合は再びステップＳ１１３に戻り、終了の指示があったと判定した場合には、処理を終了する。なお、走行履歴構成の終了指示がない場合でも、次の走行支援開始の指示があった場合には、次の走行支援時の処理に移行するように処理を構成しても良い。

次に、走行支援時の処理を図５のフローチャートを参照して説明する。なお、走行支援に先立ち、上記走行履歴構成時の処理が実施されているものとする。
まずステップＳ１２０では、ディスプレイ６に走行支援を開始するか否かのボタンを表示し、タッチパネル７を通じて運転者の入力を受け付ける。運転者から開始指示があったと判定した場合は、ステップＳ１２１に移行し、開始指示がないと判定した場合には、ステップＳ１２０の問い合わせ、若しくは開始指示の入力確認の処理を繰り返し、運転者からの入力を待つ。
ステップＳ１２１では、自車位置推定部３３が、カメラ２が撮像した現在の撮像画像を用いて、読み込まれた三次元特徴点群との相対関係で、特に、蓄積画像選択部３４が選択した蓄積画像中の特徴点との相対関係から、初期（経路生成前）の自車位置を推定し、ステップＳ１２２へ進む。

ステップＳ１２２では、走行経路算出部３５が、走行履歴ＤＢ５から過去の自車位置の履歴及び、三次元特徴点群（上記マップ情報）を読み込み、現在の車両位置と上記三次元特徴点群との相対関係、特に蓄積画像選択部３４が選択した蓄積画像中の特徴点との相対関係に基づき、現在の車両ＭＭの進行方向に対応した過去の自車位置の軌跡を、走行経路の候補として算出する。その後、ステップＳ１２３へ進む。なお、車両進行方向に過去の自車位置の軌跡が形成できないと判定した場合には、上記三次元特徴点群（上記マップ情報）に基づき自車両ＭＭが走行可能な走行経路を算出するようにしても良い。

ステップＳ１２３では、走行経路上の自車位置情報から、経路始点に近ければ順方向に、終点に近ければ逆方向に利用するよう、経路の方向を定めた上で、現在の自車位置が始点となるよう走行経路の向きを設定して、ステップＳ１２４へ進む。
なお、蓄積画像選択部３４が選択した蓄積画像を中心とした上記三次元特徴点群によって特定される障害物と上記走行経路との関係に基づき、その走行経路がより適性な経路となるように経路の補正を行ってもよい。
ステップＳ１２４では、ディスプレイ６に自動運転を行うか否かのボタンを表示し、タッチパネル７を通じて運転者の入力を受け付け可能な状態とする。運転者から自動運転指示があった場合は、ステップＳ１２５に移行し、自動運転指示がない場合は、ステップＳ１２６に移行する。

ステップＳ１２５では、ステップＳ１２３で設定した走行経路に基づき、自動運転を行う。車速は例えば一定速とし、操舵角は、支援する走行経路と自車位置・姿勢を補正するよう算出する。車両ＥＣＵ８で、算出結果を実現するためのアクセル開度、ブレーキ制御量、ステアリング操舵角の指令値を算出し、その指令値を制御ユニット群１０に送ることで、走行支援経路に沿って車両ＭＭを自動運転の処理を実施する。その後、ステップＳ１２７に進む。

ステップＳ１２６では、ステップＳ１２３で設定した走行経路に基づき、走行支援を行い、ステップＳ１２７に移行する。具体的には、車両進行方向に向いている前方カメラ２（２ａ）で撮像した撮像画像をディスプレイ６に表示させた上で、前方カメラ２（２ａ）の座標系に変換した上記設定した走行経路と、現在の操舵角に基づく予想軌跡とを、重畳表示させる。これによって、運転者が走行支援経路に沿って運転することを支援する。他の走行支援手法としては、例えばステップＳ１２５で算出するステアリング操舵角に基づき、ＥＰＳ制御を行い、反力でステア操舵誘導を行っても良い。
ステップＳ１２７では、自車位置が走行支援経路の終端の一定範囲内に到達したか否かを判定する。到達していない場合は、ステップＳ１２４の判断結果に基づき、ステップＳ１２５もしくはステップ１２６の処理を繰り返し実行する。一方、終端に到達した場合は、処理を終了する。
以上で、走行支援の処理を終了する。

（動作その他）
高速道路の直進走行や追従走行といった単純な状況ではなく、狭路や駐車場に代表されるような一般走行環境で走行支援もしくは自動運転を実現するためには、走行経路生成技術が重要となる。
一般的に走行経路を算出する場合、センサ情報に基づき、走行可能領域か障害物か、などの情報を含む環境地図を構築し、環境に対する車両位置などの車両情報と合わせて経路算出を行う。しかし、こうした技術では、理想経路生成に環境情報と車両情報を使って経路を算出するため、演算に時間がかかる。また、環境認識のために高価なセンサが必要となったり、センサの認識状況によっては必ずしも経路が計算できなかったりする場合もある。

これに対し、本実施形態は、過去の走行軌跡を利用するメモリベースドな走行経路の生成技術であり、蓄積した走行履歴を読み込むだけなので、計算コストは低い。さらに、過去に一度通っている経路の場合には、走行軌跡は必ず存在させることができ、障害物回避等のための必要最低限のセンサを備えれば十分である。このため、本実施形態の経路生成技術は一般に低コストで済む。

また、本実施形態では、少なくとも車両前方を撮像するカメラ２と、車両前方を撮像するカメラ２との２台のカメラ２を備える。このように前後にカメラ２を搭載することで、後方カメラ２（２ｂ）で見えていた特徴点を前方カメラ２（２ａ）で、前方カメラ２（２ａ）で見えていた特徴点を後方カメラ２（２ｂ）で、それぞれ観測可能となる。このため、車両ＭＭの向きに大きな変化があっても、過去の自己位置の履歴による走行軌跡に基づき、経路生成が可能となる。

例えば、図６（ａ）に示すように、車両前方から駐車場に進入して駐車位置に駐車する場合を考える。なお、この駐車の際に、運転者が走行履歴開始指示を入力したとする。
この場合、車両ＭＭの進行に伴い順次実施した自己位置の推定結果の走行履歴によって、図６（ａ）中に示すような、太線の矢印で示す走行軌跡を得ることが可能である。また、駐車位置ＰＡに向けて走行時に前方カメラ２（２ａ）及び後方カメラ２（２ｂ）で撮像した画像から、それぞれ特徴点○及び△が取得することが出来る。図６（ａ）に○で示した点が前方カメラ２（２ａ）で捉えた特徴点であり、△で示した点が後方カメラ２（２ｂ）で捉えた点である。このように、各カメラ２で捉えられる特徴点に差異がある。

上記のように上記駐車位置ＰＡに対応した走行履歴が既に構成されている状態で、図６（ｂ）のように、同じ駐車位置ＰＡから車両前方から発進する場合を想定する。この場合には、上記走行軌跡の履歴を取ったときと、経路生成時とで車両ＭＭの向きが大きく異なる。

このとき、通常は、前方カメラ２（２ａ）で撮像した撮像画像に基づき自車位置を推定して走行軌跡を生成することになる。本実施形態では、前方カメラ２（２ａ）の撮像方向角とのズレが小さく且つ現在の前方カメラ２（２ａ）位置（若しくは車両位置）に近い蓄積画像は、図６から分かるように、後方カメラ２（２ｂ）が撮像した蓄積画像（特徴点△を撮像した画像）となる。そして、本実施形態では、蓄積画像選択部３４が、その後方カメラ２（２ｂ）が撮像した蓄積画像を選択し、その選択した後方カメラ２（２ｂ）による蓄積画像中の特徴点と、前方カメラ２（２ａ）が撮像した撮像画面中の特徴点とを比較することで、マップ情報（構築されている三次元特徴点群）中における自己位置を推定すると共に、過去の自己位置の履歴に基づき、駐車場から出る際の走行経路を生成することが出来る。

なおこのとき、駐車場に進入する際の走行軌跡の方向と、駐車場から出る際の走行経路とは向きが反対であるので、駐車場に進入する際の走行軌跡の向きが反対になるように、走行経路を生成する。すなわち、前後にカメラ２を搭載することで、軌跡保存時とは逆向きに走行経路を生成可能となる。

ここで、２つ以上のカメラ２を搭載した場合、三次元特徴点構築のための画像選択が難しくなる。例として、図３に示す状況で、現在のカメラ画像に対して複数の過去画像（蓄積画像）の中から、最適な蓄積画像を選択する状況を考える。一般に、三次元特徴点の構築では、任意のカメラ画像に対して、同一の特徴点を多く含む蓄積画像を選択し、二つのカメラ画像間で特徴点の相関を算出することで、特徴点を三次元化する。ここで、１つのカメラ２だけを使用する場合には、通常は、撮像画像に最も近い位置で撮像したカメラ画像（図３では候補（１））を選択すれば良い。しかし、複数カメラ２が存在する場合、一番近い位置のカメラ画像は必ずしも最適ではない。例えば図３において、直感的には候補（３）を選択すべきであるが、単純には解決できない。なお、図３では問題をわかりやすく示すために３つのカメラ２を図示しているが、前後２つのカメラ２だけでも、どちらのカメラ２が撮像した蓄積画像を取得すべきか、という課題があることに変わりはない。

これに対し、本実施形態では、自車位置と撮像方向角に基づき検索することで、現在の撮像画像に好適な蓄積画像を選択できる。すなわち、撮像方向角のズレが小さい蓄積画像を選択することで、画像の重なりが大きい画像を選択できる。また、予め設定した接近距離以内の画像を選択することで、より共通する特徴点、特に現在のカメラ位置に近い位置にある特徴点が多く存在する蓄積画像を選択する可能性が増大する。この結果、前後に取り付けた複数のカメラ２を用いた場合でも、安定して走行経路生成が可能となる。

また、前後にカメラ２を取り付けていることにより、上述のように、車両ＭＭが反対向きとなっても安定して自車位置推定が可能となり、経路の逆向き再生ができる。これにより、記録した経路の２倍の経路生成が可能となり、経路の保存効率が倍増する効果がある。
ここで、データ蓄積部３２は画像蓄積手段を構成する。自車位置推定部３３は車両位置検出手段を構成する。蓄積画像選択部３４は画像選択手段を構成する。特徴点検出部３１は特徴点検出手段を構成する。走行経路算出部３５は走行経路算出手段を構成する。

（本実施形態の効果）
（１）本装置は、カメラ２として、少なくとも車両前方及び車両前方をそれぞれ個別に撮像可能な２以上のカメラ２を備える。データ蓄積部３２は、上記各カメラ２がそれぞれ撮像した撮像画像を、撮像方向である撮像方向角の情報及び撮像したときのカメラ位置の情報と関連付けて、蓄積画像として蓄積する。蓄積画像選択部３４は、上記カメラ２が撮像する撮像方向角及び撮像時のカメラ位置と、上記蓄積された各蓄積画像に関連付けられた撮像方向角及びカメラ位置とを比較して、上記カメラ２が撮像する撮像画像との画像の重なりが相対的に大きいと推定される蓄積画像を選択する。
走行経路算出部３５は、上記選択された蓄積画像内の上記特徴点から車両の走行経路を算出する。

撮像方向角が全く異なる複数方向の画像を蓄積画像として蓄積できる。このため、カメラ２の撮像方向角（カメラ２の向き）も考慮に入れて蓄積画像を選択することが可能となる。この結果、進行方向が逆方向に変化するなど経路生成の際における自車両ＭＭの方向に大きな変化があったとしても、自己位置を推定するための蓄積画像として、微小時間前の蓄積画像以外の蓄積画像も適用することが出来る。また蓄積画像で自己位置が推定できるので、経路生成も可能となる。

また、撮像方向角及びカメラ位置に基づき蓄積画像を選択することで、異なるカメラ２による蓄積画像が混在していても、より適切な蓄積画像を選択可能となる。
特に本発明によれば、少なくとも車両前方及び車両前方をそれぞれ個別に撮像可能となっているので、例えば今までの走行経路を逆向きに走行する場合であっても、より確実に自己位置が推定出来ると共に経路生成が可能となる。
以上のように、本発明によれば、車両ＭＭの進行方向に大きな変化があってもより確実に自己位置を推定して経路生成を可能となる。

（２）蓄積画像選択部３４は、上記車両位置検出手段が検出した車両位置の履歴情報も関連付けられ、その車両位置の履歴情報も加味して選択している。
上記蓄積画像選択部３４が選択した蓄積画像と上記自車位置推定部３３が推定した自己位置の履歴とに基づき走行経路を生成する。
上述のように、走行履歴構成時と経路生成時とで車両ＭＭの進行方向に大きな変化があっても、より確実に自車位置の推定が出来るので、より確実に経路生成が可能となる。

（３）上記蓄積画像選択部３４は、上面視において、上記カメラ２が撮像する撮像方向角と蓄積画像に関連付けられた撮像方向角との方向のズレ角が予め設定した設定ズレ角度以内であり、且つ上記カメラ２が撮像するときのカメラ位置と蓄積画像に関連付けられたカメラ位置との距離が予め設定した設定接近距離以内の蓄積画像を選択する。
このようにすることで、ズレ角が予め設定した設定ズレ角度以内とすることで、カメラ２が撮像する撮像画像と相対的に多く重なっている蓄積画像を選択することが可能となる。また設定接近距離以内の蓄積画像を選択することで、上記カメラ２が撮像する撮像画像内の特徴点と、より共通の特徴点が多い蓄積画像を選択することが可能となる。

「第２実施形態」
次に、第２実施形態について図面を参照して説明する。なお、第１実施形態と同様の構成には同一の符号を付して説明する。
本実施形態の基本構成は、上記第１実施形態と同様である。ただし、蓄積画像選択部３４の処理が異なる。なお、その他の構成及び処理については、上記第１実施形態と同様である。このため、その他の構成及び処理の詳細は省略する。

次に、走行支援ＥＣＵ３の処理ブロックである蓄積画像選択部３４の処理、及びその処理に係わる他の処理内容について説明する。
蓄積画像選択部３４は、カメラ２の撮像画像と照合するために相応しい撮像画像を、走行履歴ＤＢ５から選択する。
まず、特徴点検出部３１が、現在の撮像画像の特徴点を算出する。また、特徴点検出部３１によって、走行履歴ＤＢ５の各蓄積画像についても特徴点を検出している。そして、蓄積画像選択部３４は、各蓄積画像について、現在の撮像画像と共通の特徴点が何個存在するかを算出する。そして、蓄積画像選択部３４は、共通の特徴点が最も多い蓄積画像を選択する。

ここで、対象とする蓄積画像群は、現在のカメラ２の撮像方向角とのズレが予め設定した設定ズレ角以下であり、且つ、現在のカメラ２との距離が予め設定した接近距離以下の位置で撮像した蓄積画像の集合とする。
これにより、より確実に同じ特徴点が写っている蓄積画像を選択することが可能となるので、より確実に三次元特徴点の構成が可能となると共に自己位置推定がより確実に可能となる。

ここで、共通の特徴点の数を算出する場合に、全ての特徴点を加算するのではなく、各特徴点までの距離を算出した上で、カメラ位置から一定の距離範囲内の特徴点のみを加算しても良い。これにより、近くに共通の特徴点を多く持つ撮像画像を優先的に選択できるため、三次元特徴点の構成を行う場合に、誤差が少なくなる。この効果は、遠くの特徴点で画像比較するよりも、近くの特徴点で画像比較した方が、誤差が少ないことに起因する。

上記の一定の距離範囲の定め方としては、特徴点までの平均距離を算出し、平均距離の一定倍、例えば１．５倍の範囲としても良い。これにより、周囲の特徴点の分布状況に合わせて、最適に距離を定めることが出来る。
なお、撮像方向角とカメラ２画角による制限を追加して選択対象を絞っているので、特徴点数で蓄積画像を選択するようにしても、演算負荷を抑えることが出来る。
以上のように、蓄積画像選択部３４は、複数カメラ２での撮像画像が混在する走行履歴ＤＢ５から、現在の撮像画像に好適な撮像画像を選択することができる。
なお、走行支援装置１００の処理手順は、第１実施形態と同じであるため、省略する。

（動作その他）
この第２実施形態によると、撮像画像と蓄積画像に共通に含まれる特徴点の数を算出し、共通特徴点が最も多い蓄積画像を選択するので、確実に共通の特徴点を含む撮像部分が多い蓄積画像を選択することが可能となる。
また、特徴点までの距離を考慮に入れて蓄積画像を選択する場合には、特徴点が多く集まってる部分を含む蓄積画像を優先的に選択することができる。この結果、周囲環境の特徴点分布状況に合わせてロバストに、最適な撮像画像を選択することが可能となる。
第１実施形態のように、撮像方向角とカメラ２画角によって一つの蓄積画像を選択する場合に比べて、共通の特徴点も考慮することで、より好適な蓄積画像を選択することが可能となる。すなわち、この第２実施形態では、特徴点の分布状況によっては、周囲環境に合わせてロバストにより好適な撮像画像を選択できる。

（本実施形態の効果）
本実施形態では、第１実施形態に記載した効果に加えて、次のような効果も奏する。
（１）上記蓄積画像選択部３４は、カメラ２が撮像した撮像画像に含まれる特徴点と、蓄積画像に含まれる特徴点とを比較して、カメラ２が撮像した撮像画像に含まれる特徴点と共通する特徴点である共通特徴点の数が最も多い蓄積画像を選択する。
共通特徴点が最も多い蓄積画像を選択するので、より確実に共通な撮像部分が多い蓄積画像を選択することが可能となる。

（２）上記画像選択手段は、撮像画像から検出した特徴点から、上記撮像画像を撮像したときのカメラ位置までの距離である特徴点距離を算出し、その算出した特徴点距離に基づき共通特徴点を限定する。
特徴点までの距離を考慮に入れて蓄積画像を選択するので、特徴点が多く集まってる部分を含む撮像画像を優先的に選択する可能性が高くする。この結果、周囲環境の特徴点分布状況に合わせて、より最適な撮像画像を選択することが可能となる。

２ カメラ
２ａ 前方カメラ
２ｂ 後方カメラ
３１ 特徴点検出部
３２ データ蓄積部
３３ 自車位置推定部
３４ 蓄積画像選択部
３５ 走行経路算出部
３６ 走行支援部
１００ 走行支援装置
ＭＭ 車両
θ１、θ２，θ３ ズレ角

Claims (6)

1. 車両に少なくとも車両前方及び車両後方をそれぞれ個別で車両周囲環境を撮像可能に搭載した２以上のカメラが撮像した画像内にある車両周囲環境の特徴点に基づき、走行経路を生成する走行経路生成装置であって、
   車両位置を検出する車両位置検出手段と、
   上記各カメラがそれぞれ撮像した撮像画像内の特徴点を、撮像方向である撮像方向角の情報及び撮像したときの上記車両位置検出手段が検出した車両位置に基づくカメラ位置の情報と関連付けて、蓄積画像として蓄積する画像蓄積手段と、
   上記カメラが撮像する撮像方向角及び撮像時のカメラ位置と、上記蓄積された各蓄積画像に関連付けられた撮像方向角及びカメラ位置とを比較して、上記カメラが撮像する撮像画像との画像の重なりが相対的に大きいと推定される蓄積画像を選択する画像選択手段と、
   上記選択された画像内の上記特徴点から車両の走行経路を算出する走行経路算出手段と、
   を備えることを特徴とする走行経路生成装置。
2. 上記画像選択手段は、上記車両位置検出手段が検出した車両位置の履歴情報も関連付けられ、その車両位置の履歴情報も加味して選択していることを特徴とする請求項１に記載した走行経路生成装置。
3. 上記画像選択手段は、上面視において、上記カメラが撮像する撮像方向角と蓄積画像に関連付けられた撮像方向角との方向のズレ角が予め設定した設定ズレ角度以内であり、且つ上記カメラが撮像するときのカメラ位置と蓄積画像に関連付けられたカメラ位置との距離が予め設定した設定接近距離以内の蓄積画像を選択することを特徴とする請求項１又は請求項２に記載した走行経路生成装置。
4. 撮像画像から特徴点を検出する特徴点検出手段を備え、
   上記画像選択手段は、カメラが撮像した撮像画像に含まれる特徴点と、蓄積画像に含まれる特徴点とを比較して、カメラが撮像した撮像画像に含まれる特徴点と共通する特徴点である共通特徴点の数が最も多い蓄積画像を選択することを特徴とする請求項１〜請求項３のいずれか１項に記載した走行経路生成装置。
5. 上記画像選択手段は、撮像画像から検出した特徴点から、上記撮像画像を撮像したときのカメラ位置までの距離である特徴点距離を算出し、その算出した特徴点距離に基づき共通特徴点を限定することを特徴とする請求項４に記載した走行経路生成装置。
6. 車両位置を把握できる車両に搭載した少なくとも車両前方及び車両後方をそれぞれ個別に車両周囲環境を撮像可能な２以上のカメラで、少なくとも車両前方及び車両後方を個別に撮像すると共に、画像蓄積手段によって、上記各撮像画像内の特徴点を撮像したときの車両位置に基づくカメラ位置及び撮像方向角を関連付けて蓄積画像として蓄積し、
   画像選択手段によって、撮像方向角及びカメラ位置に基づき上記蓄積画像を照合して、カメラが撮像する撮像画像と画像の重なりが相対的に大きいと推定される蓄積画像を選択し、
   走行経路算出手段によって、上記選択された蓄積画像内の上記特徴点に基づき車両の走行経路を生成することを特徴とする走行経路生成方法。

Images