Abnormality determination device and method, vehicle state estimation device and readable storage medium

Abstract

The present invention provides an abnormality determination device and method, a vehicle state estimation device and a readable storage medium. The abnormality determination device is used to determine whether a 6-axis inertial measurement sensor installed in a vehicle to detect the front and rear acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate of the vehicle has an abnormality, and the abnormality determination device includes: a 3-axis inertial measurement sensor, which detects the front and rear acceleration, lateral acceleration and yaw rate; and an abnormality determination unit, which determines whether the 6-axis inertial measurement sensor has an abnormality, wherein the abnormality determination unit determines whether the 6-axis inertial measurement sensor has an abnormality by comparing the front and rear acceleration, lateral acceleration and yaw rate obtained by the 6-axis inertial measurement sensor with the front and rear acceleration, lateral acceleration and yaw rate obtained by the 3-axis inertial measurement sensor.

Description

Technical Field

[0001] The present invention relates to an abnormality determination device, an abnormality determination method and an abnormality determination program (stored in a non-transitory computer-readable storage medium) for determining whether an inertial measurement sensor for measuring acceleration and angular velocity (particularly a 6-axis inertial measurement sensor for estimating the velocity and attitude angle of a vehicle) has an abnormality. The present invention also relates to a vehicle state estimation device for estimating a vehicle state based on a measurement value obtained by the 6-axis inertial measurement sensor. Background Art

[0002] There is known a vehicle motion detection device for detecting motion of a vehicle, the vehicle motion detection device including: a motion detection portion for detecting motion of the vehicle; and a fault detection portion for detecting a fault (abnormality) of the motion detection portion (for example, JP6663006B2).

[0003] The motion detection section includes a 6-axis inertial sensor that detects roll rate, pitch rate, yaw rate, and acceleration in the front-rear direction (longitudinal direction), lateral direction, and vertical direction of the vehicle.

[0004] The fault detection portion includes a 3-axis inertial sensor, a wheel speed sensor, and a pressure sensor. The 3-axis inertial sensor detects the roll rate and the acceleration of the vehicle in the vertical and lateral directions. The wheel speed sensor measures the wheel speeds of at least two wheels, and the fault detection portion detects the front-rear (longitudinal) acceleration based on the measured wheel speeds. The pressure sensor measures the brake fluid pressure of the front and rear wheels, and the fault detection portion detects the yaw rate based on the measured brake fluid pressure.

[0005] The fault detection section compares the vertical acceleration, lateral acceleration and roll rate of the vehicle obtained by the 6-axis inertial sensor in the motion detection section with the vertical acceleration, lateral acceleration and roll rate of the vehicle obtained by the 3-axis inertial sensor, respectively. In addition, the fault detection section compares the front-rear acceleration and yaw rate of the vehicle obtained by the 6-axis inertial sensor in the motion detection section with the front-rear acceleration based on wheel speed detection and the yaw rate based on brake fluid pressure detection. When there is a predetermined mismatch between the corresponding detection values, the fault detection section determines that there is a fault (abnormality) in the 6-axis inertial sensor.

[0006] In an autonomous vehicle that can travel autonomously, in order to accurately identify the position of the vehicle, it is particularly necessary to correctly determine the abnormality of the sensor for detecting the front and rear acceleration of the vehicle. Since the fault detection portion of JP6663006B2 detects the front and rear acceleration based on the wheel speed, when the wheel slips, the front and rear acceleration detected based on the wheel speed deviates from the true value, and therefore, the fault detection portion may erroneously determine that there is a fault (abnormality) in the 6-axis inertial sensor. Summary of the invention

[0007] In view of the above background, the main object of the present invention is to provide an abnormality determination device, an abnormality determination method, an abnormality determination program (stored in a non-temporary computer-readable storage medium) and a vehicle state estimation device, which can correctly determine the abnormality of a 6-axis inertial measurement sensor that obtains the vehicle's fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate.

[0008] In order to achieve the above objectives, one aspect of the present invention provides an abnormality determination device, which is used to determine whether a 6-axis inertial measurement sensor 5 installed in a vehicle S to detect the fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate of the vehicle has an abnormality, and the abnormality determination device includes: a 3-axis inertial measurement sensor 7, which detects the fore-and-aft acceleration, the lateral acceleration and the yaw rate; and an abnormality determination unit, which determines whether the 6-axis inertial measurement sensor has an abnormality, wherein the abnormality determination unit determines whether the 6-axis inertial measurement sensor has an abnormality by respectively comparing the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 6-axis inertial measurement sensor with the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 3-axis inertial measurement sensor.

[0009] According to this aspect, it is possible to determine whether there is an abnormality in the 6-axis inertial measurement sensor by comparing the front-rear acceleration of the vehicle acquired by the 6-axis inertial sensor with the front-rear acceleration of the vehicle acquired by the 3-axis inertial sensor. Therefore, even in the case of wheel slip, it is possible to correctly determine the abnormality of the 6-axis inertial sensor in the front-rear acceleration direction.

[0010] In order to achieve the above purpose, another aspect of the present invention provides an abnormality determination method, which is used to determine whether there is an abnormality in a 6-axis inertial measurement sensor 5 installed in a vehicle S to detect the fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate of the vehicle, and the abnormality determination method includes the following steps: obtaining the fore-and-aft acceleration, the lateral acceleration and the yaw rate from a 3-axis inertial measurement sensor 7 installed in the vehicle to detect the fore-and-aft acceleration, the lateral acceleration and the yaw rate; and determining whether there is an abnormality in the 6-axis inertial measurement sensor by respectively comparing the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 6-axis inertial measurement sensor with the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 3-axis inertial measurement sensor.

[0011] According to this aspect, it is possible to determine whether there is an abnormality in the 6-axis inertial measurement sensor by comparing the front-rear acceleration of the vehicle acquired by the 6-axis inertial sensor with the front-rear acceleration of the vehicle acquired by the 3-axis inertial sensor. Therefore, contrary to the case where the wheel speed is used, even in the case of wheel slip, it is possible to correctly determine an abnormality in the front-rear acceleration direction of the 6-axis inertial sensor.

[0012] In order to achieve the above objective, another aspect of the present invention provides a non-transitory computer-readable storage medium, which includes a stored program, wherein the stored program performs the method according to claim 2 when executed by a processor.

[0013] According to this aspect, it is possible to determine whether there is an abnormality in the 6-axis inertial measurement sensor by comparing the front-rear acceleration of the vehicle acquired by the 6-axis inertial sensor with the front-rear acceleration of the vehicle acquired by the 3-axis inertial sensor. Therefore, contrary to the case where the wheel speed is used, even in the case of wheel slip, it is possible to correctly determine an abnormality in the front-rear acceleration direction of the 6-axis inertial sensor.

