JP6856496B2 - Orientation error function acquisition device, method and program - Google Patents

Description

The present invention relates to a directional error function acquisition device, method and program for acquiring a directional error function indicating an observed directional error with respect to a true directional object with the directional object as a variable.

Conventionally, in a vehicle control system, various orientation measuring devices for measuring the orientation of an object such as another vehicle with respect to the own vehicle have been used. In such an orientation measuring device, an error occurs in the observed orientation with respect to the true orientation due to various factors. Therefore, in order to obtain the accurate orientation of the object, it is necessary to evaluate the error generated in the observed orientation and correct the observed orientation (see, for example, Patent Document 1).

Japanese Unexamined Patent Publication No. 2002-228479

The error that occurs in the observed orientation changes according to the orientation of the object. For example, when an in-vehicle radar is installed on a bumper, an error occurs in the observed orientation according to the orientation of the object due to the interference between the radar wave and the bumper. Therefore, in order to properly correct the observed azimuth, an azimuth error function that indicates the error of the observed azimuth with respect to the true azimuth with the azimuth of the object as a variable is required.

The present invention has been made in view of the above problems, and an object of the present invention is an directional error function capable of obtaining an directional error function indicating an observed directional error with respect to a true directional object with the directional object as a variable. To provide acquisition devices, methods and programs.

In the first embodiment of the present invention, based on the observed value of observing the moving state of the object, the moving state amount indicating the moving state of the object and the directional error amount indicating the observed directional error with respect to the true direction. A direction error amount update unit that estimates the current time state and updates the current time state estimated values of the movement state amount and the direction error amount, and a current time of the direction error amount updated by the direction error amount update unit. A directional error function including an directional error function estimation unit that updates the estimated value of the directional error function indicating the observed directional error with respect to the true azimuth with the directional object as a variable based on the state estimated value. It is an acquisition device.

In the second embodiment of the present invention, the moving state amount indicating the moving state of the object and the directional error amount indicating the observed directional error with respect to the true direction are based on the observed values obtained by observing the moving state of the object. The current time of the directional error amount updated in the directional error amount updating step of estimating the current time state and updating the current time state estimated values of the moving state amount and the directional error amount, and the current time of the directional error amount updated in the directional error amount updating step. A directional error function including a directional error function estimation step for updating the estimated value of the directional error function indicating the observed directional error with respect to the true azimuth with the directional object as a variable based on the state estimated value. This is the acquisition method.

In the third embodiment of the present invention, the moving state amount indicating the moving state of the object and the azimuth indicating the error of the observed azimuth with respect to the true direction are shown on the computer based on the observed value of the moving state of the object. The directional error amount updating function that estimates the current time state of the error amount and updates the current time state estimated values of the moving state amount and the directional error amount, and the directional error amount updated by the directional error amount updating function. Based on the current time state estimate value of, the directional error function estimation function that updates the estimated value of the directional error function indicating the observed directional error with respect to the true directional object with the directional object as a variable is realized. This is a directional error function acquisition program.

In the present invention, it is possible to obtain an azimuth error function indicating an observed directional error with respect to the true directional by using the directional object as a variable.

The block diagram which shows the direction measuring apparatus of one Embodiment of this invention. The flow chart which shows the direction error function acquisition method of one Embodiment of this invention. The schematic diagram which shows the directional error amount update step and the directional error function estimation step of one Embodiment of this invention. Explanatory drawing explaining the directional error function of one Embodiment of this invention. The schematic diagram which shows the geometric relation of the direction acquisition system of one Embodiment of this invention. The figure which shows the estimation result of the position of another vehicle for setting the initial value used in the directional error amount update step of one Embodiment of this invention.

An embodiment of the present invention will be described with reference to FIGS. 1 to 6.
In the directional error function acquisition device and method of the present embodiment, the directional error of the other vehicle is changed as a variable based on the observed value of the traveling state of the other vehicle traveling behind the own vehicle while the own vehicle is traveling on the straight running road. And obtain the directional error function indicating the observed directional error with respect to the true directional.

In the following, after explaining the directional error function acquisition device and method of the present embodiment, (1) directional error function, (2) coordinate conversion from the vehicle coordinate system to the radar coordinate system, and (3) directional error amount update step. The state model of the Kalman filter used in (4) Setting the initial value of the current time state estimate value of the state amount used in the directional error amount update step, (5) Updating the estimated value of the directional error function in the directional error function estimation step, Will be explained in detail one by one.

