JP2012141220A - Installation angle detection device, method, program, and storage medium - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an installation angle detection device, method, program, and storage medium with improved reliability of detecting an installation angle of a detection device such as a pitch gyro.SOLUTION: The present invention comprises: a yaw angle change amount acquisition unit 110 which acquires a yaw angle change amount θz of a vehicle in a prescribed detection range; a pitch angle change amount calculation unit 120 which calculates a pitch angle change amount θy in the prescribed detection range; and an installation angle detection unit 130 which detects an installation angle φx of a pitch gyro 21 on a roll axis with respect to the vehicle based on a ratio between the yaw angle change amount θz detected by the yaw angle change amount acquisition unit 110 and the pitch angle change amount θy calculated by the pitch angle change amount calculation unit 120. Thus, the invention can improve reliability of detecting the installation angle φx of a detecting device such as the pitch gyro 21.

Description

  The present invention relates to a mounting angle detection device, a method, a program, and a recording medium for a detection device such as a gyro sensor.

  For example, a navigation device mounted on a vehicle or the like acquires travel distance and moving direction information based on position information received from a GPS (Global Positioning System) satellite and detection information by a gyro sensor or an acceleration sensor, and the current position Some of them are displaying guidance by guessing the direction of travel. However, when the above-described gyro sensor or acceleration sensor is mounted with the detection axis tilted with respect to the vehicle, the output value decreases from a desired value, and an accurate value cannot be obtained.

  For this reason, it is necessary to detect the attachment angle of the detection device attached to the vehicle-mounted device or the like. There has been proposed an in-vehicle device that notifies that the mounting angle of such an in-vehicle device is abnormal (see Patent Document 1).

  This conventional vehicle-mounted device includes a gyro sensor and a GPS receiver, and is a vehicle-mounted device mounted on a vehicle. A ratio between a reference angular velocity obtained from an output of the GPS receiver and an angular velocity detected by the gyro sensor is calculated. Thus, the sensitivity of the gyro sensor is obtained, and when the obtained sensitivity exceeds an allowable range, it is notified that the mounting angle of the in-vehicle device is abnormal. The in-vehicle device also integrates the acceleration sensor output As when the acceleration component in the traveling direction is zero, calculates an average value Aave, and divides the averaged value Aave by the gravitational acceleration g. Thus, sin (θ pitch) corresponding to the mounting angle in the pitch direction is obtained.

Japanese Patent No. 4191499

  However, in the above-described conventional in-vehicle device, when the sensor is attached to be inclined around the roll axis, it is affected by the change in the yaw angle, that is, the left / right turning of the vehicle. In particular, the angle change of the yaw angle can occur from 0 to 360 degrees. Therefore, the effect becomes large when the angle change is large. Also, even if the pitch angle changes greatly, it is about ± 5 degrees, which is very small compared to the yaw angle. Therefore, using the acceleration value affected by the yaw angle to determine the pitch angle is an error range. There was a problem that became large.

  Furthermore, even when data is received from the GPS and used, the use of the azimuth position information data corresponding to the yaw angle results in a slight error in a wide range, but the altitude information data corresponding to the pitch angle is The range of data values to be used is narrow, and the error rate is high. For this reason, there is a problem that reliability is lowered when it is attempted to detect the mounting angle around the pitch axis directly from the pitch angle information.

  The present invention has been made in order to solve the above-described problem of the conventional problem as an example, and an attachment angle detection device, method, and program for improving the reliability of detection of the attachment angle of a detection device such as a gyro sensor. Another object is to provide a recording medium.

