JP3525698B2 - State estimation device and vehicle traveling control device - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a state estimating device for estimating at least road curvature and a vehicle traveling control device using the state estimating device.

[0002]

2. Description of the Related Art As a conventional state estimating device for estimating a road curvature, there is, for example, a device shown in FIG. 16 disclosed in Japanese Patent Laid-Open No. 4-283900. This estimation device includes an image pickup unit 101, a luminance change detection unit 103, a world coordinate system detection unit 105, a curve detection unit 107, and a display unit 109.

The image pickup means 101 is composed of a CCD camera or the like and picks up a grayscale image of the road surface ahead of the vehicle. In the image from the image pickup unit 101, the brightness change detection unit 103 obtains a brightness change amount of a pixel from the individual pixels at the bottom of the image toward the upper side of the image, and among the pixels whose change amount is a predetermined value or more, It seeks the pixel closest to the bottom of the image.

The world coordinate system detecting means 105 obtains the position in the real world coordinate system from the position in the image coordinate system of the pixel obtained by the luminance change detecting means 103. The curve detecting means 107 obtains the curvature and radius of curvature of the road from the real-world coordinates of the brightness change pixels obtained by the world coordinate system detecting means 105. The display means 109 displays the result of the detected road shape to the driver.

Therefore, in this conventional state estimating device, the brightness change detecting means 103 finds a pixel whose brightness is changing from the road image data in front of the vehicle, determines that it is the edge of the road, and determines the pixel on the image plane. The curvature and radius of the road are obtained by converting the position at 1 to the coordinates in the real world with the image pickup means 101 as the origin.

[0006]

However, in the above estimation device, a grayscale image in front of the vehicle as shown in FIG. 17 is captured by the image pickup means 101, and the coordinate transformation of the curve is performed based on the luminance change amount of the grayscale image. Because
Many pixels have to be calculated, resulting in a significant increase in calculation time. Therefore, when attempting to automatically steer the vehicle using the road curvature estimated by the estimation device, there is a possibility that a calculation delay occurs in the traveling vehicle and control becomes unstable. . Therefore, in the conventional estimation device, it is necessary to compensate for the calculation delay by capturing a grayscale image of a road farther away.

However, when a grayscale image of a road is photographed up to a distant place as shown in FIG.
Is displayed with a small number of pixels on the image, and the road width P in the foreground is displayed with a larger number of pixels. Therefore, in the conventional state estimation device, the distant resolution is lowered and the curvature of the road is accurately estimated. There is a problem that you cannot do it.

Further, in the conventional state estimating device, a line such as a white line of a road along a lane is obtained by polynomial approximation,
Since the curvature of the road is calculated based on the result, FIG.
As shown in (3), the number of data points of the image becomes large, and it takes time to perform the arithmetic processing, and in this case as well, there is a possibility that the control becomes unstable due to the arithmetic delay.

In view of the above, the present invention provides a state estimating device that can regulate image processing and perform a quick calculation and that enables stable vehicle traveling control, as well as vehicle traveling using the state estimating device. An object is to provide a control device.

[0010]

As means for solving the above-mentioned problems, in the invention of claim 1, relative lateral displacement measuring means for measuring the relative lateral displacement of the vehicle with respect to the road at a predetermined vehicle front gazing point is provided. A steering angle detecting means for detecting a steering angle of at least one of a front wheel and a rear wheel of the vehicle, and a calculation for estimating at least a road curvature and the relative lateral displacement for feedback by a calculation based on the relative lateral displacement and the steering angle. and means, said computing means, the same songs and the road curvature
The vehicle in the front-rear direction on the target travel line
With the core wires touching at the center of gravity of the vehicle,
Distance between the viewpoint and the target travel line in the vehicle width direction
The separation is used to estimate the relative lateral displacement for the feedback.
A configuration is adopted in which calculation is performed as the effect of road curvature .

Therefore, in the first aspect of the invention, the relative lateral displacement measuring means measures the relative lateral displacement of the vehicle with respect to the road at a predetermined vehicle front gazing point, and the steering angle detecting means is the front wheel or rear wheel of the vehicle. The steering angle of at least one of the wheels is detected. Then, the calculation means calculates at least the road lateral curvature and feedback by calculation based on the relative lateral displacement measured by the relative lateral displacement measurement means and the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detection means. And the relative lateral displacement of the above is estimated, and the influence of the road curvature is considered in the estimation of the relative lateral displacement for feedback. Therefore, the calculation means is
Relative lateral displacement for feedback considering the influence of
Can be estimated.

[0012]

[0013]

The invention of claim 2, there is provided a state estimating apparatus of claim 1 Symbol placement, the calculating means, in addition to the relative lateral displacement for the road curvature and feedback, the direction along the road ahead of the vehicle Relative declination of the vehicle with respect to, the relative center of gravity position lateral displacement of the vehicle with respect to the road is estimated, and the relative lateral displacement for feedback as a linear combination of the estimated road curvature, relative declination and relative center of gravity position lateral displacement. It is characterized by representing.

Therefore, according to the second aspect of the present invention, the calculating means can estimate the relative lateral displacement for feedback by calculating only the linear equation, and therefore, the relative lateral displacement for feedback can be estimated. Non-linear expressions can be eliminated from the calculation.

The invention according to claim 3 is the state estimating apparatus according to any one of claims 1 and 2 , further comprising a vehicle speed detecting means for detecting a vehicle speed of the vehicle, wherein the computing means comprises the relative lateral displacement. The calculation is performed based on the relative lateral displacement measured by the measuring means, the steering angle detected by the steering angle detecting means, and the vehicle speed detected by the vehicle speed detecting means.

