JP4831441B2 - Correction coefficient calculation device and calculation program for direction sensor - Google Patents

Description

  The present invention relates to a correction coefficient calculation device and calculation program for calculating a correction coefficient for correcting output information from a direction sensor using a signal from a GPS satellite, and a self-position recognition device and a recognition program.

Regarding a technique for correcting output information of an orientation sensor such as a gyroscope, for example, the following Patent Document 1 discloses a technique related to a vehicle current position detection device as described below. This vehicle current position detecting device includes a gyro that outputs a signal corresponding to the rotational angular velocity of the vehicle, a distance sensor that outputs a pulse signal corresponding to the moving distance of the vehicle, and a transmission radio wave from a GPS (Global Positioning System) satellite. A GPS receiver that receives and detects the position, heading (traveling direction), speed, etc. of the vehicle, and performs dead reckoning navigation based on the output from the gyro, distance sensor, GPS receiver, etc. A current position detecting unit for detecting data for the purpose.

  The current position detecting unit includes a moving distance calculating unit that calculates a moving distance of the vehicle based on a pulse signal from a distance sensor, and an azimuth changing amount calculating unit that calculates an azimuth changing amount based on a detection signal from the gyro. And a relative trajectory calculation unit that calculates a relative trajectory and a vehicle speed based on the calculated azimuth change amount and moving distance, and an absolute azimuth and absolute position that are also calculated based on the azimuth change amount and moving distance. Various calculation parameters and calculation results used to calculate the vehicle speed, heading, and position, using the difference between the calculated value in the position calculation unit, relative trajectory calculation unit and absolute position calculation unit and the detected value in the GPS receiver as an observation value An error estimator composed of a Kalman filter that obtains an estimated value of the state quantity and an estimated value of the state quantity (that is, error) calculated by the error estimator, And a correcting unit for correcting the parameters and calculated values. Here, as one of the state quantities, a conversion gain error (gain error) from gyro output to angular velocity is estimated. Then, the azimuth change amount calculated using the gyro output and the conversion gain is corrected based on the gain error estimated value obtained by the error estimation unit.

  In addition to the above-described configuration, the second embodiment of Patent Document 1 described below further includes a posture inclination angle calculation unit that calculates the inclination angle of the vehicle posture based on an output from an acceleration sensor or the like. Then, the posture variation correction determined from the gain correction amount calculated based on the calculation result of the Kalman filter as described above and the front and rear inclination angles and the lateral inclination angles of the vehicle attitude obtained by the attitude inclination angle calculation unit. A configuration for adjusting the gain correction amount using the amount is described.

  This vehicle current position detection device complicates the device configuration even when the gyro conversion gain includes a gain error due to changes in the gyro, the installation angle, and the vehicle attitude. The purpose is to make it possible to stably and accurately determine the amount of azimuth change from the output of the gyro.

Japanese Unexamined Patent Publication No. 2000-55678 (pages 4-5, 9-10, and 8-9)

  In the technique described in Patent Document 1, the process of calculating the error is repeated every time information is obtained from the GPS receiver, regardless of whether or not the direction of the vehicle is changed. There is an advantage that can be corrected at any time. However, with the Kalman filter, the difference between the calculated value in the relative trajectory calculating unit and the absolute position calculating unit and the detected value in the GPS receiver is used as an observed value, and the gyro gain error is estimated as one of the state quantities. Therefore, there is a problem that a high processing load is required due to a heavy calculation load on the calculation processing device.

  On the other hand, the difference between the output from the gyro and the detected value at the GPS receiver becomes significant when the orientation is changed at a relatively large angle. Therefore, before and after such azimuth change, the azimuth change calculated based on the information from the gyro and the azimuth change calculated based on the information from the GPS receiver are compared to calculate the error, If the method for correcting the output information of the gyro is used, it is possible to reduce the calculation load of the calculation processing device. By the way, also in this method, it is desirable to perform correction according to the installation angle of the gyro and the inclination angle of the vehicle posture. However, when correcting the gyro output information based on the information from the gyro and GPS receiver before and after the azimuth change, the gyro is appropriately adjusted according to the change in the gyro installation angle and the dynamic inclination angle of the vehicle attitude. No technology has been established for correcting the output information error.

  The present invention has been made in view of the above problems, and its purpose is to calculate a correction coefficient for correcting azimuth sensor information based on GPS information and azimuth sensor information before and after the azimuth change. , An orientation sensor correction coefficient calculation device capable of appropriately calculating a dynamic correction coefficient in consideration of a static installation angle of the orientation sensor and a dynamic change of the installation angle, a program thereof, and a program for using the same An object of the present invention is to provide a self-recognition apparatus and its program.

In order to achieve the above object, the characteristic configuration of the correction coefficient calculation device of the azimuth sensor according to the present invention includes GPS information acquisition means for acquiring GPS information including position information and traveling azimuth information based on a signal from a GPS satellite; and orientation sensor information acquisition means for acquiring azimuth sensor information from the azimuth sensor, and a GPS orientation change amount calculation means for calculating an amount of change proceeds whereabouts position based on the GPS information, it proceeds whereabouts position based on the azimuth sensor information a sensor orientation change amount calculation means for calculating a change amount of the the previous SL GPS orientation change amount based on the difference between the sensor orientation change amount, and the basic correction coefficient calculating means for calculating a basic correction coefficient of the azimuth sensor information A static correction coefficient calculating means for calculating a static correction coefficient obtained by correcting the basic correction coefficient in accordance with a static installation angle of a detection axis of the direction sensor; and the direction sensor Dynamic change detection means for detecting a dynamic change in the detection axis direction, and a dynamic correction coefficient is calculated by dynamically correcting the static correction coefficient in accordance with the dynamic change in the detection axis direction of the azimuth sensor. Dynamic correction coefficient calculating means, azimuth change ratio calculating means for calculating an azimuth change ratio between the GPS azimuth change amount and the sensor azimuth change amount, and the azimuth change ratio as a calculation result of the azimuth change ratio calculation means Storage means for storing the information, neighborhood area setting means for setting a neighborhood area near the own position, and a plurality of rear areas having different distances from the neighborhood area behind the neighborhood area in the traveling direction comprising a rear region setting means, wherein the orientation change ratio calculation means, for a plurality of said rear region, said heading change ratio calculated respectively, information for each of the azimuth rate of change between the neighboring region Is stored in the storage means, the basic correction coefficient calculating means is the basic correction factor based in whole or in part of the average value of a plurality of direction change ratio stored in the storage means to the point you operation.

According to this characteristic structure, calculates a basic correction factor based on the difference between G PS orientation change amount and the sensor orientation change amount, since the calculation of the static correction coefficient and dynamic correction factor based on the basic correction factor Thus, it is possible to avoid unnecessary calculation processing and to reduce the calculation load of the apparatus. In addition, the basic correction coefficient is corrected according to the static installation angle of the detection axis of the azimuth sensor to calculate the static correction coefficient, and further, the direction of the detection axis of the azimuth sensor due to the ground gradient, the swing of the installation position, etc. Because the dynamic correction coefficient is calculated by dynamically correcting the static correction coefficient according to the dynamic change of the direction sensor, the static installation angle of the azimuth sensor and the dynamic change of the installation angle are taken into account. The correction coefficient can be calculated appropriately, and the accuracy of the correction coefficient of the azimuth sensor can be improved. Moreover, if comprised as mentioned above, it will use the several azimuth | direction change ratio memorize | stored in the memory | storage means, the influence by the specific information in the calculation result of azimuth | direction change ratio will be reduced, and an average azimuth | direction change Since the correction coefficient of the azimuth sensor can be calculated based on the ratio, the accuracy of the basic correction coefficient can be improved, and further, the accuracy of the static correction coefficient and the dynamic correction coefficient calculated based on the basic correction coefficient can be increased. It becomes possible.

