JP6693145B2

JP6693145B2 - State estimation device

Info

Publication number: JP6693145B2
Application number: JP2016012550A
Authority: JP
Inventors: 利之松原; 渚小山; 照元小森
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2016-01-26
Filing date: 2016-01-26
Publication date: 2020-05-13
Anticipated expiration: 2036-01-26
Also published as: JP2017134515A

Description

本発明は、状態推定装置に関するものである。 The present invention relates to a state estimation device.

従来、下記特許文献１のように、カルマンフィルタを用いて先行車の位置と速度とを推定する推定装置が知られている。この特許文献１の推定装置では、カルマンフィルタが予測する先行車の位置の予測値と、センサで計測した先行車の位置の計測値との差が所定値以上の場合には、センサの計測値を棄却しカルマンフィルタに反映させないようにしている。 BACKGROUND ART Conventionally, an estimation device that estimates the position and speed of a preceding vehicle using a Kalman filter is known, as in Patent Document 1 below. In the estimation device of Patent Document 1, when the difference between the predicted value of the position of the preceding vehicle predicted by the Kalman filter and the measured value of the position of the preceding vehicle measured by the sensor is equal to or more than a predetermined value, the measured value of the sensor is calculated. It is rejected and is not reflected in the Kalman filter.

特開平9-96521号公報JP-A-9-96521

しかしながら、上記推定装置では、カルマンフィルタによる予測値とセンサによる計測値との差を判断基準として、計測値を棄却するか否かを決定しているので、カルマンフィルタによる予測値が不正確である推定初期段階では、センサの計測値が不必要に棄却されるおそれがあった。 However, in the above estimation device, the difference between the predicted value by the Kalman filter and the measured value by the sensor is used as the criterion, and it is determined whether or not to reject the measured value. Therefore, the predicted value by the Kalman filter is inaccurate. At the stage, the measured value of the sensor might be unnecessarily discarded.

本発明は、推定初期段階において計測値が不必要に棄却される現象が生じることを抑制する状態推定装置を提供することを目的とする。 It is an object of the present invention to provide a state estimation device that suppresses the phenomenon that measurement values are unnecessarily rejected at the initial stage of estimation.

本発明の状態推定装置は、前回得られた物体の状態の推定値に基づいて物体の状態の予測値を得ると共に、物体の状態の観測値に基づいて予測値を更新し、更新された予測値に基づいて今回の推定値を得るカルマンフィルタによって、物体の状態を推定する状態推定装置であって、前回の推定値に基づいて予測値を得る予測部と、物体の状態を観測して観測値を得る観測部と、観測値と予測値とに基づいて演算を行い今回の推定値の候補である推定値候補を得る観測更新部と、予測値と推定値候補との差が所定範囲外である場合には、観測更新部による演算で用いられる所定のパラメータを変更し、差が所定範囲内である場合には、推定値候補を今回の推定値として出力する収束確認部と、を備え、差が所定範囲外である場合には、差が所定範囲内になるまで、収束確認部によるパラメータの変更と、変更後の当該パラメータを用いた観測更新部による演算と、が繰り返し実行される。 The state estimation device of the present invention obtains a predicted value of the state of the object based on the previously obtained estimated value of the state of the object, updates the predicted value based on the observed value of the state of the object, and the updated prediction A state estimation device that estimates the state of an object by a Kalman filter that obtains the estimated value this time based on the value, and a prediction unit that obtains the predicted value based on the previous estimated value, and an observed value that observes the state of the object , An observation updating unit that obtains an estimated value candidate that is a candidate for the current estimated value by performing an operation based on the observed value and the estimated value, and the difference between the estimated value and the estimated value candidate is outside the predetermined range. In some cases, a predetermined parameter used in the calculation by the observation update unit is changed, and if the difference is within a predetermined range, a convergence check unit that outputs an estimated value candidate as the current estimated value, If the difference is outside the specified range, the difference is Until 囲内, the change of parameters by the convergence checking unit, and the operation by observing update unit using the parameter after the change, but it is repeatedly executed.

本発明によれば、推定初期段階において計測値が不必要に棄却される現象が生じることを抑制する状態推定装置を提供することができる。 ADVANTAGE OF THE INVENTION According to this invention, the state estimation apparatus which suppresses that a phenomenon that a measured value is unnecessarily discarded in an estimation initial stage arises can be provided.

実施形態に係る状態推定装置を示すブロック図である。It is a block diagram which shows the state estimation apparatus which concerns on embodiment. 状態推定装置が搭載される自車両と状態推定の対象である他車両とを示す平面図である。It is a top view showing the own vehicle in which a state estimating device is carried, and other vehicles which are the targets of state estimation. 状態推定装置による処理（ｔ）での処理を示すフローチャートである。It is a flowchart which shows the process in the process (t) by a state estimation apparatus.