[0014] In order to achieve the above purpose, another aspect of the present invention provides a vehicle state estimation device 46, which is used to estimate the vehicle state based on the measurement values ​​obtained by the 6-axis inertial measurement sensor 5 installed in the vehicle S to detect the fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate of the vehicle, and the vehicle state estimation device includes: a 3-axis inertial measurement sensor 7, which detects the fore-and-aft acceleration, the lateral acceleration and the yaw rate; and an abnormality determination unit 31, which determines whether the 6-axis inertial measurement sensor has an abnormality, wherein the abnormality determination unit determines whether the 6-axis inertial measurement sensor has an abnormality by respectively comparing the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 6-axis inertial measurement sensor with the fore-and-aft acceleration, the lateral acceleration and the yaw rate obtained by the 3-axis inertial measurement sensor.

[0015] According to this aspect, it is possible to determine whether there is an abnormality in the 6-axis inertial measurement sensor by comparing the front-rear acceleration of the vehicle acquired by the 6-axis inertial sensor with the front-rear acceleration of the vehicle acquired by the 3-axis inertial sensor. Therefore, contrary to the case where the wheel speed is used, even in the case of wheel slip, it is possible to correctly determine an abnormality in the front-rear acceleration direction of the 6-axis inertial sensor.

[0016] In the above aspects, preferably, the vehicle state estimation device includes: a wheel speed sensor 9, which is used to measure the wheel speed of the wheels set on the vehicle; and a vehicle state estimation unit 31, which is used to calculate the attitude angle of the vehicle based on the measurement values ​​measured by the wheel speed sensor and the 6-axis inertial measurement sensor, wherein, when the difference between the attitude angle calculated by the vehicle state estimation unit and the steady-state attitude angle based on the gravity direction calculated using the wheel speed when the vehicle is traveling without slipping of the wheels becomes greater than or equal to a specified threshold, the abnormality determination unit determines that there is an abnormality in the 6-axis inertial measurement sensor or the wheel speed sensor.

[0017] According to this aspect, the steady-state attitude angle and the attitude angle calculated by performing the integration operation are obtained, and therefore, by calculating the difference between the two, abnormality of the 6-axis inertial measurement sensor and / or the wheel speed sensor can be detected.

[0018] In the above aspects, preferably, when the difference between the roll angle calculated by the vehicle state estimation unit and the roll angle of the steady-state attitude angle is greater than or equal to a specified threshold, or when the difference between the pitch angle calculated by the vehicle state estimation unit and the pitch angle of the steady-state attitude angle is greater than or equal to a specified threshold, the abnormality determination unit determines that there is an abnormality in the 6-axis inertial measurement sensor or the wheel speed sensor.

[0019] According to this aspect, it is possible to detect abnormalities in the wheel speed sensor and abnormalities in the roll direction and the pitch direction of the six-axis inertial measurement sensor.

[0020] According to the above configuration, it is possible to provide an abnormality determination device, an abnormality determination method, an abnormality determination program (stored in a non-temporary computer-readable storage medium) and a vehicle state estimation device that can correctly determine the abnormality of a 6-axis inertial measurement sensor that obtains the vehicle's fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate and yaw rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 is a perspective view of a vehicle equipped with a vehicle state estimation device according to an embodiment of the present invention;

[0022] Figure 2 is a functional configuration diagram of a vehicle state estimation device;

[0023] Figure 3 is a flowchart of an abnormality determination process; and

[0024] Figure 4A and Figure 4BIt is a graph showing the true value of the lateral velocity when the vehicle is traveling without slipping (grip driving) and when the vehicle is slipping (slip driving), the steady-state vehicle velocity, the instantaneous vehicle velocity and the estimated value of the vehicle velocity (estimated vehicle velocity), respectively. DETAILED DESCRIPTION

[0025] One embodiment in which an abnormality determination device, an abnormality determination method, an abnormality determination program (stored in a non-transitory computer-readable storage medium), and a vehicle state estimation device according to the present invention are applied to a vehicle S capable of autonomous driving will be described below with reference to the accompanying drawings.

[0026] The abnormality determination device, the abnormality determination method, the abnormality determination program, and the vehicle state estimation device according to the embodiment are provided in a vehicle S capable of autonomous driving.

[0027] Hereinafter, for convenience of explanation, a coordinate system (eg, Figure 1 ) will be referred to as the fixed coordinate system (also called the inertial coordinate system).

[0028] like Figure 2 As shown in FIG, a vehicle S is provided with a vehicle control system 3 for controlling the behavior of the vehicle S so that the vehicle S can travel autonomously. The vehicle control system 3 includes: two inertial measurement sensors 5, 7; a wheel speed sensor 9; an external environment recognition device (not shown in the figure); a steering and acceleration / deceleration device 13; a human-machine interface (HMI) 15; and a control device 17.

[0029] The inertial measurement sensors 5 and 7 are fixed to the vehicle body, respectively. One of the inertial measurement sensors 5 and 7 (in the present embodiment, the main inertial measurement sensor 5) is a so-called six-axis inertial measurement sensor, and detects acceleration G along three axes. sens and the angular velocities ω about the three axes.

[0030] The main inertial measurement sensor 5 measures the acceleration G sens The main inertial measurement sensor 5 outputs an acceleration G with respect to a coordinate system having an X-axis, a Y-axis, and a Z-axis and fixed to the main inertial measurement sensor 5 (ie, fixed to the vehicle body). sens. Hereinafter, this coordinate system will be referred to as the acceleration coordinate system. Note that the main inertial measurement sensor 5 is fixed to the vehicle body so that the positive direction of the x-axis coincides with the forward direction of the vehicle body, the positive direction of the y-axis coincides with the left direction of the vehicle body, and the positive direction of the z-axis coincides with the upward direction of the vehicle body. That is, the x-axis is set to extend in the front-to-rear direction of the vehicle body, the y-axis is set to extend in the lateral direction of the vehicle body, and the z-axis is set to extend in the vertical direction of the vehicle body. Below, for ease of explanation, the origin of the acceleration coordinate system is located at the position where the main inertial measurement sensor 5 is set.

[0031] In the present embodiment, the main inertial measurement sensor 5 includes three shells held by the vehicle body, and each shell has a detection electrode. Each shell has a counterweight contained therein, so that the counterweight is held in the shell in one of the x-axis, y-axis, and z-axis directions via two springs. The main inertial measurement sensor 5 measures the inertial force applied to each counterweight by detecting the change in electrostatic capacitance between the counterweight and the corresponding detection electrode, divides the inertial force by the mass of the counterweight, and reverses the direction (or sign) of the value obtained by the division, thereby calculating the acceleration and outputting the acceleration. Please note that the main inertial measurement sensor 5 is not limited to this configuration, and can be embodied by using a piezoelectric element, for example, when deformed due to the inertial force, the piezoelectric element generates a voltage according to the deformation. In this case, the main inertial measurement sensor 5 can obtain the inertial force by detecting the voltage generated by the piezoelectric element, and calculate and output the acceleration based on the inertial force.

[0032] If the acceleration G output from the main inertial measurement sensor 5 sens The x component, y component and z component of G are sensx , G sensy , G sensz , then the acceleration G acting on the vehicle S in the fixed coordinate system (G = ΣF / m, where ΣF represents the sum of the forces acting on the vehicle S except for gravity, and m represents the mass of the vehicle S) satisfies the following formula (1):