The directional error function acquisition device and method of the present embodiment will be described with reference to FIGS. 1 to 3.
The orientation measuring device of the present embodiment will be described with reference to FIG.
As shown in FIG. 1, the directional measurement device includes a radar unit 10, a directional error function acquisition device 20, and a directional correction unit 40.

The radar unit 10 observes the traveling state of the other vehicle with respect to the own vehicle for the other vehicle traveling behind the own vehicle, and acquires the observed value of the traveling state of the other vehicle. The observed values of the running state of the other vehicle include at least the observed values of the orientation of the other vehicle with respect to the own vehicle.

The directional error function acquisition device 20 is a device that acquires a directional error function that indicates an observed directional error with respect to the true azimuth with the directional error of another vehicle as a variable, and is a directional error amount update unit 30 and a directional error function estimation unit. 32 and.

The directional error amount updating unit 30 indicates the traveling state amount indicating the traveling state of the other vehicle and the observed directional error with respect to the true orientation based on the observed value of the traveling state of the other vehicle observed by the radar unit 10. It has a function of estimating the directional error amount and updating the running state amount and the current time state estimated value of the directional error amount. Further, the directional error amount updating unit 30 has a function of calculating the current time state estimated value of the directional direction from the current time state estimated value of the traveling state amount.

The directional error function estimation unit 32 has a function of updating the estimated value of the directional error function based on the directional error amount updated by the directional error amount updating unit 30 and the current time state estimated value of the directional direction.

The directional correction unit 40 has a function of storing the directional error function acquired by the directional error function acquisition device 20 and correcting the directional observation value acquired by the radar unit 10.

The directional error function acquisition method of the present embodiment will be described with reference to FIGS. 2 and 3.
The directional error function acquisition method of the present embodiment is a method of acquiring the directional error function described above while the vehicle is traveling, and has the following steps as shown in FIG.

Observation value acquisition step S10
In the observation value acquisition step S10, the traveling state of the other vehicle traveling behind the own vehicle is observed with respect to the own vehicle at a predetermined update cycle, and the observed value of the traveling state of the other vehicle is acquired.

Orientation error amount update step S30
In the azimuth error amount update step S30, the current time state of the running state amount and the azimuth error amount is estimated by using the Kalman filter based on the state model in which the running state amount and the azimuth error amount are the state amounts, and the current time state estimated value is obtained. I will update it. In addition, the current time state estimated value of the direction is calculated from the current time state estimated value of the traveling state amount.

That is, as shown in FIG. 3, as the state model of the Kalman filter, the running state amount indicating the running state of another vehicle and the directional error amount indicating the observed directional error with respect to the true directional are set as the state amount. A state model is used. For each update cycle, the state prediction one hour ahead is calculated from the estimated state time before the state quantity, and the current time state estimation value of the state quantity is calculated from the state prediction one hour ahead and the observed value of the running state of another vehicle. To do. The same calculation is repeated with the current time state estimated value as the previous time state estimated value, and the current time state estimated value of the state quantity is updated.

The initial value for updating the current time state estimated value of the state quantity is based on the observed value at multiple times by the maximum likelihood method using the probability density function of the observed value of the running state and the prior distribution function of the running state quantity. Then, the state quantity at a predetermined time is estimated, and the state quantity at the predetermined time is set as the initial value of the current time state estimated value.

Orientation error function estimation step S32
In the direction error function estimation step S32, the estimated value of the direction error function is updated based on the direction error amount updated in the direction error amount update step S30 and the current time state estimated value of the direction.

That is, as shown in FIG. 3, for each update cycle, based on the directional error amount updated in the directional error amount update step S32 and the current time state estimated value of the directional error, and the pre-stage estimated value of the directional error function. , Calculate the current stage estimate of the directional error function. The same operation is repeated with the current stage estimated value as the previous stage estimated value, and the estimated value of the orientation error function is updated.
The initial value of the estimated value of the orientation error function is set to zero.

When the estimated value of the directional error function converges within a predetermined threshold, the update of the estimated value of the directional error function is stopped, and the final stage estimated value is acquired as the directional error function.

The directional error function of the present embodiment will be described in detail with reference to FIG.
As shown in FIG. 4 (a), in the radar coordinate system C _s, the azimuth angle of the other vehicle T and θ the distance from the radar S to the other vehicle T R, for the radar S, the orientation as u. The direction u is represented by the following equation (1).