In order to solve the above-described problem, the invention according to claim 1 is an attachment angle detection device that detects an attachment angle of a detection device attached to a moving body that moves in an XY plane with a change in the Z-axis direction in an XYZ coordinate system. , A Z-axis rotation angle change amount acquisition means for acquiring a Z-axis rotation angle change amount around the Z axis of the movable body in a detection range from a predetermined detection start position to a predetermined detection end position, and detected by the detection device Y-axis rotation angle change amount calculation means for calculating a Y-axis rotation angle change amount around the Y axis in the detection range based on the detected value, and a Z-axis acquired by the Z-axis rotation angle change amount acquisition means Based on the ratio between the rotation angle change amount and the Y-axis rotation angle change amount calculated by the Y-axis rotation angle change amount calculation means, the attachment angle of the detection device around the X-axis with respect to the moving body is detected. take And only the angle detection means, characterized by comprising a.
The variation in the Z-axis direction refers to displacement of the moving body caused by the inclination or unevenness of the traveling surface substantially parallel to the XY plane on which the moving body travels.

  According to a twelfth aspect of the present invention, there is provided an attachment angle detection method for detecting an attachment angle of a detection device attached to a moving body that moves on an XY plane accompanied by a change in the Z-axis direction in an XYZ coordinate system. A Z-axis rotation angle change amount acquisition step for acquiring a Z-axis rotation angle change amount around the Z axis of the moving body in a detection range from a start position to a predetermined detection end position, and a detection result detected by the detection device A Y-axis rotation angle change amount calculating step for calculating a Y-axis rotation angle change amount around the Y axis in the detection range, and a Z-axis rotation angle change amount acquired in the Z-axis rotation angle change amount acquiring step, Based on the ratio of the Y-axis rotation angle change amount calculated in the Y-axis rotation angle change amount calculation step to the Y-axis rotation angle change amount, an attachment angle detection for detecting the attachment angle of the detection device around the X axis with respect to the moving body. A method, characterized by comprising a.

  Further, the invention described in claim 13 is a mounting angle detection program for detecting a mounting angle of a detection device mounted on a moving body that moves on an XY plane accompanied by a change in the Z-axis direction in an XYZ coordinate system. A Z-axis rotation angle change amount acquisition step for acquiring a Z-axis rotation angle change amount around the Z axis of the moving body in a detection range from a start position to a predetermined detection end position, and a detection result detected by the detection device A Y-axis rotation angle change amount calculating step for calculating a Y-axis rotation angle change amount around the Y axis in the detection range, and a Z-axis rotation angle change amount acquired in the Z-axis rotation angle change amount acquiring step, Mounting for detecting an attachment angle of the detection device around the X axis with respect to the movable body based on a ratio to the Y axis rotation angle change amount calculated in the Y axis rotation angle change amount calculating step. Characterized in that to execute the angle detecting step, to the computer.

  Furthermore, a recording medium according to a fourteenth aspect is characterized in that the mounting angle detection program according to the thirteenth aspect is recorded.

It is a block diagram which shows schematic structure of the attachment angle detection apparatus in this Embodiment. It is a figure which shows the definition of the coordinate system in this Embodiment, and the attachment angle of a detection apparatus. It is a figure explaining the operation principle in this Embodiment. It is a flowchart which shows the attachment angle detection process in this Embodiment. It is a flowchart which shows the correction process of the pitch angle variation | change_quantity in this Embodiment. It is a flowchart which shows the attachment angle detection process in this Embodiment.

Embodiments of the present invention will be described below with reference to the drawings.
FIG. 1 is a block diagram showing a schematic configuration of a mounting angle detection device in the present embodiment. FIG. 2 is a diagram illustrating the definition of the coordinate system and the attachment angle of the detection device in the present embodiment. In the present embodiment, the vehicle 10 is used as a moving body, and the pitch gyro 21 is used as an example of a detection device attached to the moving body, and the mounting angle φx around the X axis (roll axis) of the pitch gyro 21 with respect to the vehicle 10 is set. The mounting angle detection device 100 to be detected will be described.
The vehicle 10 is basically based on turning and movement on the XY plane, although the X-axis direction is the traveling direction and changes slightly in the Z-axis direction due to inclination and unevenness.

  As shown in FIG. 1, the attachment angle detection device 100 calculates a yaw angle change amount acquisition unit 110 that acquires a yaw angle change amount θz in a predetermined detection range, and a pitch angle change amount θy in the predetermined detection range. Pitch angle change amount calculation unit 120, attachment angle detection unit 130 that detects attachment angle φx, roll axis inclination angle calculation unit 140 that calculates inclination angle ψx of vehicle 10, and pitch angular velocity ωy detected by pitch gyro 21 Or the correction | amendment part 150 which correct | amends the pitch angle variation | change_quantity (theta) y calculated by pitch angular velocity (omega) y based on the attachment angle (phi) x is provided.

  Here, the yaw angle change amount θz means a change amount θz of the Z-axis rotation angle around the Z-axis (yaw axis) of the vehicle 10. The pitch angle change amount θy means the change amount θy of the Y-axis rotation angle around the Y axis (pitch axis). Furthermore, the attachment angle φx means the attachment angle φx around the X axis of the pitch gyro 21 with respect to the vehicle 10.