[0033]

[0034] Please note that i, j and k represent the unit vectors in the x-axis, y-axis and z-axis directions in the acceleration coordinate system, respectively, and Gv represents the gravitational acceleration. Formula (1) shows that due to the gravitational acceleration Gv (G sens =G-Gv), the acceleration G measured by the main inertial measurement sensor 5 sens Since there is a deviation from the acceleration G acting on the vehicle S, it is necessary to correct the acceleration G measured by the main inertial measurement sensor 5 while taking into account the influence of the gravity acceleration Gv. sens , to obtain the acceleration G acting on the vehicle S.

[0035] The deviation due to the gravitational acceleration Gv can be explained as follows. When the sum of the forces applied to the vehicle S is 0 and the vehicle S is stopped, according to formula (1), the acceleration G output from the main inertial measurement sensor 5 is sens =Equal to -1 times the gravitational acceleration Gv. This is because the main inertial measurement sensor 5 calculates acceleration based on inertial force and cannot distinguish the gravity acting on the counterweight from the inertial force. In other words, the main inertial measurement sensor 5 regards the vertically downward gravity acting on the counterweight as an inertial force caused by the vertically upward acceleration of the shell (vehicle body) and outputs the acceleration accordingly. The correction taking into account the influence of the gravitational acceleration Gv is a correction taking into account the influence of the acceleration calculated by the main inertial measurement sensor 5 based on inertial force.

[0036] The acceleration G measured by the primary inertial measurement sensor 5 sens The main inertial measurement sensor 5 outputs an angular velocity ω relative to the acceleration coordinate system, which is related to the acceleration G sens Similarly, the x component of the angular velocity ω corresponds to the roll rate, the y component of the angular velocity ω corresponds to the pitch rate, and the z component of the angular velocity ω corresponds to the yaw rate. The main inertial measurement sensor 5 acquires the angular velocity ω and the acceleration G sens Output to the control device 17.

[0037] The other of the inertial measurement sensors 5 and 7 (hereinafter referred to as the auxiliary inertial measurement sensor 7) is a so-called three-axis inertial measurement sensor, and acquires the acceleration G′ in the x-axis direction in a fixed coordinate system based on a similar principle to the main inertial measurement sensor 5. sensx , acceleration in the y-axis direction G' sensy and the angular velocity ω' about the z-axis z , and output them to the control device 17.

[0038] For example, the auxiliary inertial measurement sensor 7 may be a sensor provided in a device not specifically designed for the purpose of estimating the vehicle state, and may be a sensor provided in a side slip control system (also referred to as a vehicle stability assist system (VSA)).

[0039] The wheel speed sensor 9 acquires the wheel speed of at least one wheel of the vehicle S. The wheel speed sensor 9 can acquire the wheel speed of the driving wheel. The wheel speed sensor 9 outputs the acquired wheel speed to the control device 17.

[0040] The external environment recognition device is a device for acquiring information about the surrounding environment of the vehicle S, including, for example, a vehicle-mounted camera, a radar, a laser radar, etc.

[0041] The steering and acceleration / deceleration device 13 performs steering and acceleration / deceleration of the vehicle S. The steering and acceleration / deceleration device 13 includes: a steering device for steering the vehicle S; a driving device for providing driving force to the vehicle S, such as an internal combustion engine or a motor; and a braking device for decelerating the vehicle S.

[0042] The HMI 15 is a device for receiving input from the occupant and notifying the occupant. In the present embodiment, the HMI 15 is composed of a touch panel. When the HMI 15 receives input from the occupant to instruct the start of autonomous driving, but the estimation accuracy of the vehicle state is low, the HMI 15 notifies that autonomous driving is not possible.

[0043] The control device 17 is a so-called microcomputer provided with a processor, RAM, etc., and is connected to the two inertial measurement sensors 5 , 7 , the wheel speed sensor 9 , the external environment recognition device, the steering and acceleration / deceleration device 13 , and the HMI 15 .

[0044] The control device 17 includes: a vehicle state estimator 31 (vehicle state estimation unit) that estimates the vehicle state; an abnormality determiner 33 (abnormality determination unit) that determines the abnormality of the main inertial measurement sensor 5; a state management unit 35 that manages the autonomous driving level of the vehicle S based on the input to the HMI 15; an own vehicle position estimation unit 37 that estimates the own vehicle position; and a driving control unit 39 that controls the steering and acceleration / deceleration of the vehicle S to make the vehicle S autonomously drive. The vehicle state estimated by the vehicle state estimator 31 includes the vehicle speed, the time rate of change of the vehicle speed (hereinafter referred to as motion acceleration), and the attitude angle.

[0045] When there is an input from the HMI 15 instructing to start autonomous driving, the abnormality determiner 33 determines whether there is an abnormality in the main inertial measurement sensor 5. When the abnormality determiner 33 determines that there is no abnormality in the main inertial measurement sensor 5, the state management unit 35 provides an instruction to the own vehicle position estimation unit 37 so that the own vehicle position estimation unit 37 estimates the own vehicle position based on the signal from the external environment recognition device and the vehicle state estimated by the vehicle state estimator 31 based on the measurement value from the main inertial measurement sensor 5 (6-axis inertial measurement sensor). The driving control unit 39 controls the steering and acceleration / deceleration device 13 by using the own vehicle position estimated by the own vehicle position estimation unit 37 so that the vehicle S drives autonomously.

[0046] The vehicle control system 3 is further provided with a slip determination device 41 (determination unit) which determines whether the wheel is in a slip state (or a slipping state). The slip determination device 41 includes an anti-lock brake system (ABS) 43 and a traction control system (TCS) 45.

[0047] The anti-lock braking system 43 prevents the wheels from stopping rotating (or locking the wheels) due to sudden braking, thereby preventing the wheels from slipping. In the present embodiment, the anti-lock braking system 43 determines whether the wheels are locked and slipping based on the wheel speed and the vehicle speed. When it is determined that the wheels are slipping, the anti-lock braking system 43 outputs a flag signal indicating that the wheels are slipping to the control device 17, releases the braking force to restore the wheel speed, and then repeatedly maintains and increases the braking pressure to decelerate the vehicle S.

[0048] The traction control system 45 prevents the wheels from spinning at the time of starting or sudden acceleration. In the present embodiment, the traction control system 45 determines whether the wheels are spinning and slipping by comparing the wheel speed and the vehicle speed. When it is determined that the wheels are spinning and slipping, the traction control system 45 outputs a flag signal indicating that the wheels are slipping to the control device 17, and controls the rotation speed of the drive wheels.

[0049] Next, the vehicle state estimator 31 will be described in detail. The vehicle state estimator 31 calculates estimated values ​​of the vehicle state including vehicle speed, motion acceleration and attitude angle based on signals from the main inertial measurement sensor 5, the wheel speed sensor 9 and the slip determination device 41, and updates the vehicle state with the calculated estimated values.

