JP4652143B2 - Axis deviation angle estimation device and axis deviation angle estimation method - Google Patents

Description

  The present invention relates to an axis misalignment angle estimation apparatus and an axis misalignment angle estimation method for estimating an axis misalignment angle that is an installation error of an alignment offset angle of radar, sonar, laser radar, or the like using radio waves, sound waves, or light waves.

  Some conventional axis deviation angle estimation apparatuses use self-speed data Vm output from a self-speed sensor prepared separately, together with the relative speed qm of the reflector. Then, the speed ratio (qm / Vm) obtained by dividing the relative speed qm by the own speed sensor output Vm at that time is used as an observation value, and the own speed error ratio A and the offset angle φ are directly regarded as unknown parameters. Two unknown parameters [A, φ] are estimated from these simultaneous equations (see, for example, Patent Document 1). Note that this φ corresponds to the offset angle to be obtained.

JP 2002-228749 A (first page, FIG. 1)

  However, the prior art has the following problems. In order to estimate and calculate the offset angle, the conventional off-axis angle estimation device uses not only the observation data of the relative speed and the arrival angle of the reflector observed by the radar, but also the vehicle speed data output from the vehicle speed sensor at each observation time. Also need. In addition, since the angle search range of a radar or the like cannot generally be made wide, the change in relative speed due to the angle of the reflector is only a few km / hour (that is, the fluctuation width of cos (θm−φ) is small). Therefore, high stability is required for the vehicle speed data at each observation time.

  Further, the conventional axis deviation angle estimation device solves [A, φ] by the least square method using a large number of observation data of the reflector observed by a radar or the like in order to estimate and calculate the offset angle with sufficient accuracy. There is a need. For this reason, computation is performed using a multidimensional matrix using Taylor expansion, and there is a problem that the amount of computation increases in the estimation processing using a large number of observation data. Furthermore, as the amount of computation increases, the hardware scale increases.

  The present invention has been made to solve the above-described problems. It eliminates the need for detecting the own speed, reduces the amount of calculation compared to the conventional apparatus, and estimates the axis deviation angle without using Taylor expansion. An object of the present invention is to obtain a shaft misalignment angle estimation device and a shaft misalignment angle estimation method.

An axis deviation angle estimation apparatus according to the present invention includes a detection unit that detects observation data including a relative speed and an azimuth angle with respect to a moving radar for each of a plurality of radar reflectors that are stationary objects, a radar moving direction, When the angle between the radar reference direction and the radar offset angle is the true value of the radar offset angle, the radar offset angle estimate is calculated by the least square method based on the vector calculation based on the observation data detected for each of the multiple radar reflectors. And calculating means for converting the relative speed included in the observation data into an axial speed along the radar axis direction using the azimuth angle included in the observation data, and including the converted axial speed and azimuth angle. Axis speed calculation means for generating axis speed correspondence data, and an azimuth angle based on the axis speed correspondence data generated for each of a plurality of radar reflectors In which the inclination of the shaft speed is calculated by the least squares method by vector operation, the calculated inclination and azimuth angle and a vector minimum square calculation means for calculating the offset angle estimated value of the radar from the axis velocity when zero for is there.

The axis deviation angle estimation method according to the present invention includes a step of detecting observation data including a relative speed and an azimuth angle with respect to a moving radar for each of a plurality of radar reflectors that are stationary objects, and a moving direction of the radar When the angle between the radar reference direction and the radar reference direction is the true value of the radar offset angle, the relative velocity included in the observation data is converted to the axial velocity along the radar axis direction using the azimuth angle included in the observation data. Generating axis velocity correspondence data including the converted axis velocity and azimuth angle, and calculating a radar offset angle estimate using a least square method based on vector calculation based on the generated axis velocity correspondence data , The step of calculating the radar offset angle estimate value is based on the axis velocity correspondence data, and the gradient of the axis velocity relative to the azimuth angle is calculated by the least-squares vector Calculating the one in which the calculated inclination and azimuth angle and a step of calculating an offset angle estimated value of the radar from the axis velocity when zero.

  According to the present invention, the vehicle speed and the offset angle are regarded as unknown parameters, and the radar offset is calculated by the least square method based on the vector calculation based on the observation data including the relative speed and the azimuth angle obtained for each of the plurality of radar reflectors. By calculating the estimated angle, it is not necessary to detect the own speed, and the amount of calculation is reduced compared to the conventional device, and the misalignment angle estimator and the misalignment can be estimated without using Taylor expansion. An angle estimation method can be obtained.

