JP6669001B2 - 走行軌跡推定方法、及び走行軌跡推定装置 - Google Patents

Description

本発明は、走行軌跡推定方法、及び走行軌跡推定装置に関する。

車両周辺の他車両の走行軌跡をする技術として、例えば特許文献１に記載の技術が知られている。
特許文献１に記載の運転支援装置は、車両が備える相対位置検出手段により他車両の相対位置を検出し、検出した相対位置から他車両の走行軌跡を算出する。

特開２００４−３２２９１６号公報

車両と他車両との距離が長くなると、この車両に搭載されて車両周辺の物体の位置を検出するセンサにより他車両を走査する走査点が減少する。このため、他車両の検出位置の誤差が増大する。大きな誤差を含んだ検出位置から他車両の走行軌跡を算出すると、算出した走行軌跡が不安定に蛇行するおそれがある。
本発明は、車両に搭載されて車両周辺の物体の位置を検出するセンサによる検出位置から算出した他車両の走行軌跡が、車両と他車両との距離が長くなることで実際の走行軌跡よりも蛇行するのを抑制することを目的とする。

本発明の一態様に係る走行軌跡推定方法では、車両に搭載されこの車両周辺の物体の位置を検出するセンサにより他車両の位置を検出し、他車両の検出位置の時系列を記憶する。そして、車両と他車両との距離が長いほど大きな閾値を設定する。他車両の車幅方向位置の偏差が閾値以上の場合には検出位置の時系列を曲線近似で補間することにより他車両の走行軌跡を推定する。偏差が閾値未満の場合には検出位置の時系列を直線近似で補間することにより走行軌跡を推定する。

本発明によれば、車両に搭載されて車両周辺の物体の位置を検出するセンサによる検出位置から算出した他車両の走行軌跡が、車両と他車両との距離が長くなることで実際の走行軌跡よりも蛇行するのを抑制できる。

実施形態に係る走行軌跡推定装置を備える車両の構成例を示す図である。 位置測定センサによる周辺物体の位置の測定の説明図である。 比較的近距離の他車両の走査点を説明する概略図である。 比較的近距離の他車両の検出位置を説明する概略図である。 比較的遠距離の他車両の走査点を説明する概略図である。 比較的遠距離の他車両の検出位置を説明する概略図である。 実施形態に係る走行軌跡推定装置の機能構成の一例を示す図である。 他車両の検出位置、及び車両と他車両との距離の説明図である。 検出位置の時系列の分割方法を説明する概略図である。 実施形態に係る走行軌跡推定方法のフローチャートである。 再分割処理の一例のフローチャートである。 車両と他車両との間の車幅方向位置の差分の説明図である。 車両と他車両との間の車幅方向位置の差分が比較的大きい場合の走査点を説明する概略図である。

以下、本発明の実施形態について、図面を参照しつつ説明する。
（第１実施形態）
（構成）
図１を参照する。車両１は、車輪２と、車輪速センサ３と、ステアリングホイール４と、操舵角センサ５と、位置測定センサ６と、ヨーレートセンサ７と、コントローラ１０と、運転支援システム１１を備える。

車輪速センサ３は、車輪速を検出し、検出した車輪速の情報である車輪速情報をコントローラ１０に出力する。車輪速センサ３は、例えば車輪速パルスを生成するロータリエンコーダ等のパルス発生器を備えてよい。
操舵角センサ５は、ステアリングホイール４の操舵角を検出し、検出した操舵角の情報である操舵角情報をコントローラ１０へ出力する。操舵角センサ５は、ステアリング軸などに設けられる。操舵角センサ５に代えて、操向輪の転舵角を検出する転舵角センサを設け、転舵角の情報である転舵角情報を操舵角情報に代えてコントローラ１０へ入力してもよい。

位置測定センサ６は、車両１周囲の物体の表面を走査して車両１から物体表面上の走査点までの距離を測定し且つ走査点までの方向を取得することにより、車両１に対する走査点の相対位置を測定する。位置測定センサ６は、例えば、車両１に搭載されたレーザレンジファインダ、レーザレーダ又はカメラであってよい。
図２を参照する。図２に示すｘ軸方向及びｙ軸方向は、それぞれ車両１の前後方向及び車幅方向である。位置測定センサ６が車両１に搭載される場合、位置測定センサ６は、車両１の周辺の物体２０の表面上の走査点２１の相対位置を、例えば、車両１の位置を原点とする相対座標系における走査点２１までの距離ｒ１と方位角（すなわち水平方向角度）θ１によって定まる位置として検出してよい。

位置測定センサ６により検出された相対位置の情報を以下の説明において「検出位置情報」と表記することがある。また、車両１に対する相対位置を「相対位置」と表記することがある。位置測定センサ６は、検出位置情報をコントローラ１０へ出力する。
図１を参照する。ヨーレートセンサ７は、車両１のヨーレート（車体が旋回する方向への回転角の変化速度）を検出し、検出したヨーレートの情報であるヨーレート情報をコントローラ１０へ出力する。

