JP2018055539A - 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体 - Google Patents

移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体 Download PDF

Info

Publication number
JP2018055539A
JP2018055539A JP2016193045A JP2016193045A JP2018055539A JP 2018055539 A JP2018055539 A JP 2018055539A JP 2016193045 A JP2016193045 A JP 2016193045A JP 2016193045 A JP2016193045 A JP 2016193045A JP 2018055539 A JP2018055539 A JP 2018055539A
Authority
JP
Japan
Prior art keywords
vehicle
speed
unit
sensor
error variance
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.)
Pending
Application number
JP2016193045A
Other languages
English (en)
Inventor
平井　義人
Yoshito Hirai
義人 平井
裕人 向井
Hiroto Mukai
裕人 向井
芸芸 曹
Geigei So
芸芸 曹
広志 田中
Hiroshi Tanaka
広志 田中
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.)
Panasonic Intellectual Property Management Co Ltd
Original Assignee
Panasonic Intellectual Property Management Co Ltd
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 Panasonic Intellectual Property Management Co Ltd filed Critical Panasonic Intellectual Property Management Co Ltd
Priority to JP2016193045A priority Critical patent/JP2018055539A/ja
Priority to EP17187861.4A priority patent/EP3301474A1/en
Priority to US15/695,754 priority patent/US20180095103A1/en
Priority to CN201710804136.1A priority patent/CN107884002A/zh
Publication of JP2018055539A publication Critical patent/JP2018055539A/ja
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/64—Devices characterised by the determination of the time taken to traverse a fixed distance
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
    • B60T7/00—Brake-action initiating means
    • B60T7/12—Brake-action initiating means for automatic initiation; for initiation not subject to will of driver or passenger
    • B60T7/22—Brake-action initiating means for automatic initiation; for initiation not subject to will of driver or passenger initiated by contact of vehicle, e.g. bumper, with an external object, e.g. another vehicle, or by means of contactless obstacle detectors mounted on the vehicle
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/10—Path keeping
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/14—Adaptive cruise control
    • B60W30/16—Control of distance between vehicles, e.g. keeping a distance to preceding vehicle
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
    • B60W60/00274—Planning or execution of driving tasks using trajectory prediction for other traffic participants considering possible movement changes
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
    • B60W60/00276—Planning or execution of driving tasks using trajectory prediction for other traffic participants for two or more other traffic participants
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/06—Systems determining position data of a target
    • G01S13/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S13/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G07—CHECKING-DEVICES
    • G07C—TIME OR ATTENDANCE REGISTERS; REGISTERING OR INDICATING THE WORKING OF MACHINES; GENERATING RANDOM NUMBERS; VOTING OR LOTTERY APPARATUS; ARRANGEMENTS, SYSTEMS OR APPARATUS FOR CHECKING NOT PROVIDED FOR ELSEWHERE
    • G07C5/00—Registering or indicating the working of vehicles
    • G07C5/08—Registering or indicating performance data other than driving, working, idle, or waiting time, with or without registering driving, working, idle or waiting time
    • G07C5/0808—Diagnosing performance data
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
    • B60T2201/00—Particular use of vehicle brake systems; Special systems using also the brakes; Special software modules within the brake system controller
    • B60T2201/02—Active or adaptive cruise control system; Distance control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
    • B60T2201/00—Particular use of vehicle brake systems; Special systems using also the brakes; Special software modules within the brake system controller
    • B60T2201/02—Active or adaptive cruise control system; Distance control
    • B60T2201/024—Collision mitigation systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
    • B60T2250/00—Monitoring, detecting, estimating vehicle conditions
    • B60T2250/03—Vehicle yaw rate
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
    • B60T2270/00—Further aspects of brake control systems not otherwise provided for
    • B60T2270/86—Optimizing braking by using ESP vehicle or tire model
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/40—Dynamic objects, e.g. animals, windblown objects
    • B60W2554/404—Characteristics
    • B60W2554/4042—Longitudinal speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/80—Spatial relation or speed relative to objects
    • B60W2554/805—Azimuth angle
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2556/00—Input parameters relating to data
    • B60W2556/20—Data confidence level
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/9327—Sensor installation details
    • G01S2013/93271—Sensor installation details in the front of the vehicles
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/9327—Sensor installation details
    • G01S2013/93274—Sensor installation details on the side of the vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Mechanical Engineering (AREA)
  • Transportation (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

【課題】より高精度に移動体の状態を算出可能な状態算出装置等を提供すること。【解決手段】状態算出装置は、車両において用いられる第一センサによって検知された車両の周辺に存在する複数の物体の方位および車両に対する相対速度を物標情報として受け取り、さらに、車両に搭載されて誤差分散を有する第二センサによって検知された車両の速度および移動方向を状態情報として受け取る入力部と、状態情報を用いて物標情報から抽出された複数の方位および相対速度に基づき車両の速度および移動方向を複数算出し、算出された複数の速度の平均値および誤差分散と、算出された複数の移動方向の平均値および誤差分散と、第二センサによって検知された速度および移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、車両の速度および移動方向のうち少なくとも一方を出力する制御部と、を備えている。【選択図】図１

Description

本開示は、移動体の状態を示す情報を算出する状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体に関する。
従来、この種の状態算出装置等としては、例えば、下記特許文献１に記載の車載装置がある。この車載装置は、車両に搭載された車両状態検知部から得られたデータをフィルタにかける際に、レーダ装置またはカメラから進路を予測した結果を反映する。
特開２０１４−１９１５９６号公報
しかしながら、特許文献１では、車両状態センサから得られる結果と、実際の車両の挙動との間には、様々な要因で誤差が生じる場合がある。このような場合、特許文献１の車載装置は車両の状態を正しく推定することが難しい。
そこで、本開示は、より高精度に移動体の状態を算出可能な状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体を提供することを目的とする。
本開示の一形態は、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取る入力部と、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力する制御部と、
を備えた状態算出装置に向けられる。
本開示の他の形態は、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を備えた状態算出方法に向けられる。
本開示のさらに他の形態は、
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラムに向けられる。
本開示のさらに他の形態は、
コンピュータに、
車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
を実行させるプログラムを記録した記録媒体に向けられる。
本開示によれば、より高精度に移動体の状態を算出可能な状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体を提供することが出来る。
本開示の一実施形態に係る状態算出装置の構成を示すブロック図 図１の第一センサ部の設置位置と、移動速度および移動方位との関係を示す図 図１の移動推定部で使用される方位−ドップラー速度のパワーマップを例示する図 図１の第一センサ部の視野角内における複数の静止物体を示す図 方位−ドップラー速度のパワーマップにおける静止物曲線と静止物体マージンを示す図 図１の移動推定部の処理手順を示すフロー図 図１のフィルタ部の詳細な構成を示すブロック図 図１のフィルタ部への情報入力のタイミングを示す図 誤差の共分散行列推定値における車速の誤差分散値の経時変化を示す図 本開示の変形例に係る状態算出装置の構成を示すブロック図
＜１．実施形態の状態算出装置の構成＞
以下、図面を参照して、本開示の一実施形態に係る状態算出装置１を説明する。
図１において、状態算出装置１は、所謂センサフュージョンにより、後述の第一センサ部３からの物標情報と、第二センサ部５からの状態情報とを組み合わせることで、移動体の状態を高精度に算出する。
本開示では、図１に示すように、移動体の一例としての車両Ｍに、状態算出装置１の一形態としてのＥＣＵ１と、第一センサ部３と、第二センサ部５と、が搭載される。
まず、第一センサ部３について説明する。
第一センサ部３、例えば、ミリ波帯のレーダ送信波を用いたパルス方式、または、ＦＭＣＷ（Ｆｒｅｑｕｅｎｃｙ−Ｍｏｄｕｌａｔｅｄ Ｃｏｎｔｉｎｕｏｕｓ Ｗａｖｅ）方式のレーダセンサである。
具体的には、第一センサ部３は、アレイアンテナ（図示は省略）からレーダ送信波を、自身の視野内に向けて所定の角度刻みで出射する。各出射波が車両Ｍの周囲に存在する物体によって反射され、反射波の一部を戻り波として、第一センサ部３のアレイアンテナが受信する。また、第一センサ部３では、信号処理回路（図示せず）が、各アレイ素子に対応する複数のブランチの信号に対して周波数解析および方位推定を行う。その結果、第一センサ部３は、大略的には予め定められた基準方位に対する反射点の方位（視野角方位）、車両Ｍから反射点までの距離、戻り波の受信強度、車両Ｍに対する反射点のドップラー速度等を物標情報として算出し、例えばＣＡＮやＦｌｅｘＲａｙ等、予め定められたデータ伝送方式に従ってＥＣＵ１に送信する。
第二センサ部５は、状態算出装置１（ひいては車両Ｍ）の移動状態を検出する複数のセンサである。本開示では、第二センサ部５は、少なくとも、車両Ｍの速度（以下、車速という）と、ヨーレートまたはヨー角（以下、ヨーレート等という）を検出する。車速は、周知の車速センサにより、ヨー角はステアリングホイールに設けられた周知の舵角センサにより、また、ヨーレートは周知のヨーセンサ等により検出される。
第二センサ部５は、検出した車速と、ヨーレート等を、状態情報として、例えばＣＡＮ等、予め定められたデータ伝送方式に従ってＥＣＵ１に出力する。
ＥＣＵ１は、上記の通り、状態算出装置１の一実施形態であって、ケース内に収容された基板上に、入力部１１と、制御部１５と、を備えている。
入力部１１は、第一センサ部３からの物標情報を受け取るための入力インタフェイスである。入力部１１はさらに、制御部１５の制御下で、受け取った物標情報を制御部１５のワーキングメモリ（後述）に転送する。これにより、制御部１５は、物標情報を取得する。
入力部１１は、第二センサ部５からの状態情報を受け取るための入力インタフェイスでもある。入力部１１はさらに、制御部１５の制御下で、受け取った状態情報をワーキングメモリへ転送する。これにより、制御部１５は、状態情報を取得する。
制御部１５は、例えば、プログラムメモリと、ワーキングメモリと、マイコンと、を含む。
プログラムメモリは、例えばＥＥＰＲＯＭのような不揮発性メモリである。プログラムメモリには、後述の処理手順を記述したプログラムが予め格納される。
ワーキングメモリは、例えばＳＲＡＭ等の半導体メモリであって、マイコンがプログラムを実行する際に各種演算等に使用される。
マイコンは、ワーキングメモリを用いてプログラムを実行することで、少なくとも、移動推定部と、フィルタ部として機能する。
＜２．状態算出装置の処理＞
まず、移動推定部の処理について、図６のフロー図も参照にしつつ説明する。
移動推定部は、第一センサ部３から得られた物標情報から、反射点の方位およびドップラー速度を取得し、これらに基づき、第一センサ部３の移動速度Ｖｓと移動方位θｓを算出し、車両Ｍの移動速度ＶＶとヨーレートωＶを算出する。
ここで、移動方位θｓは、図２に示すように、第一センサ部３の軸方向（図２中θ＝０°の方位）を基準とする第一センサ部３の移動する方位である。なお、図２の例では、第一センサ部３は、車両Ｍの進行方向において前方左側に設置されている。より具体的には、第一センサ部３は、例えば車両Ｍのバンパー内に設置される。
図３は、移動推定部が取得した方位−ドップラー速度のパワーマップの例示である。
図３において、横軸は方位、縦軸はドップラー速度であり、各丸印が戻り波に対応し、各丸印の大きさがパワー（戻り波強度）を表している。
ここで、図２において、車両Ｍが走行状態で、第一センサ部３の軸方向を基準として移動方向はθｓであり、移動速度がＶｓである。図２の第一センサ部３が測定した静止物体のドップラー速度Ｖは下式（１）で表すことができる。
Figure 2018055539
また、図２の静止物体Ａは、方位−ドップラー速度のパワーマップ上では、図３に例示したように表される。なお、図２および図３において、θａは、静止物体Ａの第一センサ部３の視野角方位である。
静止物体の視野角方位θとドップラー速度Ｖは観測値でありかつ既知であるので、上式（１）から下式（２）が成り立つ。
Figure 2018055539
第一センサ部３の視野角内において、互いの異なる二つ方位、即ち、図４に例示するように、視野角方位θ１，θ２に静止物体Ｂ，Ｃが存在し、静止物体Ｂのドップラー速度がＶ１、Ｃのドップラー速度がＶ２であるとすると、次式（３），（４）からなる連立方程式が得られる。
Figure 2018055539
上式（３），（４）からなる連立方程式に基づいて、二つの静止物体の視野角方位−ドップラー速度から、レーダ移動速度ＶＳと移動方位θＳとは、下式（５）と（６）で算出することができる。
Figure 2018055539
移動推定部は、上式（５），（６）を用いて、移動方位θＳと移動速度ＶＳとを求める。しかし、第一センサ部３で得られた全ての物標情報に対し、上式（５），（６）を適用して良い訳では無く、静止物体の物標情報に限定適用する必要がある。具体的には、静止物体に関しては、前式（１）が成り立つため、上式（５），（６）により移動方位θＳと移動速度ＶＳとが導出可能であることに着目している。
移動推定部は、第二センサ部５から得られた状態情報（即ち、車速とヨーレート等）から、まず、方位−ドップラー速度のパワーマップにおける理論上の静止物曲線を前式（１）に基づいて算出する。静止物曲線とは、静止物体であれば自車両Ｍが移動していたとしても、理論上、方位−ドップラー速度のパワーマップにのる曲線であり、例えば、図５に示す実線で描いた曲線である。
移動推定部は、算出した静止物曲線を基準に、予め設定された設定値を用いて、第一センサ部３の視野角方位毎に、静止物体のドップラー速度値の範囲を算出する（図６のステップＳ００１）。ドップラー速度範囲は、例えば、図５において、二本の点線の曲線で表される。以下、各視野角方位におけるドップラー速度の上限および下限の間を、静止物体マージンと呼ぶ。
移動推定部は、方位−ドップラー速度のパワーマップにおいて、静止物体マージン内の戻り波強度が所定の閾値以上であるものを静止物体からの戻り波に由来するものと定義し、対応する方位θおよびドップラー速度Ｖを静止物体のサンプルとして集める（図６のステップＳ００３）。
移動推定部は、集めた静止物体サンプルに対し、同一方位θにある静止物体サンプルのドップラー速度Ｖの平均値を重心として算出する（図６のステップＳ００５）。なお、この処理は、静止物体が存在しない方位θではスキップされる。
上記処理で、例えばＮ個の方位θの静止物体の重心が算出されたとする。この場合（図６のステップＳ００７でＹＥＳ）、移動推定部は、得られたＮ個の（方位θ，方位θにおける重心）をペアリングし、Ｎ／２個のサンプルペア｛（θ１，Ｖ１），（θ２，Ｖ２）｝を作成する（図６のステップＳ００９，Ｓ０１１）。
次に、移動推定部は静止物体のサンプルペア毎に、前式（５），（６）を用いて移動方位θＳと移動速度ＶＳを算出する（図６のステップＳ０１３）。
次に、移動推定部が、ステップＳ０１３で算出したθＳ、ＶＳ及び第一センサの車両Ｍにおける設置位置情報を用いて、車両基準点（例えば車両の後輪中心）の速度ＶＶとヨーレートωＶを算出する（図６のステップＳ０１４）。
上記ステップＳ００９〜Ｓ０１４が全てのサンプルペアに対し行われると（図６のステップＳ０１５でＹＥＳ）、Ｎ／２個のωＶとＮ／２個のＶＶが算出されたことになる。
ここで、第一センサ部３が出力する物標情報には、反射点の方位（視野角方位）およびドップラー速度には誤差が存在するため、上記処理で算出されたＮ／２個のωＶとＮ／２個のＶＶには誤差が重畳されている。その誤差の影響を軽減するため、移動推定部は、算出結果であるＮ／２個のωＶとＮ／２個のＶＶに対し刈り込み平均処理を行い、それぞれの平均値を、ヨーレートωＶおよび移動速度ＶＶとして出力する（図６のステップＳ０１７）。
具体的には、移動推定部は、得られたＮ／２個のωＶを昇順または降順でソーティング処理し、上位および下位それぞれの所定の割合（例えば２０％）を控除した後、残りの中位（例えば６０％）のωＶから平均値を求めて、ヨーレートωＶとして出力する。移動推定部はさらに、中位に分布する複数のωＶの誤差分散ＰωＶも算出して出力する。
同様に、移動推定部は、得られたＮ／２個のＶＶを昇順または降順でソーティング処理し、上位および下位それぞれの所定の割合（例えば２０％）を控除した後、残りの中位（例えば６０％）のＶＶから平均値を求めて、移動速度ＶＶとして出力する。移動推定部はさらに、中位に分布する複数のＶＶの誤差分散ＰＶＶも算出して出力する。
次に、フィルタ部の処理について説明する。
まず、マイコンは、第二センサ部５から出力される状態情報（本開示では、車速とヨーレート等）から、車速の誤差分散およびヨーレート等の誤差分散を算出する。なお、車速の誤差分散およびヨーレート等の誤差分散は、第二センサ部５の特性であるため、必ずしもマイコンで算出される必要はない。この場合、車速の誤差分散およびヨーレート等の誤差分散はマイコンに予め保持される。
フィルタ部は、第二センサ部５から出力される車速とヨーレート等と、それらの誤差分散と、移動推定部から出力されるヨーレートωＶおよび移動速度ＶＶと、それらの誤差分散ＰωＶ，ＰＶＶと、を入力とする。フィルタ部は、入力信号に対してベイズフィルタ処理を適用する。ベイズフィルタ処理として、本開示ではカルマンフィルタを用いた処理を例示的に説明する。
ここで、図７は、フィルタ部の詳細な構成を示すブロック図である。
図７において、フィルタ部は、車速選択部１５９１と、ヨーレート選択部１５９３、観測更新部１５９５、車速予測部１５９７、ヨーレート予測部１５９９、車速分散値選択部１５１０１、ヨーレート分散値選択部１５１０３で構成される。
車速選択部１５９１は、第二センサ部５および移動推定部から入力される車速および移動速度ＶＶを一系統にまとめて、車速として出力する。第二センサ部５からは、例えば数十ミリ秒の間隔で車速が入力される。なお、第二センサ部５および移動推定部からの入力タイミングは異なっていても良い。また、もし第二センサ部５および移動推定部からの入力タイミングは実質的に同じであれば、車速選択部１５９１は、車速および移動速度ＶＶのいずれか一方を先に出力し、いずれか他方を後に出力する。
ヨーレート選択部１５９３は、第二センサ部５および移動推定部から入力されるヨーレート等およびヨーレートωＶを一系統にまとめて、ヨーレート等として出力する。第二センサ部５からは、例えば数十ミリ間隔でヨーレート等が入力される。上記と同様、第二センサ部５および移動推定部からの入力タイミングは異なっていても良い。また、もし第二センサ部５および移動推定部からの入力タイミングは実質的に同じであれば、ヨーレート選択部１５９３は、ヨーレート等とωVのいずれか一方を先に出力し、いずれか他方を後に出力する。
車速分散値選択部１５１０１は、第二センサ部５から入力される車速の誤差分散と、移動推定部から入力される誤差分散ＰＶＶとを一系統にまとめて、車速の誤差分散として出力する。第二センサ部５からは、車速とその誤差分散とは同期して、車速選択部１５９１および車速分散値選択部１５１０１に入力される。このため、車速分散値選択部１５１０１は、車速選択部１５９１と同様に、車速の誤差分散と、誤差分散ＰＶVを選択する。なお、第二センサ部５から車速の誤差分散の入力が無いか、移動推定部から誤差分散ＰＶVの入力が無い場合には、予め定められた固定的な誤差分散が車速分散値選択部１５１０１に与えられても良い。この場合、固定的な誤差分散は予め測定などにより、第一センサ部３および第二センサ部５が持つ誤差分散値を測定して車速分散値選択部１５１０１に与えておく。
ヨーレート分散値選択部１５１０３は、第二センサ部５から入力されるヨーレート等の誤差分散と、移動推定部から入力される誤差分散ＰωVとを一系統にまとめて、ヨーレート等の誤差分散として出力する。第二センサ部５からは、ヨーレート等とその誤差分散とは同期して、ヨーレート選択部１５９３およびヨーレート分散値選択部１５１０３に入力される。このため、ヨーレート分散値選択部１５１０３は、車速分散値選択部１５１０１と同様に、ヨーレート等の誤差分散と、誤差分散ＰωVを選択する。なお、第二センサ部５からヨーレート等の誤差分散の入力が無いか、移動推定部から誤差分散ＰωVの入力が無い場合には、予め定められた固定的な誤差分散がヨーレート分散値選択部１５１０３に与えられても良い。この場合、固定的な誤差分散は予め測定などにより、第一センサ部３および第二センサ部５が持つ誤差分散値を測定してヨーレート分散値選択部１５１０３に与えておく。
観測更新部１５９５は、車速選択部１５９１の出力（車速および移動速度ＶＳを）と、ヨーレート選択部１５９３の出力（ヨーレート等およびωV）と、車速予測部１５９７が出力する車速予測値と、ヨーレート予測部１５９９が出力するヨーレート予測値を入力とする。観測更新部１５９５は、カルマンフィルタにおける観測更新処理を行う。
ここで、カルマンフィルタについて説明する。カルマンフィルタは一般的な線形カルマンフィルタを使用する。カルマンフィルタは、システムの状態遷移を表す状態方程式と、センサの観測モデルを表す観測方程式とで推定するシステムをモデル化する。
Figure 2018055539
上式（７）および（８）は、カルマンフィルタの状態方程式および観測方程式を示している。ここで、Ｆｋは、システム状態の時間遷移モデル、Ｇｋは、システムノイズの時間遷移モデル、ｗｋは平均ゼロで共分散行列Ｑｋのシステムノイズ、Ｈｋは観測モデル、ｖｋは平均ゼロで共分散行列Ｒｋの観測ノイズである。
以下に示すカルマンフィルタのアルゴリズムを用いて、上記で定義したシステムモデルｘｋを推定する。カルマンフィルタによる推定は予測ステップと観測更新ステップで構成される。
Figure 2018055539
上式（９）および（１０）は、カルマンフィルタの予測ステップの演算を示している。特に、上式（９）は予測推定値を、上式（１０）は予測誤差分散行列の演算を示している。予測推定演算は、前回の推定値ｘ＾ｋ−１｜ｋ−１から、時間遷移モデルＦｋを用いて次の状態ｘ＾ｋ｜ｋ−１を予測する。予測誤差共分散行列の演算では、前回の共分散行列Ｐｋ−１｜ｋ−１とその時間遷移モデルＦｋで共分散行列の状態遷移を計算し、システムノイズ共分散行列Ｑｋとその時間遷移モデルＧｋでシステムノイズの増加分を計算し、それらを加算することにより予測共分散行列Ｐｋ｜ｋ−１を計算する。上式（９）の予測推定値と、上式（１０）の予測誤差共分散行列を予測ステップの出力とする。
Figure 2018055539
上式（１１）〜（１５）はカルマンフィルタの観測更新ステップを示している。上式（４）の推定値ｘ＾ｋ｜ｋと、上式（５）の共分散行列推定値Ｐｋ｜ｋを計算するために、上式（１１）〜（１３）が算出される。観測更新ステップの推定値ｘ＾ｋ｜ｋは、上式（１３）で算出されるカルマンゲインと、上式（１）で算出される観測残差とを用いて計算される。観測残差は、予測値を観測値の空間へ変換し、観測値との残差から算出される。上式（１２）で算出される観測残差の共分散は、測定値の共分散と予測値の共分散から求められる。上式（１３）で算出されるカルマンゲインは、予測値の共分散と観測残差の共分散の比で計算される。
このように求めた（１１）〜（１３）の値を用いて、推定値ｘ＾ｋ｜ｋと、共分散行列推定値Ｐｋ｜ｋとが算出され、観測ステップの出力およびカルマンフィルタの出力とされる。
次に、カルマンフィルタで用いる変数ｘｋ、Ｆｋ、Ｇｋについて説明する。
変数ｘｋは、推定するシステムを表す。本開示の推定する状態は、速度ｖとヨーレートωである。よって、ｘｋ＝（ｖｋ,ωｋ）Ｔとする。
変数Ｆｋは、状態ｘｋの時間遷移を表し、下式（１６）の通りである。
変数Ｗｋは、システムノイズを表し、下式（１７）の通りである。なお、下式（１７）において、ａｖは車両Ｍの進行方向における加速度であり、ａωは、車両Ｍの旋回方向への加速度である。
また、変数Ｇｋは、システムノイズの時間遷移を表し、下式（１８）の通りである。なお、下式（１８）において、Δｔは時刻ｋと１クロック分の時刻だけ前のｋ−１との差を表す。
Figure 2018055539
Ｚｋは、測定値の（ｖｋ，ωｋ）Ｔとする。Ｒｋは、測定値の誤差共分散行列である。それぞれには、第一センサ部３および第二センサ部５で測定したデータを用いる。これらをカルマンフィルタのアルゴリズムに代入して、ｘｋ（つまり、速度ｖとヨーレートω）を推定する。
ここで、図７を参照する。観測更新部１５９５の処理は、上記カルマンフィルタの説明における観測更新ステップを実行する。観測更新部１５９５は、車速とヨーレートとの推定結果を出力する。なお、観測更新部１５９５は必ずしも観測値の入力が無い場合、例えば下記のような処理を行う。すなわち、車速選択部１５９１から出力される車速や、ヨーレート選択部１５９３から出力されるヨーレート等が入力されない場合でも、観測更新部１５９５は、車速とヨーレート等の推定結果を出力する。この場合、観測更新部１５９５は、カルマンフィルタの観測更新ステップを実行せず、観測更新部１５９５へ入力される車速予測値とヨーレート予測値を推定結果として出力する。
車速予測部１５９７は、観測更新部１５９５からの車速の推定値を入力とする。車速予測部１５９７は、入力データに対して１クロック分の時刻だけ進んだｋ＋１の時間の車速を予測して出力する。予測処理は、上記カルマンフィルタの予測ステップに相当する。
ヨーレート予測部１５９９は、観測更新部１５９５からのヨーレート等の推定値を入力とする。ヨーレート予測部１５９９は、入力データに対して１クロック分の時刻だけ進んだｋ＋１の時間のヨーレートを予測して出力する。予測処理は上記カルマンフィルタの予測ステップに相当する。
図８は、第二センサ部５からフィルタ部への車速とヨーレート等と、移動推定部からフィルタ部への移動速度ＶＶおよびヨーレートωＶとの入力タイミングを示している。
上記から明らかなように、車両Ｍの速度ｖに関しては、第二センサ部５が出力する車速と、移動推定部から出力される移動速度ＶＶの二種類が存在する。ヨーレートωに関しても、第二センサ部５が出力するヨーレート等と、移動推定部から出力されるヨーレートωＶの二種類が存在する。
第二センサ部５が車速を出力する間隔と、移動速度ＶＶが出力される間隔とは異なることが多い。ヨーレート等およびωＶについても同様である。さらに、移動推定部からの移動速度ＶＶおよびヨーレートωＶに関しては一定間隔で出力される期間もあれば、そうでない期間も存在する。
フィルタ部は、車速ｖおよびヨーレートωを入力順に処理する。図８において、第二センサ部５からの車速が最初にフィルタ部に入力される（参照符号３０１を参照）。その後、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力され（参照符号３０３，３０５を参照）、その後、第二センサ部５からのヨーレート等が入力され（参照符号３０７を参照）、さらに、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力される（参照符号３０９，３０１１を参照）。その後、第二センサ部５からの車速およびヨーレート等が順番に複数回入力された後（参照符号３０１３，３０１５，３０１７，３０１９を参照）、移動速度ＶＶとヨーレートωＶが同時にフィルタ部に入力される（参照符号３０２１，３０２３を参照）。
＜３．状態算出装置の作用・効果＞
フィルタ部は、入力データを、入力順に処理する。このとき、誤差の共分散行列推定値Ｐｘ｜ｘにおける車速の誤差分散値の変化を図９に示す。
図９において、第二センサ部５からの車速と、移動推定部からの移動速度ＶＶのフィルタ部への入力タイミングは、図８に示した通りである。それ故、図９において、図８に示す車速等と対応するものには同一参照符号を付している。また、本開示では、第二センサ部５が出力する車速よりも、移動推定部により得られた移動速度ＶＶの方が高精度とする。
車速の誤差分散値４０１は、第二センサ部５からの車速のみがフィルタ部へ入力された場合における車速の誤差分散値の経時変化を示している。第二センサ部５からの車速３０１，３０１３，３０１７が入力されるたびに誤差の分散値が小さくなる。従って、第二センサ部５のみからの車速の入力がある程度連続すると、誤差の分散値はある程度の値に収束する。ここで、収束する値は第二センサ部５の精度に依存する。
上記に対し、車速の誤差分散値４０３は、第二センサ部５からの車速に加え、移動推定部の移動速度ＶＶがフィルタ部に入力された場合の車速の誤差分散値の経時変化を示している。第二センサ部５からの車速の入力開始後、誤差分散値４０３は、第二センサ部５だけの場合の誤差分散値４０１と比較して早く小さくなる。これは、移動推定部の測定精度が第二センサ部５よりも良いことと、データ入力間隔が短いことに起因する。第二センサ部５と移動推定部の両方からデータ入力がある時間区間の誤差分散値は、第二センサ部５のみの誤差分散値４０１に比べて小さくなる。このように、第二センサ部５と、移動推定部とが出力する車速を両方用いてカルマンフィルタ処理を実施することにより、車速の推定値の精度が改善できる。
上記の説明と同様に、ヨーレート等の場合も二種類のセンサからの測定結果をカルマンフィルタ処理することにより、車速と同様に、ヨーレート等の推定値の精度を向上させることができる。
さらに、本開示では車速およびヨーレート観測値とその分散値を入力として推定処理カルマンフィルタを実施した。これにより、分散値を固定値で入力する場合と比較して車速とヨーレートの推定値の精度を向上させることができる。
また、上記の通り、二種類の車速の測定結果をカルマンフィルタで処理することにより、推定精度が向上する概念を以下に説明する。一方のセンサの出力する測定値が平均ｘ１、誤差分散Ｐ１であるとする。他方のセンサの出力する測定値が平均ｘ２、誤差分散Ｐ２であるとする。これら誤差分散の異なる二種類の測定結果を重み付け平均することを考える。
誤差分散Ｐ１とＰ２を用いた重み付け平均結果をｘ、その誤差分散をＰとすると、ｘ，Ｐは、以下のように計算できる。
Figure 2018055539
したがって、重み付け平均後の誤差分散Ｐは入力値の誤差分散Ｐ1およびＰ2よりも小さくなる。このように、第二センサ部５から得られる車速およびヨーレート等と、第一センサ部３の物標情報に基づき求めた移動速度ＶＶおよびヨーレートωＶを用いて、カルマンフィルタのような重み付け平均処理を行うことにより、例えば、車両Ｍがスリップ等による影響で第二センサ部５が実際の車両Ｍの挙動と異なる検出結果を出力した場合でも、本開示に係る状態算出装置１によれば、より高精度に車速およびヨーレート等を出力することが可能となる。
＜４．実施形態の付記＞
なお、上記の重みづけ平均の考え方でヨーレートの誤差分散も二種類のセンサの測定値を用いることにより、精度を改善できる。
また、上記実施形態の説明では、移動体として車両Ｍが例示された。しかし、移動体としては、自動二輪や産業用ロボットでも構わない。
また、状態算出装置１において、移動推定部およびフィルタ部はコンピュータプログラムで実現されても良い。コンピュータプログラムは、ＤＶＤ等のような配布媒体に格納されて提供されても良いし、ネットワークを介してダウンロード可能にネットワーク上のサーバ装置に格納されても良い。
＜５．変形例＞
次に、図１０を参照して、上記実施形態の変形例である状態算出装置１ａについて説明する。
図１０において、状態算出装置１ａは、前述の状態算出装置１と比較すると、上記プログラムとは別のプログラムを実行する点で相違する。それ以外に、両状態算出装置１，１ａの間に相違点は無い。それ故、図１０において、図１に示す構成に相当するものには同一参照符号を付け、それぞれの説明を省略する。
なお、マイコンは、別のプログラムを実行することで、移動推定部およびフィルタ部に加え、物体追従部、物体識別部およびアプリケーション部として機能する。
物体追従部は、第一センサ部３からの物標情報と、フィルタ部から出力された車速およびヨーレート等とに基づいて、物標の追従を行う。物標の追従とは、複数フレームの物標情報に亘り、第一センサ部３で観測された物標の位置、距離、移動速度と移動方向等の追跡を行って追従情報を生成することを意味する。追従を行う際に物標の状態が推定されるが、フィルタ部からの車速およびヨーレート等の測定精度が追従性能に大きく影響する。そのため、本開示におけるフィルタ部からの車速およびヨーレート等は物標の追従性能向上に大きく貢献する。物体追従部は、推定された全ての物標の追従情報を物体識別部およびアプリケーション部に出力する。
物体識別部は、物体追従部から出力された追従情報に基づいて、物体の識別を行う。物体の識別とは、追従した物標が例えば、自家用車なのか、トラックなどの大型車なのか、人なのか、バイクなのか、自転車なのか、猫や犬などの動物なのか、ビルや橋などの建築物なのかなどを判定することである。物体識別部は、識別結果をアプリケーション部に出力する。
アプリケーション部は、物体追従部から出力された追従情報と、物体識別部の識別結果とに基づいて、運転を支援するさまざまな機能を実現する。機能の種類は、多く存在するが、以下に例示列挙する。
ＡＣＣ（Ａｄａｐｔｉｖｅ Ｃｒｕｉｓｅ Ｃｏｎｔｒｏｌ）：自車両の前方を走行する車両との車間距離を一定に保つため自動的にアクセル、ブレーキの制御をおこなう。また必要に応じてドライバーへの警告を行う。
衝突被害軽減ブレーキ：前方の障害物との衝突を予想して警報し、衝突被害を軽減するために自車両の制動を制御する。
後側方車両検知警報：走行中の進路変更時、斜め後方に走行車両がいる場合に警報して確認を促す。
自動合流：高速道路の合流時に、合流レーンの他車の状況を判断し自動的に合流する。
＜６．変形例の付記＞
上記変形例の説明では、物体追従部、物体識別部およびアプリケーション部が、移動推定部およびフィルタ部と同一のＥＣＵ１ａに実装されるとして説明した。しかし、これに限らず、物体追従部、物体識別部およびアプリケーション部は、移動推定部およびフィルタ部と異なるＥＣＵに実装されても良い。
本開示に係る状態算出装置、状態算出方法、プログラムおよびこれを記録した記録媒体は、車両の状態を高精度に算出可能であり、車載用途に好適である。
１，１ａ 状態算出装置
１１ 入力部
１５ 制御部
３ 第一センサ部
５ 第二センサ部

Claims (6)

  1. 車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取る入力部と、
    前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力する制御部と、
    を備えた状態算出装置。
  2. 前記制御部は、前記状態情報を用いて、前記物標情報から、複数の方位にある静止物体であろう物標情報を２つずつ抽出し、複数の連立方程式の解を算出することで、前記車両の速度および移動方向を複数算出する、
    請求項１に記載の状態算出装置。
  3. 前記制御部は、前記車両の速度の平均値および誤差分散と、前記車両の移動方向の平均値および誤差分散と、前記状態情報を構成する前記車両の速度および移動方向と、それらの誤差分散とを、カルマンフィルタを用いてフィルタリングする、
    請求項１に記載の状態算出装置。
  4. 車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
    前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
    を備えた状態算出方法。
  5. コンピュータに、
    車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
    前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
    を実行させるプログラム。
  6. コンピュータに、
    車両において用いられる第一センサによって検知された前記車両の周辺に存在する複数の物体の方位および前記車両に対する相対速度を物標情報として受け取り、さらに、前記車両に搭載されて誤差分散を有する第二センサによって検知された前記車両の速度および移動方向を状態情報として受け取るステップと、
    前記状態情報を用いて前記物標情報から抽出された複数の前記方位および前記相対速度に基づき前記車両の速度および移動方向を複数算出し、前記算出された複数の速度の平均値および誤差分散と、前記算出された複数の移動方向の平均値および誤差分散と、前記第二センサによって検知された前記速度および前記移動方向の少なくとも一方とを所定のフィルタを用いてフィルタリングすることで、前記車両の速度および移動方向のうち少なくとも一方を出力するステップと、
    を実行させるプログラムを記録した記録媒体。
JP2016193045A 2016-09-30 2016-09-30 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体 Pending JP2018055539A (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2016193045A JP2018055539A (ja) 2016-09-30 2016-09-30 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体
EP17187861.4A EP3301474A1 (en) 2016-09-30 2017-08-25 State calculation apparatus, state calculation method, and recording medium storing program for moving object
US15/695,754 US20180095103A1 (en) 2016-09-30 2017-09-05 State calculation apparatus, state calculation method, and recording medium storing program for moving object
CN201710804136.1A CN107884002A (zh) 2016-09-30 2017-09-08 移动体的状态计算装置和计算方法、程序和记录介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2016193045A JP2018055539A (ja) 2016-09-30 2016-09-30 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体

Publications (1)

Publication Number Publication Date
JP2018055539A true JP2018055539A (ja) 2018-04-05

Family

ID=59923211

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2016193045A Pending JP2018055539A (ja) 2016-09-30 2016-09-30 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体

Country Status (4)

Country Link
US (1) US20180095103A1 (ja)
EP (1) EP3301474A1 (ja)
JP (1) JP2018055539A (ja)
CN (1) CN107884002A (ja)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6597517B2 (ja) * 2016-08-10 2019-10-30 株式会社デンソー 物標検出装置
JP7188894B2 (ja) * 2018-03-16 2022-12-13 株式会社デンソーテン レーダ装置及び信号処理方法
DE102018122054A1 (de) * 2018-09-10 2020-03-12 Wabco Gmbh Querlenkverfahren und Querlenkvorrichtung für das Bewegen eines Fahrzeugs in eine Zielposition, und Fahrzeug dafür
US11460568B2 (en) * 2019-08-29 2022-10-04 Zoox, Inc. Estimating in-plane velocity from an arbitrary radar return
US11420630B2 (en) 2019-10-24 2022-08-23 Zoox, Inc. Trajectory modifications based on a collision zone
US11643073B2 (en) * 2019-10-24 2023-05-09 Zoox, Inc. Trajectory modifications based on a collision zone
EP4042200A4 (en) * 2019-11-08 2024-02-07 Vayyar Imaging Ltd SYSTEMS AND METHODS FOR DETECTING THE SURROUNDINGS OF A VEHICLE
KR20210076412A (ko) * 2019-12-16 2021-06-24 현대자동차주식회사 차량 센서 성능 평가 장치 및 그의 성능 평가 방법
CN115039095A (zh) * 2020-02-17 2022-09-09 华为技术有限公司 目标跟踪方法以及目标跟踪装置

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE19736965C1 (de) * 1997-08-25 1999-05-06 Mannesmann Vdo Ag Verfahren und Anordnung zur Überprüfung der Gierrate eines sich bewegenden Objektes
US8855848B2 (en) * 2007-06-05 2014-10-07 GM Global Technology Operations LLC Radar, lidar and camera enhanced methods for vehicle dynamics estimation
JP5610847B2 (ja) * 2010-05-26 2014-10-22 三菱電機株式会社 角速度推定装置及びコンピュータプログラム及び角速度推定方法
CN102959600B (zh) * 2010-06-29 2015-01-14 本田技研工业株式会社 车辆行进路线推定装置
JP5852036B2 (ja) 2013-03-27 2016-02-03 株式会社日本自動車部品総合研究所 車載装置
JP6425130B2 (ja) * 2014-12-18 2018-11-21 パナソニックＩｐマネジメント株式会社 レーダ装置及びレーダ状態推定方法
US9903945B2 (en) * 2015-02-04 2018-02-27 GM Global Technology Operations LLC Vehicle motion estimation enhancement with radar data

Also Published As

Publication number Publication date
EP3301474A1 (en) 2018-04-04
CN107884002A (zh) 2018-04-06
US20180095103A1 (en) 2018-04-05

Similar Documents

Publication Publication Date Title
JP2018055539A (ja) 移動体の状態算出装置、状態算出方法、プログラムおよびそれを記録した記録媒体
CN107792079B (zh) 利用路径预测的自主车辆
US9733348B2 (en) Vehicle radar with beam adjustment
CN113015922B (zh) 一种检测方法、检测装置以及存储介质
US10338208B2 (en) Object detection in multiple radars
CN110194162B (zh) 用于基于可用空间信号来检测靠近切入车辆的系统和方法
CN108536139B (zh) 驾驶车辆的系统和方法
US11586862B2 (en) Enhanced object detection with clustering
US20180120419A1 (en) Vehicle radar control
US11477567B2 (en) Method and system for locating an acoustic source relative to a vehicle
CN114170274B (zh) 目标跟踪方法和装置、电子设备和存储介质
US20220113397A1 (en) Target tracking device
CN111103587A (zh) 用于预测同时并线车辆的方法和设备及包括其的车辆
JP2019144094A (ja) レーダ装置及び信号処理方法
CN114518574A (zh) 用于传感器融合系统的基于峰度的修剪
JP2008190904A (ja) 移動速度推定装置
CN111337926B (zh) 经由高阶相位项的多普勒模糊分辨率
CN110678776B (zh) 用于增强的对象跟踪的系统
US20230103178A1 (en) Systems and methods for onboard analysis of sensor data for sensor fusion
US11555919B2 (en) Radar calibration system
CN111596267B (zh) 通过跟踪解决雷达系统中的多普勒模糊
CN110691985B (zh) 用于增强的对象追踪的系统
US20240194077A1 (en) Method for operating a driver assistance system, computer program product, driver assistance system, and vehicle
US20240199024A1 (en) Lane-based vehicle control
CN116893391A (zh) 至少部分地基于低置信度检测的跟踪关联