JP6495609B2 - Direction detection apparatus and direction detection method - Google Patents

Description

  The present invention relates to an azimuth detecting device and an azimuth detecting method for calculating an azimuth based on an output of a turning angle sensor.

  The direction detection device is mounted on a vehicle or the like and is used to detect a traveling direction. And many azimuth | direction detection apparatuses are equipped with turning angle sensors, such as a gyro sensor. The turning angle sensor detects the turning angle of the vehicle every predetermined time. The bearing detection device can detect the traveling direction of the vehicle by integrating the turning angles obtained from the turning angle sensor. However, since the output of the turning angle sensor includes a bias, a scale factor error, etc., it is necessary to estimate these errors every predetermined time and correct the output of the turning angle sensor.

As an apparatus for estimating the bias of the turning angle sensor, for example, there is one described in Patent Document 1. The device described in Patent Document 1 determines the moving state of the vehicle from the rotational speeds of the left and right wheels obtained from the wheel speed sensor. Then, the bias is estimated by averaging the outputs of the turning angle sensors when the vehicle is stopped.
Patent Document 2 describes a method for estimating a bias of a turning angle sensor. In the method described in Patent Document 2, a bias is estimated by averaging a predetermined one of the outputs of the turning angle sensor when the vehicle is stopped.

  Further, as a method for estimating the scale factor error of the turning angle sensor, there is a method using a positioning system such as GPS (Global Positioning System). For example, first, the azimuth of the vehicle is calculated from the observation amount obtained from the positioning system. Then, the scale factor error of the turning angle sensor is estimated by comparing the calculated direction and the direction obtained from the turning angle sensor.

  Patent Document 3 describes a method for correcting the attitude angle of a vehicle. In this method, a posture angle error is estimated using a high-precision positioning system and low-precision turning angle sensors and acceleration sensors. In Patent Document 4, an error of a turning angle caused by a road inclination angle and a gyro sensor installation angle is estimated. Patent Document 5 describes a method of calculating a scale factor of a turning angle sensor by performing a predetermined work.

JP-A-4-62419 JP-A-3-90812 International Publication No. 2008/142757 JP 2004-354160 A JP 7-508349

  As described above, the scale factor error of the turning angle sensor can be estimated using the positioning system. However, the positioning accuracy of a positioning system such as GPS deteriorates due to the influence of buildings, surrounding shielding, and the like. If the scale factor error is estimated based on the measurement value with reduced accuracy, there is a possibility that sufficient accuracy may not be obtained, and much time is required to estimate the scale factor error. That is, in an environment where the positioning accuracy of the positioning system is lowered, it is difficult to correct the scale factor of the turning angle sensor using the positioning system.

  An object of the present invention is to provide an azimuth detecting device having a high reliability by correcting the scale factor of a turning angle sensor with sufficient accuracy.

  The azimuth detecting device according to the present invention includes a turning angle sensor, a wheel speed sensor, a first turning angle calculation unit, a scale factor correction unit, and an azimuth calculation unit. The turning angle sensor detects the turning angle of the vehicle. The wheel speed sensor detects the number of rotations of the left and right wheels of the vehicle. The first turning angle calculation unit calculates the first turning angle based on the rotation speed. The scale factor correction unit corrects the scale factor of the turning angle sensor based on the turning angle output from the turning angle sensor and the first turning angle. The bearing calculation unit calculates the traveling direction of the vehicle based on the turning angle output from the turning angle sensor. The turning angle sensor corrects the turning angle using the scale factor correction value from the scale factor correction unit.

  In this configuration, the scale factor of the turning angle sensor can be corrected using the wheel speed sensor. Thereby, it is possible to realize a highly reliable azimuth detecting device even in an environment where the positioning accuracy of the positioning system is lowered, an environment where the positioning system cannot be used, or the like.

  According to the present invention, the scale factor of the turning angle sensor can be corrected using the wheel speed sensor. As a result, a highly reliable azimuth detecting device can be realized.

It is a block diagram which shows the structure of the azimuth | direction detection apparatus which concerns on 1st Embodiment. It is a figure explaining turning angle psi of vehicles. FIG. 3A is a flowchart showing a correction flow of the scale factor S according to the first embodiment. FIG. 3B is a flowchart showing a correction flow of the bias b _{0 according} to the first embodiment. It is a block diagram which shows the structure of the azimuth | direction detection apparatus which concerns on 2nd Embodiment. It is a flowchart which shows the correction | amendment flow of the scale factor S which concerns on 2nd Embodiment.

<< First Embodiment >>
An orientation detection apparatus 10 according to the first embodiment of the present invention will be described. FIG. 1 is a block diagram showing the configuration of the azimuth detecting device 10. The direction detection device 10 includes a turning angle sensor 11, a wheel speed sensor 12, a turning angle calculation unit 13, a scale factor correction unit 14, a bias estimation unit 15, and an direction calculation unit 16. The direction detection device 10 is mounted on a vehicle or the like and detects the traveling direction of the vehicle.

