JP7238490B2 - Sensor error correction device and sensor error correction program - Google Patents

Description

The present invention relates to a sensor error correction device and a sensor error correction program.

A vehicle in motion performs motions in three axial directions, that is, longitudinal motion, lateral motion, and turning motion, with respect to a road surface. During the motion, the vehicle dynamically changes its attitude angle, which is the angle with respect to the earth coordinate system (ie, the ground). For example, a vehicle body that is turning has a roll angle that causes the vehicle body to tilt in the left-right direction. In addition, the accelerating vehicle body and the decelerating vehicle body have a pitch angle at which the vehicle body is tilted in the traveling direction of the vehicle body.

If the roll angle and pitch angle of the vehicle can be grasped, the steering angle, accelerator opening degree, and brake strength related to vehicle control can be appropriately determined, and more stable control becomes possible in automatic driving of the vehicle. In addition, accurate estimation of the attitude angle of the vehicle body enables more accurate calculation of the amount of movement of the vehicle on the map, so that the self-position of the vehicle can be estimated more appropriately in automatic driving.

However, when the vehicle makes a turning motion while traveling on a slope or the like, an error may occur in the sensor that detects the motion in the left-right direction. Techniques for appropriately correcting sensor errors that detect motion in the left-right direction have been proposed.

For example, there is a technology that estimates the horizontal plane yaw rate using the measured yaw rate and the estimated roll angle, compares the amount of change in the horizontal plane yaw rate and the amount of change in the velocity vector, and corrects the acceleration in the horizontal direction (patented Reference 1).

JP 2018-112520 A

However, in the technique described in Patent Document 1, the magnitude relationship is compared using the absolute value of the actual roll angle (true value) and the estimated absolute value of the roll angle. was only compared, and the direction of the roll angle was not sufficiently considered. Therefore, in the technique described in Patent Document 1, when the vehicle tilts in the opposite direction to the turning direction due to the reverse bank turning in the opposite direction to the inclined slope or the influence of centrifugal force, etc., the left-right motion It was not always possible to accurately correct the value of the sensor that detects .

The present disclosure has been made in view of the above circumstances. It is an object of the present invention to provide a sensor error correction device and a sensor error correction program capable of accurately correcting .

To achieve the above object, the sensor error correction device according to claim 1 comprises an angular velocity sensor mounted on a mobile body for detecting rotational motion of the mobile body, an acceleration sensor for detecting translational motion of the mobile body, and Rotation in the pitch angle direction of the mobile object using an acquisition unit that acquires travel information including values detected by each of the wheel speed sensors that detect the speed of the wheels of the mobile object, and the values detected by the angular velocity sensors of the travel information. estimating a first angle change amount indicating the amount of rotation, and using the values detected by the acceleration sensor and wheel speed sensor of the travel information, estimating a second angle change amount indicating the amount of rotation of the moving body in the pitch angle direction an estimating unit; and a correcting unit that derives an error in travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit based on the error. , the error of the travel information is the error of the acceleration sensor in the lateral direction along the vehicle width direction of the moving object, and the correction unit corrects the value of the acceleration sensor in the lateral direction of the travel information.

Further, according to the second aspect of the invention, in the first aspect of the invention, the traveling information is acquired by the acquisition unit during a period from when the moving object starts to when it finishes turning.

Further, according to the invention of claim 3, in the invention of claim 1 or 2, the correcting unit uses the error of the derived traveling information and the error of the traveling information previously derived by the correcting unit to correct the travel. Further correct the information.

Further, the invention according to claim 4 is the invention according to any one of claims 1 to 3 , wherein the estimation unit further estimates the roll angle from the traveling information, and the correction unit corrects the roll angle error. Further derive and correct the roll angle.