コントローラ１０は、プロセッサ１２と、記憶装置１３等のＣＰＵ周辺部品とを含む電子制御ユニットである。プロセッサ１２は、例えばＣＰＵ（Central Processing Unit）、やＭＰＵ（Micro-Processing Unit）であってよい。記憶装置１３は、ＲＯＭ（Read Only Memory）及びＲＡＭ（Random Access Memory）等のメモリを備える。記憶装置１３は、不揮発性メモリ又はハードディスクドライブ装置を含んでもよい。
コントローラ１０は、少なくとも車輪速情報とヨーレート情報に基づいて車両１の移動量を算出する。コントローラ１０は、車両１の周囲の他車両を位置測定センサ６により走査して得た走査点の検出位置情報に基づいて、他車両の相対位置を測定する。
位置測定センサ６の検出位置情報に基づいて測定された他車両の相対位置を、以下の説明において「検出位置」と表記することがある。コントローラ１０は、他車両の検出位置と車両１の移動量と基づいて、他車両の走行軌跡を推定する。コントローラ１０は、推定した他車両の走行軌跡を運転支援システム１１に出力する。
コントローラ１０は、特許請求の範囲に記載の推定回路の一例である。

運転支援システム１１は、コントローラ１０と同様に、プロセッサと、記憶装置等のＣＰＵ周辺部品とを含む電子制御ユニットである。
運転支援システム１１は、コントローラ１０から入力して他車両２２の走行軌跡に基づいて、運転者による車両１の運転に対する運転支援を行う。
運転支援の一例は、車両１の制動制御、加速制御及び操舵制御の少なくとも１つを含む車両１の走行状態の制御であってもよい。例えば運転支援システム１１は、車両１が他車両の走行軌跡に沿って他車両に追従するように、車両１の制駆動及び操舵を制御してよい。例えば運転支援システム１１は、他車両２２の走行軌跡に応じて、車両１の制動制御及び操舵制御のいずれか一方を選択して実行して、他車両２２との衝突を回避してもよい。

図３を参照する。参照符号２２は車両１の周辺の他車両を示す。図中の範囲２３内の円形のプロットは、位置測定センサ６で他車両２２を走査したときの走査点を示す。
位置測定センサ６は、水平方向角度の角度分解能を有する。このため、車両１と他車両２２との距離が短いほど他車両２２の表面の走査点は多くなる。
この結果、車両１と他車両２２との距離が比較的短い場合には、より多くの走査点の相対位置から他車両２２の検出位置を算出することができるので、検出位置の誤差が小さくなる。
図４を参照する。四角形のプロットＰＡ_ｉ（ｉ＝１〜１０）は、それぞれ時刻ｔ_ｉにおける他車両２２の検出位置を示す。車両１と他車両２２との距離が比較的短い場合には、検出位置ＰＡ_ｉの誤差に起因する検出位置ＰＡ_ｉの車幅方向位置の偏差を小さく抑えることができる。

一方で、車両１と他車両２２との距離が長いほど他車両２２の表面の走査点は少なくなる。図５を参照する。図中の範囲２４内の円形のプロットは、位置測定センサ６で他車両２２を走査したときの走査点を示す。車両１と他車両２２との距離が比較的長いことにより、範囲２４内の走査点は、車両１と他車両２２との距離が比較的短い場合の範囲２３内の走査点よりも少ない。
この結果、車両１と他車両２２との距離が比較的長い場合には、他車両２２の検出位置の算出に使用する走査点が減るので検出位置の誤差が大きくなる。
図６を参照する。車両１と他車両２２との距離が比較的長い場合には、検出位置ＰＡ_ｉの誤差に起因する検出位置ＰＡ_ｉの車幅方向位置の偏差が増大する。

このため、誤差が大きな検出位置ＰＡ_ｉから他車両２２の走行軌跡を算出すると、複雑な曲線で近似しなければ近似誤差が小さくならないため、算出した走行軌跡の形状が複雑になり不安定に蛇行するおそれがある。
さらに、精度の低い検出位置ＰＡ_ｉに基づいて高次多項式で走行軌跡を近似すると振動が発生して、低次多項式で近似するよりも劣った近似結果となることがある。

そこで、実施形態に係る走行軌跡推定装置は、他車両２２の走行軌跡を近似により推定する際に、車両１と他車両２２との距離が長いほどより単純な近似線で近似され易くすることにより、走行軌跡の近似結果が不安定に蛇行するのを抑制する。
例えば、走行軌跡推定装置は、車両１と他車両２２との距離が長いほど近似誤差の許容閾値を大きく設定することにより、単純な近似線でも許容閾値の要求を満たし易くする。

図７を参照する。走行軌跡推定装置３０は、他車両認識部３１と、評価部３２と、移動量算出部３３と、検出位置蓄積部３４と、走行軌跡推定部３５とを備える。
コントローラ１０のプロセッサ１２は、記憶装置１３に記憶されたコンピュータプログラムを実行することにより、他車両認識部３１、評価部３２、移動量算出部３３、検出位置蓄積部３４、及び走行軌跡推定部３５の機能を実現する。

図８を参照する。他車両認識部３１は、位置測定センサ６からの検出位置情報に基づいてある時刻ｔ_ｎにおける他車両２２の検出位置ＰＡ_ｎ＝（ＸＡ_ｎ，ＹＡ_ｎ）を認識する。他車両認識部３１は、認識結果を示す他車両情報を生成する。
評価部３２は、他車両認識部３１の認識結果毎に、認識状態の評価値Ｅｖ_ｎを決定する。評価値Ｅｖ_ｎは、車両１と他車両２２との間の距離が長くなるほど増加する値である。評価値Ｅｖ_ｎは、例えば、車両１と他車両２２との間の距離ＬＡ_ｎ＝√（ＸＡ_ｎ ^２＋ＹＡ_ｎ ^２）そのものであってよい。評価部３２は、検出位置ＰＡ_ｎの情報を含む他車両情報に評価値Ｅｖ_ｎを付加する。

