Scale factor estimation device, scale factor estimation method, and scale factor estimation program

Abstract

To correct a scale factor of an inertia measurement unit.SOLUTION: A scale factor estimation device includes: an amount-of-change in azimuth angle calculation portion 46 that calculates an amount of change in azimuth angle of a vehicle body on the basis of an output of an IMU mounted on the vehicle body; an amount-of-change in GNSS azimuth angle calculation portion 42 that is installed on the vehicle body leaving distance more than the prescribed distance between the calculation portion 42 and the vehicle body, and calculates an amount of change in azimuth angle in a yaw direction from difference information between a first antenna and second antenna each capable of acquiring positioning information from a satellite; and a scale factor estimation portion 48 that refers the amount-of-change in azimuth angle calculated in the amount-of-change in GNSS azimuth angle calculation portion 42 relative to the amount of change in azimuth angle calculated in the amount-of-change in azimuth angle calculation portion 46 to thereby estimate a scale factor of an IMU, and corrects the output of the IMU on the basis of the estimated scale factor.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present invention relates to a scale factor estimation device, a scale factor estimation method, and a scale factor estimation program for an inertial measurement device such as an IMU (Inertial Measurement Unit) that uses a gyroscope. [Background technology]

[0002] The output of the gyro used in inertial measurement units contains errors, the main errors being (1) bias error, also known as zero point error or offset error, (2) scale factor error, (3) linearity, and (4) noise components.

[0003] Figure 11 is a schematic diagram showing the errors contained in the gyro output, showing a true value 120 indicating the gyro output that does not include errors, a gyro output 122 that includes bias error and scale factor error, and a bias error corrected gyro output 124 that has been corrected for bias error but still includes scale factor error.

[0004] The bias error is indicated by intercept 126 on the vertical axis. This bias error can be detected and corrected relatively easily because it can be corrected so that the angular velocity indicated by gyro output 122 becomes zero when the vehicle body equipped with the inertial measurement unit is stationary. In particular, when the angular velocity indicated by the horizontal axis in FIG. 11 is small, the bias error becomes an offset error with a nearly constant value. Therefore, it has been conventional to correct the bias error when the vehicle is traveling approximately straight, that is, when the yaw rate, which is the angular velocity in the yaw direction, is small, by referring to orientation information obtained using a GNSS (Global Navigation Satellite System) or the like.

[0005] A scale factor error (hereinafter abbreviated as "scale factor") 128 is an error in the slope deviation from the true value 120 after the bias error has been corrected, as indicated by a bias error corrected gyro output 124.

[0006] As shown in FIG. 11, both the line indicating the true value 120 and the line indicating the bias error corrected gyro output 124 including the scale factor 128 pass through the origin O, so the scale factor 128 has the property of being small when the angular velocity is small and becoming significant when the angular velocity is large.

[0007] Therefore, the scale factor 128 can be effectively detected and corrected by referring to external azimuth angle information or azimuth angular velocity information during a turn in which the angular velocity is equal to or greater than a certain threshold value.

[0008] Patent Document 1 discloses an invention of a direction detection device that estimates and corrects the scale factor of the gyro output using a turning angle measured by a turning angle sensor (yaw rate gyro) and either a turning angle (first turning angle) calculated from the left and right wheel speeds or a turning angle (second turning angle) calculated using GNSS. [Prior art documents] [Patent documents]

[0009] [Patent Document 1] Patent No. 6495609 Summary of the Invention [Problem to be solved by the invention]

[0010] However, the technology described in Patent Document 1 has a problem in that when the turning angle is calculated from the left and right wheel speeds, tire slippage causes an error in the estimation of the turning angle. Also, although the second turning angle and its accuracy are calculated based on observation amounts obtained by GNSS, there is no detailed description of how the turning angle is calculated, which results in a problem in that it is difficult to accurately estimate and correct the scale factor.

[0011] The present invention has been made in view of the above circumstances, and has as its object to provide a scale factor estimation device, a scale factor estimation method, and a scale factor estimation program that can correct the scale factor of an inertial measurement unit. [Means for solving the problem]

[0012] In order to achieve the above object, the scale factor estimation device according to claim 1 includes an inertial azimuth angle change amount calculation unit that calculates an azimuth angle change amount in the yaw direction of a vehicle body based on the output of an inertial measurement unit mounted on the vehicle body, and an inertial azimuth angle change amount calculation unit that is installed on the vehicle body at a distance equal to or greater than a predetermined distance and that can acquire positioning information from satellites. a difference between the positioning results obtained by the first antenna and the second antenna, a difference between the pseudoranges of the signals received by the first antenna and the second antenna from the satellite, and a difference between the phases of the carrier waves of the signals received by the first antenna and the second antenna from the satellite; and a time change in the calculated relative position. Calculate the amount of change in azimuth angle in the yaw direction of the vehicle body from Alternatively, a relative velocity vector, which is a difference between the velocity vector of the first antenna and the velocity vector of the second antenna, is divided by a baseline vector, which is a relative position between the first antenna and the second antenna, to calculate an amount of change in azimuth angle in the yaw direction of the vehicle body. The inertial measurement unit includes a satellite positioning azimuth angle change amount calculation unit, and a scale factor estimation unit that estimates a scale factor of the inertial measurement unit by referring to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit for the azimuth angle change amount calculated by the inertial azimuth angle change amount calculation unit, and corrects the output of the inertial measurement unit based on the estimated scale factor.

[0013] The scale factor estimation device according to claim 2 further includes a turning state determination unit that determines that the vehicle body is turning when the azimuth velocity in the yaw direction output by the inertial measurement unit is equal to or greater than a predetermined threshold, and the scale factor estimation unit estimates the scale factor of the inertial measurement unit based on the azimuth change amount in the yaw direction of the vehicle body calculated by the inertial azimuth angle change amount calculation unit and the satellite positioning azimuth angle change amount calculation unit when the turning state determination unit determines that the vehicle body is turning.