Further, the characteristic configuration of the correction coefficient calculation program of the direction sensor according to the present invention for achieving the above object is a GPS information acquisition step of acquiring GPS information including position information and traveling direction information based on a signal from a GPS satellite. a GPS orientation change amount calculation step of calculating an amount of change proceeds whereabouts position based on the GPS information from a sensor orientation change amount calculation step of calculating an amount of change proceeds whereabouts position based on the azimuth sensor information from the direction sensor the before and Symbol GPS orientation change amount based on the difference between the sensor orientation change amount, and the basic correction coefficient calculation step of calculating the basic correction coefficient of the azimuth sensor information, static installation angle of the detection axis of the azimuth sensor A static correction coefficient calculation step for calculating a static correction coefficient obtained by correcting the basic correction coefficient in accordance with a dynamic change in the detection axis direction of the azimuth sensor. A dynamic change detection step to be issued; and a dynamic correction coefficient calculation step for dynamically correcting the static correction coefficient and calculating a dynamic correction coefficient in accordance with a dynamic change in the detection axis direction of the azimuth sensor; A azimuth change ratio calculation step for calculating a ratio between the GPS azimuth change amount and the sensor azimuth change amount, and a memory for storing information on the azimuth change ratio as a calculation result of the azimuth change ratio calculation step in a predetermined storage unit A neighborhood region setting step for setting a neighborhood region in the vicinity of the own position, a rear region setting step for setting a plurality of rear regions having different distances from the neighborhood region behind the neighborhood region in the traveling direction, the cause the computer to execute, in the direction change ratio calculating step for a plurality of said back region, and calculates the azimuth rate of change between the neighboring regions, respectively, Information on each of the azimuth change ratios is stored in the storage means, and in the basic correction coefficient calculation step, the plurality of azimuth change ratios stored in the storage means are based on an average value of all or part of the azimuth change ratios. It lies in you calculating the basic correction factor.

Here, it is preferable that the basic correction coefficient calculating unit is configured to calculate the basic correction coefficient based on the latest partial average value among a plurality of azimuth change ratios stored in the storage unit.

  In the basic correction coefficient calculating step, the basic correction coefficient calculating program calculates the basic correction coefficient based on the latest average value of a plurality of azimuth change ratios stored in the storage means. It is preferable to have a configuration for calculating.

  The apparatus further comprises a plurality of azimuth change determination means for determining whether or not there have been a plurality of azimuth changes in different directions between the neighboring area and the rear area based on the azimuth sensor information. When it is determined by the azimuth change determination means that the azimuth change has been made a plurality of times, the basic correction coefficient calculation means is configured to determine whether the azimuth change has been made a plurality of azimuth changes. It is preferable that the information on the azimuth change ratio is excluded from the calculation target of the basic correction coefficient.

  The correction coefficient calculation program for the azimuth sensor determines a plurality of azimuths based on the azimuth sensor information to determine whether or not there have been a plurality of azimuth changes in different directions between the neighboring area and the rear area. A change determination step is further executed, and when it is determined that there have been multiple azimuth changes in the multiple azimuth change determination step, it is determined in the basic correction coefficient calculation step that there have been multiple azimuth changes. It is preferable that information on the azimuth change ratio between the neighborhood area and the rear area is excluded from the calculation target of the basic correction coefficient.

Further , the static correction coefficient calculation means, when the static installation angle of the detection axis of the azimuth sensor is α ° with respect to the vertical direction, the static correction coefficient,
(The static correction coefficient) = (the basic correction coefficient) × cos α
The dynamic correction coefficient calculation means is based on the direction inclined by the installation angle α ° with respect to the vertical direction, and when the dynamic change angle in the detection axis direction of the azimuth sensor is β °, The dynamic correction factor,
(Dynamic correction coefficient) = (static correction coefficient) / cos α × cos (α + β)
It is preferable to calculate as follows.

  If comprised in this way, the static correction coefficient and the dynamic correction coefficient which considered the inclination of the static installation angle of an azimuth | direction sensor and the dynamic change of the installation angle can be calculated appropriately.

The azimuth sensor correction coefficient calculation program may include the static correction coefficient calculation step when the static installation angle of the detection axis of the azimuth sensor is α ° with respect to the vertical direction. Correction factor
(The static correction coefficient) = (the basic correction coefficient) × cos α
Operate as
In the dynamic correction coefficient calculating step, when the dynamic change angle in the detection axis direction of the azimuth sensor is β ° with reference to the direction inclined by the installation angle α ° with respect to the vertical direction, the dynamic correction is performed. Coefficient
(Dynamic correction coefficient) = (static correction coefficient) / cos α × cos (α + β)
It is preferable to calculate as follows.

  Further, it is determined whether or not a predetermined dynamic correction execution condition including a condition relating to a dynamic change in the detection axis direction of the direction sensor is satisfied, and when the dynamic correction execution condition is not satisfied, the dynamic correction is performed. It is preferable to omit the calculation of the dynamic correction coefficient by the coefficient calculation means.

  With this configuration, the calculation of the dynamic correction coefficient is omitted when correction of the direction sensor information by the dynamic correction coefficient is unnecessary, such as when there is no dynamic change in the detection axis direction of the direction sensor. Therefore, it is possible to avoid unnecessary calculation processing and further reduce the calculation load of the apparatus.

  Further, the correction coefficient calculation program of the azimuth sensor determines whether or not a predetermined dynamic correction execution condition including a condition relating to a dynamic change in the detection axis direction of the azimuth sensor is satisfied, and the dynamic correction execution condition is set as the dynamic correction execution condition. If not, it is preferable to omit the dynamic correction coefficient calculation step.

  A characteristic configuration of the self position recognizing apparatus according to the present invention is a self position recognizing apparatus provided with the correction coefficient calculating device of the azimuth sensor configured as described above, wherein the static correction coefficient and the dynamic correction coefficient Direction sensor information correction means for correcting the direction sensor information using at least one, distance sensor information acquisition means for acquiring distance sensor information from a distance sensor for detecting a moving distance, corrected direction sensor information, and the Self-position calculating means for calculating the movement locus of the self-position based on the distance sensor information and recognizing the self-position.

  According to this characteristic configuration, it is possible to recognize the own position based on the azimuth sensor information and the distance sensor information after correction by at least one of the static correction coefficient and the dynamic correction coefficient. Therefore, when performing self-position recognition by autonomous navigation, high-precision self-position recognition is possible.

  Further, a characteristic configuration of the self-position recognition program according to the present invention is a self-position recognition program including a correction coefficient calculation program for the azimuth sensor configured as described above, wherein the static correction coefficient and the dynamic correction are provided. A direction sensor information correction step for correcting the direction sensor information using at least one of coefficients, a distance sensor information acquisition step for acquiring distance sensor information from a distance sensor for detecting a moving distance, and the corrected direction sensor information And a local position calculating step for calculating the movement locus of the own position based on the distance sensor information from the distance sensor for detecting the movement distance and recognizing the own position.

The block diagram which shows schematic structure of the own vehicle position recognition apparatus which concerns on embodiment of this invention. Schematic diagram showing an example of the relationship between GPS information and gyro information before and after the azimuth change Schematic diagram showing an example of gyro information when there are multiple heading changes The figure which shows the relationship between the output of the gyro sensor and the installation angle The figure which shows the relationship between the output of a gyro sensor, an installation angle, and a dynamic change angle The flowchart which shows the calculation process of the correction coefficient which concerns on embodiment of this invention The flowchart which shows the calculation process of the correction coefficient which concerns on embodiment of this invention The flowchart which shows the recognition process of the own vehicle position which concerns on embodiment of this invention Histogram for calculating a basic correction coefficient according to another embodiment of the present invention

  Embodiments of the present invention will be described below with reference to the drawings. In the present embodiment, a description will be given by taking as an example a vehicle position recognition device 2 for a vehicle provided with a correction coefficient calculation device 1 for a direction sensor according to the present invention. FIG. 1 is a block diagram illustrating a schematic configuration of a vehicle position recognition device 2 according to the present embodiment.