Therefore, in the third aspect of the present invention, the calculating means determines the relative lateral displacement measured by the relative lateral displacement measuring means and the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means. At least the road curvature and the relative lateral displacement for feedback are estimated by calculation based on the vehicle speed detected by the vehicle speed detecting means.

According to a fourth aspect of the present invention, a relative lateral displacement measuring means for measuring a relative lateral displacement of the vehicle with respect to a road at a predetermined vehicle gazing point, and a steering angle of at least one of front wheels and rear wheels of the vehicle are detected. A steering angle detecting means, a computing means for estimating at least the road curvature and the relative lateral displacement for feedback by a calculation based on the relative lateral displacement and the steering angle, and an automatic steering mechanism for automatically steering the vehicle. And a controller for controlling the automatic steering mechanism based on an output from the calculating means, wherein the calculating means has the same curvature as the road curvature.
The vehicle center line heading in the front-rear direction of the vehicle
When the vehicle is in contact with each other at the center of gravity,
The distance from the target travel line along the vehicle width direction
The road curvature is used to estimate the relative lateral displacement for feedback.
It is characterized in that it is calculated as the effect of .

Therefore, in the invention of claim 4 , the relative lateral displacement measuring means measures the relative lateral displacement of the vehicle with respect to the road at a predetermined vehicle front gazing point, and the steering angle detecting means is the front wheel or rear wheel of the vehicle. The steering angle of at least one of the wheels is detected. The calculating means calculates the relative lateral displacement measured by the relative lateral displacement measuring means and the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means, thereby calculating at least the road curvature and the feedback. Relative lateral displacement is estimated, and the influence of road curvature is considered in estimating relative lateral displacement for feedback. Then, the controller controls the automatic steering mechanism based on the output from the calculation means, and the automatic steering mechanism is
Automatically steer the vehicle. Therefore, the calculation means is
Relative feedback for accurately considering the effect of path curvature
Lateral displacement can be estimated.

[0020]

[0021]

According to a fifth aspect of the present invention, there is provided the vehicle travel control device according to the fourth aspect , wherein the calculating means is arranged in addition to the road curvature and the relative lateral displacement for feedback, in addition to the direction along the road in front of the vehicle. Relative declination of the vehicle with respect to, the relative center of gravity position lateral displacement of the vehicle with respect to the road is estimated, and the relative lateral displacement for feedback as a linear combination of the estimated road curvature, relative declination and relative center of gravity position lateral displacement. It is characterized by representing.

Therefore, in the invention of claim 5 , the calculating means can estimate the relative lateral displacement for feedback by calculating only the linear equation, and therefore, the relative lateral displacement for feedback can be estimated. Non-linear expressions can be eliminated from the calculation.

The invention of claim 6 is the vehicle motion control device according to any have <br/> deviation of claim 4 or 5, comprising a vehicle speed detecting means for detecting a vehicle speed of said vehicle, said computing means Is
The calculation is performed based on the relative lateral displacement measured by the relative lateral displacement measuring means, the steering angle detected by the steering angle detecting means, and the vehicle speed detected by the vehicle speed detecting means.

Therefore, in the sixth aspect of the present invention, the calculating means determines the relative lateral displacement measured by the relative lateral displacement measuring means and the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means. At least the road curvature and the relative lateral displacement for feedback are estimated by calculation based on the vehicle speed detected by the vehicle speed detecting means.

[0026]

According to the first aspect of the present invention, the calculating means calculates the relative lateral displacement measured by the relative lateral displacement measuring means and the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means. Since the road curvature is estimated by the calculation based on this, it is possible to regulate the image processing in the estimation of the road curvature and perform a quick calculation.

Moreover, in the invention of claim 1, since the calculation means estimates the relative lateral displacement for feedback in consideration of the influence of the road curvature, the accuracy of the estimated relative lateral displacement for feedback is improved and the relative lateral displacement for feedback is improved. The convergence time until the estimated value of the relative lateral displacement of converges to the true value is shortened,
The calculation time required for estimating the road curvature is shortened. For example, when used in a vehicle with automatic steering, it is possible to perform stable driving control by quick calculation. Therefore, extremely smooth and accurate driving control can be performed when driving on a curve. Is possible.

Furthermore, in the invention of claim 1, since the calculating means can estimate the relative lateral displacement for feedback to accurately considering the effect of road curvature, the relative transverse displacement of the feedback was estimated boss accuracy Can be further improved, the convergence time until the estimated value of the relative lateral displacement for feedback converges to the true value can be further shortened, and the calculation time required for estimating the road curvature can be further shortened.

In the invention of claim 2 , since the non-linear expression can be eliminated from the calculation for estimating the relative lateral displacement for feedback, as compared with the invention of claim 1 or 2,
The algorithm of the calculation means can be made simpler,
The calculation time required for estimating the road curvature can be further shortened.

According to the third aspect of the invention, the calculating means includes the relative lateral displacement measured by the relative lateral displacement measuring means, the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means, and the vehicle speed detection. At least the road curvature and the relative lateral displacement for feedback are estimated by the calculation based on the vehicle speed detected by the means, so that the road curvature can be accurately estimated over a wide range of vehicle speeds.

According to the fourth aspect of the present invention, the calculating means estimates the road curvature by the calculation based on the relative lateral displacement measured by the relative lateral displacement measuring means and the steering angle detected by the steering angle detecting means. It is possible to control the image processing in the estimation of and to perform a quick calculation, and moreover, since the calculation means estimates the relative lateral displacement for feedback in consideration of the influence of the road curvature, the accuracy of the estimated value is improved. The convergence time until the estimated value converges to the true value is shortened, the calculation time required to estimate the road curvature is shortened, and as a result, stable traveling control can be performed by quick calculation, and when traveling on a curve. It enables extremely smooth and accurate traveling control.