Further, the inclination angle ψx of the vehicle 10 means an inclination angle ψx between the XY plane and the horizontal plane on which the vehicle 10 is grounded. The pitch angular velocity ωy refers to the amount of change in pitch angle per unit time, that is, the Y-axis rotational angular velocity ωy about the Y-axis.
Further, hereinafter, the Z-axis rotation angular velocity ωz is referred to as a yaw angular velocity ωz.

  The yaw angle change amount acquisition unit 110 includes a yaw gyro 111 that detects the yaw angular velocity ωz, a GPS receiver 112 that acquires position information P, and a yaw angle change amount calculation unit that calculates the yaw angle change amount θz in a predetermined detection range. 113, a Z-axis rotation angle change amount acquisition means.

Here, the predetermined detection range is a position where the yaw angular velocity ωz becomes a predetermined angular velocity threshold value ωth, for example, 3 [° / s], as a detection start position, and is less than the predetermined angular velocity threshold value ωth. The detected position is set as the detection end position.
As described above, the attachment angle detection device 100 according to the present embodiment detects an attachment value φx by detecting the attachment angle φx in a case where the yaw angular velocity ωz is equal to or greater than the predetermined angular velocity threshold ωth, thereby obtaining an unstable value. The detection error can be reduced and the reliability of detection of the mounting angle φx can be increased.

Note that the angular velocity threshold value ωth for determining the detection start position and the angular velocity threshold value ωth for determining the detection end position may be different values.
The position information P acquired by the GPS receiver 112 is information that can acquire the east longitude, north latitude, altitude, etc. of the vehicle 10 for each position of the vehicle 10 at any time.

  The yaw gyro 111 is a Z-axis rotation angular velocity detection unit that detects the yaw angular velocity ωz of the vehicle 10. The GPS receiver 112 is position information acquisition means for acquiring position information P such as east longitude and north latitude of the vehicle 10. The yaw angle change amount calculation unit 113 calculates the yaw angle change amount θz of the vehicle 10 in a predetermined detection range from the transition of the yaw angular velocity ωz acquired by the yaw gyroscope 11 or the position information P acquired by the GPS receiver 112. It is an angle change amount calculation means.

  As described above, the mounting angle detection device 100 according to the present embodiment is based on the yaw angular velocity ωz detected by the yaw gyro 111 or the position information P acquired by the GPS receiver 112, and the yaw angle change amount of the vehicle 10. Since θz can be acquired and the mounting angle φx is detected, the detection reliability can be enhanced with simple components.

  The pitch angle variation calculation unit 120 acquires the pitch angular velocity ωy of the vehicle 10 from the pitch gyro 21 and calculates the pitch angle variation θy of the vehicle 10 in a predetermined detection range from the transition of the acquired pitch angular velocity ωy. It is an angle change amount calculation means.

The attachment angle detection unit 130 is attached based on a ratio between the yaw angle change amount θz acquired by the yaw angle change amount acquisition unit 110 and the pitch angle change amount θy calculated by the pitch angle change amount calculation unit 120. It is an attachment angle detection means for detecting the angle φx.
Thereby, the mounting angle detection apparatus 100 in this Embodiment can improve the reliability of detection of the mounting angle (phi) x around the roll axis with respect to the vehicle 10 of a pitch gyro.

In addition, the attachment angle detection unit 130, when the yaw angle change amount θz acquired by the yaw angle change amount acquisition unit 110 is a predetermined rotation angle change amount threshold θzth, for example, 60 [°] or more, The mounting angle φx is detected.
As described above, the attachment angle detection device 100 according to the present embodiment has a large displacement by detecting the attachment angle φx when the yaw angle change amount θz is equal to or larger than the predetermined rotation angle change amount threshold value θzth. The detection error can be reduced by detecting only the time, and the reliability of the detection of the mounting angle φx can be increased.

Further, the attachment angle detection unit 130 acquires the yaw angle change amount θz and the pitch angle change amount θy in each of a plurality of detection ranges, and detects an attachment angle detection value φxn (n = 1, 2, 3) in each detection range. ..)) And the detected attachment angle φxn is averaged to detect the attachment angle φx.
Thereby, the attachment angle detection apparatus 100 in this Embodiment can make a detection error small, and can improve the reliability of detection of the attachment angle (phi) x.

