JP6178776B2 - Speed measuring device - Google Patents

Description

  The present invention relates to a speed measurement device that is mounted on a vehicle and measures the speed of the vehicle.

IMU (Internal Measurement Unit) mounted on the vehicle
And a GPS (Global Positioning System) receiver are used to measure the speed of the vehicle with high accuracy (see, for example, Patent Documents 1 and 2).

  Here, from the IMU, data updated in substantially real time is output in synchronization with a clock of, for example, 100 Hz. However, in the IMU, acceleration is directly measured, and the velocity is measured by integrating the acceleration. For this reason, the acceleration measured by the IMU is accumulated sequentially with the offset error of the acceleration measured directly by the IMU. Therefore, if the speed data is left uncorrected, there is a risk that the data gradually moves away from the true value and gradually includes data having a large error.

  On the other hand, as one of the speed measurement methods in the GPS receiver, a measurement method for measuring the speed (hereinafter sometimes referred to as “Doppler speed”) based on the Doppler shift amount of the carrier wave of the radio wave transmitted from the GPS satellite. It has been known. If this measurement method is adopted, highly accurate speed measurement can be performed. However, for example, when the radio wave can be received only from less than the prescribed number (4) of GPS satellites due to trees, buildings, etc., accurate speed measurement cannot be performed. There are restrictions on the conditions under which speed measurement can be performed.

  Patent Documents 1 and 2 disclose a configuration in which output speed data is generated by correcting speed data derived from an IMU based on Doppler speed data derived from a GPS receiver. Specifically, in Patent Documents 1 and 2, in order to generate difference data between speed data derived from a GPS receiver and output speed data, and to correct speed data derived from IMU based on the difference data. A speed measuring device has been proposed that calculates output data and generates output speed data by correcting IMU-derived speed data using the correction data.

  Here, Patent Document 1 evaluates the reliability of GPS-derived Doppler velocity data, calculates a coefficient, and according to the coefficient, strongly corrects the IMU-derived velocity data when the reliability is high. It has been proposed to reduce the correction amount when the degree is low.

  Patent Document 2 adopts a GPS receiver in which speed data derived from a GPS receiver is synchronized with a clock of, for example, 20 Hz, and speed data can be obtained only at a slower cycle than the IMU. A configuration in which interpolation processing is performed on speed data derived from a receiver is disclosed.

JP 2012-42318 A JP 2013-170904 A

  Here, in the case of the speed measurement device disclosed in the above-mentioned Patent Documents 1 and 2, when the reliability of the speed data derived from the GPS receiver is high, highly accurate output speed data is generated. However, when the reliability of the speed data from the GPS receiver is low, such as when only radio waves from a GPS satellite less than the specified number can be received, offset errors are accumulated sequentially. Output speed data in which the disadvantages of IMU-derived speed data appear as they are can be obtained.

  The present invention has been made in view of the above circumstances, and an object thereof is to provide a speed measuring device capable of generating speed data corrected with high accuracy even at a timing when only speed data with low reliability can be obtained from a GPS receiver. And

The speed measuring device of the present invention that achieves the above-described object provides:
A gain correction unit for correcting the gain of the CAN speed representing the vehicle speed based on the primary IMU speed representing the vehicle speed derived from the IMU corrected based on the GPS speed derived from the GPS receiver;
A delay estimator for estimating a delay time at which an error between the primary IMU speed and the CAN speed is minimized with respect to the primary IMU speed by a correlation calculation between the primary IMU speed and the CAN speed ;
A speed correction unit that generates a secondary IMU speed by correcting a speed derived from I MU ,
A synchronization processing unit that generates a delayed secondary IMU speed that is temporally synchronized with the CAN speed by delaying the secondary IMU speed by a delay time of the CAN speed estimated by the delay estimation unit;
A differentiator for generating a difference between the CAN speed corrected by the gain correction unit and the delayed secondary IMU speed generated by the synchronization processing unit;
A Kalman gain calculating unit that inputs a difference generated by the differentiator and generates a Kalman gain that corrects a current velocity derived from the IMU based on an equation of motion; and
And a speed correction unit having a correction speed generation unit that generates the secondary IMU speed by correcting the current speed derived from the IMU using the Kalman gain calculated by the Kalman gain calculation unit. To do.

  The speed measuring device of the present invention corrects the gain of the CAN speed measured based on the rotation of the wheel and the like flowing in a CAN (Control Area Network) which is a network built in the vehicle, and delays the CAN speed. Generate a secondary IMU speed by estimating the time and correcting the IMU-derived speed based on the CAN speed with the gain corrected and considering that the CAN speed is delayed by the estimated delay time Is.

  For this reason, even when the reliability of the primary IMU speed corrected with the GPS-derived speed data is low, a highly accurate secondary IMU speed can be obtained. Therefore, highly accurate speed data is always generated.

  Here, in the speed measurement device of the present invention, the gain correction unit is a timing at which a primary IMU speed corrected based on a GPS speed determined based on a predetermined determination criterion is input. And it is characterized in that the gain of the CAN speed is corrected on condition that the vehicle is traveling at a constant speed.

  If the correction is made based on the GPS speed and the vehicle is traveling at a constant speed, the gain of the CAN speed can be corrected with high accuracy based on the primary IMU speed. In this way, the gain of the CAN speed is corrected using the primary IMU speed corrected based on the GPS speed, and the speed derived from the IMU is corrected using the CAN speed with the gain corrected. Thus, a highly accurate secondary IMU speed is generated even at a timing when the reliability of the GPS speed is low.

  Further, in the speed measurement device of the present invention, the delay estimation unit is a timing at which a primary IMU speed, which is determined based on a predetermined determination criterion and is accurately corrected based on a GPS speed, is input. It is preferable that the CAN speed delay time is estimated on the condition that the vehicle speed is changing.

  If the primary IMU speed is corrected based on the GPS speed and the vehicle speed is changing, the CAN speed delay time can be estimated with high accuracy based on the primary IMU speed. . By synchronizing the secondary IMU speed with the CAN speed using the delay time estimated in this way, a highly accurate secondary IMU speed is generated even at a timing when the reliability of the GPS speed is low.