Hereinafter, preferred embodiments of an axis deviation angle estimation device and axis deviation angle estimation method of the present invention will be described with reference to the drawings.
The axis deviation angle estimation apparatus of the present invention uses the reflector relative velocity qm as an observed value without using the own velocity Vm, and regards the offset angle φ as an unknown parameter, and the simultaneous equations of the following equation (2): By estimating the unknown parameter φ more, vector calculation without depending on the vehicle speed v and without using Taylor expansion is performed, and the installation offset angle is estimated with reduced calculation amount.

Embodiment 1 FIG.
FIG. 1 is a configuration diagram of an axis deviation angle estimation device according to Embodiment 1 of the present invention. 1 includes a detection unit 110 and a calculation unit 120. Further, the calculation means 120 is composed of an axis speed calculation means 121 and a vector least square calculation means 122.

  The radar 1 includes M reflection points each consisting of a pair of an incident angle θm of an echo from a reflection point m (m = 1, 2,..., M) and a relative velocity qm obtained from the Doppler frequency of the echo. Observation data {qm, θm} (m = 1, 2,..., M) are observed.

  Φ, which is an angle between the radar moving direction and the radar reference direction, is a true value of the radar offset angle. Further, φ (hat) obtained by calculation in the axis deviation angle estimation device 100 means an estimated value of the radar offset angle. Here, in the following description, what is described as (hat) means a character with a ∧ on the previous character, and represents an estimated value.

  Here, when the radar 1's own speed is v, the relative speed qm is represented by v × cos (θm−φ) as shown in FIG.

  On the other hand, the detection means 110 in the axis deviation angle estimation apparatus 100 acquires M observation data {qm, θm} at a certain time from the radar 1. Further, the calculation means 120 calculates the radar offset angle estimated value based on the M observation data {qm, θm} by the least square method based only on vector calculation without performing Taylor expansion.

  FIG. 2 is a diagram showing a distribution of M observation data {qm, θm} at a certain time in the first embodiment of the present invention. M observation data {qm, θm} (m = 1, 2,..., M) obtained at a certain time by the detection means 110 are distributed as shown in FIG. Here, when it is assumed that the own speed v for generating the speed qm is along the radar reference direction, and the speed value on the axis is defined as the axis speed ξ (θm), the axis speed ξ (θm) is Can be represented by the following formula (3).

  The shaft speed calculating means 121 calculates the shaft speed ξ (θm) from the relationship of the expression (3). FIG. 3 is a diagram showing the distribution of the axial velocity ξ (θm) with respect to the angle θ in the first embodiment of the present invention. As shown in FIG. 3, the axial speed ξ (θm) is distributed linearly with respect to the angle θ, and the partial differentiation by the angle θ is generally expressed by the following equation (4). The data {ξ (θm), θm} composed of the shaft speed ξ (θm) and θm calculated by the shaft speed calculation means 121 is referred to as axis speed correspondence data in the following description.

  In the observation data {qm, θm} obtained from the front of the radar 1, sin (φ) in the equation (4) has a dominant characteristic. When the offset angle of the radar 1 does not exist, that is, when θ = 0, partial differential δξ (θ) / δθ = 0. Therefore, the correction amount φ (hat) is estimated so that the partial differential value of the shaft speed ξ (θm) with respect to the angle θ is zero.

  It is difficult to obtain partial differential values from observation data {qm, θm} obtained discretely. Therefore, the vector least squares calculation means 122 calculates the inclination x of the axis speed approximate line in the axis speed correspondence data {ξ (θm), θm} from the least square method of the following equation (5), and substitutes the partial differential value. To do. Here, the vector θ in Expression (5) is expressed by Expression (6), and θ (bar) that is the average angle of Expression (6) is expressed by Expression (7). In addition, the vector ξ in Expression (5) is expressed by Expression (8), and ξ (bar) that is the average axial speed in Expression (8) is expressed by Expression (9).

  In the following description, what is described as (bar) means a character with a-added on the preceding character, and represents an average value. Note that since Equation (5) is a least square method using only vector operations, it is 40% or less compared to the amount of calculation of the least square estimation method using a matrix by Taylor expansion.

  Further, the vector least squares calculation means 122 calculates the intercept ξ (0) of the axial velocity ξ (θ) from the M pieces of axial velocity correspondence data {ξ (θm), θm} and the gradient x obtained by the equation (5). ) Can be obtained by the following equation (10).

  Here, x / ξ (0) is from the relationship of the following equation (11). It is equal to tanφ.

  Therefore, the vector least squares calculation unit 122 can directly estimate the correction amount φ (hat) using the following equation (12).

  Since the slope x is an approximate value, it is effective to increase the accuracy of the correction amount φ (hat) by repeatedly calculating the equations (5) and (12). Specifically, the shaft speed calculation means 121 corrects the M incident angles θm with the correction amount φ (hat) calculated by the equation (12) to obtain the corrected incident angle θm.

  Further, the axial speed calculation means 121 obtains a corrected axial speed ξ (θm) corresponding to the corrected incident angle θm, and obtains corrected axial speed correspondence data. Then, the vector least squares calculation means 122 obtains the corrected gradient x by the equation (5) using the M corrected shaft speed correspondence data {ξ (θm), θm}.

  Further, the vector least squares calculation means 122 calculates the equations (10) to (10) from the M corrected shaft speed correspondence data {ξ (θm), θm} and the corrected gradient x obtained by the equation (5). Using (12), a new correction amount φ (hat) is obtained.

  The computing unit 120 can increase the accuracy of the correction amount φ (hat) by repeating the above-described series of processing by the axis speed computing unit 121 and the vector least squares computing unit 122. For example, the calculation means 120 repeats the processing by a predetermined number of times determined in advance, or by repeating the processing until the difference between the previous value and the current value of the correction amount φ hat falls within the allowable range, thereby estimating the accuracy of the estimation. Can be improved.