以下、図面を参照しつつ本発明に係る状態推定装置の実施形態について詳細に説明する。図１に示される状態推定装置１は、カルマンフィルタ１０を用いて、物体の状態を推定するものである。すなわち、状態推定装置１は、カルマンフィルタ１０を用いて、前回の処理で得られた物体の状態の推定値に基づいて物体の状態の予測値を得ると共に、物体の状態の観測値に基づいて予測値を更新し今回の処理における推定値を得る。状態推定装置１では、推定値を得るための上記のような状態推定処理が繰返し実行される。以下の説明においては、繰返し実行される状態推定処理のうちのｊ回目の処理を「処理（ｊ）」と表すものとする（ｊは自然数）。状態推定装置１は、例えば、物体を検出する物体検出装置の一部として存在するものである。 Hereinafter, an embodiment of a state estimation device according to the present invention will be described in detail with reference to the drawings. The state estimation device 1 shown in FIG. 1 estimates the state of an object using a Kalman filter 10. That is, the state estimation device 1 uses the Kalman filter 10 to obtain the predicted value of the state of the object based on the estimated value of the state of the object obtained in the previous process, and also performs the prediction based on the observed value of the state of the object. The value is updated to obtain the estimated value in this processing. In the state estimation device 1, the above state estimation process for obtaining the estimated value is repeatedly executed. In the following description, the j-th process of the repeatedly performed state estimation process is represented as “process (j)” (j is a natural number). The state estimation device 1 exists, for example, as a part of an object detection device that detects an object.

図２に示されるように、状態推定装置１が自車両３１に搭載される場合、状態を推定する対象の物体は、例えば、自車両３１の周辺に存在し後述する計測部３（例えばレーザレーダ）により検出可能な範囲に位置する障害物である。図２の例の場合、上記障害物は、自車両３１の周辺を走行する他車両Ｄのうちの一台（以下「他車両３３」）である。物体の状態とは、例えば、自車両３１に対する他車両３３の相対位置及び相対速度である。以下の説明では、自車両３１に搭載された状態推定装置１が、他車両３３の相対位置及び相対速度を推定する場合を例として説明する。なお、以下の説明において、他車両３３の状態に関して、単に「位置」、「速度」と言う場合には、自車両３１に対する他車両３３の相対位置及び相対速度を意味するものとする。 As shown in FIG. 2, when the state estimation device 1 is mounted on the host vehicle 31, the object whose state is to be estimated is, for example, a measuring unit 3 (for example, a laser radar) that exists around the host vehicle 31 and will be described later. ) Is an obstacle located in a range detectable by. In the case of the example of FIG. 2, the obstacle is one of the other vehicles D (hereinafter referred to as “other vehicle 33”) traveling around the host vehicle 31. The state of the object is, for example, the relative position and the relative speed of the other vehicle 33 with respect to the own vehicle 31. In the following description, the case where the state estimation device 1 mounted on the host vehicle 31 estimates the relative position and relative speed of the other vehicle 33 will be described as an example. In the following description, when referring to the state of the other vehicle 33, simply referring to “position” and “speed” means the relative position and relative speed of the other vehicle 33 with respect to the own vehicle 31.

図１に示されるように、状態推定装置１は、計測部（観測部）３とＥＣＵ[EngineControl Unit]５とを備える。 As shown in FIG. 1, the state estimation device 1 includes a measurement unit (observation unit) 3 and an ECU [Engine Control Unit] 5.

例えば、計測部３は、自車両３１に搭載されるレーザレーダ(LIDAR［Laser Imaging Detection and Ranging］)であってもよい。レーザレーダは、自車両３１の前端に設けられ、レーザーを利用して車両前方の障害物（例えば、他車両３３）を検出する。レーザレーダは、レーザーを車両前方に送信し、障害物に反射したレーザーを受信することで障害物を検出する。レーザレーダは、検出した障害物に関する障害物情報をＥＣＵ５に送信する。なお、レーザレーダに代えて、ミリ波レーダ等を用いてもよい。ミリ波レーダは、自車両３１の車体前端及び自車両３１の車体後端に設けられ、ミリ波を利用して自車両３１前後の障害物を検出する。ミリ波レーダは、更に自車両３１の側面に設けられ、自車両３１の側方の障害物を検出してもよい。ミリ波レーダは、ミリ波を自車両３１の前後に送信し、他車両等の障害物に反射したミリ波を受信することで障害物を検出する。ミリ波レーダは、検出した障害物に関する障害物情報をＥＣＵ５に送信する。なお、計測部３からＥＣＵ５に送信される上記の障害物情報には、障害物（例えば、他車両３３）の位置情報や速度情報が含まれる。 For example, the measurement unit 3 may be a laser radar (LIDAR [Laser Imaging Detection and Ranging]) mounted on the vehicle 31. The laser radar is provided at the front end of the vehicle 31, and detects an obstacle (for example, another vehicle 33) in front of the vehicle by using a laser. The laser radar detects an obstacle by transmitting a laser to the front of the vehicle and receiving the laser reflected by the obstacle. The laser radar transmits obstacle information regarding the detected obstacle to the ECU 5. A millimeter wave radar or the like may be used instead of the laser radar. The millimeter-wave radar is provided at the front end of the vehicle body of the host vehicle 31 and at the rear end of the vehicle body of the host vehicle 31, and detects obstacles in front of and behind the host vehicle 31 using millimeter waves. The millimeter wave radar may be further provided on the side surface of the own vehicle 31 to detect an obstacle on the side of the own vehicle 31. The millimeter wave radar detects an obstacle by transmitting a millimeter wave before and after the vehicle 31 and receiving a millimeter wave reflected by an obstacle such as another vehicle. The millimeter wave radar transmits obstacle information regarding the detected obstacle to the ECU 5. The obstacle information transmitted from the measuring unit 3 to the ECU 5 includes position information and speed information of the obstacle (for example, the other vehicle 33).