[0050] When the vehicle speed is expressed as u(=u x i+u y j+u z k), the motion acceleration a is expressed by the following formula (2).

[0051]

[0052] The motion acceleration a is the time change of the vehicle speed relative to the acceleration coordinate system, and is also called the relative derivative of the vehicle speed. On the other hand, the time change of the vehicle speed relative to the fixed coordinate system (du / dt) corresponds to the acceleration G in the fixed coordinate system, and is also called the absolute derivative of the vehicle speed. The acceleration G and the motion acceleration meet the following relationship.

[0053]

[0054] The vehicle state estimator 31 outputs the attitude angle by using the Euler angle (or Tait-Bryan angle). When , the pitch angle is denoted by θ, and the yaw angle is denoted by ψ, the attitude angle Ω is expressed by the following formula (4).

[0055] Ω=[φ θ ψ] T (4)

[0056] In the description below, the estimated value of the vehicle speed will be referred to as the estimated vehicle speed v, the estimated value of the motion acceleration will be referred to as the estimated motion acceleration α, the estimated value of the attitude angle will be referred to as the estimated attitude angle θ, the estimated vehicle speed before updating will be referred to as the pre-updated estimated vehicle speed v0, and the estimated attitude angle before updating will be referred to as the pre-updated estimated attitude angle θ0.

[0057] The vehicle state estimator 31 (vehicle state estimation unit) includes, as functional units for estimating the vehicle state, an activation determination unit 51, a motion acceleration calculation unit 53, a motion acceleration correction / calculation unit 55, a motion acceleration integrator 57, an attitude angle change rate calculation unit 59 (also referred to as an angular velocity coordinate conversion unit), a gravity term estimation unit 61, an angle resolution unit 63, an attitude angle change rate correction / calculation unit 65, and an attitude angle change rate integrator 67. These functional units are configured by the processor executing a vehicle state estimation program.

[0058] The activation determination unit 51 sets an observer gain related to speed estimation (hereinafter referred to as acceleration gain K) and an observer gain related to attitude estimation (hereinafter referred to as attitude angle gain K) based on the flag signal calculated by the anti-lock brake system 43 and the flag signal from the traction control system 45. ANG ). The activation determination unit 51 sets the acceleration gain K and the attitude angle gain K ANG Output to the motion acceleration correction / calculation unit 55.

[0059] Note that when any one of the flag signal indicating activation of the anti-lock braking system 43 from the anti-lock braking system 43 and the flag signal indicating activation of the traction control system 45 from the traction control system 45 is input, the activation determination unit 51 determines that the wheel is slipping and sets the acceleration gain K and the attitude angle gain K to the value of the activation determination unit 51. ANG Each of is set to be smaller than the other.

[0060] The motion acceleration calculation unit 53 is configured to receive: the angular velocity ω and the acceleration G acquired by the main inertial measurement sensor 5 sens ; pre-updated estimated vehicle speed v0; and pre-updated estimated attitude angle The motion acceleration calculation unit 53 calculates the gravity acceleration in the acceleration coordinate system according to formula (5):

[0061]

[0062] Please note that in formula (5), g represents the magnitude of gravity acceleration (approximately 9.8 m / s 2 ).

[0063] Thereafter, the motion acceleration calculation unit 53 calculates the gravitational acceleration calculated according to formula (5) by The pre-updated estimated vehicle velocity v0, the angular velocity ω and the acceleration G obtained by the inertial measurement sensor sens Enter the following formula (6) to calculate the instantaneous motion acceleration α w Thereafter, the motion acceleration calculation unit 53 converts the pre-updated estimated vehicle speed v0 and the instantaneous motion acceleration α w Output to the motion acceleration correction / calculation unit 55.

[0064] α w =G v (θ0,φ0)+G sens -ω×v0 (6)

[0065] According to formula (1) and formula (3), it can be understood that formula (6) is derived from formula (1) and formula (3).

[0066] In addition, the motion acceleration calculation unit 53 calculates the drag acceleration α according to the following formula (7) by using the angular velocity ω acquired by the main inertial measurement sensor 5 and the pre-updated estimated vehicle speed v0: c (also called centripetal acceleration), and will involve acceleration α c Output to the gravity term estimation unit 61.

[0067] α c =ω×v0 (7)

[0068] The motion acceleration correction / calculation unit 55 is configured to receive the wheel speed from the wheel speed sensor 9, the pre-updated estimated vehicle speed v0 from the motion acceleration calculation unit 53, and the motion acceleration α w And the acceleration gain K from the activation determination unit 51 .

[0069] The motion acceleration correction / calculation unit 55 obtains the steady-state vehicle velocity v based on the wheel speed input from the wheel speed sensor 9. meas , which is an estimate of the vehicle speed. In the absence of wheel slip, the steady-state vehicle speed v meas is roughly matched to the vehicle velocity. However, in the case of wheel slip, the steady-state vehicle velocity v meas Does not match vehicle speed.

[0070] Furthermore, the motion acceleration correction / calculation unit 55 calculates the motion acceleration α by w , pre-updated estimated vehicle speed v0, steady-state vehicle speed v meas And the acceleration gain K are input into the following formula (8) to calculate and update the estimated motion acceleration α.

[0071] α=α w -K(v0-v mes ) (8)

[0072] The right side of formula (8) corresponds to using the feedback term (-K(v0-v meas )) for the instantaneous motion acceleration α w By referring to formula (2), it can be understood that formula (8) is a differential equation about the rate, and formula (8) corresponds to the so-called observer equation about the rate (hereinafter, formula (8) will be referred to as the rate observer equation). In other words, the motion acceleration correction / calculation unit 55 calculates the estimated motion acceleration α by using the rate observer equation (formula (8)), which is a differential equation about the rate, in which the instantaneous motion acceleration α is corrected using the feedback term w .

[0073] The motion acceleration correction / calculation unit 55 outputs the calculated estimated motion acceleration α to the gravity term estimation unit 61 and the own vehicle position estimation unit 37 .

[0074] The motion acceleration integrator 57 is configured to receive the estimated motion acceleration α from the motion acceleration correction / calculation unit 55. The motion acceleration integrator 57 time-integrates the estimated motion acceleration α and updates the estimated vehicle speed v with a value (∫αdt) obtained by the time integration.

[0075] Note that when performing time integration on the estimated motion acceleration α, the motion acceleration integrator 57 may ignore the time variation of the unit vectors i, j, k, and separately integrate the x component, y component, and z component of the estimated motion acceleration α to obtain the estimated vehicle velocity v. Specifically, for example, the motion acceleration integrator 57 may utilize the pre-updated estimated vehicle velocity v0 (assuming that its x, y, and z components are v0x, v0y, and v0z, respectively, when calculating) and the estimated motion acceleration α (assuming that its x, y, and z components are α, respectively, when calculating) by using the so-called Euler method. x , α y , α z More specifically, the motion acceleration integrator 57 can calculate (v 0y +α x Δt)i+(v 0y +α y Δt)j+(v 0z +α zΔt)k (note that i, j, k are unit vectors in the x, y, z directions when calculating the estimated vehicle speed, respectively, and Δt is the control interval of the estimation calculation, more specifically, the time elapsed from the calculation of the pre-updated estimated vehicle speed v0), and the calculated value is output as the estimated vehicle speed v.