Furthermore, in the speed measurement device of the present invention, the speed correction unit is
A synchronization processing unit that synchronizes with the CAN speed by delaying the secondary IMU speed by a delay amount corresponding to a delay time of the CAN speed estimated by the delay estimation unit;
A difference value calculation unit that calculates a difference value between the CAN speed gain-corrected by the gain correction unit and the secondary IMU speed delayed by the synchronization processing unit;
A Kalman gain calculation unit that generates a correction value for calculating the secondary IMU speed by inputting the difference value calculated by the difference value calculation unit and calculating the Kalman gain;
It is preferable to include a correction speed generation unit that generates a secondary IMU speed by correcting the IMU-derived speed based on the correction value generated by the Kalman gain calculation unit.

  By providing this configuration, the secondary IMU speed corrected with high accuracy from the speed derived from the IMU is generated.

  Furthermore, the speed measurement device of the present invention may further include a primary IMU speed generation unit that generates a primary IMU speed by correcting a speed derived from the IMU based on the GPS speed.

  The speed measurement device of the present invention may receive a primary IMU speed from another device, but as described above, a primary IMU speed generation unit that generates a primary IMU speed is incorporated. May be.

  In the speed measurement device of the present invention, the speed correction unit may generate a secondary IMU speed by correcting a speed derived from the IMU generated independently of the primary IMU speed, or The speed correction unit may generate a secondary IMU speed by correcting the primary IMU speed.

  According to the present invention described above, speed data corrected with high accuracy can be generated even at a timing when only speed data with low reliability can be obtained from a GPS receiver.