On the other hand, in order to achieve the above object, a sensor error correction program according to claim 5 comprises an angular velocity sensor mounted on a mobile body for detecting rotational motion of the mobile body and an acceleration sensor for detecting translational motion of the mobile body. , and a step of acquiring traveling information including values detected from each of wheel speed sensors that detect the speed of the wheels of the moving body, and using the values detected from the angular velocity sensors of the traveling information to determine the pitch angle direction of the moving body. A first angle change amount indicating the amount of rotation is estimated, and a second angle change amount indicating the amount of rotation in the pitch angle direction of the moving object is estimated using the values detected by the acceleration sensor and wheel speed sensor of the travel information. and a step of deriving an error of the travel information from the difference between the first angle change amount and the second angle change amount, and correcting the travel information acquired by the acquisition unit based on the error. The error of the information is the error of the acceleration sensor in the lateral direction along the vehicle width direction of the moving body, and the step of correcting causes the computer to execute the process of correcting the value of the acceleration sensor in the lateral direction of the travel information.

According to the present disclosure, even if the vehicle is tilted in the opposite direction to the turning direction due to the reverse bank turning in the opposite direction to the inclined slope or the influence of centrifugal force, etc., the sensor value in the lateral direction can be detected. can be corrected.

It is a schematic diagram which shows typically an example of the vehicle which mounts the sensor which concerns on this embodiment. It is a block diagram showing an example of the hardware constitutions of the sensor error correction device concerning this embodiment. It is a block diagram showing an example of a functional configuration of a sensor error correction device according to the present embodiment. FIG. 3 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value according to the present embodiment; FIG. 3 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a new zero point correction value according to the present embodiment; FIG. 4 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value using an approximated pitch angle according to the present embodiment; 5 is a graph showing an example of the relationship between yaw rate and time according to the embodiment; 6 is a flowchart showing an example of sensor error correction processing according to the embodiment;

Embodiments for implementing the technology of the present disclosure will be described in detail below with reference to the drawings. In addition, this embodiment illustrates the case where the moving object is a vehicle. However, mobile bodies are not limited to vehicles. The moving object may be a flying object as long as it is a moving object. The sensor error correction device mounted on the vehicle will be described in detail below.

FIG. 1 is a schematic diagram schematically showing an example of a vehicle 10 equipped with a sensor according to this embodiment. The vehicle 10 is equipped with an angular velocity sensor 11 that detects rotational motion of the vehicle 10 , an acceleration sensor 12 that detects translational motion of the vehicle 10 , and a wheel speed sensor 13 that detects the speed of the wheels of the vehicle 10 .

The angular velocity sensor 11 detects a pitch rate Q that indicates the amount of change in rotation of the vehicle 10 in the pitch angle direction per unit time. The angular velocity sensor 11 also detects a roll rate P that indicates the amount of change in rotation of the roll angle of the vehicle 10 per unit time. The angular velocity sensor 11 also detects a yaw rate R that indicates the amount of change in rotation of the vehicle 10 about the vertical axis per unit time.

The acceleration sensor 12 detects an acceleration G _x in the longitudinal direction along the traveling direction of the vehicle 10 , an acceleration G _y in the lateral direction along the width direction of the vehicle 10 , and an acceleration G _z in the vertical direction along the vertical axis of the vehicle 10 . do. Moreover, the wheel speed sensor 13 detects the speed U of the wheels of the vehicle 10 .

FIG. 2 is a block diagram showing an example of the hardware configuration of the sensor error correction device 20 according to this embodiment. As shown in FIG. 2, a sensor error correction device 20 according to the present embodiment is mounted on a vehicle 10 and includes a CPU (Central Processing Unit) 21, a ROM (Read Only Memory) 22, a RAM (Random Access Memory) 23, a storage 24, a display unit 25, and sensors 11, 12, and 13. CPU 21 , ROM 22 , RAM 23 , storage 24 , display unit 25 , and sensors 11 , 12 , 13 are interconnected by bus 26 .