移動量算出部３３は、車輪速センサ３からの車輪速情報とヨーレートセンサ７からのヨーレート情報とに基づいて、車両１の移動量を算出する。車両１の移動量は、例えば、車両１の並進運動量ΔＸ及びΔＹと、回転量Δφを含んでよい。
検出位置蓄積部３４は、他車両認識部３１により生成され評価部３２により評価値Ｅｖ_ｎが付加された他車両情報を、記憶装置１３に蓄積する。すなわち、検出位置蓄積部３４は、検出位置ＰＡ_ｎの時系列を記憶装置１３に記憶する。

具体的には、検出位置蓄積部３４は、今回検出した検出位置ＰＡ_ｎの他車両情報を記憶装置１３に記憶する。また、検出位置蓄積部３４は、記憶装置１３に記憶された前回検出した検出位置ＰＡ_ｎ−１までの検出位置ＰＡ_ｉ（ｉ＝１〜ｎ−１）を、車両１の移動量（ΔＸ，ΔＹ，Δφ）に基づいて更新する。

走行軌跡推定部３５は、記憶装置１３に記憶された検出位置ＰＡ_ｉ（ｉ＝１〜ｎ）の時系列を補完することにより、他車両２２の走行軌跡を推定する。
このとき走行軌跡推定部３５は、他車両情報に付加された評価値Ｅｖ_ｎに基づき、評価値Ｅｖ_ｎの代表値Ｅｖを決定する。代表値Ｅｖは、例えば平均値、最大値、又は中間値等の統計学で用いられる代表値であってよい。

走行軌跡推定部３５は、代表値Ｅｖに基づき近似誤差の許容閾値γを算出する。許容閾値γは、代表値Ｅｖが大きくなるほど増加する値である。すなわち許容閾値γは、車両１と他車両２２との間の距離が長くなるほど増加する値となる。例えば、位置測定センサ６の水平方向角度の角度分解能がｄθである場合、許容閾値γはγ＝Ｅｖ×ｓｉｎｄθであってよい。

次に、走行軌跡推定部３５は、検出位置ＰＡ_ｉの時系列を補間する近似線を表す近似式ｙ＝ｆ（ｘ）を算出する。
走行軌跡推定部３５は、まず直線近似により検出位置ＰＡ_ｉの時系列を補完する近似式ｙ＝ｆ（ｘ）を算出する。例えば、近似式ｙ＝ｆ（ｘ）が多項式である場合、関数ｆ（ｘ）は一次多項式となる。
そして、走行軌跡推定部３５は、近似式ｙ＝ｆ（ｘ）による近似誤差Ｅｒを次式（１）により算出する。

近似式ｙ＝ｆ（ｘ）が直線を表す場合、近似誤差Ｅｒは、近似直線を基準とする検出位置ＰＡ_ｉの時系列の車幅方向位置ＹＡ_ｉの偏差を示す。
近似誤差Ｅｒが許容閾値γ以上の場合、走行軌跡推定部３５は、検出位置の時系列を曲線近似により補完する近似式ｙ＝ｆ（ｘ）を算出する。
一方で、近似誤差Ｅｒが許容閾値γ未満の場合には、走行軌跡推定部３５は、直線近似の近似式ｙ＝ｆ（ｘ）により表される直線を、他車両２２の走行軌跡として推定する。走行軌跡推定部３５は、推定した走行軌跡を運転支援システム１１へ出力する。

すなわち、走行軌跡推定部３５は、検出位置ＰＡ_ｉの時系列における他車両の車幅方向位置の偏差（すなわち近似誤差Ｅｒ）が許容閾値γ以上の場合には検出位置ＰＡ_ｉの時系列を曲線近似で補間することにより他車両２２の走行軌跡を推定する。
近似誤差Ｅｒが許容閾値γ未満の場合には検出位置ＰＡ_ｉの時系列を直線近似で補間することにより走行軌跡を推定する。

検出位置ＰＡ_ｉの時系列を曲線近似で補間する場合に走行軌跡推定部３５は、所定の曲率半径から順に曲率半径を小さくしていき、その曲率半径の近似曲線を生成する曲線近似式の近似誤差Ｅｒが許容閾値γ未満となるまで、より小さな曲率半径の曲線で補完する曲線近似式を繰り返し算出する。まず、所定の曲率半径の近似曲線を生成する曲線近似式を算出する。例えば、所定の曲率半径は、直線に近い曲率半径として、Ｒ１０００ｍなどに設定する。また、地図情報などから現在走行中の走路の曲率半径の情報が得られている場合には、その曲率半径に対して大きめの曲率半径を所定の曲率半径として設定してもよい。

そして走行軌跡推定部３５は、この曲線近似式による他車両２２の走行軌跡の近似誤差Ｅｒが許容閾値γ未満であるか否かを判断する。
近似誤差Ｅｒが許容閾値γ未満である場合、走行軌跡推定部３５は、この曲線近似式による曲線近似により走行軌跡を推定する。すなわち走行軌跡推定部３５は、この曲線近似式により表される近似曲線を他車両２２の走行軌跡として推定する。