It is a block diagram showing the speed measuring device as a 1st embodiment of the present invention. FIG. 2 is a block diagram illustrating an internal configuration of a gain correction unit illustrated by one block in FIG. 1. It is a block diagram showing the internal structure of the delay estimation part shown by one block in FIG. It is the schematic diagram which showed the processing content in a delay part and an error calculating part. FIG. 2 is a block diagram showing an internal configuration of a speed correction unit shown by one block in FIG. 1. It is the figure which showed the last IMU speed output from the speed measuring device shown in FIG. 1 corresponding to satellite acquisition on and off. It is a block diagram showing the speed measuring device as 2nd Embodiment of this invention. It is a block diagram which shows the structure of a primary IMU speed generation apparatus.

  Embodiments of the present invention will be described below.

  FIG. 1 is a block diagram showing a speed measuring apparatus as a first embodiment of the present invention.

  The speed measurement device 100A includes a gain correction unit 10, a delay estimation unit 20, a speed correction unit 30, and a changeover switch 40.

  The gain correction unit 10 receives the CAN speed and the primary IMU speed, and performs the CAN speed gain correction so that the CAN speed becomes the same as the primary IMU speed.

  Here, the CAN speed is a speed obtained from a speed signal that is measured based on the rotation of the wheels of the vehicle and flows to a CAN (Control Area Network) built in the vehicle. This CAN speed is approximately proportional to the exact speed of the vehicle, but is an inaccurate speed and has a response delay.

  On the other hand, the primary IMU speed is a speed derived from an IMU (Internal Measurement Unit) mounted on the vehicle, and is corrected by a speed derived from a GPS (Global Positioning System) receiver mounted on the same vehicle. . The IMU is called an inertial measurement device, and incorporates a three-axis gyro sensor and a three-axis acceleration sensor, and requires a three-dimensional angular velocity and acceleration in three directions. From the IMU, these angular velocities and accelerations are measured and output in real time and with a period of, for example, 10 ms. The angular velocity and acceleration output from the IMU are the angular velocity and acceleration with reference to the traveling direction of the vehicle on which the IMU is mounted.

  Further, as one of the speed measurement methods in the GPS receiver, a measurement method for measuring the speed (hereinafter sometimes referred to as “Doppler speed”) based on the Doppler shift amount of the carrier wave of the radio wave transmitted from the GPS satellite. It has been known. If this measurement method is employed, highly accurate speed measurement can be performed. However, as described above, for example, when the radio wave can be received only from a predetermined number (4) of GPS satellites due to trees, buildings, etc., accurate speed measurement becomes impossible. There are restrictions on the conditions under which accurate speed measurement can be performed. From this GPS receiver, each speed of the ground axis reference (north direction, east direction, downward direction) is calculated instead of the vehicle traveling direction reference. In addition, the speed obtained with this GPS receiver is, for example, 100 ms or less, and the update speed is slower than that of the IMU, and as an example, is accompanied by a time delay of about 140 to 150 ms and jitter (time delay fluctuation).

  The primary IMU speed described here is the speed in the traveling direction of the vehicle, which is calculated based on the IMU-derived acceleration, etc., and which is real-time secured and corrected based on the speed derived from the GPS receiver. Say. The method of correcting the speed calculated based on the acceleration derived from the IMU based on the speed derived from the GPS receiver is not limited to a specific correction calculation. An example will be described. The IMU acceleration is also the acceleration in the traveling direction of the vehicle output from the above-mentioned IMU.

  In addition, information on “gradient”, “satellite acquisition on / off”, and “reverse” is further input to the velocity measuring device shown in FIG.

  “Gradient” is information representing the gradient of the currently traveling road.

  This “gradient” information may be information derived from an IMU, information derived from a GPS receiver, or information obtained by integrating both pieces of information. However, normally, information on “gradient” cannot be obtained from CAN.

  “Satellite capture on / off” is information indicating whether or not the GPS receiver can receive radio waves from a number of GPS satellites more than necessary for accurate speed measurement (on) or not (off). is there. In the present embodiment, this “satellite acquisition on / off” information is used as information on whether or not the IMU-derived speed is correctly corrected based on the GPS receiver-derived speed. However, the index for evaluating the reliability of the speed derived from the GPS receiver is not only the information of “satellite acquisition on / off”. For example, a Doppler accuracy index value representing an index of Doppler velocity accuracy calculated based on velocity dispersion in a GPS receiver is adopted, and whether or not the IMU-derived velocity is correctly corrected based on the GPS-derived velocity. The determination may be made based on the Doppler accuracy index value. Or you may determine whether it correct | amends correctly based on the quality factor proposed in the above-mentioned patent document 1. FIG.

  FIG. 2 is a block diagram showing the internal configuration of the gain correction unit shown by one block in FIG.

  The gain correction unit 10 corrects the gain of the CAN speed so that the CAN speed matches the primary IMU speed.

  The gain correction unit 10 includes a determination unit 11. In the determination unit 11, it is determined whether or not it is a timing at which an accurate correction coefficient α for gain correction of the CAN speed can be calculated in a correction coefficient calculation unit 13 described later.

  The determination unit 11 makes a determination based on the following determination condition, and outputs an enable signal EN at a timing at which an accurate correction coefficient α can be calculated.

-The vehicle is moving forward This is a criterion for excluding the timing when the vehicle is backing. Whether or not the vehicle is moving forward can be determined from “reverse” information flowing in the CAN.

-Both the CAN speed and the primary IMU speed indicate speeds of 10 km / h or higher. This is a criterion for aiming at the timing when the vehicle is traveling at a speed capable of correcting the CAN speed. The determination unit 11 receives the CAN speed and the primary IMU speed, and this determination criterion is performed based on them.

-Road slope is within ± 5% This is a criterion for aiming at the timing when driving on a horizontal road. This is determined based on “gradient = ten (vertical speed / horizontal speed)” information obtained from the IMU or GPS receiver.

-The reliability of the speed measured by the GPS receiver is high. This is a criterion for aiming at the timing at which the primary IMU speed accurately corrected by the speed derived from the GPS receiver is obtained. In the present embodiment, the determination is made based on the information of “satellite acquisition on / off”.

-Speed fluctuation is within ± 5% Since CAN speed has a large response delay, it is a criterion for aiming at constant speed driving where response delay does not become a problem. The determination unit 11 receives an acceleration calculated when the first-order IMU speed passes through the differentiator 12, and is determined based on the acceleration.

  The gain correction unit 10 shown in FIG. 2 includes a correction coefficient calculation unit 13, and an enable signal EN is sent to the correction coefficient calculation unit 13 at a timing when the determination unit 11 satisfies all of the above determination criteria. Is output. In the correction coefficient calculation unit 13, the correction coefficient α is repeatedly calculated while the enable signal EN is input. On the other hand, in the correction coefficient calculation unit 13, when the enable signal EN from the determination unit 11 is interrupted, the correction coefficient α calculated immediately before is maintained as it is.

The correction coefficient calculation unit 13 includes a differentiator 131. In the differencer 131, a first-order IMU speed that has been input and a CAN speed that has been gain-corrected by the correction unit 14 to be described later. A difference value V _e is calculated.

The difference value V _e is inputted to the cumulative value calculator 132, the cumulative value of the difference value V _e

