JP7234617B2 - Body attitude angle estimation device - Google Patents

Description

TECHNICAL FIELD The present invention relates to a vehicle body posture angle estimation device, and more particularly to a vehicle body posture angle estimation device for estimating a vehicle body posture angle based on a relationship between a running road and a vehicle body motion including sideslip.

A vehicle body during running makes motions in three axial directions, i.e., longitudinal motion, lateral motion, and yaw motion, with respect to the road surface. changes dynamically. 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 body can be grasped, the steering angle, accelerator opening degree, and brake strength related to the control of the vehicle body 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.

Patent Literature 1 discloses an invention of a road surface gradient estimating device that estimates a longitudinal gradient of a vehicle body using a wheel speed sensor and an acceleration sensor.

Patent Document 2 discloses an invention of a road shape prediction device for estimating the attitude angle of a vehicle body from the yaw rate, lateral acceleration, longitudinal acceleration, and steering angle of the vehicle body.

Patent Literature 3 discloses an invention of a lane keeping assist device that estimates a camber angle of a road surface from values of a yaw rate and a lateral acceleration of a vehicle body, and estimates a lateral velocity of the vehicle body from an equation of motion of the vehicle body.

JP 2017-144973 A JP 2014-151853 A JP 2017-13520 A

However, the invention described in Patent Document 1 does not take into consideration the side slipping of the vehicle body when it turns, so there is a risk that the accuracy of the pitch angle estimation will deteriorate when the vehicle body travels through an intersection or the like where the side slipping becomes prominent. there were.

In the invention described in Patent Document 2, the motion model used does not have flexibility with respect to the environment, and there is a possibility that the accuracy of estimating the attitude angle of the vehicle body may deteriorate when the road surface environment changes.

The invention described in Patent Document 3 uses a dynamic model of the steering system and a dynamic model of the tire. In particular, the dynamic model of the tire is significantly affected by the environment, and may adversely affect the estimation result. In addition, the above model has a problem in that it largely depends on the specifications of the vehicle body, and there is a difficulty in versatility between the vehicle bodies.

SUMMARY OF THE INVENTION The present invention has been made in view of the above problems, and provides a vehicle body attitude angle estimating apparatus that accurately estimates the attitude angle of a vehicle body from an image acquired by an imaging device, vehicle speed, vehicle yaw rate, and vehicle body acceleration. With the goal.

In order to achieve the above object, the vehicle body attitude angle estimation device according to claim 1 comprises an imaging device that acquires image information of the surroundings of the vehicle body; an image information processing unit that calculates a position deviation and a yaw angle deviation between the target path and the vehicle body; The lateral position deviation and the yaw angle are calculated using an equation representing the relationship between the movement and the motion of the vehicle body including sideslip, and an equation representing the relationship between the motion of the vehicle body in the longitudinal direction or the lateral direction including the acceleration of the vehicle body. a calculation unit for estimating the lateral velocity of the vehicle body and the attitude angle of the vehicle body from measured values including the deviation, the longitudinal velocity obtained from the vehicle speed sensor, the yaw rate obtained from the yaw rate sensor, and the acceleration obtained from the acceleration sensor. there is

In another aspect of the vehicle body posture angle estimation device, in the vehicle body posture angle estimation device according to claim 1, the expression representing the relationship between the movement of the target position and the motion of the vehicle body including side slip is The amount of change in the lateral position deviation obtained by subtracting the product of the yaw rate and the lateral position deviation from the speed and the product of the result obtained by correcting the result using the yaw angle deviation and the sine value of the yaw angle deviation; A vehicle body in the longitudinal direction or the lateral direction including an acceleration of the vehicle body, including an equation representing a relationship between the result of subtracting the lateral velocity of the imaging device from the position of the center of gravity of the vehicle accompanying the yaw motion of the vehicle body and the lateral velocity. is obtained from the acceleration sensor and the result of subtracting the product of the yaw rate and the lateral speed and the product of the acceleration of gravity and the sine value of the pitch angle from the amount of change in the longitudinal speed. The computation unit estimates the pitch angle as the posture angle.

Further, the vehicle body attitude angle estimation apparatus according to claim 2 is the vehicle body attitude angle estimation apparatus according to claim 1 , wherein the equation expressing the relationship of the vehicle body motion in the longitudinal direction or the lateral direction including the acceleration of the vehicle body is: Represents a relationship in which the result of subtracting the product of the yaw rate and the lateral velocity and the product of the gravitational acceleration and the sine value of the pitch angle is equal to the longitudinal acceleration obtained from the acceleration sensor from the amount of change in the longitudinal velocity. the result obtained by adding the product of the yaw rate and the longitudinal velocity, the product of the gravitational acceleration, the sine value of the roll angle, and the cosine value of the pitch angle to the amount of change in the lateral velocity; and an equation representing a relationship in which the obtained lateral acceleration is equal, and the calculation unit estimates the pitch angle and the roll angle as the attitude angles.