The turning angle sensor 11 detects the turning angle ψ of the vehicle. Since the turning angle output by the turning angle sensor 11 includes an error, the turning angle becomes the sensor turning angle ψ _gyro . The turning angle sensor 11 is, for example, a gyro sensor. The turning angle ψ is an angle at which the vehicle turns at a predetermined time. FIG. 2 is a diagram illustrating the turning angle ψ of the vehicle. In Figure 2, the vehicle is traveling in the e ^→ _{(t 0)} direction in time _{t 0,} running on e ^→ _{(t 1)} direction to the time _{t 0.} In this case, the turning angle ψ of the vehicle from time t ₀ to time t ₁ is an angle formed by e ^→ (t ₀ ) and e ^→ (t ₁ ).

から算出される。ここで、ｒ_Ｌは左車輪の半径であり、ｒ_Ｒは右車輪の半径である。また、図２に示すように、ｄは左右車輪間の距離である。 Wheel speed sensor 12 detects the rotational speed n _R of the right wheel rotational speed n _L and vehicle on the left wheel of the vehicle. The turning angle calculation unit 13 calculates the first turning angle ψ _w based on the rotation speeds n _L and n _R. Since the first turning angle ψ _w includes an error, the first turning angle ψ _w has a value different from the turning angle ψ. The turning angle calculation unit 13 corresponds to the first turning angle calculation unit of the present invention. The first turning angle ψ _w is, for example,

Is calculated from Here, r _L is the radius of the left wheel and r _R is the radius of the right wheel. Further, as shown in FIG. 2, d is the distance between the left and right wheels.