Is calculated.

  Here, dt is a repetition cycle of the calculation (10 ms in this embodiment).

  In the present embodiment, the CAN speed and the primary IMU speed are input at 10 ms intervals in synchronization with the 100 Hz clock, and each calculation is calculated at 10 ms intervals.

  The accumulated value calculated by the accumulated value calculation unit 132 is amplified (attenuated) by a predetermined magnification (attenuation rate) in the amplifier (attenuator) 133, whereby the correction coefficient α is calculated. The calculated correction coefficient α is input to the multiplier 141 of the correction unit 14.

  In the multiplier 141, the input CAN speed is multiplied by the correction coefficient α and input to the adder 142. The adder 142 adds the input CAN speed and the CAN speed multiplied by the correction coefficient α to generate a CAN speed after gain correction.

That is, the correction unit 14 calculates CAN speed (gain correction) = (1 + α) · CAN speed.

  In the gain correction unit 10 shown in FIG. 2, the CAN speed after gain correction is calculated in this way.

  FIG. 3 is a block diagram showing the internal configuration of the delay estimation unit shown by one block in FIG.

  The delay estimation unit 20 estimates the delay time of the CAN speed with reference to the primary IMU speed that ensures real-time characteristics.

  The delay estimation unit 20 is also provided with a determination unit 21 and a differentiator 22 that differentiates the primary IMU speed.

The determination unit 11 of the gain correction unit 10 is one condition for outputting the enable signal EN that the vehicle is traveling at a constant speed. However, the determination unit 21 of the delay estimation unit 20 uses acceleration or deceleration instead. One condition is that the timing is such that the acceleration is performed (for example, acceleration exceeds ± 3 m / s ² ). This is because the delay time cannot be estimated when traveling at a constant speed. The other determination conditions are the same as the determination conditions in the gain correction unit 10, and a duplicate description thereof is omitted here.

  The enable signal EN output from the determination unit 21 is input to the error calculation unit 24. In this error calculation unit 24, the CAN speed after gain correction and the primary IMU speed delayed by the delay unit 23 are input, and the error calculation described below is performed in response to the input of the enable signal EN. . Further, in the error calculation unit 24, when the enable signal EN is interrupted, the previous calculation result is held, and the calculation is resumed when the enable signal EN is input again.

  FIG. 4 is a schematic diagram showing processing contents in the delay unit 23 and the error calculation unit 24.

FIG. 4A shows data stored in the delay unit 23 and the like at a certain time. Here, a _i (i = 0, 1, 2,...) Represents the primary IMU speed inputted every 10 ms. a ₁ is input next to a ₀ (after 10 ms), then a ₂ is input (after 10 ms later), and the data with a larger i value of a _i is the data input later is there.

The delay unit 23 has 30 storage areas for the input data, and the data a ₀ input from the left side of the figure is shifted one by one to the right every 10 ms, as shown in FIG. At the time shown, the storage area has moved to the rightmost storage area of the delay unit 23.

At the time shown in FIG. 4A, the data a ₃₀ is stored in the leftmost storage area, and the data a ₂₉ is stored in the storage area on the right. And right now, the data a ₃₀ are input. In addition, b _i (i = 30, 31,...) Represents the CAN speed after gain correction, and data b ₃₀ is just input in synchronization with the primary IMU speed.

  As shown in FIG. 3, the error calculator 24 includes a subtractor 241, a square calculator 242, and an integrator 243.

  Returning to FIG. 4, the calculation processing in the error calculation unit 24 will be described.

3 includes the 30 differentiators 241_1, 241_2, 241_3,..., 241_30 shown in FIG. These 30 amino differentiator 241_1,241_2,241_3, ..., In 241_30, _{respectively, a 30 -b 30, a 29} -b 30, a 28 -b 30, ··· a 0 -b 30 is calculated The That is, the difference unit 241_1 calculates the difference between the two data a ₃₀ and b _{30 of} the same time input this time, and the difference unit 241_2 inputs the primary IMU speed a ₂₉ input one cycle before (10 ms before). And the CAN speed (gain correction) b ₃₀ inputted this time is calculated, and the differencer 241_3 calculates the primary IMU speed a ₂₈ inputted two cycles before (20 ms before) and the CAN speed inputted this time ( The difference from the gain correction) b ₃₀ is calculated. Similarly, in the subtractor 241_30, the primary IMU speed a ₀ input 30 cycles before (300 ms before) and the CAN speed (gain correction) b input this time are calculated. The difference from ₃₀ is calculated.

The square calculator 242 illustrated in FIG. 3 is an aggregate of the 30 square calculators 242_1, 242_2, 242_3,..., 242_30 illustrated in FIG. At the time shown in FIG. 4A, these 30 square calculators 242_1, 242_2, 242_3,..., 242_30 respectively have (a ₃₀ −b ₃₀ ) ² , (a ₂₉ −b ₃₀ ) ² , (A ₂₈ -b ₃₀ ) ² , ... (a ₀ -b ₃₀ ) ²
Is calculated.

  Further, the integrator 243 shown in FIG. 3 is an aggregate of 30 integrators 243_1, 243_2, 243_3,..., 243_30 shown in FIG. In the 30 integrators 243_1, 243_2, 243_3,..., 243_30, the square values of the errors output from the square calculators 242_1, 242_2, 242_3,. Is accumulated.