ＥＣＵ５は、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]、ＣＡＮ［Controller Area Network］通信回路等を有する電子制御ユニットである。ＥＣＵ５では、例えば、ＲＯＭに記憶されているプログラムをＣＡＮ通信回路を介してＲＡＭにロードし、ＲＡＭにロードされたプログラムをＣＰＵで実行することにより、後述する予測部１１、観測更新部１３、及び収束確認部１５等の各種の機能を実現する。ＥＣＵ５は、複数の電子制御ユニットから構成されていてもよい。 The ECU 5 is an electronic control unit including a CPU [Central Processing Unit], a ROM [Read Only Memory], a RAM [Random Access Memory], a CAN [Controller Area Network] communication circuit, and the like. In the ECU 5, for example, the program stored in the ROM is loaded into the RAM via the CAN communication circuit, and the program loaded in the RAM is executed by the CPU, whereby the prediction unit 11, the observation update unit 13, and the Various functions such as the convergence confirmation unit 15 are realized. The ECU 5 may be composed of a plurality of electronic control units.

ＥＣＵ５は、前述の通り、他車両３３の位置及び速度を、カルマンフィルタ１０を用いて推定する状態推定処理を実行する。具体的には、状態推定処理では、他車両３３のｘ方向の位置（ｘ）、ｙ方向の位置（ｙ）、ｘ方向の速度（ｖ_x）及びｙ方向の速度（ｖ_y）が推定される。任意のｔ回目の状態推定処理（処理（ｔ））で得られる、他車両３３の位置及び速度の推定値Ｍ_tは、下式(1)で定義される。

As described above, the ECU 5 executes the state estimation process of estimating the position and speed of the other vehicle 33 using the Kalman filter 10. Specifically, in the state estimation process, the position (x) of the other vehicle 33 in the x direction, the position (y) in the y direction, the speed in the x direction (v _x ) and the speed in the y direction (v _y ) are estimated. It The estimated value M _t of the position and speed of the other vehicle 33, which is obtained by the arbitrary t-th state estimation process (process (t)), is defined by the following equation (1).

また、ＥＣＵ５には、計測部３で計測された他車両３３の位置（ｘ，ｙ）の計測値が入力される。上記計測値は、前述の通り、他車両３３の位置情報として、計測部３から送信された障害物情報に含まれている。この計測値は、上記カルマンフィルタ１０に入力される観測値ｚ_tとして状態推定処理に使用される。この観測値ｚ_tは、下式(2)で定義される。

In addition, the measurement value of the position (x, y) of the other vehicle 33 measured by the measurement unit 3 is input to the ECU 5. As described above, the measured value is included in the obstacle information transmitted from the measurement unit 3 as the position information of the other vehicle 33. This measured value is used in the state estimation process as the observed value z _t input to the Kalman filter 10. This observed value z _t is defined by the following equation (2).

続いて、ＥＣＵ５の機能的構成について説明を行う。ＥＣＵ５は、カルマンフィルタ１０を構成する、予測部１１と、観測更新部１３と、収束確認部１５と、を備えている。また、収束確認部１５は、収束判定部２１と、切替部２３と、パラメータ変更部２５と、を有している。 Next, the functional configuration of the ECU 5 will be described. The ECU 5 includes a prediction unit 11, an observation update unit 13, and a convergence confirmation unit 15, which form the Kalman filter 10. The convergence confirmation unit 15 also includes a convergence determination unit 21, a switching unit 23, and a parameter changing unit 25.

予測部１１は、前回の処理（ｔ−１）で得られた、他車両３３の位置及び速度の推定値Ｍ_t-1に基づいて、今回の処理（ｔ）における位置及び速度の予測値ｍ_tを更新し出力する。具体的には、予測部１１は、下式(3),(4)によって、予測値ｍ_tを算出する。

The prediction unit 11 calculates the position and speed predicted value m in the current process (t) based on the position and speed estimated value M _t-1 of the other vehicle 33 obtained in the previous process (t-1). Update and output _t . Specifically, the prediction unit 11 calculates the predicted value m _t by the following equations (3) and (4).

ここでΔｔは、処理（ｔ−１）から処理（ｔ）までに要する時間を表す。なお、予測部１１による予測値ｍ_tの算出においては、自車両３１の走行制御に関する他の制御入力等の情報が更にカルマンフィルタ１０に入力され、当該制御入力等の情報が予測値ｍ_tに反映されてもよい。また、他車両３３の走行制御に関する制御入力の情報が通信等で取得可能な場合には、当該他車両３３の情報が更にカルマンフィルタ１０に入力され、当該他車両３３の情報が予測値ｍ_tに反映されてもよい。 Here, Δt represents the time required from the processing (t-1) to the processing (t). Note that in the calculation of the predicted value m _t by the prediction unit 11, information such as other control inputs related to the traveling control of the vehicle 31 is further input to the Kalman filter 10, and the information such as the control input is reflected in the predicted value m _t . May be done. In addition, when the information of the control input regarding the traveling control of the other vehicle 33 can be acquired by communication or the like, the information of the other vehicle 33 is further input to the Kalman filter 10, and the information of the other vehicle 33 becomes the predicted value m _t . May be reflected.

