JP2020037338A - 障害物回避装置および障害物回避経路生成装置 - Google Patents

Abstract

【課題】自車を障害物から回避させるときの自車から障害物までの間隔を指定可能な障害物回避装置を提供する。【解決手段】障害物回避装置２０１は、障害物の移動予測を行う障害物移動予測部２２０と、障害物を左右どちらに回避するか判断した上で、自車を障害物に接触させないための進入禁止領域を設定することにより、自車の状態量または制御入力に制約を設定する制約生成部２３０とを備える。制約生成部２３０は、障害物を右に回避すると判断した場合には、障害物の左側方の領域を進入禁止領域に含ませ、障害物を左に回避すると判断した場合には、障害物の右側方の領域を進入禁止領域に含ませる。【選択図】図１

Description

本発明は、車両の障害物回避装置および障害物回避経路生成装置に関するものである。

自車が走行中の道路内に存在する障害物を検出し、自車を自動操舵して障害物を回避させる障害物回避装置が種々提案されている。例えば下記の特許文献１に開示された障害物回避装置は、障害物の位置、道路境界、自車の予測走行軌跡、障害物回避後の自車の予測姿勢角および目標姿勢角に基づく評価関数を設定し、その評価関数の出力値が最小となる予測走行軌跡を実現するように自車を制御することによって、自車を障害物から回避させる。この評価関数は、自車から障害物までの間隔および自車から道路境界までの間隔が大きいほど、また障害物回避後の自車の予測姿勢角と目標姿勢角との差が小さいほど、出力値が小さくなるように設定される。

特開２００７−２５３７４５号公報

特許文献１の障害物回避装置では、自車を障害物から回避させるときの自車から障害物までの間隔（以下「回避間隔」という）を明確に指定することができない。すなわち、回避間隔は自車の予測走行軌跡に依存し、自車の予測走行軌跡は、回避間隔、自車から道路境界までの間隔、障害物回避後の自車の予測姿勢角と目標姿勢角の差、の３要素の評価関数に対する重みによって決定されるため、回避間隔を明確に指定できないのである。また、回避間隔を指定できないため、評価関数の設定によっては、自車が障害物と接触するような予測走行軌跡で評価関数の出力値が最小となる可能性もある。

本発明は、上記のような問題点を解決するためになされたものであり、自車を障害物から回避させるときの自車から障害物までの間隔（回避間隔）を指定可能な障害物回避装置を提供することを目的とする。

本発明に係る障害物回避装置は、自車の周辺に存在する障害物の位置の情報を含む障害物情報に基づき、前記障害物の移動予測を行う障害物移動予測部と、前記移動予測の結果に基づき前記障害物を左右どちらに回避するか判断した上で、少なくとも前記自車を前記障害物に接触させないための進入禁止領域を設定することにより、前記自車の状態量または制御入力に制約を設定する制約生成部と、少なくとも前記自車の目標経路に対する予測経路の誤差である経路追従誤差を評価項目とし、前記経路追従誤差が小さいほど評価の度合いが良くなる評価関数を設定する評価関数設定部と、前記制約のもとで前記評価関数の評価の度合いが良くなる解を演算する解演算部と、前記解に基づき前記自車の状態量の予測値を演算する車両状態量予測部と、前記解および前記予測された状態量の少なくとも片方に基づき前記自車の操舵量を制御するアクチュエータの目標値を演算する目標値演算部と、を備え、前記制約生成部は、前記障害物を右に回避すると判断した場合には、前記障害物の左側方の領域を前記進入禁止領域に含ませ、前記障害物を左に回避すると判断した場合には、前記障害物の右側方の領域を進入禁止領域に含ませる。

本発明によれば、車線維持と障害物回避のための車両制御を同一のロジックで実現でき、また、自車の進入禁止領域を設定して自車の状態変数に制約を設けることで、自車から障害物までの間隔（回避間隔）の最小値を明確に指定して障害物回避を行うことができる。また、障害物を左右どちらに回避するか事前判断することで、回避までの時間的な余裕を確保でき、回避動作が滑らかになるため、乗り心地が向上する。

本発明の実施の形態１に係る障害物回避システムの構成を示すブロック図である。 本発明の実施の形態１に係る障害物回避システムのハードウェア構成例を示す図である。 本発明の実施の形態１で用いる座標系を説明するための図である。 本発明の実施の形態１に係る障害物回避システムの動作を示すフローチャートである。 本発明の実施の形態１における制約の生成処理のフローチャートである。 本発明の実施の形態１における進入禁止領域の設定方法を説明するための図である。 道路形状に応じて変形された進入禁止領域の例を示す図である。 無限遠方に設定される進入禁止領域の例を示す図である。 拡張された進入禁止領域の例を示す図である。 拡張された進入禁止領域の例を示す図である。 拡張された進入禁止領域の例を示す図である。 車両重心のＹ方向の上限値および下限値を説明するための図である。 本発明の実施の形態３に係る障害物回避システムが設定する進入禁止領域の例を示す図である。 本発明の実施の形態４に係る障害物回避システムが設定する上限速度の制約の例を示す図である。 本発明の実施の形態５に係る障害物回避システムの構成を示すブロック図である。 本発明の実施の形態５に係る障害物回避システムの動作を示すフローチャートである。

＜実施の形態１＞
図１は、本発明の実施の形態１の障害物回避システムの概略構成を示すブロック図である。当該障害物回避システムは車両に搭載されており、車両制御ユニット２００と、それに接続された障害物情報取得部１１０、車線情報取得部１２０、車両情報取得部１３０およびアクチュエータ制御部３１０とを備えている。

車両制御ユニット２００は、当該障害物回避システムを搭載する車両（以下「自車」という）の動作を統括的に制御する。車両制御ユニット２００は、例えば、先進運転支援システム電子制御ユニット（ＡＤＡＳ−ＥＣＵ）などである。

障害物情報取得部１１０は、自車の周辺に存在する障害物の位置の情報を含む障害物情報を取得する。障害物情報取得部１１０は、例えば、自車の前方の風景を撮影する前方カメラである。あるいは、障害物情報取得部１１０は、ＬｉＤＡＲ（Light Detection and Ranging）、レーダ、ソナー、車車間通信装置、路車間通信装置などでもよい。

車線情報取得部１２０は、自車が走行する車線の情報である車線情報を取得する。車線情報には、自車が走行する車線の左右の区画線の位置と車線中央の位置の情報が含まれるものとする。車線情報取得部１２０は、例えば前方カメラである。あるいは、車線情報取得部１２０は、ＬｉＤＡＲによる車線の検出結果と地図データとから車線情報を求めてもよいし、ＧＮＳＳ（Global Navigation Satellite System）センサを用いて測定した自車の位置と地図データとから車線情報を求めてもよい。

車両情報取得部１３０は、自車の速度、加速度、方位、角速度などの車両情報を取得する。車両情報取得部１３０は、例えば、操舵角センサ、操舵トルクセンサ、ヨーレートセンサ、速度センサ、加速度センサなどである。

アクチュエータ制御部３１０は、自車のアクチュエータが目標値を実現するように制御する。アクチュエータ制御部３１０は、例えば、電動パワーステアリングＥＣＵ（ＥＰＳ−ＥＣＵ）、パワートレインＥＣＵ、ブレーキＥＣＵなどである。

図１のように、車両制御ユニット２００は、障害物回避装置２０１および車両状態量推定部２１０を備えている。さらに、障害物回避装置２０１は、障害物移動予測部２２０と、制約生成部２３０と、評価関数設定部２４０と、解演算部２５０と、車両状態量予測部２６０と、目標値演算部２７０とを備えている。

車両状態量推定部２１０は、車両情報取得部１３０が取得した車両情報に基づいて、自車の車両状態を推定する。障害物移動予測部２２０は、障害物情報取得部１１０が取得した障害物情報に基づいて、自車周辺の障害物の移動予測を行う。

制約生成部２３０は、障害物移動予測部２２０による障害物の移動予測の結果と、車線情報取得部１２０が取得した車線情報とに基づいて、自車が障害物を回避するときの自車の状態量または制御入力の制約を生成する。詳細は後述するが、制約生成部２３０が行う制約の生成には、自車を障害物に接触させないための進入禁止領域の設定が含まれる。また、制約生成部２３０は、進入禁止路を設定するのに先立って、自車が障害物を左右のどちらに回避するか（自車が障害物を回避するときに障害物の左右どちら側を通過するか）を判断し、自車の進入禁止領域はその判断結果に基づいて設定される。