In addition, the calculation means 120 calculates K correction amounts φ (hat) ^(k) (k = 1,... ⁾ Based on the respective observation data in a certain time zone k (k = 1,..., K). It is also possible to obtain K) and determine the final correction amount φ (bar) by averaging them.

Furthermore, the calculating means 120, when the determination of K correction amount phi (hat) ^(k) over a time period k, the correction amount phi (hat) which Motoma' in the previous time slot k-1 ^(k- By calculating the correction amount φ (hat) ^(k) in the current time zone k using ¹⁾ as an initial value, convergence can be accelerated and the number of repetitions of the calculation can be reduced.

  In addition, in this way, by using the correction amount obtained in the previous time zone as the initial value when calculating the current correction amount, the predetermined number of times determined in advance according to the time zone is gradually reduced. Is also possible.

  As described above, according to the first embodiment, the offset angle can be estimated with a small amount of computation without using the Taylor expansion by using the inclination of the axial speed approximation line based on the converted shaft speed. it can. Furthermore, it is not necessary to install a self-speed sensor or its connection line, and the entire apparatus can be made inexpensive and lightweight and compact, and restrictions on equipment installation can be reduced.

Embodiment 2. FIG.
In the second embodiment, a modified example of the calculation unit 120 in the first embodiment will be described. In the first embodiment, the case where the offset angle φ is obtained from the inclination x has been described. In the second embodiment, the offset angle is updated by updating the offset angle by Δφ that is a preset azimuth angle correction amount using the sign of the partial differential value δξ (θ) / δθ of Equation (4) as a reference. A method of estimating the will be described. The configuration of the axis deviation angle estimation device 100 is the same as the configuration of FIG. 1 shown in the first embodiment.