[0016] Claim 3 In the scale factor estimation device according to the present invention, when the turning state determination unit determines that the vehicle body is turning, the scale factor estimation unit estimates the scale factor from the ratio of the azimuth angle change amount calculated by the inertial azimuth angle change amount calculation unit to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit.

[0017] Claim 4 In the scale factor estimation device according to the present invention, when a change in the azimuth angle of the vehicle body obtained based on the azimuth angle change in the yaw direction of the vehicle body by either the inertial azimuth angle change amount calculation unit or the satellite positioning azimuth angle change amount calculation unit exceeds a predetermined azimuth angle change amount threshold from the time when the turning state determination unit determines that the vehicle body has started turning, the scale factor estimation unit estimates the scale factor of the inertial measurement unit based on the azimuth angle change in the yaw direction of the vehicle body calculated by each of the inertial azimuth angle change amount calculation unit and the satellite positioning azimuth angle change amount calculation unit.

[0018] In order to achieve the above object, claims 5 The scale factor estimation method according to the present invention includes an inertial azimuth angle change amount calculation step of calculating an azimuth angle change amount in the yaw direction of the vehicle body based on the output of an inertial measurement unit mounted on the vehicle body, and an inertial measurement unit mounted on the vehicle body at a distance equal to or greater than a predetermined distance and capable of acquiring positioning information from satellites. a difference between the positioning results obtained by the first antenna and the second antenna, a difference between the pseudoranges of the signals received by the first antenna and the second antenna from the satellite, and a difference between the phases of the carrier waves of the signals received by the first antenna and the second antenna from the satellite; and a time change in the calculated relative position. Calculate the amount of change in azimuth angle in the yaw direction of the vehicle body from Alternatively, a relative velocity vector, which is a difference between the velocity vector of the first antenna and the velocity vector of the second antenna, is divided by a baseline vector, which is a relative position between the first antenna and the second antenna, to calculate an amount of change in azimuth angle in the yaw direction of the vehicle body. a satellite positioning azimuth angle change amount calculation step; and a scale factor estimation step of estimating a scale factor of the inertial measurement unit by referring to the azimuth angle change amount calculated in the satellite positioning azimuth angle change amount calculation step for the azimuth angle change amount calculated in the inertial azimuth angle change amount calculation step, and correcting the output of the inertial measurement unit based on the estimated scale factor. The computer executes the process .

[0019] In order to achieve the above object, a scale factor estimation program according to claim 8 includes a computer that includes an inertial azimuth angle change amount calculation unit that calculates an azimuth angle change amount in the yaw direction of a vehicle body based on an output of an inertial measurement unit mounted on the vehicle body, and an inertial azimuth angle change amount calculation unit that is installed on the vehicle body at a distance equal to or greater than a predetermined distance and that is capable of acquiring positioning information from satellites. a difference between the positioning results obtained by the first antenna and the second antenna, a difference between the pseudoranges of the signals received by the first antenna and the second antenna from the satellite, and a difference between the phases of the carrier waves of the signals received by the first antenna and the second antenna from the satellite; and a time change in the calculated relative position. Calculate the amount of change in azimuth angle in the yaw direction of the vehicle body from Alternatively, a relative velocity vector, which is a difference between the velocity vector of the first antenna and the velocity vector of the second antenna, is divided by a baseline vector, which is a relative position between the first antenna and the second antenna, to calculate an amount of change in azimuth angle in the yaw direction of the vehicle body.The satellite positioning azimuth angle change amount calculation unit and the inertial azimuth angle change amount calculation unit refer to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit to estimate a scale factor of the inertial measurement unit, and function as a scale factor estimation unit that corrects the output of the inertial measurement unit based on the estimated scale factor. [Effects of the Invention]

[0020] According to the scale factor estimation device, the scale factor estimation method, and the scale factor estimation program of the present invention, the scale factor of an inertial measurement unit can be corrected. [Brief explanation of the drawings]

[0021] [Figure 1] 1 is a block diagram showing an example of the configuration of a scale factor estimation device according to a first embodiment of the present invention. [Figure 2] 1 is a schematic diagram showing an example of the arrangement of a first GNSS antenna and a second GNSS antenna in a first embodiment of the present invention. FIG. [Figure 3] 1 is a block diagram showing an example of a specific configuration of a calculation device of a scale factor estimation device according to a first embodiment of the present invention. FIG. [Figure 4] (A) is a schematic diagram showing the direction of movement of the positions of the first GNSS antenna and the second GNSS antenna in a vehicle body making a right turn, and (B) is a schematic diagram showing an example of changes in, for example, the center of gravity position of the vehicle body and the azimuth angles of the first GNSS antenna and the second GNSS antenna. [Figure 5] 1 is a block diagram showing an outline of the processing flow of a scale factor estimation device according to a first embodiment of the present invention. [Figure 6] 3 is a flowchart showing an example of processing by the scale factor estimation device according to the first embodiment of the present invention. [Figure 7] FIG. 10 is a block diagram showing an example of a specific configuration of a calculation device of a scale factor estimation device according to a second embodiment of the present invention. [Figure 8](A) is a schematic diagram showing an example of the respective arrangements of the first GNSS antenna and the second GNSS antenna on a vehicle body making a right turn, and the velocity vectors of the first GNSS antenna and the second GNSS antenna, and (B) is an explanatory diagram showing the relationship between the relative velocity vector and the inter-antenna baseline vector. [Figure 9] FIG. 10 is a block diagram showing an outline of the processing flow of a scale factor estimation device according to a second embodiment of the present invention. [Figure 10] 10 is a flowchart showing an example of processing by a scale factor estimation device according to a second embodiment of the present invention. [Figure 11] FIG. 2 is a schematic diagram showing an error included in the output of a gyro. DETAILED DESCRIPTION OF THE INVENTION