更に、予測部１１は、前回の処理（ｔ−１）の推定誤差共分散Ｓ_t-1に基づいて、今回の処理（ｔ）における予測誤差共分散ｓ_tを算出し出力する。具体的には、予測部１１は、下式(5),(6)によって、予測誤差共分散ｓ_tを算出する。

Furthermore, the prediction unit 11, based on the estimated error covariance S _t-1 of the previous process (t-1), to calculate the prediction error covariance s _t in the current process (t) output. Specifically, the prediction unit 11, the following equation (5), by (6), to calculate the prediction error covariance s _t.

ここで、Ｒは、処理（ｔ）での予測に伴う推定誤差の拡大を表す行列であり、システムノイズ共分散と呼ばれる。σ_x ²，σ_y ²，σ_vx ²，及びσ_vy ²は、他車両３３のｘ方向の位置、ｙ方向の位置、ｘ方向の速度、及びｙ方向の速度、のそれぞれの分散値であり、これらの値は、状態推定装置１の特性に応じて予め設定され、例えばＥＣＵ５の記憶領域に保存されている。 Here, R is a matrix representing the expansion of the estimation error accompanying the prediction in the process (t), and is called the system noise covariance. σ _x ² , σ _y ² , σ _vx ² , and σ _vy ² are respective dispersion values of the position of the other vehicle 33 in the x direction, the position in the y direction, the velocity in the x direction, and the velocity in the y direction. , These values are preset according to the characteristics of the state estimation device 1, and are stored in, for example, the storage area of the ECU 5.

観測更新部１３は、予測部１１で得られた予測値ｍ_t及び予測誤差共分散ｓ_tと、計測部３から入力された観測値ｚ_tと、に基づいて、推定値候補Ｍ'_tを算出し出力する。具体的には、観測更新部１３は、下式(7),(8),(9),(10)によって、推定値候補Ｍ'_tを算出する。推定値候補Ｍ'_tは、処理（ｔ）における推定値Ｍ_tの候補となる。

The observation updating unit 13 determines the estimated value candidate M ′ _t based on the prediction value m _t and the prediction error covariance s _t obtained by the prediction unit 11 and the observation value z _t input from the measurement unit 3. Calculate and output. Specifically, the observation updating unit 13 calculates the estimated value candidate M ′ _t by the following equations (7), (8), (9), (10). The estimated value candidate M ′ _t becomes a candidate for the estimated value M _t in the process (t).

ここで、Ｑは処理（ｔ）での観測に伴う誤差を表す行列であり、観測ノイズ共分散と呼ばれる。また、Ｋ_tは、状態推定装置１における予測値と観測の誤差に対するゲインであり、カルマンゲインと呼ばれる。観測ノイズ共分散Ｑの値は、状態推定装置１の特性に応じて予め設定され、例えばＥＣＵ５の記憶領域に保存されている。 Here, Q is a matrix representing the error associated with the observation in the process (t), and is called the observation noise covariance. Further, K _t is a gain for the prediction value and the observation error in the state estimation device 1, and is called a Kalman gain. The value of the observed noise covariance Q is preset according to the characteristics of the state estimation device 1, and is stored in, for example, the storage area of the ECU 5.

更に、観測更新部１３は、予測部１１で得られた予測誤差共分散ｓ_tに基づき、下式(11)によって、推定誤差共分散Ｓ_tを算出し出力する。

Further, the observation update unit 13 calculates and outputs the estimated error covariance S _t by the following formula (11) based on the predicted error covariance s _t obtained by the prediction unit 11.

収束判定部２１は、予測部１１から入力される予測値ｍ_tと、観測更新部１３から入力される推定値候補Ｍ'_tとの差（｜Ｍ'_t−ｍ_ｔ｜）が、予め設定された閾値θ未満であるか否かを判定する。閾値θは、下式(12)のように定義される。具体的な閾値θの値は、予め設定され、例えばＥＣＵ５の記憶領域に保存されている。

The convergence determination unit 21 sets in advance the difference (| M ′ _t −m _t |) between the predicted value m _t input from the prediction unit 11 and the estimated value candidate M ′ _t input from the observation update unit 13. It is determined whether or not it is less than the threshold value θ. The threshold value θ is defined by the following equation (12). The specific value of the threshold value θ is set in advance and is stored in, for example, a storage area of the ECU 5.

そして、下式(13)に示されるように、上記差が閾値θ未満である場合には、収束判定部２１はtrueフラグを出力し、それ以外の場合には、収束判定部２１はfalseフラグを出力する。

Then, as shown in the following expression (13), the convergence determination unit 21 outputs a true flag when the difference is less than the threshold value θ, and in other cases, the convergence determination unit 21 outputs the false flag. Is output.

