JP2017154516A - 衝突判定装置、衝突判定方法、およびプログラム - Google Patents

Abstract

【課題】円弧状の計画経路を走行する車両の衝突判定を精度良く行うことができる衝突判定装置を提供する。【解決手段】衝突判定装置１は、円弧軌跡の中心点を原点とする極座標を設定する極座標設定部２１と、車両周辺の物体の情報からその軌道を算出する物体軌道算出部２３と、車両領域の輪郭を過不足なく覆う極座標の原点を中心とした扇状被覆を設定する扇状被覆設定部２４と、走行軌道に沿って扇状被覆が移動するときに通過する領域を動径占有領域として求める動径占有領域算出部２５および偏角占有領域として求める偏角占有領域算出部２６と、物体の軌道が動径占有領域に侵入するか否かを判定する動径占有領域侵入判定部２７と、物体の軌道が偏角占有領域に侵入するか否かを判定する偏角占有領域侵入判定部２８と、偏角占有領域侵入判定部２８による判定結果に基づいて車両と物体とが衝突するか否かを判定する衝突判定部２９とを備える。【選択図】図１

Description

本発明は、車両の衝突判定装置に関し、特に、車両が旋回し、その走行予定経路が円弧となる場合または円弧で近似される場合における衝突判定装置に関するものである。

従来、車両が計画軌道上を走行した場合に、経路周辺の障害物と衝突するか否かを判定する技術が開発されている。従来の一般的な衝突予測は、図１４に示すように、計画軌道に沿って、車両を仮想的に、逐次進行させた各状態において、車両を示す領域内に障害物が含まれる場合に、障害物と衝突すると判定するものである。なお、本発明に関連する先行技術として、以下の文献がある。

特開平７−１３２７８８号公報

ところが、かかる従来技術では、計画軌道が円弧状である場合、図１４に示すように車両を仮想的に進行させたケースにおいて、実際には障害物と衝突するにもかかわらず、障害物が車両領域内に含まれず、衝突はないと判定されることになる。このような誤判定を防ぐためには、車両の仮想的な進行の幅を細かくすることが考えられる。しかし、進行幅を細かくするほど、判定回数が増え、計算負荷が増大する。

本発明は、上記の問題に鑑みてなされたものであり、計算負荷を増大させることなく、円弧状の計画軌道を走行する車両の衝突判定を精度よく行うことができる衝突判定装置を提供することを目的とする。

本発明の衝突判定装置は、車両が旋回する場合における衝突判定装置であって、前記車両の走行予定軌道に基づいて、前記車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定する極座標設定部と、前記車両周辺の物体を検出するセンサから、車両周辺の物体の位置及び速度を取得する物体情報取得部と、前記物体の位置及び速度に基づいて物体の軌道を算出する物体軌道算出部と、前記車両の形状情報に基づいて衝突判定対象とすべき車両領域を決定し、当該車両領域の輪郭を過不足なく覆う前記極座標の原点を中心とした扇状の領域を扇状被覆として設定する扇状被覆設定部と、前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の動径と時間とを軸とする時間動径座標平面上で動径占有領域として求める動径占有領域算出部と、前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の偏角と時間とを軸とする時間偏角座標平面上で偏角占有領域として求める偏角占有領域算出部と、前記時間動径座標平面において前記物体の軌道が前記動径占有領域に侵入するか否かを判定する動径占有領域侵入判定部と、前記動径占有領域侵入判定部にて前記物体の軌道が前記動径占有領域に侵入すると判定された場合に、前記物体の軌道が前記動径占有領域に侵入する時間区間を求め、当該時間区間に前記時間偏角座標平面において前記物体の軌道が前記偏角占有領域に侵入するか否かを判定する偏角占有領域侵入判定部と、前記偏角占有領域侵入判定部による判定結果に基づいて車両と物体とが衝突するか否かを判定する判定部とを備える。

このように走行予定軌道を動く車両が通る領域を円弧に沿って動く扇状被覆で近似し、極座標系で処理することで判定漏れを防ぐことができる。時間動径座標平面上における扇状被覆の動径占有領域、および、時間偏角座標平面上における扇状被覆の偏角占有領域は簡単な形状で表わされるため、物体の軌跡との判定を容易に行え、計算負荷を増大させることなく衝突判定ができる。

本発明の衝突判定装置において、前記扇状被覆設定部は、前記車両領域の輪郭を分割した所定の区間ごとに前記扇状被覆を求め、前記動径占有領域侵入判定部は、それぞれの前記扇状被覆について、前記物体の軌道が前記動径占有領域に侵入するか否かを判定し、前記偏角占有領域侵入判定部は、それぞれの前記扇状被覆について、前記物体の軌道が前記偏角占有領域に侵入するか否かを判定してもよい。

このように前記車両領域の輪郭を分割した所定の区間ごとに扇状被覆を設定して衝突の判定を行うことにより、衝突判定の精度を高めることができる。

本発明の衝突判定装置において、前記扇状被覆設定部は、前記車両領域を後輪軸の前後で前側の矩形と後側の矩形に分割し、前記各矩形のうち前記車両領域の輪郭を構成する各辺について前記扇状被覆を設定してもよい。

後輪タイヤの横滑りがないと仮定した場合、車両は後輪軸の延長線上の１点を回転中心として旋回するので、後輪軸の前後で分割した前側の矩形と後側の矩形の辺について扇状被覆を設定し、それぞれの扇状被覆で衝突判定を行うことで、適切な判定を行える。