FIGS. 4B and 4C show calculation results after one cycle (10 ms) and two cycles (20 ms), respectively, from the time point shown in FIG. 4A. Thus, the calculation is performed for each period (every 10 ms), and the 30 integrators 243_1, 243_2, 243_3,.
... + (a ₃₀ -b ₃₀ ) ² + (a ₃₁ -b ₃₁ ) ² + (a ₃₂ -b ₃₂ ) ² + ...
... + (a ₂₉ -b ₃₀ ) ² + (a ₃₀ -b ₃₁ ) ² + (a ₃₁ -b ₃₂ ) ² + ...
... + (a ₂₈ -b ₃₀ ) ² + (a ₂₉ -b ₃₁ ) ² + (a ₃₀ -b ₃₂ ) ² + ...
・ ・ ・ ・ ・ ・
... + (a ₀ -b ₃₀ ) ² + (a ₁ -b ₃₁ ) ² + (a ₂ -b ₃₂ ) ² + ...
Is calculated.

That is, each integrator 243_1,243_2,243_3, ..., In 243_30, primary IMU speed a _i and CAN rate (gain correction) relates b _i, respectively, the integrated value of the squared error each other the same time, one period An integrated value of the square error between the time points shifted, an integrated value of the square error between the time points shifted by two cycles,..., An integrated value of the square error between the time points shifted by 30 cycles is calculated.

  The error calculation unit 24 shown in FIG. 3 performs the calculation as described above, and the calculation result is input to the delay time calculation unit 25. The delay time calculation unit 25 compares the outputs of the 30 integrators 243_1, 243_2, 243_3,..., 243_30 shown in FIG. Is estimated. More specifically, when the output value of the integrator 243_1 that calculates the integrated value of the square error between the same times is the minimum value, the delay time I = 0 ms, and the 2 of the time points shifted by one cycle (10 ms). When the output value of the integrator 243_2 that calculates the integrated value of the multiplication error is the minimum value, the integrated value of the square error between the time points when the delay time I = 10 ms and two periods (20 ms) are shifted is calculated. When the output value of the integrator 243_2 is the minimum value, the output value of the integrator 243_30 that calculates the integrated value of the square error between the delay times I = 20 ms,..., 30 cycles (300 ms). Is the minimum value, it is estimated that the delay time I = 300 ms.

  The delay estimation unit 20 shown in FIGS. 1 and 3 estimates the CAN delay time I with respect to the primary IMU speed by the above calculation.

  FIG. 5 is a block diagram showing the internal configuration of the speed correction unit shown by one block in FIG.

  The speed correction unit 30 includes a correction speed generation unit 31, a Kalman gain calculation unit 32, a subtractor 33, and a synchronization processing unit 34.

  The corrected speed generation unit 31 receives the IMU acceleration, and calculates the IMU speed from the IMU acceleration according to the equation of motion. Here, in order to distinguish from the primary IMU speed on the input side shown in FIG. 1, the IMU speed output from the correction speed generation unit 31 is referred to as a secondary IMU speed.

  A strap-down navigator is widely known that inputs data on three-axis angular velocities and directional accelerations derived from an IMU and calculates a three-dimensional IMU velocity based on a three-dimensional equation of motion. The correction speed generation unit 31 shown in FIG. 5 has a configuration in which the processing in the strap-down navigator is simplified to one-dimensional processing only in the traveling direction of the vehicle, and is therefore obvious from the strap-down navigator having a well-known configuration. Therefore, the detailed description of the processing in the correction speed generation unit 31 is omitted.

  The secondary IMU speed calculated by the correction speed generation unit 31 is input to the synchronization processing unit 34. In the synchronization processing unit 34, the input secondary IMU speed is delayed by a delay amount corresponding to the delay time of the CAN speed obtained by the delay estimation unit 20, and is synchronized with the CAN speed.

  The secondary IMU speed delayed by the synchronization processing unit 34 is input to the subtractor 33, and a difference value δx from the CAN speed (gain correction) input to the subtractor 33 is calculated.

  The calculated difference value δx is input to the Kalman gain calculation unit 32, and the Kalman gain calculation unit 32 calculates the Kalman gain K from a Kalman filter based on the equation of motion. The Kalman filter and the Kalman gain are widely known algorithms, and a detailed description thereof is omitted here.

  The Kalman gain calculation unit 32 calculates the Kalman gain K and outputs a correction value Kδx. The correction value Kδx is input to the correction speed generation unit 31 and is used as a correction value when calculating the secondary IMU speed.