より詳細には、｜Ｍ'_t−ｍ_ｔ｜＝[Δｘ，Δｙ，Δｖ_x，Δｖ_y]と定義すれば、Δｘ＜Δｘ_maxと、Δｙ＜Δｙ_maxと、Δｖ_x＜Δｖ_x,maxと、Δｖ_y＜Δｖ_y,maxと、がすべて満たされた場合に、収束判定部２１はtrueフラグを出力し、上記以外の場合には収束判定部２１はfalseフラグを出力する。なお、数式(8)から、観測更新部１３による修正量Ｍ'_t−ｍ_ｔは下式(14)のように求められる。また、前述の「予測値ｍ_tと推定値候補Ｍ'_tとの差」は、当該修正量の絶対値（｜Ｍ'_t−ｍ_ｔ｜）である。

More specifically, if defined as | M ′ _t −m _t | = [Δx, Δy, Δv _x , Δv _y ], Δx <Δx _max , Δy <Δy _max , and Δv _x <Δv _{x, max} , Δv _y <Δv _{y, max} are all satisfied, the convergence determination unit 21 outputs a true flag, and otherwise, the convergence determination unit 21 outputs a false flag. Incidentally, from the formula (8), the correction amount M _'t -m _t by observing updating unit 13 is determined by the following equation (14). In addition, the above-mentioned “difference between the predicted value m _t and the estimated value candidate M ′ _t ” is the absolute value (| M ′ _t −m _t |) of the correction amount.

切替部２３は、収束判定部２１からの入力がtrueであるかfalseであるかによって、観測更新部１３からの入力を、後段の他の処理部２９に出力するか、パラメータ変更部２５側に出力するかを切り替える切替器として機能する。具体的には、切替部２３は、収束判定部２１からの入力がfalseである場合には、観測更新部１３からの入力をパラメータ変更部２５に出力する。これに対応して、パラメータ変更部２５は、演算のパラメータである観測ノイズ共分散Ｑを観測更新部１３から取得する。観測ノイズ共分散Ｑは、観測更新部１３において推定値候補Ｍ'_tの算出の演算に使用されるパラメータの一つである。そして、パラメータ変更部２５は、当該観測ノイズ共分散Ｑを変更する。具体的には、パラメータ変更部２５は、取得された観測ノイズ共分散Ｑに係数ｋを乗じてｋＱとし、変更後の新たな観測ノイズ共分散ｋＱを観測更新部１３に出力する。 The switching unit 23 outputs the input from the observation update unit 13 to another processing unit 29 in the subsequent stage, or the parameter change unit 25 side, depending on whether the input from the convergence determination unit 21 is true or false. It functions as a switch that switches between output and output. Specifically, when the input from the convergence determination unit 21 is false, the switching unit 23 outputs the input from the observation update unit 13 to the parameter changing unit 25. In response to this, the parameter changing unit 25 acquires the observation noise covariance Q, which is a parameter of the calculation, from the observation updating unit 13. The observation noise covariance Q is one of the parameters used by the observation update unit 13 to calculate the estimated value candidate M ′ _t . Then, the parameter changing unit 25 changes the observed noise covariance Q. Specifically, the parameter changing unit 25 multiplies the acquired observation noise covariance Q by a coefficient k to obtain kQ, and outputs the new observation noise covariance kQ after the change to the observation updating unit 13.

その後、処理は観測更新部１３に戻され、観測更新部１３は、新たな観測ノイズ共分散ｋＱを用いて、式(7)〜(10)に従って新たな推定値候補Ｍ'_tを算出すると共に、式(11)に従って新たな推定誤差共分散Ｓ_tを算出する。上記の係数ｋは、例えば、１＜ｋを満たす固定値とされてもよい。この場合、パラメータ変更部２５による変更処理ごとに、観測ノイズ共分散Ｑは大きくなる。 After that, the process is returned to the observation updating unit 13, and the observation updating unit 13 calculates a new estimated value candidate M ′ _t according to the equations (7) to (10) using the new observation noise covariance kQ. , A new estimation error covariance S _t is calculated according to equation (11). The coefficient k may be a fixed value that satisfies 1 <k. In this case, the observed noise covariance Q increases with each change process by the parameter changing unit 25.

その一方、切替部２３は、収束判定部２１からの入力がtrueである場合には、観測更新部１３から入力された推定値候補Ｍ'_t及び推定誤差共分散Ｓ_tを、処理（ｔ）における推定値Ｍ_t及び推定誤差共分散Ｓ_tとして、後段の処理部２９に出力する。これらの推定値Ｍ_t及び推定誤差共分散Ｓ_tは、他車両３３の位置及び速度の推定値であり、状態推定処理の成果として後段の処理部２９に引渡され利用される。また、推定値Ｍ_t及び推定誤差共分散Ｓ_tは、予測部１１にも再び入力され、次回の処理（ｔ＋１）に利用される。なお上記の処理部２９は、他車両３３の位置及び速度に基づいて、例えば、自車両３１の所定の運転支援プログラム等を実行するものである。 On the other hand, when the input from the convergence determination unit 21 is true, the switching unit 23 processes the estimated value candidate M ′ _t and the estimated error covariance S _t input from the observation update unit 13 by processing (t). The estimated value M _t and the estimated error covariance S _t are output to the processing unit 29 in the subsequent stage. The estimated value M _t and the estimated error covariance S _t are estimated values of the position and speed of the other vehicle 33, and are delivered to and used by the processing unit 29 in the subsequent stage as a result of the state estimation processing. The estimated value M _t and the estimated error covariance S _t are also input again to the prediction unit 11 and used for the next process (t + 1). The processing unit 29 described above executes, for example, a predetermined driving support program for the host vehicle 31 based on the position and speed of the other vehicle 33.