[0032] In the invention of claim 4, since the arithmetic means can estimate the relative lateral displacement for feedback to accurately considering the effect of road curvature, the relative transverse displacement of the feedback was estimated boss accuracy Can be further improved, the convergence time until the estimated value of the relative lateral displacement for feedback converges to the true value can be further shortened, and the calculation time required for estimating the road curvature can be further shortened. As a result, smoother and more accurate traveling control becomes possible when traveling on a curve.

In the invention of claim 5 , since the non-linear expression can be eliminated from the operation for estimating the relative lateral displacement for feedback, the algorithm of the operation means can be made simpler as compared with the invention of claim 4. It is possible to shorten the calculation time required for estimating the road curvature,
As a result, smoother and more accurate traveling control becomes possible when traveling on a curve.

According to the sixth aspect of the invention, the calculating means includes the relative lateral displacement measured by the relative lateral displacement measuring means, the steering angle of at least one of the front wheels and the rear wheels of the vehicle detected by the steering angle detecting means, and the vehicle speed detection. By calculating at least the road curvature and the relative lateral displacement for feedback by the calculation based on the vehicle speed detected by the means, it is possible to accurately estimate the road curvature over a wide range of vehicle speeds, and as a result, when traveling on a curve. Thus, smooth and accurate traveling control is possible over a wide range of vehicle speeds.

[0035]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS FIG. 1 shows an example of an embodiment in which the inventions according to claims 1 to 6 are combined and implemented.
(A) is a schematic view seen from the side of the automobile, and (b) is
[Fig. 3] is a schematic view seen from a plane showing a relationship with a drive system.

The automobile shown in FIG. 1 is provided with a state estimating device 1 for estimating a road curvature and the like, a vehicle traveling control device 2 using the state estimating device 1, and a manual steering mechanism 3. The state estimating device 1 includes a lateral displacement measuring means 7 for measuring a relative lateral displacement of a vehicle with respect to a road at a predetermined vehicle front gazing point, and a front wheel steering as a front wheel steering angle measuring means for measuring a steering angle of a front wheel 21 of the vehicle. The angle sensor 9, the rear wheel steering angle sensor 11 as the rear wheel steering angle measuring means for measuring the steering angle of the rear wheel 23 of the vehicle, the vehicle speed sensor 13 as the vehicle speed measuring means for measuring the vehicle speed, the road curvature, etc. An arithmetic unit 15 as an arithmetic means for performing arithmetic operations is provided.

The lateral displacement measuring means 7 is composed of a CCD camera 17 for capturing an image of the front of the vehicle and an image processing device 19 for processing the image of the CCD camera 17. In the image processing device 19, the CCD camera 17 captures the image. The relative lateral displacement of the vehicle with respect to the road at the forward gazing point is calculated from the captured image, and the calculated value is output to the arithmetic unit 15 as a measurement value obtained by measuring the relative lateral displacement of the vehicle.

The relative lateral displacement of the vehicle with respect to the road at the front gazing point means, as shown in FIG. 4, a lane line at the front gazing point Z ahead of a predetermined distance Ks on the vehicle center line in the vehicle front-rear direction. Is the displacement ysr of the vehicle in the vehicle width direction from the white line or the like.

As shown in FIG. 1, the arithmetic unit 15 includes a vehicle relative lateral displacement measured by the lateral displacement measuring means 7, a steering angle of the front wheels 21 measured by the front wheel steering angle sensor 9, and a rear wheel steering angle sensor. The steering angle of the rear wheel 23 measured by 11 and the vehicle speed measured by the vehicle speed sensor 13 are input to calculate the road curvature and the like by the state estimation of the modern control theory, and the calculation result is calculated by the vehicle traveling control device 2. The data is output to the controller 40 and display means (not shown). The display means displays the road curvature calculated by the calculation device 15 to the driver.

The vehicle traveling control device 2 controls the automatic steering mechanism 5 on the basis of traveling environment information such as the output from the computing device 15, the vehicle speed, and the distance to the preceding vehicle.
And an automatic steering mechanism 5 for automatically steering the vehicle. The automatic steering mechanism 5 includes an electric motor 41, and an output shaft 43 of the electric motor 41 has a rear wheel side pinion gear 4 attached thereto.
5, the rear wheel side pinion gear 45 is meshed with the rear wheel side rack 47.

The rear wheel side rack 47 is supported by a rack housing (not shown) on the vehicle body side so as to be translationally movable in the left-right direction, and rear wheel side rods 49 are connected to both ends thereof. A rear wheel side axle 53 is connected to the rear wheel side rod 49 via a rear wheel side knuckle arm 51, and the rear wheel side axle 53 supports the rear wheel 23. In addition,
The rear wheel steering angle sensor 11 is adapted to detect the lateral translational movement amount of the rear wheel side rack 47.

When the electric motor 41 of the automatic steering mechanism 5 is driven, the rear wheel side pinion gear 45 attached to the output shaft 43 of the electric motor 41 rotates, and the rear wheel side rack 47.
Moves to the left or right, and this translation is transmitted to the rear wheel side rod 49 and the rear wheel side knuckle arm 51, and the left and right rear wheels 23 are steered via the rear wheel side axle 53.

Therefore, in the vehicle traveling control device 2, when the vehicle travels on a curve, the controller 40 controls the electric motor 41 of the automatic steering mechanism 5 based on the output from the arithmetic unit of the state estimating device 1, and the state The vehicle is automatically steered based on the road curvature and the like estimated by the estimation device 1. Both outputs from the front wheel steering angle sensor 9 and the rear wheel steering angle sensor 11 are fed back to the controller 40 of the vehicle travel control device 2 via the arithmetic unit 15.