評価関数設定部２４０は、少なくとも自車の目標経路に対する予測経路の誤差である経路追従誤差を評価する評価関数を設定する。解演算部２５０は、制約生成部２３０が生成した制約のもとで評価関数を小さくする解、すなわち制御入力の予測値を演算する。車両状態量予測部２６０は、解演算部２５０が算出した制御入力の予測値に基づいて、自車の車両状態量の予測値を演算する。

目標値演算部２７０は、車両状態量予測部２６０が算出した状態量の予測値と、解演算部２５０が算出した制御入力の予測値とから、アクチュエータの目標値を演算する。目標値演算部２７０が算出したアクチュエータの目標値は、アクチュエータ制御部３１０に入力され、アクチュエータ制御部３１０は、その目標値を実現するようにアクチュエータを制御する。

図２は、障害物回避システムのハードウェア構成例を示す図である。当該障害物回避システムを搭載する自車１は、ステアリングホイール２と、ステアリング軸３と、操舵ユニット４と、ＥＰＳモータ５と、パワートレインユニット６と、ブレーキユニット７と、前方カメラ１１１と、レーダセンサ１１２と、ＧＮＳＳセンサ１２１と、ナビゲーション装置１２２と、操舵角センサ１３１と、操舵トルクセンサ１３２と、ヨーレートセンサ１３３と、速度センサ１３４と、加速度センサ１３５と、図１に示した車両制御ユニット２００と、ＥＰＳコントローラ３１１と、パワートレインコントローラ３１２と、ブレーキコントローラ３１３とを備える。

ステアリングホイール２は、ドライバが自車１を操作するための、いわゆるハンドルである。ステアリングホイール２はステアリング軸３に結合されており、ステアリング軸３には操舵ユニット４が連接されている。操舵ユニット４は、操舵輪としての前輪を回動自在に支持すると共に、車体フレームに転舵自在に支持されている。従って、ドライバのステアリングホイール２の操作によって発生した操舵トルクはステアリング軸３を回転させ、ステアリング軸３の回転に応じて操舵ユニット４が前輪を左右方向へ転舵させる。これによって、ドライバは自車１が前進または後進する際の自車１の横移動量を操作することができる。

なお、ステアリング軸３はＥＰＳモータ５によって回転させることも可能である。ＥＰＳコントローラ３１１は、ＥＰＳモータ５に流れる電流を制御することで、ドライバのステアリングホイール２の操作と独立して、前輪を自在に転舵させることができる。

車両制御ユニット２００には、前方カメラ１１１、レーダセンサ１１２、ＧＮＳＳセンサ１２１、ナビゲーション装置１２２、操舵角センサ１３１、操舵トルクセンサ１３２、ヨーレートセンサ１３３、速度センサ１３４、加速度センサ１３５、ＥＰＳコントローラ３１１、パワートレインコントローラ３１２、ブレーキコントローラ３１３が接続されている。

前方カメラ１１１は、車両前方の区画線が画像として検出できる位置に設置され、画像情報を基に、車線情報や障害物の位置などの自車の前方環境を検出する。ここでは、前方環境を検出する前方カメラのみを示しているが、後方や側方の環境を検出するカメラも自車１に設置されてもよい。レーダセンサ１１２は、レーダを照射し、その反射波を検出することで、自車１を基準にした障害物の相対距離および相対速度を出力する。レーダセンサ１１２としては、ミリ波レーダ、ＬｉＤＡＲ、レーザーレンジファインダ、超音波レーダなど周知のものを用いることができる。ここでは、図１に示した障害物情報取得部１１０は、前方カメラ１１１およびレーダセンサ１１２により構成されるものとする。

ＧＮＳＳセンサ１２１は測位衛星からの電波をアンテナで受信し、測位演算することによって自車１の絶対位置および絶対方位を出力する。ナビゲーション装置１２２は、ドライバが設定した行き先（目的地）に対する最適な走行ルートを演算する機能を有し、道路網を構成する各道路の道路情報を含む地図データを記憶している。道路情報は、道路線形を表現する地図ノードデータであり、各地図ノードデータには各ノードの絶対位置（緯度、経度、標高）、車線幅、カント角、傾斜角情報などが含まれている。ここでは、図１に示した車線情報取得部１２０は、ＧＮＳＳセンサ１２１およびナビゲーション装置１２２により構成されるものとする。

操舵角センサ１３１は、ステアリングホイール２の操舵角を検出する。操舵トルクセンサ１３２は、ステアリング軸３の操舵トルクを検出する。ヨーレートセンサ１３３は、自車１のヨーレートを検出する。速度センサ１３４は、自車１の速度を検出する。加速度センサ１３５は、自車１の加速度を検出する。ここでは、図１に示した車両情報取得部１３０は、操舵角センサ１３１、操舵トルクセンサ１３２、ヨーレートセンサ１３３、速度センサ１３４および加速度センサ１３５により構成されるものとする。

車両制御ユニット２００は、マイクロプロセッサ等の集積回路であり、Ａ／Ｄ変換回路、Ｄ／Ａ変換回路、ＣＰＵ（Central Processing Unit）などのプロセッサ、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）などのメモリ等を備える。車両制御ユニット２００のプロセッサは、上記の各センサから入力された情報を、ＲＯＭに格納されたプログラムに従って処理することで、自車１の目標操舵角、目標駆動力および目標制動力を算出する。

言い換えれば、車両制御ユニット２００は、プロセッサにより実行されるときに、自車１の周辺に存在する障害物の位置の情報を含む障害物情報に基づき、障害物の移動予測を行う処理と、移動予測の結果に基づき障害物を左右どちらに回避するか判断した上で、少なくとも自車１を障害物に接触させないための進入禁止領域を設定することにより、自車１の状態量または制御入力に制約を設定する処理と、少なくとも自車１の目標経路に対する予測経路の誤差である経路追従誤差を評価項目とし、経路追従誤差が小さいほど評価の度合いが良くなる評価関数を設定する処理と、制約のもとで評価関数の評価の度合いが良くなる解を演算する処理と、解に基づき自車１の状態量の予測値を演算する処理と、解および予測された状態量の少なくとも片方に基づき自車１の操舵量を制御するアクチュエータの目標値を演算する処理と、が結果的に実行されることになるプログラムを格納するためのメモリを備える。換言すれば、このプログラムは、障害物回避装置２０１の構成要素の動作の手順や方法をコンピュータに実行させるものであるともいえる。

そして、車両制御ユニット２００は、ＥＰＳコントローラ３１１に目標操舵角を送信し、パワートレインコントローラ３１２に目標駆動力を送信し、ブレーキコントローラ３１３に目標制動力を送信する。なお、車両制御ユニット２００で加減速制御（加減速量の制御）を行わない場合は、車両制御ユニット２００にパワートレインコントローラ３１２とブレーキコントローラ３１３が接続される必要はない。

ＥＰＳコントローラ３１１は、車両制御ユニット２００から受信した目標操舵角を実現するように、ＥＰＳモータ５を制御する。パワートレインコントローラ３１２は、車両制御ユニット２００から受信した目標駆動力を実現するように、パワートレインユニット６を制御する。ブレーキコントローラ３１３は、車両制御ユニット２００から受信した目標制動力を実現するように、ブレーキユニット７を制御する。ここでは、図１に示したアクチュエータ制御部３１０は、ＥＰＳコントローラ３１１、パワートレインコントローラ３１２およびブレーキコントローラ３１３により構成されるものとする。

なお、速度制御をドライバが行う場合、パワートレインコントローラ３１２は、アクセルペダルの踏み込み量に基づいてパワートレインユニット６を制御し、ブレーキコントローラ３１３は、ブレーキペダル踏み込み量に基づきブレーキユニット７を制御する。

図２には、自車１の例として、エンジンのみを駆動力源とする車両を示したが、自車１は、電動モータのみを駆動力源とする電気自動車や、エンジンと電動モータの両方を駆動力源とするハイブリッド車などでもよい。

次に、実施の形態１に係る障害物回避システムの動作を説明する。図３に、実施の形態１に係る障害物回避システムが用いる座標系を模式的に示す。図３において、Ｘ軸およびＹ軸は慣性座標系を表しており、Ｘ_ｃ，Ｙ_ｃ，θは慣性座標系での自車１の重心位置および方位角を表している。ｘ軸およびｙ軸は自車１の重心を原点とする自車座標系を表しており、自車１の前方にｘ軸、左手方向にｙ軸をとっている。本実施の形態では、制御周期ごとに自車の重心位置Ｘ_ｃ，Ｙ_ｃと方位角θとを０に初期化する。すなわち、慣性座標系と自車座標系とを制御周期ごとに一致させる。この処理により、方位角θの変化が小さい場合に、Ｘ方向を自車１の前方向、Ｙ方向を自車１の横方向として考えることができる。