本発明の衝突判定装置において、前記扇状被覆設定部は、前記辺をｎ分割して得られたそれぞれの線分について扇状被覆を求め、前記各扇状被覆の前記辺からの垂直方向の最大距離をｄとしたとき、分割数ｎは、前記辺の両端と前記極座標の原点とがなす角θ_FANと、前記辺の前記極座標の原点に近い方の端と前記原点とをつなぐ直線と前記辺とがなす角φ_FANと、前記辺の前記極座標の原点に近い方の端と前記原点との距離ｒ_sとに基づいて求めてもよい。ここで、前記分割数ｎを次の式（１）により求め、各線分の端点α_i-1の偏角を式（２）により求めもよい。

本発明の衝突判定装置において、前記車両領域は、前記車両の形状に所定のマージンを加えた領域であってもよい。このように所定のマージンを加えた領域との衝突を判定することで、センサの観測誤差や自動運転制御の制御誤差があった場合にも、物体の無衝突を保証できる。

本発明の衝突判定装置は、前記偏角占有領域侵入判定部にて前記物体の軌道が前記偏角占有領域に侵入すると判定された場合、前記物体の軌道が前記動径占有領域に侵入する時間区間を細分化して新たな時間区間を設定して偏角占有領域侵入判定を行い、前記物体の軌道が前記偏角占有領域に侵入すると判定された新たな時間区間のうちで、最先の時間区間を衝突発生時刻として求める衝突発生時刻算出部を備えてもよい。

このように、侵入時間区間を細分化して物体との衝突を判定することで、車両と物体とが衝突する時刻を特定することができる。ここで、前記衝突発生時刻算出部は、衝突が検出された時間区間をさらに細分化して新たな時間区間を設定して偏角占有領域侵入判定を行い、新たな時間区間において前記物体の軌道が前記偏角占有領域に侵入すると判定された時間区間のうちで、最先の時間区間を衝突発生時刻とする処理を繰り返し行ってもよい。

本発明の衝突判定装置は、車両が旋回する場合に前記車両が物体を衝突するか否かを衝突判定装置によって判定する方法であって、前記車両の走行予定軌道に基づいて、前記車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定するステップと、前記車両周辺の物体を検出するセンサから、車両周辺の物体の位置及び速度を取得するステップと、前記物体の位置及び速度に基づいて物体の軌道を算出するステップと、前記車両の形状情報に基づいて衝突判定対象とすべき車両領域を決定し、当該車両領域の輪郭を過不足なく覆う前記極座標の原点を中心とした扇状の領域を扇状被覆として設定するステップと、前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の動径と時間とを軸とする時間動径座標平面上で動径占有領域として求めるステップと、前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の偏角と時間とを軸とする時間偏角座標平面上で偏角占有領域として求めるステップと、前記時間動径座標平面において前記物体の軌道が前記動径占有領域に侵入するか否かを判定するステップと、前記動径占有領域への侵入判定のステップにて前記物体の軌道が前記動径占有領域に侵入すると判定された場合に、前記物体の軌道が前記動径占有領域に侵入する時間区間を求め、当該時間区間に前記時間偏角座標平面において前記物体の軌道が前記偏角占有領域に侵入するか否かを判定するステップと、前記偏角占有領域への侵入判定のステップでの判定結果に基づいて車両と物体とが衝突するか否かを判定するステップとを備える。また、本発明の衝突判定プログラムは、上記の衝突判定方法の各ステップをコンピュータに実行させるプログラムである。

本発明によれば、判定漏れを防ぐことができると共に、計算負荷を増大させることなく、車両周辺の物体と車両との衝突判定を精度良く行うことができる。

本実施の形態の衝突判定装置の構成を示す図である。 車両の運動モデルを示す図である。 車両の衝突判定に用いる車両領域を示す図である。 車両前方矩形と車両後方矩形の扇状被覆を示す図である。 車両前方矩形の扇状被覆が走行予定軌道を移動するときに通る領域を示す図である。 時間動径座標における動径占有領域と物体の移動軌道、および、時間偏角座標における偏角占有領域と物体の移動軌道を示す図である。 XY平面において中心Oから物体の移動軌跡に引いた垂線の距離ｒoと扇状被覆の内径ｒ_SBLと外径ｒ_SFRとの大小関係を示す図である。 車両前方矩形、車両後方矩形のいずれの辺が衝突したかを判定するために設定する扇状被覆の例を示す図である。 矩形の各辺を細分化した線分とし、各線分について扇状被覆を設定する例を示す図である。 衝突判定装置の動作を示すフローチャートである。 衝突時刻を求める処理を示すフローチャートである。 物体と扇状被覆との衝突を判定する処理を示すフローチャートである。 車両が前進反時計周りで、前方左手から５つの障害物が接近するケースの衝突判定結果を示す図である。 従来技術を説明するための図である。

以下、本発明の実施の形態の衝突判定装置について、図面を参照しながら説明する。
（衝突判定装置の構成）
図１は、本発明の実施の形態の衝突判定装置１の構成を示す図である。図１に示すように、本実施の形態の衝突判定装置１は、制御部２０と、経路決定部１０と、センサ１１と、車両形状情報記憶部１２とを備えている。