The manual steering mechanism 3 has a front wheel side pinion gear 31 on the lower end side of a steering shaft 29 having a steering wheel 27, and the front wheel side pinion gear 3 is provided.
1 is meshed with the front wheel side rack 33. The front wheel side rack 33 is supported in a vehicle-body side rack housing so as to be translatable in the left-right direction, and front wheel side rods 35 are connected to both ends thereof. A front wheel side axle 39 is connected to the front wheel side rod 35 via a front wheel side knuckle arm 37, and the front wheel 21 is supported on the front wheel side axle 39. The front wheel steering angle sensor 9 is adapted to detect the lateral translational movement amount of the front wheel side rack 33.

In this manual steering mechanism 3, the front wheel side pinion gear 31 is rotated by steering the steering wheel 27 to the left or right, and the front wheel side rack 33 translates to either the left or right side. Rod 3
5, It is transmitted to the front wheel side knuckle arm 37, and the left and right front wheels 21 are steered in the steering direction via the front wheel side axle 39.

The vehicle shown in FIG. 1 is equipped with a switching means for switching between manual steering and automatic steering. When automatic steering is selected by this switching means, the controller 40 of the vehicle travel control device 2 is in the state. The driving environment information such as the output from the arithmetic unit 15 of the estimation device 1, the vehicle speed, the distance between the vehicle and the preceding vehicle is input to control the electric motor 41 of the automatic steering mechanism 5, and the automatic steering is performed by the control. At this time,
The manual steering mechanism 3 is fixed at the position where the steering angle is 0 degree, and the automatic steering is performed only by the steering by the automatic steering mechanism 5.

FIG. 2 shows a flow chart executed by the state estimating device 1. In step S1, initial setting processing of each unit is performed. In step S2, an image capturing process is performed, and the image captured by the CCD camera 17 is sent to the image processing device 19.

In step S3, the image processing apparatus 19 measures the relative lateral displacement ysr by image processing.
Relative lateral displacement ysr of the vehicle with respect to the white line at the front gazing point Z
Is calculated and sent to the arithmetic unit 15. In step S4, a vehicle speed measurement process is performed, and a vehicle speed signal is sent from the vehicle speed sensor 13 to the arithmetic unit 15. In step S <b> 5, both steering angles of the front wheels 21 and the rear wheels 23 are processed, and signals from the front wheel steering angle sensor 9 and the rear wheel steering angle sensor 11 are sent to the arithmetic unit 15.

In step 6, road curvature etc. is estimated. That is, the road curvature and the like are calculated in the calculation device 15 based on the relative lateral displacement ysr of the vehicle with respect to the white line at the forward gazing point Z, the vehicle speed, and the steering angles of both the front wheels and the rear wheels, which are input to the calculation device 15. As a result of the calculation of the road curvature and the like, the electric motor 41 of the automatic steering mechanism 5 is controlled by the controller 40 of the vehicle travel control device 2 so that the automatic steering can be smoothly performed even when traveling on a curve.

Here, the calculation of the road curvature will be described. First, the equation of motion is formulated using the vehicle as a general two-wheel model shown in FIG. The cornering power of the front wheel of the vehicle is Cf, the cornering power of the rear wheel is Cr, the vehicle mass is m, the yaw moment of inertia is I, the distance between the front wheel axle and the vehicle center of gravity G in the front-rear direction is a, and the rear wheel axle and the vehicle center of gravity G are Let b be the front-rear distance and V be the vehicle speed.

The steering angle of the front wheels is δf, and the steering angle of the rear wheels is δ.
r, the yaw angle, which is the deviation angle of the vehicle with respect to the direction in front of the vehicle, is ψ, and the yaw rate is ψ ^* (where the subscript “ ^* ” represents the primary time derivative d / dt, and the subscript “ ^** ” represents the secondary Time derivative d ²
/ Dt ² . The same applies hereinafter in this specification. ), The vehicle center-of-gravity position lateral displacement is yc, the vehicle center-of-gravity position lateral displacement velocity is yc ^* , the vehicle body slip angle is β, and the road curvature is ρ.

Further, the relative yaw angle which is the relative declination of the vehicle with respect to the road, the relative yaw rate, the relative vehicle center-of-gravity position lateral displacement of the vehicle with respect to the road, and the relative vehicle center-of-gravity position lateral displacement velocity of ψr, ψr ^* , ycr, ycr, respectively. It is expressed by adding the subscript "r" (meaning reference or relative) like ^* .

A general vehicle equation of motion is

[Equation 1] By the way, yc ^* = V (ψ + β) (3) That is, β = −ψ + yc ^* / V (3) ′. Therefore, when formula (3) ′ is substituted into formulas (1) and (2),

[Equation 2] ^{Here, ψr * = ψ * -ρV (} 5) ycr ** = from yc is ^{** -ρV 2} (6), equation of motion when a curved road vehicle is running is as follows.

[0054]

[Equation 3] However,

[Equation 4] Is.

As shown in FIG. 4, the lateral displacement of the vehicle relative to the road at the front gazing point Z is ysr (subscript "s" means sensor), from the vehicle center of gravity G to the front gazing point Z. If the distance is Ks,

[Equation 5] Therefore, the vehicle is modeled using the equation (7) as a state equation and the equation (9) as an output equation.

However, since the road curvature ρ is an uncontrollable input, the algorithm of the arithmetic unit 15 is constructed by converting the road curvature ρ into a state variable. First, in converting the road curvature ρ into a state variable, the behavior of ρ is assumed to be the Poisson square wave in FIG. The amplitude of ρ is ρ ₀ , and ν (ω) = 2λρ ₀ ² / (λ ² + ω ² ) (10) λ = It is represented by 2ν (11). The symbol ω is the angular frequency (the unit is rad /
s) is represented.