[0022] [First embodiment] A first embodiment of the present invention will be described in detail below with reference to the drawings. Fig. 1 is a block diagram showing an example of the configuration of a scale factor estimation device 10 according to this embodiment. The scale factor estimation device 10 includes a storage device 18 that stores data necessary for calculations by a calculation device 14 and the results of calculations by the calculation device 14, an image information processing unit 20 that extracts information such as white lines on a road from image data acquired by an imaging device 22, and a first GNSS antenna 28 and a second GNSS antenna 30 that detect the information extracted by the image information processing unit 20, the longitudinal speed of the vehicle body detected by a vehicle speed sensor 24, the angular velocity and acceleration of the azimuth angle of the vehicle body detected by an IMU 26 that is capable of detecting three-dimensional angular velocity and acceleration using a three-axis gyro and a three-directional accelerometer, and 1. The vehicle body 200 is configured with an input device 12 to which GNSS information received from satellites via each of the input devices 12 and map information stored in a map information database 34 is input, a calculation device 14 configured with a computer or the like that performs calculations to correct errors in the angular velocity output by the IMU 26 based on the input data input from the input device 12 and the data stored in the storage device 18, a V2X communication unit 36 ​​capable of wireless communication with the outside world of the vehicle body 200, and a display device 16 configured with a CRT, LCD or the like that can display the calculation results, etc., by the calculation device 14.

[0023] The imaging device 22 according to this embodiment is an in-vehicle camera or the like, and, as an example, analyzes image information of the vehicle's surroundings acquired by photographing to detect white lines on the road, buildings, etc. as edges. The IMU 26 is an inertial measurement unit capable of detecting three-axial angular velocities (pitch rate, roll rate, yaw rate) and three-axial accelerations (longitudinal acceleration, lateral acceleration, vertical acceleration) that indicate the behavior of the vehicle while traveling.

[0024] FIG. 2 is a schematic diagram showing an example of the arrangement of the first GNSS antenna 28 and the second GNSS antenna 30 in this embodiment.

[0025] As shown in FIG. 2 , in this embodiment, the first GNSS antenna 28 and the second GNSS antenna 30 are arranged parallel to the X-axis direction, which is the fore-and-aft direction of the vehicle body 200, at a distance greater than or equal to a predetermined distance so that differential information between the first GNSS antenna 28 and the second GNSS antenna 30 can be acquired. Generally, the longer the distance between the two antennas, the more accurate the azimuth angle estimation. Therefore, the predetermined distance between the first GNSS antenna 28 and the second GNSS antenna 30 is, for example, a value close to the distance from the front end to the rear end of the roof of the vehicle body 200, in order to maximize the baseline length of the first GNSS antenna 28 and the second GNSS antenna 30. Specifically, the predetermined distance is preferably at least 50 cm. In this embodiment, it is not necessary to arrange the first GNSS antenna 28 and the second GNSS antenna 30 parallel to the X-axis direction, which is the fore-and-aft direction of the vehicle body 200. Therefore, for example, the first GNSS antenna 28 and the second GNSS antenna 30 may be arranged diagonally across the roof. With this arrangement, the baseline lengths of the first GNSS antenna 28 and the second GNSS antenna 30 can be made longer than when the first GNSS antenna 28 and the second GNSS antenna 30 are arranged parallel to the X-axis direction, which is the longitudinal direction of the vehicle body 200. As will be described later, in this embodiment, differential information between the first GNSS antenna 28 and the second GNSS antenna 30 at the same time is used, making it possible to obtain azimuth angle information of the vehicle body 200 with high accuracy, independent of the vehicle speed or the traveling direction of the vehicle body 200. Details of estimating the amount of change in azimuth angle based on information obtained by the first GNSS antenna 28 and the second GNSS antenna 30 will be described later.

[0026] 3 is a block diagram showing an example of a specific configuration of the arithmetic device 14. The arithmetic device 14 is a type of computer, and includes a CPU (Central Processing Unit) 14B, a ROM (Read Only Memory) 14A, a RAM (Random Access Memory) 14C, and an input / output port 14D.

[0027] In the arithmetic device 14, the CPU 14B, ROM 14A, RAM 14C, and input / output port 14D are connected to one another via various buses such as an address bus, a data bus, and a control bus. The input / output port 14D is connected to various input / output devices such as the input device 12, a storage device 18 such as a hard disk (HDD), a display device 16, and a V2X communication unit 36.

[0028] A scale factor estimation program for estimating a scale factor is installed in the storage device 18. In this embodiment, the CPU 14B executes the scale factor estimation program to perform scale factor estimation. The CPU 14B also displays the processing results of the scale factor estimation program on the display device 16. There are several methods for installing the scale factor estimation program of this embodiment into the arithmetic device 14. For example, the scale factor estimation program may be stored on a CD-ROM, DVD, or the like together with a setup program, and the scale factor estimation program may be installed in the storage device 18 by inserting the disk into a disk drive or the like, which is an input / output device, and executing the setup program on the CPU 14B. Alternatively, the scale factor estimation program may be installed in the storage device 18 by communicating with another information processing device connected to the arithmetic device 14 via a public telephone line or a network.

[0029] Next, a description will be given of various functions realized by execution of the scale factor estimation program by the CPU 14B of the arithmetic device 14. The scale factor estimation program functions as a GNSS azimuth angle estimation function that estimates the azimuth angle of the vehicle body 200 from the relative positioning results of the first GNSS antenna 28 and the second GNSS antenna 30, a GNSS azimuth angle change amount calculation function that calculates the amount of azimuth angle change by obtaining information on the estimated azimuth angle at predetermined time intervals, a turning state determination function that determines the turning state of the vehicle body 200 based on the output of the yaw rate gyro provided in the IMU 26, an azimuth angle change amount calculation function that calculates the amount of azimuth angle change of the vehicle body 200 based on the output of the yaw rate gyro, and a scale factor estimation function that estimates a scale factor by comparing the amount of azimuth angle change calculated using the GNSS with the amount of azimuth angle change calculated based on the output of the yaw rate gyro. The CPU 14B executes a scale factor estimation program to function as a GNSS azimuth angle estimation unit 40, a GNSS azimuth angle change amount calculation unit 42, a turning state determination unit 44, an azimuth angle change amount calculation unit 46, and a scale factor estimation unit 48.