近似誤差Ｅｒが許容閾値γ以上である場合、走行軌跡推定部３５は、この曲線近似式により表される近似曲線（すなわち先の近似曲線）の曲率半径よりも小さな曲率半径の曲線（すなわち次の近似曲線）で検出位置ＰＡ_ｉの時系列を補完する曲線近似式を算出する。
以下、近似誤差Ｅｒが許容閾値γ未満となるまで、より小さな曲率半径の曲線で補完する曲線近似式を繰り返し算出する。

このように、検出位置ＰＡｉの時系列を曲線近似で補間する場合に、より大きな曲率半径の近似曲線を生成する曲線近似式から優先して、より小さな曲率半径の近似曲線を生成する曲線近似式へと順次算出する。
換言すると、走行軌跡推定部３５は、検出位置ＰＡｉの時系列を補完する第１曲線近似式を算出するよりも先に、第１曲線近似式が表す第１近似曲線の曲率半径Ｒ１よりも大きな曲率半径Ｒ２の曲線で検出位置ＰＡｉの時系列を補完する第２曲線近似式を算出する。第２曲線近似式が表す近似曲線を「第２近似曲線」と表記する。例えば、曲率半径Ｒ２は、直線に近い曲率半径として、Ｒ１０００ｍなどに設定することで、曲率半径Ｒ１が既知ではない段階でも、曲率半径Ｒ１に対して大きな曲率を設定することができる。また、地図情報などから現在走行中の走路の曲率半径の情報が得られている場合には、曲率半径Ｒ１に現在走行中の走路の曲率半径の情報を設定することで、曲率半径Ｒ１に対して大きめの曲率半径を、曲率半径Ｒ２として設定することができる。

走行軌跡推定部３５は、第２曲線近似式による他車両２２の走行軌跡の近似誤差Ｅｒが許容閾値γ未満であるか否かを判断する。
近似誤差Ｅｒが許容閾値γ未満である場合、走行軌跡推定部３５は、第２曲線近似式による曲線近似により走行軌跡を推定する。すなわち走行軌跡推定部３５は、第２曲線近似式により表される第２近似曲線を他車両２２の走行軌跡として推定する。
近似誤差Ｅｒが許容閾値γ以上である場合、走行軌跡推定部３５は第１曲線近似式を算出する。

走行軌跡推定部３５は、第１曲線近似式による他車両２２の走行軌跡の近似誤差Ｅｒが許容閾値γ未満であるか否かを判断する。
近似誤差Ｅｒが許容閾値γ未満である場合、走行軌跡推定部３５は、第１曲線近似式による曲線近似により走行軌跡を推定する。すなわち走行軌跡推定部３５は、第１曲線近似式により表される第１近似曲線を他車両２２の走行軌跡として推定する。

近似誤差Ｅｒが許容閾値γ以上である場合、走行軌跡推定部３５は、第１近似曲線の曲率半径Ｒ１よりも小さな曲率半径の曲線で検出位置ＰＡｉの時系列を補完する曲線近似式を算出する。
以下、近似誤差Ｅｒが許容閾値γ未満となるまで、より小さな曲率半径の曲線で補完する曲線近似式を繰り返し算出する。

ここで、多項式により他車両２２の走行軌跡を近似する場合を想定する。他車両２２の走行軌跡が形成する曲線は、曲率半径が小さくなるほど（すなわちカーブが急になるほど）複合的になる。このため、小さな曲率半径の曲線の近似誤差を小さくするには、より高次の曲線近似を行う必要がある。
言いかえれば、より高次の近似曲線はより小さな曲率半径の曲線を小さな近似誤差で補完できる。このため、図６のような検出位置ＰＡ_ｉの車幅方向位置のバラツキが大きな時系列を近似する場合、比較的高次の近似曲線の曲率半径は、比較的低次の近似曲線の曲率半径よりも小さくなる。

そこで、検出位置ＰＡ_ｉの時系列を多項式近似する場合に、走行軌跡推定部３５はより低い次数の近似式から順に算出する。
すなわち、走行軌跡推定部３５は、比較的低いある次数の多項近似式を算出する。そして走行軌跡推定部３５は、この多項近似式による他車両２２の走行軌跡の近似誤差Ｅｒが許容閾値γ未満であるか否かを判断する。
近似誤差Ｅｒが許容閾値γ未満である場合、走行軌跡推定部３５はこの多項近似式による曲線近似により走行軌跡を推定する。
近似誤差Ｅｒが許容閾値γ以上である場合、走行軌跡推定部３５はこの多項近似式よりも次数が高い多項近似式を算出する。
以下、近似誤差Ｅｒが許容閾値γ未満となるまで、走行軌跡推定部３５は次数を上げながら曲線近似式を繰り返し算出する。

なお、過度に高い多項近似式を算出しても、演算負荷が増大する一方で近似誤差が小さくならないおそれがある。このため走行軌跡推定部３５は、多項近似式の次数が上限に達しても近似誤差Ｅｒが許容閾値γ未満にならない場合には、上限の次数の多項近似式による曲線近似により走行軌跡を推定してもよい。
例えば、他車両２２の走行路の曲線には３次程度の多項式近似で十分であるため、上限の次数は３次であってよい。