Here, the first-order system driven by white Gaussian noise having the same power spectral density function as in the equation (10) is expressed as follows.

Ρ ^* = − λρ + W (12) Here, the symbol W represents white Gaussian noise whose variance is 2λρ ₀ ² , and the subscript “ ^* ” is the first-order time derivative d /, as described above. represents dt.

As a result, the Poisson square wave shown in FIG. 5 and the system driven by the white Gaussian noise represented by the equation (12) have different probability density functions, but have the same variance and average value. become. Therefore, based on equation (12), ρ
If equation (7) is rewritten with a new state variable,

[Equation 6] From the equation (9), the output equation is

[Equation 7] Becomes

Therefore, the algorithm configuration of the arithmetic unit at this time is as shown in FIG. That is, the arithmetic unit 15 ′ includes the first coefficient unit 55, the second coefficient unit 57, the third coefficient unit 59, the fourth coefficient unit 61, the fifth coefficient unit 63, and the integrator 6.
5, first adder 67, second adder 69 and third adder 7
1 is provided. The first to fourth coefficient units 55, 5
Reference numerals A, B, C and D attached to 7, 59 and 61 are,
The matrices A, B, described in equations (13) and (14)
The sign S attached to the integrator 65 represents C and D, and the sign L attached to the fifth coefficient unit 63 represents a matrix called an output error feedback coefficient.

This matrix L is given by equation (12) when the arithmetic unit 15 'is constructed as a Kalman filter.
Let Γ be the variance of the white Gaussian noise W, and Σ be the variance of the relative lateral displacement measurement value ysr.

L = PC ^T Σ ⁻¹ (15) However, P satisfies AP + PA ^T + Γ−PC ^T Σ ⁻¹ CP = 0 (16), and A and C represent the matrices A and C, respectively. Where T represents a transposed value.

In this arithmetic unit 15 ', as shown in FIG. 6, the measured front wheel steering angle δf and rear wheel steering angle δr are matrix u.
And the measured relative lateral displacement ysr of the vehicle are input, and the yaw rate ψ ^* , relative yaw angle ψr, relative center-of-gravity position lateral displacement velocity ycr ^* , relative center-of-gravity position lateral displacement ycr, and road curvature ρ of the vehicle are input. It is estimated and output by calculation. Also, the relative lateral displacement y for feedback
sr 'is also estimated by calculation, and the estimated relative lateral displacement ysr' is fed back to the second adder 69.

The second adder 69 detects the relative lateral displacement ysr of the vehicle at the vehicle front gazing point Z measured by the relative lateral displacement measuring means 7 and the relative lateral displacement ys for feedback.
The deviation from r ′ is corrected and reflected in the matrix x ^* . Then, in the arithmetic unit 15 ′, the deviation amount between the measured relative lateral displacement ysr and the estimated relative lateral displacement ysr ′ for feedback is fed back through the fifth coefficient unit 63, and thus the fifth coefficient unit When the value of the matrix L of 63 is increased, the estimated value ysr 'converges to the true value earlier, but the noise of the measured value ysr' has a greater effect on the estimated value ysr '.

By the way, the arithmetic unit 15 'operates properly when the vehicle is traveling on a straight road as shown in FIG. However, since the algorithm of the arithmetic unit 15 'does not consider the influence of the road curvature ρ in the estimation of the relative lateral displacement ysr' for feedback, when the vehicle travels on a curved road as shown in FIG. Is different from the measured relative lateral displacement ysr and the estimated relative lateral displacement ysr 'for feedback. Therefore, the arithmetic unit 15 ′ shown in FIG. 6 is still insufficient. The symbol Δysr in FIG. 7 indicates the relative lateral displacement ys for feedback.
The distance of the influence of the road curvature ρ on the estimation of r ′ is shown.

Therefore, in order to configure the algorithm of the arithmetic unit 15 'in consideration of the influence of the road curvature ρ on the estimation of the relative lateral displacement ysr' for feedback, first, the radius of curvature of the road is calculated based on FIG. When an exact solution of the relative lateral displacement ysr of the vehicle with respect to the road at the front gazing point Z is obtained as R,

[Equation 8] Becomes

However, the equation (17) is too non-linear and cannot be formulated as an output equation. Therefore, the influence of the road curvature ρ on the relative lateral displacement ysr is calculated based on FIG. That is, in a state in which the vehicle center line heading in the vehicle front-rear direction is in contact with the target travel line M having the same curvature ρ as the road curvature ρ at the position of the center of gravity G of the vehicle, between the vehicle front gazing point Z and the target travel line M. The separation distance Δysr along the vehicle width direction is defined as the relative lateral displacement ys for feedback.
When calculated as the influence of the road curvature ρ on the estimation of r ′, Δysr = R−√ (R ² −Ks ² ) (18).

Here, the radius of curvature R = 1 / ρ is given by the equation (18).
Δysr = 1 / ρ−√ (1 / ρ ² −Ks ² ) (19) In the numerator and denominator on the right side of the equation (19), 1 / ρ + √
Multiplying by (1 / ρ ² −Ks ² ), Δysr = ρKs ² / (1 + √ (1−ρ ² Ks ² )) (20) Here, if an approximation of (1 + X) ⁿ ≈1 + nX is used, When ^{(1-ρ 2 Ks 2)} ≒ 1-ρ 2 Ks 2/2 (21) equation (21) into equation (20),

[Equation 9] By ignoring the higher order terms of the equation (22), △ ysr ≒ -ρKs 2/2 (23) is obtained.

Therefore, the relative lateral displacement y for feedback
The influence of road curvature ρ on the estimation of sr 'is calculated as (-ρKs ²
By approximating with / 2), the output equation becomes