障害物回避装置２０１の解演算部２５０は、車両のダイナミクスを数学的に表した動的車両モデルを用いて、現在の時刻０から一定周期Ｔ_ｓの間隔で時間Ｔ_ｈだけ未来までの自車１の挙動を予測し、自車１の望ましい動作を表現する評価関数を最小化する制御入力ｕを求める最適化問題を一定期間ごとに解くことで、その解として、最適な目標操舵量、あるいは、最適な目標操舵量および目標加速度を算出する。このとき、予測する車両状態量の点数は、Ｎ点（Ｎ＝Ｔ_ｈ／Ｔ_ｓ）となる。現在の時刻から時間Ｔ_ｈだけ未来までの間の時間を「ホライズン」と称す。

なお、最適化問題の解は、必ずしも評価関数を最小化するものでなくてもよく、評価関数による評価の度合いをある程度以上良くできるものであればよい。例えば、評価関数を予め定められた閾値よりも小さくできた入力値を解としてもよい。また、予め定められた回数だけ反復計算を行っても評価関数が閾値を下回らなかった場合に、それまでの反復計算で評価関数を最も小さくできた入力値を解としてもよい。

また、本実施の形態では最適化問題を最小化問題として扱うが、評価関数の符号を反転させることで最大化問題として扱うこともできる。その場合も、最適化問題の解は、必ずしも評価関数を最大化するものでなくてもよい。例えば、評価関数が予め定められた閾値よりも大きくできた入力値を解としてもよい。また、予め定められた回数だけ反復計算を行っても評価関数が閾値を上回らなかった場合に、それまでの反復計算で評価関数を最も大きくできた入力値を解としてもよい。

以下、解演算部２５０が解く最適化問題の例を示す。以下の説明において、記号「＿」はそれに続く文字に付されたアンダーバーを意味し、記号「￣」はそれに続く文字に付されたオーバーバーを意味し、記号「＾」はそれに続く文字に付されたハット（キャレット）を意味している。

本実施の形態では、解演算部２５０は、次に示す制約付き最適化問題を一定期間ごとに解く。

ここで、Ｊは評価関数、ｘは車両状態量、ｕは制御入力、ｆは動的車両モデルに関するベクトル値関数、ｘ_０は初期値、ｇは制約に関するベクトル値関数である。

本実施の形態では、車両制御ユニット２００は操舵制御のみを行うこととし、車両状態量ｘおよび制御入力ｕを次のように設定する。

ここで、βは横滑り角、γはヨーレート、δは舵角、ωは舵角速度である。このとき、制御入力ｕとアクチュエータ制御部３１０の制御量とを一致させる必要はない。そのため、アクチュエータ制御部３１０の制御量に依存せずに車両状態量ｘおよび制御入力ｕを設定できる。そして、舵角ではなく舵角速度を制御入力とすることで、評価関数や制約の設定次第で舵角の変動の大きさを容易に制限でき、車両の乗り心地が向上する。

動的車両モデルは次に示す二輪モデルを用いる。

ここで、Ｍは車両質量、Ｖは速度、Ｉは車両のヨー慣性モーメント、ｌ_ｆ，ｌ_ｒは車両重心から前後車軸までの距離である。Ｙ_ｆ，Ｙ_ｒは、それぞれ前輪および後輪のコーナリングフォースであり、前輪および後輪のコーナリングスティフネスＣ_ｆ，Ｃ_ｒを用いて次式のように近似できる。

式１０６および式１０７を用いて式１０５を整理すると、動的車両モデルｆは次式のようになる。

なお、動的車両モデルは、車両の位置を車両状態量に含むものであれば、二輪モデル以外のモデルを用いてもよい。また、モデルをコンパクトにするために、ｘ＝［Ｘ_ｃ，Ｙ_ｃ，θ，β，γ］^Ｔ，ｕ＝［δ］^Ｔとしてもよい。

本実施の形態では、評価関数Ｊには次式を用いる。

ここで、ｘ_ｋは予測点ｋ（ｋ＝０，…，Ｎ）における車両状態量、ｕ_ｋは予測点ｋ（ｋ＝０，…，Ｎ−１）における制御入力である。ｈは評価項目に関するベクトル値関数、ｈ_Ｎは終端（予測点Ｎ）における評価項目に関するベクトル値関数であり、ｒ_ｋは予測点ｋ（ｋ＝０，…，Ｎ）における目標値である。Ｗ，Ｗ_Ｎは重み行列であり、各評価項目に対する重みを対角成分に有する対角行列であり、パラメータとして適宜変更可能である。本実施の形態では、小さな制御入力で車両が車線中央を走行するように操舵制御するものとし、評価項目に関するベクトル値関数ｈ，ｈ_Ｎを次のように設定する。

ここで、ｅ_Ｙ，ｋは予測点ｋ（ｋ＝０，…，Ｎ）における経路追従誤差（自車の目標経路に対する予測経路の誤差）である。目標経路が自車座標系において関数ｙ＝ｌ（ｘ）で表現されているとすると、経路追従誤差ｅ_Ｙ，ｋ＝Ｙ_ｃ，ｋ−ｌ（Ｘ_ｃ，ｋ）である。なお、目標経路は関数表現されている経路であればよく、例えば、車線中央を関数近似したものでよく、車線情報取得部１２０で生成される。そして、経路追従誤差ｅ_Ｙ，ｋと操舵速度ω_ｋが小さくなるように、目標値ｒ_ｋ，ｒ_Ｎを次のように設定する。

経路追従誤差を評価することにより、ホライズン内に障害物が存在しない場合には車線維持を行い、障害物が存在する場合には、障害物の位置を含む進入禁止領域が設定されて、障害物回避を行うことが可能となる。そのため、車線維持と障害物回避を同一の制御ロジックで実現でき、スムーズな回避が可能となる。なお、本実施の形態では、経路追従誤差と操舵速度のみを評価するように評価項目を設定したが、経路追従性や乗り心地を向上させるために、方位、ヨーレートなどを評価項目に加えてもよい。

次に、制約に関するベクトル値関数ｇについて説明する。関数ｇは、制約付き最適化問題において、車両状態量ｘと、制御入力ｕの上下限値とを設定するためのものであり、最適化はｇ（ｘ，ｕ）≦０の条件のもと実行される。

本実施の形態では、進入禁止領域を設定し、一定範囲内の操舵速度で障害物の回避を行うために、車両の重心位置Ｙ_ｃの上限値を￣Ｙ_ｃ（＞０）、下限値を＿Ｙ_ｃ（＜０）、操舵速度ωの上限値を￣ω（＞０）、下限値を＿ω（＜０）として、関数ｇを以下のように設定する。

ホライズン内で方位角θの変化が小さい場合には、Ｙ方向を車両の横方向と考えることができるので、Ｙ_ｃの上下限値を設定することで、車両の横位置に制約を課すことができる。また、操舵速度ωの上下限値を設定することで、乗り心地を確保した車両制御を行うことができる。式１１４のように、車両状態量や制御入力の取りうる範囲を連続な１つの区間で表現することにより、制約が単純な形状になり、演算負荷の軽減や、求解確率の向上といった効果が得られる。

図４は、実施の形態１に係る障害物回避システムの動作を示すフローチャートである。以下、図４を参照しつつ、障害物回避装置２０１の動作を説明する。

障害物回避装置２０１が起動すると、以下のステップＳ１１０，Ｓ１２０，Ｓ１３０が実行される。ステップＳ１１０では、障害物情報取得部１１０が、障害物の位置の情報を含む障害物情報を取得する。本実施の形態では、障害物が自車１から見て左前方に存在する場合には、自車座標系での障害物の右前端Ｐ_ＦＲ、右後端Ｐ_ＲＲ、左後端Ｐ_ＲＬの位置が障害物情報として取得され、障害物が自車から見て右前方に存在する場合には、自車座標系での障害物の左前端Ｐ_ＦＬ、左後端Ｐ_ＲＬ、右後端Ｐ_ＲＲの位置が障害物情報として取得されるものとする。さらに、障害物情報取得部１１０は、取得したそれらの位置情報に基づいて、障害物の左前端Ｐ_ＦＬあるいは右前端Ｐ_ＦＲの位置、障害物の中心Ｏ_ｏｂｓの位置ｘ_ｏｂｓ，ｙ_ｏｂｓ、方位θ_ｏｂｓ、速度Ｖ_ｏｂｓ、長さｌ_ｏｂｓ、幅ｗ_ｏｂｓを推定する。