According to a third aspect of the present invention, there is provided the vehicle attitude angle estimating apparatus according to the second aspect, wherein the calculation unit calculates the yaw rate by multiplying the pitch angle change amount by the sine value of the roll angle. is divided by the product of the cosine value of the roll angle and the cosine value of the pitch angle, and the amount of change in the yaw angle is equal. The pitch angle, the roll angle and the yaw angle are estimated as follows.

A fourth aspect of the present invention is the vehicle body attitude angle estimation device according to any one of the first to third aspects, wherein the calculation unit includes movement of the target position and side slip. a prediction unit that predicts the state quantity of the vehicle body at the next time based on the state quantity of the vehicle body including the lateral velocity and the attitude angle estimated last time based on an expression representing the relationship with the motion of the vehicle body; Corresponding to the predicted state quantity of the vehicle body at the next time, which is calculated using a predetermined observation equation including an expression representing the relationship between the vehicle body motion in the longitudinal direction or the lateral direction including the acceleration of the vehicle body. Then, based on the difference between the lateral position deviation, the yaw angle deviation, the longitudinal velocity, the yaw rate and the acceleration, and the respective measured values, and the previously estimated state quantity of the vehicle body, the prediction unit an updating unit that estimates the lateral velocity and the attitude angle by correcting the predicted state quantity of the vehicle body.

Advantageous Effects of Invention According to the present invention, it is possible to realize a vehicle body attitude angle estimating apparatus that accurately estimates a vehicle body attitude angle from an image acquired by an imaging device, vehicle speed, vehicle body yaw rate, and vehicle body acceleration.

1 is a block diagram showing an example of a configuration of a vehicle body attitude angle estimation device according to a first embodiment of the present invention; FIG. It is a schematic diagram showing a coordinate system in the first embodiment of the present invention. FIG. 4 is an explanatory diagram showing an example of the relationship between the position of an imaging device, the center of gravity of a vehicle, lateral position deviation, and yaw angle deviation; FIG. 4 is an explanatory diagram showing the relationship between the motion of the vehicle, including sideslip, and the target route; FIG. 2 is an explanatory diagram showing an outline of inputs and outputs of the vehicle body posture angle estimation device according to the first embodiment of the present invention; FIG. 2 is an example of a functional block diagram of an arithmetic device of the vehicle body attitude angle estimation device according to the first embodiment of the present invention; It is an example of the functional block diagram of a prior estimation part. It is an example of a functional block diagram of a filtering unit. FIG. 4 is a schematic diagram showing an example of vehicle turning when the lateral position deviation and the yaw angle deviation are large, such as when turning right or left at an intersection; FIG. 7 is an explanatory diagram showing an outline of input/output of a vehicle body attitude angle estimation device according to a second embodiment of the present invention; FIG. 11 is an explanatory diagram showing an outline of inputs and outputs of a vehicle body attitude angle estimation device according to a third embodiment of the present invention;

[First embodiment]
BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the vehicle body posture angle estimating apparatus 10 according to the present embodiment includes a storage device 18 that stores data required for calculation by a calculation device 14 described later and results of calculation by the calculation device 14; an image information processing unit 20 that calculates the lateral position deviation between the target route and the vehicle, the yaw angle deviation of the vehicle, and the curvature of the target route from the image information acquired by the image information processing unit 20; An input device 12 to which the deviation, curvature, longitudinal speed detected by the vehicle speed sensor 24, the yaw rate detected by the yaw rate sensor 26, and the acceleration detected by the acceleration sensor are input, and the input data input from the input device 12 and the storage device 18. It consists of an arithmetic unit 14 composed of a computer or the like for calculating the lateral velocity of the vehicle body and the attitude angle of the vehicle body based on the stored data, and a CRT or LCD or the like for displaying the attitude angle or the like calculated by the arithmetic unit 14. and a display device 16.

Next, a coordinate system related to the behavior of the vehicle body 200 is defined as shown in FIG. The earth coordinate system 204 is a coordinate system in which the gravitational acceleration direction and z _e are parallel to the earth plane, and y _e faces the north direction. The road surface coordinate system 206 is a coordinate system in which z _r is oriented in a direction perpendicular to the road surface through the center of gravity of the vehicle body 200 and x _r is oriented in the vehicle traveling direction. A vehicle body coordinate system 208 is a coordinate system fixed on the vehicle body springs, with z _v pointing in the vertical direction of the vehicle body and x _v pointing in the direction of travel of the vehicle body.

Also, the roll angle φ, the pitch angle θ, and the yaw angle ψ, which are the Euler attitude angles, are defined with respect to the earth coordinate system 204 as shown in FIG. For example, the roll angle φ is the rotation angle about the x-axis, the pitch angle θ is the rotation angle about the y-axis, and the yaw angle ψ is the rotation angle about the z-axis. Also, each of the roll angle φ, the pitch angle θ, and the yaw angle ψ shows a positive value for rotation in the direction of the right-handed screw (in FIG. 2, each arrow direction). In the present embodiment, for the sake of convenience, the yaw angle deviation, which will be described later, uses the road surface coordinate system 206 as the reference coordinate system, and the Euler attitude angle originally defined with respect to the earth coordinate system 204 is set with respect to the vehicle body coordinate system 208. , roll angle φ _v , pitch angle θ _v and yaw angle ψ _v . In the following description, the roll angle φ, the pitch angle θ, and the yaw angle ψ are basically assumed to be attitude angles defined with respect to the vehicle body coordinate system 208 .