制御部２０は、極座標設定部２１と、物体情報取得部２２と、物体軌道算出部２３と、扇状被覆設定部２４と、動径占有領域算出部２５と、偏角占有領域算出部２６と、動径占有領域侵入判定部２７と、偏角占有領域侵入判定部２８と、衝突判定部２９と、衝突時刻算出部３０とを備えている。制御部２０は、ハードウェアとしては、電子制御ユニット（ＥＣＵ）によって構成される。このようなＥＣＵを制御するためのプログラムも本発明の範囲に含まれる。センサ１１は、例えば、レーザレーダやカメラであり、車両周辺に存在する物体およびその移動速度を検出し、検出結果を制御部２０に出力する。

衝突判定装置１には、車両の自動運転制御を行うための自動運転制御装置４０が接続されている。自動運転制御装置４０は、衝突判定装置１による衝突判定結果に基づいて、車両の走行軌道を変更したり、ブレーキをかけて車両を停止させたりすることができる。

経路決定部１０は、車両の走行予定軌道を決定し、衝突判定装置１に出力する。経路決定部１０で決定される走行予定軌道の始点及び終点は、車両の後輪軸の中点を基準として設定される。つまり、走行予定軌道は、本実施の形態においては、車両の後輪軸の中点の軌跡である。後輪タイヤの横滑りがないと仮定した場合、ステアリングを切ると、車両は、後輪軸の延長線上の１点を回転中心として、後輪軸中点が円弧を描くように旋回することが知られている（アッカーマンステアリングジオメトリ）。本実施の形態では、車両がかかる旋回をする場合の走行予定軌道を想定しており、したがって、経路決定部１０が出力する経路は、円弧状である。

本実施の形態では、車両は後輪が滑らないものとして車両運動をモデル化する。図２に示すように、後輪軸の前後で、車両を車両前方矩形と車両後方矩形に分け、それぞれを剛体とみなして、走行予定軌道上での衝突判定を行う。

極座標設定部２１は、経路決定部１０にて決定された走行予定軌道が、原点を中心とする円弧となるように、また、走行予定軌道の始点と原点とを結ぶ直線が始線となるように座標系を設定する。具体的には、走行予定軌道の始点からの距離と、終点からの距離が等しくなる位置を原点とし、原点と当該始点とを結ぶ直線をθ＝０とする極座標を設定する。

物体情報取得部２２は、センサ１１から、車両周辺に存在する物体の位置及び移動速度の情報を取得する。物体軌道算出部２３は、物体情報取得部２２にて取得した物体情報に基づいて、予測される物体の軌道を算出する。本実施の形態では、物体が等速直線運動をすることを前提として、物体軌道算出部２３は、物体の位置および移動速度の情報に基づいて物体の軌道を算出する。

扇状被覆設定部２４は、車両の衝突判定に用いる車両の領域を決定した上で、当該車両の領域の輪郭を過不足なく覆う扇状の領域（この領域を「扇状被覆」という）を設定する。

図３は、車両の衝突判定に用いる車両の領域を示す図である。本実施の形態において、車両の衝突判定に用いる領域は、車両の外形に所定のマージンを加えた矩形の形状の領域である。ここで、所定のマージンは、センサ１１の観測誤差に基づくマージンと、運転制御の誤差に基づくマージンを含んでおり、例えば、５ｃｍ〜５０ｃｍ程度である。

車両形状情報記憶部１２は、車両の車幅データ、車両データのうち車両前端から後輪軸中心までの長さデータ、車両後端から後輪軸中心までの長さ等の情報を記憶している。扇状被覆設定部２４は、車両形状情報記憶部１２に記憶された車両の形状に関する情報に基づいて、図３に示すような車両領域を決定した上で、矩形の車両領域を後輪軸の前後で車両前方矩形と車両後方矩形に分割し、それぞれの矩形の輪郭を過不足なく覆う扇状被覆を設定する。

図４は、車両前方矩形の扇状被覆C_FS1と車両後方矩形の扇状被覆C_FS2を示す図である。扇状被覆C_FS1は、極座標の中心Oを中心とする扇形の形状をした領域であり、車両前方矩形の輪郭を過不足なく覆っている。より詳しく述べると、扇状被覆C_FS1は、バームクーヘンを半径方向に切ったような形状であり、円弧状の帯の領域である。帯を構成する外側の円弧は極座標の原点Oから最も遠い頂点を通る円弧であり、内側の円弧は原点Oから最も近い頂点を通る円弧である。これら２つの円弧とこれを挟む半径方向の直線によって扇状被覆C_FS1が構成されている。ここでは、扇状被覆C_FS1について述べたが、扇状被覆C_FS2についても同様である。

次に、動径占有領域算出部２５と偏角占有領域算出部２６について説明するが、その前に、扇状被覆が走行予定軌道を移動するときに扇状被覆が移動する領域について説明する。図５は、車両前方矩形の扇状被覆が走行予定軌道を移動するときに通る領域を示す図である。図５に示すように扇状被覆が円弧状の走行予定軌道を移動すると、最初の時刻ｔ_segStartにおいて扇状被覆は、半径ｒ_SBL〜ｒ_SFR、角度θ_SBL〜θ_SFLの領域を占有している。極座標の原点Oを中心として扇状被覆が円弧状に移動すると、半径ｒ_SBL〜ｒ_SFRを保持したまま、占有角度θが変位していき、時刻ｔ_segFinishにおいて扇状被覆は、半径ｒ_SBL〜ｒ_SFR、角度θ_GBL〜θ_GFLの領域を占有することになる。なお、移動物体は、時刻ｔ_segStartから時刻ｔ_segFinishにかけて、図５に示すような等速直線運動をする。

