JP2014077769A - Sensor tilt determination device and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To enable whether or not an axis of a yaw-rate sensor is tilted to be accurately determined.SOLUTION: HDOP is calculated by a Doppler reliability determination part 76. A change in a GPS estimation azimuth angle of an own vehicle is estimated by an azimuth angle change estimation part 24, using a GPS Doppler. On the basis of a yaw rate detected by a gyro sensor 14, an angle change in the gyro sensor is calculated by an angle change acquisition part 26. When it is determined by a gyro axis determination part 78 that a calculated HDOP is less than a threshold value, and reliability of a GPS is excellent, and further that a residual dispersion in an optimal estimation based on the change in the GPS estimation azimuth angle and the angle change in the gyro sensor is more than the threshold value and the yaw rate generates, the gyro axis determination part 78 determines that an axis of a gyro sensor 16 is tilted to a vertical direction with respect to a horizontal plane.

Description

  The present invention relates to a sensor tilt determination apparatus and program, and more particularly, to a sensor tilt determination apparatus and program for determining whether an axis of a yaw rate sensor mounted on a moving body is tilted with respect to a direction perpendicular to a horizontal plane.

  2. Description of the Related Art Conventionally, an angular velocity correction device that corrects sensor mounting error, vehicle pitch angle, and gyro sensitivity error using GPS is known (Patent Document 1). In this angular velocity correction device, the azimuth angle output using GPS is estimated based on the time difference of the positioning position. Further, the angular velocity correction device corrects the sensor so as to match the inclination angle and azimuth angle obtained from GPS.

JP 2009-139227 A

  However, in the technique described in Patent Document 1, since the direction is estimated from the difference between the GPS positioning results, a time delay occurs and the positioning accuracy of the GPS itself is about several meters. There is a problem that the estimation accuracy is not high. In addition, even when the tilt angle is estimated, there is a problem that the tilt angle cannot be accurately estimated because the accuracy in the height direction is significantly degraded as compared with the positional accuracy in the plane.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to provide a sensor tilt determination device and a program capable of accurately determining whether or not the axis of a yaw rate sensor is tilted. To do.

  In order to achieve the above object, a sensor inclination determination device according to the present invention is mounted on a moving body, and the yaw rate sensor that detects the yaw rate generated in the moving body and the GPS satellites transmitted from each of the plurality of GPS satellites are provided. Satellite information acquisition means for acquiring satellite information including information on the position of each GPS satellite, information on the distance between each of the GPS satellites and the mobile body, and information on the relative speed of the mobile body with respect to each of the GPS satellites And based on the yaw rate detected by the yaw rate sensor, based on the satellite information acquired by the satellite information acquisition means, first calculation means for calculating a change in the angle of the moving body in the reference direction, A second calculation means for calculating a change in the azimuth angle of the reference direction of the moving body; and a change in the angle calculated by the first calculation means; Yaw rate is generated in the moving body based on the angle change determination means for determining whether or not the change in the azimuth calculated by the second calculation means corresponds to the yaw rate detected by the yaw rate sensor. The yaw rate determination means for determining whether or not the angle change determination means determines that the change in the angle does not correspond, and the yaw rate determination means determines that the yaw rate is occurring. And an inclination determination means for determining that the axis of the yaw rate sensor is inclined with respect to a direction perpendicular to the horizontal plane.

  Further, the program of the present invention causes a computer to transmit information on the position of each of the GPS satellites transmitted from each of a plurality of GPS satellites, information on the distance between each of the GPS satellites and a moving object, and the GPS satellites. Satellite information acquisition means for acquiring satellite information including information related to the relative speed of the moving body with respect to each, based on the yaw rate detected by the yaw rate sensor mounted on the moving body and detecting the yaw rate generated in the moving body A first calculating means for calculating a change in the angle of the movable body around the yaw rate sensor, and a change in the azimuth angle of the moving body in the reference direction based on the satellite information obtained by the satellite information obtaining means. Second calculating means for calculating the angle change of the angle calculated by the first calculating means, and the second calculating means. Based on the yaw rate detected by the yaw rate sensor, the angle change determination means for determining whether or not the calculated change in the azimuth corresponds, and whether or not yaw rate is generated in the moving body. When the yaw rate determination means and the angle change determination means determine that the change in angle does not correspond, and the yaw rate determination means determines that yaw rate is occurring, the yaw rate sensor This is a program for causing an axis to function as an inclination determination unit that determines that an axis is inclined with respect to a direction perpendicular to a horizontal plane.

  According to the present invention, the yaw rate sensor detects the yaw rate that is mounted on the moving body and is generated on the moving body. Information on the position of each of the GPS satellites transmitted from each of a plurality of GPS satellites by the satellite information acquisition means, information on the distance between each of the GPS satellites and the mobile object, and the movement with respect to each of the GPS satellites Obtain satellite information including information about the relative velocity of the body.

  Then, the first calculation means calculates a change in the angle of the movable body around the axis of the yaw rate sensor based on the yaw rate detected by the yaw rate sensor. Based on the satellite information acquired by the satellite information acquisition means, the second calculation means calculates a change in the azimuth angle of the moving body in the reference direction.

  Then, an angle change determination unit determines whether or not the change in angle calculated by the first calculation unit corresponds to the change in azimuth angle calculated by the second calculation unit. Based on the yaw rate detected by the yaw rate sensor, the yaw rate determination means determines whether or not yaw rate is generated in the moving body. When the inclination determination means determines that the angle change does not correspond to the angle change determination means, and the yaw rate determination means determines that yaw rate is occurring, the yaw rate sensor axis is It is determined that it is inclined with respect to the direction perpendicular to the horizontal plane.

  Thus, when it is determined that the change in the angle calculated from the yaw rate sensor does not correspond to the change in the azimuth angle calculated using the satellite information, and the yaw rate is occurring, the yaw rate sensor It is possible to accurately determine whether or not the axis of the yaw rate sensor is inclined by determining that the axis is inclined with respect to the direction perpendicular to the horizontal plane.

  The sensor inclination determination device according to the present invention is detected by the yaw rate sensor based on the change in the angle calculated by the first calculation means and the change in the azimuth angle calculated by the second calculation means. Bias correction means for correcting the yaw rate bias, the yaw rate bias-corrected by the bias correction means, and the amount of change in the azimuth angle change calculated by the second calculation means. Inclination angle estimation means for estimating an inclination angle at which the axis is inclined with respect to a direction perpendicular to the horizontal plane may be further included. As a result, the tilt angle at which the axis of the yaw rate sensor is tilted can be accurately estimated.

  Further, the sensor inclination determination device may be configured such that, based on a change in inclination angle estimated by the inclination angle estimation means, the inclination of the shaft of the yaw rate sensor is an inclination due to an attachment error, or the roll direction of the moving body It is possible to further include a roll determination means for determining whether the inclination is caused by the movement of the movement.

  The sensor inclination determination device according to the present invention is detected by the yaw rate sensor based on the change in the angle calculated by the first calculation means and the change in the azimuth angle calculated by the second calculation means. A bias correction unit that performs bias correction of the yaw rate, a yaw rate based on the change in the angle calculated by the first calculation unit, a ratio of the yaw rate bias-corrected by the bias correction unit, and the moving body in the roll direction On the basis of the yaw rate ratio obtained in advance when not moving, the inclination of the shaft of the yaw rate sensor is an inclination due to an attachment error or an inclination due to the movement of the movable body in the roll direction. It is possible to further include roll determining means for determining whether or not.

  The sensor inclination determination device according to the present invention further includes a reliability calculation means for calculating an index indicating a GPS reliability obtained from satellite information of each of the plurality of GPS satellites, and the inclination determination means includes the reliability When it is determined that the reliability of the GPS is high based on the index calculated by the calculation unit, the angle change determination unit determines that the change in angle does not correspond, and the yaw rate When it is determined by the determination means that yaw rate is occurring, it can be determined that the axis of the yaw rate sensor is inclined with respect to the direction perpendicular to the horizontal plane.

  The sensor inclination determination apparatus according to the present invention includes a height change calculation unit that calculates a change in the height direction of the mobile body based on the satellite information acquired by the satellite information acquisition unit, and the plurality of GPS satellites. And a reliability calculating means for calculating a first index indicating the reliability of GPS in the horizontal plane and a second index indicating the reliability of GPS in the height direction obtained from each satellite information The determination unit is a case where it is determined that the GPS reliability in the horizontal plane direction is high based on the first index calculated by the reliability calculation unit, and the angle change determination unit changes the angle. When it is determined that the yaw rate is not supported and the yaw rate determining means determines that yaw rate is occurring, the axis of the yaw rate sensor is in a direction perpendicular to the horizontal plane. Calculated by the height change calculating means when it is determined that the GPS is inclined and the reliability of the GPS in the height direction is determined to be high based on the second index calculated by the reliability calculating means. A roll for determining whether the inclination of the axis of the yaw rate sensor is an inclination due to an attachment error or an inclination due to the movement of the moving body in the roll direction based on the change in the height direction of the moving body. A determination means can be further included.

  The index indicating the reliability of the GPS is obtained from the satellite information of each of the plurality of GPS satellites received by the GPS receiver at the same time, and the arrangement of the plurality of GPS satellites transmitting each of the plurality of satellite information. It can be set as DOP which shows the fall rate of corresponding positioning accuracy.

  The sensor inclination determination device indicates a velocity vector including a traveling direction of the moving body, or a speed of the moving body and a traveling direction of the moving body, based on the satellite information acquired by the satellite information acquiring unit. Further comprising speed calculating means for calculating speed information, wherein the second calculating means estimates the azimuth angle of the moving body in the reference direction based on the speed information calculated by the speed calculating means, It is possible to calculate the change in the azimuth angle of the moving body in the reference direction.

  The speed calculation means is viewed from the mobile body based on the position of the mobile body obtained from information on the position of each of the GPS satellites and information on the distance between each of the GPS satellites and the mobile body. Calculate the direction of each of the GPS satellites, calculate the velocity of each of the GPS satellites based on the information on the position of each of the GPS satellites in time series, and each of the GPS satellites viewed from the mobile body Based on information on the direction, the speed of each of the GPS satellites, and the relative speed of the mobile body with respect to each of the GPS satellites, the speed of each direction of the GPS satellites of the mobile body is calculated, and a plurality of the mobile bodies The velocity vector of the moving body can be calculated based on the velocity in the direction of each of the GPS satellites.

  The information on the position of each of the GPS satellites is satellite orbit information, the information on the distance between each of the GPS satellites and the moving body is pseudo-range information, and the relative speed of the moving body with respect to each of the GPS satellites The information regarding can be Doppler frequency information.

  The storage medium for storing the program of the present invention is not particularly limited, and may be a hard disk or a ROM. Further, it may be a CD-ROM, a DVD disk, a magneto-optical disk or an IC card. Furthermore, the program may be downloaded from a server or the like connected to the network.

  As described above, according to the sensor inclination determination apparatus and program of the present invention, it is determined that the change in angle calculated from the yaw rate sensor does not correspond to the change in azimuth calculated using satellite information. When the yaw rate is occurring, it is determined that the yaw rate sensor axis is tilted by determining that the yaw rate sensor axis is tilted with respect to the direction perpendicular to the horizontal plane. The effect that it can judge with sufficient precision is acquired.