また、走行軌跡推定部３５は、検出位置ＰＡ_ｉの時系列を複数の近似対象区間に分割し、近似対象区間毎に走行軌跡を推定してもよい。近似対象区間毎に走行軌跡を推定することにより、他車両２２の走行路の形状が複雑であっても、走行軌跡を精度良く推定することができる。
図９を参照して、検出位置ＰＡ_ｉの時系列の分割方法の一例を説明する。円形のプロットＰＡ_１、ＰＡ_２、ＰＡ_３、…ＰＡ_ｋ、ＰＡ_ｋ＋１、…ＰＡ_ｍ、ＰＡ_ｍ＋１…は、それぞれ時刻ｔ_１、ｔ_２、ｔ_３、…ｔ_ｋ、ｔ_ｋ＋１、…ｔ_ｍ、ｔ_ｍ＋１…における他車両２２の検出位置を示す。

走行軌跡推定部３５は、記憶装置１３に記憶した検出位置ＰＡ_ｉの時系列に基づいて、検出位置ＰＡ_ｉにおける他車両２２の旋回半径ＲＡ_ｉを算出する。
走行軌跡推定部３５は、近似対象区間Ｓ１の開始位置ＰＤ_１直後の検出位置ＰＡ_１における旋回半径ＲＡ_１と、各検出位置ＰＡ_ｉにおける旋回半径ＲＡ_ｉとを比較する（ｉ＝２，３,…）。
走行軌跡推定部３５は、ＲＡ_ｋが次式（２）又は（３）を満たす場合に、検出位置ＰＡ_ｋと検出位置ＰＡ_ｋ＋１との間の何れかの分割位置ＰＤ_２で近似対象区間を分割し、近似対象区間Ｓ１の終了位置と、続く近似対象区間Ｓ２の開始位置を決定する。
ＲＡ_ｋ＞β_１×ＲＡ_１…（２）
ＲＡ_ｋ＜ＲＡ_１／β_２…（３）
係数β_１及びβ_２の値は例えば２であってよい。位置ＰＤ_２は、例えば検出位置ＰＡ_ｋと検出位置ＰＡ_ｋ＋１の何れかであってもよい。

以下同様に、走行軌跡推定部３５は、近似対象区間Ｓ２の開始位置ＰＤ_２直後の検出位置ＰＡ_ｋ＋１における旋回半径ＲＡ_ｋ＋１と、各検出位置ＰＡ_ｉにおける旋回半径ＲＡ_ｉとを比較する（ｉ＝ｋ＋２，ｋ＋３,…）。
走行軌跡推定部３５は、次式（４）又は（５）を満たす場合に、検出位置ＰＡ_ｍと検出位置ＰＡ_ｍ＋１との間の何れかの分割位置ＰＤ_３で近似対象区間を分割し、近似対象区間Ｓ２の終了位置と、続く近似対象区間Ｓ３の開始位置を決定する。
ＲＡ_ｍ＞β_１×ＲＡ_ｋ＋１…（４）
ＲＡ_ｍ＜ＲＡ_ｋ＋１／β_２…（５）
以下同様に、走行軌跡推定部３５は、検出位置ＰＡｉの時系列の分割を繰り返す。

走行軌跡推定部３５は、近似対象区間毎に代表値Ｅｖに基づき許容閾値γを算出してよい。代表値Ｅｖは、例えば各近似対象区間における評価値Ｅｖ_ｎの平均値、最大値、又は中間値等の統計学で用いられる代表値であってよい。
ある近似対象区間において、多項近似式の次数が上限に達しても近似誤差Ｅｒが許容閾値γ未満にならない場合、走行軌跡推定部３５はこの近似対象区間をさらに複数の再分割区間に分割してもよい。走行軌跡推定部３５は、係数β_１及びβ_２の値を減少させることにより、近似対象区間をさらに短い再分割区間に分割できる。
走行軌跡推定部３５は、再分割区間毎に走行軌跡を推定する。

（動作）
以下、第１実施形態に係る走行軌跡推定装置３０の動作の一例について説明する。図１０を参照する。
ステップＳ１において他車両認識部３１は、他車両２２の検出位置ＰＡ_ｎを認識し、認識結果を示す他車両情報を生成する。
ステップＳ２において評価部３２は、他車両認識部３１の認識結果毎に、認識状態の評価値Ｅｖ_ｎを決定する。

ステップＳ３において移動量算出部３３は、車両１の移動量を算出する。
ステップＳ４において検出位置蓄積部３４は、他車両認識部３１により生成され評価部３２により評価値Ｅｖ_ｎが付加された他車両情報を記憶装置１３に蓄積する。これにより検出位置ＰＡ_ｉの時系列が記憶装置１３に記憶される。
このとき検出位置蓄積部３４は、今回検出した検出位置ＰＡ_ｎの他車両情報を記憶装置１３に記憶する。また、検出位置蓄積部３４は、記憶装置１３に記憶された前回検出した検出位置ＰＡ_ｎ−１までの検出位置ＰＡ_ｉ（ｉ＝１〜ｎ−１）を、車両１の移動量に基づいて更新する。