[0030] Fig. 4(A) is a schematic diagram showing the direction of movement of the respective positions of the first GNSS antenna 28 and the second GNSS antenna 30 in a vehicle body 200 making a right turn, and Fig. 4(B) is a schematic diagram showing an example of changes in, for example, the position of the center of gravity of the vehicle body 200 and the azimuth angles of the first GNSS antenna 28 and the second GNSS antenna 30. Fig. 4(B) shows a vehicle body azimuth angle 110 indicating the azimuth angle of the vehicle body 200, a first GNSS antenna azimuth angle 112 indicating the azimuth angle of the first GNSS antenna 28, and a second GNSS antenna azimuth angle 114 indicating the azimuth angle of the second GNSS antenna 30.

[0031] As shown in Figure 4(A), during a right turn, the first GNSS antenna 28 moves in a different direction as indicated by a first velocity vector 70, and the second GNSS antenna 30 moves in a different direction as indicated by a second velocity vector 72. As a result, in Figure 4(B), when the vehicle body 200 starts to turn right, the second GNSS antenna azimuth angle 114 changes before the vehicle body azimuth angle 110 and the first GNSS antenna azimuth angle 112, and then the vehicle body azimuth angle 110 and the first GNSS antenna azimuth angle 112 change in that order. The second GNSS antenna azimuth angle 114 stops changing before the vehicle body 200 finishes turning right, and then the vehicle body azimuth angle 110 and the first GNSS antenna azimuth angle 112 stop changing in that order.

[0032] 4(A) and 4(B), in azimuth estimation using GNSS, a phenomenon occurs in which the azimuth angles indicated by the first GNSS antenna azimuth angle 112 and the second GNSS antenna azimuth angle 114 deviate from the vehicle body azimuth angle 110 while the vehicle body 200 is turning. The magnitude of such azimuth angle deviation varies with changes in the speed and turning radius of the vehicle body 200. Therefore, with only one GNSS antenna, it is difficult to accurately estimate the change in azimuth angle (turning angle) of the vehicle body 200.

[0033] However, in the present embodiment, relative positioning using each of the first GNSS antenna 28 and the second GNSS antenna 30 is not affected by azimuth deviation in the direction of travel of the antenna that occurs depending on the installation position of the antenna as shown in Figures 4(A) and 4(B), and therefore the azimuth angle of the vehicle body 200 can be calculated with high accuracy.

[0034] 5 is a block diagram showing an outline of the processing flow of the scale factor estimation device 10 according to this embodiment. In block B100, information received by the first GNSS antenna 28 is acquired by the first GNSS receiver included in the input device 12, and in block B102, information received by the second GNSS antenna 30 is acquired by the second GNSS receiver included in the input device 12.

[0035] In block B104, the GNSS azimuth angle estimator 40 estimates the azimuth angle of the vehicle body 200 based on the information acquired by each of the first GNSS receiver and the second GNSS receiver.

[0036] In block B104, the azimuth angle of the vehicle body 200 is estimated by relative positioning using differential information between the first GNSS antenna 28 and the second GNSS antenna 30 at a certain time. To obtain the relative positioning result in block B104, the first GNSS antenna 28 and the second GNSS antenna 30 may individually obtain positioning results at the same time and then take the difference. Alternatively, the relative positioning result may be obtained by performing a relative positioning calculation using a technology such as D-GNSS (Differential Global Navigation Satellite System), which uses the difference in pseudorange information between the first GNSS antenna 28 and the second GNSS antenna 30, or MB-RTK (Moving Base Real Time Kinematic), which uses the difference in carrier phase information. The azimuth angle information of the vehicle body 200 can be obtained from the relative positioning result.

[0037] Block B106 calculates the amount of change in azimuth angle by obtaining information on the azimuth angle estimated at predetermined time intervals in block B104. Since the scale factor cannot be detected accurately unless the vehicle is turning and the yaw rate value becomes large to a certain extent, the calculation of the amount of change in azimuth angle in block B106 is performed according to the determination result of the turning state determination unit 44 shown in block B110, which will be described later.

[0038] The output of the yaw rate gyro is input to block B108. The bias error of the output of the yaw rate gyro input to block B108 has been corrected in advance.

[0039] In block B110, the turning state determination unit 44 determines whether the yaw rate output by the yaw rate gyro is equal to or greater than a predetermined threshold. The yaw rate threshold is specifically determined through simulations at the design stage or experiments using an actual vehicle. The determination result in block B110 is output to blocks B106 and B112.

[0040] In block B112, the azimuth angle change amount calculation unit 46 estimates the amount of change in the azimuth angle of the vehicle body 200 based on the yaw rate output by the yaw rate gyro. The instantaneous amount of change in the azimuth angle indicated by the output of the yaw rate gyro is the yaw rate itself, and the amount of change in the azimuth angle of the vehicle body 200 can be calculated by integrating the yaw rate over a predetermined time interval. The predetermined time interval in block B112 is the same as the predetermined time interval in block B106.

[0041] In block B114, the scale factor estimation unit 48 compares the amount of change in azimuth angle due to the yaw rate gyro with the amount of change in azimuth angle based on GNSS relative positioning, and corrects the scale factor. Because the scale factor is related to the yaw rate value as a coefficient, it is estimated, for example, by calculating the ratio of the amount of change in azimuth angle due to the yaw rate gyro to the amount of change in azimuth angle based on GNSS relative positioning, which is a reference value, and the output of the IMU 26 is corrected with the estimated scale factor.

[0042] 6 is a flowchart showing an example of processing by the scale factor estimation device 10 according to this embodiment. In step S100, the bias error of the yaw rate gyro is estimated, and the output of the yaw rate gyro is corrected using the estimated bias error. The bias error may be corrected so that the angular velocity indicated by the yaw rate gyro becomes zero when the vehicle body 200 equipped with the IMU 26 is stationary. More specifically, when it is estimated that the vehicle is traveling substantially straight ahead based on orientation information acquired using GNSS or the like, the yaw rate output by the IMU 26 is detected as a bias error, and the output of the IMU 26 is corrected using the detected bias error.