It is a block diagram which shows the sensor inclination determination apparatus which concerns on 1st Embodiment. It is a block diagram which shows the angle change estimation part of the sensor inclination determination apparatus which concerns on 1st Embodiment. It is an image figure which shows a mode that the speed of the own vehicle of a GPS satellite direction is calculated based on the velocity vector of each GPS satellite, and the direction of each GPS satellite. It is a graph which shows the change of the gyro estimated angle based on the output of a yaw rate sensor, and the change of the GPS estimated azimuth based on GPS Doppler. It is a figure which shows the output and locus | trajectory of a gyro sensor at the time of left turn and right turn in case there is no axis | shaft shift of a gyro sensor and an installation position shift | offset | difference with respect to the turning center of a vehicle. It is a figure which shows the output and locus | trajectory of a gyro sensor at the time of left turn and right turn in the case where there is no installation position shift with respect to the turning center of a vehicle, and there is a shaft shift of the gyro sensor. It is a figure which shows the output and locus | trajectory of a gyro sensor at the time of left turn and right turn in case there exists a shift | offset | difference of the axis | shaft of a gyro sensor and an installation position shift with respect to the turning center of a vehicle. (A) Graph showing changes in GPS estimated azimuth angle based on GPS Doppler and changes in gyro estimated angle after bias correction, (B) GPS estimated azimuth angle variation based on GPS Doppler, and bias correction 6 is a graph showing the yaw rate and (C) a graph showing a change in the speed in the height direction of the host vehicle. It is a figure which shows the flow which determines the axis | shaft deviation of a gyro sensor. It is a flowchart which shows the content of the gyro sensor axial angle estimation process routine in the computer of the sensor inclination determination apparatus which concerns on 1st Embodiment. It is a flowchart which shows the content of the azimuth angle change estimation process routine in the computer of the sensor inclination determination apparatus which concerns on 1st Embodiment. It is a block diagram which shows the sensor inclination determination apparatus which concerns on 2nd Embodiment. It is a figure for demonstrating the method of determining whether the axis | shaft deviation of a gyro sensor is based on a roll motion, or a mounting error. It is a block diagram which shows the sensor inclination determination apparatus which concerns on 3rd Embodiment. It is a figure which shows the cooperation type ACC system which follows a preceding vehicle using communication. It is a block diagram which shows the cooperation type ACC system which concerns on 4th Embodiment.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the present embodiment, a case where the present invention is applied to a sensor shaft angle estimation device that is mounted on a vehicle and acquires GPS information transmitted from a GPS satellite and estimates a tilt angle of a gyro sensor will be described as an example. To do.

  As shown in FIG. 1, the sensor shaft angle estimation apparatus 10 according to the first embodiment includes a GPS receiver 12 that receives radio waves from GPS satellites, a gyro sensor 14 that detects yaw rate of the host vehicle, Based on the speed sensor 16 that detects the speed of the vehicle, the received signal from the GPS satellite received by the GPS receiver 12, the detected value of the gyro sensor 14, and the detected value of the speed sensor 16, the shaft angle of the gyro sensor 14 And a computer 18 that executes a process for estimating the travel locus of the host vehicle. The gyro sensor 14 is an example of a yaw rate sensor.

  The GPS receiver 12 receives radio waves from a plurality of GPS satellites, and from the received signals from all the received GPS satellites, as GPS satellite information, the satellite number of the GPS satellite, orbit information of the GPS satellite (ephemeris) The time when the GPS satellite transmits the radio wave, the intensity of the received signal, the frequency, and the like are acquired and output to the computer 18.

  The computer 18 includes a storage device such as a CPU, a ROM that stores a program for realizing a later-described gyro sensor shaft angle estimation processing routine, a RAM that temporarily stores data, and an HDD.

  When the computer 18 is represented by a functional block, as shown in FIG. 1, GPS satellite information is acquired from the GPS receiver 12 for all GPS satellites that have received radio waves, and GPS pseudorange data, Doppler frequency, and Based on the GPS information acquisition unit 20 that calculates and acquires the position coordinates of the GPS satellite, the data storage unit 22 that stores the detection value of the gyro sensor 14 and the detection value of the speed sensor 16, and the acquired GPS satellite information, An azimuth angle change estimation unit 24 that estimates a change in the azimuth angle of the host vehicle, an angle change acquisition unit 26 that estimates a change in the angle of the host vehicle based on the stored detection value of the gyro sensor 14, and an azimuth change Posture for estimating the axial angle of the gyro sensor 14 based on the change in azimuth and the change in angle obtained by the estimation unit 24 and the angle change acquisition unit 26 A tough 28, the locus estimating unit 30 for estimating the trajectory of the vehicle in the predetermined time can be represented by the inclusive configure. The azimuth angle change estimation unit 24 is an example of a first calculation unit, and the angle change acquisition unit 26 is an example of a second calculation unit.

  The GPS information acquisition unit 20 acquires GPS satellite information for all GPS satellites that have received radio waves from the GPS reception unit 12, and at the time when the GPS satellites transmit radio waves and the time at which the vehicle receives radio waves. Based on this, GPS pseudorange data is calculated. Further, the GPS information acquisition unit 20 sets the Doppler frequency of the received signal from each GPS satellite based on the known frequency of the signal transmitted from each GPS satellite and the frequency of the received signal received from each GPS satellite. calculate. The Doppler frequency is obtained by observing the Doppler shift amount of the carrier frequency due to the relative speed between the GPS satellite and the own vehicle. Further, the GPS information acquisition unit 20 calculates the position coordinates of the GPS satellites based on the orbit information of the GPS satellites and the time at which the GPS satellites transmitted radio waves.

  As shown in FIG. 2, the azimuth angle change estimation unit 24 calculates the position of the GPS satellite and the position of the host vehicle based on the acquired GPS information, and the Doppler frequency of each acquired GPS satellite. Based on the relative speed calculation unit 60 that calculates the relative speed of the host vehicle with respect to each GPS satellite, and the satellite velocity calculation that calculates the velocity vector of each GPS satellite based on the acquired time series data of the position coordinates of each GPS satellite. Unit 62, satellite direction calculation unit 66 for calculating the direction (angle relation) of each GPS satellite based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, the calculated relative velocity, and each GPS Based on the satellite velocity vector and the direction of each GPS satellite, a satellite direction own vehicle speed calculation unit 68 that calculates the speed of the own vehicle in each GPS satellite direction, and a plurality of calculated GPS satellites A host vehicle speed calculation unit 70 that calculates a speed vector of the host vehicle based on the speed of the host vehicle in the direction, and a change in azimuth by calculating an azimuth angle of the host vehicle based on the calculated speed vector of the host vehicle. The vehicle azimuth calculating unit 72 for calculating (time series) can be represented by a configuration.

  The position calculation unit 58 calculates the position coordinates of the GPS satellites based on the orbit information of the GPS satellites and the time when the GPS satellites transmitted radio waves.

  Moreover, the position calculation part 58 calculates the position of the own vehicle using the GPS pseudo distance data of each GPS satellite acquired by the GPS information acquisition part 20 as follows.

  In positioning using GPS, the position of the host vehicle is estimated according to the principle of triangulation based on the known position coordinates of GPS satellites and the pseudo distance that is the propagation distance of the received signal received from each GPS satellite. The