[Equation 10] Becomes

As a result, the output equation (24) is obtained.
Since the equation (13) is obtained as the state equation, the arithmetic unit 15 in the state estimating apparatus 1 according to the present invention is obtained.
The configuration of the algorithm is as shown in FIG. That is, the arithmetic unit 15 includes the first coefficient unit 55, the second coefficient unit 57, the third coefficient unit 60, the fourth coefficient unit 61, and the fifth coefficient unit 6.
3, an integrator 65, a first adder 67, a second adder 69, and a third adder 71.

Then, the first to fourth coefficient units 55, 5
Reference numerals A, B, E and D attached to 7, 60 and 61 are,
The matrices A, B, described in equations (13) and (24)
The sign S attached to the integrator 65 represents E and D, and the sign L attached to the fifth coefficient unit 63 represents a matrix called an output error feedback coefficient.

When the arithmetic unit 15 is configured as a Kalman filter, this matrix L is given by the following equation, where Γ is the variance of the white Gaussian noise W in equation (12) and Σ is the variance of the relative lateral displacement measurement value ysr. expressed.

L = PE ^T Σ ⁻¹ (25) where P satisfies AP + PA ^T + Γ−PE ^T Σ ⁻¹ EP = 0 (26), A is the matrix A and E is E ,
The matrix E is shown, and T is a transposed value.

By the way, each of the matrices A, B, D, E and L is a vehicle speed variable parameter which changes with a change in the vehicle speed V. In the present embodiment, based on the vehicle speed V detected by the vehicle speed sensor 13. To change each matrix A, B, D, E, L. However, each of the matrices A, B, D, E, and L has a large tolerance for modeling error, and for example, V = 80 k
Each matrix A, B, D, E, L modeled by m / h is V =
Since no problem actually occurred even when used at 30 km / h, it is not always essential to change each matrix A, B, D, E, L according to the vehicle speed V.

However, as in the present embodiment, the vehicle speed V is detected, and based on the detected vehicle speed V, each matrix A, B,
It is preferable to change D, E, and L because the output value from the computing device 15 can be maintained with high accuracy over a wide range of vehicle speed V.

As shown in FIG. 10, in the arithmetic unit 15 as well, as in the arithmetic unit 15 'shown in FIG. 6, the measured front wheel steering angle δf and rear wheel steering angle δr are input as a matrix u and measured. The relative lateral displacement ysr of the vehicle is input,
The vehicle yaw rate ψ ^* , the relative yaw angle ψr, the relative center-of-gravity position lateral displacement velocity ycr ^* , the relative center-of-gravity position lateral displacement ycr, and the road curvature ρ are estimated by calculation and output. Further, the relative lateral displacement ysr ′ for feedback is also estimated by calculation, and the estimated relative lateral displacement ysr ′ is fed back to the second adder 69.

Here, the arithmetic unit 15 'shown in FIG. 6 and FIG.
The comparison with the arithmetic unit 15 shown in FIG. 11 to 14
Is the distance Ks from the vehicle center of gravity G to the front gazing point Z of 15
Relative lateral displacement ysr 'for feedback when m
And a road curvature ρ are graphs showing a vehicle relative gravity center position lateral displacement ycr and a relative yaw angle ψr as parameters. Then, in each graph, for comparison, an exact solution of the relative lateral displacement ysr of the vehicle at the forward gazing point Z obtained by the expression (17) is also displayed.

The relationship between each graph and both parameters is as shown in the following table.

[0079]

[Table 1] As apparent from FIGS. 11 to 14, in the arithmetic unit 15 ′ shown in FIG. 6, the relative lateral displacement ysr ′ for feedback is used.
Since the influence of the road curvature ρ on the estimation of is not taken into consideration, the relative lateral displacement for feedback ysr ′ calculated by the calculation device 15 ′ shown in FIG. 6 becomes an exact solution as the road curvature ρ increases. The error is increasing. In contrast,
The relative lateral displacement for feedback ysr ′ calculated by the calculation device 15 shown in FIG. 10 is in good agreement with the exact solution.

FIG. 15 shows a simulation result when the lane-following automatic traveling of the vehicle on a curved road is performed based on the output from the arithmetic unit 15 'shown in FIG. 6, and the output from the arithmetic unit 15 shown in FIG. It is a figure which overlaps and shows the simulation result at the time of performing lane following automatic running of the vehicle on a curved road based on road curvature ρ.
(B) shows the transition of the relative center-of-gravity position lateral displacement ycr of the vehicle with respect to the road, (c)
Indicates the transition of the steering angle of the steering wheel. For the simulation, the vehicle speed is 80 km / h and the radius is 100
The vehicle is set to enter the curved road of m.

As shown in FIG. 15, when the vehicle automatically travels on a curved road based on the output from the arithmetic unit 15 'shown in FIG. 6, it is based on the output from the arithmetic unit 15 shown in FIG. Road curvature ρ
The estimated value of 1 converges slowly, and as a result, a large relative center of gravity position lateral displacement ycr occurs, and a slight relative center of gravity position lateral displacement ycr occurs in the vehicle traveling on a curve. Therefore, FIG.
By controlling the electric motor 41 of the automatic steering mechanism 5 with the controller 40 on the basis of the output from the arithmetic unit 15 shown in the figure, it can be seen that the vehicle can be made to smoothly curve.

As described above, in the state estimating device 1 according to this embodiment, the computing device 15 includes the relative lateral displacement ysr measured by the relative lateral displacement measuring means 7 and the front wheel steering angle sensor 9.
To calculate the road curvature ρ based on the steering angle δf of the front wheels 21 of the vehicle, the steering angle δr of the rear wheels 23 of the vehicle detected by the rear wheel steering angle sensor 11, and the vehicle speed V detected by the vehicle speed sensor 13. Since the estimation is performed, the image processing in estimating the road curvature ρ can be restricted to perform a quick calculation.