ステップＳ１２０では、車線情報取得部１２０が、自車が走行すべき車線の左右の区画線の位置および車線中央の位置の情報を含む車線情報を取得する。本実施の形態では、自車座標系で左右の区画線を３次の多項式で表現した際の係数が車線情報として取得されるものとする。すなわち、左の区画線に対しては、次式のＣ_ｌ０〜Ｃ_ｌ３の値が取得される。

また、右の区画線に対しては、次式ののＣ_ｒ０〜Ｃ_ｒ３の値が取得される。

このとき、車線中央（目標経路）は、次式のように表現される。

なお、区画線の情報は３次の多項式に限らず、いかなる関数で表現されていてもよい。また、目標経路は車線中央でなくてもよく、関数表現されている任意の経路でよい。

ステップＳ１３０では、車両情報取得部１３０が、自車の操舵角、ヨーレート、速度、加速度などの車両情報を取得する。本実施の形態では、車両情報として、舵角δ、ヨーレートγ、速度Ｖ、加速度ａが取得されるものとする。

次に、ステップＳ２１０において、車両状態量推定部２１０が自車の車両状態量ｘを推定する。車両状態量の推定には、ローパスフィルタ、オブザーバ、カルマンフィルタ、粒子フィルタなど、公知の技術を用いることができる。

また、ステップＳ２２０において、障害物移動予測部２２０が障害物の移動予測を行う。具体的には、障害物移動予測部２２０は、各予測点ｋ（ｋ＝１，…，Ｎ）における障害物の中心位置ｘ_{ｏｂｓ，ｋ}，ｙ_{ｏｂｓ，ｋ}を予測する。本実施の形態では、障害物の運動を等速直線運動で近似し、障害物情報取得部１１０が取得した現時刻での障害物の中心位置ｘ_{ｏｂｓ，０}，ｙ_{ｏｂｓ，０}、方位θ_ｏｂｓ、速度Ｖ_ｏｂｓを用いて、中心位置ｘ_{ｏｂｓ，ｋ}，ｙ_{ｏｂｓ，ｋ}を次の式から予測する。

障害物が複数存在する場合には、障害物移動予測部２２０は、各障害物に対して中心位置ｘ_{ｏｂｓ，ｋ}，ｙ_{ｏｂｓ，ｋ}の予測を行う。なお、等速直線運動の近似を用いるのではなく、二輪モデルなどの車両モデルを用いて移動予測を行ってもよい。

次に、ステップＳ２３０において、制約生成部２３０が、制約（式１１４）を生成する。また、ステップＳ２４０において、評価関数設定部２４０が、評価関数（式１０９）を生成する。

そして、ステップＳ２５０において、解演算部２５０が、評価関数（式１０９）と制約（式１１４）とを用いて、制約付き最適化問題の解を演算する。解の演算には、K.U.Leuven大学により開発された、ＡＣＡＤＯ（Automatic Control And Dynamic Optimization）や、Ｃ／ＧＭＲＥＳ（Continuation/Generalized Minimum Residual）法をベースとして最適化問題を解く自動コード生成ツールであるＡｕｔｏＧｅｎなど、公知の手段を用いることができる。

次に、ステップＳ２６０において、車両状態量予測部２６０が、自車の車両状態量の予測値を演算する。ステップＳ２５０で、ＡＣＡＤＯやＡｕｔｏＧｅｎを用いた場合、各予測点ｋ（ｋ＝１，…，Ｎ−１）での最適化された制御入力の時系列（最適制御入力）ｕ^＊が、解演算部２５０の出力となる。すなわち、解演算部２５０の出力は、次式で表される。

この場合、ステップＳ２６０の車両状態量の予測では、ｕ^＊と動的車両モデルｆを用いて、最適化された車両状態量の時系列（最適状態量）ｘ^＊が演算される。このときｘ^＊は次式のように表現される。

次に、ステップＳ２７０において、目標値演算部２７０が、アクチュエータの目標値を演算する。本実施の形態では、車両制御ユニット２００は操舵制御のみを行うので、目標値演算部２７０は、次の式で表される目標操舵角δ_ｒｅｆをアクチュエータ制御部３１０に送信する。

ここで、ｘ_ｋ ^＊［ｉ］は、予測点ｋにおける最適状態量ｘ_ｋ ^＊のｉ番目の要素を意味する。なお、目標値演算部２７０が他の目標値をアクチュエータ制御部３１０へ送信する場合、最適状態量ｘ^＊や最適制御入力ｕ^＊から目標値を演算すればよい。例えば、目標値演算部２７０がアクチュエータ制御部３１０へ目標操舵速度ω_ｒｅｆを送信する場合には、ω_ｒｅｆ＝ｕ_０ ^＊［１］＝ω_０ ^＊とすればよい。

次に、ステップＳ３１０において、アクチュエータ制御部３１０が、目標値演算部２７０から受信した目標値を実現するようにアクチュエータを制御する。本実施の形態では、車両制御ユニット２００は操舵制御のみを行うので、アクチュエータ制御部３１０は、目標操舵角δ_ｒｅｆを実現するように、ＥＰＳモータ５を制御する。障害物回避システムは、以上の動作を繰り返し実行する。

ここで、図４のステップＳ２３０で制約生成部２３０が行う制約（式１１４）の生成について説明する。図５は、制約の生成処理を示すフローチャートである。

制約の処理では、まず、ステップＳ２３１において、制約生成部２３０が、障害物を左右どちらに回避するかを判断する。障害物をどちらに回避するかを事前に判断することにより、回避までの時間的な余裕を確保できるため、回避動作が滑らかになり、乗り心地が向上する。この判断は、障害物の移動予測結果に基づいて行われる。

例えば、自車が走行する車線における障害物の位置に基づいて、自車が障害物を左右どちらに回避するかを判断することができる。すなわち、自車が走行する車線において、障害物が車線中央よりも左に存在するときは右に回避すると判断し、障害物が車線中央よりも右に存在するときは左に回避すると判断するとよい。

あるいは、障害物の右側に自車が走行可能な領域があれば、障害物の位置に関わらず、右に回避すると判断してもよい。例えば、障害物が自車の前を走行する他の車両（先行車）である場合、追い越しの方向が右側と定められている地域でも、道路交通法に抵触せずに先行車を回避できる。

また、障害物の左側に走行可能な領域があれば、障害物の位置に関わらず、左に回避すると判断してもよい。例えば、障害物が先行車である場合、追い越しの方向が左側と定められている地域でも、道路交通法に抵触せずに先行車を回避できる。

続いて、制約生成部２３０は、ステップＳ２３２において進入禁止領域を設定し、ステップＳ２３３において進入禁止領域を拡張する。進入禁止領域の設定および拡張については後述する。

その後、制約生成部２３０は、ステップＳ２３４において、前回の最適制御入力ｕ^＊を用いて、最適状態量ｘ^＊を予測する。このとき、次の更新式により予測最適状態量＾Ｘ_ｋ＋１ ^＊（ｋ＝０，…，Ｎ−１）を演算する。

ここで、＾ｘ_０ ^＊＝ｘ_０である。また、＾ｕ_ｋ ^＊（ｋ＝０，…，Ｎ−１）は、前回の最適制御入力ｕ_ｋ ^＊（ｋ＝０，…，Ｎ−１）を用いて次式から求められる予測最適制御入力である。

次に、ステップＳ２３５において、制約生成部２３０は、自車の重心位置Ｙ_ｃの上限値￣Ｙ_ｃおよび下限値＿Ｙ_ｃ、操舵速度ωの上限値￣ω（＞０）および下限値＿ω（＜０）を、それぞれ設定する。その結果、式１１４で表される制約が生成される。ＡｕｔｏＧｅｎやＡＣＡＤＯを用いる場合、各予測点ｋ（ｋ＝１，…，Ｎ）の自車の重心位置Ｙ_ｃ，ｋの上限値￣Ｙ_ｃ，ｋおよび下限値＿Ｙ_ｃ，ｋを設定可能であるため、本実施の形態では、予測点ｋ（ｋ＝１，…，Ｎ）ごとに自車の重心位置Ｙ_ｃ，ｋの上限値￣Ｙ_ｃ，ｋおよび下限値＿Ｙ_ｃ，ｋを設定する。

図６は、図５のステップＳ２３２で設定される進入禁止領域の設定方法を説明するための図である。ここでは、自車（不図示）が直線道路を走行しており、障害物情報取得部１１０により、先行車１１が障害物として検出されたものと仮定する。先行車１１は、自車の左前方で車線に対して平行に停止あるいは走行しているものとする。また、制約生成部２３０により、自車は先行車１１を右に回避する（先行車１１の右側を追い抜く）と判断されたものとする。なお、車線は直線道路でなくてもよい。