Here, the true distance r _j to the GPS satellite is expressed by the following equation (1), and the pseudorange ρ _j observed by GPS is expressed by the following equation (2).

However, (X _j , Y _j , Z _j ) is the position coordinate of the GPS satellite j, and (x, y, z) is the position coordinate of the host vehicle. s is a distance error due to a clock error of the GPS receiver 12.

  From the above equations (1) and (2), by solving the simultaneous equations of the following equation (3) obtained from the GPS pseudorange data of four or more GPS satellites, the position of the host vehicle (x, y, z ) Is calculated.

  The relative speed calculation unit 60 calculates the relative speed of the host vehicle with respect to each GPS satellite from the Doppler frequency of the received signal from each GPS satellite according to the following equation (4) representing the relationship between the Doppler frequency and the relative speed with respect to the GPS satellite. calculate.

Here, v _j is a relative velocity with respect to the GPS satellite j, and D1 _1j is a Doppler frequency (Doppler shift amount) obtained from the GPS satellite j. C is the speed of light, and F ₁ is a known L1 frequency of a signal transmitted from a GPS satellite.

The satellite velocity calculation unit 62 calculates the velocity vector (three-dimensional velocity VX _j , VY _j , VZ _j ) of each GPS satellite from the acquired time series data of the position coordinates of each GPS satellite using the differentiation of Kepler's equation. calculate. For example, using the method described in a non-patent document (Pratap Misra and Per Eng, the original Japanese Navigational Society GPS Study Group translation: “Sophisticated GPS Basic Concept / Positioning Principle / Signal and Receiver”, Shoyo Bunko, 2004.) The velocity vector of each GPS satellite can be calculated.

The satellite direction calculation unit 66, based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, the angular relationship between the position of each GPS satellite j and the position of the own vehicle (elevation angle θ _{j with} respect to the horizontal direction, north direction) the phi _j) azimuth angle to be calculated as the direction of each GPS satellite.

As shown in FIG. 3, the satellite direction own vehicle speed calculating unit 68 calculates the relative speed v _j of the own vehicle with respect to each GPS satellite, the velocity vectors (VX _j , VY _j , VZ _j ) of each GPS satellite, Based on the GPS satellite direction R _j (θ _j , φ _j ), the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the following equation (5).

v _j is a relative speed of the host vehicle with respect to the GPS satellite j (relative speed with respect to the GPS satellite in the satellite direction). Vs _j is the speed of the GPS satellite j in the direction of the vehicle, and is obtained by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T. Vv _j is the vehicle speed in the direction of the GPS satellite j, and vCb is a clock bias fluctuation.

  As described above, the own vehicle speed in the direction of the GPS satellite is calculated not only by the three-dimensional position of the GPS satellite position but only by the orientation relationship with the GPS satellite. The GPS satellite is far away, and the angle change per minute is 0.5 degrees because it makes two rounds of the earth in one day. Usually, since the clock error between the GPS satellite and the GPS receiver is usually 1 msec or less, the azimuth relationship with the GPS satellite is not greatly affected. Similarly, since the GPS satellite is far away, even if an error of about several hundred meters occurs in determining the position of the own vehicle, the orientation relationship with the GPS satellite is not greatly affected. For this reason, even if it is a situation where an error is likely to ride on the pseudorange, the own vehicle speed in the GPS satellite direction can be calculated relatively accurately.

  The host vehicle speed calculation unit 70 performs optimum estimation of the speed vector of the host vehicle, as will be described below.