Moreover, in the state estimation device 1, the calculation device 1
5 estimates the relative lateral displacement ysr 'for feedback in consideration of the influence of the road curvature ρ very accurately, so the accuracy of the estimated relative lateral displacement ysr' for feedback is improved, and the estimated relative lateral displacement ysr 'for feedback is improved. The convergence time until the displacement ysr 'converges to the true value is shortened, and the calculation time required for estimating the road curvature ρ is shortened. For example, when used in a vehicle with automatic steering, stable traveling control is achieved by quick calculation. Therefore, extremely smooth and accurate traveling control can be performed when traveling on a curve.

Further, in the state estimating device 1, since the non-linear expression is eliminated from the calculation for estimating the relative lateral displacement ysr 'for feedback, the algorithm of the calculating device 15 is simple and the calculation required for estimating the road curvature ρ. Time is reduced.

Furthermore, in the state estimation device 1, the calculation device 15
Is the relative lateral displacement ysr measured by the relative lateral displacement measuring means 7.
And the steering angle δf of the front wheel 21 of the vehicle detected by the front wheel steering angle sensor 9 and the rear wheel 23 of the vehicle detected by the rear wheel steering angle sensor 11.
Since the road curvature ρ is estimated by a calculation based on the steering angle δr and the vehicle speed V detected by the vehicle speed sensor 13, the road curvature ρ can be accurately estimated over a wide range of vehicle speeds V.

In the vehicle traveling control system 2 according to this embodiment, the arithmetic unit 15 includes the relative lateral displacement ysr measured by the relative lateral displacement measuring means 7 and the steering angle of the front wheel 21 of the vehicle detected by the front wheel steering angle sensor 9. Since the road curvature ρ is estimated by calculation based on δf, the steering angle δr of the rear wheel 23 of the vehicle detected by the rear wheel steering angle sensor 11, and the vehicle speed V detected by the vehicle speed sensor 13, the road curvature ρ can be estimated. The image processing is regulated to enable a quick calculation, and the calculation device 15 estimates the relative lateral displacement ysr 'for feedback in consideration of the influence of the road curvature ρ very accurately.
The accuracy of r ′ is improved, the convergence time for the estimated value ysr ′ to converge to the true value is shortened, and the calculation time required for estimating the road curvature ρ is shortened. As a result, stable traveling control is achieved by quick calculation. Therefore, it is possible to perform extremely smooth and accurate traveling control when traveling on a curve.

Further, in the vehicle running control device 2, since the non-linear expression is eliminated from the calculation for estimating the relative lateral displacement ysr 'for feedback, the algorithm of the calculation device 15 is simple and required for estimating the road curvature. The calculation time is shortened, and as a result, smoother and more accurate traveling control can be performed when traveling on a curve.

Further, in the vehicle traveling control device 2, the arithmetic unit 15 has the relative lateral displacement y measured by the relative lateral displacement measuring means 7.
sr, the steering angle δf of the front wheel 21 of the vehicle detected by the front wheel steering angle sensor 9, the steering angle δr of the rear wheel 23 of the vehicle detected by the rear wheel steering angle sensor 11, and the vehicle speed V detected by the vehicle speed sensor 13. Since the road curvature ρ is estimated by a calculation based on, it is possible to accurately estimate the road curvature ρ over a wide range of vehicle speeds V, and as a result, when traveling on a curve, smoothly and accurately travel over a wide range of vehicle speeds V. It becomes possible to control.

In the present embodiment, the steering angles δf and δr of the front wheels 21 and the rear wheels 23 are detected and input to the arithmetic unit 15. However, what is input to the arithmetic unit 15 is the steering of the front wheels 21. Either the angle δf or the steering angle δr of the rear wheel 23 may be used.

That is, when only the steering angle δf of the front wheels 21 is input to the arithmetic unit 15, the matrix B of the equation (13) is changed to

[Equation 11] Then, the matrix D of the equation (24) may be set to D = 0.

When only the steering angle δr of the rear wheels 23 is input to the arithmetic unit 15, the matrix B of the equation (13) is changed to

[Equation 12] Then, the matrix D of the equation (24) may be set to D = 0.

As a result, even when only the steering angle δf of the front wheels 21 is detected and input to the arithmetic unit 15, or when only the steering angle δr of the rear wheels 23 is detected and input to the arithmetic unit 15. Even in this case, the algorithm of the arithmetic unit 15 can be configured by the same process as in the case of detecting both steering angles δf and δr and inputting them to the arithmetic unit 15.

[Brief description of drawings]

FIG. 1 is a schematic configuration diagram showing an example of an embodiment in which the inventions according to claims 1 to 6 are also implemented, wherein (a) is a schematic configuration view seen from a side of an automobile, and (b) is It is a schematic block diagram seen from a plane.

FIG. 2 is a flowchart executed by the state estimation device according to the present embodiment.

FIG. 3 is an explanatory diagram showing a two-wheel model for designing the state estimation device according to the present embodiment.

FIG. 4 is an explanatory diagram showing a relative lateral displacement of a vehicle with respect to a straight road at a gazing point in front of the vehicle.

FIG. 5 is a graph of Poisson square wave showing the behavior of road curvature.

FIG. 6 is an explanatory diagram showing an algorithm of a state estimation device which is not adopted in the present invention.

FIG. 7 is an explanatory diagram showing relative lateral displacement of the vehicle with respect to a curved road at a vehicle front gazing point estimated by the one shown in FIG. 6;