The partial differential value δξ (θ) / δθ is positive or negative depending on the sign of the offset angle φ. In the equation (4), the denominator is cos ² (θ) ≧ 0, and the sign of the numerator sin (φ) is determined depending on whether the true value of the radar axis deviation angle is on the left or right of the moving direction. The same applies to the slope x of the approximate straight line obtained by equation (5). The true value of the offset angle is located in a section where the sign of the partial differential value δξ (θ) / δθ or the gradient x is reversed.

  In the second embodiment, the vector least squares calculation means 122 first calculates the slope x of the equation (5) by the procedure described in the first embodiment. On the other hand, the axis speed calculation means 121 switches the correction direction according to the sign of the calculated inclination x, and changes the incident angle θm included in the observation data to a minute step size Δφ that is a predetermined azimuth angle correction amount. Correct with.

  Furthermore, the axis speed calculation means 121 generates the corrected axis speed correspondence data by converting the relative speed qm included in the observation data using the corrected azimuth angle according to the procedure described in the first embodiment. .

  On the other hand, the vector least squares calculation means 122 calculates | requires the inclination x of Formula (5) by the least squares method by vector calculation using the corrected axial speed corresponding | compatible data. Then, the axis speed calculation means 121 and the vector least square calculation means 122 included in the calculation means 120 repeat the process of obtaining the inclination x while updating the azimuth angle correction amount.

  Finally, the vector least squares calculation means 122 completes the estimation process by repetition in the section where the sign of the gradient x is inverted, and the total value of the azimuth angle correction amount added to the incident angle θm at this time is an estimated value. Estimated as φ (hat).

  Depending on the set value of the step size Δφ, there may be an estimation error of about the step size Δφ in the end. For this reason, it is effective to change the step size Δφ for each update until reaching the section where the sign is inverted. In general, as the update proceeds, the estimated value φ (hat) approaches the offset angle, so the step size Δφ is reduced for each update.

  Further, in estimating the section where the gradient x is reversed, the following procedure can be considered as a method of obtaining the gradient x approaching zero by gradually changing the step size Δφ. For example, in the initial stage, the slope x when corrected by a larger Δφ is calculated, and the slope x is inverted, and the correction is made in the reverse direction by Δφ / 2, and the correction amount is gradually corrected by half of the previous time. By repeating this process, it is also possible to estimate a correction amount in which the slope x approaches zero.

In addition, the calculation means 120 calculates K correction amounts φ (hat) ^(k) (k = 1,... ⁾ Based on the respective observation data in a certain time zone k (k = 1,..., K). It is also possible to obtain K) and determine the final correction amount φ (bar) by averaging them.

  As described above, according to the second embodiment, the offset angle is estimated by repeatedly performing the calculation while correcting the azimuth angle correction amount so that the inclination of the axial velocity correspondence data converted from the observation data becomes zero. It can be performed. Furthermore, the estimation accuracy can be improved by gradually reducing the correction amount or averaging the offset angles obtained in a plurality of time zones.

Embodiment 3 FIG.
In the axis deviation angle estimation methods shown in the first and second embodiments, the least square method is repeated until the offset angle estimation value φ (hat) in the k-th time zone converges. In the third embodiment, a method for reducing the number of repetitions will be described for the purpose of further reducing the amount of calculation.

  The configuration of the axis deviation angle estimation apparatus 100 in the third embodiment is the same as that of the axis deviation angle estimation apparatus 100 shown in FIG. However, the axis deviation angle estimation apparatus 100 according to the third embodiment is characterized by performing the following arithmetic processing in order to reduce the number of repetitions.

  In the first or second embodiment, in order to obtain the offset angle estimated value φ (hat) in one time zone, observation data {qm, θm} (m = 1, 2,..., M in that time zone is obtained. ) Was repeated as an initial value. Therefore, the amount of iterative computation until convergence in one time zone becomes enormous, and when the averaging process up to the Kth time zone is performed in a lump, the amount of computation may further become enormous.

  Therefore, in the third embodiment, the number of iterations in one time zone is limited to a small number, and the averaging process is replaced with a simple addition process, thereby reducing the amount of computation.

  Specifically, in the estimation performed in the k-th time zone, the axis deviation angle estimation apparatus 100 limits the number of calculation iterations by the axis speed calculation unit 121 and the vector least square calculation unit 122 to a small number, and estimates the offset angle. The value φ (hat) is obtained.