上述のような収束確認部１５によれば、前述の修正量の絶対値（｜Ｍ'_t−ｍ_ｔ｜）が閾値θ未満の場合にのみ、推定値Ｍ_tと推定誤差共分散Ｓ_tとが状態推定装置１から出力される。そして、修正量の絶対値が閾値θ以上の場合には、修正量の絶対値がθ未満になるまで、パラメータ変更部２５による観測ノイズ共分散Ｑの変更と、変更後の観測ノイズ共分散ｋＱを用いた観測更新部１３による推定値候補Ｍ'_t及び推定誤差共分散Ｓ_tの算出と、が繰り返し実行される。 According to the convergence confirmation unit 15 as described above, the estimated value M _t and the estimated error covariance S _t are calculated only when the absolute value (| M ′ _t −m _t |) of the correction amount is less than the threshold value θ. Is output from the state estimation device 1. When the absolute value of the correction amount is equal to or larger than the threshold value θ, the observation noise covariance Q is changed by the parameter changing unit 25 and the observation noise covariance kQ after the change is changed until the absolute value of the correction amount becomes less than θ. The calculation of the estimated value candidate M ′ _t and the estimated error covariance S _t by the observation update unit 13 using is repeatedly executed.

また、数式(7),(14)によれば、修正量Ｍ'_t−ｍ_ｔは、予測誤差共分散ｓ_tと観測ノイズ共分散Ｑとの相対関係によって決まる。予測誤差共分散ｓ_tが固定で観測ノイズ共分散Ｑが増加すれば、予測値ｍ_tの誤差に対する観測値ｚ_tの誤差が相対的に大きくなる。このため、観測ノイズ共分散Ｑが大きくなった場合に、この状態推定処理におけるカルマンフィルタ１０は、観測値ｚ_tに基づく修正量Ｍ'_t−ｍ_ｔを小さくする。このような観測ノイズ共分散Ｑと修正量Ｍ'_t−ｍ_ｔとの関係を利用して、修正量の絶対値が閾値θ未満となるような観測ノイズ共分散Ｑが求められる。また、上記のような観測ノイズ共分散Ｑと修正量Ｍ'_t−ｍ_ｔとの関係を勘案して係数ｋを決定することで、推定値候補Ｍ'_tの再計算の回数を削減することができる。上記の係数ｋは、前述の通り、１＜ｋを満たす固定値としてもよく、二分木探索等の手法により｜Ｍ'_t−ｍ_ｔ｜＜θを満たす最小の係数ｋを求めてもよい。 Further, Equation (7), according to (14), the correction amount M _'t -m _t is determined by the relationship between the observation noise covariance Q and the prediction error covariance s _t. If the prediction error covariance s _t is fixed and the observed noise covariance Q increases, the error of the observed value z _t relative to the error of the predicted value m _t becomes relatively large. Therefore, if the observation noise covariance Q is increased, the Kalman filter 10 in the state estimation process, to reduce the correction amount M _'t -m _t based on the observed value z _t. By utilizing the relationship between such observation noise covariance Q and correction amount M _{_'t} -m _t, observation noise covariance Q as the absolute value is less than the threshold value θ of the correction amount is calculated. Furthermore, 'by determining the coefficient k in consideration of the relationship between _t -m _t, estimated value candidates M' correction amount M and the observation noise covariance Q as described above to reduce the number of recalculation of _t You can As described above, the coefficient k may be a fixed value that satisfies 1 <k, or the minimum coefficient k that satisfies | M ′ _t −m _t | <θ may be obtained by a method such as a binary tree search.

続いて、任意の処理（ｔ）において実行される状態推定装置１の処理について、図３のフローチャートを参照しながら説明する。まず、処理（ｔ）では、予測部１１が、前回の処理（ｔ−１）で得られた推定値Ｍ_t-1及び推定誤差共分散Ｓ_t-1を取得する（図３のＳ１０１）。予測部１１は、推定値Ｍ_t-1に基づいて、数式(3),(4)により予測値ｍ_tを算出する。また、予測部１１は、推定誤差共分散Ｓ_t-1に基づいて、数式(5),(6)により予測誤差共分散ｓ_tを算出する（図３のＳ１０３）。 Next, the processing of the state estimation device 1 executed in the arbitrary processing (t) will be described with reference to the flowchart of FIG. First, in the process (t), the prediction unit 11 acquires the estimated value M _t-1 and the estimated error covariance S _t-1 obtained in the previous process (t-1) (S101 in FIG. 3). The prediction unit 11 calculates the predicted value m _t by the mathematical expressions (3) and (4) based on the estimated value M _t-1 . Further, the prediction unit 11 calculates the prediction error covariance s _t by the mathematical formulas (5) and (6) based on the estimated error covariance S _t-1 (S103 in FIG. 3).

観測更新部１３は、計測部３から入力された観測値ｚ_tを取得し（図３のＳ１０５）、観測値ｚ_tと予測値ｍ_tとに基づいて、数式(7)〜(10)により推定値候補Ｍ'_tを算出する。更に、観測更新部１３は、予測誤差共分散ｓ_tに基づいて、数式(11)により推定誤差共分散Ｓ_tを算出する（図３のＳ１０７）。 The observation update unit 13 acquires the observation value z _t input from the measurement unit 3 (S105 in FIG. 3), and based on the observation value z _t and the predicted value m _t , according to formulas (7) to (10). The estimated value candidate M ′ _t is calculated. Further, the observation updating unit 13 calculates the estimation error covariance S _t by the mathematical expression (11) based on the prediction error covariance s _t (S107 in FIG. 3).

次に、収束判定部２１は、予測値ｍ_tと推定値候補Ｍ'_tとに基づき、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされているか否かを判定する（図３のＳ１０９）。ここで、上記条件が満たされていない場合には、パラメータ変更部２５が、観測ノイズ共分散ＱをｋＱに変更し（Ｓ１１１）、変更後の観測ノイズ共分散ｋＱを観測更新部１３に引き渡す。そして、処理は前述のＳ１０７に戻り、観測更新部１３では、観測ノイズ共分散Ｑを観測ノイズ共分散ｋＱに変更して、推定値候補Ｍ'_tと推定誤差共分散Ｓ_tとが再計算される（図３のＳ１０７）。 Next, the convergence determination unit 21 determines whether or not the condition of | M ′ _t −m _t | <θ is satisfied based on the predicted value m _t and the estimated value candidate M ′ _t (see FIG. 3). S109). Here, when the above conditions are not satisfied, the parameter changing unit 25 changes the observation noise covariance Q to kQ (S111), and delivers the changed observation noise covariance kQ to the observation updating unit 13. Then, the process returns to S107 described above, and the observation update unit 13 changes the observation noise covariance Q to the observation noise covariance kQ and recalculates the estimated value candidate M ′ _t and the estimated error covariance S _t. (S107 in FIG. 3).