まず、制約生成部２３０は、障害物情報取得部１１０で取得された先行車１１の左前端Ｐ_ＦＬ、右前端Ｐ_ＦＲ、右後端Ｐ_ＲＲ、左後端Ｐ_ＲＬの位置に基づいて、図６のように、Ｐ_ＦＬ，Ｐ_ＦＲ，Ｐ_ＲＲ，Ｐ_ＲＬを頂点とする矩形の領域を障害物領域（以下「障害物領域Ｐ_ＦＬＰ_ＦＲＰ_ＲＲＰ_ＲＬ」と称す）として設定する。

次に、制約生成部２３０は、障害物領域Ｐ_ＦＬＰ_ＦＲＰ_ＲＲＰ_ＲＬを、車線と平行な方向に自車の長さｌ_ｅｇｏの半分だけ前と後ろ（自車から見て奥と手前）に拡張し、さらに車線と垂直な方向に自車の幅ｗ_ｅｇｏの半分だけ右と左に拡張することで得られる、Ｑ_ＦＬ，Ｑ_ＦＲ，Ｑ_ＲＲ，Ｑ_ＲＬを頂点とする矩形の領域を接触領域（以下「接触領域Ｑ_ＦＬＱ_ＦＲＱ_ＲＲＱ_ＲＬ」と称す）として設定する。接触領域Ｑ_ＦＬＱ_ＦＲＱ_ＲＲＱ_ＲＬは、自車が車線に平行に走行するときに、自車の重心が進入すると先行車１１と接触してしまう領域である。

続いて、制約生成部２３０は、接触領域Ｑ_ＦＬＱ_ＦＲＱ_ＲＲＱ_ＲＬの手前側の辺Ｑ_ＲＬＱ_ＲＲの中点から距離ｌ_Ｒだけ手前に位置する点Ｒ_ＲＣと、接触領域Ｑ_ＦＬＱ_ＦＲＱ_ＲＲＱ_ＲＬの奥側の辺Ｑ_ＦＬＱ_ＦＲの中点から距離ｌ_Ｆだけ奥に位置する点Ｒ_ＦＣと、接触領域Ｑ_ＦＬＱ_ＦＲＱ_ＲＲＱ_ＲＬを車線と垂直な方向に距離ｌ_Ｓだけ右と左に拡張することで得られる矩形の頂点Ｒ_ＦＬ，Ｒ_ＦＲ，Ｒ_ＲＲ，Ｒ_ＲＬとを求め、それらＲ_ＦＬ，Ｒ_ＦＣ，Ｒ_ＦＲ，Ｒ_ＲＲ，Ｒ_ＲＣ，Ｒ_ＲＬを頂点とする六角形の領域を進入禁止領域（以下「進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬ」と称す）として設定する。ここで、ｌ_Ｒ，ｌ_Ｆ，ｌ_Ｓは回避間隔を規定するパラメータである。

このように進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬを設定することで、回避間隔を明確に指定できる。また、ｌ_Ｒ，ｌ_Ｆ，ｌ_Ｓの値は、適宜変更可能であり、自車が走行する車線の制限速度などに応じて設定される。例えば、自車が制限速度４０ｋｍ／ｈの車線を走行中であれば、走行速度４０ｋｍ／ｈのときの一般的な停止距離（空走距離と制動距離との和）が２０ｍ程度であるため、ｌ_Ｒ＝２０［ｍ］と設定するとよい。なお、進入禁止領域の形状は六角形に限らず、いかなる形状でもよい。ただし、進入禁止領域を凸形状（ここでは凸六角形）に設定することで、最適解の求解速度の向上や求解可能性の向上を図ることができる。また、障害物が複数存在する場合には、それぞれの障害物に対して、同様に進入禁止領域を設定すればよい。また、車線の外側の領域（区画線よりも外側の領域）を進入禁止領域に加えてもよい。

しかし、図７のように、障害物である先行車１１が曲線路に存在する場合、先行車１１の横に充分な回避スペースがあるにも関わらず、上記の方法で設定された進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬが車線を塞ぐおそれがある。そのため、本実施の形態では、制約生成部２３０は、進入禁止領域の形状を道路形状に応じて修正する。

図７には、制約生成部２３０が、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬの頂点Ｒ_ＦＣおよびＲ_ＲＣを道路形状に応じてそれぞれ点Ｒ_ＦＣ’およびＲ_ＲＣ’に置き換えることによって修正した、進入禁止領域Ｒ_ＦＬＲ_ＦＣ’Ｒ_ＦＲＲ_ＲＲＲ_ＲＣ’Ｒ_ＲＬを示している。点Ｒ_ＦＣ’およびＲ_ＲＣ’は、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬの頂点Ｒ_ＦＣおよびＲ_ＲＣを、それぞれ自車座標系のＹ方向（自車１の横方向）に距離ｄ_ＦＣおよびｄ_ＲＣだけ移動した点である。

距離ｄ_ＦＣおよびｄ_ＲＣは、制約生成部２３０によって次のように設定される。まず、制約生成部２３０は、自車座標系でＸ＝ｘ_ｏｂｓ（障害物の中心Ｏ_ｏｂｓのＸ座標）となる位置での車線中央の接線ｌ_ｃを求める。そして、制約生成部２３０は、点Ｒ_ＦＣのＸ座標における車線中央と接線ｌ_ｃとのＹ方向偏差を距離ｄ_ＦＣとして設定し、点Ｒ_ＲＣのＸ座標における車線中央と接線ｌ_ｃとのＹ方向偏差を距離ｄ_ＲＣとして設定する。

次に、図５のステップＳ２３３で行われる進入禁止領域の拡張について説明する。ここで、制約生成部２３０は、図８のように、自車１の左右の無限遠方にそれぞれ進入禁止領域Ａ_ＬおよびＡ_Ｒを予め設定しているものとする。すなわち、進入禁止領域Ａ_Ｌは、自車１が走行する車線の左側に位置し、Ｙ＞∞で定義される領域である。進入禁止領域Ａ_Ｒは、自車１が走行する車線の右側に位置し、Ｙ＜−∞で定義される領域である。図８のように進入禁止領域Ａ_ＬおよびＡ_Ｒのみが設定されている状態では、実質的に制約がないのと同じである。

制約生成部２３０は、障害物（先行車１１）の位置に基づいて設定された進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬと、進入禁止領域Ａ_ＬおよびＡ_Ｒのいずれか片方とを結合させることによって、進入禁止領域を拡張する。このとき、制約生成部２３０は、図５のステップＳ２３１で障害物を右に回避すると判断した場合には、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬと左側の進入禁止領域Ａ_Ｌとを結合し、障害物を左に回避すると判断した場合には、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬと右側の進入禁止領域Ａ_Ｒとを結合する。

例えば図９のように、障害物である先行車１１が車線の左側に存在する場合、制約生成部２３０は先行車１１を右に回避すると判断し、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬと左側の進入禁止領域Ａ_Ｌとを結合する。制約生成部２３０は、その結果得られた図９の斜線部の領域を、最終的な進入禁止領域として設定する。左側の最終的な進入禁止領域を「進入禁止領域Ｓ_Ｌ」、右側の最終的な進入禁止領域を「進入禁止領域Ｓ_Ｒ」と称す。

また、例えば図１０のように、障害物である先行車１１が車線の右側に存在する場合、制約生成部２３０は先行車１１を左に回避すると判断し、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬと右側の進入禁止領域Ａ_Ｒとを結合する。制約生成部２３０は、その結果得られた図１０の斜線部の領域を、最終的な進入禁止領域Ｓ_Ｌ，Ｓ_Ｒとして設定する。

先に述べたように、制約生成部２３０は、障害物が自車の走行する車線の右側にある場合でも、障害物を右に回避すると判断してもよい。その場合、制約生成部２３０は、図１１のように自車１の走行する車線の右側に障害物（先行車１１）が存在しても、進入禁止領域Ｒ_ＦＬＲ_ＦＣＲ_ＦＲＲ_ＲＲＲ_ＲＣＲ_ＲＬを左側の進入禁止領域Ａ_Ｌと結合する。これにより、自車１は、走行車線の右側に存在する先行車１１を、右隣の車線を使って回避することができる。

なお、障害物が複数存在する場合には、制約生成部２３０は、各障害物に対応する進入禁止領域を進入禁止領域Ａ_ＲもしくはＡ_Ｌと順に結合する。また、区画線よりも外側を進入禁止領域として規定する場合も同様に、制約生成部２３０は、各障害物に対応する進入禁止領域を区画線より外側の進入禁止領域と順に結合すればよい。その結果、最終的な進入禁止領域には、複数の前記進入禁止領域の全てが含まれることになる。