Then, the vector least squares calculation unit 122 calculates a correction amount φ (hat) ^{(k−1) obtained} by adding the estimated values up to the ^(k−1) -th time zone (in the time zone where k = 1, the initial value is used). To get). On the other hand, the axis speed calculation means 121 sums up the estimated values up to the (k−1) -th time zone for the incident angle θm included in the observation data of each reflection point obtained from the radar in the k-th time zone. A value (θm−φ (hat) ^(k−1) ) corrected by the correction amount is set as an initial angle in the k-th time zone.

In the k-th time zone, the initial value of the observation data is {qm, θm−φ (hat) ^(k−1) } (m = 1, 2,..., M), and the estimated offset angle φ (hat) ), The amount of calculation can be reduced and the estimation result of the previous time can be reflected to improve the estimation accuracy of the offset angle.

  As described above, according to the third embodiment, when calculating the estimated value of the offset angle at the current time, instead of reducing the number of repeated calculations, the estimated value of the offset angle at the previous time is used as the observation data at the current time. By reflecting and performing the same processing in the subsequent time zone, it is possible to reduce both the amount of computation and improve the estimation accuracy.

It is a block diagram of the axial deviation angle estimation apparatus in Embodiment 1 of this invention. It is the figure which showed distribution of M observation data {qm, (theta) m} of a certain time in Embodiment 1 of this invention. It is the figure which showed distribution of axial velocity (xi) ((theta) m) with respect to angle (theta) in Embodiment 1 of this invention.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 radar, 100 axis deviation angle estimation apparatus, 110 detection means, 120 calculation means, 121 axis speed calculation means, 122 vector least squares calculation means

Claims (9)