一方、Ｓ１０９の判定において、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされている場合には、推定値候補Ｍ'_tが処理（ｔ）における推定値Ｍ_tとして出力されると共に、推定誤差共分散Ｓ_tが出力され（図３のＳ１１３）、処理（ｔ）が終了する。なお、処理（ｔ）が終了すると、観測ノイズ共分散Ｑの値は初期値に戻る。 On the other hand, in the determination of S109, when the condition of | M ′ _t −m _t | <θ is satisfied, the estimated value candidate M ′ _t is output as the estimated value M _t in the process (t), and The estimation error covariance S _t is output (S113 in FIG. 3), and the process (t) ends. When the process (t) ends, the value of the observed noise covariance Q returns to the initial value.

上述のＳ１０７，Ｓ１０９，及びＳ１１１の処理によれば、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされていない場合には、｜Ｍ'_t−ｍ_ｔ｜＜θの条件が満たされるまで、推定値候補Ｍ'_tの算出に用いられるパラメータ（観測ノイズ共分散Ｑ）を変えながら、推定値候補Ｍ'_tの再計算が繰り返され、｜Ｍ'_t−ｍ_ｔ｜＜θの条件を満足する推定値Ｍ_tの導出が繰り返し試行される。 According to the process of the above S107, S109, and _{_{S111, | M 't -m t}} | if <condition θ is not _{_{satisfied, | M' t -m t |}} < condition θ is satisfied Until the estimated value candidate M ′ _t is recalculated repeatedly while changing the parameter (observation noise covariance Q) used to calculate the estimated value candidate M ′ _t , the condition of | M ′ _t −m _t | <θ Derivation of the estimated value M _t that satisfies

以上説明したような状態推定装置１による作用効果について説明する。従来のカルマンフィルタでは、例えば、計測部が、対象の物体の一部を欠損して計測したり、複数の物体を結合して計測したりするなど、想定外の事象が発生した場合には、予測値と観測値との残差が大きくなる。この場合、予測値に対して観測値に基づく過剰な修正が加わり、推定値が急変動するといった問題があった。これに対し、状態推定装置１によれば、収束確認部１５の機能により、修正量の絶対値が所定以上の場合には推定値Ｍ_tが出力されず、修正量の絶対値が所定未満の場合にのみ推定値Ｍ_tが出力されるので、状態推定処理における推定値Ｍ_tの急激な変動を抑えることができる。 The operation and effect of the state estimation device 1 as described above will be described. In the conventional Kalman filter, for example, when an unexpected event occurs, such as when the measurement unit loses a part of the target object for measurement or combines multiple objects for measurement, prediction is performed. The residual between the value and the observed value becomes large. In this case, there was a problem that the estimated value fluctuates abruptly because of excessive correction based on the observed value. On the other hand, according to the state estimation device 1, the function of the convergence confirmation unit 15 does not output the estimated value M _t when the absolute value of the correction amount is greater than or equal to the predetermined value, and the absolute value of the correction amount is less than the predetermined value. Since the estimated value M _t is output only in this case, it is possible to suppress a sudden change in the estimated value M _t in the state estimation processing.