動径占有領域算出部２５は、図５のように扇状被覆が円弧状の予定走行軌道を移動するときに、動径と時間を軸とした時間動径座標平面上において扇状被覆が通過する領域を算出する。

図６の上のグラフは、動径占有領域（図では「ｒ占有領域」と記載している）を示す図である。扇状被覆が円弧状の走行軌道を移動しても、扇状被覆が占有する半径は半径ｒ_SBL〜ｒ_SFRで変わらないため、時間にかかわらず高さが一定の動径占有領域は矩形上の領域となる。

偏角占有領域算出部２６は、図５のように扇状被覆が円弧状の予定走行軌道を移動するときに、偏角と時間を軸とした時間偏角座標平面上において扇状被覆が通過する領域を算出する。

図６の下のグラフは、偏角占有領域（図では「θ占有領域」と記載している）を示す図である。扇状被覆が円弧状の走行軌道を移動すると、最初にθ_SBL〜θ_SFLを占有していた扇状被覆は、その角度幅を保ったまま角度θが増加していき、最後にθ_GBL〜θ_GFLを占有することになる。なお、図６において上下のグラフの時間軸の値は一致している。

動径占有領域侵入判定部２７は、時間動径座標平面上において、移動物体が動径占有領域に侵入するか否かを判定する。本実施の形態では、移動物体は等速直線運動することを前提としているので、図５に示す物体の移動軌跡は、時間動径座標平面上においては、図６の上のグラフに示すように下に凸の軌跡（以下「ｒ軌跡」という）となる。扇状被覆の動径占有領域は矩形であるので、ｒ軌跡の動径占有領域への侵入時間区間［ｔ_rIN，ｔ_rOUT］を簡単な幾何的計算によって求めることができる。

図７（ａ）〜図７（ｃ）に示すように、XY平面において物体の移動軌跡およびその延長線に対して、予定走行軌道の中心Oから垂線を引き、その距離をｒoとしたとき、距離ｒoと扇状被覆が移動したときに通る領域の内径ｒ_SBLと外径ｒ_SFRとの大小関係から、動径占有領域に侵入する区間があるか否か、及び、あるとした場合に１区間か２区間かを求めることができる。すなわち、図７（ａ）のように、ｒo＞ｒ_SFR＞ｒ_SBLの関係がある場合には、移動軌跡は扇状被覆に侵入しないと分かる。図７（ｂ）のようにｒ_SFR＞ｒo＞ｒ_SBLの関係がある場合には、移動軌跡は扇状被覆に１区間で侵入する。図７（ｃ）のようにｒ_SFR＞ｒ_SBL＞ｒoの関係がある場合には、移動軌跡は扇状被覆に２区間で侵入する。

図７（ａ）に示すように、動径占有領域への侵入がない場合には、動径占有領域侵入判定部２７は、動径占有領域への侵入がないと判定する。図７（ｂ）及び図７（ｃ）に示す関係にある場合、動径占有領域侵入判定部２７は、時間動径座標上において、移動軌跡の動径占有領域への侵入時間区間［ｔ_rIN，ｔ_rOUT］を求める。

なお、図４に示すように車両領域を車両前方矩形と車両後方矩形に分割してそれぞれに対して扇状被覆を設定した場合には、各扇状被覆に対して、動径占有領域侵入判定を行う。また、車両領域をさらに細分化して扇状被覆を設定した場合には、すべての扇状被覆に対して動径占有領域侵入判定を行う。すべての扇状被覆について、その動径占有領域に移動物体のｒ軌跡が侵入しないと判定されれば、移動物体は車両に衝突しないと判定できる。

偏角占有領域侵入判定部２８は、侵入時間区間［ｔ_rIN，ｔ_rOUT］に、移動物体の軌跡（以下「θ軌跡」という）が偏角占有領域に侵入するか否かを判定する。等速直線運動する移動物体の時間偏角座標上における軌跡は、時間に対して単調な関数となる。また、扇状被覆が予定走行軌道を移動するθ軌跡も単調な関数であるため、曲線と曲線の交差判定によって、移動物体が偏角占有領域へ侵入するか否かを判定することができる。なお、偏角占有領域侵入判定部２８も、複数の扇状被覆がある場合には、すべての扇状被覆の偏角占有領域に対して移動物体が侵入しないか否かを判定する。

衝突判定部２９は、動径占有領域侵入判定部２７および偏角占有領域侵入判定部２８での判定結果に基づいて、車両と物体とが衝突するか否かを判定する機能を有する。衝突判定部２９は、移動物体のｒ軌跡が動径占有領域に侵入し、当該侵入時間区間において移動物体のθ軌跡が偏角占有領域にも侵入する場合に、車両と移動物体とが衝突すると判定する。

衝突時刻算出部３０は、衝突判定部２９にて、車両と物体とが衝突すると判定されたときに、車両と物体が衝突する時刻を算出する機能を有する。衝突時刻算出部３０は、動径占有領域へ移動物体が侵入すると判定された侵入時間区間［ｔ_rIN，ｔ_rOUT］を細分化していくことで、侵入時間区間を算出する。