Furthermore, the attachment angle detector 130 detects the attachment angle φx by correcting the yaw angle change amount θz and the pitch angle change amount θy based on the inclination angle ψx calculated by the roll axis inclination angle calculation unit 140. You can also.
Thereby, the attachment angle detection apparatus 100 in this Embodiment can improve the reliability of a detection.

  The roll axis inclination angle calculation unit 140 is an X axis inclination angle calculation unit that calculates the inclination angle ψx of the vehicle 10. For example, the roll axis inclination angle calculation unit 140 acquires the roll angular velocity of the vehicle 10 from the roll gyro, and calculates the inclination angle ψx of the vehicle 10 based on the acquired value of the roll angular velocity.

The correction unit 150 is a correction unit that corrects the pitch angular velocity ωy or the pitch angle change amount θy detected by the pitch gyro 21 based on the attachment angle φx.
As described above, the mounting angle detection device 100 according to the present embodiment corrects the pitch angular velocity ωy or the pitch angle change amount θy detected by the pitch gyro 21 based on the detected mounting angle φx, and therefore a highly reliable pitch. The angular velocity ωy or the pitch angle change amount θy can be output.

  In the above configuration, the roll gyro mounting angle φx can be detected instead of the pitch gyro 21 as a detection device for detecting the mounting angle φx. When detecting the mounting angle φx of the roll gyro, the mounting angle φx around the pitch axis can be detected with the same configuration as described above with the X axis as the pitch axis and the Y axis as the roll axis.

  That is, the pitch angle change amount calculation unit 120 functions as a roll angle change amount calculation unit using the Y axis as a roll axis, acquires the roll angular velocity ωy of the vehicle 10 from the roll gyro, and determines a predetermined value from the acquired transition of the roll angular velocity ωy. The roll angle change amount θy of the vehicle 10 in the detection range is calculated.

  The attachment angle detection unit 130 attaches based on the ratio between the yaw angle change amount θz acquired by the yaw angle change amount acquisition unit 110 and the roll angle change amount θy calculated by the pitch angle change amount calculation unit 120. The angle φx is detected.

  The roll axis inclination angle calculation unit 140 calculates the inclination angle ψx of the vehicle 10, but unlike the above embodiment, the roll axis inclination angle calculation unit 140 functions as a pitch axis inclination angle calculation unit, for example, a pitch angular velocity of the vehicle 10 from a pitch gyro. And the inclination angle ψx of the vehicle 10 is calculated based on the acquired pitch angular velocity value.

The correction unit 150 corrects the roll angular velocity ωy or the roll angle change amount θy detected by the roll gyro based on the attachment angle φx.
Thereby, the mounting angle detection apparatus 100 in this Embodiment can improve the reliability of detection of the mounting angle (phi) x around the pitch axis with respect to the vehicle 10 of a roll gyro.

Next, an operation principle for detecting the mounting angle φx in the present embodiment will be described.
2 and 3 are diagrams for explaining the principle of detecting the mounting angle φx in the mounting angle detection device 100 of the present embodiment.

When the pitch gyro 21 is mounted horizontally with respect to the vehicle 10, the pitch angle θy does not change even when the vehicle 10 turns around the yaw axis.
On the other hand, as shown in FIG. 2B, when the pitch gyro 21 is attached to the vehicle 10 so as to be inclined around the roll axis, when the vehicle 10 turns around the yaw axis, the pitch angle θy is Change. Further, the pitch angle θy that changes with the turning around the yaw axis is a value corresponding to the magnitude of the turning of the vehicle 10 around the yaw axis.

  Therefore, the mounting angle detection device 100 of the present embodiment detects the pitch angle change amount θy that changes with the yaw angle change amount θz of the vehicle 10, and the ratio between the yaw angle change amount θz and the pitch angle change amount θy. Based on the above, the mounting angle φx around the roll axis of the pitch gyro 21 is detected.

Specifically, according to the following formula (1):
φx = sin ⁻¹ {(θy / θz) * (180 / π)} (1)
An attachment angle φx is calculated.
The yaw angle change amount θz and the pitch angle change amount θy are determined by the following method. That is, as shown in FIG. 3, the yaw angle change amount θz is calculated by integrating the yaw angular velocity ωz within the detection range with the range where the yaw angular velocity ωz is equal to or greater than the angular velocity threshold ωth as the detection range. Then, the pitch angle change amount θy is calculated by integrating the pitch angular velocity ωy within the same detection range.