また、前述の特許文献１の推定装置では、カルマンフィルタによる予測値とセンサによる計測値との差を判断基準として、当該差が所定値以上の場合には、センサの計測値を棄却しカルマンフィルタに反映させないようにしているので、カルマンフィルタによる予測値が不正確である推定初期段階では、センサの計測値が不必要に棄却されるという問題があった。これに対して状態推定装置１では、修正量の絶対値が所定以上の場合には、修正量の絶対値が所定未満になるまで推定値候補Ｍ'_tの再計算が繰り返され、修正量の絶対値が所定未満となったときに推定値候補Ｍ'_tが推定値Ｍ_tとして出力される。従って、推定初期段階においても、計測値が不必要に棄却されることを抑えることができる。 Further, in the estimation device of Patent Document 1 described above, the difference between the predicted value by the Kalman filter and the measured value by the sensor is used as a criterion, and when the difference is equal to or more than a predetermined value, the measured value of the sensor is rejected and reflected in the Kalman filter. Therefore, there is a problem that the measurement value of the sensor is unnecessarily rejected in the initial stage of estimation when the prediction value by the Kalman filter is inaccurate. On the other hand, in the state estimation device 1, when the absolute value of the correction amount is equal to or larger than the predetermined value, recalculation of the estimated value candidate M ′ _t is repeated until the absolute value of the correction amount becomes less than the predetermined value, and the correction amount When the absolute value becomes less than the predetermined value, the estimated value candidate M ′ _t is output as the estimated value M _t . Therefore, it is possible to prevent unnecessary rejection of the measured value even in the initial stage of estimation.

また、特許文献１の推定装置では、前述のように想定外の事象が発生した場合に、推定値が実際の値から徐々に乖離してしまうという問題があるところ、これに対して状態推定装置１では、修正量が所定未満である推定値Ｍ_tが得られるので、推定値Ｍ_tを常に計測値に近づけることができる。 Further, the estimation device of Patent Document 1 has a problem that the estimated value gradually deviates from the actual value when an unexpected event occurs as described above. In the case of 1, the estimated value M _t whose correction amount is less than the predetermined value is obtained, so that the estimated value M _t can always be close to the measured value.

また、観測更新部１３による推定値候補Ｍ'_tの再計算は、その演算に用いるパラメータを変更しながら繰り返される。ここで、変更するパラメータとしては、計測部３の計測誤差に関連するパラメータである観測ノイズ共分散Ｑが採用されている。このような観測ノイズ共分散Ｑを増加させながら上記再計算を繰り返すことにより、修正量の絶対値は、再計算ごとに減少するように推移する。従って、推定値候補Ｍ'_tの再計算の回数が低減され、推定値Ｍ_tが出力されるまでの時間が短縮される。 Further, the recalculation of the estimated value candidate M ′ _t by the observation updating unit 13 is repeated while changing the parameter used for the calculation. Here, as the parameter to be changed, the observation noise covariance Q, which is a parameter related to the measurement error of the measurement unit 3, is adopted. By repeating the above recalculation while increasing the observed noise covariance Q, the absolute value of the correction amount changes so as to decrease with each recalculation. Therefore, the number of times of recalculation of the estimated value candidate M ′ _t is reduced, and the time until the estimated value M _t is output is shortened.

本発明は、上述した実施形態を始めとして、当業者の知識に基づいて種々の変更、改良を施した様々な形態で実施することができる。また、上述した実施形態に記載されている技術的事項を利用して実施例の変形例を構成することも可能である。各実施形態の構成を適宜組み合わせて使用してもよい。 The present invention can be implemented in various forms including the above-described embodiment and various modifications and improvements based on the knowledge of those skilled in the art. Further, it is also possible to configure a modified example of the example by utilizing the technical matters described in the above-described embodiment. The configurations of the respective embodiments may be appropriately combined and used.

１…状態推定装置、３…計測部（観測部）、１０…カルマンフィルタ、１１…予測部、１３…観測更新部、１５…収束確認部、３３…他車両（物体）。 1 ... State estimation device, 3 ... Measurement part (observation part), 10 ... Kalman filter, 11 ... Prediction part, 13 ... Observation update part, 15 ... Convergence confirmation part, 33 ... Other vehicle (object).

Claims

Along with obtaining the predicted value of the state of the object based on the estimated value of the state of the object obtained last time, update the predicted value based on the observed value of the state of the object, based on the updated predicted value A Kalman filter that obtains the current estimated value, a state estimation device that estimates the state of the object,
A prediction unit that obtains the predicted value based on the previous estimated value,
An observation unit for observing the state of the object to obtain the observation value,
An observation update unit that performs an operation based on the observed value and the predicted value to obtain an estimated value candidate that is a candidate for the estimated value this time,
When the difference between the predicted value and the estimated value candidate is out of a predetermined range, a predetermined parameter regarding the variance of the observed value used in the calculation by the observation update unit is changed, and the difference is the predetermined value. If it is within the range, a convergence confirmation unit that outputs the estimated value candidate as the current estimated value,
If the difference is outside the predetermined range, the convergence confirmation unit changes the parameter until the difference falls within the predetermined range, and the calculation by the observation update unit using the changed parameter. And a state estimation device that repeatedly executes.