In this embodiment, image information obtained by the imaging device 22, vehicle speed detected by the vehicle speed sensor 24, vehicle yaw rate detected by the yaw rate sensor 26, and vehicle acceleration detected by the acceleration sensor 28 are used to measure the lateral velocity of the vehicle body 200. (hereinafter abbreviated as "vehicle lateral velocity") and attitude angle are calculated. In the present embodiment, when calculating the vehicle _body lateral speed, as shown in _{FIG .} Assume a lateral position deviation e ₁ and a yaw angle deviation e ₂ . Note that the sensor position P _s is located at a distance _ls (offset distance between the vehicle center of gravity _{P r and} the sensor position P _s ) in the forward direction of the vehicle body 200 from the vehicle center of gravity P r , and an imaging device such as an in-vehicle camera or the like is located. 22 are provided. As shown in FIG. 3, the amount of movement of the vehicle body 200 in the traveling direction based on the sensor position P _s is x _S , and the amount of lateral movement of the vehicle body 200 based on the sensor position P _s is y _s . be.

Next, FIG. 4 shows the relationship between the movement of the vehicle body 200 including sideslip and the target route 220, which is used when estimating the vehicle body lateral speed. In general, the vehicle body 200 is skidding on the road surface, and is accompanied by motion having a lateral velocity. With respect to the behavior of the vehicle body 200, the target position _Pt on the driving lane is moving with only longitudinal velocity and no lateral velocity. By using this feature and deriving the difference between the motion of the vehicle body 200 and the motion of the target position _Pt , it is possible to derive the conditional expression necessary for estimating the vehicle body lateral speed.

In this embodiment, variables are set as shown in FIGS. Assuming that the target position P _t on the target path 220 has only a tangential velocity component U _t , the velocity vector of the target position P _t is V _t =[U _t 0] ^T , and the target position P _t is P _t =[ x _t y _t ] ^T , and the yaw angle tangential to the x-axis direction of the earth coordinate system is defined as ψ _t . Furthermore, assuming that the vehicle body 200 has lateral velocity (has a vehicle body slip angle) as well as the longitudinal direction, the origin of the road surface coordinate system 206 is P _r =[x _r y _r ] ^T , the yaw angle is ψ _r , and each of the vehicle body 200 Let the axial velocity component be ν _r =[U _r V _r ] ^T , as shown in FIG. In this embodiment, in order to facilitate the relationship with sensor information measured with reference to the vehicle body coordinate system 208 later, the reference coordinate system of the deviation is the road surface coordinate system 206, and the yaw angle deviation is ψ _e =ψ _t − Define _ψr .

If the deviation vector of the apparent target position P _t from the road surface coordinate system 206 is P _e =[x _e y _e ], the differentiation of the deviation vector P _e is expressed by the following two equations.

Image information around the vehicle body 200 acquired by an imaging device 22 such as an in-vehicle camera is image-analyzed by a known method in the image information processing unit 20 to obtain the curvature ρ of the driving lane at the target position P _t and the sensor position P A lateral position deviation e ₁ and a yaw angle deviation e ₂ with respect to the target position P _t with _s as a reference are extracted. Since the imaging device 22 is a type of sensor, the curvature ρ, the lateral position deviation e ₁ and the yaw angle deviation e ₂ extracted from the image information acquired by the imaging device 22 are used as sensor information acquired by the sensor. Furthermore, the following equation 3 is obtained by modifying the above equation using the longitudinal velocity U _r that is the velocity in the longitudinal direction of the vehicle body 200 detected by the vehicle speed sensor 24 and the yaw rate R _r detected by the yaw rate sensor 26 as sensor information.

Furthermore, the following equations (1), (2) and (3) are obtained by arranging the above three equations for the vehicle body lateral velocity V _r and the yaw rate R _r .

The following equation (4) is obtained from the above equations (1) and (3). All the variables on the right side of Equation (4) can be obtained from the sensor. Therefore, according to equation (4), the vehicle body lateral velocity V _r can be calculated based on the data detected by the imaging device 22 , the vehicle speed sensor 24 and the yaw rate sensor 26 .

Equation (4) corrects the turning radius by subtracting R r _{e 1} obtained by multiplying the yaw rate R _r of the vehicle body 200 by the lateral position deviation e ₁ from the longitudinal velocity U _r , and further corrects the turning radius by subtracting it from the longitudinal velocity U r _. The moving speed U _t (see formula ( ₃ )) of the target position P _t on the target path 220 obtained by dividing by the cosine cos 2 of the yaw angle deviation e ₂ sine sine ₂ obtained by multiplying The amount of _change in the lateral position deviation _{e 1 from the} target _path (e ₁ ) and the lateral velocity R _r l _s of the imaging device 22 with respect to the vehicle center-of-gravity position P _r associated with the yaw motion of the vehicle body 200 is equal to the lateral velocity V _r of the vehicle body 200. .