FIG. 4 shows a flowchart of the detection process of the attachment angle φx in the attachment angle detection apparatus 100, and will be described.
The attachment angle detection device 100 includes, for example, a CPU, a ROM, and a RAM, stores an attachment angle detection program in which attachment angle detection processing is recorded in the ROM, and the CPU uses the RAM as a work area and attaches the attachment angle. The detection program is executed.

As shown in FIG. 4, the attachment angle detection apparatus 100 performs an initialization process when the attachment angle detection process is started (step S11). Specifically, the attachment angle detection device 100 clears (= 0) the values of the calculation count N and the attachment angle average value φxav.
Next, the attachment angle detection device 100 performs angle change amount initialization processing (step S12). Specifically, the attachment angle detection device 100 clears (sets to 0) the values of the yaw angle change amount θz and the pitch angle change amount θy.

  Next, the attachment angle detection device 100 determines whether or not the detection unit time has elapsed since the previous acquisition of the yaw angular velocity ωz (step S13). If the detection unit time has not elapsed since the previous acquisition of the yaw angular velocity ωz, the process waits until the detection unit time elapses. If the detection unit time has elapsed, the process proceeds to step S14.

Next, the attachment angle detection device 100 acquires the yaw angular velocity ωz by the yaw gyro 111 (step S14).
Next, the attachment angle detection device 100 determines whether or not the acquired yaw angular velocity ωz is equal to or higher than the yaw angular velocity threshold ωth, and when the acquired yaw angular velocity ωz is equal to or higher than the yaw angular velocity threshold ωth, The process proceeds to step S16, and if the acquired yaw angular velocity ωz is not equal to or higher than the yaw angular velocity threshold ωth, the process proceeds to step S21 (step S15).

If the acquired yaw angular velocity ωz is equal to or greater than the yaw angular velocity threshold ωth, the attachment angle detection device 100 updates the value of the yaw angle change amount θz based on the yaw angular velocity ωz (step S16). Specifically, a value obtained by multiplying the yaw angular velocity ωz acquired this time by the detection unit time is added to the previous yaw angle change amount θz.
Further, the attachment angle detection device 100 acquires the pitch angular velocity ωy by the pitch gyro 21 (step S17).

  Next, the attachment angle detection device 100 updates the value of the pitch angle change amount θy based on the acquired pitch angular velocity ωy, and proceeds to step S13 (step S18). Specifically, a value obtained by multiplying the pitch angle velocity ωy acquired this time by the detection unit time is added to the pitch angle change amount θy until the previous time.

  On the other hand, when the acquired yaw angular velocity ωz is not equal to or greater than the yaw angular velocity threshold ωth (determined as NO in step S15), the attachment angle detecting device 100 determines that the yaw angle variation θz is equal to or greater than the yaw angle variation threshold θzth. If the yaw angle change amount θz is equal to or greater than the yaw angle change amount threshold value θzth, the process proceeds to step S22, where the yaw angle change amount θz is the yaw angle change amount threshold value θzth. If not, the process proceeds to step S12 (step S21).

When the yaw angle change amount θz is equal to or greater than the yaw angle change amount threshold value θzth, the attachment angle detection device 100 determines the attachment angle φx based on the ratio of the yaw angle change amount θz and the pitch angle change amount θy. Detect (step S22). Specifically, sin ⁻¹ {(θy / θz) * (180 / π)} is calculated by the above equation (1), and the attachment angle φx is calculated.

Next, the attachment angle detection device 100 adds “1” to the calculation count N (step S23). Next, the attachment angle detection device 100 performs an averaging process of the attachment angle φx. (Step S24). Specifically, a value obtained by dividing the difference value between the currently calculated mounting angle φx and the previous mounting angle average value φxav by the calculation number N is added to the previous mounting angle average value φxav. The following formula (2) shows a calculation formula for the above-mentioned attachment angle average value φxav.
φxav _(n) = φxav _(n−1) + (φx−φxav _(n−1) ) / N (2)

  Next, the attachment angle detection device 100 determines whether or not the end condition of the detection process of the attachment angle φx is satisfied. If the end condition is satisfied, the attachment angle detection apparatus 100 ends the attachment angle detection process, and the end condition is If not, the process proceeds to step S12 (step S25). For example, when the calculation number N reaches a predetermined detection process end number, the main attachment angle detection process is ended.

  Through the above processing, the mounting angle detection device 100 can detect the mounting angle φx of the pitch gyro 21 with respect to the vehicle 10.