Returning to FIG. 1, the description of the speed measuring device 100A of the present embodiment will be continued.
The speed measuring device 100A includes a changeover switch 40. The changeover switch 40 outputs the primary IMU speed inputted to the speed measuring device 100A as the final IMU speed as it is when satellite acquisition is on, according to whether satellite acquisition is on or off. Are switched to output the secondary IMU speed generated by the speed measuring apparatus 100A as the final IMU speed.

  FIG. 6 is a diagram showing the final IMU speed output from the speed measurement apparatus 100A shown in FIG. 1 in association with satellite acquisition on and off.

  When satellite acquisition is on, the changeover switch 40 is switched to the side that outputs the primary IMU speed, and the primary IMU speed is output as the final IMU speed. That is, the final IMU speed in the section of hygiene acquisition on in FIG. 6 is the primary IMU speed. The primary IMU speed is derived from the IMU, ensures real-time characteristics, and is a speed corrected with high accuracy by the Doppler speed derived from the GPS receiver when satellite acquisition is on.

  On the other hand, when the satellite acquisition is off, the changeover switch 40 is switched to the side that outputs the secondary IMU speed, and the secondary IMU speed is output as the final IMU speed. When the satellite acquisition is off, the GPS receiver does not perform accurate speed measurement, so the primary IMU speed is not corrected or corrected inaccurately depending on the speed derived from the GPS receiver. The speed measurement apparatus 100A shown in FIG. 1 obtains the gain and delay time of the CAN speed based on the primary IMU speed, and at the timing when the accuracy of the primary IMU speed decreases, it is corrected with the obtained gain. The accurate secondary IMU speed is calculated using the CAN speed and the delay time.

  Therefore, as can be seen from FIG. 6, a smooth and continuous high-precision final IMU speed is obtained even at the timing of switching between the primary IMU speed in the satellite acquisition on section and the secondary IMU speed in the satellite acquisition off section. It is done.

  FIG. 7 is a block diagram showing a speed measuring device as a second embodiment of the present invention. Here, differences from the above-described first embodiment (mainly see FIG. 1) will be described.

  Similarly to the speed measurement device 100A of the first embodiment, the speed measurement device 100B of the second embodiment shown in FIG. 7 also includes a gain correction unit 10, a delay estimation unit 20, and a speed correction unit 30 '. However, in the speed measuring device 100B of the second embodiment, the changeover switch 40 (see FIG. 1) provided in the speed measuring device 100A of the first embodiment is not necessary. The gain correction unit 10 and the delay estimation unit 20 in the speed measurement device 100B of the second embodiment are the same as the gain correction unit 10 and the delay estimation unit 20 in the speed measurement device 100 of the first embodiment, respectively. Description is omitted. However, the speed correction unit 30 ′ has the following changes compared to the speed correction unit 30 (see FIG. 1) in the first embodiment. That is, the acceleration in the traveling direction of the vehicle derived from the IMU is input to the speed correction unit 30 (see FIG. 1) in the first embodiment, and the secondary IMU speed is calculated from the input acceleration in the speed correction unit 30. Calculated. On the other hand, the primary IMU speed is input to the speed correction unit 30 ′ of the second embodiment instead of the acceleration. Accordingly, the speed correction unit 30 'calculates the secondary IMU speed from the primary IMU speed. For this reason, the velocity calculation unit 30 ′ employs an equation of motion for calculating the velocity (secondary IMU velocity) from the velocity (primary IMU velocity). Since this change is obvious from the conventionally well-known technique, detailed description thereof is omitted.

  The secondary IMU speed is output from the speed correction unit 30 '. This secondary IMU speed is a speed obtained by further correcting the highly accurate primary IMU speed when satellite acquisition is on. Although this is a useless process, there is no problem caused by this correction process, and a high-accuracy speed is output. In addition, when the satellite acquisition is off, the accuracy of the primary IMU speed is reduced, but the secondary IMU speed accurately corrected by the speed correction unit 30 ′ is the same as the speed correction unit 30 in the first embodiment described above. Generated.

  Next, a primary IMU speed generation algorithm will be briefly described.

  FIG. 8 is a block diagram showing the configuration of the primary IMU speed generation device.

  The primary IMU speed generator 50 corresponds to an example of a primary IMU speed generator according to the present invention.

  A primary IMU speed generation device 50 shown in FIG. 8 is an apparatus that generates the above-mentioned primary IMU speed by correcting data from the IMU 1 with data from the GPS receiver 2.

  From the IMU 1, three-dimensional angular velocity and acceleration data in three directions are output in synchronization with a 100 Hz clock and input to the strapdown navigator unit 51. The strapdown navigator 51 calculates ground axis-based speed data based on the angular velocity and acceleration based on the traveling direction of the vehicle output from the IMU 1. Further, in the strap-down navigator unit 51, the speed data is further integrated and the ground axis reference position data is also generated. The strapdown navigator 51 further corrects the generated speed data and position data based on the correction data, and outputs corrected speed data and corrected position data. Both the speed data and the position data calculated by the strapdown navigator 51 are three-dimensional data based on the ground axis. The strap-down navigator unit 51 itself has a well-known configuration, and further description thereof is omitted here.

  In FIG. 8, the correction data is represented as Kδx or 0. The strapdown navigator 51 performs correction based on the correction data when the correction data = Kδx, and does not perform correction when the correction data = 0, and is calculated based only on the data derived from the IMU. Speed data and position data are output.

  Here, when correction data = Kδx, K is a Kalman coefficient, and δx is a difference value. The difference value δx includes both a speed difference and a position difference. The contents of the difference value δx will be described later.

  The GPS receiver 2 outputs speed data representing the Doppler speed and position data. These velocity data and position data are all three-dimensional data based on the ground axis. The speed data and the position data output from the GPS receiver 2 are data that are updated in synchronization with a slow clock such as 10 Hz or 20 Hz, for example, compared to the data output from the IMU 1. In addition, the data to be updated is data that has already been delayed by, for example, about 100 ms to 150 ms from the real time and is accompanied by jitter (delay fluctuation) of about several ms. That is, although the data output from the GPS receiver 2 is subjected to highly accurate speed measurement at the timing when speed measurement conditions such as correctly receiving radio waves from a predetermined number of GPS satellites are satisfied, The data is slow and the update speed is slow. In addition, when speed measurement conditions are not satisfied, accurate speed measurement is impossible.

  Here, whether or not speed measurement conditions are satisfied is represented by satellite capture on / off that indicates whether or not radio waves from a predetermined number or more of GPS satellites are correctly received.

  Speed data and position data output from the GPS receiver 2 are input to the subtractor 52.

  On the other hand, speed data and position data output from the strapdown navigator unit 51 are input to the synchronization processing unit 53. The synchronization processing unit 53 delays input speed data and position data. As described above, the speed data and position data output from the GPS receiver 2 are data with a time delay. Therefore, in the synchronization processing unit 53, the data output from the strapdown navigator unit 51 is subjected to delay processing in order to synchronize with the data output from the GPS receiver 2.

  The output data of the strapdown navigator unit 51 that has been subjected to the delay processing by the synchronization processing unit 53 is input to the subtractor 52 together with the data output from the GPS receiver 2.

  The difference unit 52 calculates a difference value δx between the two data.