Here, the true azimuth and u, the observation azimuth and u _z. Observation orientation u in an ideal state containing no errors in _z, as indicated by the dotted line graph in FIG. 4 (b), a u z _(u) = u. On the other hand, in the normal state where an error is included in the observation direction u _z, as indicated by the solid line in FIG. _{3 (b), u z (} u) is the true azimuth u, true to the orientation u as a variable The directional error function f (u) indicating the observed directional error with respect to the directional is added. Therefore, the orientation error function f (u) is expressed by the following equation (2).

方位誤差関数ｆ（ｕ）は、次式（４）に示されるように、窓関数ｇ（ｕ）とベクトルｆとの内積をとることで得られる。

ここで、窓関数ｇ（ｕ）については、離散関数を連続関数に変換する関数である。本実施形態では、式（５）乃至（７）で示される窓関数を用いる。

As shown in FIG. 4 (c), _u n regularly spaced (n = 1, ..., N ) to introduce a discrete function that takes discrete values f _{(u n)} for each. Furthermore, as shown in equation (3), the n-th row (n = 1, ..., N) component of introducing N-dimensional column vector f is f _{(u n).}

The azimuth error function f (u) is obtained by taking the inner product of the window function g (u) and the vector f as shown in the following equation (4).

Here, the window function g (u) is a function that converts a discrete function into a continuous function. In this embodiment, the window function represented by the equations (5) to (7) is used.

In the directional error function acquisition method of the present embodiment, the vector f described above is acquired. In the following, f will be described as an orientation error function.

また、右側レーダＳ_ｒ、左側レーダＳ_ｌの出射軸は、夫々、Ｙ軸に対して反時計回りにφ_ｒ、時計回りにφ_ｌの角度をなしている。 With reference to FIG. 5, the coordinate conversion from the vehicle coordinate system to the radar coordinate system in the present embodiment will be described in detail.
As shown in FIG. 5, in the direction acquisition system of the present embodiment, the other vehicle T is located behind the own vehicle U, and the right side radar S _r and the left side radar S _l are arranged on the rear right and left sides of the own vehicle U, respectively. Has been done. _{The vehicle coordinate system CU} and the radar coordinate system C _τ are set in the own vehicle U and the radar S _τ , respectively, in the same horizontal plane. The subscripts τ indicate r and l, and r and l indicate right right and left left, respectively. The vehicle coordinate system C _U, the rear direction y-axis direction, the right direction and has a x-axis direction. In the vehicle coordinate system _CU , the position of the other vehicle T is x, and the position of the radar S _τ is t _τ . The position x of the other vehicle T and the position t _{τ of the radar S τ} are expressed by the following equations (8) and (9).

The emission axes of the right-side radar S _r and the left-side radar S _l have an angle _{of φ r} counterclockwise and φ _l clockwise with respect to the Y axis, respectively.