The own vehicle position recognition device 2 according to the present embodiment is acquired from a gyro sensor 4 as an orientation sensor based on GPS information acquired by receiving a signal from a GPS (Global Positioning System) satellite by a GPS receiver 3. Based on the corrected gyro information and the pulse number information acquired from the distance sensor 5, the movement locus of the own vehicle position is calculated, and the current own vehicle position is recognized.
Hereinafter, based on FIG. 1, the structure of each part of the own vehicle position recognition apparatus 2 is demonstrated in detail. In addition, each function part and means of the own vehicle position recognition device 2 is a hardware or software (function or unit) for performing various processes on input data, with an arithmetic processing unit such as a CPU as a core member. Program) or both.

  The GPS receiver 3 receives a signal from a GPS satellite. Signals from this GPS satellite are received every second and sent to the GPS information acquisition unit 6. The GPS information acquisition unit 6 analyzes the received signal from the GPS satellite, information on the position (latitude and longitude) of the vehicle, the latest trip number information (hereinafter referred to as “GPS trip number information”) at the time of signal reception, Information such as traveling direction information, moving speed information, positioning satellite number information, error radius information, and height direction speed information is acquired, and GPS information including these pieces of information is generated. This GPS information is generated periodically every time a signal from a GPS satellite is received, that is, every second, and stored in the GPS information storage unit 7. Here, the GPS information acquisition unit 6 constitutes “GPS information acquisition means” in the present invention.

  In the vehicle position recognition device 2 according to the present embodiment, the minimum unit of the movement distance in the vehicle position recognition device 2 determined based on the movement distance of the vehicle position detected by the distance sensor 5 described later, that is, the unit. The distance is 1 trip. In this example, three pulses of the distance sensor 5 output signal are defined as one trip, and one trip is defined as about 1.2 [m]. The GPS trip number information is information indicating the latest number of trips at the time of receiving a signal from a GPS satellite that is a generation source of the GPS information.

  The gyro sensor 4 is a direction sensor that detects a change in the direction of the vehicle. As the gyro sensor 4, various types of gyroscopes such as a vibration gyroscope and an optical fiber gyroscope can be used. In this example, a vibration gyroscope is used. The gyro sensor 4 generates an output such as a voltage proportional to the rotational angular velocity around the predetermined detection axis X (see FIG. 4). The output from the gyro sensor 4 is sent to the gyro information acquisition unit 8. The gyro information acquisition unit 8 performs analog / digital conversion for converting an output of an analog signal such as a voltage from the gyro sensor 4 into a digital signal, and generates a gyro output value composed of digital signals of a plurality of stages (for example, 256 stages). . Then, based on the generated gyro output value and the resolution per unit output value, gyro information representing the azimuth change amount is generated. Here, the gyro information is generated in the gyro information acquisition unit 8 for each trip with the movement of the vehicle position, and is stored in the gyro information storage unit 9. And the advancing direction of the own vehicle can be calculated | required by integrating | accumulating this gyro information. Here, the gyro sensor 4 corresponds to the “direction sensor” in the present invention, the gyro information corresponds to the “direction sensor information” in the present invention, and the gyro information acquisition unit 8 performs the “direction sensor information acquisition unit” in the present invention. Constitute.

  The distance sensor 5 is a sensor that detects the moving distance of the vehicle position. In this example, a vehicle speed pulse sensor is used as the distance sensor 5. The distance sensor 5 outputs a pulse signal every time a drive shaft or wheel of the vehicle rotates by a certain amount. The output from the distance sensor 5 is sent to the pulse number information acquisition unit 10. The pulse number information acquisition unit 10 integrates the number of pulses of the pulse signal output from the distance sensor 5 to generate pulse number information. Here, the pulse number information is generated by the pulse number information acquisition unit 10 for each trip (three pulses in this example) with the movement of the vehicle position, and is stored in the pulse number information storage unit 11. The movement distance is calculated by multiplying the difference in pulse number information between any two points stored in the pulse number information storage unit 11 by the standard distance per pulse (0.4 [m] in this example). be able to. By combining the travel distance information calculated from the pulse number information and the traveling direction information calculated from the gyro information, it is possible to recognize the position of the vehicle by autonomous navigation. Here, the pulse number information corresponds to “distance sensor information” in the present invention, and the pulse number information acquisition unit 10 constitutes “distance sensor information acquisition means” in the present invention.

  The gradient sensor 41 is a sensor for detecting a tilt angle in the front-rear direction of the host vehicle, that is, a gradient angle. In this example, an acceleration sensor or the like is used as the gradient sensor 41. The gradient information acquisition unit 42 acquires gradient angle information representing the inclination angle in the front-rear direction of the host vehicle based on the output from the gradient sensor 41. In this example, as shown in FIG. 4, the gyro sensor 4 is installed in the host vehicle in a state inclined α [°] with respect to the vertical direction V, and its detection axis X is orthogonal to the bottom surface of the gyro sensor 4. It is fixed in the direction to do. Therefore, a dynamic change in the detection axis X direction of the gyro sensor 4 can be detected based on a change in the inclination angle (β in FIG. 5) of the vehicle in the front-rear direction indicated by the gradient angle information. Therefore, here, the gradient sensor 41 and the gradient information acquisition unit 42 constitute “dynamic change detection means” in the present invention.

The region setting unit 12 includes a neighborhood region setting unit 13 and a rear region setting unit 14. The neighborhood area setting means 13 is a means for setting a predetermined neighborhood area arA in the vicinity of the vehicle position. Further, the rear area setting means 14 is a means for setting the rear areas arB, arC,... Separated by a predetermined distance rearward in the traveling direction with respect to the neighboring area arA. The neighboring area arA and the rear areas arB, arC... Will be described with reference to FIG.
FIG. 2 is a schematic diagram illustrating an example of a relationship between GPS information and gyro information before and after a change in direction (right turn). In this figure, the GPS locus Lg represents the movement locus of the own vehicle position based on the GPS information, and the sensor locus Ls represents the movement locus of the own vehicle position calculated based on the pulse number information and the gyro information. On the GPS locus Lg, GPS information A1 to A3, B1 to B3, and the like used for calculation are represented by circles, and the traveling direction indicated by each GPS information is represented by an arrow. On the sensor locus Ls, the position of a predetermined point used for calculation is represented by a circle. Here, each point on the sensor locus Ls is defined based on the number of trips, and is associated with each GPS information having GPS trip number information based on the number of trips.

As shown in FIG. 2, in this example, the neighborhood area arA selects the GPS information A1 to A3 from the current point Pg0, which is the current number of trips, to the previous three points, and then sets the current point Pg0 as the front end. A point Pg1 retroactive to the rear of the 10-trip traveling direction with respect to the number of trips indicated in the GPS trip number information of the GPS information A3 is set as a rear end region. Here, the names of the GPS information are A1, A2, and A3 in order from the front side in the traveling direction. The points on the sensor locus Ls corresponding to the GPS information A1, A2, and A3 are Pa1, Pa2, and Pa3, respectively.
Further, in this example, the rear area arB selects the GPS information B1 to B3 from the current point Pg0 to the previous three points as a reference point, and points to the GPS trip B1 of the GPS information B1. The point Pg2 that has advanced forward in the 10-trip traveling direction with respect to the number of trips indicated in the GPS trip number information is used as the front end, and the number of trips indicated in the GPS trip number information in the GPS information B3 is traced backward in the 10-trip traveling direction. It is set in a region having the point Pg3 as the rear end. Here, the names of the GPS information are B1, B2, and B3 in order from the front side in the traveling direction. The points on the sensor locus Ls corresponding to the GPS information B1, B2, and B3 are Pb1, Pb2, and Pb3, respectively.
Further, the rear area setting means 14 performs processing for sequentially setting a plurality of areas having different distances from the neighboring area arA as the rear areas arB, arC. Therefore, also in the rear area arC and later, for example, 150 points, 200 trips, 250 trips, 300 trips, 350 trips, and 400 trips in the rearward direction from the current point Pg0 are used as reference points in order, and the rear area Set up to arC, arD ... arH. In the following description, the case where the rear area arB is set will be basically described. However, the same processing is performed even when other rear areas arC, arD... ArH are set. Is called.

As shown in FIG. 1, the GPS condition determination unit 15 includes a GPS straight travel determination unit 16, a speed determination unit 17, a reception state determination unit 18, and a GPS direction change determination unit 19.
The GPS straight travel determination means 16 is a means for determining whether or not the vehicle position has traveled straight within the vicinity area arA based on GPS information. In this example, the GPS straight traveling determination means 16 is based on the traveling azimuth information included in the GPS information A1, A2, A3, and the azimuth change amount between each of the GPS information A1 and A2, A2 and A3, and A1 and A3. It is determined whether or not the absolute value of is less than 3 [°]. Here, the GPS straight travel determination means 16 constitutes the “first straight travel determination means” in the present invention.

The speed determination means 17 is a means for determining whether or not the host vehicle is traveling at a predetermined speed or higher based on the GPS information. In this example, based on the movement speed information included in the GPS information A1, A2, and A3, it is determined whether or not the speed of all the GPS information A1, A2, and A3 is 5 [m / s] or more. .
The reception state determination unit 18 is a unit that determines whether the reception state of the signal from the GPS satellite when the GPS information is acquired is good or bad. In this example, based on the positioning satellite number information included in the GPS information A1, A2, and A3, it is determined whether or not the number of positioning satellites is four in all of the GPS information A1, A2, and A3.
The GPS azimuth change determination means 19 changes the azimuth between the vicinity area arA and the rear area arB based on the GPS azimuth change ang-GPS (see FIG. 2) calculated by the GPS azimuth change calculation means 22 described later. It is means for determining whether or not there has been. In this example, it is determined whether or not the absolute value of the GPS azimuth change amount ang-GPS is 60 ° to 120 ° (60 ° ≦ | ang−GPS | ≦ 120 °).