Specifically, the speed data V ₁ and the position data P ₁ output from the GPS receiver 2 are respectively
V ₁ = (V _1n , V _1e , V _1d )
P ₁ = (P _1n , P _1e , P _1d )
However,
V _1n is the north component of the velocity V _1e is the east component of the velocity V _1d is the downward component of the velocity P _1n is the north component of the location P _1e is the east component of the location P _1d represents the downward component of the position.
The speed data V ₂ and the position data P ₂ subjected to the delay processing in the synchronization processing unit 53 are respectively
V ₂ = (V _2n , V _2e , V _2d )
P ₂ = (P _2n , P _2e , P _2d )
However,
V _2n is the north component of the velocity V _2e is the east component of the velocity V _2d is the downward component of the velocity P _2n is the north component of the location P _2e is the east component of the location P _2d represents the downward component of the position.
In the subtractor 52, the difference value δx, that is,

Is calculated.

here,
δP is a position error δV is a speed error,
δP = (δP _n , δP _e , δP _d )
_{= (P 1n -P 2n, P} 1e -P 2e, P 1d -P 2d)
δV = (δV _n , δV _e , δV _d )
= (V _1n −V _2n , V _1e −V _2e , V _1d −V _2d )
It is.

  Difference data representing the difference value δx calculated by the differentiator 52 is input to the changeover switch 54.

  When the satellite capture information output from the GPS receiver 2 is “satellite capture on” indicating that the satellite capture is correctly performed, the changeover switch 54 calculates the difference value δx calculated by the subtractor 52. Is transmitted to the extended Kalman filter unit 55, and when the satellite capture information is “satellite capture off” indicating that only the satellites less than the specified number are correctly captured, the value “0” is transmitted to the extended Kalman filter unit 55. That is, when the speed is measured with the GPS receiver 2 with high accuracy, the difference value δx is highly reliable, and the difference value δx is directly reflected as an input of the extended Kalman filter unit 55 and measured with the GPS receiver 2. When the speed reliability is low, the reliability of the difference value δx is low, which means that the value “0” is input to the extended Kalman filter unit 55.

  The extended Kalman filter unit 30 calculates the Kalman coefficient K based on the equation of motion, generates correction data to correct the data derived from the IMU 1, and outputs the correction data to the strapdown navigator unit 51. The correction data is correction data = K · δx when satellite acquisition is on, and correction data = 0 when satellite acquisition is off.

  Since the extended Kalman filter unit 30 itself is widely known, further detailed explanation is omitted here.

  In the strap-down navigator 51, the velocity data and the position data derived from the IMU 1 are corrected based on the correction data = K · δx received from the extended Kalman filter unit 55 in the satellite acquisition on period.

  The speed data and the position data output from the strapdown navigator unit 51 are data in which real-time property is ensured in synchronization with a 100 Hz clock derived from the IMU. Further, the velocity data and the position data output from the strapdown navigator 51 are highly accurate data in which an offset error or the like is canceled by the data derived from the GPS receiver 2 during the satellite capture on period. However, since the velocity data and position data output from the strapdown navigator 51 are not corrected during the satellite acquisition off period, offset errors and the like are sequentially accumulated during the satellite acquisition off period. The data will deviate from the correct speed and position over time.

  The primary IMU speed generation unit 56 calculates a one-dimensional speed in the traveling direction of the vehicle from the speed data output from the strapdown navigator unit 51 and outputs it as the primary IMU speed. The primary IMU speed is based on the output data of the strap-down navigator unit 51. Therefore, the primary IMU speed represents a high-accuracy speed when the satellite acquisition is on, and a gradually shifted speed when the satellite acquisition is off.

  The primary IMU speed output from the primary IMU speed generation device 50 shown in FIG. 8 is input to the speed measurement device 100A shown in FIG. 1 or the speed measurement device 100B shown in FIG.

  FIG. 8 is an example of a configuration for generating the primary IMU speed based on the data derived from the IMU 1 and the data derived from the GPS receiver 2, and the primary IMU speed generation unit referred to in the present invention is shown in FIG. It is not limited to the configuration shown in FIG.