Next, correction processing of the pitch angle change amount θy will be described using the mounting angle φx detected by the mounting angle detection device 100.
FIG. 5 shows a flowchart of the correction process of the pitch angle change amount θy in the mounting angle detection apparatus 100 and will be described.

  As shown in FIG. 5, when the pitch angle change amount correction process is started, the attachment angle detection device 100 determines whether or not the attachment angle φx has been detected (step S31). If the attachment angle φx has been detected, the process proceeds to step S32. If the attachment angle φx has not been detected, the pitch angle variation correction processing is terminated.

  If the attachment angle φx has already been detected, the attachment angle detection device 100 calculates the influence of the yaw angle change amount θz that affects the pitch angle change amount θy (step S32). Specifically, the influence degree is calculated by θz * sin {φx * (π / 180)}.

  Next, the mounting angle detection apparatus 100 subtracts the influence degree from the calculated pitch angle change amount θy to calculate the corrected pitch angle change amount θy (step S33), and corrects the pitch angle change amount. The process ends.

  Next, FIG. 6 shows a flowchart of an attachment angle φx detection process in the attachment angle detector 100 when the GPS receiver 112 is used, and will be described.

As shown in FIG. 6, the attachment angle detection device 100 performs an initialization process when the attachment angle detection process is started (step S41). Specifically, the attachment angle detection device 100 clears (= 0) the values of the calculation count N and the attachment angle average value φxav.
Next, the mounting angle detection device 100 acquires the current position by the GPS reception device 112 and stores it as the previous position Po (step S42).

  Next, the attachment angle detection device 100 determines whether or not the detection unit time has elapsed since the previous acquisition of the current position (step S43). If the detection unit time has not elapsed since the previous acquisition of the current position, the process waits until the detection unit time elapses. If the detection unit time has elapsed, the process proceeds to step S44.

Next, the attachment angle detection device 100 acquires the current position by the GPS reception device 112 and stores it as the current position Pn (step S44).
Next, the attachment angle detection device 100 calculates the current movement direction An based on the current position Pn and the previous position Po (step S45).

  Next, the attachment angle detection device 100 performs angle change amount initialization processing (step S51). Specifically, the attachment angle detection device 100 clears (sets to 0) the values of the yaw angle change amount θz and the pitch angle change amount θy.

  Next, the attachment angle detection device 100 determines whether or not the detection unit time has elapsed since the previous acquisition of the current position (step S52). If the detection unit time has not elapsed since the previous acquisition of the current position, the process waits until the detection unit time elapses. If the detection unit time has elapsed, the process proceeds to step S53.

Next, the mounting angle detection device 100 substitutes the current position Pn for the previous position Po (step S53), and substitutes the current movement direction An for the previous movement direction Ao (step S54).
Next, the mounting angle detection device 100 acquires the current position by the GPS reception device 112 and stores it as the current position Pn (step S55).

Next, the attachment angle detection device 100 calculates the current movement direction An based on the current position Pn and the previous position Po (step S56).
Next, the attachment angle detection device 100 calculates the azimuth difference value θzdef based on the current movement azimuth An and the previous movement azimuth Ao (step S57).

  Next, the attachment angle detection device 100 determines whether or not the calculated azimuth difference value θzdef is equal to or greater than a predetermined azimuth difference value threshold θzdefth, and the calculated azimuth difference value θzdef is equal to or greater than the azimuth difference value threshold θzdefth. If YES, the process proceeds to step S61. If the calculated azimuth difference value θzdef is not equal to or greater than the azimuth difference value threshold θzdefth, the process proceeds to step S71 (step S58).

If the calculated azimuth difference value θzdef is greater than or equal to the azimuth difference value threshold θzdefth, the attachment angle detection device 100 updates the value of the yaw angle change amount θz based on the azimuth difference value θzdef (step S61). . Specifically, the azimuth difference value θzdef calculated this time is added to the previous yaw angle change amount θz.
Further, the mounting angle detection device 100 acquires the pitch angular velocity ωy by the pitch gyro 21 (step S62).

  Next, the attachment angle detection device 100 updates the value of the pitch angle change amount θy based on the acquired pitch angular velocity ωy, and proceeds to step S52 (step S63). Specifically, a value obtained by multiplying the pitch angle velocity ωy acquired this time by the detection unit time is added to the pitch angle change amount θy until the previous time.

  On the other hand, when the calculated azimuth difference value θzdef is not greater than or equal to the azimuth difference value threshold θzdefth (determined as NO in step S58), the attachment angle detection device 100 determines that the yaw angle change amount θz is the yaw angle change threshold value. It is determined whether or not it is greater than or equal to θzth. If the yaw angle change amount θz is greater than or equal to the yaw angle change amount threshold value θzth, the process proceeds to step S72, where the yaw angle change amount θz is greater than the yaw angle change amount threshold. If it is not equal to or greater than the value θzth, the process proceeds to step S51 (step S71).