[0076] The motion acceleration integrator 57 outputs the updated estimated vehicle speed v to the motion acceleration calculation unit 53 and the travel control unit 39 .

[0077] The attitude angle change rate calculation unit 59 is configured to receive the pre-updated attitude angle estimate (pre-updated estimated attitude angle θ0) and the angular velocity ω acquired by the main inertial measurement sensor 5. The attitude angle change rate calculation unit 59 inputs the angular velocity ω and the pre-updated estimated attitude angle θ0 into the following formula (9) to calculate the attitude angle change rate: (This is the time rate of change of the attitude angle), and outputs it to the attitude angle change rate correction / calculation unit 65.

[0078]

[0079] Formula (9) shows that the attitude angle change rate It corresponds to the attitude angle change rate predicted by performing coordinate conversion of the angular velocity ω.

[0080] The gravity term estimation unit 61 is configured to receive the acceleration G acquired by the inertial measurement sensor. sens , the estimated motion acceleration α updated by the motion acceleration correction / calculation unit 55, and the involved acceleration α from the motion acceleration calculation unit 53 c The gravity term estimation unit 61 converts the acceleration G sens , the updated estimated motion acceleration α and the implicated acceleration α c Enter the following formula (10) to calculate the gravity term G std , and outputs it to the angle decomposition unit 63.

[0081] G std =α+α c -G sens (=-(G sens -α-α c )) (10)

[0082] Formula (10) shows that the gravity term G std Corresponds to the value obtained by the following operation: From the acceleration G acquired by the inertial measurement sensor sens Subtract the estimated motion acceleration α0 and the involved acceleration α c(=ω×v0), and considering that the gravitational acceleration is calculated based on the inertial force, the subtraction result is inverted.

[0083] Gravity term G std is input from the gravity term estimation unit 61 to the angle resolution unit 63. The angle resolution unit 63 obtains the gravity term G based on the following formula (11): std Quantity g estX , g estY and g estZ .

[0084]

[0085] Then, the angle resolution unit 63 converts the gravity term G std Input into the following formula (12) to obtain the steady-state attitude angle θ grav , and the steady-state attitude angle Θ grav Output to the attitude angle change rate correction / calculation unit 65 (here, g is about 9.8 m / s 2 ).

[0086]

[0087] Note that in formula (12), there is no gravity term G std The yaw angle ψ is calculated, and only the roll angle is calculated and the pitch angle θ. By referring to formula (5), it can be understood that formulas (11) and (12) indicate that the steady-state attitude angle θ grav It is done by using the estimated gravitational acceleration direction (i.e. the gravity term G std ) The estimated attitude angle vertically downward.

[0088] The attitude angle change rate correction / calculation unit 65 is configured to receive the steady-state attitude angle θ from the angle resolution unit 63. grav and the attitude angle gain K for attitude estimation from the activation determination unit 51 ANG By using the steady-state attitude angle θ input from the angle resolution unit 63 grav , about the attitude angle gain K of attitude estimation ANG And the following formula (13) of the pre-updated estimated attitude angle Θ0, the attitude angle change rate correction / calculation unit 65 obtains the time change rate ΔΘ of the estimated attitude angle (i.e., the time derivative of the estimated attitude angle) and outputs it to the attitude angle change rate integrator 67.

[0089] ΔΘ=ΔΘ T (θ0,φ0,ω)-K ANG (Θ0-Θ grav ) (13)

