JP5614489B2 - 状態推定装置 - Google Patents

状態推定装置 Download PDF

Info

Publication number
JP5614489B2
JP5614489B2 JP2013502098A JP2013502098A JP5614489B2 JP 5614489 B2 JP5614489 B2 JP 5614489B2 JP 2013502098 A JP2013502098 A JP 2013502098A JP 2013502098 A JP2013502098 A JP 2013502098A JP 5614489 B2 JP5614489 B2 JP 5614489B2
Authority
JP
Japan
Prior art keywords
observation
model
vehicle
state
state estimation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
JP2013502098A
Other languages
English (en)
Other versions
JPWO2012117528A1 (ja
Inventor
中村　弘
弘 中村
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Publication of JPWO2012117528A1 publication Critical patent/JPWO2012117528A1/ja
Application granted granted Critical
Publication of JP5614489B2 publication Critical patent/JP5614489B2/ja
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/66—Tracking systems using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/165—Anti-collision systems for passive traffic, e.g. including static obstacles, trees
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Evolutionary Biology (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Algebra (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Description

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

Claims (8)

  1. 自車両の周辺に存在する他車両を測定する測定装置により測定した測定データを状態推定用モデルに当てはめて前記他車両の状態を推定する状態推定装置であって、
    前記状態推定用モデルは、前記測定装置により測定できる前記他車両の一面又は二面を表した複数の観測モデルを含み、
    前記測定データを当てはめる前記観測モデルを、前記測定装置から見える前記他車両の面に対応する前記観測モデルに変更する変更手段を有し、
    前記変更手段は、前記測定装置に対する前記他車両の中心位置の方向と前記他車両の向きとの差分から、変更する前記観測モデルを選択する、状態推定装置。
  2. 前記変更手段は、前回の推定で用いた観測モデルに基づいて、前記測定データを当てはめる観測モデルを絞り込む、請求項１に記載の状態推定装置。
  3. 記変更手段は、前回推定した前記他車両の状態に基づいて、前記測定装置に対する前記他車両の中心位置の方向、又は、前記他車両の向きを推定する、請求項１又は２に記載の状態推定装置。
  4. 前記変更手段は、前記他車両が存在する位置の地図情報に基づいて、前記他車両の向きを推定する、請求項１に記載の状態推定装置。
  5. 前記状態推定用モデルは、前記測定装置の測定により生じる観測ノイズを分散値で表す観測ノイズモデルを含み、
    前記変更手段は、前記他車両の面に対する向きに基づいて、前記観測ノイズモデルの分散値を変更する、請求項１〜４の何れか一項に記載の状態推定装置。
  6. 前記変更手段は、前記他車両までの距離に基づいて、前記観測ノイズモデルを変更する、請求項５に記載の状態推定装置。
  7. 前記状態推定用モデルは、前記他車両の運動状態を表す運動モデルと、前記運動モデルにおけるステアリング角度の変化量を示す運動ノイズモデルと、を含み、
    前記変更手段は、前記他車両の速度が高いと、前記他車両の速度が低いときに比べて、前記運動ノイズモデルにおけるステアリング角度の変化量を小さくする、請求項１〜６の何れか一項に記載の状態推定装置。
  8. 複数の異なる前記観測モデルを用いて、前記他車両の状態を推定するとともに、前記他車両の状態の推定分散値を算出し、前記推定分散値が最も小さい前記他車両の状態を出力する、請求項１〜７の何れか一項に記載の状態推定装置。
JP2013502098A 2011-03-01 2011-03-01 状態推定装置 Expired - Fee Related JP5614489B2 (ja)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2011/054651 WO2012117528A1 (ja) 2011-03-01 2011-03-01 状態推定装置

Publications (2)

Publication Number Publication Date
JPWO2012117528A1 JPWO2012117528A1 (ja) 2014-07-07
JP5614489B2 true JP5614489B2 (ja) 2014-10-29

Family

ID=46757490

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2013502098A Expired - Fee Related JP5614489B2 (ja) 2011-03-01 2011-03-01 状態推定装置

Country Status (5)

Country Link
US (1) US20130332112A1 (ja)
JP (1) JP5614489B2 (ja)
CN (1) CN103492903B (ja)
DE (1) DE112011104992T5 (ja)
WO (1) WO2012117528A1 (ja)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6035671B2 (ja) * 2012-09-27 2016-11-30 株式会社Ihi デバイスの状態同定方法と装置
US9563530B2 (en) * 2013-12-03 2017-02-07 Kabushiki Kaisha Toshiba Device state estimation apparatus, device power consumption estimation apparatus, and program
JP6091658B2 (ja) * 2013-12-26 2017-03-08 三菱電機株式会社 情報処理装置及び情報処理方法及びプログラム
CN104730537B (zh) * 2015-02-13 2017-04-26 西安电子科技大学 基于多尺度模型的红外/激光雷达数据融合目标跟踪方法
US9576185B1 (en) * 2015-09-14 2017-02-21 Toyota Motor Engineering & Manufacturing North America, Inc. Classifying objects detected by 3D sensors for autonomous vehicle operation
DE112015007054B4 (de) 2015-11-20 2019-11-28 Mitsubishi Electric Corp. Fahrunterstützungsvorrichtung, fahrunterstützungssystem, fahrunterstützungsverfahren und fahrunterstützungsprogramm
US10557942B2 (en) 2016-06-07 2020-02-11 DSCG Solutions, Inc. Estimation of motion using LIDAR
JP6836940B2 (ja) * 2017-03-22 2021-03-03 本田技研工業株式会社 レーザ式測距装置のノイズデータの特定方法
EP3470878B1 (de) * 2017-10-13 2023-04-19 Leuze electronic GmbH + Co. KG Optischer sensor
US10408638B2 (en) * 2018-01-04 2019-09-10 Mitsubishi Electric Research Laboratories, Inc. System and method for controlling a vehicle under sensor uncertainty
CN111868564B (zh) 2018-03-22 2024-05-07 日立安斯泰莫株式会社 物体识别装置
US10369923B1 (en) * 2018-04-30 2019-08-06 Automotive Research & Testing Center Operation method of adaptive driving beam headlamp system
DE102018206751A1 (de) * 2018-05-02 2019-11-07 Continental Automotive Gmbh Konturerkennung eines fahrzeugs anhand von messdaten einer umfeldsensorik
JP7318600B2 (ja) * 2020-07-06 2023-08-01 トヨタ自動車株式会社 車両及び他車両の認識方法
US11420647B2 (en) * 2020-08-13 2022-08-23 Argo AI, LLC Enhanced static object classification using lidar
EP4148460A1 (de) * 2021-09-13 2023-03-15 Leuze electronic GmbH + Co. KG Optischer sensor und verfahren zur detektion von objekten mittels eines optischen sensors

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005274300A (ja) * 2004-03-24 2005-10-06 Mitsubishi Electric Corp 目標追尾装置
JP2009042181A (ja) * 2007-08-10 2009-02-26 Denso Corp 推定装置
JP2009128298A (ja) * 2007-11-27 2009-06-11 Mitsubishi Electric Corp 目標追尾装置

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6121919A (en) * 1999-07-23 2000-09-19 Eaton-Vorad Technologies, L.L.C. Method and apparatus for range correction in a radar system
US6628227B1 (en) * 2002-07-23 2003-09-30 Ford Global Technologies, Llc Method and apparatus for determining a target vehicle position from a source vehicle using a radar
JP4433887B2 (ja) * 2003-07-01 2010-03-17 日産自動車株式会社 車両用外界認識装置
US8855848B2 (en) * 2007-06-05 2014-10-07 GM Global Technology Operations LLC Radar, lidar and camera enhanced methods for vehicle dynamics estimation
JP2009098023A (ja) * 2007-10-17 2009-05-07 Toyota Motor Corp 物体検出装置及び物体検出方法
US20090292468A1 (en) * 2008-03-25 2009-11-26 Shunguang Wu Collision avoidance method and system using stereo vision and radar sensor fusion
JP5051468B2 (ja) * 2008-12-25 2012-10-17 トヨタ自動車株式会社 センサ校正装置、及び、センサ校正方法
FR2953021B1 (fr) * 2009-11-26 2011-12-09 Tanguy Griffon Methode de mesure des emissions hebdomadaires et annuelles d'un gaz a effet de serre sur une surface donnee

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005274300A (ja) * 2004-03-24 2005-10-06 Mitsubishi Electric Corp 目標追尾装置
JP2009042181A (ja) * 2007-08-10 2009-02-26 Denso Corp 推定装置
JP2009128298A (ja) * 2007-11-27 2009-06-11 Mitsubishi Electric Corp 目標追尾装置

Also Published As

Publication number Publication date
CN103492903A (zh) 2014-01-01
DE112011104992T5 (de) 2014-01-23
US20130332112A1 (en) 2013-12-12
CN103492903B (zh) 2015-04-01
WO2012117528A1 (ja) 2012-09-07
JPWO2012117528A1 (ja) 2014-07-07

Similar Documents

Publication Publication Date Title
JP5614489B2 (ja) 状態推定装置
US9797981B2 (en) Moving-object position/attitude estimation apparatus and moving-object position/attitude estimation method
CN105539437B (zh) 对象检测装置
US8989913B2 (en) Travel route estimation device and travel route estimation method used in the same device
JP5664252B2 (ja) 歩行者検出装置
CN110632617B (zh) 一种激光雷达点云数据处理的方法及装置
US9031773B2 (en) Apparatus and method for detecting narrow road in front of vehicle
JP7487388B2 (ja) 測定装置、測定方法、及び、プログラム
US8847792B2 (en) Object detection apparatus and object detection program
JP2005025458A (ja) 車両用外界認識装置
JP5651642B2 (ja) 物体位置検知装置
US20210245777A1 (en) Map generation device, map generation system, map generation method, and storage medium
JP5904226B2 (ja) 車両挙動予測装置及びプログラム
JP2018054315A (ja) 軸ずれ推定装置
KR20150106200A (ko) 차량 레이더 오차 보정 시스템 및 그 방법
JP2017138660A (ja) 物体検出方法、物体検出装置、およびプログラム
CN104620297A (zh) 速度算出装置、速度算出方法以及碰撞判定装置
KR20190051601A (ko) 차량의 센서 데이터 연계 장치 및 방법
JP2008003662A (ja) 車両識別システム
JP2013113789A (ja) 速度推定装置及びプログラム
JPWO2019123582A1 (ja) 物体情報生成装置及び物体情報生成プログラム
JP5682302B2 (ja) 走行道路推定装置、方法およびプログラム
JP7308141B2 (ja) 自己位置推定方法及び自己位置推定装置
JP4506163B2 (ja) 前方物体検出装置及び前方物体検出方法
US20230242102A1 (en) Position estimation system

Legal Events

Date Code Title Description
TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20140812

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20140825

LAPS Cancellation because of no payment of annual fees