First, when the velocity vector of the vehicle and (Vx, Vy, Vz), the relationship between the speed Vv _j of the GPS satellites direction of the vehicle is expressed by the following equation (6).

  From the above equation (6) obtained for each GPS satellite j, simultaneous equations represented by the following equation (7) with Vx, Vy, Vz and Cb as estimated values are obtained.

  When the number of GPS satellites that have received radio waves is four or more, the optimal value of the velocity vector (Vx, Vy, Vz) of the host vehicle is calculated by solving the simultaneous equation (7).

  The own vehicle azimuth angle calculation unit 72 calculates the azimuth angle in the front-rear direction of the own vehicle from the speed vector of the own vehicle calculated by the own vehicle speed calculation unit 70 using a trigonometric function, and stores it in a memory (not shown). . The own vehicle azimuth calculating unit 72 outputs the change (time series) of the azimuth of the own vehicle stored in the memory as an estimation result of the azimuth change. The azimuth angle estimated here is an azimuth angle estimated based on GPS information, and is hereinafter referred to as “GPS estimated azimuth angle”.

  On the other hand, the angle of the host vehicle calculated from the detection value of the gyro sensor 14 by the angle change acquisition unit 26 is referred to as a “gyro estimated angle”. However, since the gyro sensor 14 can only measure the change in angle, the estimated gyro angle is calculated from the detection value of the gyro sensor 14 by integrating the elapsed time with a certain initial value as a reference.

  The angle change acquisition unit 26 outputs the change (time series) of the angle of the host vehicle.

  The posture estimation unit 28 is based on the gyro bias estimation unit 74 that estimates the bias of the gyro sensor 14 based on the change in GPS estimated azimuth angle and the change in gyro angle, and on all GPS information acquired at the same time, Based on the Doppler reliability determination unit 76 that determines the DOP (dilution of precision) value as the Doppler reliability, and the bias estimation result of the gyro sensor 14 and the DOP, it is determined whether the axis of the gyro sensor 14 is inclined. Based on the detected value of the gyro axis inclination determination unit 78, the bias-corrected gyro sensor 14, the gyro axis angle estimation unit 80 that estimates the inclination angle of the axis of the gyro sensor 14, and the estimated inclination angle, Whether the inclination of the axis of the gyro sensor 14 is due to an installation error or a roll motion including a bank is determined. And a roll determination section 82. The gyro axis inclination determination unit 78 is an example of an azimuth angle change determination unit, a yaw rate determination unit, and an inclination determination unit. The gyro bias estimation unit 74 is an example of a bias correction unit. The gyro axis angle estimation unit 80 is an example of an inclination angle estimation unit.

As shown in FIG. 4, the gyro bias estimation unit 74 compares the GPS estimated azimuth angle change with the gyro angle change, sets the initial angle and the bias as unknowns, and performs various GPS Doppler estimates and bias correction. The initial angle and the bias of the detected value of the gyro sensor 14 are estimated by performing the optimal estimation so that the error from the gyro value of the gyro sensor 14 is minimized. Specifically, an initial angle θ _initialGyro and a bias G _yroBias that minimize the function f shown in the following equation (8) are obtained.