When the yaw angle change amount θz is equal to or greater than the yaw angle change amount threshold value θzth, the attachment angle detection device 100 determines the attachment angle φx based on the ratio of the yaw angle change amount θz and the pitch angle change amount θy. It detects (step S72). Specifically, sin ⁻¹ {(θy / θz) * (180 / π)} is calculated by the above equation (1), and the attachment angle φx is calculated.

  Next, the attachment angle detection device 100 adds “1” to the calculation count N (step S73). Next, the attachment angle detection device 100 performs an averaging process of the attachment angle φx. (Step S74). Specifically, a value obtained by dividing the difference value between the currently calculated mounting angle φx and the previous mounting angle average value φxav by the calculation number N is added to the previous mounting angle average value φxav. The calculation formula for the attachment angle average value φxav is the same as the formula (2) described above.

Next, the attachment angle detection device 100 determines whether or not the end condition of the detection process of the attachment angle φx is satisfied. If the end condition is satisfied, the attachment angle detection apparatus 100 ends the attachment angle detection process, and the end condition is If not, the process proceeds to step S51 (step S75).
With the above processing, the mounting angle detection device 100 can detect the mounting angle φx of the pitch gyro 21 with respect to the vehicle 10.

  As described above, according to the attachment angle detection device, the attachment angle detection method, the attachment angle detection program, and the recording medium according to the present embodiment, the yaw angle change amount acquisition unit that acquires the yaw angle change amount θz in the predetermined detection range. 110, a pitch angle change amount calculation unit 120 that calculates the pitch angle change amount θy in the predetermined detection range, and a ratio between the acquired yaw angle change amount θz and the calculated pitch angle change amount θy. Since the mounting angle detector 130 for detecting the angle φx is provided, the reliability of detection of the mounting angle φx of the detection device such as the pitch gyro 21 can be improved.

  Similarly, in the recording medium on which the present attachment angle detection method, the attachment angle detection program, and the attachment angle detection program are recorded, the attachment angle φx is detected based on the ratio of the yaw angle change amount θz and the pitch angle change amount θy. Therefore, the reliability of detection of the mounting angle φx of the detection device such as the pitch gyro 21 can be enhanced.

DESCRIPTION OF SYMBOLS 10 Vehicle 100 Attachment angle detection apparatus 110 Yaw angle change amount acquisition part 111 Yaw gyro 112 GPS receiver 113 Yaw angle change amount calculation part 120 Pitch angle change amount calculation part 130 Attachment angle detection part 140 Roll axis inclination angle calculation part 150 Correction | amendment means

Claims (14)