具体的には、侵入時間区間［ｔ_rIN，ｔ_rOUT］を評価区間［ｔ_CHKIN，ｔ_CHKOUT］とし、これを前半評価区間［ｔ_CHKIN，ｔ_CHKM］、後半評価区間［ｔ_CHKM，ｔ_CHKOUT］に分割する。そして、前半評価区間［ｔ_CHKIN，ｔ_CHKM］において、移動物体のθ軌跡が偏角占有領域に侵入するか否か、後半評価区間［ｔ_CHKM，ｔ_CHKOUT］において移動物体のθ軌跡が偏角占有領域に侵入するか否かを判定する。

前半評価区間［ｔ_CHKIN，ｔ_CHKM］において移動物体のθ軌跡が偏角占有領域に侵入すると判定された場合には、前半評価区間［ｔ_CHKIN，ｔ_CHKM］を新たな前半評価区間［ｔ_CHKIN，ｔ_CHKM］、新たな後半評価区間［ｔ_CHKM，ｔ_CHKOUT］に分割して、それぞれの評価区間において移動物体のθ軌跡が偏角占有領域に侵入するか否かを判定する。前半評価区間［ｔ_CHKIN，ｔ_CHKM］において偏角占有領域への侵入がない場合には、後半評価区間［ｔ_CHKM，ｔ_CHKOUT］において偏角占有領域への侵入があると考えられるので、後半評価区間［ｔ_CHKM，ｔ_CHKOUT］を新たな前半評価区間［ｔ_CHKIN，ｔ_CHKM］、新たな後半評価区間［ｔ_CHKM，ｔ_CHKOUT］に分割して、それぞれの評価区間において移動物体のθ軌跡が偏角占有領域に侵入するか否かを判定する。

以下、同様に、評価区間が所定の大きさになるまで分割を繰り返していき、移動物体のθ軌跡が偏角占有領域に侵入する最先の評価区間を求め、当該評価区間の開始時刻を衝突時刻として算出する。

（判定精度の向上）
図４に示すように車両領域を前後に分割して、車両前方矩形、車両後方矩形を過不足なく覆う扇状被覆は、車両領域よりも相当に大きいため、実際には物体が衝突しない場合にも、物体に衝突すると誤って判断する可能性がある。車両領域に対して扇状被覆の方が大きいのは、安全サイドで判定していることに他ならないので、車両が物体に衝突しないことを保証するという意味では、衝突判定装置としての十分に目的を達している。

しかし、実際の走行において、回避すべき物体は一つではなく、例えば、対向車、並走するバイク、道路を横断する歩行者等、複数ある。複数の物体に対して、大ざっぱな衝突判定をしてしまうと走行可能な経路を引くことができなくなってしまうので、より精度の高い判定が求められる。

そこで、本実施の形態では、車両領域の輪郭を細分化して、各輪郭を覆う小さい扇状被覆を設定し、それぞれの扇状被覆が円弧状に移動したときに物体と衝突するかを判定する。なお、センサ１１によって複数の物体が検出された場合には、複数の物体と複数の扇状被覆とが衝突するか否かを判定する。

本実施の形態の衝突判定装置１は、図４に示す車両前方矩形の扇状被覆C_FS1と車両後方矩形の扇状被覆C_FS2を用いて衝突判定を行った結果、いずれかの扇状被覆が物体と衝突すると判定された場合、当該矩形のいずれの辺が衝突するか否かを判定する。続いて、衝突した辺を細分化して複数の線分とし、各線分に対して扇状被覆を設定して衝突判定を行う。

図８は、車両前方矩形、車両後方矩形のいずれの辺が衝突するかを判定するために設定する扇状被覆の例を示す図である。例えば、車両前方矩形の扇状被覆C_FS1が物体と衝突すると判定された場合、車両前方矩形の前方の辺と側方の２辺の３つの辺のそれぞれについて、その辺を過不足なく覆う扇状被覆を設定する。

図８に示す例では、車両前方矩形の前方の辺に対しては扇状被覆C_FS1-1が設定され、左側方の辺については扇状被覆C_FS1-2が設定され、右側方の辺については扇状被覆C_FS1-3が設定されている。このように辺に対して扇状被覆を設定しているのは、物体と車両とが衝突するときには、最初に車両の外形と衝突するので、辺との衝突を考えればよいからである。

衝突判定装置１は、このように設定した扇状被覆のそれぞれについて、上記したのと同様の要領で、動径占有領域および偏角占有領域を求め、動径占有領域侵入判定および偏角占有領域判定を行って、物体との衝突判定を行う。これにより、どの辺において衝突が発生するかを判定することができる。

図９は、矩形の各辺を細分化した線分とし、各線分について扇状被覆を設定する例を示す図である。図９では、すべての辺について扇状被覆を設定しているが、実際には、図８を用いて説明した判定によって衝突があると判定された辺について細分化した扇状被覆を設定すればよい。このとき各辺を細分化すればするほど衝突判定の精度は高くなるが、その分だけ計算の負荷が大きくなる。したがって、適切な分割数を決定することが好ましい。