The CPU 21 supervises and controls the sensor error correction device 20 as a whole. The ROM 22 stores various programs including a sensor error correction program used in this embodiment, data, and the like. The RAM 23 is a memory used as a work area when executing various programs. The CPU 21 develops a program stored in the ROM 22 in the RAM 23 and executes it, thereby detecting and correcting sensor values. The storage 24 is, for example, a HDD (Hard Disk Drive), SSD (Solid State Drive), flash memory, or the like. Note that the storage 24 may store a sensor error correction program or the like.

The display unit 25 displays data and the like detected by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor. The display unit 25 is, for example, a liquid crystal monitor, a CRT (Cathode Ray Tube) monitor, an FPD (Flat Panel Display) monitor, or the like.

In the present embodiment, the configuration in which the traveling information acquired by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 is displayed on the display unit 25 has been described. However, it is not limited to this. The acquired travel information may be linked to another device installed in the vehicle 10 , or the corrected travel information may be displayed on the display unit 25 .

The sensors 11 , 12 , 13 are an angular velocity sensor 11 , an acceleration sensor 12 , and a wheel speed sensor 13 that detect the posture and running state of the vehicle 10 while the vehicle 10 is running.

Note that, in the present embodiment, the configuration in which the sensor error correction device 20 is mounted on the vehicle 10 has been described. However, it is not limited to this. The sensor error correction device 20 may be provided outside the vehicle 10 . In this case, the data detected by the sensors 11, 12, and 13 are transmitted to the sensor error correction device 20 installed at another location through a communication line or the like, and the sensor error correction device 20 corrects the travel information error. processing may be performed.

Next, the functional configuration of the sensor error correction device 20 will be described. FIG. 3 is a block diagram showing an example of the functional configuration of the sensor error correction device 20 according to this embodiment.

As shown in FIG. 3 , the sensor error correction device 20 has an acquisition section 31 , an estimation section 32 and a correction section 33 . The CPU 13 functions as an acquisition unit 31 , an estimation unit 32 , and a correction unit 33 by executing the sensor error correction program.

Acquisition unit 31 acquires data from angular velocity sensor 11 , acceleration sensor 12 , and wheel speed sensor 13 mounted on vehicle 10 . Specifically, the acquisition unit 31 acquires a roll rate P, a pitch rate Q, and a yaw rate R that indicate the amount of change in the tilt of the vehicle body from an angular velocity sensor mounted on the vehicle 10 . The acquisition unit 31 also acquires the longitudinal acceleration Gx, the lateral acceleration G _y , and the vertical acceleration G _z from the acceleration sensor 12 , and acquires the wheel speed U from the wheel speed sensor 13 . That is, the acquisition unit 31 acquires data (hereinafter referred to as “travel information”) including values detected by each of the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 mounted on the vehicle 10. Further, the travel information is acquired by the acquisition unit 31 during the period from when the moving object starts to when it finishes turning.

It has been described that the travel information according to the present embodiment is data obtained from the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13. FIG. However, it is not limited to this. The travel information may include information indicating the position of the vehicle 10 and the date and time when the data was acquired. It is good also as a form containing.

The estimating unit 32 estimates a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving object, using the value detected by the angular velocity sensor 11 of the travel information. The estimating unit 32 also estimates a second angle change amount indicating the amount of rotation in the pitch angle direction of the moving body using the values detected by the acceleration sensor 12 and the wheel speed sensor 13 of the travel information. That is, the estimation unit 32 estimates the amount of change in the first pitch angle using the travel information acquired from the angular velocity sensor 11 . The estimator 32 also uses the travel information acquired from the acceleration sensor 12 and the wheel speed sensor 13 to estimate the amount of change in the roll angle φ, the pitch angle θ, and the second pitch angle.

The correction unit 33 derives the error of the travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit 31 based on the error. That is, the correction unit 33 calculates the error included in the estimated roll angle φ from the difference between the first pitch angle change amount and the second pitch angle change amount, and the acceleration G and the error contained in _y . Further, the correction unit 33 corrects the estimated roll angle φ and the acceleration _Gy included in the travel information using the derived roll angle error and _acceleration Gy error.