[0043] In step S102, it is determined whether or not the vehicle body 200 has started turning. When the vehicle body 200 starts turning, the yaw rate output by the yaw rate gyro of the IMU 26 changes, and if the yaw rate output by the yaw rate gyro is equal to or greater than a predetermined threshold, it is determined that the vehicle body 200 has started turning. The yaw rate used for the determination may be a differential value of the azimuth angle of the vehicle body 200 acquired using GNSS or the like. If it is determined in step S102 that the vehicle body 200 has started turning, the procedure proceeds to step S104, and if it is determined that the vehicle body 200 has not started turning, the determination of whether or not the vehicle body 200 has started turning continues.

[0044] In step S104, yaw rate gyro azimuth angle change amount estimation is performed to estimate the azimuth angle change amount of the vehicle body 200 based on the output of the yaw rate gyro. In step S104, the yaw rate output by the yaw rate gyro is integrated at predetermined time intervals to calculate the azimuth angle change amount of the vehicle body 200.

[0045] In step S106, the azimuth angle of the vehicle body 200 is estimated based on information acquired by each of the first GNSS antenna 28 and the second GNSS antenna 30, and the amount of change in the azimuth angle is calculated. Specifically, the azimuth angle of the vehicle body 200 is estimated by relative positioning using differential information between the first GNSS antenna 28 and the second GNSS antenna 30 at a certain time. The relative positioning result may be obtained by calculating the difference between positioning results acquired individually at the same time by the first GNSS antenna 28 and the second GNSS antenna 30. Furthermore, the relative positioning result may be obtained by performing a relative positioning calculation using a technique such as D-GNSS, which performs high-precision positioning by calculating the difference between the information acquired by the first GNSS antenna 28 and the information acquired by the second GNSS antenna 30 to cancel out errors common to both antennas, or MB-RTK, which performs positioning based on the path difference of the carrier waves calculated from the phase difference of the carrier waves between the first GNSS antenna 28 and the second GNSS antenna 30. Since the distance between the first GNSS antenna 28 and the second GNSS antenna 30 does not change, the estimation accuracy of the azimuth angle can be improved by using this distance as a constraint. In step S106, the azimuth angle of the vehicle body 200 is calculated from the relative positioning result, and the amount of change in the azimuth angle of the vehicle body 200 is found from the difference between the azimuth angles estimated at predetermined time intervals.

[0046] In step S108, the amount of change in azimuth angle due to the yaw rate gyro is compared with the amount of change in azimuth angle based on GNSS relative positioning, and the scale factor is corrected. As described above, the scale factor is related to the yaw rate value as a coefficient, and therefore can be estimated by calculating the ratio between the amount of change in azimuth angle based on GNSS relative positioning and the amount of change in azimuth angle due to the yaw rate gyro. Then, the output of the IMU 26 is corrected using the estimated scale factor.

[0047] In step S110, it is determined whether the vehicle body 200 has finished turning. Specifically, if the yaw rate output by the yaw rate gyro is less than the above-mentioned predetermined threshold, it is determined that the vehicle body 200 has finished turning. The yaw rate used for the determination may be a differential value of the azimuth angle of the vehicle body 200 acquired using GNSS or the like. If it is determined in step S110 that the vehicle body 200 is continuing to turn, the procedure proceeds to step S104, and estimation of the amount of change in azimuth angle and correction of the scale factor are continued again. If it is determined in step S110 that the vehicle body 200 has finished turning, the process of correcting the scale factor error is terminated.

[0048] As described above, according to the scale factor estimation device 10 of this embodiment, by using two GNSS antennas, the first GNSS antenna 28 and the second GNSS antenna 30, and receivers corresponding to these antennas, relative positioning becomes possible, and the estimation accuracy of the amount of change in azimuth angle referenced for scale factor correction is improved.

[0049] Estimation of the azimuth angle by GNSS relative positioning is not affected by wheel slip due to sharp turns or the side slip angle relative to the vehicle body 200, which also ensures a high level of accuracy in estimating the azimuth angle.

[0050] According to the scale factor estimation device 10 of this embodiment, the amount of change in azimuth angle that is referenced for scale factor correction can be estimated with high accuracy, and therefore the scale factor of the yaw rate gyro included in the IMU 26 can be estimated and corrected with higher accuracy, thereby improving the accuracy of estimating the orientation of the vehicle body 200.

[0051] [Second embodiment] Next, a second embodiment of the present invention will be described. The configuration of the scale factor estimation device according to this embodiment is similar to that of the scale factor estimation device 10 according to the first embodiment, so the same components as those in the first embodiment are denoted by the same reference numerals and detailed descriptions thereof will be omitted.

[0052] As described above, in the first embodiment, the scale factor of the yaw rate gyro is corrected by the amount of change in azimuth angle estimated by relative positioning using two GNSS antennas, the first GNSS antenna 28 and the second GNSS antenna 30.

[0053] In this embodiment, the scale factor of the yaw rate gyro is corrected using the azimuth angular velocity calculated based on the relative velocity vectors of the first GNSS antenna 28 and the second GNSS antenna 30.

[0054] FIG. 7 is a block diagram showing an example of a specific configuration of the arithmetic unit 14 in this embodiment. In this embodiment, the CPU 14B executes a scale factor estimation program different from that in the first embodiment, thereby correcting the scale factor of the yaw rate gyro in a manner different from that in the first embodiment. Therefore, various functions realized by the CPU 14B executing the scale factor estimation program according to this embodiment will be described below. The scale factor estimation program according to this embodiment functions as a GNSS azimuth velocity estimation function that estimates the azimuth velocity of the vehicle body 200 based on the relative velocity vectors of the first GNSS antenna 28 and the second GNSS antenna 30, a turning state determination function that determines the turning state of the vehicle body 200 based on the output of the yaw rate gyro provided in the IMU 26, and a scale factor estimation function that estimates the scale factor by comparing the azimuth velocity calculated using the GNSS with the output of the yaw rate gyro. By executing the scale factor estimation program, the CPU 14B functions as a GNSS azimuth velocity estimation unit 140, a turning state determination unit 142, and a scale factor estimation unit 144.