However, ω _Gyro is a gyro yaw rate measurement value, and θ _GPS is a GPS estimated azimuth angle.

  When performing the optimal estimation, if the GPS Doppler residual is greater than or equal to a certain level, the accuracy of the GPS Doppler is assumed to be degraded, and the accuracy can be improved by removing it from the estimation. The gyro bias estimation unit 74 obtains a detection value of the gyro sensor 14 that has been subjected to bias correction. The gyro bias estimation unit 74 calculates a residual at each time between the change in the gyro angle based on the detected value of the gyro sensor 14 that has performed the bias correction and the change in the GPS estimated azimuth angle using the Doppler. Variation (dispersion) is calculated.

  The Doppler reliability determination unit 76 calculates the DOP of the GPS information group using all the GPS information acquired at the same time (hereinafter also referred to as “GPS information group”). The DOP is an index that represents the rate of decrease in positioning accuracy in accordance with the geometrical arrangement of a plurality of GPS satellites that have transmitted each of a plurality of GPS information. A well-known calculation method described in "Basic Concept, Positioning Principle, Signal and Receiver" by Partap Misra and Per Enge can be used.

  If the GPS satellites are scattered, the DOP will be good (small). Since the GPS positioning is performed based on the principle of triangulation, the error due to the geometric arrangement becomes smaller as the dispersion degree of the GPS satellites becomes larger. On the other hand, in an example where the DOP is bad (large), there are cases where GPS satellites are fixedly arranged near the zenith, or GPS satellites are fixedly arranged only in a certain direction. In such a situation, since the dispersion of the GPS satellites is small, the DOP becomes large and the error due to the geometric arrangement also becomes large.

  Here, the principle of determining whether the rotation axis of the yaw rate detected by the gyro sensor 14 is inclined will be described.

  The bias correction method using the GPS Doppler by the gyro bias estimation unit 74 described above is performed when the axis of the gyro sensor 14 coincides with the axis of rotation of the moving body (axis perpendicular to the horizontal plane). It is established, and is not established when the axis of the rotation plane and the axis of the gyro sensor 14 are deviated.

  For example, as shown in FIG. 5, when there is no axial displacement or installation position displacement of the gyro sensor with respect to the turning center of the vehicle, in the situation where the bias of the gyro sensor and the vehicle speed are estimated sufficiently accurately, And the locus output by the gyro sensor draw almost the same shape.

  However, as shown in FIG. 6, when the installation position is offset, the yaw rate sensor outputs the same value as the correct value, but the speed at the installation position of the gyro sensor is different from the speed of the turning center. There is a difference in the estimated trajectory shape. In this case, the offset amount of the gyro sensor can often be measured in advance, and does not affect the yaw rate itself.

  On the other hand, as shown in FIG. 7, when the axis of the gyro sensor is at an angle with respect to the axis of the actual turning center of the vehicle (axis perpendicular to the horizontal plane), the absolute value of the actual yaw rate is A small value is output. Similar errors occur not only at the installation angle but also when the vehicle body itself is tilted, such as in a bank. In that case, there is a problem in using the output result of the yaw rate sensor as it is. The purpose of this embodiment is to increase vehicle motion measurement accuracy by estimating the attitude of the gyro sensor.

  If the measured value of GPS Doppler is always reliable, the estimated value from GPS Doppler may be used instead of the yaw rate. However, the accuracy of GPS tends to deteriorate as the number of satellites decreases. There are cases where it cannot be estimated in central Tokyo, and the robustness is low. For this reason, a method is known in which the azimuth angle is estimated where GPS Doppler can be received, and the locus is estimated from the detection value of the gyro sensor and the vehicle speed.

  The actual bias correction of the gyro sensor is performed by the gyro bias estimation unit 74 using the result of the optimum estimation as shown in FIG. At this time, since it is assumed that the angle change based on the gyro sensor and the azimuth angle change based on the GPS Doppler are the same, if the gyro sensor has an axis shift as shown in FIG. There is sex. If there is an axis shift, the output value of the yaw rate sensor is wrong in the first place. Therefore, even if yaw rate integration is performed in consideration of the bias component, a correct angle change cannot be obtained.

  When there is an attachment error or an axis shift due to a bank, a change in azimuth angle as shown in FIG. 8A and a change in yaw rate as shown in FIG. 8B are expected.

  If there is an axis shift in a situation where yaw rate occurs, a value smaller than the actual yaw rate is output, so that the amplitude is suppressed. Since only the portion where the yaw rate is generated is suppressed, an error occurs between the angle obtained by integrating the yaw rate and the azimuth angle obtained from the GPS Doppler. Usually, when there is an axis shift, there is a situation where the situation becomes inconsistent (a situation in which a change in azimuth angle does not correspond) only by estimating a certain bias error.

  Therefore, in the present embodiment, the gyro axis inclination determination unit 78 determines the GPS doppler state and then determines the situation where the above-mentioned disagreement is not satisfied, whereby the axis shift of the gyro sensor 14 (the gyro sensor 14 is It is determined whether or not the rotation axis of the yaw rate to be detected is inclined with respect to the direction perpendicular to the horizontal plane. When it is determined that there is an axis deviation, the axis inclination can be estimated from the yaw rate ratio.

  FIG. 9 shows a flow for determining the axis deviation of the gyro sensor 14.

  In general, the positioning accuracy of GPS results from the positioning of satellites and the accuracy of pseudoranges. The estimation accuracy of the velocity vector is also due to the satellite arrangement and the Doppler accuracy. Therefore, when the geometric error (DOP: Division Of Precision) calculated from the satellite arrangement is small, it is considered that the accuracy of the change in the GPS estimated azimuth angle by the GPS Doppler is high.

The GPS HDOP (horizontal plane DOP) used to estimate the velocity vector after the past T seconds is less than the threshold TH _DOPh , and it is determined that the GPS reliability is high. When the variation (variance) of the residual in the optimal estimation calculated in (2) is less than the threshold THσ, it is determined that the Doppler accuracy is high and the axis deviation of the gyro sensor 14 is small. Note that the determination of whether or not the variation (variance) of the residual in the optimal estimation is less than the threshold TH _σ is an example of determining whether or not the gyro angle change corresponds to the GPS estimated azimuth angle change. is there.

When the GPS HDOP (horizontal DOP) is less than the threshold TH _DOPh and the reliability of the GPS is determined to be high, and the variation (variance) of the residual in the optimal estimation is greater than or equal to the threshold TH _σ Is the situation in which yaw rate is occurring at that time (when turning of the vehicle is considered: the average of N times of yaw rate is greater than the threshold value TH _ω ) (see FIG. 8B), the above residual due to the shaft misalignment Therefore, it is determined that there is a possibility that the gyro sensor 14 is misaligned.

Even if the variance (variance) of the residual in the optimal estimation is greater than or equal to the threshold TH _σ , if the yaw rate does not occur (when the average of N yaw rates is less than or equal to the threshold TH _ω ), the error factor is The gyro sensor 14 is determined to be other than the axis shift, and the inclination angle of the gyro sensor 14 is not estimated.

In the past T seconds, when the GPS HDOP average is _{equal to} or greater than the threshold value TH _DOPh , it is determined that the GPS reliability is not high and the accuracy of estimation of the velocity vector by Doppler is not so high. At this time, since the accuracy as the determination information of the tilt angle is insufficient, the tilt angle of the shaft of the gyro sensor 14 described later is not estimated.

Gyro axis angle estimation unit 80, when it is determined that there is axial misalignment of the gyro sensor 14, the detection value (yaw rate) [Delta] H _Gyro and variation of the GPS estimated azimuth calculated from the GPS gyro sensor 14 that bias correction ( Yaw rate) ΔH _gps . Assuming that the axis of the gyro sensor 14 is inclined by θ with respect to the turning center (perpendicular to the horizontal plane), the yaw rate is a value of the original cos θ times. Then, since a relational expression ΔH _gyro = ΔH _gps × cos θ is obtained, the yaw rate ratio is estimated as the inclination angle θ of the axis of the gyro sensor 14 according to the relational expression, and the estimated inclination of the axis of the gyro sensor 14 is obtained. The angle θ is stored in a memory (not shown).

  When it is determined that the gyro sensor 14 has an axis deviation, the roll determination unit 82 determines the axis of the gyro sensor 14 based on the change in the tilt angle θ of the axis of the gyro sensor 14 estimated by the gyro axis angle estimation unit 80. It is determined whether the deviation is due to an attachment error or due to a roll motion of the host vehicle (for example, bank running). For example, when the estimated inclination angle θ of the axis of the gyro sensor 14 is substantially constant (when it falls within a predetermined range including the average value of the inclination angle θ of the axis of the gyro sensor 14), the gyro sensor 14 If the estimated tilt angle θ of the gyro sensor 14 is fluctuating (within a predetermined range including the average value of the tilt angle θ of the gyro sensor 14). If not, it is determined that the axis deviation of the gyro sensor 14 is due to the roll motion of the host vehicle (for example, bank travel).

  Further, when the HDOP calculated by the Doppler reliability determination unit 76 is less than the threshold value, the trajectory estimation unit 30 determines that the GPS reliability is high and acquires the detection value without using the detection value of the gyro sensor 14. The own vehicle trajectory is estimated using the Doppler frequency of each GPS satellite. In this case, the trajectory of the host vehicle is calculated by integrating the speed vector of the host vehicle calculated by the host vehicle speed calculating unit 70 for a predetermined time. The predetermined time is set to an appropriate time for obtaining a desired accuracy. By calculating the speed vector of the host vehicle using the host vehicle speed in the GPS satellite direction that is accurately calculated compared to the pseudorange, and calculating the trajectory of the host vehicle using this, a highly accurate shape and direction can be obtained. The trajectory of the host vehicle can be calculated.

  On the other hand, when the HDOP calculated by the Doppler reliability determination unit 76 is equal to or greater than the threshold value, the trajectory estimation unit 30 determines that the GPS reliability is low, and the gyro sensor 14 bias-corrected by the gyro bias estimation unit 74 is detected. And the detected value of the speed sensor 16 stored in the data storage unit 22 are used to estimate the host vehicle trajectory. In addition, about the estimation method of the own vehicle locus | trajectory, what is necessary is just to use a conventionally well-known method, and abbreviate | omits detailed description.

  Next, the operation of the sensor shaft angle estimation apparatus 10 according to the first embodiment will be described.

  The yaw rate and speed are detected by the gyro sensor 14 and the speed sensor 16, and the gyro sensor shaft angle estimation process shown in FIG. The routine is executed repeatedly.

  In step 100, information on a plurality of GPS satellites is acquired from the GPS receiver 12, and GPS pseudorange data, Doppler frequency, and position coordinates of the GPS satellites are calculated and acquired. GPS information for a plurality of GPS satellites acquired at the same time is acquired as a GPS information group.

  Next, in step 102, each detection value is acquired from the gyro sensor 14 and the speed sensor 16, and the yaw rate and speed are stored in the data storage unit 22.

  Next, in step 104, an azimuth angle change estimation process based on GPS information described later is executed to calculate a change in the GPS estimated azimuth angle for a predetermined time, and then stored in the data storage unit 22 in step 106. Based on the yaw rate for the predetermined time, the angle change of the gyro for the predetermined time is calculated.

  In step 108, optimal estimation of the initial azimuth angle and gyro bias is performed based on the change in GPS estimated azimuth calculated in step 104 and the change in gyro angle calculated in step 106. In step 110, using the gyro bias estimated in step 108, the detection value of the gyro sensor 14 obtained in step 102 is supplemented and stored in a memory (not shown).

  In the next step 112, the variance of the residual is calculated based on the result of the optimal estimation in step 108 and stored in the memory. In step 114, the HDOP of the GPS information group acquired in step 100 is calculated and stored in the memory.

  In step 116, the average value of the residual variance for the past T seconds calculated in step 112, the average value of the HDOP for the past T seconds calculated in step 114, and the value acquired in step 102 are obtained. Based on the average of the yaw rate for the past T seconds, the presence / absence of the axis deviation of the gyro sensor 14 is determined according to the determination flow of FIG.

In step 118, it is determined whether or not the gyro sensor 14 has an axis deviation based on the determination result in step 116. If there is no axis deviation, the process returns to step 100. On the other hand, if there is axial misalignment, in step 120, the corrected detected value of the gyro sensor 14 in step 110 (yaw rate) [Delta] H _Gyro and the variation of the GPS estimated azimuth angle estimated in step 104 (yaw rate ) Based on ΔH _gps , the inclination angle θ of the axis of the gyro sensor 14 is calculated and stored in the memory.

  In step 122, based on the change in the tilt angle θ of the axis of the gyro sensor 14 stored in the memory in step 120, whether the axis deviation of the gyro sensor 14 is caused by an attachment error or the roll motion ( For example, it is determined whether it is due to bank travel), the determination result is output, and the process returns to step 100.

  Next, the azimuth angle change estimation processing routine will be described with reference to FIG.

In step 130, the relative speed vj of the _host vehicle with respect to each GPS satellite is calculated from the Doppler frequency of the received signal from each GPS satellite according to the above equation (1).

Next, in step 132, the velocity vector (VX _j , VY _j , VZ _j ) of each GPS satellite is calculated from the obtained time series data of the position coordinates of each GPS satellite using the differential of Kepler's equation.

  Next, in step 134, the position of the host vehicle is calculated according to the above equations (2) to (4) using the GPS pseudorange data of each GPS satellite. Here, the position of the own vehicle is calculated in order to obtain the direction of the GPS satellite (the angle between the GPS satellite and the own vehicle), and it is sufficient that a rough position can be determined as the position of the own vehicle. The position may be determined from the above information, or the position of the host vehicle may be determined from information such as past position measurement history and beacons.

Next, in step 136, based on the position of the host vehicle calculated in step 134 and the acquired position coordinates of each GPS satellite, the angular relationship R _j (the position of each GPS satellite j and the position of the host vehicle). The elevation angle θ _{j with} respect to the horizontal direction and the azimuth angle φ _{j with} respect to the north direction are calculated as the directions of the respective GPS satellites.

Next, in step 138, the relative speed v _j of the host vehicle with respect to each GPS satellite calculated in step 130, and the velocity vector V _j (VX _j , VY _j , VZ _{j of} each GPS satellite calculated in step 132 above. ) And the direction R _j (θ _j , φ _j ) of each GPS satellite calculated in step 136, the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the above equation (5). To do. The velocity Vs _j of the GPS satellite j in the direction of the vehicle in the equation (5) is calculated by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T.

  Next, in step 140, the optimum value of the speed vector (Vx, Vy, Vz) of the host vehicle is calculated according to the above equations (6) and (7).

  Next, in step 142, the GPS estimated azimuth angle of the host vehicle is calculated from the speed vector of the host vehicle calculated in step 120 and stored in the memory. In step 144, a change in the estimated GPS azimuth is calculated based on the estimated GPS azimuth of the host vehicle stored in the memory, and the process returns.

  As described above, according to the sensor axis inclination determination device according to the first embodiment, HDOP is less than the threshold value, and is calculated using the gyro angle change calculated from the gyro sensor and the GPS information. When the yaw rate is determined to be inconsistent with the change in the estimated GPS azimuth angle, it is determined that the gyro sensor axis is determined to be inclined with respect to the vertical direction with respect to the horizontal plane. This makes it possible to accurately determine the axis shift of the gyro sensor.

  When the axis of the gyro sensor is deviated from the original rotation plane, a value smaller than the actual value is output, so that the turning trajectory tends to swell. The same applies not only to the installation angle but also when the vehicle body itself is tilted by a bank or the like. In this embodiment, it is possible to determine the axis deviation of a gyro sensor mounted on a navigation or a vehicle with a simple configuration, and by estimating the inclination angle of the axis of the gyro sensor, a sensor error due to the axis deviation is detected. Thus, the estimation accuracy of the azimuth change is improved. In addition, the estimation accuracy of the trajectory is improved.

  Further, it is possible to perform highly accurate correction by using a highly accurate GPS Doppler instead of a GPS position having a large error. By using Doppler, it is possible to estimate the speed in the earth coordinate system at that time, so the azimuth angle of the host vehicle can be calculated in one epoch, and more accurate correction is possible.

  When the axis of the gyro sensor is inclined with respect to the axis of the turning center, a value smaller than the actual yaw rate is output. In the estimation of the locus by the existing GPS / INS integration, the gyro sensor is corrected using the GPS positioning result, so the time delay is large and it is difficult to determine the instantaneous posture. In this embodiment, it is possible to estimate the azimuth angle for each epoch by using the GPS Doppler. Therefore, the GPS estimated azimuth angle is directly compared with the gyro estimated angle based on the yaw rate, and the difference between the output results is compared. By observing this, it becomes possible to determine the axis deviation of the gyro sensor. From the difference between the output values of the GPS and the gyro sensor, the inclination angle of the axis of the gyro sensor can also be calculated, and the inclination angle of the axis of the gyro sensor can be corrected during traveling.

  In addition, even if there is an attachment error, it is possible to estimate the inclination angle of the shaft of the gyro sensor, so that the cost for attaching the gyro sensor is reduced. Since it is possible to correct the error due to the axis shift of the gyro sensor, the estimation accuracy of the trajectory is improved.

  Next, a second embodiment will be described. In addition, about the sensor axis angle estimation apparatus of 2nd Embodiment, about the structure same as the sensor axis angle estimation apparatus 10 of 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the second embodiment, it is determined whether the axis deviation of the gyro sensor 14 is due to an attachment error or a roll motion by using the ratio between the change amount of the GPS estimated azimuth angle and the detected value of the gyro sensor. This is mainly different from the first embodiment.

  As shown in FIG. 12, in the computer 218 of the sensor axis angle estimation apparatus 210 according to the second embodiment, the posture estimation unit 228 includes a gyro bias estimation unit 74, a Doppler reliability determination unit 76, and a gyro axis inclination determination unit. 78, a gyro axis angle estimation unit 80, and a roll determination unit 282.

  Here, the principle of determining whether the axis deviation of the gyro sensor 14 is due to roll motion or attachment error will be described.

  Since the axis deviation of the gyro sensor 14 due to an attachment error is always deviated, the inclination angle θ is constant, but the axis deviation of the gyro sensor 14 caused by a traveling environment such as a bank changes because θ varies depending on the situation. By learning the yaw rate variation, it is possible to determine whether the axis deviation of the gyro sensor 14 is due to an attachment error or a roll motion of the vehicle such as bank running.

  Therefore, in the present embodiment, the roll determination unit 282 determines the GPS estimated azimuth angle estimated by the azimuth angle change estimation unit 24 when the gyro axis inclination determination unit 78 determines that the gyro sensor 14 is misaligned. Based on the ratio of the azimuth angle change amount (yaw rate) based on the change and the yaw rate bias-corrected by the gyro bias estimation unit 74, and the ratio of the yaw rate obtained in advance when the host vehicle is not moving in the roll direction Thus, it is determined whether the axis deviation of the gyro sensor 14 is an inclination due to an attachment error or an inclination due to a roll motion of the host vehicle.

As shown in FIG. 13, first, the ratio between the azimuth angle change amount (yaw rate) based on the change in the GPS estimated azimuth angle estimated by the azimuth angle change estimation unit 24 and the yaw rate corrected by the gyro bias estimation unit 74 is Save when is constant, it is determined that the inclination angle of the gyro sensor 14 is constant, the variation omega and _GPS GPS estimated azimuth angle, the ratio of the yaw rate output omega _gyro gyro sensor 14 that is bias correction To do.

  Then, the ratio between the azimuth angle change amount (yaw rate) based on the change in the GPS estimated azimuth angle estimated by the azimuth angle change estimation unit 24 and the yaw rate output of the bias-corrected gyro sensor 14 matches the stored ratio. When it does, it determines with the axial shift | offset | difference of the gyro sensor 14 resulting from an attachment error.

  On the other hand, the ratio of the azimuth angle change amount (yaw rate) based on the change in the GPS estimated azimuth angle estimated by the azimuth angle change estimation unit 24 and the yaw rate output of the bias-corrected gyro sensor 14 matches the stored ratio. If not, it is determined that the axis shift of the gyro sensor 14 is due to the roll motion of the host vehicle.

  In addition, about the other structure and effect | action of the sensor axis angle estimation apparatus 210 which concern on 2nd Embodiment, since it is the same as that of the sensor axis angle estimation apparatus 10 which concerns on 1st Embodiment, description is abbreviate | omitted.

  As described above, according to the sensor shaft angle estimation device according to the second embodiment, it is possible to accurately determine the shaft misalignment of the gyro sensor, and to determine the tilt angle at which the shaft of the gyro sensor is tilted. It can be estimated with high accuracy.

  Next, a third embodiment will be described. In addition, about the sensor axis angle estimation apparatus of 3rd Embodiment, about the structure same as the sensor axis angle estimation apparatus 10 of 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the third embodiment, it is determined whether the axis deviation of the gyro sensor 14 is due to an attachment error or a roll motion based on a change in height direction speed obtained using a GPS Doppler. This is mainly different from the first embodiment.

  As shown in FIG. 14, in the computer 318 of the sensor shaft angle estimation apparatus 310 according to the third embodiment, the posture estimation unit 328 includes a gyro bias estimation unit 74, a Doppler reliability determination unit 76, and a gyro axis inclination determination unit. 78, a gyro axis angle estimation unit 80, and a roll determination unit 382.

  Here, the principle of determining whether the axis deviation of the gyro sensor 14 is due to roll motion or attachment error will be described.

  When the GPS antenna is installed at a position away from the axle, the height of the antenna position changes when the vehicle body is tilted by the bank. The estimation accuracy of the GPS Doppler height direction velocity (see FIG. 8C) is not so high with respect to the horizontal plane, but by finding the height change obtained by the velocity integration, the axis deviation of the gyro sensor 14 is It can be determined that there is a high possibility that it is not due to attachment errors but due to roll motion.

  Therefore, in the present embodiment, the GPS receiver 12 is installed at a position away from the axle of the host vehicle. Further, the Doppler reliability determination unit 76 calculates the GPS information group HDOP (horizontal plane direction DOP) and VDOP (height direction DOP) using the GPS information group acquired at the same time. The azimuth angle change estimation unit 24 further integrates the calculated height component of the velocity vector to calculate the change in the height direction of the host vehicle position. HDOP is an example of the first index, and VDOP is an example of the second index.

As shown in FIG. 8C, the roll determination unit 382 calculates the host vehicle when it is determined that the calculated _{VDOP is} less than the threshold value TH _DOPv and the reliability of the GPS Doppler in the height direction is high. Based on the change in the height direction of the position, it is determined whether the axial deviation of the gyro sensor 14 is an inclination due to an attachment error or an inclination due to a roll motion of the vehicle. For example, when the height direction of the host vehicle position temporarily changes (when it does not fall within a predetermined range including the average height of the host vehicle position), the axis deviation of the gyro sensor 14 is It is determined that the inclination is due to the roll motion. On the other hand, when the height direction of the host vehicle position is substantially constant (when it falls within a predetermined range including the average height of the host vehicle position), the axis deviation of the gyro sensor 14 is caused by an attachment error. Judge that it is.

  In addition, about the other structure and effect | action of the sensor axis angle estimation apparatus 310 which concern on 3rd Embodiment, since it is the same as that of the sensor axis angle estimation apparatus 10 which concerns on 1st Embodiment, description is abbreviate | omitted.

  As described above, according to the sensor shaft angle estimation device according to the third embodiment, the shaft misalignment of the gyro sensor can be accurately determined, and the tilt angle at which the shaft of the gyro sensor is tilted can be determined. It can be estimated with high accuracy.

  Next, a fourth embodiment will be described. In addition, about the structure same as the sensor shaft angle estimation apparatus 10 of 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the fourth embodiment, a case where the present invention is applied to a cooperative ACC (auto cruise control) system that follows a preceding vehicle using communication as shown in FIG. 15 will be described as an example.

  As shown in FIG. 16, the cooperative ACC system 410 includes a preceding vehicle-side vehicle-mounted device 410A and a following vehicle-side vehicle-mounted device 410B.

  The preceding vehicle-side in-vehicle device 410 includes a GPS receiver 12, a gyro sensor 14, a speed sensor 16, a computer 18, and a transmitter 420.

  The transmission unit 420 determines whether the tilt angle of the shaft of the gyro sensor 14 and the shaft misalignment of the gyro sensor 14 are due to an attachment error or a roll motion, along with information such as the vehicle position, speed trajectory, and azimuth angle. The transmission information including is transmitted with respect to the following vehicle side onboard equipment 410B.

  The following vehicle side vehicle-mounted device 410 </ b> B includes a receiving unit 422 and a computer 424.

  The receiving unit 422 receives the transmission information transmitted from the preceding vehicle-side in-vehicle device 410 and outputs it to the computer 424.

  The computer 424 includes a preceding vehicle information determination unit 426 and a vehicle control unit 428. The preceding vehicle information determination unit 426 determines the trajectory included in the transmission information from the preceding vehicle-side vehicle-mounted device 410 based on the inclination angle of the axis of the gyro sensor 14 included in the transmission information transmitted from the preceding vehicle-side vehicle-mounted device 410. Determine the possibility of azimuth error. In addition, the preceding vehicle information determination unit 426 determines the possibility of roll motion (bank travel) based on the determination result included in the transmission information transmitted from the preceding vehicle-side vehicle-mounted device 410.

  The vehicle control unit 428 corrects the trajectory, azimuth, and yaw rate based on the error possibility of the trajectory and azimuth included in the transmission information from the preceding vehicle-side in-vehicle device 410, and corrects the corrected trajectory, azimuth, and yaw rate. Is used to control the vehicle so as to follow the preceding vehicle. In addition, the vehicle control unit 428 performs forward roll prediction based on the possibility of roll motion (bank travel) and performs vehicle control.

  The vehicle control unit 428 uses the state of the preceding vehicle (vehicle speed, position, yaw rate, etc.) acquired from the preceding vehicle by communication to appropriately control the movement of the vehicle, thereby improving energy efficiency and reducing driving load. Realize. At this time, if it is known that the information to be transmitted contains an error in advance, the following vehicle can change the control method by reducing the reliability of the information.

  In the fourth embodiment, the case where the method for estimating the tilt angle of the gyro sensor shaft described in the first embodiment is applied to the cooperative ACC system has been described as an example. It is not limited. The method for estimating the axis inclination angle of the gyro sensor described in the second embodiment and the third embodiment may be applied to the cooperative ACC system.

  Moreover, in said 2nd Embodiment, by storing information, such as a bank based on the determination result of a roll determination part, with positional information in the own vehicle or a center, the vehicle which passes there stores the said bank information. It may be made available.

  In the first to fourth embodiments, the case where the speed vector is calculated has been described as an example. However, the present invention is not limited to this, and the azimuth of the host vehicle is calculated and the speed of the host vehicle is calculated. , And speed information including the azimuth and speed of the host vehicle may be used.

  Moreover, although the said 1st-4th embodiment demonstrated the sensor inclination determination apparatus mounted in a vehicle, the mobile body in which the sensor inclination determination apparatus of this invention is mounted is not limited to a vehicle. For example, a sensor tilt determination device may be mounted on a robot.

  In the first to fourth embodiments, the GPS is determined to have high reliability, and the gyro angle change calculated from the gyro sensor and the GPS calculated using the GPS information are used. When it is determined that the change in estimated azimuth is not consistent, and yaw rate is occurring, the gyro sensor axis is determined to be inclined with respect to the vertical direction with respect to the horizontal plane. As described above, the present invention is not limited to this. The case where the axis of the yaw rate sensor is determined to be inclined with respect to the vertical direction with respect to the horizontal plane is limited to the case where it is determined that the reliability of the GPS is high. Absent. For example, when it is determined that the change in the gyro angle calculated from the gyro sensor and the change in the GPS estimated azimuth angle calculated using the GPS information do not match, and the yaw rate is occurring, the gyro It may be determined that the axis of the sensor is inclined with respect to the direction perpendicular to the horizontal plane.

10, 210, 310 Sensor axis angle estimation device 12 Receiver 14 Gyro sensor 16 Speed sensor 18, 218, 318 Computer 20 Information acquisition unit 22 Data storage unit 24 Azimuth angle change estimation unit 26 Angle change acquisition unit 28, 228, 328 Attitude Estimation unit 74 Gyro bias estimation unit 76 Doppler reliability determination unit 78 Gyro axis tilt determination unit 80 Gyro axis angle estimation units 82, 282, 382 Roll determination unit

Claims (11)

A yaw rate sensor mounted on the moving body and detecting a yaw rate generated in the moving body;
Information on the position of each of the GPS satellites transmitted from each of a plurality of GPS satellites, information on the distance between each of the GPS satellites and the mobile object, and information on the relative speed of the mobile object with respect to each of the GPS satellites Satellite information acquisition means for acquiring satellite information including:
First calculating means for calculating a change in the angle of the moving body based on the yaw rate detected by the yaw rate sensor;
Second calculation means for calculating a change in the azimuth angle of the reference direction of the mobile body based on the satellite information acquired by the satellite information acquisition means;
Angle change determination means for determining whether or not the change in angle calculated by the first calculation means corresponds to the change in azimuth angle calculated by the second calculation means;
Yaw rate determination means for determining whether or not yaw rate is generated in the moving body based on the yaw rate detected by the yaw rate sensor;
When the angle change determining means determines that the change in angle does not correspond and the yaw rate determining means determines that yaw rate is occurring, the axis of the yaw rate sensor is perpendicular to the horizontal plane. Inclination determination means for determining that the vehicle is inclined with respect to
A sensor inclination determination device including: Bias correction means for performing bias correction of the yaw rate detected by the yaw rate sensor based on the change in the angle calculated by the first calculation means and the change in the azimuth angle calculated by the second calculation means. When,
Based on the yaw rate bias-corrected by the bias correcting means and the change amount of the change in the azimuth angle calculated by the second calculating means, the axis of the yaw rate sensor is inclined with respect to the direction perpendicular to the horizontal plane. An inclination angle estimating means for estimating an inclination angle of
The sensor inclination determination apparatus according to claim 1, further comprising:   Based on the change in the inclination angle estimated by the inclination angle estimation means, it is determined whether the inclination of the shaft of the yaw rate sensor is an inclination due to an attachment error or an inclination due to a movement of the moving body in the roll direction. The sensor inclination determination apparatus according to claim 2, further comprising a roll determination unit that performs the operation. Bias correction means for performing bias correction of the yaw rate detected by the yaw rate sensor based on the change in the angle calculated by the first calculation means and the change in the azimuth angle calculated by the second calculation means. When,
The ratio of the yaw rate based on the change in the angle calculated by the first calculating means and the yaw rate bias-corrected by the bias correcting means, and the previously obtained when the moving body is not moving in the roll direction. And a roll determining unit that determines whether an inclination of an axis of the yaw rate sensor is an inclination due to an attachment error or an inclination due to a movement of the movable body in a roll direction based on a yaw rate ratio. The sensor inclination determination apparatus according to 1 or 2. A reliability calculation means for calculating an index indicating the reliability of GPS obtained from satellite information of each of the plurality of GPS satellites;
The inclination determination means is a case where the reliability of the GPS is determined to be high based on the index calculated by the reliability calculation means, and the change in angle corresponds to the angle change determination means. When it is determined that the yaw rate is generated by the yaw rate determination means, it is determined that the axis of the yaw rate sensor is inclined with respect to a direction perpendicular to the horizontal plane. The sensor inclination determination apparatus according to claim 4. Based on the satellite information acquired by the satellite information acquisition means, a height change calculation means for calculating a change in the height direction of the moving body;
Reliability calculation means for calculating a first index indicating the reliability of GPS in the horizontal direction and a second index indicating the reliability of GPS in the height direction, obtained from satellite information of each of the plurality of GPS satellites; In addition,
The inclination determining means is a case where it is determined that the reliability of GPS in the horizontal plane direction is high based on the first index calculated by the reliability calculating means, and the angle change determining means determines the angle of the angle. When it is determined that the change does not correspond and the yaw rate determination unit determines that yaw rate is occurring, it is determined that the axis of the yaw rate sensor is inclined with respect to the vertical direction of the horizontal plane. And
The height of the moving object calculated by the height change calculating means when it is determined that the reliability of the GPS in the height direction is high based on the second index calculated by the reliability calculating means. The roll determination means which determines whether the inclination | tilt of the axis | shaft of the said yaw rate sensor is an inclination by an attachment error based on the change of a direction, or an inclination by the motion of the roll direction of the said mobile body. Or the sensor inclination determination apparatus of 2 description.   The index indicating the reliability of the GPS is obtained from the satellite information of each of the plurality of GPS satellites received by the GPS receiver at the same time, and corresponds to the arrangement of the plurality of GPS satellites that transmitted each of the plurality of satellite information. The sensor inclination determination apparatus according to claim 5 or 6, wherein the DOP represents a rate of decrease in positioning accuracy. Based on the satellite information acquired by the satellite information acquisition means, a speed calculation means for calculating a speed vector including the traveling direction of the moving body, or speed information indicating the speed of the moving body and the traveling direction of the moving body. Further including
The second calculating means calculates a change in the azimuth angle of the moving body in the reference direction by estimating the azimuth angle of the moving body in the reference direction based on the speed information calculated by the speed calculating means. The sensor inclination determination apparatus according to any one of claims 1 to 7.   The speed calculation means is based on the position of the mobile body obtained from information on the position of each of the GPS satellites and information on the distance between each of the GPS satellites and the mobile body. The direction of each GPS satellite is calculated, the speed of each GPS satellite is calculated based on the time-series information on the position of each GPS satellite, and the direction of each GPS satellite as viewed from the mobile object , Based on information on the speed of each of the GPS satellites and the relative speed of the mobile body with respect to each of the GPS satellites, the speed of each direction of each of the GPS satellites of the mobile body is calculated, The sensor inclination determination apparatus according to claim 8, wherein a velocity vector of the moving body is calculated based on a velocity in each direction of the GPS satellite.   Information on the position of each of the GPS satellites is satellite orbit information, information on the distance between each of the GPS satellites and the moving body is pseudo-range information, and the relative speed of the moving body with respect to each of the GPS satellites The sensor inclination determination apparatus according to claim 1, wherein the information is Doppler frequency information. Computer
Information on the position of each of the GPS satellites transmitted from each of a plurality of GPS satellites, information on the distance between each of the GPS satellites and the moving body, and information on the relative speed of the moving body with respect to each of the GPS satellites Satellite information acquisition means for acquiring satellite information including,
First calculating means for calculating a change in angle of the moving body based on a yaw rate detected by a yaw rate sensor mounted on the moving body and detecting a yaw rate generated in the moving body;
Second calculation means for calculating a change in the azimuth angle of the reference direction of the mobile body based on the satellite information acquired by the satellite information acquisition means;
Angle change determination means for determining whether or not the change in angle calculated by the first calculation means corresponds to the change in azimuth angle calculated by the second calculation means;
Based on the yaw rate detected by the yaw rate sensor, the yaw rate determining means for determining whether or not yaw rate is generated in the moving body, and the angle change determining means determines that the change in angle does not correspond. When the yaw rate is determined to be generated by the yaw rate determining means, the yaw rate sensor is configured to function as an inclination determining means for determining that the axis of the yaw rate sensor is inclined with respect to a direction perpendicular to the horizontal plane. program.