次に、図５のステップＳ２３５において制約生成部２３０が各予測点における自車１の重心位置Ｙ_ｃ，ｋの上限値￣Ｙ_ｃ，ｋ、下限値＿Ｙ_ｃ，ｋを設定する方法を、図１２を用いて説明する。

制約生成部２３０は、図５のステップＳ２３４で算出した予測最適状態量＾ｘ_ｋ ^＊（ｋ＝１，…，Ｎ）に基づき、自車１の軌道（＾Ｘ_ｋ ^＊，＾Ｙ_ｋ ^＊）（ｋ＝１，…，Ｎ）を予測する。そして、図１２に矢印で示すように、各＾Ｘ_ｋ ^＊における左側の進入禁止領域Ｓ_ＬのＹ座標の最小値を上限値￣Ｙ_ｃ，ｋ、右側の進入禁止領域Ｓ_ＲのＹ座標の最大値を下限値＿Ｙ_ｃ，ｋとする。このように重心位置Ｙ_ｃ，ｋの制約を設定することにより、重心位置Ｙ_ｃ，ｋの取りうる範囲を連続な１つの区間として表現できるので、制約が単純な形状になり、演算負荷の軽減や、求解確率の向上といった効果が得られる。

以上のように実施の形態１に係る障害物回避装置２０１によれば、車線維持のための車両制御と障害物回避のための車両制御とを同一のロジックで実現できる。また、自車の進入禁止領域を設定して自車の状態変数に制約を設けることで、自車から障害物までの間隔（回避間隔）の最小値を明確に指定して障害物回避を行うことができる。また、障害物を左右どちらに回避するか事前判断することで、回避までの時間的な余裕を確保でき、回避動作が滑らかになるため、乗り心地が向上する。さらに、障害物の位置に応じて設定された進入禁止領域を無限遠方に設定された進入禁止領域と結合することで、障害物に対応する進入禁止領域の左側方あるいは右側方の領域が最終的な進入禁止領域に含まれるようになる。それにより、進入禁止領域の形状が単純化され、演算負荷の低減や、最適解の求解確率の向上といった効果が得られる。

＜実施の形態２＞
実施の形態１では、車両制御ユニット２００は操舵制御のみを行うものとしたが、実施の形態２では、車両制御ユニット２００が操舵制御と加減速制御の両方を行う場合の例を示す。実施の形態２に係る障害物回避システムの構成および基本的な動作は実施の形態１と同様である。そのため、ここでは実施の形態１と重複する説明は省略する。

車両制御ユニット２００に操舵制御と加減速制御の両方を実行させる場合、車両状態量ｘおよび制御入力ｕを次のように設定する。

ここで、αは加速度、ｊは躍度である。加速度ではなく躍度を制御入力とすることで、評価関数や制約の設定次第で加速度の変動の大きさを容易に制限できるため、乗り心地を向上させることができる。動的車両モデルｆは次に示す二輪モデルを用いる。

なお、モデルをコンパクトにするために、ｘ＝［Ｘ_ｃ，Ｙ_ｃ，θ，β，γ，δ，Ｖ］^Ｔ，ｕ＝［ω，α］^Ｔとしてもよい。

本実施の形態では、小さな制御入力で速度を目標速度に追従させるために、評価項目に関するベクトル値関数ｈ，ｈ_Ｎを次のように設定する。

ここで、ｅ_Ｖ，ｋは予測点ｋ（ｋ＝０，…，Ｎ）における自車の目標速度に対する予測速度の誤差である速度追従誤差である。目標速度をＶ_ｒｅｆとすると、速度追従誤差ｅ_Ｖ，ｋ＝Ｖ_ｋ−Ｖ_ｒｅｆである。なお、目標速度Ｖ_ｒｅｆは、例えば自車が走行する車線の制限速度であり、車線情報取得部１２０により取得される。そして、経路追従誤差ｅ_Ｙ，ｋ、操舵速度ω_ｋ、速度追従誤差ｅ_Ｖ，ｋおよび躍度ｊ_ｋが小さくなるように（零に近くなるように）、目標値ｒ_ｋ，ｒ_Ｎを次のように設定する。

これにより、速度制御と車線維持と障害物回避を同一のロジックで実現でき、乗り心地が向上する。なお、本実施の形態では、経路追従誤差と操舵速度と速度追従誤差と躍度とを評価項目として設定したが、乗り心地を向上させるために、加速度などを評価項目に加えてもよい。

本実施の形態では、一定範囲内の躍度で加減速制御を行うために、躍度ｊの上限値を￣ｊ（＞０）、下限値を＿ｊ（＜０）として、関数ｇを次のように設定する。

躍度ｊの上下限値を設定することで、乗り心地を確保した車両制御を行うことができる。なお、乗り心地を確保するため加速度の上下限値を設定したり、制限速度を厳守するために速度の上下限値を設定したりしてもよい。

実施の形態２の障害物回避装置２０１によれば、速度制御と車線維持と障害物回避とを同一のロジックで実現できるため、自車の乗り心地をさらに向上させることができる。

＜実施の形態３＞
実施の形態３では、自車の走行車線上に先行車が存在する場合に、障害物回避装置２０１の制約生成部２３０が、自車の前後方向の位置Ｘ_ｃに制約を設定する。実施の形態３に係る障害物回避システムの構成および基本的な動作は実施の形態１または２と同様である。そのため、ここでは実施の形態１，２と重複する説明は省略する。

実施の形態３では、例えば図１３のように自車１の前方に先行車１１が存在する場合、自車１が先行車１１との車間距離をｌ_ｆｏｌ以上に維持しながら先行車１１に追従させる。そのため、各予測点における先行車１１の中心Ｏ_ｏｂｓのＸ方向の位置ｘ_ｏｂｓから車線に平行な方向にｌ_ｆｏｌだけ手前に離れた位置Ｘ_ｂに、追従境界ｂを設定する。そして、追従境界ｂより奥の領域、すなわちＸ＞Ｘ_ｂの領域を進入禁止領域Ａ_Ｆとして設定する。ここで、ｌ_ｆｏｌは、ｌ_ｆｏｌ＞（ｌ_ｅｇｏ＋ｌ_ｏｂｓ）／２の範囲で、適宜変更可能である。例えば、自車が制限速度４０ｋｍ／ｈの車線を走行中であれば、走行速度４０ｋｍ／ｈのときの一般的な停止距離が２０ｍ程度であるため、ｌ_ｆｏｌ＝（ｌ_ｅｇｏ＋ｌ_ｏｂｓ）／２＋２０［ｍ］と設定するとよい。なお、先行車１１は停止していてもよい。

また、本実施の形態では、進入禁止領域Ａ_Ｆに自車１が進入しないようにするために、自車１の重心位置Ｘ_ｃの上限値を￣Ｘ_ｃ（≧０）として、関数ｇを次のように設定する。

そして、各予測点における追従境界ｂの位置Ｘ_ｂ，ｋ（ｋ＝１，…，Ｎ）を用いて、上限値￣Ｘ_ｃ，ｋ＝Ｘ_ｂ，ｋと設定する。

実施の形態３の障害物回避装置２０１によれば、自車の走行車線上に先行車が存在する場合に、自車と先行車との車間距離を一定以上に維持しながら、自車を先行車に追従させることが可能となる。

＜実施の形態４＞
実施の形態４では、自車の走行車線上に先行車が存在する場合に、障害物回避装置２０１の制約生成部２３０が、自車と先行車との車間距離に応じて上限速度の制約を設定する。実施の形態４に係る障害物回避システムの構成および基本的な動作は実施の形態１、２または３と同様である。そのため、ここでは実施の形態１〜３と重複する説明は省略する。

実施の形態４では、上限速度の制約を設定するために、速度Ｖの上限値を￣Ｖ（≧０）として、関数ｇを以下のように設定する。

図１４のように、自車１の前方に障害物としての先行車１１が存在する場合、制約生成部２３０は、自車１が、先行車１１との車間距離をｌ_ｆｏｌ以上に維持しながら先行車１１に追従するように、各予測点における自車１と先行車１１との車間距離ｄ_{ＩＶＤ，ｋ}＝ｘ_{ｏｂｓ，ｋ}−Ｘ_ｃ，ｋ（ｋ＝１，…，Ｎ）に応じて、速度の上限値￣Ｖ_ｋを設定する。図１４に示すグラフは、自車のＸ方向の位置と速度の上限値￣Ｖ_ｋとの関係の一例を示している。