[0055] Figure 8(A) is a schematic diagram showing an example of the respective arrangements of the first GNSS antenna 28 and the second GNSS antenna 30 in a vehicle body 200 making a right turn, and an example of the velocity vectors of the first GNSS antenna 28 and the second GNSS antenna 30, and Figure 8(B) is an explanatory diagram showing the relationship between the relative velocity vector and the inter-antenna baseline vector.

[0056] In the past, when estimating azimuth velocity (yaw rate) using two GNSS antennas, attempts have been made to use the Doppler shift of the GNSS to determine the velocity vector of the GNSS antenna.

[0057] In this embodiment, the yaw rate of the vehicle body 200 is estimated from the difference between velocity vectors at the positions of two GNSS antennas or a relative velocity vector estimated from the difference in Doppler shift.

[0058] In this embodiment, for the sake of simplicity, a turning motion in a horizontal plane is considered, and the vectors shown below are determined in a coordinate system of north, south, east, and west.

[0059] As shown in Figure 8(A), when the vehicle body 200 is turning right, the baseline vector 74, which is the relative position between the first GNSS antenna 28 and the second GNSS antenna 30, the first velocity vector 70, which is the velocity vector of the first GNSS antenna 28, the second velocity vector 72, which is the velocity vector of the second GNSS antenna 30, and the relative velocity vector 76 are each set as follows. JPEG0007824133000001.jpg5294

[0060] Here, if the vehicle body 200 is considered to be a rigid body, the magnitude of the azimuthal velocity ω is given by the following equation (1). JPEG0007824133000002.jpg2340...(1)

[0061] The direction of the relative velocity vector 76 is perpendicular to the baseline vector 74 and the rotation axis vector. In this embodiment, since the turning is performed in a horizontal plane, the rotation axis vector is a vertical vector.

[0062] In this embodiment, the scale factor of the yaw rate gyro included in the IMU 26 is corrected with reference to the azimuth angular velocity ω calculated by the above equation (1).

[0063] 8(A) shows a case where the first GNSS antenna 28 and the second GNSS antenna 30 are arranged parallel to the longitudinal direction of the vehicle body 200, but the first GNSS antenna 28 and the second GNSS antenna 30 may also be arranged diagonally to the longitudinal direction of the vehicle body 200 or in the left-right direction perpendicular to the longitudinal direction of the vehicle body 200. Even if the first GNSS antenna 28 and the second GNSS antenna 30 are arranged in such a manner, the only thing that changes is the direction of the baseline vector 74 between the antennas, and the relationship between the above-mentioned vectors and the azimuth angular velocity ω still holds.

[0064] In this embodiment, for the sake of simplicity, the turning movement within a horizontal plane has been described. However, if the road surface or the vehicle body 200 is tilted, the relationship between each of the above-mentioned vectors and the azimuth angular velocity ω will similarly hold if the tilted rotation axis vector corresponding to the change in posture during turning is taken into consideration.

[0065] 9 is a block diagram showing an outline of the processing flow of the scale factor estimation device 10 according to this embodiment. In block B200, information received by the first GNSS antenna 28 is acquired by the first GNSS receiver included in the input device 12, and in block B202, information received by the second GNSS antenna 30 is acquired by the second GNSS receiver included in the input device 12.

[0066] In block B204, the GNSS azimuth angular velocity estimation unit 140 estimates the azimuth angular velocity ω of the vehicle body 200 based on the information acquired by each of the first GNSS receiver and the second GNSS receiver. As mentioned above, the scale factor cannot be detected accurately unless the vehicle is turning, when the yaw rate value becomes relatively large. Therefore, as shown in Fig. 10 which will be described later, the estimation of the azimuth angular velocity ω in block B204 may be performed in accordance with the determination result of the turning state determination unit 142 shown in block B208.

[0067] In block B206, the output of the yaw rate gyro is input to the following blocks B208 and B210. The output of the yaw rate gyro input in block B206 has been corrected for bias error in advance.

[0068] In block B208, the turning state determination unit 142 determines whether the yaw rate output by the yaw rate gyro is equal to or greater than a predetermined threshold. The yaw rate threshold is specifically determined through simulations at the design stage or experiments using an actual vehicle. The determination result in block B208 is output to block B210.

[0069] In block B210, the scale factor estimation unit 144 compares the yaw rate output by the yaw rate gyro with the amount of change in azimuth angle based on relative positioning of the azimuth angular velocity ω estimated using GNSS, and corrects the scale factor. Since the scale factor is related to the value of the yaw rate as a coefficient, it is estimated, for example, by calculating the ratio of the yaw rate output by the yaw rate gyro to the azimuth angular velocity ω estimated using GNSS, which is a reference value, and the output of the IMU 26 is corrected with the estimated scale factor.

[0070] 10 is a flowchart showing an example of processing by the scale factor estimation device 10 according to this embodiment. In step S200, the bias error of the yaw rate gyro is estimated, and the output of the yaw rate gyro is corrected using the estimated bias error. As described in the first embodiment, when the vehicle body 200 equipped with the IMU 26 is stationary, the bias error can be corrected so that the angular velocity indicated by the yaw rate gyro becomes zero.

[0071] In step S202, it is determined whether or not the vehicle body 200 has started turning. When the vehicle body 200 starts turning, the yaw rate output by the yaw rate gyro of the IMU 26 changes, and if the yaw rate output by the yaw rate gyro is equal to or greater than a predetermined threshold, it is determined that the vehicle body 200 has started turning. The yaw rate used for the determination may be an azimuth angular velocity ω estimated using GNSS or the like. If it is determined in step S202 that the vehicle body 200 has started turning, the procedure proceeds to step S204, and if it is determined that the vehicle body 200 has not started turning, the determination of whether or not the vehicle body 200 has started turning continues.