Next, before describing the action of the sensor error correction device 20, a method for estimating the error of the sensor mounted on the vehicle 10 will be described. In this embodiment, an error estimation method using the equation of motion of a 6-axis sensor will be described.

In the equation of motion that indicates the state of translational motion, even if the pitch angle is derived using the equation of motion of a 6-axis sensor, no accumulated error occurs. Since accumulation is performed by , an accumulated error occurs in the pitch angle when derived. In this embodiment, the error is derived using the properties of the equation of motion indicating the state of translational motion and the equation of motion indicating the state of rotational motion. It should be noted that the error according to the present embodiment is a value representing deviation from the reference position (hereinafter referred to as "offset error").

In this embodiment, in order to avoid complication, the roll angle φ is assumed to be sufficiently small (inclination of 10 degrees or less), and the yaw rate R, pitch rate Q, and longitudinal acceleration _Gx have already been corrected, and errors are not included. Make it not exist. In the present embodiment, the vehicle 10 has a lateral velocity V along the width direction of the vehicle 10 and a vertical velocity W along the vertical direction of the vehicle 10, which are so small as to be negligible.

The pitch angle θ and the roll angle φ are estimated using the equation of motion of the 6-axis sensor applied to the angular velocity sensor 11 and the acceleration sensor 12 according to this embodiment. The equation of motion of the 6-axis sensor is represented by the following formulas.

Equations (1), (2), and (3) described above are equations representing the state of rotational motion of the moving body. Also, the above equations (4), (5), and (6) are equations indicating the state of the translational motion of the moving body.

Formula (2) is applied to the angular velocity sensor 11 mounted on the vehicle 10 according to the present embodiment, and formulas (4) and (5) are applied to the acceleration sensor 12 and the wheel speed sensor 13 mounted on the vehicle 10 according to the present embodiment. to derive the pitch angle θ and roll angle φ. The derived pitch angle θ and roll angle φ are represented by the following formulas.

Here, the left side of equation (7) (the symbol with a dot above θ) represents the amount of change in the pitch angle θ detected by the angular velocity sensor per minute time, and φ _A is the amount detected by the acceleration sensor 12. It represents the detected roll angle φ. Also, g represents the gravitational acceleration, and θ _A represents the pitch angle detected by the acceleration sensor 12 . Also, the first term (dotted symbol above U) on the right side of equation (8) represents the amount of change in the wheel speed U detected by the wheel speed sensor 13 per minute.

Equations (7) and (8) are used to derive the roll angle error. The sum of both sides of Equation (7) is represented by the following formula.

Here, _ΔθG is the amount of change in the pitch angle detected by the angular velocity sensor, _Ts is the time when the angular velocity sensor started detection, and _Te is the time when the angular velocity sensor finished detection.

Also, if the pitch angle depends on time, it can be approximated by the time detected by the acceleration sensor, and equation (8) is expressed by the following equation.

Here, Δθ _A is the amount of change in the pitch angle detected by the acceleration sensor, and a is the inclination of the pitch angle θ _A. Therefore, the difference between equations (10) and (11) is represented by the following equations.

Originally, the first and second terms on the right side of equation (12) are the amount of change in the pitch angle θ detected by the angular velocity sensor 11 and the acceleration sensor 12, respectively, so they have the same value and cancel each other out. However, if the roll angle φ _A includes an offset error, an error occurs in the amount of change Δθ _G in the pitch angle detected by the angular velocity sensor 11 .

Considering that the roll angle φ is sufficiently small, cos φ can be approximated to 1 and sin φ can be approximated to φ, and Equation (12) is expressed by the following formula.

Considering that the roll angle φ _A detected by the acceleration sensor 12 includes an offset error, the roll angle φ _A detected by the acceleration sensor 12 is represented by the following formula.