1 IMU
2 GPS receiver 10 Gain correction unit 11, 21 Determination unit 12, 22 Differentiator 13 Correction coefficient calculation unit 14 Correction unit 20 Delay estimation unit 23 Delay unit 24 Error calculation unit 25 Delay time calculation unit 30 Speed correction unit 31 Correction speed generation 31 Unit 32 Kalman gain calculation unit 33, 52 Difference unit 34, 53 Synchronization processing unit 40, 54 Changeover switch 50 Primary IMU speed generation device 51 Strapdown navigator unit 55 Extended Kalman filter unit 56 Primary IMU speed generation unit 100A, 100B Speed Measuring device 131 Difference unit 132 Cumulative value calculation unit 133 Amplifier (attenuator)
141 multiplier 142 adder 241, 241_1,..., 241_30 differentiator 242, 242_1,..., 242_30 square calculator 243, 243_1,.

Claims (7)

A gain correction unit for correcting the gain of the CAN speed representing the vehicle speed based on the primary IMU speed representing the vehicle speed derived from the IMU corrected based on the GPS speed derived from the GPS receiver;
A correlation operation between the primary IMU speed and the CAN speed is used to estimate a delay time of the CAN speed with respect to the primary IMU speed that minimizes an error between the primary IMU speed and the CAN speed. A delay estimator to
A speed correction unit that generates a secondary IMU speed by correcting a speed derived from I MU ,
A synchronization processing unit that generates a delayed secondary IMU speed that is temporally synchronized with the CAN speed by delaying the secondary IMU speed by a delay time of the CAN speed estimated by the delay estimation unit;
A differentiator for generating a difference between the CAN speed corrected by the gain correction unit and the delayed secondary IMU speed generated by the synchronization processing unit;
A Kalman gain calculating unit that inputs a difference generated by the differentiator and generates a Kalman gain that corrects a current velocity derived from the IMU based on an equation of motion; and
And a speed correction unit including a correction speed generation unit that generates the secondary IMU speed by correcting the current speed derived from the IMU using the Kalman gain calculated by the Kalman gain calculation unit. A speed measuring device.   The gain correction unit is at a timing when the primary IMU speed corrected based on the GPS speed, which is determined based on a predetermined determination criterion, is input, and is running at a constant speed The speed measurement apparatus according to claim 1, wherein a gain of the CAN speed is corrected on the condition of   The timing at which the primary IMU speed that has been corrected based on the GPS speed is input based on the GPS speed and the vehicle speed is changing is determined by the delay estimation unit. 3. The speed measuring apparatus according to claim 1, wherein a delay time of the CAN speed is estimated on the condition that The speed correction unit is
A synchronization processing unit that synchronizes with the CAN speed by delaying the secondary IMU speed by a delay amount corresponding to a delay time of the CAN speed estimated by the delay estimation unit; A difference value calculating unit that calculates a difference value between the corrected CAN rate and the secondary IMU rate delayed by the synchronization processing unit;
A Kalman gain calculating unit that generates a correction value for calculating the second-order IMU velocity by inputting the difference value calculated by the difference value calculating unit and calculating a Kalman gain;
4. A correction speed generation unit that generates the secondary IMU speed by correcting an IMU-derived speed based on a correction value generated by the Kalman gain calculation unit. The speed measuring device of any one of these.   5. The apparatus according to claim 1, further comprising a primary IMU speed generation unit configured to generate the primary IMU speed by correcting an IMU-derived speed based on the GPS speed. Speed measuring device.   The speed correction unit generates the secondary IMU speed by correcting a speed derived from the IMU generated independently of the primary IMU speed. The speed measuring device according to claim 1.   The speed measurement apparatus according to claim 1, wherein the speed correction unit generates the secondary IMU speed by correcting the primary IMU speed.

Images