[0072] In step S204, information on the yaw rate, which is the output of the yaw rate gyro, is acquired.

[0073] In step S206, the azimuth angular velocity ω of the vehicle body 200 is estimated based on the information acquired by each of the first GNSS antenna 28 and the second GNSS antenna 30.

[0074] In step S208, the yaw rate output by the yaw rate gyro is compared with the azimuth angular velocity ω estimated using GNSS, and the scale factor is corrected. As described above, the scale factor is related to the yaw rate value as a coefficient, and therefore can be estimated by calculating the ratio between the azimuth angular velocity ω estimated using GNSS and the yaw rate output by the yaw rate gyro. Then, the output of the IMU 26 is corrected using the estimated scale factor.

[0075] In step S210, it is determined whether the vehicle body 200 has finished turning. Specifically, if the yaw rate output by the yaw rate gyro is less than the above-mentioned predetermined threshold, it is determined that the vehicle body 200 has finished turning. The yaw rate used for the determination may be the azimuth angular velocity ω estimated using GNSS or the like. If it is determined in step S210 that the vehicle body 200 is continuing to turn, the procedure proceeds to step S204, and estimation of the azimuth angular velocity ω and correction of the scale factor are continued again. If it is determined in step S210 that the vehicle body 200 has finished turning, the process of correcting the scale factor error is terminated.

[0076] As described above, according to the scale factor estimation device 10 of this embodiment, the scale factor of the yaw rate gyro can be corrected using the azimuth angular velocity ω calculated based on the relative velocity vectors of the first GNSS antenna 28 and the second GNSS antenna 30.

[0077] In this embodiment, the azimuth angular velocity ω, which can be directly compared with the yaw rate, which is the output of the yaw rate gyro, can be estimated based on the relative velocity vectors of the first GNSS antenna 28 and the second GNSS antenna 30. Therefore, the processing is simpler than in the first embodiment, in which the amount of change in azimuth angle is calculated from the azimuth angle of the vehicle body 200 estimated by relative positioning using the two GNSS antennas, the first GNSS antenna 28 and the second GNSS antenna 30, and faster calculation processing can be expected.

[0078] In the above embodiments, when the yaw rate output by the yaw rate gyro is equal to or greater than a predetermined threshold and it is determined that the vehicle body 200 has started turning, the amount of change in azimuth angle is calculated and the scale factor is estimated, but this is not limiting. The amount of change in azimuth angle may be constantly calculated based on the outputs of the GNSS and IMU 26, and the scale factor may be estimated based on the calculated amount of change in azimuth angle when it is determined that the vehicle body 200 has started turning.

[0079] Furthermore, since the effect of the scale factor on the output of the yaw rate gyro results in a slight error in the yaw rate, if the amount of change in the azimuth angle of the vehicle body 200 is small, there is a risk that the error will be buried in the noise components contained in the output of the yaw rate gyro. In order to clearly distinguish between such noise components and the scale factor, the scale factor of the yaw rate gyro may be estimated when the change in the azimuth angle of the vehicle body 200 calculated based on the output of the GNSS or IMU 26 becomes equal to or greater than a predetermined azimuth angle change threshold after the vehicle body 200 starts turning. The predetermined azimuth angle change threshold is specifically determined through simulations at the design stage or experiments using an actual vehicle, and is, for example, approximately 30 degrees.

[0080] In the above embodiments, the scale factor estimation process executed by the CPU by loading software (programs) may be executed by various processors other than the CPU. Examples of such processors include programmable logic devices (PLDs) such as field-programmable gate arrays (FPGAs), whose circuit configuration can be changed after fabrication, and dedicated electrical circuits such as application-specific integrated circuits (ASICs), which are processors with circuit configurations specifically designed to execute specific processes. The scale factor estimation process may be executed by one of these processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). The hardware structure of these processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor devices.

[0081] In addition, in each of the above embodiments, the scale factor estimation program is described as being stored (installed) in advance in a ROM or storage, but this is not limiting. The program may be provided in a form recorded on a non-transitory recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), or a USB (Universal Serial Bus) memory. The program may also be downloaded from an external device via a network.

[0082] The "inertial measurement unit" recited in the claims corresponds to the "IMU 26" recited in the detailed description of the invention, the "inertial azimuth angle change amount calculation unit" recited in the claims corresponds to the "azimuth angle change amount calculation unit 46" recited in the detailed description of the invention, the "satellite positioning azimuth angle change amount calculation unit" recited in the claims corresponds to the "GNSS azimuth angle change amount calculation unit 42, 140" recited in the detailed description of the invention, the "turning state determination unit" recited in the claims corresponds to the "turning state determination unit 46, 142" recited in the detailed description of the invention, and the "scale factor estimation unit" recited in the claims corresponds to the "scale factor estimation unit 48, 144" recited in the detailed description of the invention.

[0083] (Additional note 1) Memory and at least one processor coupled to said memory; Including, The processor: an amount of change in azimuth angle in the yaw direction of the vehicle body is calculated based on the output of an inertial measurement unit mounted on the vehicle body; an amount of change in azimuth angle in the yaw direction of the vehicle body is calculated from differential information of a first antenna and a second antenna that are installed on the vehicle body at a distance equal to or greater than a predetermined distance and each capable of acquiring positioning information from a satellite; a scale factor of the inertial measurement unit is estimated by referring to the amount of change in azimuth angle calculated by the satellite positioning azimuth angle change calculation unit relative to the amount of change in azimuth angle calculated by the inertial azimuth angle change calculation unit; and an output of the inertial measurement unit is corrected based on the estimated scale factor. The scale factor estimation device is configured as follows. [Explanation of symbols]