自車１と先行車１１との車間距離ｄ_{ＩＶＤ，ｋ}が十分に大きい場合、自車１は目標速度で走行すればよい。そのため、制約生成部２３０は、十分な車間距離として予め定められた距離ｌ_ｄｅｃを閾値として用い、ｄ_{ＩＶＤ，ｋ}＞ｌ_ｄｅｃのときは￣Ｖ_ｋ＝Ｖ_ｒｅｆと設定する。

また、自車１と先行車１１との車間距離が縮まり、ｄ_{ＩＶＤ，ｋ}≦ｌ_ｆｏｌとなった場合には、車間距離をｌ_ｆｏｌ以上に維持するために、自車１は先行車１１以下の速度で走行する必要がある。そのため、制約生成部２３０は、各予測点における先行車の速度Ｖ_{ｏｂｓ，ｋ}（ｋ＝１，…，Ｎ）を参照して、ｄ_{ＩＶＤ，ｋ}＝ｌ_ｆｏｌのときは￣Ｖ_ｋ＝Ｖ_{ｏｂｓ，ｋ}と設定し、ｄ_{ＩＶＤ，ｋ}＜ｌ_ｆｏｌのときは￣Ｖ_ｋ≦Ｖ_{ｏｂｓ，ｋ}を満たし、￣Ｖ_ｋが連続かつ滑らかになるように設定する。

また、ｌ_ｆｏｌ＜ｄ_{ＩＶＤ，ｋ}≦ｌ_ｄｅｃの場合、制約生成部２３０は、ｄ_{ＩＶＤ，ｋ}が小さくなるほど￣Ｖ_ｋが連続かつ滑らかに単調減少するように設定する。これにより、車間距離ｄ_{ＩＶＤ，ｋ}が距離ｌ_ｄｅｃ以下になった時点で自車１が減速を開始し、車間距離ｄ_{ＩＶＤ，ｋ}＝ｌ_ｆｏｌとなったときに自車１の速度と先行車１１の速度とが同じになるように制御可能となる。

実施の形態３の障害物回避装置２０１によれば、進入禁止領域の形状によらず、先行車との車間距離を一定以上に維持して自車を先行車に追従させることができる。また、先行車が検出されたときに自車と先行車との速度差が大きくても、自車を滑らかに先行車に追従させることができる。

＜実施の形態５＞
図１５は、本発明の実施の形態５に係る障害物回避システムの構成を示すブロック図である。図１５において、図１に示したものと同一の機能を有する要素には、同一符号を付しており、ここではそれらの詳細な説明は省略する。

実施の形態５の車両制御ユニット２００は、障害物回避経路生成装置２０２と、車両状態量推定部２１０と、コントローラ部２８０とから構成されている。車両状態量推定部２１０は、図１に示したものと同様の機能を有する。障害物回避経路生成装置２０２は、図１に示した障害物回避装置２０１から目標値演算部２７０を除いた構成を有しており、自車の走行経路を生成する。コントローラ部２８０は、車両状態量予測部２６０が生成した経路に自車を追従させるように、アクチュエータの目標値を演算する。アクチュエータ制御部３１０は、その目標値を実現するようにアクチュエータを制御する。

以下、実施の形態５の障害物回避システムの動作を説明する。ここでも、実施の形態１〜４と重複する説明は省略する。

本実施の形態では、障害物回避経路生成装置２０２の車両状態量予測部２６０が自車の経路の生成までを行う。経路が滑らかであれば、舵角を滑らかにする必要はない。そこで、本実施の形態では、車両状態量ｘおよび制御入力ｕを次のように設定する。

これにより、実施の形態１の障害物回避装置２０１に比べ、障害物回避経路生成装置２０２の演算負荷を低減できる。なお、ここでは車両制御ユニット２００が操舵制御のみを行うものとする。動的車両モデルとしては次に示す二輪モデルを用いる。

図１６は、実施の形態５に係る障害物回避システムの動作を示すフローチャートである。図１６のステップＳ２８０以外は図４と同じである。

ステップＳ２８０では、コントローラ部２８０が、最適経路（Ｘ_ｃ，ｋ ^＊，Ｙ_ｃ，ｋ ^＊）に追従するためのアクチュエータの目標値を演算する。ここでの目標値の演算には、車線維持用のＰＩＤコントローラ、スライディングモードコントローラ、Ｈ∞コントローラなど、公知の技術を用いることができる。

実施の形態５に係る障害物回避経路生成装置２０２によれば、車線維持のための経路生成と障害物回避のための経路生成とを同一のロジックで実現できる。また、実施の形態１と同様に、自車の進入禁止領域を設定して自車の状態変数に制約を設けることで、自車から障害物までの間隔（回避間隔）の最小値を明確に指定して障害物回避を行うことができる。また、障害物を左右どちらに回避するか事前判断することで、回避までの時間的な余裕を確保でき、回避動作が滑らかになるため、乗り心地が向上する。さらに、障害物の位置に応じて設定された進入禁止領域の左側あるいは右側の領域を無限遠方に設定された進入禁止領域と結合して、最終的な進入禁止領域とすることで、進入禁止領域の形状を単純にでき、演算負荷の低減や、最適解の求解確率の向上といった効果が得られる。さらに、経路追従のためのアクチュエータの目標値の演算には個別のコントローラ（コントローラ部２８０）を用いることができる。

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

１ 自車、２ ステアリングホイール、３ ステアリング軸、４ 操舵ユニット、５ ＥＰＳモータ、６ パワートレインユニット、７ ブレーキユニット、１１ 先行車、１１０ 障害物情報取得部、１１１ 前方カメラ、１１２ レーダセンサ、１２０ 車線情報取得部、１２１ ＧＮＳＳセンサ、１２２ ナビゲーション装置、１３０ 車両情報取得部、１３１ 操舵角センサ、１３２ 操舵トルクセンサ、１３３ ヨーレートセンサ、１３４ 速度センサ、１３５ 加速度センサ、２００ 車両制御ユニット、２０１ 障害物回避装置、２０２ 障害物回避経路生成装置、２１０ 車両状態量推定部、２２０ 障害物移動予測部、２３０ 制約生成部、２４０ 評価関数設定部、２５０ 解演算部、２６０ 車両状態量予測部、２７０ 目標値演算部、２８０ コントローラ部、３１０ アクチュエータ制御部、３１１ ＥＰＳコントローラ、３１２ パワートレインコントローラ、３１３ ブレーキコントローラ。

本発明に係る障害物回避装置は、自車の周辺に存在する障害物の位置の情報を含む障害物情報に基づき、前記障害物の移動予測を行う障害物移動予測部と、前記移動予測の結果に基づき前記障害物を左右どちらに回避するか判断した上で、少なくとも前記自車を前記障害物に接触させないための進入禁止領域を設定することにより、前記自車の状態量または前記自車の少なくとも操舵制御のための制御入力に制約を設定する制約生成部と、少なくとも前記自車の目標経路に対する予測経路の誤差である経路追従誤差を評価項目とし、前記経路追従誤差が小さいほど評価の度合いが良くなる評価関数を設定する評価関数設定部と、前記制約のもとで前記評価関数の評価の度合いが良くなる解を演算する解演算部と、前記解に基づき前記自車の状態量の予測値を演算する車両状態量予測部と、前記解および前記予測された状態量の少なくとも片方に基づき前記自車の操舵量を制御するアクチュエータの目標値を演算する目標値演算部と、を備え、前記制約生成部は、前記自車の走行する車線の左側に位置する左側進入禁止領域と、前記自車が走行する車線の右側に位置する右側進入禁止領域とをさらに設定し、前記障害物を右に回避すると判断した場合には、前記左側進入禁止領域を前記進入禁止領域に結合することで前記進入禁止領域を拡張し、前記障害物を左に回避すると判断した場合には、前記右側進入禁止領域を前記進入禁止領域に結合することで前記進入禁止領域を拡張する。

Claims (29)

1. 自車の周辺に存在する障害物の位置の情報を含む障害物情報に基づき、前記障害物の移動予測を行う障害物移動予測部と、
   前記移動予測の結果に基づき前記障害物を左右どちらに回避するか判断した上で、少なくとも前記自車を前記障害物に接触させないための進入禁止領域を設定することにより、前記自車の状態量または制御入力に制約を設定する制約生成部と、
   少なくとも前記自車の目標経路に対する予測経路の誤差である経路追従誤差を評価項目とし、前記経路追従誤差が小さいほど評価の度合いが良くなる評価関数を設定する評価関数設定部と、
   前記制約のもとで前記評価関数の評価の度合いが良くなる解を演算する解演算部と、
   前記解に基づき前記自車の状態量の予測値を演算する車両状態量予測部と、
   前記解および前記予測された状態量の少なくとも片方に基づき前記自車の操舵量を制御するアクチュエータの目標値を演算する目標値演算部と、
   を備え、
   前記制約生成部は、前記障害物を右に回避すると判断した場合には、前記障害物の左側方の領域を前記進入禁止領域に含ませ、前記障害物を左に回避すると判断した場合には、前記障害物の右側方の領域を進入禁止領域に含ませる、
   障害物回避装置。