Note that the above equation (4) is obtained by subtracting the product of the yaw rate R _r and the lateral position deviation e ₁ from the longitudinal velocity U _r and correcting it using the yaw angle deviation e ₂ , and the yaw angle deviation e ₂ and the sine value _{of the} vehicle body _{200 .} is an example of a formula representing a relationship equal to the lateral velocity V _r of .

Acceleration sensor 28 obtains longitudinal acceleration a _x in the traveling direction (x-axis direction) of vehicle body 200 , and further obtains lateral acceleration a _y that is lateral acceleration (y-axis direction) of vehicle body 200 . An acceleration sensor 28 for detecting longitudinal acceleration a _x and an acceleration sensor 28 for detecting lateral acceleration a _y are provided to detect each of longitudinal acceleration a x _and lateral acceleration a _y . Alternatively, an acceleration sensor 28 capable of detecting both longitudinal acceleration a _x and lateral acceleration a _y is used.

The relationship between the longitudinal acceleration a _x and the lateral acceleration a _y obtained from the acceleration sensor 28 is expressed by the following equations (5) and (6), which express the relationship between the longitudinal or lateral movement of the vehicle body including the acceleration of the vehicle body 200. become.

Basically, the longitudinal acceleration a _x is a derivative of the longitudinal velocity _Ur , but it is affected by the yaw rate R _r and the lateral velocity V _r of the vehicle body 200 in the lateral direction (y-axis direction). Gravitational acceleration g also affects longitudinal acceleration a _x . As a result, the longitudinal acceleration a _x is obtained from the differential value (change amount) of the longitudinal velocity _Ur , the product of the vehicle body lateral velocity V _r and the yaw rate R _r shown in the above equation (4), the gravitational acceleration g and the pitch angle The product of the sine value of θ equals the subtracted value.

Further, the lateral acceleration _ay is basically a derivative of the vehicle body lateral velocity _Vr , but is affected by the yaw rate _Rr and the longitudinal velocity _Ur . Gravitational acceleration g also affects the value of lateral acceleration _ay . As a result, the lateral acceleration _ay is the derivative (variation) of the vehicle body lateral velocity _Vr , the product of the yaw rate _Rr and the longitudinal velocity _Ur , the gravitational acceleration g, the sine value of the roll angle φ, and the pitch angle θ. The product with the cosine value corresponds to the added value.

As described above, the vehicle body lateral velocity V _r can be calculated using the equation (4), and the longitudinal acceleration a _x and lateral acceleration a _y can be detected by the acceleration sensor 28 . Therefore, since there are two unknowns in the equations (5) and (6), the roll angle φ and the pitch angle θ, the equations (5) and (6) are a kind of two-dimensional simultaneous equations, and the unknowns are the roll angle φ and the pitch Angle θ can be calculated.

Also, the differential value of the pitch angle θ of the Euler attitude angle and the differential value of the yaw angle ψ, which is the vehicle azimuth angle, are expressed by the following equations using the pitch rate Q and the yaw rate R.

Since the present embodiment does not assume the use of a sensor that detects the pitch rate, if the pitch rate Q is eliminated from the above two equations, the differential value (change amount) of the yaw angle ψ is expressed by the following equation. .

The above formula is used when calculating the yaw angle ψ in the third embodiment of the present invention, which will be described later. In the above formula, the product of the change amount of the pitch angle θ and the sine value of the roll angle φ is added to the yaw rate R, and the result is divided by the product of the cosine value of the roll angle φ and the cosine value of the pitch angle θ. The relationship between the result obtained by the yaw angle change and the amount of change in the yaw angle is equal.

FIG. 5 is an explanatory diagram showing an outline of input/output of the vehicle body attitude angle estimation device 10 according to the present embodiment. The lateral position deviation e ₁ and the yaw angle deviation e ₂ are input from the image information processing section 20 to the arithmetic device 14 constituting the vehicle body attitude angle estimation device 10 . Further, the longitudinal velocity U _r from the vehicle speed sensor 24 , the yaw rate R _r of the vehicle body 200 from the yaw rate sensor 26 , and the longitudinal acceleration a _x of the vehicle body 200 from the acceleration sensor 28 are all input to the computing device 14 . Note that the longitudinal direction of the vehicle body 200 is a direction parallel to the x-axis of the vehicle body coordinate system 208 .

The arithmetic unit 14 calculates the vehicle body lateral velocity V r and the vehicle body pitch angle, which is the vehicle body attitude angle, from the lateral position deviation e ₁ , the yaw angle deviation e ₂ , the longitudinal velocity U _r , the yaw rate _{R r and} the longitudinal acceleration a _x by arithmetic processing to be described later. Estimate the angle θ _v .