The GPS information calculation unit 20 includes a GPS direction calculation means 21 and a GPS direction change amount calculation means 22.
The GPS azimuth calculating means 21 is a means for calculating a traveling azimuth in the vicinity area arA and a traveling azimuth in the rear area arB based on GPS information. In this example, as shown in FIG. 2, the GPS azimuth calculating means 21 is based on the traveling azimuth information included in the GPS information A1, A2, A3, and the average value of the traveling azimuth included in the GPS information A1, A2, A3. Is set as the GPS azimuth ang-A in the vicinity area arA. Further, the GPS azimuth calculating means 21 calculates the average value of the traveling azimuth included in the GPS information B1, B2, B3 based on the traveling azimuth information included in the GPS information B1, B2, B3, and calculates the average value of the rear area arB GPS direction ang-B in the inside.
The GPS azimuth change calculation means 22 calculates the difference between the GPS azimuth ang-A in the vicinity area arA and the GPS azimuth ang-B in the rear area arB calculated by the GPS azimuth calculation means 21, and the GPS azimuth change ang− Calculate as GPS. Here, the value of the GPS azimuth change ang-GPS is positive when the azimuth is changed to the right, and the value of the GPS azimuth change ang-GPS is negative when the azimuth is changed to the left. is doing. Of course, it is also possible to reverse the left-right and positive-negative relationship.

The sensor condition determination unit 23 includes a straight sensor determination unit 24, a sensor orientation change determination unit 25, and a plurality of orientation change determination units 26.
The straight sensor determination unit 24 is a unit that determines whether or not the vehicle position has traveled straight in both the vicinity area arA and the rear area arB based on the gyro information. In this example, as shown in FIG. 2, the sensor straight-advancing determining means 24 has an absolute value of the azimuth change amount between the current point Pg0 and the point Pg1, which is a point at both ends of the vicinity area arA, of 3 [°] or less. And the absolute value of the azimuth change amount between the points Pg2 and Pg3, which are both ends of the rear region arB, is determined to be 3 [°] or less. Here, the sensor straight travel determination means 24 constitutes the “second straight travel determination means” in the present invention.
The sensor azimuth change determination means 25 changes the azimuth between the vicinity area arA and the rear area arB based on a sensor azimuth change amount ang-SEN (see FIG. 2) calculated by a sensor azimuth change amount calculation means 29 described later. It is means for determining whether or not there has been. In this example, it is determined whether or not the absolute value of the sensor orientation change amount ang-SEN is 30 ° or more (30 ° ≦ | ang−SEN |).

  The multiple azimuth change determination means 26 is a means for determining whether or not there have been multiple azimuth changes in different directions between the vicinity area arA and the rear area arB based on the gyro information. In this example, the plurality of azimuth change determination means 26 are used when the azimuth changes in both the right direction and the left direction based on the straight traveling state based on the gyro information between the neighboring area arA and the rear area arB. It is determined that the direction has been changed. When it is determined by the multiple azimuth change determination means 26 that there have been multiple azimuth changes, the calculation of the basic correction coefficient for the area between the neighboring area arA and the rear area arB is omitted. Thus, for example, as shown in FIG. 3, even if the azimuth is changed to the right as a whole between the rear end point Pg1 of the neighboring area arA and the front end point Pg2 of the rear area arB, When the direction is finely changed from left to right, the information between the neighboring area arA and the rear area arB is excluded from the information to be calculated for the basic correction coefficient.

The sensor information calculation unit 27 includes a sensor direction calculation unit 28 and a sensor direction change amount calculation unit 29.
The sensor azimuth calculation means 28 is a means for calculating the traveling azimuth in the vicinity area arA and the traveling azimuth in the rear area arB based on the gyro information. In this example, as shown in FIG. 2, the sensor direction calculation means 28 is connected to the number of trips (points Pa1, Pa2, Pa3 on the sensor locus Ls) indicated by the GPS trip number information included in the GPS information A1, A2, A3. The point Pma having the number of trips closest to the average value of the correspondence) is determined as a representative point representing the neighboring area arA. And let the advancing direction based on the gyro information in this point Pma be sensor direction ang-Pma of the vicinity area arA. Here, the traveling azimuth at the point Pma can be calculated by successively integrating the azimuth change amounts indicated in the gyro information starting from the traveling azimuth of the host vehicle acquired last time. In addition, the sensor azimuth calculating means 28 is the trip closest to the average value of the trip numbers (corresponding to the points Pb1, Pb2, Pb3 on the sensor locus Ls) indicated by the GPS trip number information included in the GPS information B1, B2, B3. A number of points Pmb are determined as representative points representing the rear region arB. Then, the traveling direction based on the gyro information at the point Pmb is defined as the sensor direction ang-Pmb of the rear region arB. Here, the traveling azimuth at the point Pmb can be calculated by successively integrating the azimuth change amounts indicated in the gyro information starting from the traveling azimuth of the host vehicle acquired last time.
The sensor azimuth change amount calculation means 29 calculates a difference between the sensor azimuth ang-Pma in the vicinity area arA calculated by the sensor azimuth calculation means 28 and the sensor azimuth ang-Pmb in the rear area arB as a sensor azimuth change amount ang-SEN. Calculate. Here, the value of the sensor azimuth change amount ang-SEN is positive when the azimuth is changed to the right, and the value of the sensor azimuth change ang-SEN is negative when the azimuth is changed to the left. is doing. Of course, it is also possible to reverse the left-right and positive-negative relationship.

The azimuth change ratio calculation unit 30 is a ratio between the GPS azimuth change amount ang-GPS calculated by the GPS azimuth change amount calculation means 22 and the sensor azimuth change amount ang-SEN calculated by the sensor azimuth change amount calculation means 29. It is a means for calculating a certain direction change ratio. Specifically, the calculation is performed by the following equation (1).
(Direction change ratio) = ang-SEN / | ang-GPS | (1)
Here, the numerator is the sensor orientation change amount ang-SEN, and the denominator is the absolute value of the GPS orientation change amount ang-GPS. This is so that the value of the azimuth change ratio becomes positive when the azimuth changes in the right direction, and the value of the azimuth change ratio becomes negative when the azimuth changes in the left direction. That is, the values of the GPS azimuth change amount ang-GPS and the sensor azimuth change amount ang-SEN are both set to be positive when the azimuth changes to the right and negative when the azimuth changes to the left. Yes. Therefore, if either one of the GPS azimuth change amount ang-GPS and the sensor azimuth change amount ang-SEN is an absolute value, the value of the azimuth change ratio can be made to be different between right and left. Therefore, the sensor orientation change amount ang-SEN can be an absolute value. Here, this azimuth change ratio calculation unit 30 constitutes “azimuth change ratio calculation means” in the present invention.

  Further, in this example, the azimuth change ratio calculation unit 30 has an absolute value of the azimuth change ratio calculated by the above formula (1) within an allowable range, specifically 0.5 to 1.1 (0.5 ≦ | It is determined whether or not the azimuth change ratio | ≦ 1.1). Then, the azimuth change ratio calculation unit 30 performs a process of correcting the value within the allowable range when the value is outside the allowable range. Specifically, when the absolute value of the azimuth change ratio is less than 0.5, the absolute value of the azimuth change ratio is corrected to 0.5, and the absolute value of the azimuth change ratio is greater than 1.1. Corrects the absolute value of the azimuth change ratio to 1.1. In general, since the sensor azimuth change amount ang-SEN is often smaller in absolute value than the GPS azimuth change amount ang-GPS, the allowable range in this example is set to a wide range of 1 or less. ing.

  The ratio storage unit 31 is a unit that stores information on the azimuth change ratio as a calculation result by the azimuth change ratio calculation unit 30. The ratio calculation unit 31 includes a memory and a buffer, and can store information on a plurality of azimuth change ratios. Here, the ratio storage unit 31 constitutes “storage means” in the present invention.

  The basic correction coefficient calculator 32 is a means for calculating a basic correction coefficient Db for correcting gyro information based on the difference between the GPS azimuth change amount ang-GPS and the sensor azimuth change amount ang-SEN. Specifically, the basic correction coefficient calculation unit 32 calculates the basic correction coefficient Db based on an average value of all or some of the plurality of azimuth change ratios stored in the ratio storage unit 31. In this example, an average value of the latest eight azimuth change ratios stored in the ratio storage unit 31 is calculated, and calculation processing is performed using the value as the basic correction coefficient Db. The calculation of the basic correction coefficient Db is repeatedly performed according to the movement of the own vehicle position. Here, the basic correction coefficient calculator 32 constitutes “basic correction coefficient calculator” in the present invention.

  Further, in this example, the basic correction coefficient calculation unit 32 determines the GPS azimuth change amount ang−GPS and the sensor azimuth change amount ang− when the azimuth is changed to the right direction or the left direction depending on whether the direction is changed to the right direction or the left direction. Based on the difference from SEN, the right basic correction coefficient Dbr and the left basic correction coefficient Dbl are respectively calculated. That is, as described above, the value of the azimuth change ratio is set to be positive when the azimuth is changed in the right direction, and negative when the azimuth is changed in the left direction. Accordingly, the right basic correction coefficient Dbr is calculated using only the azimuth change ratio having a positive value, and the left basic correction coefficient Dbl is calculated using only the azimuth change ratio having a negative value. In the description of the present embodiment, the basic correction coefficient Db is a general term for the right basic correction coefficient Dbr and the left basic correction coefficient Dbl.