2. 前記評価関数の評価項目には、前記自車の目標速度に対する予測速度の誤差である速度追従誤差が含まれており、前記評価関数は前記速度追従誤差が小さいほど評価の度合いが良くなり、
   前記目標値演算部は、前記自車の加減速量を制御するアクチュエータの目標値も演算する、
   請求項１記載の障害物回避装置。
3. 前記制約生成部は、前記障害物が前記自車の走行する車線の中央より左に存在する場合には、前記障害物を右に回避すると判断し、前記障害物が前記自車の走行する車線の中央より右に存在する場合には、前記障害物を左に回避すると判断する、
   請求項１または請求項２に記載の障害物回避装置。
4. 前記制約生成部は、前記障害物の右側に前記自車が走行可能な空間がある場合には、前記障害物を右に回避すると判断する、
   請求項１または請求項２に記載の障害物回避装置。
5. 前記制約生成部は、前記障害物の左側に前記自車が走行可能な空間がある場合には、前記障害物を左に回避すると判断する、
   請求項１または請求項２に記載の障害物回避装置。
6. 前記解演算部は、前記評価関数が予め定められた閾値を下回る、または、上回るような解を出力する、
   請求項１から請求項５のいずれか一項に記載の障害物回避装置。
7. 前記解演算部は、予め定められた回数だけ反復計算を行っても前記評価関数が予め定められた閾値を下回る、または、上回るような解が得られなかった場合、それまでの反復計算で得られた解の中から１つを選択して出力する、
   請求項１から請求項６のいずれか一項に記載の障害物回避装置。
8. 前記制約生成部は、前記自車が走行する車線の区画線より外側の領域を前記進入禁止領域に含ませる、
   請求項１から請求項７のいずれか一項に記載の障害物回避装置。
9. 前記制約生成部は、前記自車が走行する車線上に前記障害物として先行車が存在する場合には、前記自車と前記先行車との車間距離が予め定められた値以下になることを禁止するように前記進入禁止領域を設定し、
   前記目標値演算部は、前記自車の加減速量を制御するアクチュエータの目標値も演算する、
   請求項１から請求項８のいずれか一項に記載の障害物回避装置。
10. 前記制約生成部は、前記自車が走行する車線上に前記障害物として先行車が存在する場合には、前記自車と前記先行車との車間距離に応じて前記自車の上限速度の制約を設定し、前記車間距離が短いほど前記上限速度を小さくし、前記車間距離が予め定められた値以下になると前記上限速度を前記先行車の速度以下にし、
    前記目標値演算部は、前記自車の加減速量を制御するアクチュエータの目標値も演算する、
    請求項１から請求項９のいずれか一項に記載の障害物回避装置。
11. 前記制約生成部はさらに、前記自車の加速度、躍度、操舵速度およびヨーレートのうちの少なくとも１つの上限または下限に制約を設定する、
    請求項１から請求項１０のいずれか一項に記載の障害物回避装置。
12. 前記評価関数の評価項目には、前記自車の加速度、躍度、操舵速度およびヨーレートのうちの少なくとも１つが含まれており、前記評価関数は、前記評価項目に含まれる前記自車の加速度、躍度、操舵速度およびヨーレートが零に近いほど評価の度合いが良くなる、
    請求項１から請求項１１のいずれか一項に記載の障害物回避装置。
13. 前記制約生成部は、前記障害物が複数存在する場合に、それぞれの障害物に対応する複数の前記進入禁止領域を設定し、複数の前記進入禁止領域の全てを含む領域を最終的な進入禁止領域とする、
    請求項１から請求項１２のいずれか一項に記載の障害物回避装置。
14. 前記制約生成部は、道路形状に応じて前記進入禁止領域の形状を変更する、
    請求項１から請求項１３のいずれか一項に記載の障害物回避装置。
15. 前記障害物移動予測部は、前記障害物の位置、速度および方位のうちの少なくとも位置に基づき、前記障害物の移動予測を行う、
    請求項１から請求項１４のいずれか一項に記載の障害物回避装置。
16. 前記障害物移動予測部は、前記進入禁止領域を凸形状に設定する、
    請求項１から請求項１５のいずれか一項に記載の障害物回避装置。
17. 自車の周辺に存在する障害物の位置の情報を含む障害物情報に基づき、前記障害物の移動予測を行う障害物移動予測部と、
    前記移動予測の結果に基づき前記障害物を左右どちらに回避するか判断した上で、少なくとも前記自車を前記障害物に接触させないための進入禁止領域を設定することにより、前記自車の状態量または制御入力に制約を設定する制約生成部と、
    少なくとも前記自車の目標経路に対する予測経路の誤差である経路追従誤差を評価項目とし、前記経路追従誤差が小さいほど評価の度合いが良くなる評価関数を設定する評価関数設定部と、
    前記制約のもとで前記評価関数の評価の度合いが良くなる解を演算する解演算部と、
    前記解に基づき前記自車の経路を含む状態量の予測値を演算する車両状態量予測部と、
    を備え、
    前記制約生成部は、前記障害物を右に回避すると判断した場合には、前記障害物の左側方の領域を前記進入禁止領域に含ませ、前記障害物を左に回避すると判断した場合には、前記障害物の右側方の領域を進入禁止領域に含ませる、
    障害物回避経路生成装置。
18. 前記制約生成部は、前記障害物が前記自車の走行する車線の中央より左に存在する場合には、前記障害物を右に回避すると判断し、前記障害物が前記自車の走行する車線の中央より右に存在する場合には、前記障害物を左に回避すると判断する、
    請求項１７に記載の障害物回避経路生成装置。
19. 前記制約生成部は、前記障害物の右側に前記自車が走行可能な空間がある場合には、前記障害物を右に回避すると判断する、
    請求項１７に記載の障害物回避経路生成装置。
20. 前記制約生成部は、前記障害物の左側に前記自車が走行可能な空間がある場合には、前記障害物を左に回避すると判断する、
    請求項１７に記載の障害物回避経路生成装置。
21. 前記解演算部は、前記評価関数が予め定められた閾値を下回る、または、上回るような解を出力する、
    請求項１７から請求項２０のいずれか一項に記載の障害物回避経路生成装置。
22. 前記解演算部は、予め定められた回数だけ反復計算を行っても前記評価関数が予め定められた閾値を下回る、または、上回るような解が得られなかった場合、それまでの反復計算で得られた解の中から１つを選択して出力する、
    請求項１７から請求項２１のいずれか一項に記載の障害物回避経路生成装置。
23. 前記制約生成部は、前記自車が走行する車線の区画線より外側の領域を前記進入禁止領域に含ませる、
    請求項１７から請求項２２のいずれか一項に記載の障害物回避経路生成装置。
24. 前記制約生成部はさらに、前記自車の加速度、躍度、操舵速度およびヨーレートのうちの少なくとも１つの上限または下限に制約を設定する、
    請求項１７から請求項２３のいずれか一項に記載の障害物回避経路生成装置。
25. 前記評価関数の評価項目には、前記自車の加速度、躍度、操舵速度およびヨーレートのうちの少なくとも１つが含まれており、前記評価関数は、前記評価項目に含まれる前記自車の加速度、躍度、操舵速度およびヨーレートが零に近いほど評価の度合いが良くなる、
    請求項１７から請求項２４のいずれか一項に記載の障害物回避経路生成装置。
26. 前記制約生成部は、前記障害物が複数存在する場合に、それぞれの障害物に対応する複数の前記進入禁止領域を設定し、複数の前記進入禁止領域の全てを含む領域を最終的な進入禁止領域とする、
    請求項１７から請求項２５のいずれか一項に記載の障害物回避経路生成装置。
27. 前記制約生成部は、道路形状に応じて前記進入禁止領域の形状を変更する、
    請求項１７から請求項２６のいずれか一項に記載の障害物回避経路生成装置。
28. 前記障害物移動予測部は、前記障害物の位置、速度および方位のうちの少なくとも位置に基づき、前記障害物の移動予測を行う、
    請求項１７から請求項２７のいずれか一項に記載の障害物回避経路生成装置。
29. 前記障害物移動予測部は、前記進入禁止領域を凸形状に設定する、
    請求項１７から請求項２８のいずれか一項に記載の障害物回避経路生成装置。

Images