Here, φ is the lateral roll angle of the vehicle 10 that does not include an error, εφ is the roll angle error included in the roll angle _φA , and the roll angle error is assumed to be a constant value. do. When formula (14) is applied to formula (13), the difference between the pitch angle variation Δθ detected by each of the angular velocity sensor 11 and the acceleration sensor 12 is represented by the following formula.

The second term on the right side of equation (15) corresponds to the amount of change in the pitch angle of the vehicle 10 that does not contain an error, and is equivalent to the first term that is the amount of change in the pitch angle detected by the acceleration sensor 12. can be regarded as Therefore, the formula (15) is represented by the following formula.

By rearranging the equation (16), the roll angle error ε _φ included in the roll angle φ _A detected by the acceleration sensor 12 is represented by the following formula.

As described above, according to the equation (17), the roll angle error ε _φ included in the roll angle φ _A is the difference between the change amount Δθ _A of the pitch angle detected by the acceleration sensor 12 and the pitch angle detected by the angular velocity sensor 11. It can be obtained from the difference from the amount of change _ΔθG .

Next, the error of the lateral acceleration _Gy of the acceleration sensor 12 is derived using equation (9). Considering that the roll angle φ is sufficiently small, sin φ can be approximated to φ, and the roll angle _φA detected by the acceleration sensor 12 is expressed by the following formula.

Here, the error in the roll angle _φA detected by the acceleration sensor 12 is due to the acceleration _Gy in the horizontal direction, and considering that the error is included in the acceleration _Gy in the horizontal direction, the error is corrected By doing so, the roll angle φ of the vehicle 10 can be obtained. The roll angle φ of the vehicle 10 is expressed by the following formula when corrected using a value for correcting the error of the acceleration _Gy detected by the acceleration sensor 12 (hereinafter referred to as "zero point correction value").

ζ _y is a zero point correction value, and Equation (19) is obtained by correcting the acceleration G _y detected by the acceleration sensor 12 using the zero point correction value ζ _y . Equation (20) is the roll angle of the vehicle 10 obtained by correcting using the zero point correction value _ζy .

Considering that the roll angle _φA detected by the acceleration sensor 12 includes an offset error, substituting equations (18) and (20) into equation (14) gives the following equation: be.

By rearranging equation (21), the zero point correction value ζ _y is represented by the following equation.

As described above, according to the equation (22), the zero point correction value ζ _y for correcting the horizontal acceleration G _y detected by the acceleration sensor 12 can be obtained using the roll angle error ε _φ and the pitch angle θ. can.

By reflecting the determined zero point correction value ζ _y in the traveling information detected by the acceleration sensor 12, when determining a new zero point correction value, the previously determined zero point correction value is taken into account to obtain a new zero point A correction value can be derived. A new zero point correction value is represented by the following formula.

Here, k is the number of times the zero point correction value is derived. That is, when deriving the zero point correction value for the second and subsequent times, it can be derived using the previously derived zero point correction value, as shown in Equation (23).

In this embodiment, a mode in which the rate of change in the pitch angle _θA during turning is constant has been described. However, it is not limited to this. If the rate of change in the pitch angle _θA during turning is not constant, the pitch angle change amount _ΔθA is the difference between the pitch angle when the acceleration sensor starts detection and the pitch angle when the acceleration sensor finishes detection. may be derived using Considering the above contents, the pitch angle change amount Δθ _A is represented by the following formula.

θ _A (T _s ) is the pitch angle at the start of turning, and θ _A (T _e ) is the pitch angle at the end of turning.

Moreover, in the present embodiment, the pitch angle θ _A has a sufficiently large value. However, it is not limited to this. When the pitch angle θ _A is sufficiently small, the pitch angle θ _A may approximate cos θ _A to 1 and sin θ _A to θ _A . Considering that the pitch angle θ _A is sufficiently small, Equation (22) is represented by the following equation.