Thus, the traveling state of another vehicle and [rho _tau in the sensor coordinate system C _tau, if the running state of another vehicle in the vehicle coordinate system C _U and chi, following equation (10) to (16) is satisfied ..

In the present embodiment, the coordinate transformation from the vehicle coordinate system to the radar coordinate system is performed by using the coordinate transformation described above.

The state model of the Kalman filter used in the directional error amount updating step of the present embodiment will be described in detail with reference to FIG.
Ζ running state quantity indicating a traveling state of another vehicle in the vehicle coordinate system C _U, when the azimuth error amount indicating an error of the azimuth observed for true orientation is b, the traveling state quantity ζ and an azimuth error amount b are each It is represented by the following equations (17) and (18).

As the state model of the Kalman filter, a state model is used in which the running state amount ζ and the orientation error amount b represented by the equations (17) and (18) are set to the state amount ξ, respectively.
The equation of state of the state model is expressed as the following equations (19) to (21), where T is the update period. As shown in equation (21), [Gv ε] ^T indicates a state error.

The observation equation of the state model is expressed as the following equations (22) and (23), where z is the observation value from the radar. As shown in equation (23), w indicates the observation error.

In the directional error amount updating step of the present embodiment, the current time state estimated values of the traveling state amount and the directional error are updated by using the Kalman filter based on the state model described above.

そして、時刻Ｍにおける走行状態量ζ_Ｍと方位誤差量ｂとを推定し、当該推定値を状態量の現時刻状態推定値の更新における初期値として設定する。 With reference to FIG. 6, the setting of the initial value of the current time state estimated value of the state amount used in the directional error amount updating step of the present embodiment will be described in detail.
Let the observed values of the running state of other vehicles at each observation time from time 1 to time M be z _{m, τ} (m = 1, ..., M), and the running state amount be ζ _m (m = 1, ..., M). , It is assumed that the directional error amount b is constant. The observed values z _{m and τ} of the running state and the running state quantity ζ _m are expressed by the following equations (24) and (25), respectively.

Then, the traveling state amount ζ _M and the directional error amount b at the time M are estimated, and the estimated value is set as the initial value in updating the current time state estimated value of the state amount.

The running state amount ζ _M and the directional error amount b at time M are estimated as follows.
Time M-1 traveling state amount until time 1 zeta from _{M-m (m = 1,} ..., M-1) for, from the running state quantity zeta _M at time M, using the matrix F of the formula (20) , Calculated based on the following equation (26).

The probability density function of the observed values z _{m and τ} follows the known white Gaussian noise distribution. In addition, it is assumed that the own vehicle and other vehicles travel straight on a straight road, and the prior distribution function, which is a Gaussian distribution with the expected value set to 0 for the lateral speed of the _{traveling state amount ζ m, and the expected value for the lateral position are set.} Introduce a prior distribution function that is a Gaussian distribution with lc. Here, l _c is the distance between the center line between the traffic lane and the traveling lane of the other vehicle in the vehicle.

The observed value z _m, the negative logarithm J _tau of the probability density function of _tau, the likelihood of that added the negative logarithm J _x lateral velocity and the prior distribution function of lateral position of the running state quantity zeta _{m Let the function J be used, and calculate the running state amount ζ M} and the orientation error amount b at the time M when the likelihood function J is minimized.

尤度関数Ｊを最小とする、時刻Ｍにおける走行状態量ζ_Ｍ及び方位誤差量ｂの算出には、ＭＡＴＬＡＢ（登録商法）のｆｍｉｎｃｏｎ等のソルバーを用いることが可能である。
図６に、他車両の位置ｘの推定結果の一例を示す。 The likelihood function J is expressed as the following equations (27) to (29) by using the functions represented by the equations (12) to (16).

A solver such as fmincon of MATLAB (registered commercial law) can be used to calculate the _{running state amount ζ M} and the bearing error amount b at time M, which minimizes the likelihood function J.
FIG. 6 shows an example of the estimation result of the position x of another vehicle.

In the present embodiment, as described above, the running state quantity ζ _M and the orientation error amount b at the time M are estimated, and the estimated values are set as the initial values in updating the current time state estimated value of the state quantity.

With reference to FIG. 3, the update of the estimated value of the directional error function in the directional error function estimation step of the present embodiment will be described in detail.
In updating the estimated value of the orientation error function, as shown in the following equation (30), the current stage estimated value f _{τ, k} is added by adding the updated value df _{τ to the previous stage estimated value f τ, k-1.} Is calculated.

式（３１）を充足し、ノルムが最小となる更新値df_τを算出するため、更新値df_τを次式（３２）のようにおく。

以上の式（３０）乃至（３２）から、Ｃ_１は次式（３３）のように算出される。

以上から方位誤差関数の更新値df_τについては、次式（３４）のように表される。

なお、１／λについては、方位誤差関数の推定値f_τの収束性を向上させるための係数である。 The method of deriving the updated value df _τ of the directional error function will be described below.
Azimuth error amount and orientation present time state estimate b _{tau in, k,} and u _k, stage estimate f _tau azimuth error _function, and the _k, and have a relationship of the following equation (31).

In order to satisfy the equation (31) and _{calculate the update value df τ} that minimizes the norm, the update value df _τ is set as in the following equation (32).

From the above equations (30) to (32), C ₁ is calculated as the following equation (33).

From the above, the updated value df _τ of the directional error function is expressed by the following equation (34).

Note that 1 / λ is a coefficient for improving the convergence of _{the estimated value f τ of the directional error function.}

In the directional error function estimation step of the present embodiment, the estimated value of the directional error function is updated as described above.

The directional error function acquisition device and method of the present embodiment have the following effects.
In the orientation error function acquisition device and method of the present embodiment, a Kalman filter based on a state model in which the traveling state amount and the orientation error amount are set as the state quantity is used, and the traveling state amount and the orientation error amount are measured based on the observed values of the traveling state. The current time state is estimated, and the current time state estimated values of the running state amount and the orientation error amount are updated. Then, the estimated value of the directional error function is updated based on the current time state estimated value of the directional error amount. In this way, it is possible to obtain a directional error function indicating the observed directional error with respect to the true directional with the directional of another vehicle as a variable.

In addition, the maximum likelihood method using the probability density function of the observed value of the running state and the prior distribution function of the running state quantity is used to estimate the state quantity at a predetermined time from the observed values at multiple times, and the state quantity is used as the state quantity. It is set as the initial value when updating the current time state estimated value. Therefore, it is possible to appropriately set the initial value of the current time state estimated value. In particular, in order to estimate the state quantity, in addition to the probability density function of the observed values of the distance, the distance derivative, and the orientation of the other vehicle with respect to the own vehicle, the prior distribution function of the lateral speed and the traveling state quantity of the lateral position is also used. Therefore, it is possible to accurately estimate the state quantity.

Furthermore, the orientation error function is a function unique to each finished vehicle. For example, when an in-vehicle radar is installed on a bumper, it occurs in the observed value due to interference between the radar wave and the bumper due to the positional relationship between the in-vehicle radar and the bumper in the completed vehicle, the shape of the bumper, and various other factors. The orientation error function of the error to be performed is a function unique to each finished vehicle. Therefore, in order to calibrate the directional measurement error, it is necessary to perform complicated calibration work using a large-scale facility for each finished vehicle before shipping, and in reality, it is difficult to calibrate the directional measurement error. It was. However, in the orientation error function acquisition device and method of the present embodiment, since the orientation error function can be acquired while the own vehicle is traveling by using the existing vehicle-mounted equipment, the equipment for calibrating the orientation measurement error. , Work, etc. can be omitted.

In one embodiment described above, the orientation error function acquisition device and the method have been described. However, a program for realizing various functions of the orientation error function acquisition device of one embodiment on a computer and an orientation error function of one embodiment on a computer. A program for executing various procedures of the acquisition method is also included in the scope of the present invention.

10 ... Radar unit 20 ... Direction error function acquisition device 30 ... Direction error amount update unit 32 ... Direction error function estimation unit 40 ... Direction correction unit

Claims (6)

Based on the observed values obtained by observing the moving state of the object, the current time state of the moving state amount indicating the moving state of the object and the directional error amount indicating the observed directional error with respect to the true direction is estimated, and the movement is described. A directional error amount update unit that updates the current time state estimated values of the state amount and the directional error amount,
Based on the current time state estimate of the directional error amount updated by the directional error amount update unit, the estimated value of the directional error function indicating the observed directional error with respect to the true directional object with the directional object as a variable is obtained. The directional error function estimation unit to be updated and
A bearing error function acquisition device comprising. The directional error amount updating unit estimates the current time state of the moving state amount and the directional error amount by using a Kalman filter based on the state model with the moving state amount and the directional error amount as the state amount, and the moving state amount. And update the current time state estimate of the amount of directional error,
The directional error function acquisition device according to claim 1. The orientation error amount update unit is an observation value at a plurality of times by a maximum likelihood method using a probability density function of an observed value for observing the moving state of an object and a prior distribution function of a moving state amount indicating the moving state of the object. Estimates the moving state amount and the orientation error amount at a predetermined time based on, and sets the moving state amount and the orientation error amount as initial values in updating the current time state estimated values of the moving state amount and the orientation error amount. ,
The directional error function acquisition device according to claim 1 or 2. An observation unit that observes the moving state of an object and acquires the observed value of the moving state of the object,
The directional error function acquisition device according to any one of claims 1 to 3,
An orientation correction unit that stores the orientation error function acquired by the orientation error function acquisition device and corrects the observation value acquired by the observation unit based on the orientation error function.
A directional measuring device comprising. Based on the observed values obtained by observing the moving state of the object, the current time state of the moving state amount indicating the moving state of the object and the directional error amount indicating the observed directional error with respect to the true direction is estimated, and the movement is described. The directional error amount update step for updating the current time state estimated values of the state amount and the directional error amount, and
Based on the current time state estimate of the directional error amount updated in the directional error amount update step, the estimated value of the directional error function indicating the observed directional error with respect to the true azimuth with the directional object as a variable is obtained. The directional error function estimation step to be updated and
A method for acquiring an orientation error function. On the computer
Based on the observed values obtained by observing the moving state of the object, the current time state of the moving state amount indicating the moving state of the object and the directional error amount indicating the observed directional error with respect to the true direction is estimated, and the movement is described. The directional error amount update function that updates the current time state estimate of the state amount and directional error amount, and
Based on the current time state estimate of the directional error amount updated by the directional error amount update function, the estimated value of the directional error function indicating the observed directional error with respect to the true directional object with the directional object as a variable is obtained. The directional error function estimation function to be updated and
Orientation error function acquisition program that realizes.

Images