(57)【要約】  高精度に観測対象の状態を推定することができる状態推定装置を提供することを目的とする。  状態推定装置１は、ＬＩＤＡＲ２による対象車両の測定データを状態推定用モデルに当てはめて演算するカルマンフィルタ更新処理を行うことで、自車両の周辺に存在する車両の状態を推定するものである。そして、状態推定装置１は、対象車両との位置関係や対象車両の状態に基づいて、カルマンフィルタ更新処理に用いる状態推定用モデルを変更する。
2012117528.tif 000002
【特許請求の範囲】
【請求項１】
観測対象を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて前記観測対象の状態を推定する状態推定装置であって、
前記観測対象との位置関係又は前記観測対象の状態に基づいて、前記状態推定用モデルを変更する変更手段を有する、状態推定装置。
【請求項２】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記測定装置に対する前記観測対象の中心位置の方向に基づいて、前記状態推定用モデルを変更する、請求項１に記載の状態推定装置。
【請求項３】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記観測対象の向きに基づいて、前記状態推定用モデルを変更する、請求項１に記載の状態推定装置。
【請求項４】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記測定装置に対する前記観測対象の中心位置の方向、及び、前記観測対象の向き、の双方に基づいて、前記状態推定用モデルを変更する、請求項１に記載の状態推定装置。
【請求項５】
前記変更手段は、前回の推定で用いた状態推定用モデルに基づいて、前記測定データを当てはめる状態推定用モデルを絞り込む、請求項１〜４の何れか一項に記載の状態推定装置。
【請求項６】
前記変更手段は、前回推定した前記観測対象の状態に基づいて、前記測定装置に対する前記観測対象の中心位置の方向、又は、前記観測対象の向きを推定する、請求項２〜５の何れか一項に記載の状態推定装置。
【請求項７】
前記変更手段は、前記観測対象が存在する位置の地図情報に基づいて、前記観測対象の向きを推定する、請求項３又は４に記載の状態推定装置。
【請求項８】
前記変更手段は、測定データから前記観測対象のモデルを生成し、前記モデルを構成する辺の数に基づいて、前記状態推定用モデルを変更する、請求項１〜７の何れか一項に記載の状態推定装置。
【請求項９】
前記状態推定用モデルは、前記測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、
前記変更手段は、前記観測対象の面に対する向きに基づいて、前記観測ノイズモデルの分散値を変更する、請求項１〜８の何れか一項に記載の状態推定装置。
【請求項１０】
前記変更手段は、前記観測対象までの距離に基づいて、前記観測ノイズモデルを変更する、請求項９に記載の状態推定装置。
【請求項１１】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記状態推定用モデルは、前記周辺車両の運動状態を表す運動モデルと、前記運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、
前記変更手段は、前記観測対象の速度が高いと、前記観測対象の速度が低いときに比べて、前記運動ノイズモデルにおけるステアリング角度の変化量を小さくする、請求項１〜１０の何れか一項に記載の状態推定装置。
【請求項１２】
複数の異なる前記状態推定用モデルを用いて、前記観測対象の状態を推定するとともに、前記観測対象の状態の推定分散値を算出し、前記推定分散値が最も小さい前記観測対象の状態を出力する、請求項１〜１１の何れか一項に記載の状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測定データを状態推定用モデルに当てはめることにより観測対象の状態を推定する推定装置に関する。
【背景技術】
【０００２】
従来から、動的な観測対象の状態を推定する技術として、特開２００２−２５９９６６号公報に記載された装置が知られている。特開２００２−２５９９６６号公報に記載された装置は、複数の認識手段を備え、所定条件に従って認識手法を切り替えることで、推定の高精度化を図るものである。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００２−２５９９６６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしならが、特開２００２−２５９９６６号公報に記載された技術であっても、十分な推定精度を得ることができないため、更に高精度な推定手法が求められている。【０００５】
そこで、近年は、カルマンフィルタ（Kalman Filter）などのフィルタを用いた状態推定手法が採用されるようになってきた。カルマンフィルタでは、まず、観測モデル、観測ノイズモデル、運動モデル及び運動ノイズモデルなどの状態推定用モデルを設定しておく。そして、カルマンフィルタでは、この設定した状態推定用モデルに観測対象の測定データを当てはめることにより、動的な観測対象の状態を高精度に推定する。【０００６】
しかしながら、従来のカルマンフィルタを用いた状態推定手法では、観測対象の状態が時々刻々と変化するにもかかわらず、状態推定用モデルが固定されているため、必ずしも高精度に観測対象の状態を推定することができないという問題がある。【０００７】
そこで、本発明は、より高精度に観測対象の状態を推定することができる状態推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る状態推定装置は、観測対象を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて観測対象の状態を推定する状態推定装置であって、観測対象との位置関係又は観測対象の状態に基づいて、状態推定用モデルを変更する変更手段を有する。【０００９】
本発明に係る状態推定装置によれば、観測対象との位置関係又は観測対象の状態に基づいて状態推定用モデルを変更するため、動的な観測対象の状態をより高精度に推定することができる。【００１０】
この場合、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、測定装置に対する観測対象の中心位置の方向に基づいて、状態推定用モデルを変更することが好ましい。測定装置に対する観測対象の中心位置の方向が異なると、観測対象の測定可能面が異なる。このため、測定装置に対する観測対象の中心位置の方向に関わらず同一の状態推定用モデルを用いると、測定データと状態推定用モデルとの対応付けを適切に行うことができない。その結果、観測対象の状態を高精度に推定できなくなる。そこで、測定装置に対する観測対象の中心位置の方向に基づいて状態推定用モデルを変更することで、測定データと状態推定用モデルとを適切に対応付けることができる。これにより、観測対象の状態の推定精度を更に向上させることができる。【００１１】
また、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、観測対象の向きに基づいて、状態推定用モデルを変更することが好ましい。観測対象の向きが異なると、観測対象の測定可能面が異なる。このため、観測対象の向きに関わらず同一の状態推定用モデルを用いると、測定データと状態推定用モデルとの対応付けを適切に行うことができなくなる。その結果、観測対象の状態を高精度に推定することができない。そこで、観測対象の向きに基づいて状態推定用モデルを変更することで、測定データと状態推定用モデルとを適切に対応付けることができるため、観測対象の状態の推定精度を更に向上させることができる。【００１２】
また、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、測定装置に対する観測対象の中心位置の方向、及び、観測対象の向き、の双方に基づいて、状態推定用モデルを変更することが好ましい。測定装置に対する観測対象の中心位置の方向、及び、観測対象の向きの双方により、観測対象の自車両に対向する面を特定することができる。このため、これら双方の情報に基づいて状態推定用モデルを変更することで、測定データと状態推定用モデルとを適切に対応付けることができるため、観測対象の状態の推定精度を更に向上させることができる。【００１３】
また、変更手段は、前回の推定で用いた状態推定用モデルに基づいて、測定データを当てはめる状態推定用モデルを絞り込むことが好ましい。通常、観測対象の挙動変化は連続的であることから、前回の推定で用いた状態推定用モデルに基づいて状態推定用モデルを絞り込むことで、誤った状態推定用モデルが選択されるのを低減することができる。【００１４】
また、変更手段は、前回推定した観測対象の状態に基づいて、測定装置に対する観測対象の中心位置の方向、又は、観測対象の向きを推定することが好ましい。このように、前回推定した情報を利用することで推定の連続性が保たれるため、観測対象の状態の推定精度を更に向上させることができる。【００１５】
また、変更手段は、観測対象が存在する位置の地図情報に基づいて、観測対象の向きを推定することが好ましい。観測対象が静止している場合や観測対象を検出した直後の場合などは、測定データによって観測対象の向きを求めることができない。そこで、観測対象が存在する位置の地図情報を利用することで、このような場合であっても、観測対象の向きを推定することができる。【００１６】
また、変更手段は、測定データから観測対象のモデルを生成し、モデルを構成する辺の数に基づいて、状態推定用モデルを変更することが好ましい。このように、測定データから生成されるモデルの辺の数に基づいて状態推定用モデルを変更することで、状態推定用モデルの変更基準が明確化されるため、観測対象の状態の推定精度を更に向上させることができる。【００１７】
また、状態推定用モデルは、測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、変更手段は、観測対象の面に対する向きに基づいて、観測ノイズモデルの分散値を変更することが好ましい。通常、観測対象の面に垂直な方向には測定データの観測ノイズが小さく、観測対象の面に水平な方向には測定データの観測ノイズが大きい。そこで、測定対象の面に対する向きに基づいて観測ノイズモデルの分散値を変更することで、観測対象の状態の推定精度を更に向上させることができる。【００１８】
また、変更手段は、観測対象までの距離に基づいて、観測ノイズモデルを変更することが好ましい。観測対象までの距離が近いと、観測対象の測定対象領域が大きいため、観測ノイズが小さくなる。一方、観測対象までの距離が遠いと、観測対象の測定対象領域が小さいため、観測ノイズが大きくなる。そこで、測定対象までの距離に応じて観測ノイズモデルを変更することで、観測対象の状態の推定精度を更に向上させることができる。【００１９】
また、観測対象は、測定装置の周辺に存在する車両であり、状態推定用モデルは、周辺車両の運動状態を表す運動モデルと、運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、変更手段は、観測対象の速度が高いと、観測対象の速度が低いときに比べて、運動ノイズモデルにおけるステアリング角度の変化量を小さくすることが好ましい。通常、車両の速度が速いと、ステアリングを大きく切る可能性が低い。そこで、観測対象の速度が高いと運動ノイズモデルにおけるステアリング角度の変化量を小さくすることで、観測対象の状態の推定精度を更に向上させることができる。【００２０】
また、複数の異なる状態推定用モデルを用いて、観測対象の状態を推定するとともに、観測対象の状態の推定分散値を算出し、推定分散値が最も小さい観測対象の状態を出力することが好ましい。これにより、観測対象との位置関係や観測対象の状態が不明な場合であっても、適切な状態推定用モデルを用いて推定した観測対象の状態を出力することができる。
【発明の効果】
【００２１】
本発明によれば、高精度に観測対象の状態を推定することができる。
【図面の簡単な説明】
【００２２】
【図１】本実施形態に係る状態推定装置を示したブロック図である。
【図２】推定したい変数を示した図である。
【図３】第１の実施形態に係る状態推定装置の推定処理を示した図である。
【図４】重心位置の方位角と重心位置の速度向きとを示す図である。
【図５】観測モデルの変更基準例を示す図である。
【図６】右斜後面観測モデルを説明するための図である。
【図７】後面観測モデルを説明するための図である。
【図８】第２の実施形態に係る状態推定装置の推定処理を示した図である。
【図９】第３の実施形態に係る状態推定装置の推定処理を示した図である。
【図１０】第４の実施形態に係る状態推定装置の推定処理を示した図である。
【図１１】第５の実施形態に係る状態推定装置の推定処理を示した図である。
【図１２】図１１のモデル選択処理を示した図である。
【図１３】第６の実施形態に係る状態推定装置の推定処理を示した図である。
【図１４】対象車両とグルーピング点群データとの関係を示した図である。
【図１５】観測ノイズモデルの概念を示した図である。
【図１６】第７の実施形態に係る状態推定装置の推定処理を示した図である。
【図１７】第８の実施形態に係る状態推定装置の推定処理を示した図である。
【図１８】第９の実施形態に係る状態推定装置の推定処理を示した図である。
【発明を実施するための形態】
【００２３】
以下、図面を参照して、本発明に係る状態推定装置の好適な実施形態について詳細に説明する。なお、全図中、同一又は相当部分には同一符号を付すこととする。
【００２４】
図１は、本実施形態に係る状態推定装置を示したブロック図である。本実施形態に係る状態推定装置１は、車両に搭載されており、ＬＩＤＡＲ（Light Detection and Ranging）２と電気的に接続されている。
【００２５】
ＬＩＤＡＲ２は、レーザ光を利用して他車両を測定するレーダであり、測定装置として機能する。ＬＩＤＡＲ２は、レーザ光を出射するとともに、この出射したレーザ光の反射光を受光することで、反射点の点列を検出する。そして、ＬＩＤＡＲ２は、レーザ光の速度、レーザ光の出射時刻、及び、反射光の受光時刻から、この検出した点列の測定データを算出する。測定データには、例えば、自車両との相対距離、自車両に対する相対方向、自車両との相対距離と自車両に対する相対方向から算出される座標などが含まれる。そして、ＬＩＤＡＲ２は、検出した点列の測定データを状態推定装置１に送信する。
【００２６】
状態推定装置１は、カルマンフィルタを用いた推定処理により自車両の周辺に存在する他車両の状態を推定するものである。
【００２７】
具体的に説明すると、状態推定装置１は、まず、自車両の周辺に存在する他車両を、観測対象である対象車両とし、この対象車両の状態を、推定したい変数として設定する。図２は、推定したい変数を示した図である。図２に示すように、推定したい変数は、例えば、中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）となる。
【００２８】
そして、状態推定装置１は、ＬＩＤＡＲ２から送信された測定データを所定の状態推定用モデルに当てはめて演算することで上記の各変数を推定し、この推定した変数を対象車両の状態推定値として出力する。なお、本実施形態では、このように変数を推定する処理を、カルマンフィルタ更新処理という。
【００２９】
更に、状態推定装置１は、対象車両との位置関係や対象車両の状態に基づいて、カルマンフィルタ更新処理に用いる状態推定用モデルを変更する。このため、状態推定装置１は、状態推定用モデルを変更する変更手段としても機能する。なお、カルマンフィルタ更新処理に用いる状態推定用モデルは、後述するように、観測モデル、観測ノイズモデル、運動モデル、運動ノイズモデルにより表される。
【００３０】
ここで、カルマンフィルタの概念について簡単に説明する。なお、カルマンフィルタ自体は周知の技術であるため、詳しい説明は省略する。
【００３１】
カルマンフィルタは、観測量（観測ベクトル）ｚｋのみが観測されるときに、観測対象の状態（状態ベクトル）ｘｋを推定するものである。このため、ｘｋが、推定により求めたい変数となる。なお、本実施形態では、ＬＩＤＡＲ２により測定される測定データが観測量に対応する。
【００３２】
時刻ｋにおける観測量ｚｋは、次の（１）式に示す観測モデルにより表される。
【数１】
2012117528.tif 000003
【００３３】
ここで、ｖｋは、観測モデルに入り込む観測ノイズを表す観測ノイズモデルである。観測ノイズは、例えば、ＬＩＤＡＲ２の特性により生じる誤差や、ＬＩＤＡＲ２の読み取り誤差などの、観測により生じる誤差である。この観測ノイズモデルｖｋは、平均０分散Ｒの正規分布に従い、次の（２）式又は（３）式で表される。
【数２】
2012117528.tif 000004
【００３４】
時刻ｋにおける状態ｘｋは、次の（４）式に示す運動モデルにより表される。
【数３】
2012117528.tif 000005
【００３５】
ここで、ｕｋは、操作量である。また、ｗｋは、運動モデルに入り込む運動ノイズを表す運動ノイズモデルである。運動ノイズは、運動モデルが想定している運動状態と異なる運動状態が行われることにより生じる誤差である。例えば、等速直線運動を行う運動モデルであった場合に、加減速が行われることにより、観測対象の速度に生じる誤差や、ステアリングが切られることにより、観測対象の速度方向に生じる誤差などがある。この運動ノイズモデルｗｋは、平均０分散Ｑの正規分布に従い、次の（５）式又は（６）式で表される。
【数４】
2012117528.tif 000006
【００３６】
そして、カルマンフィルタでは、確率ｐ（ｘｋ｜ｚ１，・・・，ｚｋ）がガウス分布であると仮定し、次の時刻の確率ｐ（ｘｋ＋１｜ｚ１，・・・，ｚｋ＋１）を逐次演算していく。すると、状態ｘｋの分布は、次の（７）式及び（８）式で表される。
【数５】
2012117528.tif 000007
【００３７】
そして、観測量ｚｋで更新した状態ｘｋの分布は、次の（９）式及び（１０）で表される。
【数６】
2012117528.tif 000008
【００３８】
以下、第１〜第９の実施形態に係る状態推定装置について詳しく説明する。なお、各実施系形態に係る状態推定装置は、実施形態の番号に併せて１１〜１９の符号を付す。
【００３９】
［第１の実施形態］
第１の実施形態に係る状態推定装置１１の推定処理について説明する。図３は、第１の実施形態に係る状態推定装置の推定処理を示した図である。
【００４０】
図３に示すように、第１の実施形態に係る状態推定装置１１は、ＬＩＤＡＲ２に対する対象車両の中心位置の方向と対象車両の向きとに基づいて、カルマンフィルタ更新処理に用いる観測モデルを変更する。観測モデルとしては、対象車両の後面を対象とした後面観測モデル、対象車両の後面及び左面を対象とした左斜後面観測モデル、対象車両の左面を対象とした左面観測モデル、対象車両の前面及び左面を対象とした左斜前面観測モデル、対象車両の前面を対象とした前面観測モデル、対象車両の前面及び右面を対象とした右斜前面観測モデル、対象車両の右面を対象とした右面観測モデル、対象車両の後面及び右面を対象とした右斜後面観測モデル、の８つがある。そこで、状態推定装置１１は、これら８つの観測モデルから、適切な観測モデルを選択する。
【００４１】
まず、状態推定装置１１は、ＬＩＤＡＲ２から送信された点列の測定データからグルーピング点群データを生成する（Ｓ１）。詳しく説明すると、状態推定装置１１は、ＬＩＤＡＲ２が反射点の点列を検出すると、所定距離内にある点列をグルーピングしたグルーピング点群データを生成する。このグルーピング点群データは、各車両に対応して生成されるため、自車両の周辺に複数の車両が存在する場合は、複数のグルーピング点群データが生成される。
【００４２】
次に、状態推定装置１１は、Ｓ１で生成したグルーピング点群データの重心位置を求める（Ｓ２）。グルーピング点群データの重心位置は、対象車両の中心位置に対応するものである。このため、グルーピング点群データの重心位置は、例えば、グルーピング点群データから車両のモデルを生成し、このモデルの重心位置を算出することにより求めることができる。
【００４３】
そして、状態推定装置１１は、ＬＩＤＡＲ２から見たＳ２で求めた重心位置の方位角を算出する（Ｓ３）。すなわち、状態推定装置１１は、Ｓ３において、ＬＩＤＡＲ２に対する対象車両の重心位置の方向を算出する。
【００４４】
一方、状態推定装置１１は、Ｓ２で求めた重心位置を過去複数回分にわたってトラッキングし、Ｓ２で求めた重心位置の速度を推定する（Ｓ４）。そして、状態推定装置１１は、Ｓ４のトラッキング及び速度の推定により、Ｓ２で求めた重心位置の速度向きを算出する（Ｓ５）。すなわち、状態推定装置１１は、Ｓ５において、対象車両の速度向きを算出する。
【００４５】
次に、状態推定装置１１は、Ｓ３で算出した重心位置の方位角とＳ５で算出した重心位置の速度向きとの差分から、観測モデルを選択する（Ｓ６）。
【００４６】
ここで、図４及び図５を参照して、Ｓ６の処理について詳しく説明する。図４は、重心位置の方位角と重心位置の速度向きとを示す図である。図５は、観測モデルの変更基準例を示す図である。なお、図４において、Ｏ（Ｘ０，Ｙ０）は、ＬＩＤＡＲ２の原点を示しており、Ｃ（ｘ，ｙ）は、Ｓ２で求めた重心位置を示している。また、θは、Ｓ５で算出した重心位置Ｃの速度向きを示しており、ψは、原点Ｏに対する重心位置Ｃの方向であって、Ｓ３で算出した方向を示している。
【００４７】
図４に示すように、状態推定装置１１は、まず、Ｓ５で算出した速度向きθからＳ３で算出した方向ψを引いた角度φを算出する。この角度φは、φ＝θ−ψで表され、０〜２π（３６０°）の範囲となる。そして、図５に示すように、状態推定装置１１は、この算出された角度φに基づいて、観測モデルを選択する。
【００４８】
角度φが２０°以下の場合は、ＬＩＤＡＲ２から対象車両の後面のみが見えるため、状態推定装置１１は、後面観測モデルを選択する。
【００４９】
角度φが２０°より大きく７０°以下の場合は、ＬＩＤＡＲ２から対象車両の後面及び左面のみが見えるため、状態推定装置１１は、左斜後面観測モデルを選択する。
【００５０】
角度φが７０°より大きく１１０°以下の場合は、ＬＩＤＡＲ２から対象車両の左面のみが見えるため、状態推定装置１１は、左面観測モデルを選択する。
【００５１】
角度φが１１０°より大きく１６０°以下の場合は、ＬＩＤＡＲ２から対象車両の前面及び左面のみが見えるため、状態推定装置１１は、左斜前面観測モデルを選択する。
【００５２】
角度φが１６０°より大きく２００°以下の場合は、ＬＩＤＡＲ２から対象車両の前面のみが見えるため、状態推定装置１１は、前面観測モデルを選択する。
【００５３】
角度φが２００°より大きく２５０°以下の場合は、ＬＩＤＡＲ２から対象車両の前面及び右面のみが見えるため、状態推定装置１１は、右斜前面観測モデルを選択する。
【００５４】
角度φが２５０°より大きく２９０°以下の場合は、ＬＩＤＡＲ２から対象車両の右面のみが見えるため、状態推定装置１１は、右面観測モデルを選択する。
【００５５】
角度φが２９０°より大きく３４０°以下の場合は、ＬＩＤＡＲ２から対象車両の後面及び右面のみが見えるため、状態推定装置１１は、右斜後面観測モデルを選択する。
【００５６】
角度φが３４０°より大きい場合は、ＬＩＤＡＲ２から対象車両の後面のみが見えるため、状態推定装置１１は、後面観測モデルを選択する。
【００５７】
ここで、図６及び図７を参照して、観測モデルの一例を詳しく説明する。図６は、右斜後面観測モデルを説明するための図である。図７は、後面観測モデルを説明するための図である。
【００５８】
図６に示すように、ＬＩＤＡＲ２から対象車両の後面及び右面のみが見える場合を考える。この場合、Ｓ１で生成したグルーピング点群データに直線を当てはめると、このグルーピング点群データは、右側に配置される点列で構成される右グルーピングと、左側に配置される点列で構成される左グルーピングと、にグルーピングされる。なお、グルーピング点群データは、反射点の点列で構成されるため、グルーピング点群データに当てはめられる直線は、対象車両の前面、後面、右面、左面に対応する。
【００５９】
上述したように、推定したい変数は、中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）である（図２参照）。このため、右斜後面観測モデルにおける変数は、
右グルーピングの中心位置（ＸＲ）
右グルーピングの中心位置（ＹＲ）
右グルーピングにおける長軸の長さ（ＬＲ）
右グルーピングにおける長軸の方位（ΘＲ）
左グルーピングの中心位置（ＸＬ）
左グルーピングの中心位置（ＹＬ）
左グルーピングにおける長軸の長さ（ＬＬ）
左グルーピングにおける長軸の方位（ΘＬ）
となる。
【００６０】
また、右斜後面観測観測モデルは、
ＸＲ＝ｘ−ｌ／２×ｃｏｓ（θ）
ＹＲ＝ｙ−ｌ／２×ｓｉｎ（θ）
ＬＲ＝ｗ
ΘＲ＝ｍｏｄ（θ＋π／２，π）
ＸＬ＝ｘ＋ｗ／２×ｓｉｎ（θ）
ＹＬ＝ｙ−ｗ／２×ｃｏｓ（θ）
ＬＬ＝ｌ
ΘＬ＝ｍｏｄ（θ，π）
となる。
【００６１】
図７に示すように、ＬＩＤＡＲ２から対象車両の後面のみが見える場合を考える。この場合、Ｓ１で生成したグルーピング点群データに直線を当てはめると、１つにグルーピングされる。
【００６２】
上述したように、推定したい変数は、中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）である（図２参照）。このため、右斜後面観測観測モデルにおける変数は、
グルーピングの中心位置（Ｘ）
グルーピングの中心位置（Ｙ）
グルーピングにおける長軸の長さ（Ｌ）
グルーピングにおける長軸の方位（Θ）
となる。
【００６３】
そして、右斜後面観測観測モデルは、
Ｘ＝ｘ−ｌ／２×ｃｏｓ（θ）
Ｙ＝ｙ−ｌ／２×ｓｉｎ（θ）
Ｌ＝ｗ
Θ＝ｍｏｄ（θ＋π／２，π）
となる。
【００６４】
そして、状態推定装置１１は、Ｓ６で選択された観測モデルを、今回の推定に用いる観測モデルとして決定する（Ｓ７）。
【００６５】
次に、状態推定装置１１は、Ｓ１で生成したグルーピング点群データと、Ｓ７で決定した観測モデルとを用いて、カルマンフィルタ更新処理を行う（Ｓ８）。このとき、状態推定装置１１は、中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）の変数を推定するとともに、推定した各変数の分散（以下「推定分散値」という）を算出する。推定分散値は、上述した（９）式で表される分散値Ｐｋに対応する。そして、状態推定装置１１は、Ｓ８のカルマンフィルタ更新処理により算出された変数を、対象車両の状態推定値として出力する（Ｓ９）。
【００６６】
このように、本実施形態に係る状態推定装置１１によれば、対象車両との位置関係や対象車両の状態に基づいて状態推定用モデルを変更するため、動的な対象車両の状態をより高精度に推定することができる。
【００６７】
そして、ＬＩＤＡＲ２に対する対象車両の中心位置の方向と対象車両の向きとの差分に基づいて観測モデルを変更することで、測定データと観測モデルとを適切に対応付けることができるため、対象車両の状態の推定精度を更に向上させることができる。
【００６８】
［第２の実施形態］
次に、第２の実施形態に係る状態推定装置１２の推定処理について説明する。第２の実施形態は、第１の実施形態と観測モデルの選択手法が異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【００６９】
図８は、第２の実施形態に係る状態推定装置の推定処理を示した図である。図８に示すように、第２の実施形態に係る状態推定装置１２は、前回の推定処理で用いた観測モデルに基づいて、今回の推定処理で用いる観測モデルを絞り込むものである。
【００７０】
通常、車両の挙動変化は連続的である。このため、時間の経過に伴い、対象車両との位置関係や対象車両の状態が変化して行ったとしても、ＬＩＤＡＲ２から見える車両の面は、後面、左斜後面、左面、左斜前面、前面、右斜前面、右面、右斜後面の順、又は、この逆の順にしか変化しない。
【００７１】
そこで、状態推定装置１２は、今回の推定処理のＳ６で選択する観測モデルを、前回の推定処理のＳ７で決定した観測モデルに基づき絞り込む（Ｓ１１）。
【００７２】
詳しく説明すると、状態推定装置１２は、まず、前回の推定処理のＳ７で決定した観測モデルを特定する。更に、状態推定装置１２は、上記の順又は逆順においてこの観測モデルと隣接関係にある２つの観測モデルを特定する。そして、状態推定装置１２は、今回の推定処理のＳ６で選択する観測モデルを、この特定した３つの観測モデルに絞り込む。例えば、前回の推定処理のＳ７で決定した観測モデルが後面観測モデルであった場合は、後面観測モデル、右斜後面モデル及び左斜後面モデルの３つの観測モデルで、今回の推定処理のＳ６で選択する観測モデルを絞り込む。
【００７３】
そして、状態推定装置１２は、Ｓ６において、Ｓ３で算出した重心位置の方位角とＳ５で算出した重心位置の速度向きとの差分から選択する観測モデルが、Ｓ１１で絞り込まれた観測モデルである場合は、第１の実施形態と同様に処理を続ける。
【００７４】
一方、状態推定装置１２は、Ｓ６において、Ｓ３で算出した重心位置の方位角とＳ５で算出した重心位置の速度向きとの差分から選択する観測モデルが、Ｓ１１で絞り込まれた観測モデルでない場合は、今回の観測モデルの選択に誤りがある可能性が高いと判断する。そして、状態推定装置１２は、Ｓ６で選択する観測モデルを、前回の推定処理のＳ７で決定した観測モデルに変更し、又は、今回の推定処理により出力された観測対象の状態推定値は信頼性が欠けるものとして取り扱う。
【００７５】
このように、第２の実施形態に係る状態推定装置１２によれば、前回の推定処理で用いた観測モデルに基づいて、今回の推定処理に用いる観測モデルを絞り込むため、誤った観測モデルが選択されるのを低減することができる。
【００７６】
［第３の実施形態］
次に、第３の実施形態に係る状態推定装置１３の推定処理について説明する。第３の実施形態は、第１の実施形態と観測モデルの選択手法が異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【００７７】
図９は、第３の実施形態に係る状態推定装置の推定処理を示した図である。上述したように、第１の実施形態では、ＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きを、Ｓ１で生成したグルーピング点群データに基づいて求めていた。これに対し、図９に示すように、第３の実施形態では、ＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きを、前回の推定処理で出力した対象車両の状態推定値に基づいて求める。
【００７８】
詳しく説明すると、状態推定装置１３は、前回の推定処理のＳ９において出力した対象車両の状態推定値から対象車両の位置（ｘ，ｙ）を抽出し、この抽出した対象車両の位置から、ＬＩＤＡＲ２に対する対象車両の中心位置の方向を算出する（Ｓ１３）。また、状態推定装置１３は、前回の推定処理のＳ９において出力した対象車両の状態推定値から対象車両の速度向き（θ）を抽出する（Ｓ１４）。
【００７９】
そして、状態推定装置１３は、Ｓ１３で算出したＬＩＤＡＲ２に対する対象車両の中心位置の方向と、Ｓ１４で抽出した対象車両の速度向きとの差分から、観測モデルを選択する（Ｓ６）
【００８０】
このように、第３の実施形態に係る状態推定装置１３によれば、前回の推定処理で出力した対象車両の状態推定値を利用することで、推定の連続性が保たれるため、対象車両の状態の推定精度を更に向上させることができる。
【００８１】
［第４の実施形態］
次に、第４の実施形態に係る状態推定装置１４の推定処理について説明する。第４の実施形態は、第１の実施形態と観測モデルの選択手法が異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【００８２】
図１０は、第４の実施形態に係る状態推定装置の推定処理を示した図である。上述したように、第１の実施形態では、対象車両の向きを、Ｓ１で生成したグルーピング点群データに基づいて求めていた。これに対し、図１０に示すように、第４の実施形態では、対象車両の向きを、地図情報に基づいて求める。
【００８３】
詳しく説明すると、状態推定装置１４は、まず、地図情報を取得する（Ｓ１６）。この地図情報は、例えば、ナビゲーションシステムなどのように車両に搭載された記憶装置に記憶されているものであってもよく、路車間通信などにより車外から取得するものであってもよい。
【００８４】
次に、状態推定装置１４は、Ｓ１６で取得した地図情報にＳ２で算出した重心位置を重ね合わせることで、地図情報における対象車両が存在する位置を特定する。そして、状態推定装置１４は、この特定した位置の地図上の道路の向きを算出し、この算出した地図上の道路の向きが対象車両の速度向きであると推定する（Ｓ１７）。
【００８５】
なお、第４の実施形態では、Ｓ２において、グルーピング点群データの重心位置を算出するだけではなく、グルーピング点群データから対象車両の位置も推定し、Ｓ１７において、この推定した対象車両の位置に基づいて、地図上における対象車両の存在する位置を特定するものとしてもよい。
【００８６】
このように、第４の実施形態に係る状態推定装置１４によれば、対象車両が存在する位置に基づいて対象車両の向きを推定するため、例えば、対象車両が静止している場合や対象車両を検出した直後の場合などであっても、対象車両の向きを推定することができる。
【００８７】
［第５の実施形態］
次に、第５の実施形態に係る状態推定装置１５の推定処理について説明する。第５の実施形態は、第１の実施形態と観測モデルの選択手法が異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【００８８】
図１１は、第５の実施形態に係る状態推定装置の推定処理を示した図であり、図１２は、図１１のモデル選択処理を示した図である。上述したように、第１の実施形態では、グルーピング点群データから算出されるＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きに基づいて、観測モデルを選択していた。これに対し、図１１及び図１２に示すように、第５の実施形態では、グルーピング点群データから算出される辺の数に基づいて、観測モデルを選択する。
【００８９】
詳しく説明すると、図１０に示すように、状態推定装置１５は、Ｓ１においてグルーピング点群データを生成すると、次に説明するモデル選択処理を行う（Ｓ１９）。
【００９０】
図１１を参照して、Ｓ１９のモデル選択処理について詳しく説明する。
【００９１】
状態推定装置１５は、まず、Ｓ１で生成したグルーピング点群データの凸包を算出する（Ｓ２１）。この凸包算出では、まず、グルーピング点群データから、右端の点と左端の点を特定する。そして、右端（又は左側）の点から、左側（又は右側）に向けてグルーピング点群データの点を順次連結していき、左端（又は右側）の点に到達すると、点の連結を終了する。なお、グルーピング点群データは、反射点の点列で構成されるため、凸包算出で連結された線は、対象車両の側面に対応した１本又は２本の直線となる。
【００９２】
次に、状態推定装置１５は、Ｓ２１で算出した凸包の辺を分割する（Ｓ２２）。上述したように、グルーピング点群データは、反射点の点列で構成されるため、Ｓ２１の凸包算出で連結された線は、対象車両の側面に対応した１本又は２本の直線となる。このため、Ｓ２１で凸包の辺を分割することで、ＬＩＤＡＲ２から対象車両の面が、何面見えるのかを判断することができる。
【００９３】
次に、状態推定装置１５は、辺の数が１であるか否かを判定する（Ｓ２３）。そして、状態推定装置１５は、辺の数が１であると判定すると（Ｓ２３：ＹＥＳ）、辺の長さが所定の閾値よりも短いか否かを判定し（Ｓ２４）、辺の数が１ではないと判定すると（Ｓ２３：ＮＯ）、右辺よりも左辺の方が長いか否かを判定する（Ｓ３１）。なお、Ｓ２４の閾値は、車両の前面及び後面と左面及び右面とを区別するための値である。このため、Ｓ２４の閾値は、一般的な車両の、前面及び後面の幅と左面及び右面の長さとの間の値となる。
【００９４】
Ｓ２４に進んだ状態推定装置１５は、辺の長さが所定の閾値よりも短いと判定すると（Ｓ２４：ＹＥＳ）、対象車両の速度向きが自車両に対して離れていく方向であるか否かを判定し（Ｓ２５）、辺の長さが所定の閾値よりも短くないと判定すると（Ｓ２４：ＮＯ）、対象車両の速度向きが自車両から見て右向きであるか否かを判定する（Ｓ２８）。なお、対象車両の速度向きは、様々な手法により検出することができる。例えば、第１の実施形態のように、グルーピング点群データの重心位置をトラッキングすることにより求めてもよく、また、第３の実施形態のように、前回の推定処理で出力した状態推定値から求めてもよい。
【００９５】
Ｓ２５に進んだ状態推定装置１５は、対象車両の速度向きが自車両に対して離れていく方向であると判定すると（Ｓ２５：ＹＥＳ）、後面観測モデルを選択し（Ｓ２６）、対象車両の速度向きが自車両に対して離れていく方向ではないと判定すると（Ｓ２５：ＮＯ）、前面モデルを選択する（Ｓ２７）。
【００９６】
Ｓ２８に進んだ状態推定装置１５は、対象車両の速度向きが自車両から見て右向きであると判定すると（Ｓ２８：ＹＥＳ）、右面観測モデルを選択し（Ｓ２９）、対象車両の速度向きが自車両から見て右向きではないと判定すると（Ｓ２８：ＮＯ）、左面観測モデルを選択する。
【００９７】
Ｓ３１に進んだ状態推定装置１５は、上述したＳ３１の判定において、左辺は右辺よりも長いと判定すると（Ｓ３１：ＹＥＳ）、対象車両の速度向きが自車両に対して離れていく方向であるか否かを判定し（Ｓ３２）、左辺は右辺よりも長くないと判定すると（Ｓ３１：ＮＯ）、対象車両の速度向きが自車両に対して離れていく方向であるか否かを判定する（Ｓ３５）。
【００９８】
Ｓ３２に進んだ状態推定装置１５は、対象車両の速度向きが自車両に対して離れていく方向であると判定すると（Ｓ３２：ＹＥＳ）、左斜後面観測モデルを選択し（Ｓ３３）、対象車両の速度向きが自車両に対して離れていく方向ではないと判定すると（Ｓ３２：ＮＯ）、右斜前面モデルを選択する（Ｓ３４）。
【００９９】
Ｓ３５に進んだ状態推定装置１５は、対象車両の速度向きが自車両に対して離れていく方向であると判定すると（Ｓ３５：ＹＥＳ）、右斜後面観測モデルを選択し（Ｓ３６）、対象車両の速度向きが自車両に対して離れていく方向であると判定すると（Ｓ３５：ＮＯ）、左斜後面観測モデルを選択する（Ｓ３７）。
【０１００】
なお、Ｓ３５では、対象車両の速度向きが自車両から見て右向きであるか否かを判定してもよい。この場合、状態推定装置１５は、対象車両の速度向きが自車両から見て右向きであると判定すると、右斜後面観測モデルを選択し（Ｓ３６）、対象車両の速度向きが自車両から見て右向きでないと判定すると、左斜後面観測モデルを選択する（Ｓ３７）。
することにしてもよい。
【０１０１】
このようにして観測モデルが選択されると、図１０に示すように、状態推定装置１５は、Ｓ１９で選択された観測モデルを、今回の推定に用いる観測モデルとして決定する（Ｓ７）。
【０１０２】
このように、第５の実施形態に係る状態推定装置１５によれば、グルーピング点群データから求められる辺の数に基づいて観測モデルを変更することで、観測モデルの選択基準が明確化されるため、対象車両の状態の推定精度を更に向上させることができる。
【０１０３】
［第６の実施形態］
次に、第６の実施形態に係る状態推定装置１６の推定処理について説明する。第６の実施形態は、観測モデルの観測ノイズモデルのみを変更する点で第１の実施形態と異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【０１０４】
図１３は、第６の実施形態に係る状態推定装置の推定処理を示した図である。上述したように、第１の実施形態では、グルーピング点群データから算出されるＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きに基づいて、観測モデルを選択していた。これに対し、図１３に示すように、第６の実施形態では、グルーピング点群データから算出される辺の方位角に基づいて、観測ノイズモデルを変更する。
【０１０５】
ここで、対象車両の面に対する向きと、観測誤差との関係について説明する。
【０１０６】
一般的に、ＬＩＤＡＲ２は、１０ｃｍ程度の分解能を有するため、点列ｐ自体の測定誤差は小さい。一方、ＬＩＤＡＲ２は、端部からは点列を検出しにくいという特性を有するため、ＬＩＤＡＲ２で検出された点列の中心は、対象車両の面の中心からずれた位置となる。このため、対象車両３の面に対して垂直な方向の観測ノイズは小さいが、対象車両３の面に対して水平な方向の観測ノイズは、対象車両３の面に対して垂直な方向の観測ノイズよりも大きくなる。
【０１０７】
図１４は、対象車両とグルーピング点群データとの関係を示した図であり、図１５は、観測ノイズモデルの概念を示した図である。なお、図１４の矢印は、対象車両の進行方向を示している。
【０１０８】
図１４に示すように、ＬＩＤＡＲ２から対象車両３の前面３Ａ及び左面３Ｂが見えており、対象車両３の前面３Ａ及び左面３Ｂに、ＬＩＤＡＲ２から出射したレーザ光の反射点の点列ｐが検出された場合を考える。
【０１０９】
この場合、前面３Ａの右部（図１４において左上の部分）と左面３Ｂの後部（図１４において右上の部分）からは点列ｐが検出されない。このため、前面３Ａにおける点列ｐの中心ＰＡ’は、前面３Ａの中心ＰＡよりも前面３Ａの左側（図１４において右下側）にずれてしまう。また、左面３Ｂにおける点列ｐの中心ＰＢ’は、左面３Ｂの中心ＰＢよりも左面３Ｂの前側（図１４において左下側）にずれてしまう。
【０１１０】
上述したように、中心位置（ｘ、ｙ）は観測モデルの変数である。このため、ＬＩＤＡＲ２で検出された点列ｐに基づいて前面３Ａの中心位置を算出すると、対象車両３の前面３Ａに対して水平な方向の観測ノイズが、前面３Ａに対して垂直な方向の観測ノイズよりも大きくなる。また、ＬＩＤＡＲ２で検出された点列ｐに基づいて左面３Ｂの中心位置を算出すると、対象車両３の左面３Ｂに対して水平な方向の観測ノイズが、左面３Ｂに対して垂直な方向の観測ノイズよりも大きくなる。
【０１１１】
そこで、図１５に示すように、通常は、観測ノイズモデルにおける中心位置の分散値Ｒ’は真円で表されるが、第６の実施形形態では、対象車両の面に対して垂直な方向の観測ノイズよりも、対象車両の面に対して水平な方向の観測ノイズが大きくなるように、観測ノイズモデルにおける中心位置の分散値Ｒを変更する。
【０１１２】
具体的に説明すると、対象車両の面に対して垂直な方向の誤差をσｙ、対象車両の面に対して水平な方向の誤差をσｘ、回転行列をＲθとすると、観測ノイズモデルにおける中心位置の分散値Ｒは、次の（１１）式で表される。なお、（１１）式の算出方法を（１２）式に記載した。
【数７】
2012117528.tif 000009
【数８】
2012117528.tif 000010
【０１１３】
次に、図１３を参照して、状態推定装置１６の処理について説明する。
【０１１４】
状態推定装置１６は、まず、Ｓ１で生成したグルーピング点群データの凸包を算出し（Ｓ４１）、この算出した凸包の辺を分割する（Ｓ４２）。なお、Ｓ４１の凸包算出は、第５の実施形態に係る状態推定装置１６が行うＳ２１の凸包算出（図１２参照）と同様である。
【０１１５】
次に、状態推定装置１６は、Ｓ４２で分割した辺を、１本又は２本の直線に当てはめ（Ｓ４３）、この当てはめた直線の方位角を算出する（Ｓ４４）。
【０１１６】
そして、状態推定装置１６は、上述した（１１）式で表されるように、Ｓ４４で算出した直線の方位角に基づいて、観測ノイズモデルにおける中心位置の分散値Ｒを変更する（Ｓ４５）。
【０１１７】
そして、状態推定装置１６は、Ｓ４５で分散値が変更された観測ノイズモデルを組み込んだ観測モデルを、今回の推定に用いる観測モデルとして決定する（Ｓ４６）。
【０１１８】
このように、第６の実施形態に係る状態推定装置１６によれば、対象車両の面に対する向きに基づいて観測ノイズモデルの分散値を変更するため、対象車両の状態の推定精度を更に向上させることができる。
【０１１９】
［第７の実施形態］
次に、第７の実施形態に係る状態推定装置１７の推定処理について説明する。第７の実施形態は、観測モデルの観測ノイズモデルのみを変更する点で第１の実施形態と異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【０１２０】
図１６は、第７の実施形態に係る状態推定装置の推定処理を示した図である。上述したように、第１の実施形態では、グルーピング点群データから算出されるＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きに基づいて、観測モデルを選択していた。これに対し、図１６に示すように、第７の実施形態では、対象車両との距離に基づいて、観測ノイズモデルを変更する。
【０１２１】
状態推定装置１７は、まず、前回の推定処理のＳ９で出力した対象車両の状態推定値から、対象車両の位置を抽出する。このとき、状態推定装置１７は、前回の推定処理のＳ９で出力した状態推定値の代わりに、第１の実施形態のように、今回の推定処理のＳ１で生成したグルーピング点群データから算出される重心位置を用いてもよい。次に、状態推定装置１７は、この抽出した対象車両の位置から、自車両から対象車両までの距離を算出する。そして、状態推定装置１７は、この算出した自車両から対象車両までの距離に基づいて、観測ノイズモデルにおける観測ノイズを変更する（Ｓ４８）。
【０１２２】
詳しく説明すると、自車両から対象車両までの距離が近いと、ＬＩＤＡＲ２による対象車両の測定対象領域が大きくなるため、観測ノイズが小さくなる。一方、自車両から対象車両までの距離が遠いと、ＬＩＤＡＲ２による対象車両の測定対象領域が小さくなるため、観測ノイズが大きくなる。そこで、状態推定装置１７は、自車両から対象車両までの距離が長くなるほど、観測ノイズモデルにおける観測ノイズを大きくする。なお、観測ノイズモデルにおける観測ノイズは、例えば、自車両から対象車両までの距離に応じて連続的に変化させてもよく、自車両から対象車両までの距離に応じて１又は複数の段階に分けて変化させてもよい。後者の場合、例えば、１又は複数の距離を設定しておき、自車両から対象車両までの距離がこの設定した距離を超える度に、観測ノイズモデルにおける観測ノイズを大きくすることができる。また、変更する観測ノイズは、対象車両の面の中心位置、対象車両の速さ、対象車両の向きなど、様々なノイズを用いることができる。
【０１２３】
そして、状態推定装置１７は、Ｓ４８で変更された観測ノイズモデルを組み込んだ観測モデルを、今回の推定に用いる観測モデルとして決定する（Ｓ４９）。
【０１２４】
このように、第７の実施形態に係る状態推定装置１７によれば、対象車両までの距離に基づいて観測ノイズモデルにおける観測ノイズを変更することで、対象車両の状態の推定精度を更に向上させることができる。
【０１２５】
［第８の実施形態］
次に、第８の実施形態に係る状態推定装置１８の推定処理について説明する。第８の実施形態は、運動ノイズモデルのみを変更する点で第１の実施形態と異なるが、基本的に第１の実施形態と同様である。このため、以下では、第１の実施形態と相違する部分のみを説明し、第１の実施形態と同様の部分の説明を省略する。
【０１２６】
図１７は、第８の実施形態に係る状態推定装置の推定処理を示した図である。上述したように、第１の実施形態では、ＬＩＤＡＲ２に対する対象車両の中心位置の方向、及び、対象車両の向きに基づいて、観測モデルを変更していた。これに対し、図１７に示すように、第８の実施形態では、対象車両の速度に基づいて、運動モデルの運動ノイズモデルを変更する。
【０１２７】
ここで、運動ノイズモデルについて詳しく説明する。上述したように、推定したい変数は、中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）である（図２参照）。このため、運動モデルは、
ｘ：＝ｘ＋ｖ×ｃｏｓ（θ）
ｙ：＝ｙ＋ｖ×ｓｉｎ（θ）
ｖ：＝ｖ
θ：＝θ＋ｖ／ｂ×ｔａｎ（ξ）
ξ：＝ξ
ｂ：＝ｂ
ｌ：＝ｌ
ｗ：＝ｗ
で表される。
【０１２８】
そして、運動モデルが等速直線運動である場合に、これら運動モデルに入り込む運動ノイズモデルは、例えば、
σ（ｘ）＝０
σ（ｙ）＝０
σ（ｖ）＝加減速度
σ（θ）＝０
σ（ξ）＝ステアリング変化量（ステアリング角度の変化量）
σ（ｂ）＝０
σ（ｌ）＝０
σ（ｗ）＝０
となる。
【０１２９】
このように、運動モデルに入り込む運動ノイズモデルには、ステアリング変化量と加減速度が設定されるが、従来は、何れも固定的な値を運動ノイズモデルに設定していた。しかしながら、車両の速度が高くなるに従い、ステアリングを大きく切る可能性が低くなる傾向にある。
【０１３０】
そこで、状態推定装置１８は、まず、前回の推定処理のＳ９で出力した対象車両の状態推定値から、対象車両の速度を抽出する。そして、状態推定装置１８は、この抽出した対象車両の速度に基づいて、運動ノイズモデルにおけるステアリング変化量σ（ξ）を変更する（Ｓ５１）。具体的に説明すると、状態推定装置１８は、対象車両の速度が高いほど、運動ノイズモデルにおけるステアリング変化量σ（ξ）を小さくする。なお、ステアリング変化量σ（ξ）は、例えば、対象車両の速度に応じて連続的に変化させてもよく、対象車両の速度に応じて１又は複数の段階に分けて変化させてもよい。後者の場合、例えば、１又は複数の速度を設定しておき、対象車両の速度が設定した速度を超える度に、ステアリング変化量σ（ξ）を小さくすることができる。
【０１３１】
そして、状態推定装置１８は、Ｓ５１で変更された運動ノイズモデルを組み込んだ運動モデルを、今回の推定に用いる運動モデルとして決定する（Ｓ５２）。
【０１３２】
このように、第８の実施形態に係る状態推定装置１８によれば、対象車両の速度が高いと運動ノイズモデルにおけるステアリング変化量σ（ξ）を小さくすることで、対象車両の状態の推定精度を更に向上させることができる。
【０１３３】
［第９の実施形態］
次に、第９の実施形態に係る状態推定装置１９の推定処理について説明する。第１の実施形態では、推定処理に用いる観測ノイズモデルを変更して、対象車両の状態を推定した。これに対し、第９の実施形態では、複数の異なる観測モデルで対象車両の状態を推定し、推定分散値が最も小さくなる観測モデルを用いて推定した観測対象の状態を出力する。
【０１３４】
図１８は、第９の実施形態に係る状態推定装置の推定処理を示した図である。図１８に示すように、状態推定装置１９は、複数の異なる観測モデルを用意する（Ｓ５４）。Ｓ５４で用意する観測モデルは、後面観測モデル、左斜後面観測モデル、左面観測モデル、左斜前面観測モデル、前面観測モデル、右斜前面観測モデル、右面観測モデル及び右斜後面観測モデルの８の観測モデルである。なお、以下の説明では、Ｓ５４で用意する観測モデルは上述した８つであるものとして説明するが、少なくとも２以上の観測モデルを用意すれば、特に用意する観測モデルの数は制限されない。
【０１３５】
次に、状態推定装置１９は、Ｓ５４で用意した８つの観測モデルに、それぞれＳ１で生成したグルーピング点群データを当てはめて、並列的にカルマンフィルタ更新処理を行う（Ｓ５５）。Ｓ５５のカルマンフィルタ更新処理は、第１の実施形態におけるＳ８のカルマンフィルタ更新処理と同様である。
【０１３６】
そして、状態推定装置１９は、Ｓ５５の各カルマンフィルタ更新処理において推定された中心位置（ｘ）、中心位置（ｙ）、速さ（ｖ）、向き（θ）、タイヤ角（ξ）、ホイールベース（ｂ）、長さ（ｌ）、幅（ｗ）の各変数を出力する（Ｓ５６）。
【０１３７】
また、状態推定装置１９は、Ｓ５５の各カルマンフィルタ更新処理において算出した各変数の推定分散値を算出する（Ｓ５７）。
【０１３８】
そして、状態推定装置１９は、Ｓ５６で出力された８つのカルマンフィルタ出力の内、推定分散値の最も小さいカルマンフィルタ出力を最終出力とする（Ｓ５９）。
【０１３９】
このように、第９の実施形態に係る状態推定装置１９によれば、対象車両との位置関係や対象車両の状態が不明な場合であっても、適切な観測モデルを用いて推定した対象車両の状態推定値を出力することができる。
【０１４０】
以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。
【０１４１】
例えば、上記実施形態では、対象車両の状態の推定手段として、カルマンフィルタを採用するものとして説明した。しかしながら、測定データをモデルに当てはめて対象車両の状態を推定するものであれば、如何なる手段、如何なるフィルタを採用してもよい。例えば、パーティクルフィルタ（Particle Filter）を採用してもよい。
【０１４２】
また、上記実施形態では、観測対象として、自車両の周辺に存在する周辺車両を採用したが、バイクや自転車など、あらゆるものを観測対象としてもよい。
【０１４３】
また、第１の実施形態では、ＬＩＤＡＲ２に対する対象車両の中心位置の方向と対象車両の向きとの差分に基づいて観測モデルを変更するものとして説明したが、ＬＩＤＡＲ２に対する対象車両の中心位置の方向のみに基づいて観測モデルを変更してもよく、又は、対象車両の向きのみに基づいて観測モデルを変更してもよい。
【０１４４】
ＬＩＤＡＲ２に対する対象車両の中心位置の方向が異なっても、対象車両の測定可能面が異なり、また、対象車両の向きが異なっても、対象車両の測定可能面が異なる。このため、ＬＩＤＡＲ２に対する対象車両の中心位置の方向、又は、対象車両の向きの何れか一方にのみ基づいて観測モデルを変更することでも、測定データと観測モデルとを適切に対応付けることができる。これにより、対象車両の状態の推定精度を更に向上させることができる。
【０１４５】
なお、上記の各実施形態は、適宜組み合わせることが可能である。例えば、第１の実施形態と第６の実施形態とを組み合わせて、観測モデルと観測ノイズモデルとを変更してもよく、第１の実施形態と第８の実施形態とを組み合わせて、観測モデルと運動モデルとを変更してもよい。
【産業上の利用可能性】
【０１４６】
本発明は、周辺車両の状態を推定する状態推定装置として利用可能である。
【符号の説明】
【０１４７】
１（１１〜１９）…状態推定装置、２…ＬＩＤＡＲ（測定装置）、３…対象車両。
【図１】
2012117528.tif 000011
【図２】
2012117528.tif 000012
【図３】
2012117528.tif 000013
【図４】
2012117528.tif 000014
【図５】
2012117528.tif 000015
【図６】
2012117528.tif 000016
【図７】
2012117528.tif 000017
【図８】
2012117528.tif 000018
【図９】
2012117528.tif 000019
【図１０】
2012117528.tif 000020
【図１１】
2012117528.tif 000021
【図１２】
2012117528.tif 000022
【図１３】
2012117528.tif 000023
【図１４】
2012117528.tif 000024
【図１５】
2012117528.tif 000025
【図１６】
2012117528.tif 000026
【図１７】
2012117528.tif 000027
【図１８】
2012117528.tif 000028
【手続補正書】
【提出日】平成23年9月12日(2011.9.12)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
観測対象を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて前記観測対象の状態を推定する状態推定装置であって、
前記状態推定用モデルは、前記測定装置により測定できる前記観測対象の一面又は二面を表した観測モデルを含み、
前記観測対象との位置関係に基づいて、前記観測モデルを変更する変更手段を有する、状態推定装置。
【請求項２】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記測定装置に対する前記観測対象の中心位置の方向に対応した観測モデルに変更する、請求項１に記載の状態推定装置。
【請求項３】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記観測対象の向きに対応した観測モデルに変更する、請求項１に記載の状態推定装置。
【請求項４】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記変更手段は、前記測定装置に対する前記観測対象の中心位置の方向、及び、前記観測対象の向き、の双方に対応した観測モデルに変更する、請求項１に記載の状態推定装置。
【請求項５】
前記変更手段は、前回の推定で用いた観測モデルに基づいて、前記測定データを当てはめる観測モデルを絞り込む、請求項１〜４の何れか一項に記載の状態推定装置。
【請求項６】
前記変更手段は、前回推定した前記観測対象の状態に基づいて、前記測定装置に対する前記観測対象の中心位置の方向、又は、前記観測対象の向きを推定する、請求項２〜５の何れか一項に記載の状態推定装置。
【請求項７】
前記変更手段は、前記観測対象が存在する位置の地図情報に基づいて、前記観測対象の向きを推定する、請求項３又は４に記載の状態推定装置。
【請求項８】
前記変更手段は、測定データから前記観測対象のモデルを生成し、前記モデルを構成する辺の数に基づいて、前記観測モデルを変更する、請求項１〜７の何れか一項に記載の状態推定装置。
【請求項９】
前記状態推定用モデルは、前記測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、
前記変更手段は、前記観測対象の面に対する向きに基づいて、前記観測ノイズモデルの分散値を変更する、請求項１〜８の何れか一項に記載の状態推定装置。
【請求項１０】
前記変更手段は、前記観測対象までの距離に基づいて、前記観測ノイズモデルを変更する、請求項９に記載の状態推定装置。
【請求項１１】
前記観測対象は、前記測定装置の周辺に存在する車両であり、
前記状態推定用モデルは、前記周辺車両の運動状態を表す運動モデルと、前記運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、
前記変更手段は、前記観測対象の速度が高いと、前記観測対象の速度が低いときに比べて、前記運動ノイズモデルにおけるステアリング角度の変化量を小さくする、請求項１〜１０の何れか一項に記載の状態推定装置。
【請求項１２】
複数の異なる前記観測モデルを用いて、前記観測対象の状態を推定するとともに、前記観測対象の状態の推定分散値を算出し、前記推定分散値が最も小さい前記観測対象の状態を出力する、請求項１〜１１の何れか一項に記載の状態推定装置。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】０００２
【補正方法】変更
【補正の内容】
【０００２】
いという問題がある。
［０００７］
そこで、本発明は、より高精度に観測対象の状態を推定することができる状態推定装置を提供することを目的とする。
課題を解決するための手段
［０００８］
本発明に係る状態推定装置は、観測対象を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて観測対象の状態を推定する状態推定装置であって、状態推定用モデルは、測定装置により測定できる観測対象の一面又は二面を表した観測モデルを含み、観測対象との位置関係に基づいて、観測モデルを変更する変更手段を有する。
［０００９］
本発明に係る状態推定装置によれば、観測対象との位置関係に基づいて観測モデルを変更するため、動的な観測対象の状態をより高精度に推定することができる。
［００１０］
この場合、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、測定装置に対する観測対象の中心位置の方向に対応した観測モデルに変更することが好ましい。測定装置に対する観測対象の中心位置の方向が異なると、観測対象の測定可能面が異なる。このため、測定装置に対する観測対象の中心位置の方向に関わらず同一の観測モデルを用いると、測定データと観測モデルとの対応付けを適切に行うことができない。その結果、観測対象の状態を高精度に推定できなくなる。そこで、測定装置に対する観測対象の中心位置の方向に対応した観測モデルに変更することで、測定データと観測モデルとを適切に対応付けることができる。これにより、観測対象の状態の推定精度を更に向上させることができる。
［００１１］
また、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、観測対象の向きに対応した観測モデルに変更することが好ましい。観測対象の向きが異なると、観測対象の測定可能面が異なる。このため、観測対象の向きに関わらず同一の観測モデルを用いると、測定データと観測モデルとの対応付けを適切に行うことができなくなる。その結果、観測対象の状態を高精度に推定することができない。そこで、観測対象の
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】０００３
【補正方法】変更
【補正の内容】
【０００３】
向きに対応した観測モデルに変更することで、測定データと観測モデルとを適切に対応付けることができるため、観測対象の状態の推定精度を更に向上させることができる。
［００１２］
また、観測対象は、測定装置の周辺に存在する車両であり、変更手段は、測定装置に対する観測対象の中心位置の方向、及び、観測対象の向き、の双方に対応した観測モデルに変更することが好ましい。測定装置に対する観測対象の中心位置の方向、及び、観測対象の向きの双方により、観測対象の自車両に対向する面を特定することができる。このため、これら双方の情報に対応した観測モデルに変更することで、測定データと観測モデルとを適切に対応付けることができるため、観測対象の状態の推定精度を更に向上させることができる。
［００１３］
また、変更手段は、前回の推定で用いた観測モデルに基づいて、測定データを当てはめる観測モデルを絞り込むことが好ましい。通常、観測対象の挙動変化は連続的であることから、前回の推定で用いた観測モデルに基づいて観測モデルを絞り込むことで、誤った観測モデルが選択されるのを低減することができる。
［００１４］
また、変更手段は、前回推定した観測対象の状態に基づいて、測定装置に対する観測対象の中心位置の方向、又は、観測対象の向きを推定することが好ましい。このように、前回推定した情報を利用することで推定の連続性が保たれるため、観測対象の状態の推定精度を更に向上させることができる。
［００１５］
また、変更手段は、観測対象が存在する位置の地図情報に基づいて、観測対象の向きを推定することが好ましい。観測対象が静止している場合や観測対象を検出した直後の場合などは、測定データによって観測対象の向きを求めることができない。そこで、観測対象が存在する位置の地図情報を利用することで、このような場合であっても、観測対象の向きを推定することができる。
［００１６］
また、変更手段は、測定データから観測対象のモデルを生成し、モデルを構成する辺の数に基づいて、観測モデルを変更することが好ましい。
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】０００４
【補正方法】変更
【補正の内容】
【０００４】
このように、測定データから生成されるモデルの辺の数に基づいて状態推定用モデルを変更することで、状態推定用モデルの変更基準が明確化されるため、観測対象の状態の推定精度を更に向上させることができる。
［００１７］
また、状態推定用モデルは、測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、変更手段は、観測対象の面に対する向きに基づいて、観測ノイズモデルの分散値を変更することが好ましい。通常、観測対象の面に垂直な方向には測定データの観測ノイズが小さく、観測対象の面に水平な方向には測定データの観測ノイズが大きい。そこで、測定対象の面に対する向きに基づいて観測ノイズモデルの分散値を変更することで、観測対象の状態の推定精度を更に向上させることができる。
［００１８］
また、変更手段は、観測対象までの距離に基づいて、観測ノイズモデルを変更することが好ましい。観測対象までの距離が近いと、観測対象の測定対象領域が大きいため、観測ノイズが小さくなる。一方、観測対象までの距離が遠いと、観測対象の測定対象領域が小さいため、観測ノイズが大きくなる。そこで、測定対象までの距離に応じて観測ノイズモデルを変更することで、観測対象の状態の推定精度を更に向上させることができる。
［００１９］
また、観測対象は、測定装置の周辺に存在する車両であり、状態推定用モデルは、周辺車両の運動状態を表す運動モデルと、運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、変更手段は、観測対象の速度が高いと、観測対象の速度が低いときに比べて、運動ノイズモデルにおけるステアリング角度の変化量を小さくすることが好ましい。通常、車両の速度が速いと、ステアリングを大きく切る可能性が低い。そこで、観測対象の速度が高いと運動ノイズモデルにおけるステアリング角度の変化量を小さくすることで、観測対象の状態の推定精度を更に向上させることができる。
［００２０］
また、複数の異なる観測モデルを用いて、観測対象の状態を推定するとともに、観測対象の状態の推定分散値を算出し、推定分散値が最も小さい観測対象の状態を出力することが好ましい。これにより、観測対象との位
【手続補正５】
【補正対象書類名】明細書
【補正対象項目名】０００５
【補正方法】変更
【補正の内容】
【０００５】
置関係や観測対象の状態が不明な場合であっても、適切な観測モデルを用いて推定した観測対象の状態を出力することができる。
発明の効果
［００２１］
本発明によれば、高精度に観測対象の状態を推定することができる。
図面の簡単な説明
［００２２］
［図１］本実施形態に係る状態推定装置を示したブロック図である。
［図２］推定したい変数を示した図である。
［図３］第１の実施形態に係る状態推定装置の推定処理を示した図である。
［図４］重心位置の方位角と重心位置の速度向きとを示す図である。
［図５］観測モデルの変更基準例を示す図である。
［図６］右斜後面観測モデルを説明するための図である。
［図７］後面観測モデルを説明するための図である。
［図８］第２の実施形態に係る状態推定装置の推定処理を示した図である。
［図９］第３の実施形態に係る状態推定装置の推定処理を示した図である。
［図１０］第４の実施形態に係る状態推定装置の推定処理を示した図である。
［図１１］第５の実施形態に係る状態推定装置の推定処理を示した図である。
［図１２］図１１のモデル選択処理を示した図である。
［図１３］第６の実施形態に係る状態推定装置の推定処理を示した図である。
［図１４］対象車両とグルーピング点群データとの関係を示した図である。
［図１５］観測ノイズモデルの概念を示した図である。
［図１６］第７の実施形態に係る状態推定装置の推定処理を示した図である。
［図１７］第８の実施形態に係る状態推定装置の推定処理を示した図である。
［図１８］第９の実施形態に係る状態推定装置の推定処理を示した図である。
発明を実施するための形態
［００２３］
以下、図面を参照して、本発明に係る状態推定装置の好適な実施形態について詳細に説明する。なお、全図中、同一又は相当部分には同一符号を付すこととする。
［００２４］
図１は、本実施形態に係る状態推定装置を示したブロック図である。本実
【手続補正書】
【提出日】平成26年3月17日(2014.3.17)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
自車両の周辺に存在する他車両を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて前記他車両の状態を推定する状態推定装置であって、
前記状態推定用モデルは、前記測定装置により測定できる前記他車両の一面又は二面を表した複数の観測モデルを含み、
前記測定データを当てはめる前記観測モデルを、前記測定装置から見える前記他車両の面に対応する前記観測モデルに変更する変更手段を有し、
前記変更手段は、前記測定装置に対する前記他車両の中心位置の方向と前記他車両の向きとの差分から、変更する前記観測モデルを選択する、状態推定装置。
【請求項２】
前記変更手段は、前回の推定で用いた観測モデルに基づいて、前記測定データを当てはめる観測モデルを絞り込む、請求項１に記載の状態推定装置。
【請求項３】
記変更手段は、前回推定した前記他車両の状態に基づいて、前記測定装置に対する前記他車両の中心位置の方向、又は、前記他車両の向きを推定する、請求項１又は２に記載の状態推定装置。
【請求項４】
前記変更手段は、前記他車両が存在する位置の地図情報に基づいて、前記他車両の向きを推定する、請求項１に記載の状態推定装置。
【請求項５】
前記状態推定用モデルは、前記測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、
前記変更手段は、前記他車両の面に対する向きに基づいて、前記観測ノイズモデルの分散値を変更する、請求項１〜４の何れか一項に記載の状態推定装置。
【請求項６】
前記変更手段は、前記他車両までの距離に基づいて、前記観測ノイズモデルを変更する、請求項５に記載の状態推定装置。
【請求項７】
前記状態推定用モデルは、前記他車両の運動状態を表す運動モデルと、前記運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、
前記変更手段は、前記他車両の速度が高いと、前記他車両の速度が低いときに比べて、前記運動ノイズモデルにおけるステアリング角度の変化量を小さくする、請求項１〜６の何れか一項に記載の状態推定装置。
【請求項８】
複数の異なる前記観測モデルを用いて、前記他車両の状態を推定するとともに、前記他車両の状態の推定分散値を算出し、前記推定分散値が最も小さい前記他車両の状態を出力する、請求項１〜７の何れか一項に記載の状態推定装置。
【国際調査報告】
2012117528.tif 000029
2012117528.tif 000030