Next, input/output of data according to the present embodiment will be described with reference to FIGS. First, with reference to FIG. 4, data input/output when deriving a zero point correction value will be described. FIG. 4 is a block diagram showing an example of input/output of the sensor error correction device for explaining the derivation of the zero point correction value according to this embodiment.

An angular velocity sensor 11 mounted on the vehicle 10 detects a roll rate P, a pitch rate Q, and a yaw rate R, and an acceleration sensor 12 detects longitudinal acceleration G _x , lateral acceleration G _y , and vertical acceleration. _Gz is detected, and the wheel speed sensor 13 detects the wheel speed U.

The acquisition unit 31 acquires values detected by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 as travel information.

The estimating unit 32 uses the longitudinal acceleration G _x obtained from the acceleration sensor 12 and the wheel speed U obtained from the wheel speed sensor 13 to estimate the pitch angle change amount Δθ _A and the pitch angle θ _A .

The estimation unit 32 also calculates the yaw rate R obtained from the angular velocity sensor 11, the lateral acceleration G _y obtained from the acceleration sensor 12, the wheel speed U obtained from the wheel speed sensor 13, and the pitch angle θ estimated from the acceleration sensor. _A is used to estimate the roll angle φ _A.

The estimator 32 also uses the pitch rate Q and the yaw rate R obtained from the angular velocity sensor 11 and the estimated roll angle φ _A to estimate the amount of change Δθ _G in the pitch angle.

The correction unit 33 uses the yaw rate R obtained from the angular velocity sensor 11, the pitch angle θ _A estimated from the acceleration sensor 12, the pitch angle variation Δθ _A , and the pitch angle variation Δθ _G estimated from the angular velocity sensor 11. , to derive the zero-point correction value ζ _y .

Next, input/output of data when deriving a new zero-point correction value will be described with reference to FIG. FIG. 5 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a new zero point correction value according to this embodiment. In FIG. 5, unlike FIG. 4, a new zero point correction value can be derived in consideration of the previously derived zero point correction value. The process of deriving the zero point correction value shown in FIG. 5 is the same as in FIG. 4 described above, and is therefore omitted.

The correction unit 33 corrects the lateral acceleration G _y acquired from the acceleration sensor 12 using the derived zero point correction value ζ _y k. The estimation unit 32 also estimates the roll angle φ _A using the corrected left-right acceleration G _y +ζ _y k. The correction unit 33 derives a new zero point correction value ζ _y k+1 in consideration of the zero point correction value ζ _y k used for estimating the roll angle _φA .

Next, with reference to FIG. 6, input/output of data when deriving a zero-point correction value using an approximated pitch angle will be described. FIG. 6 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value using an approximated pitch angle according to this embodiment. In FIG. 6, unlike FIGS. 4 and 5, the pitch angle can be approximated to derive the zero point correction value. The process of estimating the pitch angle change amount Δθ _G , pitch angle θ _A , pitch angle change amount Δθ _A , and roll angle φ _A shown in FIG. 6 is omitted because it is the same as in FIG.

As described above, cos θ _A can be approximated to 1 when the pitch angle θ _A is sufficiently small. Considering that cos θ _A can be approximated to 1, the correction unit 33 calculates the yaw rate R obtained from the angular velocity sensor 11, the amount of change in pitch angle Δθ _A estimated from the acceleration sensor, and the amount of change in pitch angle estimated from the angular velocity sensor. Δθ _G is used to derive the zero point correction value ζ _y .

In addition, in FIG. 6, the form which approximates the pitch angle (theta _)A estimated from the acceleration sensor 12, and derives|leads-out the zero point correction value (zeta) _y was demonstrated. However, it is not limited to this. A new zero point correction value ζ _y k+1 may _be derived in the same manner as in _FIG .

In this embodiment, the variation in the first pitch angle and the variation in the second pitch angle are estimated to derive the roll angle error and the lateral acceleration _Gy . However, it is not limited to this. By estimating the amount of change in the first roll angle and the amount of change in the second roll angle, the error in the pitch angle and the error in the longitudinal acceleration _Gx may be derived and corrected.