In the present embodiment, the relationship between the target route 220 and the motion of the vehicle body 200 is used to define the equation of state f(x), which is used in the pre-estimation and filtering steps in the arithmetic device 14 . Details of the pre-estimation and filtering steps are described below.

First, using the information of the lateral position deviation e ₁ , yaw angle deviation e ₂ , longitudinal velocity U _r , yaw rate R _r and longitudinal acceleration a _x with respect to the target path 220 obtained from the imaging device 22, the vehicle body lateral velocity V _r and the pitch The state equation f(x) for estimating the angle θ _v is defined as follows.

The state equation f(x) in this case satisfies the following formula (7) with respect to the state quantity x(t) at time t.

A state equation f(x) that satisfies the relationship of the above equation (7) with respect to the above state quantity x(t) is formulated as follows.

Equations (8) and (9) are differential equations showing the motion relationship between target path 220 and vehicle body 200 .

Also, in this embodiment, the system noise for each state is defined as follows.

Next, the observable quantity by the sensor is defined as follows.

An observation equation h(x) that satisfies y=h(x) defines correspondence between a state quantity (right side) and an observation quantity (left side) as in the following equations. The equation at the bottom expresses the relationship of the longitudinal motion of the vehicle body including the longitudinal acceleration of the vehicle body.

However, the observation noise for each observable is defined as follows.

In the Kalman filter, each state equation f(x) including equations (8) and (9) is discretized and used. Therefore, the input-output relationship of the function f(x) is used in the form of x(k)=f(x(k-1)) for times t=k, k-1.

FIG. 6 is an example of a functional block diagram of the arithmetic device 14 of the vehicle body attitude angle estimation device 10 according to the present embodiment. As shown in FIG. 6, the arithmetic unit 14 uses the state equation f(x) to estimate the state based on the relationship between the driving lane, which is the target route 220, and the motion of the vehicle body 200 including skidding. A prior estimation unit 102 that estimates corresponding sensor information using an observation equation h(x) that includes an expression representing the relationship of the vehicle body motion in the longitudinal direction including the longitudinal acceleration of the vehicle body; and a filtering unit 104 that corrects the estimated value using sensor information.

Various Kalman filters, including linear and nonlinear methods, have been proposed. In this embodiment, based on the fact that the equation of state f(x) is nonlinear as described above, an example of utilizing a nonlinear Kalman filter will be shown. Among them, an unscented Kalman filter, which does not require linearization of the state equation f(x) and has a relatively small computational load, is adopted as an example. If the state equation f(x) is linear, a linear Kalman filter may be used, or a nonlinear Kalman filter other than the unscented Kalman filter may be used.

FIG. 7 is an example of a functional block diagram of the prior estimation unit 102. As shown in FIG. As shown in FIG. 7, the prior estimation unit 102 includes a first unscented transform unit 102A and a second unscented transform unit 102B.

The first unscented transform unit 102A performs a first unscented transfer to update the state quantity based on the equation of state f(x), and outputs the mean of x and the covariance matrix of x.

The second unscented transforming unit 102B uses the average of the state quantity x and the covariance matrix of the state quantity x output by the first unscented transforming unit 102A, and transforms it into the corresponding observable quantity according to the observation equation h(x). Perform a second unscented transformation to

The purpose of the unscented transformation is to accurately obtain the mean and covariance matrix of the observed quantity y below in the transformation by a certain nonlinear function y=f(x).

Therefore, this embodiment is characterized by approximating the probability density function using 2n+1 samples (sigma points) corresponding to the mean and standard deviation.

As shown in FIG. 7, first unscented transforming section 102A includes sigma point weighting factor section 102A1, function transforming section 102A2, and U transforming section 102A3. Second unscented transforming section 102B includes sigma point weighting factor section 102B1, function transforming section 102B2, and U transforming section 102B3.

Sigma point X _i : i=0, 1, 2, . is selected as

However, the scaling factor κ is chosen such that κ≧0. Also, the weight for the sigma point is defined as follows.

The conversion of each sigma point by the nonlinear function f(x) in the function conversion section 102A2 of the first unscented conversion section 102A is as follows. The following is the conversion by the state equation f(x) in the function conversion unit 102A2 of the first unscented conversion unit 102A, but the observation equation h(x) in the function conversion unit 102B2 of the second unscented conversion unit 102B is used. The transformation yields the observed value.

U transforming section 102A3 of first unscented transforming section 102A uses the value transformed by function f(x) and the aforementioned weighting factor to obtain the following average value of state quantity x and state quantity x: Calculate the covariance matrix. Q _n in the following formula is system noise.

U conversion section 102B3 of second unscented conversion section 102B performs the following calculations.

FIG. 8 is an example of a functional block diagram of the filtering unit 104. As shown in FIG. The filtering unit 104 compares the difference between the observed value corresponding to the predicted value of the state quantity calculated by the U conversion unit 102B3 and the actually observed value, and corrects the predicted value of the state quantity. I do.