The static correction coefficient calculation unit 43 is a means for calculating a static correction coefficient Ds obtained by correcting the basic correction coefficient Db according to the static installation angle of the detection axis X of the gyro sensor 4. Specifically, the static correction coefficient calculation unit 43 calculates the static correction coefficient Ds when the static installation angle of the detection axis X of the gyro sensor 4 is α [°] with respect to the vertical direction. Calculation is performed according to the following equation (2).
(Static correction coefficient Ds) = (Basic correction coefficient Db) × cos α (2)
Here, FIG. 4 is a diagram showing the relationship between the output J of the gyro sensor 4 and the installation angle α [°]. In the present application, with respect to the vertical axis V, the angle is positive on the upslope side (clockwise direction in FIGS. 4 and 5) and negative on the downslope side (counterclockwise direction in FIGS. 4 and 5). To do. As shown in FIG. 4, when the detection axis X of the gyro sensor 4 is installed with an inclination of α [°] with respect to the vertical axis V during horizontal running (when the gradient angle is 0 [°]), The output J of the gyro sensor 4 becomes Jcos α which is cos α times. In this case, the azimuth change amount indicated in the gyro information generated from the output Jcos α of the gyro sensor 4 is also multiplied by cos α. Accordingly, the gyro information is appropriately set according to the installation angle α of the detection axis X of the gyro sensor 4 by setting the value obtained by multiplying the basic correction coefficient Db by cos α as the static correction coefficient Ds as in the above equation (2). Can be corrected.

In this example, the installation angle α [°] is stored in the static correction coefficient calculation unit 43 in advance by inputting a numerical value measured with the vehicle in a horizontal state when the gyro sensor 4 is attached to the vehicle. Use what you have. However, the installation angle α [°] may be automatically detected using, for example, a sensor.
As for the static correction coefficient Ds, as with the basic correction coefficient Db, the right static correction coefficient Dsr and the left static correction coefficient Dsl are respectively calculated. That is, the right basic correction coefficient Dbr is corrected to calculate the right static correction coefficient Dsr, and the left basic correction coefficient Dbl is corrected to calculate the left static correction coefficient Dsl. Here, the static correction coefficient calculation unit 43 constitutes “static correction coefficient calculation means” in the present invention.

The dynamic correction coefficient calculation unit 44 is a means for calculating the dynamic correction coefficient Da by dynamically correcting the static correction coefficient Ds in accordance with the dynamic change in the detection axis X direction of the gyro sensor 4. Specifically, the dynamic correction coefficient calculation unit 44 detects the gyro sensor 4 with reference to the direction (X ′ direction in FIG. 5) inclined with respect to the static installation angle α [°] with respect to the vertical direction. When the dynamic change angle in the axis X direction is β [°], the dynamic correction coefficient Da is calculated by the following equation (3).
(Dynamic correction coefficient Da) = (static correction coefficient Ds) / cos α × cos (α + β) (3)
Here, FIG. 5 is a diagram showing the relationship between the output J of the gyro sensor 4, the installation angle α [°], and the dynamic change angle β [°]. As shown in this figure, the dynamic change angle β [°] is the same as the detection axis direction X ′ of the gyro sensor 4 during horizontal running (when the gradient angle is 0 °), that is, the vertical direction. The static installation angle α [°] is an angle based on the inclined direction. Therefore, when the gradient angle, that is, the dynamic change angle is β [°], the detection axis X is inclined by β [°] with respect to the detection axis X ′ during horizontal travel, and therefore the detection axis X is the vertical axis V Is inclined by α + β [°]. Therefore, the output J of the gyro sensor 4 becomes J cos (α + β) times cos (α + β). In this case, the azimuth change amount indicated in the gyro information generated from the output J cos (α + β) of the gyro sensor 4 is also multiplied by cos (α + β). On the other hand, as described above, the static correction coefficient Ds is calculated by multiplying the basic correction coefficient Db by cos α. Accordingly, the static correction coefficient Ds is multiplied by (1 / cos α) to remove the influence of the installation angle α [°], and then the value obtained by multiplying by cos (α + β) is set as the dynamic correction coefficient Da, thereby obtaining the gyro sensor 4. The gyro information can be appropriately corrected according to the dynamic change in the detection axis X direction.

Here, as the dynamic change angle β [°], an angle indicated in the gradient angle information acquired by the gradient sensor 41 and the gradient information acquisition unit 42 is used. Incidentally, in FIG. 5, the case where the dynamic change angle β [°] is equal to the road inclination angle has been described as an example. However, the actual dynamic change angle includes a static installation angle such as an inclination due to pitching of the vehicle. All angle changes in the longitudinal direction of the vehicle other than α [°] are included.
As for the dynamic correction coefficient Da, the right dynamic correction coefficient Dar and the left dynamic correction coefficient Dal are respectively calculated in the same manner as the basic correction coefficient Db and the static correction coefficient Ds. That is, the right static correction coefficient Dsr is corrected to calculate the right dynamic correction coefficient Dar, and the left static correction coefficient Dsl is corrected to calculate the left dynamic correction coefficient Dal. Here, the dynamic correction coefficient calculation unit 44 constitutes “dynamic correction coefficient calculation means” in the present invention.