ステップＳ５において走行軌跡推定部３５は、検出位置ＰＡ_ｍにおける他車両２２の旋回半径ＲＡ_ｍを算出する（ｍ＜ｎ）。
ステップＳ６において走行軌跡推定部３５は、旋回半径ＲＡ_ｍが上式（４）又は（５）を満たすか否かを判定することにより、検出位置ＰＡ_ｉの時系列の分割位置を発見たか否かを判断する。
分割位置を発見しない場合（ステップＳ６：Ｎ）に処理はステップＳ１へ戻る。

分割位置を発見した場合（ステップＳ６：Ｙ）に、走行軌跡推定部３５は、発見した分割位置で検出位置ＰＡｉの時系列を分割することにより、新たな近似対象区間を切り出す。走行軌跡推定部３５は、処理をステップＳ７に進め、新たな近似対象区間において他車両２２の走行軌跡を推定する。
ステップＳ７において走行軌跡推定部３５は、近似対象区間における近似誤差の許容閾値γを算出する。
ステップＳ８において走行軌跡推定部３５は、近似対象区間において他車両２２の走行軌跡を近似する多項近似式の次数を１次に初期化する。

ステップＳ９において走行軌跡推定部３５は、近似対象区間における検出位置ＰＡ_ｉを補完する多項近似式を算出する。
ステップＳ１０において走行軌跡推定部３５は、ステップＳ９で算出した多項近似式の近似誤差Ｅｒが許容閾値γ以上であるか否かを判断する。近似誤差Ｅｒが許容閾値γ以上である場合（ステップＳ１０：Ｙ）に処理はステップＳ１１へ進む。
近似誤差Ｅｒが許容閾値γ未満である場合（ステップＳ１０：Ｎ）に、走行軌跡推定部３５は、ステップＳ９で算出した多項近似式が表す近似線を、近似対象区間における他車両２２の走行軌跡と推定する。その後に処理はステップＳ１４へ進む。

ステップＳ１１において走行軌跡推定部３５は、多項近似式の次数が上限である所定次数であるか否かを判断する。多項近似式の次数が所定次数に至らない場合（ステップＳ１１：Ｎ）に処理はステップＳ１２に進む。多項近似式の次数が所定次数である場合（ステップＳ１１：Ｙ）に処理はステップＳ１３に進む。
ステップＳ１２において走行軌跡推定部３５は、多項近似式の次数を１つ増加させる。その後に処理はステップＳ９へ戻る。

ステップＳ１３において走行軌跡推定部３５は、再分割処理を行う。再分割処理において走行軌跡推定部３５は、近似対象区間をさらに複数の再分割区間に分割し、再分割区間毎に走行軌跡を推定する。その後に処理はステップＳ１４へ進む。
ステップＳ１４において走行軌跡推定部３５は、車両１のイグニッションスイッチ（ＩＧＮ）がオフになったか否かを判定する。イグニッションスイッチがオフになった場合（ステップＳ１４：Ｙ）に処理は終了する。イグニッションスイッチがオフにならない場合（ステップＳ１４：Ｎ）に処理はステップＳ１に戻る。

次に、図１０のステップＳ１３の再分割処理について説明する。図１１を参照する。
ステップＳ２０において走行軌跡推定部３５は、係数β_１及びβ_２の値を減少させる。
ステップＳ２１において走行軌跡推定部３５は、減少させた係数β_１及びβ_２を用いて、近似対象区間を複数の再分割区間に分割する。
以下、ステップＳ２２〜Ｓ２９において走行軌跡推定部３５は、再分割区間毎に他車両２２の走行軌跡を推定する。

ステップＳ２２において走行軌跡推定部３５は、再分割区間における近似誤差の許容閾値γを算出する。
ステップＳ２３において走行軌跡推定部３５は、再分割区間における多項近似式の次数を１次に初期化する。
ステップＳ２４において走行軌跡推定部３５は、再分割区間における多項近似式を算出する。

ステップＳ２５において走行軌跡推定部３５は、ステップＳ２４で算出した多項近似式の近似誤差Ｅｒが許容閾値γ以上であるか否かを判断する。近似誤差Ｅｒが許容閾値γ以上である場合（ステップＳ２５：Ｙ）に処理はステップＳ２６へ進む。
近似誤差Ｅｒが許容閾値γ未満である場合（ステップＳ２５：Ｎ）に、走行軌跡推定部３５は、ステップＳ２４で算出した多項近似式が表す近似線を、再分割区間における他車両２２の走行軌跡と推定する。その後に処理はステップＳ２９へ進む。

ステップＳ２６において走行軌跡推定部３５は、多項近似式の次数が上限である所定次数であるか否かを判断する。多項近似式の次数が所定次数に至らない場合（ステップＳ２６：Ｎ）に処理はステップＳ２７に進む。多項近似式の次数が所定次数である場合（ステップＳ２６：Ｙ）に処理はステップＳ２８に進む。
ステップＳ２７において走行軌跡推定部３５は、多項近似式の次数を１つ増加させる。その後に処理はステップＳ２４へ戻る。

ステップＳ２８において走行軌跡推定部３５は、再分割区間に対して再び再分割処理を行うことにより、再分割区間を複数の区間に分割して区間毎に走行軌跡を推定する。その後に処理はステップＳ２９へ進む。
ステップＳ２９において走行軌跡推定部３５は、ステップＳ２１にて近似対象区間を分割して得た再分割区間の全てにおいて、走行軌跡の多項近似式の算出が完了したか否かを判定する。全ての多項近似式の算出が完了した場合（ステップＳ２９：Ｙ）に再分割処理は終了する。多項近似式の算出が完了していない再分割区間がある場合（ステップＳ２９：Ｎ）に処理はステップＳ２２へ戻る。