FIG. 8 is an explanatory diagram for obtaining an exact solution of a relative lateral displacement of a vehicle with respect to a curved road at a vehicle front gazing point.

FIG. 9 is an explanatory diagram for obtaining an approximate solution of relative lateral displacement of a vehicle with respect to a curved road at a vehicle front gazing point.

FIG. 10 is an explanatory diagram showing an algorithm of the state estimation device according to the present embodiment.

11 is a graph showing the difference between the one shown in FIG. 6 and the one shown in FIG.

FIG. 12 is a graph showing the difference between the one shown in FIG. 6 and the one shown in FIG.

13 is a graph showing the difference between the one shown in FIG. 6 and the one shown in FIG.

14 is a graph showing the difference between the one shown in FIG. 6 and the one shown in FIG.

15 is a graph showing a difference between a vehicle traveling control device using the one shown in FIG. 6 and a vehicle traveling control device using the one shown in FIG. 10 when traveling on a curved road, FIG. The convergence state of the curvature estimated value is shown, (b) shows the transition of the relative center-of-gravity position lateral displacement of the vehicle, and (c) shows the transition of the steering wheel steering angle.

FIG. 16 is a block diagram showing an example of a conventional product.

FIG. 17 is a diagram showing an example of a road image shown in FIG. 16;

FIG. 18 is a diagram showing a coordinate system of the one shown in FIG. 16;

[Explanation of symbols]

1 State estimation device 2 Vehicle traveling control device 5 Automatic steering mechanism 7 Relative lateral displacement measuring means 9 Front wheel rudder angle sensor (rudder angle detection means) 11 Rear wheel rudder angle sensor (rudder angle detection means) 13 Vehicle speed sensor (vehicle speed detection means) 15 Computing means (computing device) 21 front wheels 23 rear wheel 40 controller G vehicle center of gravity M target travel line V vehicle speed Z vehicle front gazing point ycr Relative center of gravity lateral displacement ysr relative lateral displacement relative lateral displacement for ysr 'feedback △ ysr distance δf Front wheel steering angle δr Rear wheel steering angle ρ road curvature ψr Relative yaw angle (relative declination)

─────────────────────────────────────────────────── --- Continuation of the front page (56) References JP-A-6-300581 (JP, A) JP-A-9-175295 (JP, A) JP-A-8-263794 (JP, A) JP-A-7- 27541 (JP, A) (58) Fields investigated (Int.Cl. ⁷ , DB name) G08G 1/16 B62D 6/00 B60R 21/00

Claims (6)

(57) [Claims] 1. A relative lateral displacement measuring means for measuring a relative lateral displacement of a vehicle with respect to a road at a predetermined vehicle gazing point, and a steering angle detecting means for detecting a steering angle of at least one of a front wheel and a rear wheel of the vehicle. When the a calculating means for estimating the relative transverse displacement, the arithmetic means for at least the road curvature and feedback by calculation based on the relative lateral displacement and the steering angle, the eyes having the same curvature and the road curvature
The center line of the vehicle is
While touching at the position of the center of gravity,
The distance along the vehicle width direction from the target running line is
The road curvature affects the estimation of relative lateral displacement for feedback.
A state estimation device characterized by being calculated as a blur effect . 2. The state estimating apparatus according to claim 1, wherein the calculating means is a phase for the road curvature and feedback.
In addition to the lateral displacement, the direction along the road in front of the vehicle
Relative declination of the vehicle and the center of gravity of the vehicle relative to the road
Lateral displacement and the estimated road curvature and relative declination
And the feed as a linear combination of lateral displacement of relative center of gravity
A state estimating device characterized by expressing a relative lateral displacement for a back . 3. The state estimating device according to claim 1, wherein the vehicle speed detecting device detects a vehicle speed of the vehicle.
And a step of calculating the relative lateral displacement measuring means.
The measured relative lateral displacement and the rudder detected by the rudder angle detecting means.
Based on the angle and the vehicle speed detected by the vehicle speed detecting means.
A state estimation device characterized by calculating . 4. A vehicle on a road at a predetermined vehicle front gazing point
A relative lateral displacement measuring means for measuring the relative lateral displacement of both,
Detect the steering angle of at least one of the front and rear wheels of the vehicle
Steering angle detection means and calculation based on the relative lateral displacement and steering angle
By at least for road curvature and feedback
Computation means for estimating relative lateral displacement and steering of the vehicle
Automatic steering mechanism automatically performed and output from the computing means
A controller for controlling the automatic steering mechanism based on
A vehicle travel control device comprising:
The target vehicle has the same curvature as the road curvature,
The center line of the vehicle facing the front-back direction touches at the position of the center of gravity
State between the vehicle front gazing point and the target running line,
The separation distance along the vehicle width direction can be set as the feedback distance.
It plays as an effect of the road curvature on the estimation of relative lateral displacement.
A vehicle travel control device characterized by: 5. The vehicle traveling control device according to claim 4,
The calculation means is configured to calculate the road curvature and the feedback.
Along the road in front of the vehicle, in addition to the relative lateral displacement for
The relative declination of the vehicle with respect to and the relative weight of the vehicle with respect to the road
The lateral displacement of the heart position is estimated, and the estimated road curvature and phase are estimated.
As a linear combination of the pair declination and the lateral displacement of the relative center of gravity,
A vehicle travel control device characterized by representing a relative lateral displacement for feedback. 6. The vehicle running according to claim 4 or 5.
A line control device for detecting a vehicle speed of the vehicle
Output means, and the calculation means is the relative lateral displacement measuring hand.
The relative lateral displacement measured by the step is detected by the steering angle detection means.
Based on the steering angle and the vehicle speed detected by the vehicle speed detecting means.
Vehicle running control apparatus characterized by calculating Te.