[0084] 10 Scale factor estimator 12 Input Devices 14 Arithmetic unit 14A ROM 14B CPU 14C RAM 14D Input / Output Ports 16 Display device 18 Storage device 26 IMU 28 1st GNSS Antenna 30 Second GNSS Antenna 34 Map Information Database 40 Azimuth estimation part 42 Azimuth angle change calculation unit 44 Turning state determination unit 46 Azimuth angle change calculation unit 48 Scale factor estimation unit 70 First velocity vector 72 Second velocity vector 74 Baseline Vector 76 Relative Velocity Vector 140 Azimuth velocity estimation part 142 Turning state determination unit 144 Scale Factor Estimation Unit 200 body

Claims

1. an inertial azimuth angle change amount calculation unit that calculates an azimuth angle change amount in the yaw direction of the vehicle body based on an output of an inertial measurement unit mounted on the vehicle body; a satellite positioning azimuth angle change amount calculation unit that calculates a relative position between the first antenna and the second antenna based on one of the difference between positioning results obtained by a first antenna and a second antenna, each of which is installed on the vehicle body at a distance equal to or greater than a predetermined distance and capable of acquiring positioning information from a satellite, the difference between pseudoranges of signals received from a satellite by the first antenna and the second antenna, and the difference between phases of carrier waves of signals received from a satellite by the first antenna and the second antenna, and calculates an azimuth angle change amount in the yaw direction of the vehicle body from the time change of the calculated relative position, or calculates an azimuth angle change amount in the yaw direction of the vehicle body by dividing a relative velocity vector that is the difference between the velocity vector of the first antenna and the velocity vector of the second antenna by a baseline vector that is the relative position between the first antenna and the second antenna; a scale factor estimation unit that estimates a scale factor of the inertial measurement unit by referring to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit with respect to the azimuth angle change amount calculated by the inertial azimuth angle change amount calculation unit, and corrects the output of the inertial measurement unit based on the estimated scale factor; A scale factor estimator comprising:

2. a turning state determination unit that determines that the vehicle body is turning when the azimuth velocity in the yaw direction output by the inertial measurement unit is equal to or greater than a predetermined threshold value; 2. The scale factor estimation device according to claim 1, wherein the scale factor estimation unit estimates the scale factor of the inertial measurement unit based on an amount of change in azimuth angle in the yaw direction of the vehicle body calculated by each of the inertial azimuth angle change amount calculation unit and the satellite positioning azimuth angle change amount calculation unit when the turning state determination unit determines that the vehicle body is turning.

3. A scale factor estimation device as described in claim 2, wherein the scale factor estimation unit estimates the scale factor from the ratio of the azimuth angle change amount calculated by the inertial azimuth angle change amount calculation unit to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit when the turning state determination unit determines that the vehicle body is turning.

4. A scale factor estimation device as described in claim 2 or 3, wherein the scale factor estimation unit estimates the scale factor of the inertial measurement device based on the azimuth angle change in the yaw direction of the vehicle body calculated by each of the inertial azimuth angle change amount calculation unit and the satellite positioning azimuth angle change amount calculation unit when, from the time the turning state determination unit determines that the vehicle body has started turning, the change in the azimuth angle of the vehicle body obtained based on the azimuth angle change in the yaw direction of the vehicle body calculated by either the inertial azimuth angle change amount calculation unit or the satellite positioning azimuth angle change amount calculation unit exceeds a predetermined azimuth angle change amount threshold.

5. An inertial azimuth angle change amount calculation step of calculating an azimuth angle change amount in the yaw direction of the vehicle body based on the output of an inertial measurement device mounted on the vehicle body; a satellite positioning azimuth angle change calculation step of calculating a relative position between the first antenna and the second antenna based on one of the difference between positioning results obtained by a first antenna and a second antenna that are installed on the vehicle body at a distance equal to or greater than a predetermined distance and each capable of acquiring positioning information from a satellite, the difference between pseudoranges of signals received from a satellite by the first antenna and the second antenna, and the difference between phases of carrier waves of signals received from a satellite by the first antenna and the second antenna, and calculating an amount of change in azimuth angle in the yaw direction of the vehicle body from the time change of the calculated relative position, or calculating an amount of change in azimuth angle in the yaw direction of the vehicle body by dividing a relative velocity vector that is the difference between the velocity vector of the first antenna and the velocity vector of the second antenna by a baseline vector that is the relative position between the first antenna and the second antenna; a scale factor estimation step of estimating a scale factor of the inertial measurement unit by referring to the azimuth angle change amount calculated in the satellite positioning azimuth angle change amount calculation step with respect to the azimuth angle change amount calculated in the inertial azimuth angle change amount calculation step, and correcting the output of the inertial measurement unit based on the estimated scale factor; A scale factor estimation method in which a computer executes a process including the steps of:

6. A computer, an inertial azimuth angle change amount calculation unit that calculates an amount of change in azimuth angle in the yaw direction of the vehicle body based on the output of an inertial measurement unit mounted on the vehicle body; and an inertial azimuth angle change amount calculation unit that calculates a relative position between the first antenna and the second antenna based on any of the difference between positioning results obtained by a first antenna and a second antenna that are installed on the vehicle body at a distance equal to or greater than a predetermined distance and each capable of acquiring positioning information from a satellite, the difference between pseudoranges of signals received from satellites by the first antenna and the second antenna, and the difference between phases of carrier waves of signals received from satellites by the first antenna and the second antenna, and calculates an amount of change in azimuth angle in the yaw direction of the vehicle body from the change over time of the calculated relative position; or a satellite positioning azimuth angle change amount calculation unit that calculates an azimuth angle change amount in the yaw direction of the vehicle body by dividing a relative velocity vector that is the difference between the velocity vector of the first antenna and the velocity vector of the second antenna by a baseline vector that is the relative position between the first antenna and the second antenna, and a scale factor estimation program that causes the program to function as a scale factor estimation unit that estimates a scale factor of the inertial measurement unit by referring to the azimuth angle change amount calculated by the satellite positioning azimuth angle change amount calculation unit for the azimuth angle change amount calculated by the inertial azimuth angle change amount calculation unit, and corrects the output of the inertial measurement unit based on the estimated scale factor.

Images