Detecting means for detecting observation data including a relative velocity and an azimuth angle with respect to a moving radar for each of a plurality of radar reflectors that are stationary objects ;
When the angle between the radar moving direction and the radar reference direction is the true value of the radar offset angle, the radar is calculated by the least square method based on the observation data detected for each of the plurality of radar reflectors. Calculating means for calculating an estimated offset angle value ,
The computing means is
The relative velocity included in the observation data is converted into an axial velocity along the radar axis direction using the azimuth angle included in the observation data, and the axial velocity correspondence data including the converted axial velocity and the azimuth angle. Shaft speed calculation means for generating
Based on the axial velocity correspondence data generated for each of the plurality of radar reflectors, the inclination of the axial velocity with respect to the azimuth angle is calculated by a least square method using vector calculation, and the calculated inclination and the azimuth angle are zero. An axis deviation angle estimation device comprising: a vector least square calculation means for calculating an estimate value of the offset angle of the radar from the axis velocity at the time . In the axial deviation angle estimation apparatus according to claim 1 ,
The axial speed calculation means corrects the azimuth angle included in the observation data using the radar offset angle estimate calculated by the vector least square calculation means, and calculates the relative speed and the corrected azimuth angle. To generate corrected axis speed correspondence data,
The vector least squares calculation means calculates an estimated offset angle value of the radar based on the generated corrected shaft speed correspondence data,
The calculation means is configured to generate the corrected shaft speed correspondence data by the shaft speed calculation means and calculate the radar offset angle estimation value based on the corrected shaft speed correspondence data by the vector least square calculation means. An axis deviation angle estimation device characterized by being repeated a number of times. In the axial deviation angle estimation apparatus according to claim 2 ,
The calculation means changes the predetermined number of times according to time when calculating the radar offset angle estimated value for each time based on the observation data. In the axial deviation angle estimation apparatus according to claim 1 ,
The vector least squares calculation means calculates the inclination of the axis speed with respect to the azimuth angle based on the axis speed correspondence data by a least square method by vector calculation,
The axial velocity calculation means corrects each azimuth angle included in the axial velocity correspondence data with a predetermined azimuth angle correction amount according to the calculated sign of the inclination, and the relative velocity included in the observation data. Generating corrected axis speed correspondence data including the converted axis speed using the corrected azimuth angle and the corrected azimuth angle,
The vector least square calculation means calculates the inclination of the axis speed with respect to the corrected azimuth angle based on the corrected axis speed correspondence data by a least square method by vector calculation,
The calculation means repeats generation of corrected axis speed correspondence data by the axis speed calculation means and calculation of the inclination based on the corrected axis speed correspondence data by the vector least square calculation means, so that the inclination is zeroest An offset angle estimation apparatus for calculating an offset angle estimate value of a radar by obtaining an azimuth angle correction amount close to. In the axial deviation angle estimation apparatus according to claim 4 ,
The vector least squares calculation means, when repeating the calculation of the inclination, updates the azimuth angle correction amount according to the number of repetitions to calculate a radar offset angle estimation value, . The axis deviation angle estimation device according to claim 4 or 5 ,
The vector least square calculation means, when calculating the offset angle estimation value of the radar for each time based on the axis velocity correspondence data, changes the number of times the inclination is calculated repeatedly according to the time. An off-axis angle estimation device. In the axis deviation angle estimation device according to any one of claims 1 to 6 ,
The calculation means corrects the azimuth angle included in the observation data at the current time using the offset angle estimated value calculated at the previous time when calculating the offset angle estimated value at the current time, and performs the corrected observation. An axis misalignment angle estimation apparatus that calculates an offset angle estimation value of a radar by a least square method based on a vector operation using data as an initial value. Detecting observation data including a relative velocity and an azimuth angle with respect to a moving radar for each of a plurality of radar reflectors that are stationary objects ;
When the angle between the radar moving direction and the radar reference direction is the true value of the radar offset angle, the relative velocity included in the observation data is converted into the radar axis direction using the azimuth angle included in the observation data. Is converted into a shaft speed along the axis, and the axis speed correspondence data including the converted axis speed and the azimuth angle is generated. Based on the generated axis speed correspondence data, the radar offset is calculated using a least square method based on a vector operation. Calculating an angle estimate , and
Calculating the radar offset angle estimate,
Calculating a gradient of the axial speed with respect to the azimuth angle based on the axial speed correspondence data by a least square method by a vector operation;
Calculating an estimated value of the offset angle of the radar from the calculated axial velocity when the tilt and the azimuth angle are zero; and
Axial deviation angle estimating method characterized by comprising a. In the axial deviation angle estimation method according to claim 8 ,
Calculating the radar offset angle estimate,
Calculating a gradient of the axial speed with respect to the azimuth angle based on the axial speed correspondence data by a least square method by a vector operation;
According to the calculated sign of the inclination, each azimuth angle included in the axial velocity correspondence data is corrected with a predetermined azimuth angle correction amount, and the relative velocity included in the observation data is changed to the corrected azimuth angle. Generating corrected shaft speed correspondence data including the converted shaft speed and the corrected azimuth angle;
Calculating a slope of the axis speed with respect to the corrected azimuth angle based on the corrected axis speed correspondence data by a least square method by vector calculation;
By repeating the step of generating the corrected axis velocity correspondence data and the step of calculating the gradient of the axis velocity by the least square method by vector calculation, and obtaining the azimuth angle correction amount at which the calculated gradient is closest to zero A method for estimating an axis offset angle, comprising: calculating an estimated offset angle value of the radar.

Images