Incidentally, GPS, when the optical beacon or the like (not shown) can be utilized, _S L by the least squares method using the [psi _ref as a first pivot angle [psi _w, and calculates the _{S R.} Here, ψ _ref is the turning angle of the vehicle obtained from GPS, an optical beacon or the like.

The scale factor correction unit 14 corrects the scale factor S of the turning angle sensor 11 based on the difference between the first turning angle ψ _w and the sensor turning angle ψ _gyro when the vehicle is turning. The scale factor is the input change minus the output change. For example, the scale factor S is corrected as follows.

と表される。ここで、ｂ_０はバイアスであり、ηはスケールファクタ誤差である。バイアスとは、入力がない場合における単位時間当たりの出力である。ここで、バイアスｂ_０は、後述のようにバイアス推定部１５による推定値により補正されるので、０とみなされる。スケールファクタ誤差ηは、真のスケールファクタをＳ_Ｔ、現在のスケールファクタをＳとして、Ｓ＝（１＋η）Ｓ_Ｔと表される。
一般に、移動体が正常に走行しているとき、第１旋回角ψ_ｗはセンサ旋回角ψ_ｇｙｒоに比べて高い精度を有する。そこで、第１旋回角ψ_ｗが閾値に比べて大きいとき、車両の旋回角ψとして第１旋回角ψ_ｗを用いて最小二乗法によりスケールファクタ誤差ηを算出する。そして、Ｓ／（１＋η）を新たにスケールファクタＳとすることにより、スケールファクタＳを補正する。
スケールファクタＳの補正値は、旋回角センサ１１が補正後に旋回角ψを検出するときに用いられる。 When the vehicle turns by the turning angle ψ from time t to time t + Δt, the error ψ _{gyro ®} included in the sensor turning angle ψ _gyro is

It is expressed. Here, b ₀ is a bias and η is a scale factor error. The bias is an output per unit time when there is no input. Here, since the bias b ₀ is corrected by an estimated value by the bias estimating unit 15 as described later, it is regarded as 0. Scale factor error eta is a real scale factor _{S T,} the current scale factor as S, it is expressed as _{S = (1 + η) S} T.
In general, when the moving body is traveling normally, the first turning angle ψ _w has higher accuracy than the sensor turning angle ψ _gyro . Therefore, when the first turning angle ψ _w is larger than the threshold value, the scale factor error η is calculated by the least square method using the first turning angle ψ _w as the turning angle ψ of the vehicle. Then, the scale factor S is corrected by newly setting the scale factor S to S / (1 + η).
The correction value of the scale factor S is used when the turning angle sensor 11 detects the turning angle ψ after correction.

The bias estimation unit 15 estimates the bias b ₀ of the turning angle sensor 11 when the rotation speeds n _L and n _R are smaller than the threshold values. For example, the bias b ₀ is estimated as follows.
When the rotation speeds n _L and n _R are smaller than the threshold value, the vehicle can be regarded as stationary. Therefore, the turning angle ψ of the vehicle becomes 0, and the equation (2) becomes ψ _gyro = b ₀ Δt. The bias b ₀ is calculated from b ₀ = E (ψ _gyro / Δt). Here, E (•) means an average value. By using the rotation speeds n _L and n _R of the left and right wheels, it can be determined with high accuracy whether or not the vehicle is stationary. Thereby, the bias b ₀ can be estimated at high speed.
The turning angle sensor 11 _corrects the bias b ₀ of the sensor turning angle ψ _gyro by making ψ _gyro -b ₀ Δt a new sensor turning angle ψ _gyro .

The azimuth calculation unit 16 calculates the traveling direction h of the vehicle by integrating the sensor turning angle ψ _gyro .

Next, the process of correcting the scale factor S will be described. FIG. 3A is a flowchart showing a correction flow of the scale factor S according to the first embodiment.
A turning angle ψ of the vehicle is detected using the turning angle sensor 11 (S101). Since the turning angle output by the turning angle sensor 11 includes an error, the turning angle becomes the sensor turning angle ψ _gyro . Next, the rotation speeds n _L and n _R of the left and right wheels are detected (S102). With swivel angle calculating section 13, the rotational speed _n L, it calculates a first pivot angle [psi _w based on _{n R} (S103). If the vehicle is turning (larger first pivot angle [psi _w is compared with the threshold value) (S104: Yes), to estimate the scale factor error eta (S105), corrects the scale factor S based on the estimated (S106 ). When the vehicle is not turning, the process ends (S104: No). Note that the timing of performing step S101 is not limited to the above as long as it is before performing step S105.

Next, a process for correcting the bias b ₀ will be described. FIG. 3B is a flowchart showing a correction flow of the bias b _{0 according} to the first embodiment.
A turning angle ψ of the vehicle is detected using the turning angle sensor 11 (S111). Since the turning angle output by the turning angle sensor 11 includes an error, the turning angle becomes the sensor turning angle ψ _gyro . Next, the rotation speeds n _L and n _R of the left and right wheels are detected (S112). When the vehicle is stationary (the rotation speeds n _L and n _R are smaller than the threshold values) (S113: Yes), the bias b ₀ of the turning angle sensor 11 is estimated (S114), and the bias b ₀ is based on the estimation. Is corrected (S115). If the vehicle is not stationary, the process is terminated (S113: No). In addition, if it is before performing process S114, the time which performs process S111 is not limited above.

According to the first embodiment, the wheel speed sensor 12 is used to correct the bias b ₀ and the scale factor S of the turning angle sensor 11. Thereby, the azimuth | direction detection apparatus 10 with high reliability is realizable also in the environment where the positioning accuracy of a positioning system falls, the environment where a positioning system cannot be utilized, etc. In particular, the direction detection device 10 is effective in a tunnel or the like where the positioning system cannot be used for a long time.

<< Second Embodiment >>
An orientation detection device 20 according to a second embodiment of the present invention will be described. FIG. 4 is a block diagram showing a configuration of the azimuth detecting device 20. The bearing detection device 20 includes a GPS turning angle calculation unit 21 in addition to the configuration of the bearing detection device 10 according to the first embodiment. Further, a scale factor correction unit 24 is provided instead of the scale factor correction unit 14 according to the first embodiment. GPS corresponds to the positioning system of the present invention. The GPS turning angle calculation unit 21 corresponds to the second turning angle calculation unit of the present invention.

Turning angle calculating section 13, the rotational speed _n L, based on _{n R,} to calculate a first angle of rotation [psi _w and its accuracy. The GPS turning angle calculation unit 21 calculates the second turning angle ψ _gps and its accuracy based on the observation amount obtained from the GPS. The second turning angle ψ _gps has a different value from ψ because it includes an error.
As in the first embodiment, the scale factor correction unit 24 corrects the scale factor S of the turning angle sensor 11 when the vehicle is turning (when the turning angle ψ of the vehicle is larger than the threshold value). In this case, if the precision of the first turning angle [psi _w is greater than or accuracy of the second pivot angle [psi _gps, first turning angle [psi _w as turn angle [psi of the vehicle is used. If the accuracy of the first turning angle [psi _w is less than the accuracy of the second pivot angle [psi _gps, the second turning angle [psi _gps is used as the turn angle [psi of the vehicle.

Next, the process of correcting the scale factor S will be described. FIG. 5 is a flowchart showing a correction flow of the scale factor S according to the second embodiment.
A turning angle ψ of the vehicle is detected using the turning angle sensor 11 (S201). Since the turning angle output by the turning angle sensor 11 includes an error, the turning angle becomes the sensor turning angle ψ _gyro . Next, the first turning angle ψ _w of the vehicle and its accuracy are calculated based on the rotation speeds n _L and n _R of the left and right wheels using the turning angle calculation unit 13 (S202). Next, the second turning angle ψ _gps and the accuracy thereof are calculated based on the observation amount obtained from the GPS using the GPS turning angle calculation unit 21 (S203). If the accuracy of the first turning angle [psi _w is greater than or accuracy of the second pivot angle ψ _gps (S204: Yes), using a first pivot angle [psi _w as turn angle [psi of the vehicle (S205). If the accuracy of the first turning angle [psi _w is less than the accuracy of the second pivot angle ψ _gps (S204: No), using the second turning angle [psi _gps as turn angle [psi of the vehicle (S206). Next, when the vehicle is turning (S207: Yes), the scale factor error η is estimated (S208), and the scale factor S is corrected based on the estimation (S209). If the vehicle is not turning, the process ends (S207: No).

According to the second embodiment, when the accuracy of the first turning angle ψ _w is equal to or higher than the accuracy of the second turning angle ψ _gps , the wheel speed sensor 12 is used to correct the scale factor S of the turning angle sensor 11. Thereby, the azimuth detecting device 20 having high reliability can be realized even in an environment where the GPS positioning accuracy is lowered due to multipath or the like, or in an environment where the GPS cannot be used.

DESCRIPTION OF SYMBOLS 10,20 ... Direction detection apparatus 11 ... Turning angle sensor 12 ... Wheel speed sensor 13 ... Turning angle calculation part 14, 24 ... Scale factor correction part 15 ... Bias estimation part 16 ... Direction calculation part 21 ... GPS turning angle calculation part

Claims (8)

A turning angle sensor for detecting the turning angle of the vehicle;
A wheel speed sensor for detecting the rotational speed of the left and right wheels of the vehicle;
A first turning angle calculation unit for calculating a first turning angle based on the rotational speed;
When the first turning angle is larger than a threshold value for detecting turning of the vehicle, the scale factor of the turning angle sensor is set based on the turning angle output by the turning angle sensor and the first turning angle. A scale factor correction unit to correct,
An azimuth calculating unit that calculates a traveling direction of the vehicle based on the turning angle output by the turning angle sensor;
The turning angle sensor is an azimuth detecting device that corrects the turning angle using a correction value of the scale factor from the scale factor correction unit. The azimuth detecting device according to claim 1,
Based on a difference between the turning angle before Symbol the turning angle sensor and the first turning angle output, the scale factor correcting unit corrects the scale factor, the azimuth detecting device. The azimuth detecting device according to claim 1 or 2,
A second turning angle calculation unit for calculating a second turning angle based on an observation amount obtained from the positioning system;
When the estimation accuracy at the time of calculating the first turning angle is equal to or higher than the estimation accuracy at the time of calculating the second turning angle, based on the turning angle output by the turning angle sensor and the first turning angle, A scale factor correction unit corrects the scale factor, and is an azimuth detection device. The azimuth detecting device according to claim 1 or 2,
A second turning angle calculation unit for calculating a second turning angle based on an observation amount obtained from the positioning system;
When the estimation accuracy at the time of calculating the first turning angle is less than the estimation accuracy at the time of calculating the second turning angle, based on the turning angle output by the turning angle sensor and the second turning angle, A scale factor correction unit corrects the scale factor, and is an azimuth detection device. The azimuth detecting device according to claim 1 or 2,
A second turning angle calculation unit for calculating a second turning angle based on an observation amount obtained from the positioning system;
When the estimation accuracy at the time of calculating the first turning angle is equal to or higher than the estimation accuracy at the time of calculating the second turning angle, based on the turning angle output by the turning angle sensor and the first turning angle, The scale factor correction unit corrects the scale factor,
When the estimation accuracy at the time of calculating the first turning angle is less than the estimation accuracy at the time of calculating the second turning angle, based on the turning angle output by the turning angle sensor and the second turning angle, A scale factor correction unit corrects the scale factor, and is an azimuth detection device. The azimuth detecting device according to any one of claims 1 to 5,
An azimuth detection device comprising a bias estimation unit that estimates a bias of the turning angle sensor when the rotational speed is smaller than a threshold value.   The azimuth detecting device according to any one of claims 1 to 6,
  The first turning angle calculation unit includes:
  Using a turning angle calculated based on an observation amount obtained from the positioning system, and setting a coefficient for calculating the first turning angle from the rotational speed;
  Orientation detection device. Detecting a turning angle of the vehicle by a turning angle sensor;
Detecting the rotational speed of the left and right wheels of the vehicle by means of a wheel speed sensor;
Calculating a first turning angle based on the rotational speed;
When the first turning angle is larger than a threshold value for detecting turning of the vehicle, the scale factor of the turning angle sensor is set based on the turning angle output by the turning angle sensor and the first turning angle. A process of correcting,
Calculating the traveling direction of the vehicle based on the turning angle output by the turning angle sensor,
The azimuth detection method, wherein the turning angle sensor corrects the turning angle using a correction value of the scale factor.

Images