（第１実施形態の効果）
（１）車両１に搭載され車両１周辺の物体の位置を検出する位置測定センサ６は、他車両２２の位置を検出する。検出位置蓄積部３４は、位置測定センサ６により検出された他車両２２の検出位置の時系列を、記憶装置１３に記憶する。
走行軌跡推定部３５は、車両１と他車両２２との距離が長いほど大きな許容閾値γを設定する。
走行軌跡推定部３５は、検出位置の時系列における他車両の車幅方向位置の偏差Ｅｒが許容閾値γ以上の場合には検出位置の時系列を曲線近似で補間することにより他車両２２の走行軌跡を推定する。走行軌跡推定部３５は、偏差Ｅｒが許容閾値γ未満の場合には検出位置の時系列を直線近似で補間することにより他車両２２の走行軌跡を推定する。

このため、車両１と他車両２２との距離が長いほど大きな許容閾値γが設定されるので、車両１と他車両２２との距離が長くなると曲線近似よりも直線近似によって走行軌跡が推定され易くなる。このため、車両１と他車両２２との距離が長くなり他車両２２の検出位置の誤差が増大しても、算出した走行軌跡が実際の走行軌跡よりも蛇行するのを抑制できる。

（２）検出位置の時系列を曲線近似で補間する場合に、走行軌跡推定部３５は、曲線近似の曲線半径を、所定の曲率半径から順に小さくしていき、走行軌跡推定部３５は、曲線近似による近似誤差Ｅｒを算出し、近似誤差Ｅｒが許容閾値γ未満となる曲線近似により他車両２２の走行軌跡を推定する。
このように、走行軌跡推定部３５は、所定の曲率半径から順により小さな曲率半径の曲線で補完する曲線近似式を繰り返し算出するので、曲率半径が大きいより緩やかな走行軌跡を優先的に算出することができる。このため、算出した他車両２２の走行軌跡の蛇行を低減できる。

（３）走行軌跡推定部３５は、検出位置の時系列を複数の近似対象区間に分割し、近似対象区間毎に他車両２２の走行軌跡を推定する。検出位置の時系列を複数区間に分割して区間毎に近似線を算出するため、他車両２２の走行路の形状が複雑であっても精度よく走行軌跡を推定できる。
（４）走行軌跡推定部３５は、ある近似対象区間における近似誤差Ｅｒが許容閾値γ未満にならない場合、この近似対象区間を複数の再分割区間にさらに分割し、再分割区間毎に他車両２２の走行軌跡を推定する。
近似誤差Ｅｒが許容閾値γ未満にならない場合に更に小さな区間毎に他車両２２の走行軌跡を推定することにより、走行軌跡の推定精度を向上することができる。

（第２実施形態）
続いて、第２実施形態の走行軌跡推定装置３０を説明する。車両１が旋回している間は、他車両認識部３１による他車両２２の検出位置ＰＡ_ｉの検出精度が低下することがある。他車両２２が旋回している間も同様である。
そこで第２実施形態の走行軌跡推定装置３０は、車両１及び他車両２２の少なくとも一方が旋回している場合には、許容閾値γを増加させることによってより単純な近似線で走行軌跡を近似され易くし、走行軌跡の近似結果が不安定に蛇行するのを抑制する。

第２実施形態の走行軌跡推定装置３０の機能構成は、図７に示す機能構成と同様である。走行軌跡推定部３５は、移動量算出部３３が算出した車両１の移動量に基づいて車両１のヨーレートωを算出する。走行軌跡推定部３５は、他車両２２の検出位置ＰＡ_ｎの時系列に基づいて、他車両２２の速度と旋回半径を算出し、速度と旋回半径とに基づいて他車両２２のヨーレートρを算出する。

走行軌跡推定部３５は、車両１のヨーレートωが大きい場合の許容閾値γを、ヨーレートωが小さい場合の許容閾値γよりも大きく設定する。例えば、ヨーレートωの絶対値｜ω｜が所定閾値ω０より大きい場合には、絶対値｜ω｜が所定閾値ω０未満の場合の許容閾値をβ_３倍した値を許容閾値として設定する。β_３は１より大きな係数であり、例えばβ_３＝１．２であってよい。また、所定閾値ω０は例えば１０（ｄｅｇ／ｓ）であってよい。

また、走行軌跡推定部３５は、他車両２２のヨーレートρが大きい場合の許容閾値γを、ヨーレートρが小さい場合の許容閾値γよりも大きく設定する。例えば、ヨーレートρの絶対値｜ρ｜が所定閾値ρ０より大きい場合には、絶対値｜ρ｜が所定閾値ρ０未満の場合の許容閾値をβ_４倍した値を許容閾値として設定する。β_４は１より大きな係数であり、例えばβ_４＝１．２であってよい。また、所定閾値ρ０は例えば１０（ｄｅｇ／ｓ）であってよい。