Note that the target period for deriving the roll angle error εφ and the zero point correction value _ζy according to the present embodiment is the period during which the vehicle 10 is turning.

Next, with reference to FIG. 7 , the target period for deriving the roll angle error ε _φ and the zero point correction value ζ _y will be described. FIG. 7 is a graph showing an example of the relationship between the yaw rate and time according to this embodiment.

As shown in FIG. 7 , the sensor error correction device 20 compares the yaw rate detected by the angular velocity sensor 11 with a threshold, and if the threshold is exceeded, starts recording travel information, and travels until the yaw rate falls below the threshold. Store information. The sensor error correction device 20 uses the stored travel information to estimate the error. Further, when the turning motion is started again, the sensor error correction device 20 stores the traveling information from the period when the yaw rate exceeds the threshold until it falls below the threshold. _y is used to estimate the error.

Next, the operation of the sensor error correction program according to this embodiment will be described with reference to FIG . First, FIG. 8 is a flowchart showing an example of sensor error correction processing according to this embodiment. The sensor error correction process shown in FIG. 6 is executed by the CPU 13 executing the sensor error correction program. For example, when the user starts the vehicle 10, the sensor error correction process shown in FIG . Further, the sensor error correction process may be executed each time the sensors 11 to 13 detect, or at predetermined regular timing.

In step S101, the acquiring unit 31 compares the yaw rate acquired from the angular velocity sensor 11 with a threshold value, and determines whether or not the vehicle 10 has started turning motion. If the yaw rate exceeds the threshold (step S101: YES), the process proceeds to step S102. On the other hand, if the yaw rate is equal to or less than the threshold (step S101: NO), the process waits until the yaw rate exceeds the threshold.

In step S102, the acquisition unit 31 acquires the travel information from the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13, and stores it.

In step S103, the estimating unit 32 uses the traveling information detected by the angular velocity sensor 11 to estimate the pitch angle _θG , and uses the traveling information detected by the acceleration sensor 12 and the wheel speed sensor 13 to estimate the roll angle φ Estimate _A , and the pitch angle θ _A .

In step S104, the estimator 32 compares the yaw rate acquired from the angular velocity sensor 11 with a threshold to determine whether the vehicle 10 has finished turning. If the yaw rate is below the threshold (step S103: YES), the process proceeds to step S105. On the other hand, when the yaw rate exceeds the threshold (step S103: NO), the process proceeds to step S102 to repeatedly acquire travel information.

In step S105, the estimator 32 estimates the pitch angle change amount Δθ _G using the estimated θ _G . In step S106, the estimator 32 estimates the pitch angle change amount Δθ _A using the estimated θ _A .

In step S107, the correction unit 33 uses the pitch angle change amount Δθ _G , the pitch angle change amount Δθ _A and the pitch angle θ _A estimated by the estimation unit 32, and the yaw rate acquired by the acquisition unit 31 to obtain the roll angle and the zero- _point correction value ζ _y are derived. Further, the correction unit 33 uses the derived roll angle error ε _φ and the zero point correction value ζ _y to estimate the roll angle φ _A from the value detected by the angular velocity sensor 11 and the roll angle φ A detected from the acceleration sensor 12 . Corrects the lateral acceleration _Gy .

In step S108, the acquisition unit 31 determines whether or not the vehicle 10 has finished traveling. When the vehicle 10 reaches the destination and finishes traveling (step S108: YES), the sensor error correction process is finished. If the vehicle 10 has not finished running (step S108: NO), the process proceeds to step S101.

As described above, according to the present embodiment, the amount of change in the pitch angle estimated from the values detected by the angular velocity sensor 11 and the pitch angle estimated from the values detected by the acceleration sensor 12 and the wheel speed sensor 13 The amount of change can be used to derive the roll angle error and the zero point correction value. Therefore, even if the vehicle body tilts in the direction opposite to the turning direction due to the influence of reverse bank or centrifugal force, the tilt of the vehicle in the roll angle direction and the acceleration in the lateral direction can be accurately corrected.