The process of feeding back the predicted value of the state quantity with the actually observed value is called Kalman Gain, which is calculated by the following equation. Note that R _n in the following equation is observation noise.

Next, using this Kalman gain, processing for correcting the advance prediction value of the state quantity is performed as follows.

By repeating the above-described processes of the prior estimation unit 102 and the filtering unit 104 for each time step, the vehicle body lateral velocity V _r and the vehicle body pitch angle θ _v are estimated.

When estimating the vehicle body lateral velocity V _r and the vehicle body pitch angle θ _v , the x(k) and P _x (k) output by the filtering unit 104 are input to the prior estimation unit 102 as the previous values, and are processed in two stages as described above. are unscented transforms to compute the prior predictions of the state quantities and the corresponding observed values. The calculated predicted value of the state quantity is corrected by the filtering unit 104 using the Kalman gain and the latest observed value actually observed. x(k) and P _x (k) output by the filtering unit 104 are temporarily held in the storage device 18, and x(k) and P _x (k) held in the storage device 18 are pre-estimated as previous values. input to section 102 .

By repeating the calculation of the advance predicted value of the state quantity and the correction with the latest actually observed value, the vehicle body lateral velocity V _r and the vehicle body pitch angle θ _v , which is the vehicle attitude angle, are estimated.

Conventionally, there are many cases in which the vehicle body lateral velocity V _r is not taken into account in estimating the vehicle body attitude angle. As shown in FIG. 9, when turning left or right at an intersection or changing lanes, the lateral position deviation _e1 and the yaw angle deviation _e2 increase, and the vehicle body lateral speed _Vr and yaw rate _Rr increase. , a large error may occur in the estimation of the vehicle attitude angle. Current autonomous driving technology is based on the premise of routes with relatively smooth roads with a small curvature ρ, such as expressways. High-precision estimation of the vehicle attitude angle is required even when the vehicle makes a large turn.

Vehicle attitude angle estimating apparatus 10 according to the present embodiment does not rely on existing equations of state and does not approximate the motion of vehicle body 200. Therefore, even if vehicle body 200 greatly deviates from target path 220, it can accurately Vehicle lateral velocity V _r and vehicle pitch angle θ _v can be estimated.

As described above, according to the vehicle body posture angle estimation device 10 according to the present embodiment, the vehicle body lateral velocity _Vr and the vehicle body pitch of the vehicle body 200 are obtained from the image acquired by the imaging device, the vehicle speed, the yaw rate of the vehicle body, and the acceleration of the vehicle body. The angle θ _v can be calculated with high accuracy.

[Second embodiment]
Next, a second embodiment of the invention will be described. As will be described later, the present embodiment is slightly different from the first embodiment in the input/output of the arithmetic device 14, but the configuration of the vehicle body posture angle estimation device is the same as that of the vehicle body posture angle according to the first embodiment. Since it is the same as the estimating device 10, detailed description of the configuration is omitted.

FIG. 10 is an explanatory diagram showing an outline of input/output of the vehicle body attitude angle estimation device 10 according to the present embodiment. The lateral position deviation e ₁ and the yaw angle deviation e ₂ are input from the image information processing section 20 to the arithmetic device 14 constituting the vehicle body attitude angle estimation device 10 . Further, the longitudinal velocity U _r of the vehicle body 200 from the vehicle speed sensor 24 , the yaw rate R _r of the vehicle body 200 from the yaw rate sensor 26 , and the longitudinal acceleration a _x and the lateral acceleration a _y from the acceleration sensor 28 are input to the arithmetic unit 14 . be done.

Arithmetic unit 14 calculates vehicle body lateral velocity V _r and vehicle body pitch from lateral position deviation e ₁ , yaw angle deviation e ₂ , longitudinal velocity U _r , yaw rate R _r , longitudinal acceleration a _x and lateral acceleration a _y by arithmetic processing described later. Estimate the angle θ _v and the roll angle φ _v .

In the present embodiment, as in the first embodiment, the relationship between the target route 220 and the motion of the vehicle body 200 is used to define the state equation f(x), and the prior estimation and filtering in the arithmetic unit 14 are performed. Use with steps.

The state equation f(x) of this embodiment holds the relationship of the following formula (10) with respect to the state quantity x(t) at time t.

A state equation f(x) that satisfies the relationship of the above equation (10) for the above state quantity x(t) is formulated as follows.

Equations (11) and (12) are differential equations showing the motion relationship between target path 220 and vehicle body 200 .

Also, in this embodiment, the system noise for each state is defined as follows.

Next, the observable quantity by the sensor is defined as follows.

An observation equation h(x) that satisfies y=h(x) defines correspondence between a state quantity (right side) and an observation quantity (left side) as in the following equations. The second equation from the bottom expresses the relationship of the longitudinal motion of the vehicle including the longitudinal acceleration of the vehicle, and the bottom equation expresses the relationship of the lateral motion of the vehicle including the lateral acceleration of the vehicle. is a formula that represents