（第２実施形態の効果）
走行軌跡推定部３５は、車両１及び他車両２２の少なくとも一方が旋回している場合に許容閾値γを増加させる。
これにより、車両１及び他車両２２の少なくとも一方が旋回することにより他車両２２の検出位置の検出精度が低下しても、より単純な近似線で走行軌跡を近似され易くし、走行軌跡の近似結果が不安定に蛇行するのを抑制する。

（第３実施形態）
続いて、第３実施形態の走行軌跡推定装置３０を説明する。図１２を参照する。第２実施形態の走行軌跡推定装置３０は、車両１と他車両２２との間の車幅方向位置の差分Ｄｗが大きいほど許容閾値γを減少させる。
図１３を参照する。範囲２５及び２６内の円形のプロットは、位置測定センサ６による他車両２２の走査点を示す。車両１と他車両２２との間の車幅方向位置の差分Ｄｗが大きい場合に、位置測定センサ６は図１３に示すように他車両２２の背面に加えて側面を走査することができるので、検出位置ＰＡ_ｉの検出精度がより高くなる。
そこで、第３実施形態の走行軌跡推定装置３０は、車両１と他車両２２との間の車幅方向位置の差分Ｄｗが大きい場合には許容閾値γを減少させることで走行軌跡の近似精度を向上する。

第３実施形態の走行軌跡推定装置３０の機能構成は、図７に示す機能構成と同様である。走行軌跡推定部３５は、他車両２２の検出位置ＰＡ_ｎに基づいて、車両１と他車両２２との間の車幅方向位置の差分Ｄｗを取得する。
走行軌跡推定部３５は、差分Ｄｗが大きい場合の許容閾値γを、差分Ｄｗが小さい場合の許容閾値γよりも小さく設定する。例えば、差分Ｄｗの絶対値｜Ｄｗ｜が所定閾値Ｄ０より大きい場合には、絶対値｜Ｄｗ｜が所定閾値Ｄ未満の場合の許容閾値をβ_５倍した値を許容閾値として設定する。β_５は１より小さな係数であり、例えばβ_５＝０．９であってよい。また、所定閾値Ｄは例えば２．０ｍであってよい。

（第３実施形態の効果）
走行軌跡推定部３５は、車両１と他車両２２との間の車幅方向位置の差分Ｄｗが大きいほど許容閾値γを減少させる。
これにより、他車両２２の検出位置ＰＡ_ｉの検出精度がより高い場合には他車両２２の走行軌跡の近似誤差Ｅｒの許容閾値γを小さくすることにより、走行軌跡の近似精度を向上することができる。

１…車両、２…車輪、３…車輪速センサ、４…ステアリングホイール、５…操舵角センサ、６…位置測定センサ、７…ヨーレートセンサ、１０…コントローラ、１１…運転支援システム、１２…プロセッサ、１３…記憶装置、２２…他車両、３０…走行軌跡推定装置、３１…他車両認識部、３２…評価部、３３…移動量算出部、３４…検出位置蓄積部、３５…走行軌跡推定部

Claims (7)

1. 車両に搭載され前記車両周辺の物体の位置を検出するセンサにより他車両の位置を検出し、
   前記センサにより検出された前記他車両の検出位置の時系列を記憶し、
   前記車両と前記他車両との距離が長いほど大きな閾値を設定し、
   前記検出位置の時系列を直線近似で補間した場合の近似誤差が前記閾値以上の場合には前記検出位置の時系列を曲線近似で補間することにより前記他車両の走行軌跡を推定し、前記近似誤差が前記閾値未満の場合には前記検出位置の時系列を直線近似で補間することにより前記走行軌跡を推定する、
   ことを特徴とする走行軌跡推定方法。
2. 前記検出位置の時系列を曲線近似で補間する場合に、前記曲線近似の曲線半径を、所定の曲率半径から順に小さくしていき、
   前記曲線近似による近似誤差を算出し、前記近似誤差が前記閾値未満となる前記曲線近似により前記走行軌跡を推定する、
   ことを特徴とする請求項１に記載の走行軌跡推定方法。
3. 前記時系列を複数の区間に分割し、前記区間毎に前記走行軌跡を推定することを特徴とする請求項１または２に記載の走行軌跡推定方法。
4. 前記走行軌跡の曲線近似の近似誤差が前記閾値未満でない前記区間をさらに分割することを特徴とする請求項３に記載の走行軌跡推定方法。
5. 前記車両及び前記他車両の少なくとも一方が旋回している場合に、前記閾値を増加させることを特徴とする請求項１〜４の何れか一項に記載の走行軌跡推定方法。
6. 前記車両と前記他車両との間の車幅方向位置の差分が大きいほど前記閾値を減少させることを特徴とする請求項１〜５の何れか一項に記載の走行軌跡推定方法。
7. 車両に搭載され前記車両周辺の物体の位置を検出するセンサと、
   前記センサにより検出された他車両の検出位置の時系列を記憶する記憶装置と、
   前記車両と前記他車両との距離が長いほど大きな閾値を設定し、前記検出位置の時系列を直線近似で補間した場合の近似誤差が前記閾値以上の場合には前記検出位置の時系列を曲線近似で補間することにより前記他車両の走行軌跡を推定し、前記近似誤差が前記閾値未満の場合には前記検出位置の時系列を直線近似で補間することにより前記走行軌跡を推定する推定回路と、
   を備えることを特徴とする走行軌跡推定装置。

Images