In addition, the configuration of the sensor error correction device described in the above embodiment is merely an example, and may be changed according to circumstances without departing from the gist of the invention.

Further, the flow of processing of the program described in the above embodiment is also an example, and unnecessary steps may be deleted, new steps added, or the processing order changed without departing from the scope of the invention. good.

Note that the sensor error correction processing executed by the CPU by reading the software (program) in the above embodiment may be executed by various processors other than the CPU. In this case, the processor is a PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacturing, such as an FPGA (Field Programmable Gate Array), and an ASIC (Application Specific Integrated Circuit), which is dedicated to executing specific processing. An example is a dedicated electric circuit, which is a processor having a circuit configuration designed for In addition, the process of acquiring travel information from each sensor may be executed by one of these various processors, or a combination of two or more processors of the same or different types (for example, multiple FPGAs and It may be executed by a combination of a CPU and an FPGA, etc.). More specifically, the hardware structure of these various processors is an electric circuit in which circuit elements such as semiconductor elements are combined.

Further, in the above-described embodiment, the program for the sensor error correction processing has been pre-stored (installed) in the storage 24, but the present invention is not limited to this. The program may be provided in a form recorded on a recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), and a USB (Universal Serial Bus) memory. Also, the program may be downloaded from an external device via a network.

10 vehicle 11 angular velocity sensor 12 acceleration sensor 13 wheel speed sensor 20 sensor error correction device 21 CPU
22 ROMs
23 RAM
24 storage 25 display unit 26 input unit 27 bus 31 acquisition unit 32 estimation unit 33 correction unit

Claims (5)

Includes values detected from each of an angular velocity sensor mounted on the mobile body that detects the rotational motion of the mobile body, an acceleration sensor that detects the translational motion of the mobile body, and a wheel speed sensor that detects the speed of the wheels of the mobile body an acquisition unit that acquires travel information;
Using the value detected by the angular velocity sensor of the travel information, a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving body is estimated, and the acceleration sensor and the wheel speed sensor of the travel information are used to estimate an estimating unit that uses the detected value to estimate a second angle change amount that indicates the amount of rotation of the moving body in the pitch angle direction;
a correction unit that derives an error in the travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit based on the error;
with
The error in the travel information is an error in an acceleration sensor in the left-right direction along the vehicle width direction of the moving object,
The correction unit corrects the value of the acceleration sensor in the horizontal direction of the travel information.
Sensor error correction device. The travel information is acquired by the acquisition unit during a period from when the moving object starts to when it finishes turning.
2. The sensor error correction device according to claim 1. 3. The correcting unit further corrects the traveling information by using the error of the derived traveling information and the error of the traveling information previously derived by the correcting unit. Sensor error correction device. The estimation unit further estimates a roll angle from the travel information,
The correction unit further derives an error of the roll angle and corrects the roll angle.
The sensor error correction device according to any one of claims 1 to 3 . Includes values detected from each of an angular velocity sensor mounted on the mobile body that detects the rotational motion of the mobile body, an acceleration sensor that detects the translational motion of the mobile body, and a wheel speed sensor that detects the speed of the wheels of the mobile body a step of obtaining travel information;
Using the value detected by the angular velocity sensor of the travel information, a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving body is estimated, and the acceleration sensor and the wheel speed sensor of the travel information are used to estimate estimating a second angle change amount indicating the amount of rotation of the moving body in the pitch angle direction using the detected value;
a step of deriving an error of the travel information from the difference between the first angle change amount and the second angle change amount, and correcting the acquired travel information based on the error;
including
The error in the travel information is an error in an acceleration sensor in the left-right direction along the vehicle width direction of the moving object,
The correcting step corrects the value of the acceleration sensor in the horizontal direction of the travel information.
A sensor error correction program that causes a computer to execute processing .

Images