However, the observation noise for each observable is defined as follows.

In the Kalman filter, each state equation f(x) including equations (8) and (9) is discretized and used. Therefore, the input-output relationship of the function f(x) is used in the form of x(k)=f(x(k-1)) for times t=k, k-1. After that, as in the first embodiment, the processes of the preliminary estimation unit 102 and the filtering unit 104 are repeated at each time step to estimate the vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v and the roll angle φ _v . do.

Also when estimating the vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v and the roll angle φ _v of the vehicle body 200, the x(k) and P _x (k) output by the filtering unit 104 are used as the previous values, , and performs a two-step unscented transformation as described above to compute the prior predicted values of the state variables and the corresponding observed values. The calculated predicted value of the state quantity is corrected by the filtering unit 104 using the Kalman gain and the latest observed value actually observed. x(k) and P _x (k) output by the filtering unit 104 are temporarily held in the storage device 18, and x(k) and P _x (k) held in the storage device 18 are pre-estimated as previous values. input to section 102 .

By repeating the calculation of the advance predicted values of the state quantities and the correction with the latest actually observed values, the vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v and the roll angle φ _v of the vehicle body 200 can be accurately estimated. .

As in the first embodiment, this embodiment does not rely on the existing equation of state and does not approximate the motion of the vehicle body 200. Therefore, even if the vehicle body 200 greatly deviates from the target path 220, The vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v and the roll angle φ _v can be accurately estimated.

As described above, according to the vehicle body attitude angle estimation device 10 according to the present embodiment, the vehicle body lateral velocity V _r of the vehicle body 200 and the vehicle body pitch are obtained from the image acquired by the imaging device, the vehicle speed, the vehicle yaw rate, and the acceleration of the vehicle body. The angle θ _v and roll angle φ _v can be calculated with high accuracy.

[Third Embodiment]
Next, a description will be given of a third embodiment of the present invention. As will be described later, the present embodiment is slightly different from the first embodiment in the input/output of the arithmetic device 14, but the configuration of the vehicle body posture angle estimation device is the same as that of the vehicle body posture angle according to the first embodiment. Since it is the same as the estimating device 10, detailed description of the configuration is omitted.

FIG. 11 is an explanatory diagram showing an outline of inputs and outputs of the vehicle body attitude angle estimation device 10 according to the present embodiment. The lateral position deviation e ₁ and the yaw angle deviation e ₂ are input from the image information processing section 20 to the arithmetic device 14 constituting the vehicle body attitude angle estimation device 10 . Further, the longitudinal velocity U _r of the vehicle body 200 from the vehicle speed sensor 24 , the yaw rate R _r of the vehicle body 200 from the yaw rate sensor 26 , and the longitudinal acceleration a _x and the lateral acceleration a _y from the acceleration sensor 28 are input to the arithmetic unit 14 . be done.

Arithmetic unit 14 calculates vehicle body lateral velocity V _r and vehicle body pitch from lateral position deviation e ₁ , yaw angle deviation e ₂ , longitudinal velocity U _r , yaw rate R _r , longitudinal acceleration a _x and lateral acceleration a _y by arithmetic processing described later. Estimate the angle θ _v , the roll angle φ _v , and the yaw angle ψ _v , which is the vehicle azimuth.

In the present embodiment, as in the first embodiment, the relationship between the target route 220 and the motion of the vehicle body 200 is used to define the state equation f(x), and the prior estimation and filtering in the arithmetic unit 14 are performed. Use with steps.

The state equation f(x) of this embodiment holds the relationship of the following formula (13) with respect to the state quantity x(t) at time t.

A state equation f(x) that satisfies the relationship of the above equation (13) for the above state quantity x(t) is formulated as follows.

Equations (14) and (15) are differential equations showing the motion relationship between target path 220 and vehicle body 200 .

Also, in this embodiment, the system noise for each state is defined as follows.

Next, the observable quantity by the sensor is defined as follows.

An observation equation h(x) that satisfies y=h(x) defines correspondence between a state quantity (right side) and an observation quantity (left side) as in the following equations.

However, the observation noise for each observable is defined as follows.

In the Kalman filter, each state equation f(x) including equations (8) and (9) is discretized and used. Therefore, the input-output relationship of the function f(x) is used in the form of x(k)=f(x(k-1)) for times t=k, k-1. Thereafter, as in the first embodiment, by repeating the processing of the prior estimation unit 102 and the filtering unit 104 at each time step, the vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v , the roll angle φ _v and the yaw angle Estimate the angle ψ _v .

When estimating the vehicle body lateral velocity V _r , the vehicle body pitch angle θ _v , the roll angle φ _v and the yaw angle ψ _v of the vehicle body 200, x(k) and P _x (k) output by the filtering unit 104 are used as previous values. It is input to the prior estimator 102 to perform the two-stage unscented transformation as described above to calculate the prior predicted value of the state quantity and the corresponding observed value. The calculated predicted value of the state quantity is corrected by the filtering unit 104 using the Kalman gain and the latest observed value actually observed. x(k) and P _x (k) output by the filtering unit 104 are temporarily held in the storage device 18, and x(k) and P _x (k) held in the storage device 18 are pre-estimated as previous values. input to section 102 .