[0090] The right side of formula (13) corresponds to using the feedback term (-K ANG (Θ0-Θ grav ) for attitude angle change rate Formula (13) is a differential equation about the attitude angle, and corresponds to the so-called observer equation about the attitude angle (hereinafter, formula (13) will be referred to as the attitude angle observer equation). In other words, the attitude angle change rate correction / calculation unit 65 calculates the estimated attitude angle time change rate by using the attitude angle observer equation (formula (13)), wherein the attitude angle change rate is corrected using the feedback term

[0091] In addition, the attitude angle change rate correction / calculation unit 65 calculates the pre-updated estimated attitude angle θ0 and the gravity term G std Estimated steady-state attitude angle Θ grav Output to the abnormality determiner 33.

[0092] The attitude angle change rate integrator 67 is configured to receive the estimated attitude angle time change rate Δθ. The attitude angle change rate integrator 67 time-integrates the estimated attitude angle time change rate Δθ and updates the estimated attitude angle θ with the value obtained by the time integration (ie, ∫Δθdt).

[0093] More specifically, the attitude angle change rate integrator 67 can calculate the value of the time integration by adding the product of the time Δt required before updating and the time change rate ΔΘ of the estimated attitude angle to the pre-updated estimated attitude angle Θ0, and update the estimated attitude angle Θ with the value of the time integration (Θ0+ΔΘΔt).

[0094] The attitude angle change rate integrator 67 outputs the updated estimated attitude angle θ to the motion acceleration calculation unit 53 and the travel control unit 39.

[0095] The control device 17 (more specifically, the vehicle state estimator 31 and the abnormality determiner 33), the auxiliary inertial measurement sensor 7 (3-axis inertial measurement sensor), the vehicle speed sensor 9 and the slip determination device 41 constitute a vehicle state estimation device 46, which estimates the vehicle state including the vehicle speed and attitude angle based on the measurement values ​​obtained by the main inertial measurement sensor 5 (6-axis inertial measurement sensor).

[0096] The control device (more specifically, the abnormality determiner 33 ) and the auxiliary inertial measurement sensor 7 (3-axis inertial measurement sensor) constitute an abnormality determination device 68 that determines an abnormality of the main inertial measurement sensor 5 (6-axis inertial measurement sensor).

[0097] Next, the abnormality determiner 33 will be described. The abnormality determiner 33 executes an abnormality determination program so as to always perform an abnormality determination while the vehicle S is autonomously traveling. Figure 3 The abnormality determination processing shown in is performed to execute the abnormality determination method for determining whether an abnormality exists in the main inertial measurement sensor 5. Figure 3 The flowchart of FIG. 1 describes the abnormality determination processing performed by the abnormality determiner 33 .

[0098] In the first step ST1 of the abnormality determination process, the abnormality determiner 33 determines the acceleration G acquired by the primary inertial measurement sensor 5. sens The x component G sensx Is it consistent with the acceleration G' in the x direction obtained by the auxiliary inertial measurement sensor 7? sensx The abnormality determiner 33 can determine the acceleration G obtained by the main inertial measurement sensor 5 by performing a correlation comparison between the two. sens The x component G sensx Is it consistent with the acceleration G' in the x direction obtained by the auxiliary inertial measurement sensor 7? sensx In addition, when the difference between the two is less than or equal to a prescribed threshold, the abnormality determiner 33 may determine that the acceleration G acquired by the primary inertial measurement sensor 5 is sens The x component G sensx The acceleration G′ in the x direction obtained by the auxiliary inertial measurement sensor 7 sensx When determining that the two values ​​match each other, the abnormality determiner 33 executes step ST2, and when determining that they do not match, executes step ST3.

[0099] In step ST2, the abnormality determiner 33 determines whether the acceleration G acquired by the primary inertial measurement sensor 5 sens The y component G sensy Is it consistent with the acceleration G' in the y direction obtained by the auxiliary inertial measurement sensor 7? sensy The abnormality determiner 33 can determine the acceleration G obtained by the main inertial measurement sensor 5 by performing a correlation comparison between the two. sens The y component G sensy Is it consistent with the acceleration G' in the y direction obtained by the auxiliary inertial measurement sensor 7? sensy When the difference between the two is less than or equal to the specified threshold, the abnormality determiner 33 can determine that the acceleration G acquired by the main inertial measurement sensor 5 is sens The y component G sensy The acceleration G′ in the y direction obtained by the auxiliary inertial measurement sensor 7 sensy When determining that the two values ​​match each other, the abnormality determiner 33 executes step ST4, and executes step ST3 when determining that they do not match.

[0100] In step ST3 , the abnormality determiner 33 sets a flag indicating whether the main inertial measurement sensor 5 has an abnormality (sensor abnormality flag) to a value indicating the presence of an abnormality (eg, 1). After completing the setting, the abnormality determiner 33 ends the abnormality determination process.

[0101] In step ST4, the abnormality determiner 33 determines the z component ω of the angular velocity ω acquired by the primary inertial measurement sensor 5 z Is it consistent with the angular velocity ω' about the z direction obtained by the auxiliary inertial measurement sensor 7? z The abnormality determiner 33 can determine ω by performing a correlation comparison between the two. z Is it related to ω' z In addition, when the difference between the two is less than or equal to the specified threshold, the abnormality determiner 33 can determine ω z With ω' z When determining that the two values ​​match each other, the abnormality determiner 33 executes step ST5, and executes step ST3 when determining that they do not match.

[0102] In step ST5, the abnormality determiner 33 determines whether the x and y components of the pre-updated estimated attitude angle θ0 output from the vehicle state estimator 31 (more specifically, from the attitude angle change rate integrator 67) are respectively equal to the steady-state attitude angle θ grav The x and y components of the two pairs of corresponding components match. Preferably, the abnormality determiner 33 calculates the difference between each pair of the two pairs of corresponding components as the estimated error, and when the estimated error of each pair is less than or equal to the corresponding prescribed threshold, determines that the corresponding components of each pair match each other. When it is determined that the corresponding components in the two pairs match each other, the abnormality determiner 33 executes step ST6, and when at least one of the estimated error between the x components and the estimated error between the y components is greater than the corresponding threshold, executes step ST3.

[0103] In step ST6, the abnormality determiner 33 sets the sensor abnormality flag to a value (eg, 0) indicating that the sensor is normal (no abnormality), and outputs the sensor abnormality flag to the state management unit 35. When the output is completed, the abnormality determiner 33 ends the abnormality determination process.

[0104] When the sensor abnormality flag input from the abnormality determiner 33 has a value indicating that the sensor is normal, the state management unit 35 allows the vehicle S to travel autonomously. On the other hand, when the sensor abnormality flag has a value indicating that the sensor has an abnormality, the state management unit 35 prevents the autonomous travel of the vehicle S. More specifically, when the sensor abnormality flag has a value indicating that the sensor has an abnormality, the state management unit 35 does not accept an input to the HMI 15 for selecting the autonomous travel of the vehicle S. In addition, in the case where the sensor abnormality flag having a value indicating that the sensor has an abnormality is input during the autonomous travel of the vehicle S, the state management unit 35 causes the HMI 15 to make a display indicating that there is an abnormality in the sensor. In addition, preferably, the state management unit 35 immediately stops the autonomous travel of the vehicle S and makes the vehicle S transition to a manual driving state in which the vehicle S travels according to the driving operation of the occupant.

[0105] Next, the operation of the vehicle state estimating device 46 configured as above will be described.

[0106] When the flag signal indicating activation of the anti-lock braking system 43 is not output from the anti-lock braking system 43, and the flag signal indicating activation of the traction control system 45 is not output from the traction control system 45 (i.e., when the wheels are not slipping), the activation determination unit 51 increases the acceleration gain K and the attitude angle gain K by 1. ANG Set to a value that is sufficiently larger than when any flag signal is output.

[0107] The vehicle state estimator 31 calculates the estimated vehicle speed according to formula (8). Since the left side of formula (8) corresponds to the motion acceleration (i.e., the derivative of the vehicle speed), formula (8) corresponds to the feedback term (-K(v0-v meas )) is added to the observer equation in the differential equation about the vehicle speed (Formula (6), hereinafter referred to as the acceleration equation). When the activation determination unit 51 sets the acceleration gain K to a sufficiently large value, the vehicle state estimator 31 determines to estimate the vehicle speed so that the feedback term (-K(v0-v meas That is, the vehicle state estimator 31 determines that the estimated vehicle speed is a value close to the steady-state vehicle speed (the contribution of the steady-state vehicle speed is higher than the contribution of the instantaneous vehicle speed obtained by numerical integration of the right side of the acceleration equation).

[0108] The vehicle state estimator 31 calculates the estimated attitude angle according to formula (13). Since the left side of formula (13) corresponds to the derivative of the attitude angle, formula (13) corresponds to the feedback term (-K ANG (Θ0-Θ grav )) is added to the differential equation for the attitude angle (ΔΘ=ΔΘ T, hereinafter referred to as the attitude angle equation). When the activation determination unit 51 sets the attitude angle gain K ANG When set to a sufficiently large value, the vehicle state estimator 31 determines the estimated attitude angle so that the feedback term (-K ANG (Θ0-Θ grav That is, the vehicle state estimator 31 determines that the estimated attitude angle is close to the steady-state attitude angle θ grav The value of the steady-state attitude angle θ grav The contribution of is higher than the contribution of the instantaneous attitude angle obtained by numerical integration of the right side of the attitude angle equation).

[0109] When a flag signal indicating that the anti-lock braking system 43 is activated is output from the anti-lock braking system 43 and / or a flag signal indicating that the traction control system 45 is activated is output from the traction control system 45 (i.e., when the wheels slip), the activation determination unit 51 increases the acceleration gain K and the attitude angle gain K by 1. ANG Set to a sufficiently small value.

[0110] When the activation determination unit 51 sets the acceleration gain K to a sufficiently small value, the vehicle state estimator 31 determines the estimated vehicle speed to be a value close to the instantaneous vehicle speed obtained by numerically integrating the right side of the acceleration equation (the contribution of the instantaneous vehicle speed is higher than that of the steady-state vehicle speed). ANG When set to a sufficiently small value, the vehicle state estimator 31 determines the estimated attitude angle to be a value close to the instantaneous attitude angle obtained by numerical integration of the right side of the attitude angle equation (the contribution of the instantaneous attitude angle is higher than the contribution of the steady-state attitude angle).

[0111] Next, the effects of the vehicle state estimator 31 configured as above will be described.

[0112] When the wheel is not slipping (i.e. not in a slipping state), the vehicle speed obtained according to the wheel speed is close to the true value. On the other hand, when the wheel is in a slipping state, the vehicle speed obtained according to the wheel speed is far from the true value, and the error becomes larger.

[0113] When the wheels are not slipping, Figure 4AAs shown in , the estimated vehicle speed (estimated value of the vehicle speed) (double-dotted chain line) is determined to be a value close to the steady-state vehicle speed acquired by the wheel speed sensor 9 (dashed line). On the other hand, the instantaneous vehicle speed (single-dotted chain line) obtained by numerical integration of the right side of the acceleration equation includes an error due to the numerical integration. Therefore, by determining the estimated vehicle speed to be a value close to the steady-state vehicle speed (i.e., a value closer to the steady-state vehicle speed than to the instantaneous vehicle speed), the estimated vehicle speed can be made closer to the true value (solid line) compared to the case where the estimated vehicle speed is determined to be a value close to the instantaneous vehicle speed, thereby improving the estimation accuracy of the vehicle speed.

[0114] In addition, when the wheels are not slipping, since the vehicle speed obtained based on the wheel speed is close to the true value, and the instantaneous attitude angle obtained by the numerical integration of the right side of the attitude angle equation includes the error caused by the numerical integration, the steady-state attitude angle calculated based on the wheel speed, the acceleration obtained by the inertial measurement sensor, etc. is closer to the true value than the instantaneous attitude angle.

[0115] Therefore, when the wheel is not slipping, the estimated attitude angle (estimated value of the attitude angle) is determined to be close to the steady-state attitude angle (i.e., a value closer to the steady-state attitude angle than to the instantaneous attitude angle). Therefore, the estimation accuracy can be improved compared to the case where the estimated attitude angle is determined to be close to the instantaneous attitude angle.

[0116] On the other hand, when the wheel is slipping (i.e. in a skidding state), if Figure 4B As shown in , the estimated vehicle speed (estimated value of the vehicle speed) (double-dotted dash line) is determined to be a value close to the instantaneous vehicle speed (single-dotted dash line) obtained by numerical integration of the right side of the acceleration equation (i.e., a value closer to the instantaneous vehicle speed than to the steady-state vehicle speed). Therefore, compared with the case where the estimated value of the vehicle speed is determined to be a value close to the steady-state vehicle speed (dashed line), the estimated vehicle speed can be made closer to the true value (solid line), thereby improving the measurement accuracy of the vehicle speed.

[0117] When the wheel is slipping, the instantaneous attitude angle is closer to the true value than the steady-state attitude angle calculated based on the wheel speed. When the wheel is slipping, the estimated attitude angle is determined to be a value close to the instantaneous attitude angle (i.e., a value closer to the instantaneous attitude angle than to the steady-state attitude angle), and therefore, the measurement accuracy of the attitude angle can be improved compared to the case where the estimated attitude angle is determined to be a value close to the steady-state attitude angle.

[0118] like Figure 4AAs shown in , the instantaneous vehicle speed may change over time to deviate from the true value of the vehicle speed. In the present embodiment, the acceleration gain K is large enough, and when the wheel is not slipping, the estimated vehicle speed is calculated to be substantially equal to the steady-state vehicle speed acquired by the wheel speed sensor 9. When the wheel begins to slip, the acceleration gain K changes, and the estimated vehicle speed is calculated to be close to the instantaneous vehicle speed obtained by numerical integration, and the estimated vehicle speed is the initial value when the wheel begins to slip. Therefore, the lower limit (also referred to as the lower end) of the integral interval of the numerical integration used to calculate the instantaneous vehicle speed can be set to the time when slipping begins. Therefore, compared with the case where the lower limit of the integral interval is set to the time when the vehicle starts to move, the integral interval used to calculate the instantaneous vehicle speed can be shortened, and therefore, the error of the estimated vehicle speed (i.e., the instantaneous vehicle speed) during slipping can be reduced.

[0119] The mass S of the vehicle changes depending on the use state of the vehicle S such as the load amount, and the friction coefficient between the road surface and the wheels (tires) also changes greatly depending on the road surface conditions. Therefore, if the motion equation of the vehicle S is established by using parameters that change depending on the parameters of the tires, such as the turning power coefficient, which has uncertainty, it is necessary to change these parameters by considering the use state and the road surface conditions, which makes it difficult to estimate the vehicle state.

[0120] In the present embodiment, as shown in formula (8) and formula (13), neither the velocity observer equation nor the attitude angle observer equation includes mass and friction coefficient. Therefore, the vehicle velocity and attitude angle can be calculated without depending on parameters with uncertainty such as the mass and friction coefficient of the vehicle S. This makes it possible to estimate the vehicle state without depending on the use state of the vehicle S or the road surface condition.

[0121] By changing the acceleration gain K and attitude angle gain K ANG , it is possible to determine the estimated vehicle speed (estimated attitude angle) as a value close to the steady-state vehicle speed (steady-state attitude angle) when the wheel is not slipping, and to determine it as a value close to the instantaneous vehicle speed (instantaneous attitude angle) when the wheel is slipping. In addition, since the estimated vehicle speed (estimated attitude angle) is obtained by integrating and updating the observer equation, the instantaneous characteristics and the steady-state characteristics can be fused, and accordingly, the estimated vehicle speed (estimated attitude angle) can be determined as the most likely value within the entire steady-state / unsteady-state motion range.

[0122] In the above embodiment, based on the activation of the anti-lock braking system 43 or the traction control system 45, it is determined whether the wheel is slipping, and the acceleration gain K and the attitude angle gain K are set accordingly. ANGIn this way, activation of the anti-lock brake system 43 or the traction control system 45 is used to determine whether the wheel is in a slipping state, from which it can be easily determined.

[0123] Next, the operation and effect of the abnormality determination device 68 will be described.

[0124] When the wheels slip, the vehicle speed obtained based on the wheel speed is different from the true value, so the acceleration of the vehicle in the front-rear direction obtained by the main inertial measurement sensor 5 (6-axis inertial measurement sensor) and the acceleration of the vehicle in the front-rear direction obtained based on the wheel speed may deviate from each other. Therefore, it is difficult to determine the abnormality of the main inertial measurement sensor 5 by comparing the acceleration in the front-rear direction obtained by the main inertial measurement sensor 5 and the acceleration in the front-rear direction obtained by the wheel speed sensor 9.

[0125] In step ST1, the abnormality determiner 33 compares the acceleration of the vehicle in the front-rear direction acquired by the main inertial measurement sensor 5 (6-axis inertial measurement sensor) with the acceleration of the vehicle in the front-rear direction acquired by the auxiliary inertial measurement sensor 7 (3-axis inertial measurement sensor), and when they do not match, determines that there is an abnormality in the main inertial measurement sensor 5 (6-axis inertial measurement sensor). Therefore, compared with the case of using the acceleration in the front-rear direction acquired based on the wheel speed sensor 9, even in the case of wheel slip, it is possible to correctly determine the abnormality of the main inertial measurement sensor 5 when detecting the acceleration in the front-rear direction. Therefore, it is possible to correctly determine whether the movement amount of the vehicle S in the front-rear direction can be accurately acquired, that is, whether the own position can be correctly evaluated.

[0126] In step ST2, the abnormality determiner 33 compares the acceleration in the lateral direction acquired by the main inertial measurement sensor 5 with the acceleration of the vehicle in the lateral direction acquired by the auxiliary inertial measurement sensor 7, and when they do not match, determines that there is an abnormality in the main inertial measurement sensor 5. In addition, in step ST3, the abnormality determiner 33 determines the z component ω of the angular velocity ω (i.e., the yaw rate) acquired by the main inertial measurement sensor 5. z Is it consistent with the angular velocity ω' around the z-axis obtained by the auxiliary inertial measurement sensor 7? z (ie yaw rate) matching.

[0127] In this way, the abnormality determiner 33 compares the acceleration in the front-rear direction, the acceleration in the lateral direction, and the yaw rate acquired by the main inertial measurement sensor 5 with the corresponding values ​​acquired by the auxiliary inertial measurement sensor 7, thereby determining whether there is an abnormality. Therefore, a mutual monitoring mechanism of the two sensors can be configured, and the use of a 3-axis inertial measurement sensor whose cost is lower than that of a 6-axis inertial measurement sensor can reduce the overall cost compared to the use of two 6-axis inertial measurement sensors.

[0128] In addition, since the abnormality determiner 33 compares the acceleration in the front-to-back direction, the acceleration in the lateral direction and the yaw rate obtained by the main inertial measurement sensor 5 with the corresponding values ​​obtained by the auxiliary inertial measurement sensor 7, it is able to more reliably detect abnormalities in the acceleration in the front-to-back direction, the acceleration in the lateral direction and the yaw rate. When an abnormality occurs, such an abnormality may cause a large error in the estimated value of the own position.

[0129] In step ST5, the abnormality determiner 33 calculates the estimated value of the attitude angle (i.e., the estimated attitude angle θ0) obtained by numerically integrating the attitude angle equation (Formula (13)) and the steady-state attitude angle θ grav The difference between , and the x component and y component of the difference are regarded as the estimated error. Because the estimated error becomes too large when an abnormality occurs in the 6-axis inertial measurement sensor, the abnormality determiner 33 can determine whether there is an abnormality in the 6-axis inertial measurement sensor by using the estimated error.

[0130] More specifically, in step ST5, the abnormality determiner 33 calculates the x component of the estimated attitude angle θ0 and the steady-state attitude angle θ grav The difference between the x component of the estimated attitude angle Θ0 and the y component of the steady-state attitude angle Θ grav The abnormality determiner 33 calculates the difference between the y components of the main inertial measurement sensor 5 and determines whether each calculated difference is less than or equal to the corresponding threshold value. Therefore, the abnormality determiner 33 can detect abnormalities in the roll direction and the pitch direction in the main inertial measurement sensor 5 (6-axis inertial measurement sensor), which are not evaluated in step ST2 and step ST4.

[0131] A specific embodiment of the present invention has been described above, but the present invention is not limited to the above embodiment and can be modified or altered in various ways.

[0132] In the above embodiment, the slip determination device 41 is provided with an anti-lock braking system 43 and a traction control system 45, but the present invention is not limited to this embodiment. For example, the slip determination device 41 may be provided with a vehicle stability assist (VSA) system (also referred to as a lateral slip control system), and determine that the wheel is in a slipping state when, for example, a control is performed to suppress lateral slip of the wheel when turning. In addition, the slip determination device 41 may be provided with a steering angle sensor, and determine that the wheel is in a slipping state when the time rate of change of the steering angle detected by the steering angle sensor is greater than or equal to a prescribed threshold value. In addition, the slipping state may include: a state in which the wheel may be in a slipping state, a state in which the wheel may be in a slipping state depending on the road surface conditions, and / or a state immediately before entering a slipping state.

[0133] In the above embodiment, the velocity of the position of the main inertial measurement sensor 5 (6-axis inertial measurement sensor) is calculated, but the present invention is not limited to this embodiment. For example, it is possible to configure so that the velocity of the prescribed position of the vehicle S is estimated by converting the estimated speed obtained by the main inertial measurement sensor 5 using the following formula (14).

[0134] v=v0+ω×L (14)

[0135] Note that in formula (14), v represents the rate after conversion, v0 represents the rate before conversion, ω represents the angular velocity detected by the main inertial measurement sensor 5, and L represents a vector whose origin is located at the position where the main inertial measurement sensor 5 is set and whose end point is located at the position of the vehicle whose rate is to be estimated.

[0136] In the above embodiment, the abnormality determiner 33 calculates the estimated attitude angle value (i.e., the estimated attitude angle θ0) obtained based on the attitude angle equation and the steady-state attitude angle θ grav The estimated error in ST5 is calculated by the difference between the values ​​of ...

Claims

1. A vehicle state estimation device for estimating a vehicle state based on a measurement value obtained by a six-axis inertial measurement sensor installed in a vehicle to detect fore-and-aft acceleration, lateral acceleration, vertical acceleration, roll rate, pitch rate, and yaw rate of the vehicle, the vehicle state estimation device comprising: a 3-axis inertial measurement sensor, the 3-axis inertial measurement sensor detecting the fore-aft acceleration, the lateral acceleration, and the yaw rate; an abnormality determination unit, the abnormality determination unit determining whether the 6-axis inertial measurement sensor has an abnormality; as well as a wheel speed sensor for measuring a wheel speed of a wheel disposed on the vehicle; and a vehicle state estimation unit for calculating the attitude angle of the vehicle based on the measurement values ​​measured by the wheel speed sensor and the 6-axis inertial measurement sensor, wherein the abnormality determination unit determines whether the 6-axis inertial measurement sensor has an abnormality by respectively comparing the fore-and-aft acceleration, the lateral acceleration, and the yaw rate acquired by the 6-axis inertial measurement sensor with the fore-and-aft acceleration, the lateral acceleration, and the yaw rate acquired by the 3-axis inertial measurement sensor, and In which, when the difference between the attitude angle calculated by the vehicle state estimation unit and the steady-state attitude angle based on the gravity direction calculated using the wheel speed when the vehicle is traveling without slipping the wheels becomes greater than a specified threshold, the abnormality determination unit further determines that there is an abnormality in the 6-axis inertial measurement sensor.

2. The vehicle state estimation device according to claim 1, wherein: When the difference between the roll angle calculated by the vehicle state estimation unit and the roll angle of the steady-state attitude angle is greater than a specified threshold, or when the difference between the pitch angle calculated by the vehicle state estimation unit and the pitch angle of the steady-state attitude angle is greater than a specified threshold, the abnormality determination unit determines that there is an abnormality in the 6-axis inertial measurement sensor.

Images