本実施の形態における分割数の決定について、図９を参照しながら説明する。図９では、車両前方矩形の前方の辺を分割するときの分割数の計算例を示している。分割対象の辺の両端と極座標の原点Oを結ぶ２つの直線がなす角をθ_FANとし、極座標の原点Ｏに近い方の辺の端と原点Ｏとをつなぐ直線と辺とがなす角をφ_FANとし、極座標の原点Ｏに近い方の辺の端と原点Ｏとの距離ｒ_sと定義する。設定すべき扇状被覆の辺から垂直方向の最大距離ｄ、すなわち辺から扇状被覆がどれだけはみ出すかを示す量ｄ（以下、「余長ｄ」という）を与えたとき、扇状被覆設定部２４は、分割数ｎを、以下の式（１）によって求め、各線分の端点α_i-1の偏角を式（２）により求める。

この計算式により決定された分割数ｎは、余長ｄを満たす最小の分割数となる。衝突判定の計算量は分割数に比例するため、上記計算式で最小の分割数を決定することで、冗長な分割をなくし、衝突判定の計算量を最小限に抑えることができる。

（衝突判定の処理）
図１０は、衝突判定装置１の動作を示すフローチャートである。衝突判定装置１は、車両が曲がるときに、車両が旋回する走行予定軌道を決定し、その走行予定軌道で周辺の物体と衝突するか否かを判定する。衝突判定装置１は、まず、走行予定軌道に基づいて、車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定する（Ｓ１０）。

次に、衝突判定装置１は、センサ１１から車両周辺の物体の位置及び速度を取得する物体情報を取得し（Ｓ１１）、物体情報に基づいて、予測される物体の軌道を算出する（Ｓ１２）。本実施の形態では、物体が等速直線運動をするものと仮定して物体の軌道を計算する。

続いて、衝突判定装置１は、車両の外形に所定のマージンを加えた矩形の車両領域を設定し、車両領域を後輪軸の前後に分割して得られた車両前方矩形と車両後方矩形のそれぞれに、図４に示す扇状被覆を設定する（Ｓ１３）。

次に、衝突判定装置１は、前後の扇状被覆のそれぞれについて、扇状被覆が走行予定軌道を移動したときに、扇状被覆と物体が衝突するか否かを判定する（Ｓ１４）。ここでの衝突判定処理については、図１２を参照して後述する。

図４に示すような扇状被覆を用いて行った衝突判定において衝突なしと判定された場合には（Ｓ１５でＹＥＳ）、車両と物体とは衝突しないと判定し（Ｓ２０）、処理を終了する。ここで、衝突ありと判定された場合には、判定精度を高めて衝突の有無を判定する。このように図４に示す前方車両矩形と後方車両矩形の２つの扇状被覆を用いて行った衝突判定は、衝突するという判定結果については最終的な判定結果ではないので、衝突１次判定である。

衝突判定装置１は、車両前方矩形と車両後方矩形のうち、衝突のあった矩形の辺の中から衝突のあった辺を特定する（Ｓ１６）。ここでの辺の特定処理は、衝突判定処理（Ｓ１４）と同じであり、図１２を参照して後述する。衝突のあった辺を特定すると、その辺を細分化して線分とし、各線分に対して扇状被覆を再設定する（Ｓ１７）。そして、各扇状被覆が走行予定軌道を移動したときに物体と衝突するか否かの衝突判定を行う（Ｓ１８）。ここでの衝突判定処理については、図１２を参照して後述する。

細分化された扇状被覆のいずれについても衝突がないと判定された場合には（Ｓ１９でＹＥＳ）、車両と物体とは衝突しないと判定し（Ｓ２０）、いずれかの扇状被覆で衝突ありと判定された場合は（Ｓ１９でＮＯ）、車両と物体は衝突すると判定する（Ｓ２１）。

図１１は、車両と物体が衝突すると判定された場合に、衝突時刻を求める処理を示すフローチャートである。衝突判定装置１は、衝突があった扇状被覆について、物体の軌道が動径占有領域に侵入する侵入時間区間をすでに特定している（後述する図１２のＳ４５）。衝突判定装置１は、この侵入時間区間のうちのどこで衝突が発生したかを求める。衝突判定装置１は、侵入時間区間を衝突時刻の評価区間とする。

まず、衝突判定装置１は、評価区間を前半と後半の評価区間に２分割し（Ｓ３０）、分割した前半及び後半の各評価区間で、偏角占有領域に物体の軌道（θ軌跡）が侵入したか否かを判定する（Ｓ３１）。前半の評価区間で侵入があったと判定された場合には（Ｓ３２でＹＥＳ）、前半の評価区間を新しい評価区間とし（Ｓ３３）、前半の評価区間で侵入がなかったと判定された場合には（Ｓ３２でＮＯ）、後半の評価区間を新しい評価区間とする（Ｓ３４）。

次に、衝突判定装置１は、新たな評価区間が閾値以下であるか否かを判定する（Ｓ３５）。この判定は、衝突時刻を特定できる程度に評価区間が十分に小さいか否かを判断するためのステップである。この判定の結果、新たな評価区間が閾値以下である場合には（Ｓ３５でＹＥＳ）、新たな評価区間の開始時刻を衝突時間と判定する（Ｓ３６）。新たな評価区間が閾値以下でない場合には（Ｓ３５でＮＯ）、評価区間をさらに２分割して（Ｓ３０）、前半と後半のいずれで偏角占有領域への侵入があったかを判定し、新たな評価区間が閾値以下となったかどうかの判定をする処理を繰り返し行う（Ｓ３１〜Ｓ３５）。

