知財求人 - 知財ポータルサイト「IP Force」
▶ 株式会社日本自動車部品総合研究所の特許一覧 ▶ 株式会社デンソーの特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2025131106
(43)【公開日】2025-09-09
(54)【発明の名称】推定装置
(51)【国際特許分類】
G01S 13/66 20060101AFI20250902BHJP
G01S 13/931 20200101ALI20250902BHJP
【ＦＩ】
G01S13/66
G01S13/931
【審査請求】未請求
【請求項の数】10
【出願形態】ＯＬ
(21)【出願番号】P 2024028628
(22)【出願日】2024-02-28
(71)【出願人】
【識別番号】000004695
【氏名又は名称】株式会社ＳＯＫＥＮ
(71)【出願人】
【識別番号】000004260
【氏名又は名称】株式会社デンソー
(74)【代理人】
【識別番号】110000578
【氏名又は名称】名古屋国際弁理士法人
(72)【発明者】
【氏名】赤峰 悠介
(72)【発明者】
【氏名】▲高▼山 卓也
【テーマコード（参考）】
5J070
【Ｆターム（参考）】
5J070AB24
5J070AC02
5J070AC06
5J070AE01
5J070AF03
(57)【要約】
【課題】道路形状を利用することなく、物体の角速度を精度よく推定できるようにする。
【解決手段】推定装置１０は、センサ波を送受信して物体上の複数の反射点の位置及び相対速度を検出するセンサ部１１と、物体の任意の基準点における速度ベクトルを算出する速度ベクトル算出部２３と、基準点から反射点方位を傾きとする直線までの距離を算出する反射点距離算出部２１と、反射点の相対速度から基準点の速度ベクトルによる反射点方位における相対速度を差し引くことで反射点の回転相対速度を算出する回転相対速度算出部２５と、反射点距離算出部及び回転相対速度算出部にて算出された反射点距離及び回転相対速度に基づき物体の角速度を算出する角速度算出部２７と、を備える。
【選択図】図２
【解決手段】推定装置１０は、センサ波を送受信して物体上の複数の反射点の位置及び相対速度を検出するセンサ部１１と、物体の任意の基準点における速度ベクトルを算出する速度ベクトル算出部２３と、基準点から反射点方位を傾きとする直線までの距離を算出する反射点距離算出部２１と、反射点の相対速度から基準点の速度ベクトルによる反射点方位における相対速度を差し引くことで反射点の回転相対速度を算出する回転相対速度算出部２５と、反射点距離算出部及び回転相対速度算出部にて算出された反射点距離及び回転相対速度に基づき物体の角速度を算出する角速度算出部２７と、を備える。
【選択図】図２
【特許請求の範囲】
【請求項1】
移動体に搭載されて周囲の物体の状態を推定する推定装置であって、
周囲にセンサ波を送信し、その反射波から前記物体上の複数の反射点の位置及び相対速度を検出するように構成されたセンサ部（１１）と、
前記物体の任意の基準点の速度ベクトルを算出するように構成された速度ベクトル算出部（２３）と、
前記基準点から、前記センサ部にて検出された前記反射点方位を傾きとする直線までの距離を、反射点距離として算出するように構成された反射点距離算出部（２１）と、
前記センサ部にて検出された前記反射点の相対速度から、前記速度ベクトル算出部にて算出された基準点の前記速度ベクトルによる前記反射点方位における相対速度を差し引いた値である、前記反射点の回転相対速度を算出するように構成された回転相対速度算出部（２５）と、
前記反射点距離算出部及び前記回転相対速度算出部にて算出された、少なくとも１つの前記反射点の前記反射点距離及び前記回転相対速度に基づき、前記物体の角速度を算出するように構成された角速度算出部（２７）と、
を備えている、推定装置。
周囲にセンサ波を送信し、その反射波から前記物体上の複数の反射点の位置及び相対速度を検出するように構成されたセンサ部（１１）と、
前記物体の任意の基準点の速度ベクトルを算出するように構成された速度ベクトル算出部（２３）と、
前記基準点から、前記センサ部にて検出された前記反射点方位を傾きとする直線までの距離を、反射点距離として算出するように構成された反射点距離算出部（２１）と、
前記センサ部にて検出された前記反射点の相対速度から、前記速度ベクトル算出部にて算出された基準点の前記速度ベクトルによる前記反射点方位における相対速度を差し引いた値である、前記反射点の回転相対速度を算出するように構成された回転相対速度算出部（２５）と、
前記反射点距離算出部及び前記回転相対速度算出部にて算出された、少なくとも１つの前記反射点の前記反射点距離及び前記回転相対速度に基づき、前記物体の角速度を算出するように構成された角速度算出部（２７）と、
を備えている、推定装置。
【請求項2】
請求項１に記載の推定装置であって、
前記角速度算出部は、少なくとも１つの前記反射点についての前記反射点距離に対する前記回転相対速度の大きさに基づき、前記角速度を算出するように構成されている、推定装置。
前記角速度算出部は、少なくとも１つの前記反射点についての前記反射点距離に対する前記回転相対速度の大きさに基づき、前記角速度を算出するように構成されている、推定装置。
【請求項3】
請求項２に記載の推定装置であって、
前記角速度算出部は、
前記反射点距離と前記回転相対速度をパラメータとする座標空間における前記反射点の分布から線形回帰を行い、前記座標空間内での傾きを算出するように構成された回帰処理部（２７Ａ）を備え、
前記回帰処理部にて算出された傾きから前記角速度を算出するように構成されている、推定装置。
前記角速度算出部は、
前記反射点距離と前記回転相対速度をパラメータとする座標空間における前記反射点の分布から線形回帰を行い、前記座標空間内での傾きを算出するように構成された回帰処理部（２７Ａ）を備え、
前記回帰処理部にて算出された傾きから前記角速度を算出するように構成されている、推定装置。
【請求項4】
請求項１～請求項３の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記センサ部にて検出された複数の前記反射点について、下記の判定条件ａ）～ｃ）の少なくとも１つに基づき除外判定を行い、
ａ）前記反射波の受信電力が閾値以下である。
ｂ）前記基準点との速度差が閾値以上である。
ｃ）前記反射波が複数波である。
前記除外判定にて前記判定条件ａ）～ｃ）の少なくとも１つに適合すると判定された前記反射点については、前記角速度の算出対象から除外するように構成されている、推定装置。
前記角速度算出部は、
前記センサ部にて検出された複数の前記反射点について、下記の判定条件ａ）～ｃ）の少なくとも１つに基づき除外判定を行い、
ａ）前記反射波の受信電力が閾値以下である。
ｂ）前記基準点との速度差が閾値以上である。
ｃ）前記反射波が複数波である。
前記除外判定にて前記判定条件ａ）～ｃ）の少なくとも１つに適合すると判定された前記反射点については、前記角速度の算出対象から除外するように構成されている、推定装置。
【請求項5】
請求項１～請求項３の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記物体との距離が所定の値以下、前記反射点の数が所定値以下、前記物体の加速度の絶対値が閾値以上、の何れかの条件を満たす場合は、前記角速度の算出を行わない、又は、前記角速度の算出結果を出力しない、ように構成されている、推定装置。
前記角速度算出部は、
前記物体との距離が所定の値以下、前記反射点の数が所定値以下、前記物体の加速度の絶対値が閾値以上、の何れかの条件を満たす場合は、前記角速度の算出を行わない、又は、前記角速度の算出結果を出力しない、ように構成されている、推定装置。
【請求項6】
請求項１～請求項３の何れか１項に記載の推定装置であって、
前記複数の反射点に対応する前記物体について、前記物体の状態を推定するように構成された状態推定部（３０）を備え、
前記状態推定部は、前記物体の状態推定結果に、前記角速度算出部にて算出された前記角速度の算出値である角速度算出値を反映させるように構成されている、推定装置。
前記複数の反射点に対応する前記物体について、前記物体の状態を推定するように構成された状態推定部（３０）を備え、
前記状態推定部は、前記物体の状態推定結果に、前記角速度算出部にて算出された前記角速度の算出値である角速度算出値を反映させるように構成されている、推定装置。
【請求項7】
請求項６に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記物体の状態推定結果に含まれる前記角速度の推定結果である角速度推定値と、前記角速度算出部にて算出された前記角速度算出値とを混合し、前記角速度推定値を更新するように構成されている、推定装置。
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記物体の状態推定結果に含まれる前記角速度の推定結果である角速度推定値と、前記角速度算出部にて算出された前記角速度算出値とを混合し、前記角速度推定値を更新するように構成されている、推定装置。
【請求項8】
請求項６に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記角速度算出部にて算出された前記角速度算出値の絶対値が所定の値以上の場合に、前記状態推定部にて前記物体の状態を推定するのに用いられるフィルタのフィルタゲインを増加させるように構成されている、推定装置。
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記角速度算出部にて算出された前記角速度算出値の絶対値が所定の値以上の場合に、前記状態推定部にて前記物体の状態を推定するのに用いられるフィルタのフィルタゲインを増加させるように構成されている、推定装置。
【請求項9】
請求項６に記載の推定装置であって、
前記状態推定部は、前記状態推定部にて前記物体の状態の１つとして推定された前記物体の過去の速度ベクトルに基づき算出された、前記物体の現在の速度ベクトル予測値を、前記角速度算出部にて算出された前記角速度算出値にて補正するように構成されている、推定装置。
前記状態推定部は、前記状態推定部にて前記物体の状態の１つとして推定された前記物体の過去の速度ベクトルに基づき算出された、前記物体の現在の速度ベクトル予測値を、前記角速度算出部にて算出された前記角速度算出値にて補正するように構成されている、推定装置。
【請求項10】
請求項１～請求項３の何れか１項に記載の推定装置であって、
前記角速度算出部にて算出された前記角速度算出値を用いて、前記移動体と前記物体との衝突判定を行うように構成された衝突判定部（３２）を備えている、推定装置。
前記角速度算出部にて算出された前記角速度算出値を用いて、前記移動体と前記物体との衝突判定を行うように構成された衝突判定部（３２）を備えている、推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、移動体に搭載されて周囲の物体の状態を推定する推定装置に関する。
【背景技術】
【0002】
車載レーダにより物体追跡を行う際、物体の挙動が直進から旋回に変化する場合に追従が遅れることがある。これは、車載レーダによる物体観測には、観測誤差を抑制するために、予測と観測を混合するフィルタ処理が必要であるためである。つまり、フィルタ処理では、直進から旋回への移行時に、直進を仮定した予測が行われ、観測結果が物体の実際の挙動から乖離するためである。
【0003】
この問題を抑制する技術としては、フィルタのフィルタゲインを調整して追従性を確保することが一般的であるが、このような対策では、安定性が低下するおそれがある。これに対し、特許文献１には、道路形状にもとづき物体の進行方向及び角速度を推定することで、物体追跡時の追従性を向上させることが提案されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開２０２１－６０２４５号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
特許文献１に記載の推定装置では、道路形状に基づき物体の進行方向及び角速度を推定することから、物体の状態が直進から旋回へ変化しても、その変化を瞬時に推定して、物体を精度よく追跡できるようになる。
【0006】
しかし、特許文献１に記載の推定装置では、例えば、物体が交差点に進入した場合など、物体が道路の分岐点を通過する際には、物体の進行方向を正確に予測することができないことから、角速度の推定精度も低下してしまう、という問題がある。
【0007】
本開示の１つの局面は、道路形状を利用することなく、物体の角速度を精度よく推定できるようにすることを目的とする。
【課題を解決するための手段】
【0008】
本開示の１つの態様による推定装置は、移動体に搭載されて周囲の物体の状態を推定する推定装置であり、センサ部（１１）と、速度ベクトル算出部（２３）と、反射点距離算出部（２１）と、回転相対速度算出部（２５）と、角速度算出部（２７）と、を備える。
【0009】
このうち、センサ部は、周囲にセンサ波を送信し、その反射波から物体上の複数の反射点の位置及び相対速度を検出する。また、速度ベクトル算出部は、物体の任意の基準点における速度ベクトルを算出する。
【0010】
また、反射点距離算出部は、基準点から、センサ部で検出された反射点方位を傾きとする直線までの距離を、反射点距離として算出する。また、回転相対速度算出部は、センサ部にて検出された反射点の相対速度から、速度ベクトル算出部にて算出された速度ベクトルの反射点方位における相対速度を差し引いた値である、反射点の回転相対速度を算出する。
【0011】
そして、角速度算出部は、反射点距離算出部及び回転相対速度算出部にて算出された、少なくとも１つの反射点の反射点距離及び回転相対速度に基づき、物体の角速度を算出する。
【0012】
このように、本開示の推定装置においては、道路形状を利用することなく、センサ波を送受信することにより検出される周囲の物体の複数の反射点の位置及び相対速度に基づき、物体の角速度を推定する。
【0013】
これは、旋回中の物体についてセンサ波の送受信により観測すると、反射点ごとに異なる相対速度が検出されるためである。つまり、旋回中の物体においては、回転運動により、各反射点に、旋回中心からの距離に応じた回転速度が発生し、この回転速度の大きさが物体の角速度に依存して変化する。
【0014】
そこで、本開示の推定装置においては、反射点の相対速度から、基準点の速度ベクトルの反射点方位における相対速度を差し引くことで、反射点の回転速度を、回転相対速度として求め、その回転相対速度と反射点距離に基づき、角速度を算出するのである。
【0015】
従って、本開示の推定装置によれば、道路形状を利用することなく、物体の角速度を推定することができるようになり、物体が道路の分岐点を通過するような場合でも、角速度を精度よく推定することが可能となる。
【図面の簡単な説明】
【0016】
【図1】実施形態の推定装置のハードウェア構成を表すブロック図である。
【図2】実施形態の推定装置の機能構成を表すブロック図である。
【図3】車両におけるセンサ部の搭載位置及び検知範囲の一例を示す図である。
【図4】車両におけるセンサ部の搭載位置及び検知範囲の別の一例を示す図である。
【図5】物体の複数の反射点での相対速度と回転速度を表す説明図である。
【図6】反射点距離算出部での反射点距離の算出動作を表す説明図である。
【図7】回転相対速度算出部での回転回転相対速度の算出動作を表す説明図である。
【図8】物体の角速度の算出方法を説明する説明図である。
【図9】処理装置において実行される制御処理を表すフローチャートである。
【図15】角速度の算出値に基づくフィルタゲインの切替動作を説明する説明図である。
【図17】角速度の算出値に基づく進行角予測値の補正動作を説明する説明図である。
【図18】第２実施形態の処理装置にて実行される制御処理を表すフローチャートである。
【発明を実施するための形態】
【0017】
【0018】
センサ部１１は、レーダ、ライダー、ソナーなどである。レーダは、センサ波としてミリ波などの電波を送信し、物体６０でレーダ波が反射して生じた反射波を受信する。ライダーは、センサ波として光を送信し、物体で光が反射して生じた反射波を受信する。ソナーは、センサ波として音波を送信し、物体で音波が反射して生じた反射波を受信する。
【0019】
図３に示すように、センサ部１１は、車両５０の前方中央（例えば、前方バンパの中央）に搭載され、車両５０の前方中央の検知エリアＡ１を有していてもよい。あるいは、図４に示すように、センサ部１１は、車両５０の前方中央に加えて、車両５０の左前方、右前方、左後方及び右後方（例えば、前方バンパの左端及び右端と、後方バンパの左端及び右端）に搭載されてもよい。すなわち、センサ部１１は、検知エリアＡ１に加えて、車両５０の左前方、右前方、左後方及び右後方の検知エリアＡ２を有していてもよい。センサ部１１は、車両５０の前方中央、左前方、右前方、左後方及び右後方のうちのすくなくとも一箇所に搭載されていればよい。
【0020】
センサ部１１は、車両５０の周辺にセンサ波を送受信することで、複数の観測値を取得する。センサ部１１は、高分解能センサであり、一回のセンサ波の送信により、単一の物体６０の互いに異なる反射点で反射した複数の反射波を取得し、各反射波に基づいた観測値を取得できる。したがって、その複数の観測値の各々は、単一の物体６０の互いに異なる反射位置に対応する。
【0021】
なお、複数の観測値の各々は、物理量として、反射点の位置（すなわち、センサ部１１から反射点までの距離、及び、センサ部１１に対する反射点の方位）と、反射点と車両５０（詳しくは、センサ部１１）との相対速度を含む。
【0022】
次に、処理装置２０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を含むマイクロコンピュータを備える。処理装置２０は、ＣＰＵが非遷移的実体的記録媒体に記憶されているプログラムを実行することにより、反射点距離算出部２１、速度ベクトル算出部２３、回転相対速度算出部２５、及び、角速度算出部２７の機能を実現する。なお、この機能は、センサ部１１にて取得された単一の物体６０の複数の反射点の観測値に基づき、その物体６０の角速度、換言すればヨーレート、を算出する機能である。
【0023】
また、処理装置２０は、上記算出部２１，２３，２５，２７による角速度算出機能だけでなく、センサ部１１にて取得された複数の反射点の観測値に基づき物体６０の状態を推定する、状態推定部３０の機能も実現する。
【0024】
状態推定部３０の機能は、本実施形態では、物体６０の形状モデルを用いて、拡張物体追跡を実行することで実現される。そして、この機能により、物体６０の基準点のｘ方向位置、ｙ方向位置、速度、進行方向、角速度、といった物体６０の運動状態を表す物理量が、推定値として算出される。
【0025】
なお、ｘ方向は、車両５０の長さ方向に相当し、ｙ方向は、車両５０の幅方向に相当する。また、物体６０の基準点には、物体６０が自動車であるとして、左右後輪の中心位置（以下、後輪軸中心）が設定される。
【0026】
拡張物体追跡は、物標が形状を持つと仮定して物標をモデル化して、物標の運動状態を時系列的に推定する手法であり、本実施形態では、下記の文献Ａに記載の手順に則って実施される。具体的な推定手順については、後述の状態推定処理にて説明する。
【0027】
文献Ａ：時澤宗一郎、米陀佳祐、菅沼直樹、自動運転のための安定性とリアルタイム性とを両立した拡張物体追跡、自動車技術会論文集、Vol.52、No.5、September 2021.
次に、角速度算出機能を実現する、反射点距離算出部２１、速度ベクトル算出部２３、回転相対速度算出部２５、及び、角速度算出部２７について説明する。
次に、角速度算出機能を実現する、反射点距離算出部２１、速度ベクトル算出部２３、回転相対速度算出部２５、及び、角速度算出部２７について説明する。
【0028】
まず、反射点距離算出部２１は、センサ部１１にて検出された物体６０の複数の反射点の位置と物体６０の基準点との間の距離を、反射点距離として算出する。また、速度ベクトル算出部２３は、状態推定部３０にて検出された最新の物体６０の状態から、物体６０の基準点における速度ベクトルを算出する。ここで、速度ベクトルは状態推定処理にて推定した過去の基準点の速度ベクトルから現在の時刻における速度ベクトルを予測した予測値である。例えば、物体の挙動を等速直線運動と仮定した場合は過去に推定した速度ベクトルと現在の時刻における速度ベクトルの予測値とする。一方、例えば旋回運動を仮定する場合、過去に推定した速度ベクトルに加え過去に推定した角速度を用い、角速度に応じた向きの変化を加えたものを現在の時刻の速度ベクトルの予測値とする。
【0029】
また、回転相対速度算出部２５は、センサ部１１にて検出された複数の反射点の相対速度から、速度ベクトル算出部２３にて算出された速度ベクトルの反射点方位における相対速度を差し引くことで、各反射点の回転相対速度を算出する。なお、速度ベクトルの反射点方位における相対速度は、速度ベクトル算出部２３にて算出された基準点の速度ベクトルを、車両５０との相対速度に変換した速度を表す。
【0030】
そして、角速度算出部２７は、反射点距離算出部２１にて算出された反射点距離と、回転相対速度算出部２５にて算出された回転相対速度とに基づき、物体６０の角速度を算出する。
【0031】
角速度算出部２７による角速度の算出結果は、状態推定部３０に出力され、状態推定部３０は、物体６０の状態推定結果に、角速度算出部２７にて算出された角速度の算出値を反映させる。この結果、状態推定に用いられるカルマンフィルタ等によるフィルタ処理によって、物体６０の旋回時に、状態推定部３０の状態推定に遅れが生じたとしても、その遅れを、角速度算出部２７にて算出された角速度によって補正することができる。なお、この補正方法については、後述の状態推定処理にて説明する。
【0032】
［角速度算出原理］
次に、反射点距離算出部２１、速度ベクトル算出部２３、回転相対速度算出部２５、及び、角速度算出部２７による、角速度の算出原理について説明する。
次に、反射点距離算出部２１、速度ベクトル算出部２３、回転相対速度算出部２５、及び、角速度算出部２７による、角速度の算出原理について説明する。
【0033】
図５Ａに示すように、物体６０が直進しているときにセンサ部１１にて検出される複数の反射点の相対速度は、ほぼ同一になる。これに対し、物体６０が旋回しているときには、図５Ｂに示すように、複数の反射点の相対速度は、反射点ごとに異なる。
【0034】
このように、物体６０の旋回中に各反射点の相対速度が異なるのは、物体６０の回転運動により、各反射点に、旋回中心からの距離（ｒ）に応じた回転速度（Ｖ）が発生するためである。そして、この回転速度（Ｖ）は、円運動の速度の式「Ｖ＝ｒ・ω」から明らかなように、物体６０の角速度（ω）、換言すればヨーレートに応じて変化する。
【0035】
従って、物体内における任意の基準点Ｐｒの位置、速度ベクトルと、基準点Ｐｒとは異なる位置の反射点の相対速度、方位の関係からヨーレートを導出できる。そこで、本実施形態では、物体６０の角速度を、基準点Ｐｒの位置、速度ベクトルと、反射点の相対速度、方位から算出する。
【0036】
ここで、基準点Ｐｒと反射点は、並進移動による並進速度成分は同一であるが、回転運動による回転速度成分については、位置の違いによる差が生じる。
すなわち、物体の反射点の位置をｘ，ｙ、物体内の任意の基準点Ｐｒの位置をｘ0，ｙ0、基準点のｘ方向，ｙ方向の速度をＶｘ0，Ｖｙ0、物体の角速度をωとすると、反射点のｘ方向，ｙ方向の速度Ｖｘ，Ｖｙは、それぞれ、次式(1），（2）のように記述できる。
すなわち、物体の反射点の位置をｘ，ｙ、物体内の任意の基準点Ｐｒの位置をｘ0，ｙ0、基準点のｘ方向，ｙ方向の速度をＶｘ0，Ｖｙ0、物体の角速度をωとすると、反射点のｘ方向，ｙ方向の速度Ｖｘ，Ｖｙは、それぞれ、次式(1），（2）のように記述できる。
【0037】
Ｖｘ＝Ｖｘ0－ω（ｙ－ｙ0） … (1)
Ｖｙ＝Ｖｙ0＋ω（ｘ－ｘ0） … (2)
また、反射点の相対速度Ｖｒは、次式(3)のように記述できる。
Ｖｙ＝Ｖｙ0＋ω（ｘ－ｘ0） … (2)
また、反射点の相対速度Ｖｒは、次式(3)のように記述できる。
【0038】
Ｖｒ＝Ｖｘcosθ＋Ｖｙsinθ … (3)
そして、（3）式に(1)，(2)式を代入すると、次式(4）となる。
Ｖｒ＝（Ｖｘ0－ω（ｙ－ｙ0））cosθ＋（Ｖｙ0＋ω（ｘ－ｘ0））sinθ
＝Ｖｘ0cosθ＋Ｖｙ0sinθ
＋（－ｙcosθ＋ｘsinθ）ω
＋（ｙ0cosθ－ｘ0sinθ）ω … (4)
ここで、ｘ＝ｒcosθ，ｙ＝ｒsinθより、ｘsinθ＝ｙcosθであるため、(4)式は、次式(5)のように記述することができる。そして、次式(5)から、次式(6)式が成り立つ。
そして、（3）式に(1)，(2)式を代入すると、次式(4）となる。
Ｖｒ＝（Ｖｘ0－ω（ｙ－ｙ0））cosθ＋（Ｖｙ0＋ω（ｘ－ｘ0））sinθ
＝Ｖｘ0cosθ＋Ｖｙ0sinθ
＋（－ｙcosθ＋ｘsinθ）ω
＋（ｙ0cosθ－ｘ0sinθ）ω … (4)
ここで、ｘ＝ｒcosθ，ｙ＝ｒsinθより、ｘsinθ＝ｙcosθであるため、(4)式は、次式(5)のように記述することができる。そして、次式(5)から、次式(6)式が成り立つ。
【0039】
Ｖｒ＝Ｖｘ0cosθ＋Ｖｙ0sinθ＋（ｙ0cosθ－ｘ0sinθ）ω … (5)
（ｙ0cosθ－ｘ0sinθ）ω＝Ｖｒ－（Ｖｘ0cosθ＋Ｖｙ0sinθ） … (6)
(6)式において、左辺括弧内は、基準点Ｐｒから、反射点方位を傾きとする直線までの距離、換言すれば、基準点Ｐｒから反射点への円周方向の位置ずれに対応する。また、右辺は、反射点の相対速度Ｖｒから基準点Ｐｒの速度ベクトルの反射点方位における相対速度を差し引いた速度(以下、回転相対速度)、換言すれば、基準点Ｐｒと反射点の速度差から円周方向速度成分をキャンセルしたものに対応する。
［角速度算出動作］
本実施形態では、状態推定部３０にて物体６０の基準点Ｐｒの速度ベクトルを算出したうえで、物体６０の角速度を算出する。
（ｙ0cosθ－ｘ0sinθ）ω＝Ｖｒ－（Ｖｘ0cosθ＋Ｖｙ0sinθ） … (6)
(6)式において、左辺括弧内は、基準点Ｐｒから、反射点方位を傾きとする直線までの距離、換言すれば、基準点Ｐｒから反射点への円周方向の位置ずれに対応する。また、右辺は、反射点の相対速度Ｖｒから基準点Ｐｒの速度ベクトルの反射点方位における相対速度を差し引いた速度(以下、回転相対速度)、換言すれば、基準点Ｐｒと反射点の速度差から円周方向速度成分をキャンセルしたものに対応する。
［角速度算出動作］
本実施形態では、状態推定部３０にて物体６０の基準点Ｐｒの速度ベクトルを算出したうえで、物体６０の角速度を算出する。
【0040】
つまり、反射点距離算出部２１は、図６に示すように、センサ部１１にて検出された各反射点の方位を傾きとする直線と基準点Ｐｒとの距離を反射点距離として算出する。
なお、図６において、反射点距離は、基準点Ｐｒからみて、反射点方位の直線と直交する直線の長さを表している。しかし、図６に点線で示すように、方位測定の原点となるセンサ部１１から基準点Ｐｒまでの距離を半径とする円弧の長さを、反射点距離として求めるようにしてもよい。このようにしても、基準点Ｐｒから反射点方位の直線までの距離を求めることができる。すなわち、反射点距離は、反射点方位の直線と直交する直線の長さ、或いは、基準点Ｐｒまでの距離を半径とする円弧の長さとして、近似的に求めるようにすればよい。
また、回転相対速度算出部２５は、各反射点の相対速度Ｖｒから、図７に示す基準点Ｐｒの速度ベクトルの反射点方位における相対速度（Ｖｘ0cosθ＋Ｖｙ0sinθ）、を減じることで、回転相対速度を算出する。
なお、図６において、反射点距離は、基準点Ｐｒからみて、反射点方位の直線と直交する直線の長さを表している。しかし、図６に点線で示すように、方位測定の原点となるセンサ部１１から基準点Ｐｒまでの距離を半径とする円弧の長さを、反射点距離として求めるようにしてもよい。このようにしても、基準点Ｐｒから反射点方位の直線までの距離を求めることができる。すなわち、反射点距離は、反射点方位の直線と直交する直線の長さ、或いは、基準点Ｐｒまでの距離を半径とする円弧の長さとして、近似的に求めるようにすればよい。
また、回転相対速度算出部２５は、各反射点の相対速度Ｖｒから、図７に示す基準点Ｐｒの速度ベクトルの反射点方位における相対速度（Ｖｘ0cosθ＋Ｖｙ0sinθ）、を減じることで、回転相対速度を算出する。
【0041】
角速度算出部２７は、図８に×印で示すように、上述した反射点距離を横軸、回転相対速度を縦軸とする座標空間内で、各反射点に対応する位置を特定する。そして、図２に示す回帰処理部２７Ａにより、その座標空間における各反射点の分布から線形回帰を行い、各反射点の分布の傾きから物体６０の角速度ωを算出する。
【0042】
従って、角速度算出部２７は、カルマンフィルタなどのフィルタ処理を利用することなく物体６０の角速度を算出することができるようになり、物体６０の角速度を、応答遅れなく、瞬時に求めることができる。つまり、角速度算出部２７は、物体６０の瞬時角速度、延いては、瞬時ヨーレートを算出することができる。
【0043】
［制御処理］
次に、処理装置２０において、角速度算出機能及び状態推定機能を実現するために実行される制御処理について説明する。なお、この制御処理は、処理装置２０において、所定の処理周期で繰り返し実行される。
次に、処理装置２０において、角速度算出機能及び状態推定機能を実現するために実行される制御処理について説明する。なお、この制御処理は、処理装置２０において、所定の処理周期で繰り返し実行される。
【0044】
図９に示すように、処理装置２０において制御処理が開始されると、まずＳ１０（Ｓはステップを表す）にて、センサ部１１にセンサ波を送受信させることで、センサ部１１から、追跡対象となる物体６０の複数の観測値を取得する。なお、追跡対象となる物体６０は、車両５０の進行方向前方に位置する物体であり、後述のＳ６０にて実行される状態推定処理の推定結果により予め設定されている。
【0045】
次に、Ｓ２０では、Ｓ６０の状態推定処理にて前回推定された物体６０の基準点Ｐｒの速度ベクトルから、基準点Ｐｒの速度ベクトルを算出する、速度ベクトル算出部２３としての処理を実行する。
【0046】
Ｓ３０では、Ｓ１０にてセンサ部１１から取得した物体６０の複数の観測値に基づき、物体６０の基準点Ｐｒから、複数の反射点の反射点方位を傾きとする直線までの距離を、各反射点の反射点距離として算出する、反射点距離算出部２１としての処理を実行する。
【0047】
そして、Ｓ４０では、Ｓ１０にてセンサ部１１から取得した各反射点の相対速度と、Ｓ２０にて算出した速度ベクトルとに基づき、各反射点の相対速度から基準点Ｐｒの速度ベクトルの反射点方位における相対速度を差し引くことで、各反射点の回転相対速度を算出する。なお、Ｓ４０の処理は、回転相対速度算出部２５としての機能を実現する。
【0048】
こうして、センサ部１１にて検出された物体６０の反射点毎に、反射点距離及び回転相対速度が算出されると、Ｓ５０に移行し、上述した角速度算出原理に則って物体６０の角速度を算出する、角速度算出部２７としての処理を実行する。
【0049】
また、続くＳ６０では、Ｓ１０にてセンサ部１１から取得した観測値と、Ｓ５０にて算出された角速度（すなわち、瞬時角速度）と、Ｓ６０にて前回推定した物体６０の状態とに基づき、物体６０の最新の状態を推定する、状態推定部３０としての処理を実行する。
【0050】
そして、Ｓ６０にて状態推定処理が実行されると、Ｓ１０に移行することで、上記一連の処理を所定の処理周期で繰り返し実行する。
［角速度算出処理］
次に、Ｓ５０にて実行される角速度算出処理について、図１０～図１２を用いて説明する。
［角速度算出処理］
次に、Ｓ５０にて実行される角速度算出処理について、図１０～図１２を用いて説明する。
【0051】
図１０に示すように、角速度算出処理においては、まず、Ｓ１１０にて、センサ部１１にて検出された物体６０は、角速度の算出対象となる対象物体であるか否かを判定する、対象物体判定処理を実行する。この対象物体判定処理は、例えば、図１１に示す手順で実行される。
【0052】
すなわち、対象物体判定処理においては、図１１に示すように、まず、Ｓ２１０にて、物体６０と自車両５０との距離が、予め設定された近距離判定用の閾値以下であるか否かを判定する。そして、Ｓ２１０にて、距離は閾値以下であると判定されると、Ｓ２５０に移行して、物体６０は、角速度の算出を行わない非対象物体であると判定する。
【0053】
一方、Ｓ２１０にて、距離は閾値よりも大きいと判定されると、Ｓ２２０に移行し、センサ部１１にて検出された反射点の数が、予め設定された反射点数判定用の閾値以下であるか否かを判定する。Ｓ２２０にて、反射点の数は閾値以下であると判定されると、Ｓ２５０に移行して、物体６０は非対象物体であると判定し、Ｓ２２０にて、反射点の数は閾値よりも大きいと判定されると、Ｓ２４０に移行する。
【0054】
Ｓ２４０では、物体６０の加速度の絶対値が、予め設定された加減速判定用の閾値以上であるか否かを判定する。そして、Ｓ２４０にて、加速度の絶対値が閾値以上で、物体６０が加減速されていると判定されると、Ｓ２５０に移行して、物体６０は非対象物体であると判定する。また、Ｓ２４０にて、加速度が閾値よりも小さく、物体６０は加減速されていないと判定されると、Ｓ２４０に移行する。そして、Ｓ２４０では、物体６０は、角速度算出の対象物体であると判定し、当該対象物体判定処理を終了する。
【0055】
このように、対象物体判定処理においては、物体６０と自車両５０とが離れていて、センサ部１１にて検出された反射点の数が所定の閾値よりも多く、しかも、物体６０が加減速されていないときに、物体６０が対象物体であると判定する。これは、角速度を精度よく算出できない条件下では、物体６０は非対象物体として、角速度を算出しないようにするためである。
【0056】
つまり、物体６０と自車両５０との距離が短い場合には、物体６０からの反射波が広がり易く、受信信号に不要な信号が含まれて、角速度の算出精度が低下することが考えられる。また、反射点の数が少ない場合には、上述した線形回帰が不安定となって、角速度の算出精度が低下することが考えられる。また、物体６０が加減速されているときには、状態推定部３０としての状態推定処理にて得られる基準点Ｐｒの速度ベクトルが実際の速度ベクトルからずれて、角速度の算出精度が低下することが考えられる。
【0057】
そこで、本実施形態では、このような条件下では、物体６０は非対象物体であると判定して、角速度を算出しないようにしている。なお、物体６０が対象物体であるか非対象物体であるかの判定に用いる判定条件は、上記３つの判定条件に限定されるものではなく、上記３つの判定条件のうちの２つ或いは１つを用いるようにしてもよい。また、判定条件として、他の判定条件を加えるようにしてもよい。
【0058】
図１０に示すように、Ｓ１１０にて、上述した対象物体判定処理が実行されると、Ｓ１２０に移行し、対象物体判定処理にて、物体６０は処理対象（すなわち対象物体）であると判定されたか否かを判定する。そして、Ｓ１２０にて、物体６０は処理対象ではないと判定されると、以降の処理で角速度を算出することなく、当該角速度算出処理を終了する。
【0059】
一方、Ｓ１２０にて、物体６０は処理対象であると判定されると、Ｓ１３０に移行し、センサ部１１から取得した全ての反射点に対して、Ｓ１４０の対象反射点判定処理を実行したか否かを判定する。そして、Ｓ１３０にて、全ての反射点に対してＳ１４０の対象反射点判定処理を実行したと判定されると、Ｓ１７０に移行する。
【0060】
また、Ｓ１３０にて、全ての反射点に対してＳ１４０の対象反射点判定処理を実行していないと判定されると、Ｓ１４０に移行し、対象反射点判定処理を実行する。Ｓ１４０の対象反射点判定処理は、センサ部１１にて検出された物体６０の反射点が、角速度を算出するのに適した対象反射点であるか、角速度を算出するのに適さない非対象反射点であるかを判定する処理である。
【0061】
この対象反射点判定処理においては、センサ部１１にて検出された複数の反射点の中から、当該判定処理を実施していない反射点を取得し、図１２に示す手順で、その反射点が対象反射点か否かを判定する。
【0062】
すなわち、対象反射点判定処理においては、Ｓ３１０にて、反射点からの反射波の受信電力が、予め設定された受信電力判定用の閾値以下であるか否かを判定する。Ｓ３１０にて、反射点の受信電力が閾値以下であると判定されると、反射点からの受信信号が、マイクロドップラーやマルチパス等により、ノイズの影響を受けていることが考えられるので、Ｓ３５０に移行する。そして、Ｓ３５０では、反射点は、角速度の算出を行わない非対象反射点であると判定する。
【0063】
一方、Ｓ３１０にて、反射点の受信電力が閾値を超えていると判定されると、Ｓ３２０に移行し、反射点と基準点Ｐｒとの速度差が、予め設定された速度差判定用の閾値以上であるか否かを判定する。Ｓ３２０にて、反射点と基準点Ｐｒとの速度差が閾値以上であると判定されると、マイクロドップラー等により、反射点の速度が正常に検出できていないことが考えられるので、Ｓ３５０に移行して、反射点は非対象反射点であると判定する。
【0064】
また、Ｓ３２０にて、反射点と基準点Ｐｒとの速度差が閾値未満であると判定されると、Ｓ３３０に移行し、センサ部１１による反射点の方位推定で、反射波が複数波であったか否かを判定する。
【0065】
ここで、方位推定において複数波であるとは、方位推定処理において同一距離に複数の反射波を検知した状態である。例えば、角度ＦＦＴスペクトラムから閾値以上のピークの方位を抽出する方位推定処理において、複数のピークが閾値以上となった場合などがこれに該当する。
【0066】
そして、反射波の方位推定で反射波が複数波であった場合には、反射点の方位、換言すれば反射点の位置、の検出精度が低いことが考えられるので、Ｓ３５０に移行して、反射点は非対象反射点であると判定する。
【0067】
次に、Ｓ３３０にて、反射点の方位推定で反射波は複数波ではなかったと判定されると、Ｓ３４０に移行し、反射点は、角速度の算出に用いる対象反射点であると判定する。つまり、対象反射点判定処理では、反射点ごとに、受信電力が閾値を超えていて、基準点Ｐｒとの速度差が閾値未満であり、しかも、反射点からの反射波から反射点の方位を正常に推定できていることを確認する。そして、これら３つの条件が成立しているときに、反射点は、対象反射点であると判定する。
【0068】
なお、対象反射点判定処理においては、Ｓ３４０又はＳ３５０の処理実行後、当該対象反射点判定処理を終了して、図１０のＳ１５０に移行する。そして、次に対象反射点判定処理が開始されたときには、対象反射点であるか否かの判定を実施していない反射点について、上記と同様の手順で、対象反射点であるか否かを判定する。
【0069】
上記対象反射点判定処理においては、Ｓ３１０～Ｓ３３０の判定処理を実施することで、上記３つの判定条件が成立しているときに、反射点が対象反射点であると判定するようにしている。しかし、対象反射点判定処理においては、Ｓ３１０～Ｓ３３０の判定処理の少なくとも１つを実行するようにしてもよい。つまり、対象反射点判定処理においては、上記３つの判定条件の１つ、或いは２つが成立しているときに、反射点が対象反射点であると判定するようにされてもよい。
【0070】
次に、図１０に示すＳ１５０においては、Ｓ１４０の対象反射点判定処理にて、反射点は処理対象（すなわち対象反射点）であると判定されたか否かを判定する。そして、Ｓ１５０にて、反射点は処理対象ではないと判定されると、Ｓ１３０に移行する。
【0071】
また、Ｓ１５０にて、反射点は処理対象であると判定されると、Ｓ１６０に移行する。Ｓ１６０では、Ｓ１４０にて対象反射点であると判定された反射点の角速度算出用情報を、反射点情報として、ＲＡＭ等の記録媒体に格納し、Ｓ１３０に移行する。なお、反射点情報には、Ｓ３０の反射点距離算出処理及びＳ４０の回転相対速度算出処理にて算出された、反射点距離及び回転相対速度が含まれる。
【0072】
次に、Ｓ１３０にて、全ての反射点に対し対象反射点判定処理が実行されたと判定されたときに実行されるＳ１７０においては、Ｓ１６０にて記録媒体に格納された各反射点の反射点距離及び回転相対速度に基づき、物体６０の角速度を算出する。
【0073】
つまり、Ｓ１７０では、上述したように、反射点距離及び回転相対速度をパラメータとする座標空間内で、各反射点に対応する位置を特定し、線形回帰により各反射点の分布の傾きを求める、といった手順で、物体６０の角速度を算出する。そして、Ｓ１７０にて、角速度が算出されると、図９に示すＳ５０の角速度算出処理を終了し、Ｓ６０の状態推定処理を実行する。
【0074】
［状態推定処理］
Ｓ６０の状態推定処理は、例えば、図１３に示す手順で実行される。
すなわち、図１３に示すように、状態推定処理においては、まずＳ４１０にて、予測処理を実行する。この予測処理では、予め決められた物体６０の形状モデルと、過去の処理サイクル（例えば、前回の処理サイクル）において算出された推定値と、に基づいて、物体６０の輪郭（以下、予測輪郭）を予測する。
Ｓ６０の状態推定処理は、例えば、図１３に示す手順で実行される。
すなわち、図１３に示すように、状態推定処理においては、まずＳ４１０にて、予測処理を実行する。この予測処理では、予め決められた物体６０の形状モデルと、過去の処理サイクル（例えば、前回の処理サイクル）において算出された推定値と、に基づいて、物体６０の輪郭（以下、予測輪郭）を予測する。
【0075】
なお、物体６０の形状モデルは、例えば、円形モデル、楕円形モデル、矩形モデル、など、予め用意されている複数のモデルの中から、複数の反射点の観測値から推定される物体６０の大きさ及び形状に適したモデルを選択することにより決定される。
【0076】
予測輪郭は、過去の推定値である基準点Ｐｒの状態から予測される、物標の存在領域に対応する。
次に、Ｓ４２０では、Ｓ４１０にて算出された複数の予測値の各々を、Ｓ１０にてセンサ部１１から取得した観測値と関連付ける、関連付け処理を実行し、関連付けセットを生成する。
次に、Ｓ４２０では、Ｓ４１０にて算出された複数の予測値の各々を、Ｓ１０にてセンサ部１１から取得した観測値と関連付ける、関連付け処理を実行し、関連付けセットを生成する。
【0077】
次に、Ｓ４３０では、Ｓ４０で算出された関連付けセットにカルマンフィルタ等のフィルタを適用して、現在の推定値Ｐ２を算出する、推定処理を実行する。
また、Ｓ４３０の推定処理では、予測値に関連付けした観測値による推定値の更新量を、非線形フィルタである拡張カルマンフィルタなどを用いて算出する。そして、予測値と推定値の更新量から、推定値を更新する。なお、この更新された推定値が、今回の処理サイクルにおける推定値になる。また、推定値には少なくとも基準点Ｐｒの速度ベクトルを表すパラメータを含む。例えば、任意の座標系における縦速度と横速度、または、速度の大きさと向きのいずれかの組み合わせがこれに該当する。
また、Ｓ４３０の推定処理では、予測値に関連付けした観測値による推定値の更新量を、非線形フィルタである拡張カルマンフィルタなどを用いて算出する。そして、予測値と推定値の更新量から、推定値を更新する。なお、この更新された推定値が、今回の処理サイクルにおける推定値になる。また、推定値には少なくとも基準点Ｐｒの速度ベクトルを表すパラメータを含む。例えば、任意の座標系における縦速度と横速度、または、速度の大きさと向きのいずれかの組み合わせがこれに該当する。
【0078】
次に、Ｓ４４０では、Ｓ４３０の推定処理にて更新された推定値に含まれる基準点Ｐｒの角速度（以下、角速度推定値）と、Ｓ５０の角速度算出処理にて算出された角速度（以下、角速度算出値）とを混合する角速度混合処理を実行する。
【0079】
この角速度混合処理は、例えば、値１よりも小さい係数αを含む次式(7)を用いて、角速度推定値と角速度算出値とを混合し、角速度推定値を更新する。なお、次式(7)において、ωfilは角速度推定値を表し、ωobsは角速度算出値を表す。
【0080】
ωfil＝（１－α）・ωfil＋α・ωobs … (7)
そして、Ｓ４４０の角速度混合処理が実施されると、Ｓ６０の状態推定処理を終了し、図９のＳ１０に移行することで、次回の処理サイクルにおける角速度算出及び状態推定を開始する。
そして、Ｓ４４０の角速度混合処理が実施されると、Ｓ６０の状態推定処理を終了し、図９のＳ１０に移行することで、次回の処理サイクルにおける角速度算出及び状態推定を開始する。
【0081】
［効果］
以上説明したように、本実施形態の推定装置１０においては、角速度算出部２７が、物体６０の少なくとも１つの反射点の基準点Ｐｒとの間の反射点距離、及び、その反射点の回転相対速度に基づき、物体６０の角速度を算出する。
以上説明したように、本実施形態の推定装置１０においては、角速度算出部２７が、物体６０の少なくとも１つの反射点の基準点Ｐｒとの間の反射点距離、及び、その反射点の回転相対速度に基づき、物体６０の角速度を算出する。
【0082】
このため、本実施形態の角速度算出部２７によれば、道路形状を利用することなく、しかも、カルマンフィルタなどのフィルタ処理を利用することなく、物体６０の角速度（すなわち、角速度算出値）を算出することができる。従って、物体６０の角速度を、物体６０の実際の移動状態に対応して、正確に、しかも、応答遅れなく算出することができる。
【0083】
また、角速度算出部２７は、対象物体判定処理を実行することで、自車両５０との距離が閾値以下、センサ部１１にて検出された反射点の数が閾値以下、物体の加速度の絶対値が閾値以上、といった条件の何れかを満たすか否かを判定する。そして、これらの条件の何れかが成立すると、角速度の算出精度が低下すると判定して、その物体６０の角速度の算出を行わないようにされている。
【0084】
また、角速度算出部２７は、対象物体判定処理にて対象物体であると判定された物体６０については、対象反射点判定処理を実行する。そして、対象反射点判定処理では、センサ部１１にて検出された反射点のうち、受信電力が閾値以下である反射点、基準点Ｐｒとの速度差が閾値以上である反射点、方位推定で反射波が複数波であった反射点を、角速度の算出対象から除外する。
【0085】
従って、角速度算出部２７は、センサ部１１にて検出された複数の反射点の中から、センサ部１１にて反射点からの反射信号が良好に受信されて、反射点の位置や速度が精度よく検出された反射点を選択することができる。そして、角速度は、その選択された反射点の反射点距離及び回転相対速度に基づき算出されることから、角速度の算出精度を高めることができる。
【0086】
また更に、本実施形態では、角速度算出部２７による角速度の算出結果（すなわち、角速度算出値）は、状態推定部３０にて推定された物体６０の状態推定結果に反映される。具体的には、上述した角速度混合処理において、状態推定部３０にて推定された物体６０の状態のうち、角速度推定値を、角速度算出値にて補正する。
【0087】
この結果、カルマンフィルタなどのフィルタ処理を利用して推定される角速度推定値を、瞬時値である角速度算出値を用いて補正することができ、最終的に得られる車両状態の推定結果に含まれる推定遅れを、抑制することができる。この結果、車両５０において、その推定結果から、自車両５０をより適正に制御することができるようになる。
【0088】
［変形例］
上記実施形態では、角速度算出部２７にて算出された角速度算出値を状態推定部３０による車両状態の推定結果に反映させる際の第１例として、角速度混合処理において、角速度推定値と角速度算出値とを混合して、角速度推定値を補正することを説明した。
上記実施形態では、角速度算出部２７にて算出された角速度算出値を状態推定部３０による車両状態の推定結果に反映させる際の第１例として、角速度混合処理において、角速度推定値と角速度算出値とを混合して、角速度推定値を補正することを説明した。
【0089】
しかし、車両状態の推定結果に角速度算出値を反映させて、車両状態の推定をより適正に実施できるようにするには、必ずしも、状態推定部３０にて得られた角速度推定値を角速度算出値にて補正する必要はない。
【0090】
そこで、本変形例では、角速度算出部２７にて算出された角速度算出値を状態推定部３０による車両状態の推定結果に反映させる手法の第２例、及び、第３例について説明する。
【0091】
図１４に示すように、第２例の状態推定処理においては、まずＳ４００にて、Ｓ５０の角速度算出処理で算出された角速度算出値の絶対値が、予め設定された閾値以上であるか否かを判定する。そして、Ｓ４００にて、角速度算出値の絶対値が閾値以上であると判定された場合、つまり、物体６０のヨーレートが大きい場合には、Ｓ４０５に移行して、フィルタゲインを初期値から増加させて、Ｓ４１０に移行する。
【0092】
一方、Ｓ４００にて、角速度算出値の絶対値が閾値未満であると判定された場合、つまり、物体６０のヨーレートが小さい場合には、フィルタゲインを増加させることなく、Ｓ４１０に移行する。
【0093】
これは、物体６０の角速度が大きい場合には、Ｓ４１０による予測の信頼度が低くなると考えられるので、図１５に示すように、各反射点の観測値を信じ、物体６０の角速度が小さい場合に比べて、フィルタゲインを大きくして、追従性を向上するためである。
【0094】
そして、Ｓ４１０以降は、図１３に示したＳ４１０～Ｓ４３０の処理を実行し、当該状態推定処理を終了する。
このように、第２例の状態推定処理においては、角速度算出部２７で算出された角速度算出値に基づき、状態推定部３０で用いられるフィルタゲインを補正する。このようにしても、状態推定部３０による物体６０の状態推定精度を高めることができる。
このように、第２例の状態推定処理においては、角速度算出部２７で算出された角速度算出値に基づき、状態推定部３０で用いられるフィルタゲインを補正する。このようにしても、状態推定部３０による物体６０の状態推定精度を高めることができる。
【0095】
次に、第３例の状態推定処理においては、図１６に示すように、まずＳ４１０にて、上述した予測処理を実行した後、Ｓ４１５にて、速度ベクトル予測補正処理を行い、Ｓ４２０に移行する。
【0096】
速度ベクトル予測補正処理では、図１７に示すように、物体６０の基準点Ｐｒの速度の向きの変化量（以下、速度向き変化量）を、角速度算出部２７で算出された角速度算出値に基づき補正することで、今回の処理サイクルの速度ベクトル予測値を補正する。
【0097】
そして、Ｓ４２０以降は、図１３に示したＳ４２０、Ｓ４３０の処理を実行し、当該状態推定処理を終了する。
このように、第３例の状態推定処理においては、前回の処理サイクルで推定した基準点Ｐｒの速度の速度向き変化量を、角速度算出値に基づき補正することで、速度ベクトル予測値を補正することから、物体６０の基準点Ｐｒの速度や進行方向をより適正に推定することができる。よって、状態推定部３０による物体６０の状態推定精度を高めることができる。
このように、第３例の状態推定処理においては、前回の処理サイクルで推定した基準点Ｐｒの速度の速度向き変化量を、角速度算出値に基づき補正することで、速度ベクトル予測値を補正することから、物体６０の基準点Ｐｒの速度や進行方向をより適正に推定することができる。よって、状態推定部３０による物体６０の状態推定精度を高めることができる。
【0098】
なお、第２例におけるフィルタゲインの補正、及び、第３例の速度ベクトル予測値の補正は、第１例の角速度推定値の補正など、他の補正と組み合わせて実施するようにしてもよい。
［第２実施形態］
第２実施形態は、基本的な構成は第１実施形態と同様であるため、第１実施形態との相違点について以下に説明する。なお、第１実施形態と同じ符号は、同一の構成を示すものであって、先行する説明を参照する。
［第２実施形態］
第２実施形態は、基本的な構成は第１実施形態と同様であるため、第１実施形態との相違点について以下に説明する。なお、第１実施形態と同じ符号は、同一の構成を示すものであって、先行する説明を参照する。
【0099】
第２実施形態と第１実施形態との相違点は、図２に点線で示すように、処理装置２０に、衝突判定部３２としての機能が設けられている点である。そして、この衝突判定部３２としての機能は、処理装置２０にて実行される角速度算出処理により実現される。
【0100】
すなわち、図１８に示すように、本実施形態の角速度算出処理においては、Ｓ１７０にて角速度が算出されるか、或いは、Ｓ１２０にて物体６０は処理対象ではないと判定されると、Ｓ１８０にて衝突判定処理を実施してから、角速度算出処理を終了する。
【0101】
図１９に示すように、Ｓ１８０の衝突判定処理においては、まずＳ５１０にて、状態推定部３０による状態推定処理にて推定された物体６０の速度ベクトルの向き、詳しくは基準点Ｐｒの進行方向が、自車両５０の進路と交差しているか否かを判定する。そして、物体６０の速度ベクトルの向きが自車両５０の進路と交差していない場合には、Ｓ５６０にて、自車両５０は物体６０と衝突しない、「非衝突」であると判定して、当該衝突判定処理を終了する。
【0102】
次に、Ｓ５１０にて、物体６０の速度ベクトルの向きが自車両５０の進路と交差していると判定されると、Ｓ５２０に移行する。Ｓ５２０では、自車両５０と物体６０との距離を速度の差（すなわち、相対速度）で割ることで算出される衝突までの時間（以下、ＴＴＣ）が、予め設定された衝突判定用の閾値以下であるか否かを判定する。なお、ＴＴＣは、Time to Collisionの略である。
【0103】
Ｓ５２０にて、ＴＴＣは閾値よりも大きいと判定されると、Ｓ５６０にて、「非衝突」と判定して、当該衝突判定処理を終了し、Ｓ５２０にて、ＴＴＣは閾値以下であると判定されると、Ｓ５３０に移行する。
【0104】
Ｓ５３０では、当該角速度算出処理において、物体６０の角速度を算出したか否かを判定する。そして、Ｓ５３０にて、角速度は算出していないと判定すると、自車両５０は物体６０と衝突する可能性が高いので、Ｓ５５０に移行して、「衝突」と判定し、当該衝突判定処理を終了する。
【0105】
また、Ｓ５３０にて、角速度は算出したと判定すると、Ｓ５４０に移行して、算出した角速度の絶対値は、衝突判定用の閾値以下であるか否かを判定する。そして、角速度の絶対値が、閾値以下である場合には、自車両５０は物体６０と衝突する可能性が高いので、Ｓ５５０に移行し、「衝突」と判定して、当該衝突判定処理を終了する。
【0106】
一方、Ｓ５４０にて、角速度の絶対値が閾値よりも大きいと判定されたときには、物体６０が進路変更されて、自車両５０が物体６０に衝突する可能性が低下したと判定して、Ｓ５６０に移行し、「非衝突」と判定して、当該衝突判定処理を終了する。
【0107】
このように、本実施形態の推定装置１０においては、第１実施形態の推定装置１０と同様に、周囲の物体６０の瞬時加速度の算出、及び、物体６０の状態推定を実施することができるだけでなく、その推定結果に基づき、物体６０に衝突するか否かを判定できる。
【0108】
また、その衝突判定に、角速度算出処理にて算出した瞬時角速度を使用するため、ＴＴＣにより、衝突する可能性が高いと判定された物体６０の瞬時角速度が大きく、衝突する可能性が低くなっている場合に、「衝突」を誤判定するのを抑制できる。従って、「衝突」の誤判定によって、車両５０の乗員や自動運転装置に警報が出力されて、車両５０の操舵装置が誤制御されるのを抑制できる。
【0109】
［他の実施形態］
以上、本開示の実施形態について説明したが、本開示は上述の実施形態に限定されることなく、種々変形して実施することができる。
以上、本開示の実施形態について説明したが、本開示は上述の実施形態に限定されることなく、種々変形して実施することができる。
【0110】
上記実施形態では、状態推定部３０は、文献Ａに記載の拡張物体追跡技術に従い、物体の形状をモデル化して、角速度を含む状態推定を行うものとして説明したが、物体の状態推定は、必ずしも上記実施形態に記載の手順で実施する必要はなく、適宜変更することができる。そして、その場合でも、状態推定部３０による角速度などの推定結果を、上述した角速度算出値にて補正することで、物体の追跡精度を高めることができる。
【0111】
上記実施形態では、推定装置１０は、自動車などの車両５０に搭載されるものとして説明したが、自動車以外の移動体に搭載されていてもよい。例えば、推定装置１０は、船舶、航空機、自動二輪車、ドローンなどの移動体に搭載されていてもよい。
【0112】
本開示に記載の推定装置１０及びその手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の推定装置１０及びその手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより、実現されてもよい。もしくは、本開示に記載の推定装置１０及びその手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組み合わせにより構成された一つ以上の専用コンピュータにより、実現されてもよい。また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されてもよい。推定装置１０に含まれる各部の機能を実現する手法には、必ずしもソフトウェアが含まれている必要はなく、その全部の機能が、一つあるいは複数のハードウェアを用いて実現されてもよい。
【0113】
上記実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加又は置換してもよい。
【0114】
上述した推定装置１０の他、当該推定装置１０を構成要素とするシステム、当該推定装置としてコンピュータを機能させるためのプログラム、このプログラムを記録した半導体メモリ等の非遷移的実体的記録媒体、物体の推定方法など、種々の形態で本開示を実現することもできる。
【0115】
［本明細書が開示する技術思想］
［項目１］
移動体に搭載されて周囲の物体の状態を推定する推定装置であって、
周囲にセンサ波を送信し、その反射波から前記物体上の複数の反射点の位置及び相対速度を検出するように構成されたセンサ部（１１）と、
前記物体の任意の基準点の速度ベクトルを算出するように構成された速度ベクトル算出部（２３）と、
前記基準点から、前記センサ部にて検出された前記反射点方位を傾きとする直線までの距離を、反射点距離として算出するように構成された反射点距離算出部（２１）と、
前記センサ部にて検出された前記反射点の相対速度から、前記速度ベクトル算出部にて算出された基準点の前記速度ベクトルによる前記反射点方位における相対速度を差し引いた値である、前記反射点の回転相対速度を算出するように構成された回転相対速度算出部（２５）と、
前記反射点距離算出部及び前記回転相対速度算出部にて算出された、少なくとも１つの前記反射点の前記反射点距離及び前記回転相対速度に基づき、前記物体の角速度を算出するように構成された角速度算出部（２７）と、
を備えている、推定装置。
［項目１］
移動体に搭載されて周囲の物体の状態を推定する推定装置であって、
周囲にセンサ波を送信し、その反射波から前記物体上の複数の反射点の位置及び相対速度を検出するように構成されたセンサ部（１１）と、
前記物体の任意の基準点の速度ベクトルを算出するように構成された速度ベクトル算出部（２３）と、
前記基準点から、前記センサ部にて検出された前記反射点方位を傾きとする直線までの距離を、反射点距離として算出するように構成された反射点距離算出部（２１）と、
前記センサ部にて検出された前記反射点の相対速度から、前記速度ベクトル算出部にて算出された基準点の前記速度ベクトルによる前記反射点方位における相対速度を差し引いた値である、前記反射点の回転相対速度を算出するように構成された回転相対速度算出部（２５）と、
前記反射点距離算出部及び前記回転相対速度算出部にて算出された、少なくとも１つの前記反射点の前記反射点距離及び前記回転相対速度に基づき、前記物体の角速度を算出するように構成された角速度算出部（２７）と、
を備えている、推定装置。
【0116】
［項目２］
項目１に記載の推定装置であって、
前記角速度算出部は、少なくとも１つの前記反射点についての前記反射点距離に対する前記回転相対速度の大きさに基づき、前記角速度を算出するように構成されている、推定装置。
項目１に記載の推定装置であって、
前記角速度算出部は、少なくとも１つの前記反射点についての前記反射点距離に対する前記回転相対速度の大きさに基づき、前記角速度を算出するように構成されている、推定装置。
【0117】
［項目３］
項目２に記載の推定装置であって、
前記角速度算出部は、
前記反射点距離と前記回転相対速度をパラメータとする座標空間における前記反射点の分布から線形回帰を行い、前記座標空間内での傾きを算出するように構成された回帰処理部（２７Ａ）を備え、
前記回帰処理部にて算出された傾きから前記角速度を算出するように構成されている、推定装置。
項目２に記載の推定装置であって、
前記角速度算出部は、
前記反射点距離と前記回転相対速度をパラメータとする座標空間における前記反射点の分布から線形回帰を行い、前記座標空間内での傾きを算出するように構成された回帰処理部（２７Ａ）を備え、
前記回帰処理部にて算出された傾きから前記角速度を算出するように構成されている、推定装置。
【0118】
［項目４］
項目１～項目３の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記センサ部にて検出された複数の前記反射点について、下記の判定条件ａ）～ｃ）の少なくとも１つに基づき除外判定を行い、
ａ）前記反射波の受信電力が閾値以下である。
項目１～項目３の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記センサ部にて検出された複数の前記反射点について、下記の判定条件ａ）～ｃ）の少なくとも１つに基づき除外判定を行い、
ａ）前記反射波の受信電力が閾値以下である。
【0119】
ｂ）前記基準点との速度差が閾値以上である。
ｃ）前記反射波が複数波である。
前記除外判定にて前記判定条件ａ）～ｃ）の少なくとも１つに適合すると判定された前記反射点については、前記角速度の算出対象から除外するように構成されている、推定装置。
ｃ）前記反射波が複数波である。
前記除外判定にて前記判定条件ａ）～ｃ）の少なくとも１つに適合すると判定された前記反射点については、前記角速度の算出対象から除外するように構成されている、推定装置。
【0120】
［項目５］
項目１～項目４の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記物体との距離が所定の値以下、前記反射点の数が所定値以下、前記物体の加速度の絶対値が閾値以上、の何れかの条件を満たす場合は、前記角速度の算出を行わない、又は、前記角速度の算出結果を出力しない、ように構成されている、推定装置。
項目１～項目４の何れか１項に記載の推定装置であって、
前記角速度算出部は、
前記物体との距離が所定の値以下、前記反射点の数が所定値以下、前記物体の加速度の絶対値が閾値以上、の何れかの条件を満たす場合は、前記角速度の算出を行わない、又は、前記角速度の算出結果を出力しない、ように構成されている、推定装置。
【0121】
［項目６］
項目１～項目５の何れか１項に記載の推定装置であって、
前記複数の反射点に対応する前記物体について、前記物体の状態を推定するように構成された状態推定部（３０）を備え、
前記状態推定部は、前記物体の状態推定結果に、前記角速度算出部にて算出された前記角速度の算出値である角速度算出値を反映させるように構成されている、推定装置。
項目１～項目５の何れか１項に記載の推定装置であって、
前記複数の反射点に対応する前記物体について、前記物体の状態を推定するように構成された状態推定部（３０）を備え、
前記状態推定部は、前記物体の状態推定結果に、前記角速度算出部にて算出された前記角速度の算出値である角速度算出値を反映させるように構成されている、推定装置。
【0122】
［項目７］
項目６に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記物体の状態推定結果に含まれる前記角速度の推定結果である角速度推定値と、前記角速度算出部にて算出された前記角速度算出値とを混合し、前記角速度推定値を更新するように構成されている、推定装置。
項目６に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記物体の状態推定結果に含まれる前記角速度の推定結果である角速度推定値と、前記角速度算出部にて算出された前記角速度算出値とを混合し、前記角速度推定値を更新するように構成されている、推定装置。
【0123】
［項目８］
項目６又は項目７に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記角速度算出部にて算出された前記角速度算出値の絶対値が所定の値以上の場合に、前記状態推定部にて前記物体の状態を推定するのに用いられるフィルタのフィルタゲインを増加させるように構成されている、推定装置。
項目６又は項目７に記載の推定装置であって、
前記状態推定部は、前記角速度算出部とは異なる手法で前記角速度を含む状態を推定するように構成され、前記角速度算出部にて算出された前記角速度算出値の絶対値が所定の値以上の場合に、前記状態推定部にて前記物体の状態を推定するのに用いられるフィルタのフィルタゲインを増加させるように構成されている、推定装置。
【0124】
［項目９］
項目６～項目８の何れか１項に記載の推定装置であって、
前記状態推定部は、前記状態推定部にて前記物体の状態の１つとして推定された前記物体の過去の速度ベクトルに基づき算出された、前記物体の現在の速度ベクトル予測値を、前記角速度算出部にて算出された前記角速度算出値にて補正するように構成されている、推定装置。
項目６～項目８の何れか１項に記載の推定装置であって、
前記状態推定部は、前記状態推定部にて前記物体の状態の１つとして推定された前記物体の過去の速度ベクトルに基づき算出された、前記物体の現在の速度ベクトル予測値を、前記角速度算出部にて算出された前記角速度算出値にて補正するように構成されている、推定装置。
【0125】
［項目１０］
項目１～項目９の何れか１項に記載の推定装置であって、
前記角速度算出部にて算出された前記角速度算出値を用いて、前記移動体と前記物体との衝突判定を行うように構成された衝突判定部（３２）を備えている、推定装置。
項目１～項目９の何れか１項に記載の推定装置であって、
前記角速度算出部にて算出された前記角速度算出値を用いて、前記移動体と前記物体との衝突判定を行うように構成された衝突判定部（３２）を備えている、推定装置。
【符号の説明】
【0126】
１０…推定装置、１１…センサ部、２１…反射点距離算出部、２３…速度ベクトル算出部、２５…回転相対速度算出部、２７…角速度算出部、２７Ａ…回帰処理部、３０…状態推定部、３２…衝突判定部。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