The gyro information correction unit 33 is a unit that corrects the gyro information using the latest dynamic correction coefficient Da calculated by the dynamic correction coefficient calculation unit 44. In this example, the gyro information correction unit 33 acquires the corrected gyro information by multiplying the gyro information acquired by the gyro information acquisition unit 8 by the reciprocal of the dynamic correction coefficient Da. Specifically, the calculation is performed by the following equation (4).
(Gyro information after correction) = (Gyro information) × (1 / Da) (4)
As described above, the gyro information is information representing the amount of azimuth change generated based on the output value of the gyro sensor 4 and the resolution per unit output value. On the other hand, as described above, the dynamic correction coefficient Da is calculated by correcting the static correction coefficient Ds according to the dynamic change in the detection axis X direction of the gyro sensor 4, and the static correction coefficient Ds is calculated by the gyro sensor 4. The basic correction coefficient Db is corrected according to the static installation angle of the detection axis X. The basic correction coefficient Db is an average value of a plurality (eight in this example) of azimuth change ratios (= ang−SEN / | ang−GPS |). Therefore, the corrected gyro information calculated by multiplying the gyro information by the inverse of the dynamic correction coefficient Da reflects the dynamic change in the detection axis X direction of the gyro sensor 4 and the static installation angle. Based on the GPS information, correction is performed by reflecting the difference between the recent GPS azimuth change amount ang-GPS and the sensor azimuth change amount ang-SEN. Therefore, the corrected gyro information represents an accurate azimuth change amount taking these into consideration. Here, the gyro information correction unit 33 constitutes “direction sensor information correction means” in the present invention.

  The own position calculation unit 34 calculates the movement locus of the own vehicle position based on the corrected gyro information and the pulse number information. That is, the own position calculation unit 34 uses the pulse number information acquisition unit 10 to obtain the pulse number information and the gyro information starting from the position and traveling direction of the own vehicle calculated in the previous time stored in the own position storage unit 35 described later. Based on the gyro information corrected by the correction unit 33, the movement locus of the vehicle position from the starting point is obtained. Here, the calculation of the movement trajectory of the own vehicle position is performed by calculating the pulse number information toward the traveling direction of the own vehicle calculated based on the traveling direction at the starting point and the direction change amount indicated in the corrected gyro information. This is performed by moving from the position of the starting point by a moving distance calculated by multiplying by a predetermined standard distance. Based on this movement trajectory, the position and traveling direction of the host vehicle are calculated. The position and traveling direction of the host vehicle are periodically calculated at least every second here when GPS information is acquired by the GPS information acquisition unit 6. The calculation result by the own position calculation unit 34 is stored in the own position storage unit 35 as the current position and traveling direction of the own vehicle. Further, the information on the position and traveling direction of the own vehicle is sent as an output of the own vehicle position recognizing device 2 to a navigation device, a vehicle control device, etc. (not shown). Here, the own position calculation unit 34 constitutes “own position calculation means” in the present invention.

  The own position storage unit 35 is a storage unit that stores information on the position and traveling direction of the own vehicle that has been sequentially recognized until the previous time. The information on the position and traveling direction of the host vehicle so far stored in the host position storage unit 35 is sent to the own position calculating unit 34 and used for calculation of the current position and traveling direction of the host vehicle. .

  Next, calculation processing of the correction coefficient D by the correction coefficient calculation device 1 according to this embodiment and recognition processing of the host vehicle position by the host vehicle position recognition device 2 will be described using the flowcharts shown in FIGS. In the description of the present embodiment, the correction coefficient D is a general term for the basic correction coefficient Db, the static correction coefficient Ds, and the dynamic correction coefficient Da.

  First, calculation processing of the correction coefficient D will be described with reference to FIGS. Each calculation processing step described below is executed by each functional unit and means of the correction coefficient calculation device 1 that operates according to the correction coefficient calculation program. In this process, first, as shown in FIG. 2, the vicinity area arA is set by the vicinity area setting means 13 on the basis of the current vehicle position (step # 01). Next, in steps # 02 to # 04, it is determined whether or not the GPS information in the vicinity area arA satisfies a predetermined calculation start condition. This calculation start condition is a condition that the vehicle position is traveling straight in the vicinity area arA and that the signal reception state from the GPS satellite is good. That is, in step # 02, the GPS straight traveling determination means 16 determines whether or not the GPS information indicates a straight traveling state in the vicinity area arA. Specifically, as described above, it is determined whether or not the absolute value of the azimuth change amount between each of the GPS information A1 and A2, A2 and A3, and A1 and A3 is 3 [°] or less. In step # 03, the speed determination means 17 determines whether or not the GPS information indicates a traveling state in the vicinity area arA. Specifically, as described above, it is determined whether all of the GPS information A1, A2, and A3 have a speed of 5 [m / s] or more. In step # 04, the reception state determination means 18 determines whether the reception state of the signal from the GPS satellite when the GPS information is acquired is good or bad. Specifically, as described above, it is determined whether or not the number of positioning satellites is four in all of the GPS information A1, A2, and A3. In addition, the order of these steps # 02 to # 04 is not particularly limited, and can be exchanged. If any of these conditions of Steps # 02 to # 04 is not satisfied, the subsequent calculation process of the correction coefficient D is not executed.

  When all the conditions of steps # 02 to # 04 are satisfied, next, as shown in FIG. 2, the sensor azimuth calculation means 28 calculates the representative point Pma of the neighboring area arA, and the GPS azimuth calculation means 21 To calculate the GPS azimuth ang-A in the vicinity area arA (step # 05). Next, the rear area arB is set by the rear area setting means 14 as shown in FIG. 2 (step # 06). Then, the sensor azimuth calculating means 28 calculates the representative point Pmb of the rear area arB, and the GPS azimuth calculating means 21 calculates the GPS azimuth ang-B in the rear area arB (step # 07). Thereafter, the GPS azimuth change calculating means 22 calculates the difference between the GPS azimuth ang-A in the vicinity area arA and the GPS azimuth ang-B in the rear area arB as the GPS azimuth change ang-GPS (step #). 08).

  Whether the absolute value of the GPS azimuth change amount ang-GPS calculated in step # 08 is 60 ° or more and 120 ° or less (60 ° ≦ | ang−GPS | ≦ 120 °) by the GPS azimuth change determination means 19. It is determined whether or not (step # 09). Thereby, it is determined based on the GPS information whether or not there has been a change in direction between the vicinity area arA and the rear area arB. If there is an azimuth change between the vicinity area arA and the rear area arB (step # 09: YES), then the sensor straight-ahead determination means 24 indicates that the gyro information includes both the vicinity area arA and the rear area arB. It is determined whether or not a straight traveling state is indicated within the area (step # 10). Specifically, as described above, the absolute value of the azimuth change amount between the current point Pg0 and the point Pg1, which are the points at both ends of the neighboring area arA, is 3 [°] or less, and both ends of the rear area arB. It is determined whether or not the absolute value of the azimuth change amount between the point Pg2 and the point Pg3 is 3 [°] or less. If the condition is not satisfied in step # 09 or # 10, the process proceeds to step # 17, and the process for the next rear area arC is executed.

  When the gyro information indicates a straight traveling state in both the vicinity area arA and the rear area arB (step # 10: YES), the sensor direction calculation means 28 causes the sensor direction ang-Pma of the vicinity area arA to be detected. And the sensor orientation ang-Pmb of the rear area arB is calculated (step # 11). Then, the difference between the sensor orientation ang-Pma in the vicinity area arA and the sensor orientation ang-Pmb in the rear area arB is calculated as the sensor orientation change amount ang-SEN by the sensor orientation change amount calculation means 29 (step # 12). .

  Next, the sensor orientation change determination means 25 determines whether or not the absolute value of the sensor orientation change amount ang-SEN calculated in step # 12 is 30 ° or more (30 ° ≦ | ang−SEN |). (Step # 13). As a result, whether or not there has been a change in direction between the vicinity area arA and the rear area arB is determined based on the gyro information. If the absolute value of the sensor orientation change amount ang-SEN is less than 30 ° (step # 13: NO), the process proceeds to step # 17, and the process for the next rear area arC is executed.

  On the other hand, when the absolute value of the sensor azimuth change amount ang-SEN is 30 ° or more (step # 13: YES), for example, as shown in FIG. It is determined whether or not there have been multiple azimuth changes in different directions between the vicinity area arA and the rear area arB (step # 14). If there are a plurality of azimuth changes in different directions (step # 15: YES), the calculation of the correction coefficient relating to the vicinity area arA and the rear area arB is omitted, and the process ends.

  On the other hand, if there are no azimuth changes in different directions (step # 15: NO), the azimuth change ratio calculation unit 30 calculates the GPS azimuth change ang-GPS calculated in step # 08 and step # 12. The azimuth change ratio is calculated based on the sensor azimuth change amount ang-SEN calculated in (Step # 16). Specifically, the azimuth change ratio is calculated by the above formula (1) as a ratio in which the numerator is the sensor azimuth change amount ang-SEN and the denominator is the absolute value of the GPS azimuth change amount ang-GPS. And the information of the calculated azimuth | direction change ratio is memorize | stored in the ratio memory | storage part 31 (step # 17). By performing the above processing, for example, when changing the direction such as turning left or right at an intersection, the state of the vehicle returns to the straight-ahead state after the direction is changed from the straight-ahead state to one direction. In this case, the azimuth change ratio is calculated and stored in the ratio storage unit 31.

  Thereafter, it is determined whether or not the processing of steps # 07 to # 17 has been completed for all the rear areas arB, arC, arD... ArH (step # 18). That is, in this example, seven areas from arB to arH having different distances from the neighboring area arA are sequentially set as the rear area. Therefore, when the process for all the rear areas arB, arC, arD... ArH is not completed (step # 18: NO), after the process is completed for one rear area, for example, arB, Is set, for example, arC (step # 19). And the process of step # 07- # 17 is similarly performed about the said following back area | region.

  Thereafter, when the process for the last rear area arH is completed (step # 18: YES), it is next determined whether or not eight or more pieces of direction change ratio information are stored in the ratio storage unit 31. (Step # 20). Here, when the number of the azimuth change ratio information stored in the ratio storage unit 31 is less than eight (step # 20: NO), the azimuth change ratio information is used to calculate the basic correction coefficient Db. Therefore, the basic correction coefficient Db is set to a predetermined initial value (step # 21). Then, the calculation process of the basic correction coefficient Db is finished. On the other hand, when the number of azimuth change ratio information stored in the ratio storage unit 31 is eight or more (step # 20: YES), the basic correction coefficient calculation unit 32 calculates the basic correction coefficient Db. (Step # 22). Specifically, as described above, the average value of the latest eight azimuth change ratios stored in the ratio storage unit 31 is calculated, and the value is set as the basic correction coefficient Db. At this time, the right basic correction coefficient Dbr and the left basic correction coefficient Dbl are calculated according to whether the direction of the azimuth change is rightward or leftward.

  Next, the static correction coefficient calculation unit 43 calculates the static correction coefficient Ds (step # 23). Specifically, as described above, when the static installation angle of the detection axis X of the gyro sensor 4 is α [°] with respect to the vertical direction, the static correction is performed by multiplying the basic correction coefficient Db by cos α. A correction coefficient Ds is calculated. Then, the value of the static correction coefficient Ds is updated to a newly calculated value (step # 24).

  Next, a dynamic change in the detection axis X direction of the gyro sensor 4 is detected based on the gradient angle information acquired by the gradient sensor 41 and the gradient information acquisition unit 42 (step # 25). When a dynamic change in the detection axis X direction is not detected (step # 25: NO), that is, when traveling horizontally, the gyro information is corrected using the latest static correction coefficient Ds, and the correction coefficient The calculation process of D ends. On the other hand, when a dynamic change in the detection axis X direction is detected (step # 25: YES), the dynamic correction coefficient calculation unit 44 calculates the dynamic correction coefficient Da (step # 26). Specifically, as described above, the movement of the gyro sensor 4 in the detection axis X direction with respect to the direction inclined in the static installation angle α [°] relative to the vertical direction (the direction of X ′ in FIG. 5). When the static change angle is β [°], the static correction coefficient Ds is multiplied by (1 / cos α) to remove the influence of the installation angle α [°] and then dynamically multiplied by cos (α + β). A correction coefficient Da is calculated. Above, the calculation process of the correction coefficient D is complete | finished.

  Next, the vehicle position recognition process will be described with reference to FIG. Each calculation processing step described below is executed by each functional unit and means of the vehicle position recognition device 2 that operates according to the own position recognition program. In this process, first, the correction coefficient D is calculated (step # 101). The calculation process of the correction coefficient D is as already described with reference to FIGS. Then, the gyro information correction unit 33 uses the latest dynamic correction coefficient Da calculated in step # 101 (or the latest static correction coefficient Ds when the dynamic correction coefficient Da is not calculated), to use the gyro information. The information is corrected (step # 102). Specifically, the corrected gyro information is calculated by multiplying the gyro information before correction by the reciprocal of the dynamic correction coefficient Da (static correction coefficient Ds) according to the above equation (4). Thereby, the gyro information (corrected gyro information) representing the azimuth change amount corrected based on the GPS information is acquired.

  Next, the own position calculation unit 34 calculates the movement locus of the own vehicle position based on the corrected gyro information and the pulse number information. Specifically, the vehicle position and traveling direction calculated in the previous time stored in the own position storage unit 35 are used as starting points, and the calculation is based on the moving direction at the starting point and the direction change amount indicated in the corrected gyro information. The moving locus of the vehicle position is calculated assuming that the vehicle has moved from the starting position by the moving distance calculated by multiplying the pulse number information by a predetermined standard distance in the traveling direction of the vehicle. Based on this movement locus, the vehicle position is recognized, that is, the position and traveling direction of the vehicle are recognized (step # 104). Thereafter, when the position of the host vehicle is moving (step # 105: YES), the process returns to step # 101, and further, the position of the host vehicle is recognized (step # 101) from the calculation process of the correction coefficient D (step # 101). The processes up to # 104) are repeated. And when the own vehicle stops (step # 105: NO), the own vehicle position recognition process is complete | finished.

[Other Embodiments]
(1) In the above embodiment, when the dynamic change in the detection axis X direction of the gyro sensor 4 is not detected (when traveling horizontally), the calculation of the dynamic correction coefficient Da is omitted, and the static correction coefficient The case where the gyro information is corrected using Ds has been described. However, the application range of the present invention is not limited to this embodiment. That is, it is determined whether or not a predetermined dynamic correction execution condition is satisfied, and when the dynamic correction execution condition is not satisfied, the calculation of the dynamic correction coefficient Da by the dynamic correction coefficient calculation unit 44 may be omitted. This is one of the preferred embodiments. Here, the predetermined dynamic correction execution condition includes a condition relating to a dynamic change in the detection axis X direction of the gyro sensor 4, specifically, a dynamic change in the detection axis X direction within a predetermined range (for example, ± 3 [ It is preferable to include the following condition: When the dynamic change in the detection axis X direction is within the predetermined range, the calculation of the dynamic correction coefficient Da is omitted. As a result, when dynamic correction of gyro information is unnecessary, useless calculation processing can be avoided and the calculation load of the apparatus can be further reduced.
In addition to the predetermined dynamic correction execution conditions, for example, a condition that the vehicle position is not map-matched, and a flat road without a gradient based on the map information is determined. It is preferable that the conditions specify a case where it is not appropriate to perform correction according to the dynamic change in the detection axis X direction, such as the condition that the vehicle is traveling.
It is also one preferred embodiment that the dynamic correction coefficient calculation unit 44 always calculates the dynamic correction coefficient Da and always corrects the gyro information using the latest dynamic correction coefficient Da. .

(2) In the above-described embodiment, the basic correction coefficient calculation unit 32 calculates the average value of the latest eight azimuth change ratios stored in the ratio storage unit 31, and sets the value as the basic correction coefficient Db. Explained the case. However, the calculation method of the basic correction coefficient Db is not limited to this. That is, for example, calculating the basic correction coefficient Db using a histogram as shown in FIG. 9 is also one preferred embodiment. That is, in this histogram, the horizontal axis represents the azimuth change ratio, and the vertical axis represents the frequency. Then, a frequency is given to each azimuth change ratio calculated by the azimuth change ratio calculation unit 30 and registered in the histogram. This histogram is stored in the ratio storage unit 31. When the number of registrations exceeds a certain number (for example, 100 or more), the average value of the azimuth change ratios registered in the histogram is calculated. At this time, a range of standard deviation σ is set on both sides from the overall average value of the histogram, and an average value of the azimuth change ratios within this range is calculated, and this is set as a basic correction coefficient Db. As a result, the basic correction coefficient Db is calculated based on an average value of some of the plurality of azimuth change ratios stored in the ratio storage unit 31.

(3) In the above embodiment, the case has been described in which the reception state determination unit 18 determines only the number of positioning satellites as the reception state of signals from GPS satellites. However, it is also a preferred embodiment to determine the reception status of signals from other GPS satellites. As such a reception state determination condition, for example, an error radius condition indicated in error radius information included in GPS information can be used. That is, for example, when the error radius is 300 [m] or less, it can be determined that the reception state is good.

(4) Further, in order to improve the accuracy of the basic correction coefficient Db, it is preferable that the determination conditions in the GPS straight traveling determination unit 16 and the speed determination unit 17 are further strict. For example, in the above-described embodiment, the GPS straight traveling determination unit 16 determines whether the absolute value of the azimuth change amount between the GPS information A1 and A2, A2 and A3, and A1 and A3 is 3 ° or less. However, it is also preferable to set this to 1.5 [°] or less, or 0.7 [°] or less. In the above embodiment, the speed determination means 17 determines whether or not the speed is 5 [m / s] or more in all of the GPS information A1, A2, and A3. .5 [m / s] or more is also suitable.

(5) Similarly, in order to improve the accuracy of the basic correction coefficient Db, it is preferable to make the determination condition in the sensor straight traveling determination means 24 more stringent. For example, in the above embodiment, the sensor straight-ahead determination means 24 has an absolute value of the azimuth change amount between the current point Pg0 and the point Pg1, which are the points at both ends of the vicinity area arA, of 3 [°] or less. In addition, it is determined whether or not the absolute value of the azimuth change amount between the points Pg2 and Pg3, which are both ends of the rear region arB, is 3 [°] or less. It is also suitable as below.

(6) In the above embodiment, the case where the GPS straight-line determination unit 16, the speed determination unit 17, and the reception state determination unit 18 perform the determination only on the GPS information A1, A2, and A3 in the vicinity area arA has been described. . However, it is also one preferred embodiment that the GPS straight-running determination unit 16, the speed determination unit 17, and the reception state determination unit 18 are configured to perform the same determination for each of the rear areas arC, arD. It is. In this case, as a specific processing procedure, for example, after the rear area arB is set in step # 06 of FIG. 4, the same determination process as in steps # 02 to # 04 is performed before the process of step # 07. This is performed for the area arB. If all of these determination conditions are satisfied, the process proceeds to step # 07. On the other hand, if any of these determination conditions is not satisfied, the process proceeds to step # 18. Here, since the processing has not been completed for all the rear areas arB, arC, arD... ArH (step # 18: NO), the next rear area arC is set (step # 19). In this way, the same determination process as in steps # 02 to # 04 is performed for all the rear regions arB, arC, arD... ArH, and only the rear region satisfying these determination conditions is used for the calculation of the azimuth change ratio. Use.

(7) In the above embodiment, the vehicle position recognition device 2 of the vehicle has been described as an example. However, the present invention is applied to a self-position recognition device or the like of a vehicle other than a vehicle or a vehicle such as a ship. Is of course possible.

  Correction sensor calculation device and calculation program for azimuth sensor capable of appropriately calculating a static installation angle of the azimuth sensor and a dynamic correction coefficient in consideration of a dynamic change in the installation angle, a self-position recognition device, and A recognition program was obtained.

1: Correction coefficient calculation device 2: Own vehicle position recognition device 4: Gyro sensor (direction sensor)
5: Distance sensor 6: GPS information acquisition unit (GPS information acquisition means)
8: Gyro information acquisition unit (direction sensor information acquisition means)
10: Pulse number information acquisition unit (distance sensor information acquisition means)
14: Rear area setting means 16: GPS straight-ahead determination means (first straight-ahead determination means)
19: GPS azimuth change determination means 22: GPS azimuth change amount calculation means 24: Sensor straight-ahead determination means (second straight-ahead determination means)
26: Multiple-direction azimuth change determination means 29: Sensor azimuth change amount calculation means 30: Direction change ratio calculation section (azimuth change ratio calculation means)
31: Ratio storage unit (storage means)
32: Basic correction coefficient calculation unit (basic correction coefficient calculation means)
33: Gyro information correction unit (direction sensor information correction means)
34: Own position calculation unit (own position calculation means)
41: Gradient sensor (dynamic change detection means)
42: Gradient information acquisition unit (dynamic change detection means)
43: Static correction coefficient calculation unit (static correction coefficient calculation means)
44: Dynamic correction coefficient calculation unit (dynamic correction coefficient calculation means)
Db: Basic correction coefficient Ds: Static correction coefficient Da: Dynamic correction coefficient X: Gyro sensor detection axis ang-GPS: GPS azimuth change ang-SEN: Sensor azimuth change

Claims (10)

GPS information acquisition means for acquiring GPS information including position information and traveling direction information based on a signal from a GPS satellite;
Direction sensor information acquisition means for acquiring direction sensor information from the direction sensor;
A GPS orientation change amount calculation means for calculating an amount of change proceeds whereabouts position based on the GPS information,
A sensor orientation change amount calculation means for calculating an amount of change proceeds whereabouts position based on the azimuth sensor information,
A basic correction coefficient calculation means based on the difference between pre-Symbol GPS orientation change amount of the sensor orientation change amount, calculates a basic correction coefficient of the azimuth sensor information,
A static correction coefficient calculating means for calculating a static correction coefficient obtained by correcting the basic correction coefficient according to a static installation angle of a detection axis of the azimuth sensor;
Dynamic change detection means for detecting a dynamic change in the detection axis direction of the orientation sensor;
Dynamic correction coefficient calculating means for dynamically correcting the static correction coefficient and calculating a dynamic correction coefficient according to a dynamic change in the detection axis direction of the azimuth sensor;
An azimuth change ratio calculating means for calculating an azimuth change ratio between the GPS azimuth change and the sensor azimuth change;
Storage means for storing information on the azimuth change ratio as a calculation result of the azimuth change ratio calculation means;
Neighborhood area setting means for setting a neighborhood area in the vicinity of the own position;
A rear region setting means for setting a plurality of rear regions having different distances from the neighboring region behind the neighboring region in the traveling direction ;
The azimuth change ratio calculating means calculates the azimuth change ratio between each of the rear areas and the neighboring area, and stores information on the respective azimuth change ratios in the storage means.
The basic correction coefficient calculating means, the correction coefficient calculation unit for all or azimuth sensor you calculating the basic correction factor based in part on the average value of a plurality of direction change ratio stored in the storage means.   2. The azimuth sensor correction according to claim 1, wherein the basic correction coefficient calculation unit calculates the basic correction coefficient based on a latest average value of a plurality of azimuth change ratios stored in the storage unit. Coefficient arithmetic unit.   Based on the azimuth sensor information, further comprising a plurality of azimuth change determination means for determining whether or not there have been a plurality of azimuth changes in different directions between the vicinity area and the rear area,
  When it is determined by the plurality of azimuth change determination means that the azimuth change has been made a plurality of times, the basic correction coefficient calculation means includes the neighborhood area and the rear area determined to have a plurality of azimuth changes. The correction coefficient calculation device of the azimuth sensor according to claim 1 or 2, wherein the information of the azimuth change ratio during the period is excluded from the calculation target of the basic correction coefficient. The static correction coefficient calculation means, when the static installation angle of the detection axis of the azimuth sensor is α ° with respect to the vertical direction, the static correction coefficient,
(The static correction coefficient) = (the basic correction coefficient) × cos α
Operate as
The dynamic correction coefficient calculating means is configured to perform the dynamic correction when the dynamic change angle in the detection axis direction of the azimuth sensor is β ° with reference to a direction inclined by the installation angle α ° with respect to a vertical direction. Coefficient
(Dynamic correction coefficient) = (static correction coefficient) / cos α × cos (α + β)
The correction coefficient calculation device for the azimuth sensor according to any one of claims 1 to 3, wherein the correction coefficient calculation device is calculated as follows. A self position recognizing device comprising the azimuth sensor correction coefficient computing device according to any one of claims 1 to 4 ,
Direction sensor information correction means for correcting the direction sensor information using at least one of the static correction coefficient and the dynamic correction coefficient;
Distance sensor information acquisition means for acquiring distance sensor information from a distance sensor for detecting a moving distance;
Self-position calculation means for calculating the movement locus of the self-position based on the corrected azimuth sensor information and the distance sensor information and recognizing the self-position;
A self-recognition apparatus comprising: A GPS orientation change amount calculation step of calculating an amount of change proceeds whereabouts position based on the GPS information from the GPS information acquisition step of acquiring GPS information including the position information and traveling direction information based on signals from GPS satellites,
A sensor orientation change amount calculation step of calculating an amount of change proceeds whereabouts position based on the azimuth sensor information from the azimuth sensor,
Based on the difference between pre-Symbol GPS orientation change amount of the sensor orientation change amount, and the basic correction coefficient calculation step of calculating the basic correction coefficient of the azimuth sensor information,
A static correction coefficient calculation step of calculating a static correction coefficient obtained by correcting the basic correction coefficient according to a static installation angle of a detection axis of the azimuth sensor;
A dynamic change detecting step for detecting a dynamic change in a detection axis direction of the orientation sensor;
A dynamic correction coefficient calculating step of dynamically correcting the static correction coefficient and calculating a dynamic correction coefficient in accordance with a dynamic change in the detection axis direction of the azimuth sensor;
An azimuth change ratio calculating step for calculating a ratio between the GPS azimuth change and the sensor azimuth change;
A storage step of storing information on the azimuth change ratio as a calculation result of the azimuth change ratio calculation step in a predetermined storage unit;
A neighborhood region setting step for setting a neighborhood region in the vicinity of the own position;
A rear region setting step of setting a plurality of rear regions having different distances from the neighboring region behind the neighboring region in the traveling direction ;
In the azimuth change ratio calculation step, for a plurality of the rear areas, the azimuth change ratio between the neighboring areas is calculated, and information on each azimuth change ratio is stored in the storage unit.
Wherein the basic correction coefficient calculation step, the correction coefficient calculation program of the azimuth sensor you calculating the basic correction factor based on the average value of all or some of the plurality of direction change ratio stored in the storage means.   The azimuth sensor correction according to claim 6, wherein in the basic correction coefficient calculation step, the basic correction coefficient is calculated based on a latest average value of a plurality of azimuth change ratios stored in the storage unit. Coefficient calculation program.   Based on the azimuth sensor information, further performing a multiple azimuth change determination step for determining whether or not there have been multiple azimuth changes in different directions between the neighborhood area and the rear area,
  If it is determined in the multiple azimuth change determination step that there have been multiple azimuth changes, in the basic correction coefficient calculation step, the neighborhood area and the rear area determined to have multiple azimuth changes. The azimuth sensor correction coefficient calculation program according to claim 6 or 7, wherein information on the azimuth change ratio is excluded from calculation targets of the basic correction coefficient. In the static correction coefficient calculation step, when the static installation angle of the detection axis of the azimuth sensor is α ° with respect to the vertical direction, the static correction coefficient is
(The static correction coefficient) = (the basic correction coefficient) × cos α
Operate as
In the dynamic correction coefficient calculating step, when the dynamic change angle in the detection axis direction of the azimuth sensor is β ° with reference to the direction inclined by the installation angle α ° with respect to the vertical direction, the dynamic correction is performed. Coefficient
(Dynamic correction coefficient) = (static correction coefficient) / cos α × cos (α + β)
The correction coefficient calculation program for an orientation sensor according to any one of claims 6 to 8, wherein the correction coefficient calculation program is calculated as follows. A self-position recognition program comprising the azimuth sensor correction coefficient calculation program according to any one of claims 6 to 9 ,
A direction sensor information correction step for correcting the direction sensor information using at least one of the static correction coefficient and the dynamic correction coefficient;
A distance sensor information acquisition step for acquiring distance sensor information from a distance sensor for detecting a moving distance;
A self-position calculation step of calculating a movement locus of the own position based on the corrected azimuth sensor information and distance sensor information from a distance sensor for detecting a movement distance, and recognizing the own position;
A self-recognition program that causes a computer to execute.

Images