図１２は、扇状被覆が走行予定軌道を移動するときに、物体の軌道が扇状被覆と衝突するか否かを判定する処理を示すフローチャートである。図１２では、複数の扇状被覆と物体との衝突があるか否かを判定する例を挙げている。

衝突判定装置１は、まず、複数の扇状被覆の中から衝突判定を行う扇状被覆を選択する（Ｓ４０）。次に、衝突判定装置１は、選択した扇状被覆について、その扇状被覆が走行予定軌道を移動するときの動径占有領域を求めると共に（Ｓ４１）、偏角占有領域を求める（Ｓ４２）。

続いて、衝突判定装置１は、物体の軌道が時間動径座標平面上において動径占有領域に侵入するか否かを判定し、侵入なしと判定された場合（Ｓ４３でＹＥＳ）、その扇状被覆と物体とは衝突しないと判定する（Ｓ４４）。物体の軌道（ｒ軌跡）が動径占有領域に侵入すると判定された場合（Ｓ４３でＮＯ）、その侵入時間区間を特定する（Ｓ４５）。

次に、衝突判定装置１は、侵入時間区間において、物体の軌道（θ軌跡）が時間偏角座標平面上で偏角占有領域に侵入するか否かを判定し、侵入なしと判定された場合（Ｓ４６でＹＥＳ）、その扇状被覆と物体とは衝突しないと判定する（Ｓ４４）。物体の軌道が偏角占有領域に侵入すると判定された場合（Ｓ４６でＮＯ）、その扇状被覆と物体とは衝突すると判定する（Ｓ４７）。

次に、衝突判定装置１は、すべての扇状被覆について衝突判定を行ったか否かを判定し、すべての扇状被覆について衝突判定を行ったと判定された場合には（Ｓ４８でＹＥＳ）、衝突判定処理を終了し、判定結果を返す。また、未判定の扇状被覆があると判定された場合には（Ｓ４８でＮＯ）、未判定の扇状被覆を選択し（Ｓ４０）、上記と同様にして扇状被覆と物体との衝突判定を行う（Ｓ４１〜Ｓ４７）。

以上、本実施の形態の衝突判定装置１の構成および動作について説明した。本実施の形態の衝突判定装置１は、走行予定軌道上を移動する車両が通る領域を円弧に沿って動く扇状被覆で近似し、極座標系で処理することで判定漏れを防ぐことができる。

また、本実施の形態の衝突判定装置１は、扇状被覆を細分化することで衝突判定の精度を高めることができると共に、細分化した扇状被覆の設定を行うときの分割数を必要最小限に抑えることにより、計算の負荷を抑えることができる。

図１３（ａ）及び図１３（ｂ）は、車両が前進反時計周りで、前方左手から５つの障害物が接近するケースの衝突判定結果を示す。実線の３本の線が衝突する物体の軌跡、点線の２本の線が衝突しない物体の軌跡を示す。

本発明は、計算負荷を増大させることなく、円弧状の計画経路を走行する車両の衝突判定を精度よく行うことができるという効果を有し、衝突判定装置等として有用である。

１ 衝突判定装置
１０ 経路決定部
１１ センサ
１２ 車両形状情報記憶部
２０ 制御部
２１ 極座標設定部
２２ 物体情報取得部
２３ 物体軌道算出部
２４ 扇状被覆設定部
２５ 動径占有領域算出部
２６ 偏角占有領域算出部
２７ 動径占有領域侵入判定部
２８ 偏角占有領域侵入判定部
２９ 衝突判定部
３０ 衝突時刻算出部
４０ 自動運転制御装置

Claims (10)

1. 車両が旋回する場合における衝突判定装置であって、
   前記車両の走行予定軌道に基づいて、前記車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定する極座標設定部と、
   前記車両周辺の物体を検出するセンサから、車両周辺の物体の位置及び速度を取得する物体情報取得部と、
   前記物体の位置及び速度に基づいて物体の軌道を算出する物体軌道算出部と、
   前記車両の形状情報に基づいて衝突判定対象とすべき車両領域を決定し、当該車両領域の輪郭を過不足なく覆う前記極座標の原点を中心とした扇状の領域を扇状被覆として設定する扇状被覆設定部と、
   前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の動径と時間とを軸とする時間動径座標平面上で動径占有領域として求める動径占有領域算出部と、
   前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の偏角と時間とを軸とする時間偏角座標平面上で偏角占有領域として求める偏角占有領域算出部と、
   前記時間動径座標平面において前記物体の軌道が前記動径占有領域に侵入するか否かを判定する動径占有領域侵入判定部と、
   前記動径占有領域侵入判定部にて前記物体の軌道が前記動径占有領域に侵入すると判定された場合に、前記物体の軌道が前記動径占有領域に侵入する時間区間を求め、当該時間区間に前記時間偏角座標平面において前記物体の軌道が前記偏角占有領域に侵入するか否かを判定する偏角占有領域侵入判定部と、
   前記偏角占有領域侵入判定部による判定結果に基づいて車両と物体とが衝突するか否かを判定する判定部と、
   を備える衝突判定装置。