In an attachment angle detection device that detects an attachment angle of a detection device attached to a moving body that moves in an XY plane with a change in the Z-axis direction in the XYZ coordinate system,
Z-axis rotation angle change amount acquisition means for acquiring a Z-axis rotation angle change amount around the Z axis of the movable body in a detection range from a predetermined detection start position to a predetermined detection end position;
Y-axis rotation angle change amount calculating means for calculating a Y-axis rotation angle change amount around the Y axis in the detection range based on a detection value detected by the detection device;
The detection is based on a ratio between the Z-axis rotation angle change amount acquired by the Z-axis rotation angle change amount acquisition unit and the Y-axis rotation angle change amount calculated by the Y-axis rotation angle change amount calculation unit. An attachment angle detection means for detecting an attachment angle around the X axis with respect to the movable body of the apparatus;
A mounting angle detection device comprising:   The attachment angle detection means acquires the Z-axis rotation angle change amount and the Y-axis rotation angle change amount in each of the plurality of detection ranges, calculates a detection value of the attachment angle in each detection range, and The attachment angle detection device according to claim 1, wherein the attachment angle is detected by averaging detected values of the angle. An X-axis inclination angle calculating means for calculating an inclination angle of the movable body around the X-axis;
The attachment angle detection means detects the attachment angle by correcting the Z-axis rotation angle change amount and the Y-axis rotation angle change amount based on the inclination angle calculated by the X-axis inclination angle calculation means. The attachment angle detecting device according to claim 1 or 2, wherein   The Z-axis rotation angle change amount acquisition means sets the position where the rotation angular velocity around the Z axis is equal to or higher than a predetermined angular velocity as the detection start position. The mounting angle detection device according to item.   The Z-axis rotation angle change amount acquisition unit sets the position where the Z-axis rotation angular velocity is less than a predetermined angular velocity as the detection end position. The mounting angle detection device described in 1.   The attachment angle detection means detects the attachment angle when the Z-axis rotation angle change amount acquired by the Z-axis rotation angle change amount acquisition means is greater than or equal to a predetermined rotation angle change amount. The mounting angle detection device according to any one of claims 1 to 5.   The correction means which correct | amends the detected value detected by the said detection apparatus based on the said attachment angle detected by the said attachment angle detection means is provided, The any one of Claim 1-6 characterized by the above-mentioned. The mounting angle detection device described in 1. The Z-axis rotation angle change amount acquisition means is
Z-axis rotational angular velocity detection means for detecting a plurality of Z-axis rotational angular velocities in the detection range;
Z-axis rotation angle change amount calculating means for calculating the Z-axis rotation angle change amount of the movable body in the detection range based on the Z-axis rotation angular velocity detected by the Z-axis rotation angular velocity detection means;
The mounting angle detection device according to claim 1, further comprising: The Z-axis rotation angle change amount acquisition means is
Position information acquisition means for acquiring a plurality of pieces of position information of the moving body in the detection range;
Z-axis rotation angle change amount calculation means for calculating the Z-axis rotation angle change amount of the moving body in the detection range from the transition of the position information acquired by the position information acquisition means;
The mounting angle detection device according to claim 1, further comprising: The detection device is a pitch gyro that detects a pitch angular velocity about a pitch axis orthogonal to a traveling direction of the moving body,
The attachment angle detection device according to any one of claims 1 to 9, wherein the attachment angle detection unit detects an attachment angle of the pitch gyro around the roll axis with respect to the moving body. The detection device is a roll gyro which detects a roll angular velocity around a roll axis parallel to the traveling direction of the moving body,
The attachment angle detection device according to any one of claims 1 to 9, wherein the attachment angle detection unit detects an attachment angle of the roll gyro around the pitch axis with respect to the moving body. In an attachment angle detection method for detecting an attachment angle of a detection device attached to a moving body that moves on an XY plane with a change in the Z-axis direction in an XYZ coordinate system,
A Z-axis rotation angle change amount acquisition step for acquiring a Z-axis rotation angle change amount around the Z axis of the movable body in a detection range from a predetermined detection start position to a predetermined detection end position;
A Y-axis rotation angle change amount calculating step for calculating a Y-axis rotation angle change amount around the Y axis in the detection range based on a detection result detected by the detection device;
Based on the ratio between the Z-axis rotation angle change amount acquired in the Z-axis rotation angle change amount acquisition step and the Y-axis rotation angle change amount calculated in the Y-axis rotation angle change amount calculation step, the detection device An attachment angle detection step of detecting an attachment angle around the X axis with respect to the moving body;
A mounting angle detection method comprising: In an attachment angle detection program for detecting an attachment angle of a detection device attached to a moving body that moves on an XY plane with a change in the Z-axis direction in the XYZ coordinate system,
A Z-axis rotation angle change amount acquisition step for acquiring a Z-axis rotation angle change amount around the Z axis of the movable body in a detection range from a predetermined detection start position to a predetermined detection end position;
A Y-axis rotation angle change amount calculating step for calculating a Y-axis rotation angle change amount around the Y axis in the detection range based on a detection result detected by the detection device;
Based on the ratio between the Z-axis rotation angle change amount acquired in the Z-axis rotation angle change amount acquisition step and the Y-axis rotation angle change amount calculated in the Y-axis rotation angle change amount calculation step, the detection device An attachment angle detection step of detecting an attachment angle around the X axis with respect to the moving body;
A mounting angle detection program for causing a computer to execute the above. A recording medium on which the installation angle detection program according to claim 13 is recorded.

Images