By repeating the calculation of the advance prediction values of the state quantities and the correction with the latest actually observed values, the vehicle body lateral velocity V _r of the vehicle body 200, the vehicle body pitch angle θ _v , the roll angle φ _v and the yaw angle ψ _v can be estimated with high accuracy.

As in the first embodiment, this embodiment does not rely on the existing equation of state and does not approximate the motion of the vehicle body 200. Therefore, even if the vehicle body 200 greatly deviates from the target path 220, It is possible to accurately estimate the vehicle lateral velocity V _r , the vehicle pitch angle θ _v , the roll angle φ _v and the yaw angle ψ _v .

As described above, according to the vehicle body attitude angle estimation device 10 according to the present embodiment, the vehicle body lateral velocity V _r of the vehicle body 200 and the vehicle body pitch are obtained from the image acquired by the imaging device, the vehicle speed, the vehicle yaw rate, and the acceleration of the vehicle body. The angle θ _v , roll angle φ _v and yaw angle ψ _v can be accurately calculated.

10 Vehicle Posture Angle Estimation Device 12 Input Device 14 Calculation Device 16 Display Device 18 Storage Device 20 Image Information Processing Unit 22 Imaging Device 24 Vehicle Speed Sensor 26 Yaw Rate Sensor 28 Acceleration Sensor 102 Prior Estimation Unit 102A First Unscented Transformation Unit 102A1 Sigma Point Weight Coefficient section 102A2 Function conversion section 102A3 U conversion section 102B Second unscented conversion section 102B1 Sigma point weighting coefficient section 102B2 Function conversion section 102B3 U conversion section 104 Filtering section 200 Vehicle 204 Earth coordinate system 206 Road surface coordinate system 208 Vehicle body coordinate system 220 Target route

Claims (4)

an imaging device that acquires image information around the vehicle body;
an image information processing unit that calculates a lateral position deviation between the target route and the vehicle body and a yaw angle deviation between the target route and the vehicle body from the image information acquired by the imaging device;
An expression expressing the relationship between the movement of the target position that has only longitudinal velocity and no lateral velocity on the target path and the movement of the vehicle body including sideslip, and Using a formula that expresses the relationship between longitudinal or lateral vehicle motion including acceleration, the lateral position deviation, the yaw angle deviation, the longitudinal speed obtained from the vehicle speed sensor, the yaw rate obtained from the yaw rate sensor, and the acceleration sensor a calculation unit for estimating the lateral velocity of the vehicle body and the attitude angle of the vehicle body from measured values including the acceleration applied;
A body attitude angle estimation device including The formula representing the relationship between the vehicle body motion in the longitudinal direction or the lateral direction including the acceleration of the vehicle body is
Represents a relationship in which the result of subtracting the product of the yaw rate and the lateral velocity and the product of the gravitational acceleration and the sine value of the pitch angle is equal to the longitudinal acceleration obtained from the acceleration sensor from the amount of change in the longitudinal velocity. formula and
The result obtained by adding the product of the yaw rate and the longitudinal velocity, the product of the acceleration of gravity, the sine value of the roll angle, and the cosine value of the pitch angle to the amount of change in the lateral velocity, and the lateral velocity obtained from the acceleration sensor. a formula representing an equal relationship with acceleration ,
2. The vehicle body attitude angle estimation device according to claim 1 , wherein the calculation unit estimates the pitch angle and the roll angle as the attitude angles. The calculation unit divides the result obtained by adding the product of the change amount of the pitch angle and the sine value of the roll angle to the yaw rate by the product of the cosine value of the roll angle and the cosine value of the pitch angle. 3. The vehicle body posture angle estimating apparatus according to claim 2 , wherein the pitch angle, the roll angle, and the yaw angle are estimated as the posture angles by further using an equation representing a relationship in which the result and the amount of change in the yaw angle are equal. . The computing unit, based on the previously estimated state quantity of the vehicle body including the lateral velocity and the attitude angle, based on a formula representing the relationship between the movement of the target position and the motion of the vehicle body including sideslip, a prediction unit that predicts the state quantity of the vehicle body at the next time;
Corresponding to the predicted state quantity of the vehicle body at the next time, which is calculated using a predetermined observation equation including an expression representing the relationship between the vehicle body motion in the longitudinal direction or the lateral direction including the acceleration of the vehicle body. Then, based on the difference between the lateral position deviation, the yaw angle deviation, the longitudinal velocity, the yaw rate and the acceleration, and the respective measured values, and the previously estimated state quantity of the vehicle body, the prediction unit an updating unit that estimates the lateral velocity and the attitude angle by correcting the predicted state quantity of the vehicle body;
The vehicle body posture angle estimating device according to any one of claims 1 to 3 , comprising:

Images