2. 前記扇状被覆設定部は、前記車両領域の輪郭を分割した所定の区間ごとに前記扇状被覆を求め、
   前記動径占有領域侵入判定部は、それぞれの前記扇状被覆について、前記物体の軌道が前記動径占有領域に侵入するか否かを判定し、
   前記偏角占有領域侵入判定部は、それぞれの前記扇状被覆について、前記物体の軌道が前記偏角占有領域に侵入するか否かを判定する請求項１に記載の衝突判定装置。
3. 前記扇状被覆設定部は、前記車両領域を後輪軸の前後で前側の矩形と後側の矩形に分割し、前記各矩形のうち前記車両領域の輪郭を構成する各辺について前記扇状被覆を設定する請求項２に記載の衝突判定装置。
4. 前記扇状被覆設定部は、前記辺をｎ分割して得られたそれぞれの線分について扇状被覆を求め、
   前記各扇状被覆の前記辺からの垂直方向の最大距離をｄとしたとき、分割数ｎは、前記辺の両端と前記極座標の原点とがなす角θ_FANと、前記辺の前記極座標の原点に近い方の端と前記原点とをつなぐ直線と前記辺とがなす角φ_FANと、前記辺の前記極座標の原点に近い方の端と前記原点との距離ｒ_sとに基づいて求める請求項３に記載の衝突判定装置。
5. 前記分割数ｎを次の式（１）により求め、各線分の端点α_i-1の偏角を式（２）により求める請求項４に記載の衝突判定装置。
   
6. 前記車両領域は、前記車両の形状に所定のマージンを加えた領域である請求項１乃至５のいずれかに記載の衝突判定装置。
7. 前記偏角占有領域侵入判定部にて前記物体の軌道が前記偏角占有領域に侵入すると判定された場合、前記物体の軌道が前記動径占有領域に侵入する時間区間を細分化して新たな時間区間を設定して偏角占有領域侵入判定を行い、前記物体の軌道が前記偏角占有領域に侵入すると判定された新たな時間区間のうちで、最先の時間区間を衝突発生時刻として求める衝突発生時刻算出部を備える請求項１乃至６のいずれかに記載の衝突判定装置。
8. 前記衝突発生時刻算出部は、衝突が検出された時間区間をさらに細分化して新たな時間区間を設定して偏角占有領域侵入判定を行い、新たな時間区間において前記物体の軌道が前記偏角占有領域に侵入すると判定された時間区間のうちで、最先の時間区間を衝突発生時刻とする処理を繰り返し行う請求項７に記載の衝突判定装置。
9. 車両が旋回する場合に前記車両が物体を衝突するか否かを衝突判定装置によって判定する方法であって、
   前記車両の走行予定軌道に基づいて、前記車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定するステップと、
   前記車両周辺の物体を検出するセンサから、車両周辺の物体の位置及び速度を取得するステップと、
   前記物体の位置及び速度に基づいて物体の軌道を算出するステップと、
   前記車両の形状情報に基づいて衝突判定対象とすべき車両領域を決定し、当該車両領域の輪郭を過不足なく覆う前記極座標の原点を中心とした扇状の領域を扇状被覆として設定するステップと、
   前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の動径と時間とを軸とする時間動径座標平面上で動径占有領域として求めるステップと、
   前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の偏角と時間とを軸とする時間偏角座標平面上で偏角占有領域として求めるステップと、
   前記時間動径座標平面において前記物体の軌道が前記動径占有領域に侵入するか否かを判定するステップと、
   前記動径占有領域への侵入判定のステップにて前記物体の軌道が前記動径占有領域に侵入すると判定された場合に、前記物体の軌道が前記動径占有領域に侵入する時間区間を求め、当該時間区間に前記時間偏角座標平面において前記物体の軌道が前記偏角占有領域に侵入するか否かを判定するステップと、
   前記偏角占有領域への侵入判定のステップでの判定結果に基づいて車両と物体とが衝突するか否かを判定するステップと、
   を備える衝突判定方法。
10. 車両が旋回する場合に前記車両が物体を衝突するか否かを判定するためのプログラムであって、コンピュータに、
    前記車両の走行予定軌道に基づいて、前記車両の後輪軸中点の円弧軌跡の中心点を原点とする極座標を設定するステップと、
    前記車両周辺の物体を検出するセンサから、車両周辺の物体の位置及び速度を取得するステップと、
    前記物体の位置及び速度に基づいて物体の軌道を算出するステップと、
    前記車両の形状情報に基づいて衝突判定対象とすべき車両領域を決定し、当該車両領域の輪郭を過不足なく覆う前記極座標の原点を中心とした扇状の領域を扇状被覆として設定するステップと、
    前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の動径と時間とを軸とする時間動径座標平面上で動径占有領域として求めるステップと、
    前記走行軌道に沿って前記扇状被覆が移動するときに前記扇状被覆が通過する領域を、前記極座標の偏角と時間とを軸とする時間偏角座標平面上で偏角占有領域として求めるステップと、
    前記時間動径座標平面において前記物体の軌道が前記動径占有領域に侵入するか否かを判定するステップと、
    前記動径占有領域への侵入判定のステップにて前記物体の軌道が前記動径占有領域に侵入すると判定された場合に、前記物体の軌道が前記動径占有領域に侵入する時間区間を求め、当該時間区間に前記時間偏角座標平面において前記物体の軌道が前記偏角占有領域に侵入するか否かを判定するステップと、
    前記偏角占有領域への侵入判定のステップでの判定結果に基づいて車両と物体とが衝突するか否かを判定するステップと、
    を実行させるプログラム。