JP2917681B2 - Road shape measurement 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 road shape measuring device for measuring the shape of a curve or a slope of a road ahead in the traveling direction of a vehicle, for example, for automatic driving of a vehicle or for safe driving.

[0002]

2. Description of the Related Art In order to perform automatic driving and preventive safe driving such as rear-end collision prevention, the shape data of the road ahead of the traveling direction of the vehicle, for example, the distance from the road edge of the vehicle, the road direction and the direction of the vehicle. It is necessary to accurately and quickly measure the yaw angle, the horizontal curvature, which is the curve curvature of the road, the vertical curvature, which is the gradient change of the road, and the like.

[0003] Such three-dimensional shape data of a road is estimated from an image captured by a camera mounted on a vehicle. In this case, the height and pitch angle of the camera are conventionally determined from the attitude of the camera with respect to the road. The road shape data is measured on the assumption that the roll angle is constant.

[0004] By the way, the camera mounted on the vehicle changes its attitude due to the unevenness of the road surface when the vehicle travels. However, if the attitude of the camera changes, the three-dimensional shape data of the road also includes an error. become. However, when the vehicle actually travels, the camera attitude fluctuates due to the influence of steps on the road surface, etc. Therefore, in order to accurately obtain three-dimensional shape data of the road, the camera height, pitch angle, roll angle, etc. also fluctuate. To do this, it is necessary to accurately determine the camera attitude.

[0005]

As described above, conventionally, among the postures of a camera with respect to the road, the height of the camera,
Since the three-dimensional shape data of the road is obtained under the assumption that the pitch angle and the roll angle are constant and the curvature of the curve is also constant, the vehicle travels and the camera attitude fluctuates. In addition, errors are included in the measured three-dimensional shape data of the road, and estimation errors are unavoidable in a transition curve portion or an S-shaped curve where the curvature changes,
There is a problem that road shape data cannot be measured accurately.

[0006] The present invention has been made in view of the above,
The purpose is to create a three-dimensional road model with a polynomial curve while considering the condition that the attitude of a camera, which is an image pickup means mounted on the vehicle, fluctuates due to the running of the vehicle, and to create three-dimensional shape data of the road. It is an object of the present invention to provide a road shape measuring device capable of measuring a road at high speed with high accuracy.

[0007]

In order to achieve the above object, a road shape measuring apparatus according to a first aspect of the present invention is shown in FIG.
As shown in FIG. 7, an image pickup means 81 for picking up an image of a road ahead in the traveling direction of the vehicle, and a road shape detecting means 83 for detecting a road shape ahead of the vehicle in the traveling direction from the image taken by the image pickup means.
A road model calculating means 85 for calculating a three-dimensional road model defined on three-dimensional coordinates based on three-dimensional curve parameters; and a three-dimensional road model calculated by the road model calculating means, Coordinate conversion means 87 for converting to image coordinates based on parameters, and comparing the road shape detected by the road shape detection means with the three-dimensional road model coordinate-converted by the coordinate conversion means,
Displacement detection means 89 for detecting a displacement between the two,
The estimation updating unit 91 for estimating the amount of change of the three-dimensional curve parameter and the amount of change of the attitude parameter of the imaging unit from the positional deviation detected by the positional deviation detecting unit, and updating the parameters. I do.

[0008]

In the road shape measuring apparatus according to the first aspect of the present invention, the road shape ahead of the vehicle in the traveling direction is detected from the image picked up by the image pickup means, and the road shape is detected based on the three-dimensional curve parameters.
The three-dimensional road model defined on the three-dimensional coordinates is calculated, the three-dimensional road model is converted into image coordinates based on the posture parameters of the imaging means, and the road shape is compared with the coordinate-converted three-dimensional road model. Then, the positional deviation between the two is detected, the amount of change of the three-dimensional curve parameter and the amount of change of the posture parameter are estimated from this positional deviation, and each parameter is updated.

[0009]

Embodiments of the present invention will be described below with reference to the drawings.

FIG. 2 is a block diagram schematically showing an entire configuration of a road shape measuring apparatus according to one embodiment of the present invention. The road shape measuring device shown in FIG. 1 is a camera 1 which is an image pickup means mounted on a vehicle for imaging a road ahead in the traveling direction of the vehicle, and inputs an image signal picked up by the camera 1 and receives the input image. An image processing unit 3 that performs image processing on the signal, and a road 3 that is a measurement result obtained by the image processing unit 3
The control unit 5 displays the dimensional shape data, and controls the vehicle and makes an alarm determination based on the shape data.

An image processing unit 3 shown in FIG. 1 is continuously input with an image of a two-lane road taken by the camera 1, and from this image, the height (Dy), yaw angle (θ), pitch of the camera 1 In addition to the camera's five-axis behavior parameters except the vehicle speed, the angle (φ) and the roll angle (φ), the distance from the road edge (Dx),
A three-dimensional road structure having road shape parameters of a horizontal curve (ρ) of a road and a vertical curve (μ) representing a gradient of the road is simultaneously estimated online. More specifically, in this embodiment, a three-dimensional road model using a horizontal curve (ρ) and a vertical curve (μ) as a road shape parameter is created, and the three-dimensional road model is determined based on the five-axis behavior parameter of the camera one screen before. The two-dimensional road model is coordinate-transformed into a two-dimensional image.
The amount of change in the road shape parameter and the behavior parameter of the camera is estimated and updated from the correspondence relationship with the input image of the road captured in step (1). The road model is represented by a line graph, and the correspondence between the road model and the input image from the camera is a line correspondence. Also, assuming that the amount of change in each parameter of the road shape parameter and the camera behavior parameter is small, it can be estimated by a linear equation. This makes it possible to easily calculate line correspondence matching, equation creation and solution, etc. Suitable for. In addition, since the road curve ahead is not affected by the behavior of the vehicle, it can be obtained with high accuracy.

FIG. 3 is a block diagram showing a detailed configuration of the image processing section 3 shown in FIG. As shown in FIG. 1, the image processing unit 3 includes an image input unit 11 to which an image captured by the camera 1 is input, and a right edge of a white line of a road from the image information input from the image input unit 11. A pre-processing unit 13 to be detected, a road model creation unit 15 to create a three-dimensional road model based on a road shape parameter estimated one screen before (previous time), using a camera behavior parameter estimated at the previous time. Then, the three-dimensional road model created by the road model creating unit 15 is coordinate-transformed into the vehicle coordinate system, and further, is perspective-transformed into the image coordinate system, and the coordinate transformation unit 17 transforms the coordinate into the image coordinate system. A line correspondence matching unit 19 for associating the road model thus detected with the edge image of the road detected by the pre-processing unit 13 in a line correspondence relationship. A parameter estimating unit 21 for detecting a displacement of an image and estimating a change amount of each of a road shape parameter and a behavior parameter of a camera from the displacement, and updating a parameter for updating the parameter estimated by the parameter estimating unit 21 It is composed of a part 23.

In the preprocessing in the preprocessing unit 13, the input image is set to A (x, y) and the edge image is set to G ( x, y), the following calculation is performed, and
Detect an edge image.

[0014]

(Equation 1) By using such an arithmetic expression, an edge can be extracted even if a distant white line is almost horizontal on a curved road and the line width is 1 pixel or less.

The road model creation unit 15 and the coordinate conversion unit 1
In step 7, a three-dimensional road model is created based on the road shape parameters estimated at the previous time, the coordinates are transformed into a vehicle coordinate system using the camera behavior parameters estimated at the previous time, and further two-dimensional. Performs perspective transformation to the image coordinate system. In this embodiment, it is assumed that the parameters of the road curve and gradient change with time. The focal length of the camera 1 is assumed to be known, and the speed component of the vehicle is not estimated. This is because the apparent position of the white line does not change even if it moves parallel to the white line. Therefore, the parameters to be estimated are
The height (Dy), yaw angle (θ), pitch angle (φ), roll angle (φ), distance from the road edge (D
Road shape parameters including horizontal curvature (ρ) and vertical curvature (μ) in addition to the 5-axis behavior parameters of the camera x).

First, the coordinate system will be described with reference to FIG. As the coordinate system, three coordinate systems are defined: a road coordinate system (X, Y, Z), a vehicle coordinate system (U, V, W), and a two-dimensional image coordinate system (x, y).

The road coordinate system (X, Y, Z) represents the shape of the road based on the point where the vehicle is currently located. As shown in FIG. 4, the origin is located on the road surface at the center of the road. Take the road connection and Z parallel to the road surface
Axis, X axis parallel to the road surface to the left, Y upward perpendicular to the road surface
Right-handed system with axis. It is assumed that the center of the lens of the camera 1 is always at the position of Z = 0.

The vehicle coordinate system (U, V, W) has the origin at the center of the lens of the camera 1, the W axis in the main axis direction of the lens,
This is a right-handed system that takes the U axis and the V axis in parallel with the imaging surface.

The image coordinate system (x, y) is defined on the imaging plane, and the x-axis is parallel to the U-axis, and the y-axis is parallel to the V-axis, which are opposite to each other.

A point P (U, V, W) in the three-dimensional space represented by the vehicle coordinate system is perspectively transformed and represented by the following equation in the image coordinate system on the image.

X = −F · U / W y = −F / V / W (1) where F is the focal length of the lens of the camera 1.

The relationship between the road coordinate system and the vehicle coordinate system corresponds to the behavior of the vehicle, that is, the attitude of the camera 1. Hereinafter, the distance of the camera 1 from the road edge is Dx, and the height of the camera 1 is D.
y, the yaw angle is represented by θ, the pitch angle is represented by φ, and the roll angle is represented by φ.

In the vehicle coordinate system, first, (Dx,
(Dy, 0), and then rotate in the order of yaw angle θ, pitch angle φ, and roll angle φ. Note that the yaw angle θ is
The pitch angle φ is the angle between the W axis and the XZ plane when the W axis is projected onto the XZ plane, the pitch angle φ is the angle between the W axis and the XZ plane, and the roll angle φ is the rotation angle about the W axis and the U axis and the XZ plane. And the angle. Assuming that the rotation matrix is R, the point represented by the road coordinate system is converted to the vehicle coordinate system by the following equation.

[0024]

(Equation 2) Next, a road model created by the road model creating unit 15 will be described.

The structure of a road is determined by dividing it into a transverse curve (curve) and a vertical curve (gradient). A crossing road is defined by an arc portion having a constant curvature, a straight line portion, and a transition curve portion for smoothly connecting these portions. In addition, the longitudinal curve smoothly connects straight lines having a constant slope by a parabola.

On the three-dimensional road coordinates, both the transverse curve and the longitudinal curve are approximated by a multi-dimensional curve. The coordinates of the white line drawn on the road surface in the road coordinate system are defined by the following equation.

X = f (Z) = aZ ⁴ + bZ ³ + cZ ² + BY Y = g (Z) = dZ ³ + eZ ² (4) where B represents the distance from the road center to the white line.
For a lane road, B is a positive constant for the left white line and 0 for the center. a to e are required road shape parameters. The reason why the horizontal expression f (Z) is a quartic expression is to cope with an S-shaped curve.

The horizontal curvature ρ and the vertical curvature μ of the road are
Is represented by the following equation.

[0029]

(Equation 3) In particular, the curvature near Z = 0 is a horizontal curvature ρ = 2c and a vertical curvature μ = 2e.

In this way, if Zi (i = 1 to N) for creating a road model composed of N points is given, the three-dimensional coordinates of the white line can be calculated. It is also possible to obtain the curvature at the point Z.

The road shape parameters a, b, c, d and e shown in the above equation (4) are obtained from the previous screen (previous time).
A road model is created, and this road model is converted into a vehicle coordinate system based on the behavior parameters of the camera 1 at the previous time. Then, by observing how each point on the road model moves on the current screen (current time), the behavior fluctuation amount and the road parameter fluctuation amount in the vehicle coordinate system are obtained. That is, it is assumed that the point (x, y) on the road model has changed by (Δx, Δy) on the current screen, and the change Δx, Δy
Is derived from the fluctuation of the behavior parameter of the camera and the fluctuation of the road shape parameter, and the relationship between the fluctuation amount of each parameter and Δx and Δy is derived. Note that the parameter is obtained by integrating the variation at each time, but since the variation is based on the model, no error is accumulated.

Assuming that the point (x, y) on the road model on the image has moved to (x ', y') due to the change in the parameters, and the movement amount of the point is (Δx, Δy), the following equation is obtained. expressed.

X ′ = x + Δx y ′ = y + Δy (5) The change in the behavior parameter of the camera is considered as the movement of a point in the vehicle coordinate system. That is, by calculating the behavior variation amount in the vehicle coordinate system and combining it with the behavior parameter at the previous time,
It is converted into the camera behavior parameters at the current time, and a new vehicle coordinate system is created at the next time. The reason why the behavior fluctuation amount is obtained not in the road coordinate system but in the vehicle coordinate system is that the rotation angle always fluctuates from 0, so that a linear solution can be obtained while maintaining the approximation accuracy.

In the vehicle coordinate system, a point P (U, V, W)
Moves to the point P '(U', V ', W'),

(Equation 4) It is represented by D _U and D _U are translation components, and
α, β, and γ are rotation angles around the V axis, U axis, and W axis, respectively. However, the value of each parameter is assumed to be sufficiently small, sin δ = δ, cos δ = 1, sin α = α, co
sα = 1, sinβ = β, cosβ = 1, sinγ =
γ, cosγ = 1, and terms of second and higher order were ignored.

From the equations (6) and (1), the following equation is obtained.

[0036]

(Equation 5) Variations in image coordinates due to minute variations in each behavior parameter are:
From the first-order term of the Taler expansion of the equation (5), it is expressed as the following equation.

[0037]

(Equation 6) When the condition of α, β, γ = 0 is approximately calculated from the expression (8), the variation of the behavior parameter of the camera is obtained as shown in the following expression.

[0038]

(Equation 7) Next, the variation of the road parameter will be considered. From the above equations (2) to (4), U = R ₁₁ (X−D _x ) + R ₁₂ (Y−D _Y ) + R ₁₃ Z V = R ₂₁ (X−D _x ) + R ₂₂ (Y−D _Y) ) + R ₂₃ Z X = f (Z), Y = g (Z), so that this is substituted into equation (8), and α, β, γ = 0

(Equation 8) Is calculated, the variation of the road parameter is obtained as shown in the following equation.

[0039]

(Equation 9) Therefore, when the expressions (9) and (10) are added, a relational expression of the movement amount of the point on the image based on the camera behavior parameter and the road parameter is obtained.

[0040]

(Equation 10) Next, line matching in the line matching unit 19 will be described.

The camera posture estimating method using the model includes point correspondence and line correspondence.
That is, the tangent line in the road model is associated with the line component of the white line extracted from the screen. This is because the white line is a smooth curve and it is difficult to correspond to points.

Although the search requires searching for an image,
Here, scanning is performed on the same x axis or the same y axis. This is because the search process can be simplified, which leads to a higher speed.
Selection between x-axis scanning and y-axis scanning is made according to the magnitude of the slope of the associated line.

A point P on the road model in the image coordinate system
The slope of the tangent at (x, y) is represented by ω, and ω is

[Equation 11] Is calculated by the following equation.

[0044]

(Equation 12) It is assumed that the white line at the current time has the same inclination of ω as that of the model at a point P ′ (x ′, y ′) near the point P. That is,
It is assumed that there is no change in the inclination. It is assumed that the model obtained at the previous time has moved to the position of the white line at the current time. The relationship between the road model and the white line in this case is as shown in FIG.
Let p be the amount of movement on the same x axis and q be the amount of movement on the same y axis.
Then, the relationship between Δx and Δy is Δx / p + Δy / q = 1 and q / p = −ω, so that Δx−Δy / ω−p = 0 (13a) or −ωΔx + Δy−q = 0 (13b) is obtained. The above equation represents the relationship between the (apparent) movement amount in the direction of the coordinate axis of the straight line and the actual movement amount. When | ω | is large (close to vertical), equation (13a) is used, and | ω |
When | is small (close to horizontal), equation (13b) is used.

At some points on the road model, Pi (x
i, yi) and ωi are calculated, pi or qi is obtained from the correspondence with points on the input image, and the least-squares method is applied to obtain simultaneous equations, and the amount of change of each parameter is calculated.

The evaluation error is obtained by the following equation from the above-described equations (11) and (13).

[0047]

(Equation 13) However

[Equation 14] Next, the operation of the present embodiment will be described.

First, in the road model creation unit 15, N
Create a road model consisting of points. Then, from the estimation result of the behavior parameters (Dx, Dy, θ, φ, φ) of the camera at the previous time, a conversion matrix of Expression (3) is created. Then, the road shape parameters (a, b, c, d,
e) to road coordinate system (X, Y, Z) based on equation (4)
Calculates the three-dimensional coordinates of the road model. That is, a point sequence of a road model is created by setting Z = Zi (i = 1 to N) in equation (4) from the road shape parameters (a, b, c, d, e) at the previous time. Next, it is transformed into the vehicle coordinate system (U, V, W) by the equation (2) according to the camera behavior parameters (Dx, Dy, θ, φ, φ) at the previous time, and further, by the equation (1). (X, y) and convert the coordinates (x
i, yi) (i = 1 to N). Further, the inclination ω of the tangent is calculated by the equation (12). Each coordinate conversion is performed by the coordinate conversion unit 17.

The value calculated from the road model is (14)
Zi, Ui, Vi, Wi, ωi and x appearing in the equation
i, yi (i = 1 to N). Among them, Zi is predetermined on the road coordinates, for example, at an interval of 3 meters. That is, if Zi is given, Ari (r = 1) in equation (14)
To 10) are determined. Next, Bi is obtained by performing matching with the input image.

The line correspondence matching section 19 is provided with the image input section 1
1 performs line correspondence matching between an edge image, which is an input image obtained from the camera 1, and a model point connection. ｜ ω
When i | ≧ 1 (close to vertical), each model point (x
i, yi) is searched in the x-axis direction to obtain pi,
When | ωi | <1 (close to horizontal), search in the y-axis direction to obtain qi.

When | ωi | ≧ 1, the point (xi, yi)
Width in the center of the M _x, consider the height M _y become window. Among them, a straight line having a gradient ωi is generated, and a straight line having the largest sum of the edge densities on the straight line is selected.
The difference from i is pi. That is,

(Equation 15) Is obtained, and the center of gravity is obtained near the maximum value of Ci (τ), which is defined as pi. Since the value of Ci (τ) at this time represents certainty, it is set to Ci again.

When | ωi | <1, qi is similarly obtained. Note that the number of pixels is suitable for a width M _x, height M _y.

As described above, all the coefficients of the evaluation error represented by the equation (14) are obtained.
1 can be estimated.

Here, another error measure is considered. One is to use the fact that road parameters do not fluctuate greatly over time. That is, it can be said that Δa to Δe are close to 0. _{Therefore, E G1 = Δa, E G2} = Δb, E G3 = Δc, E G4 = Δd, E G5 = Δe, E as _{G5 ~E G10 = 0 (15)} , each of the weight Gr (r = 1 to 5 ) Is defined as a constant.

Another error relates to curvature. The road parameters c and e are 1 / curvature at the point where Z = 0.
2. Assuming that the vehicle has a known speed and is traveling at a speed v (m (meter) / 1 screen time), the curvature at the point of Z = vt before the t screen should be the curvature of Z = 0 on the current screen. It is. Therefore,

(Equation 16) However

[Equation 17] It can be. Note that c _k−1 and e _k−1 are the results of the previous screen, respectively. Further, ρ _k−1 and μ _k−1 are curvatures obtained from the result before t screens.

Each parameter is estimated by the method of least squares.
That is,

(Equation 18) Differentiate the above equation with each variable so as to minimize
far. Note that Hρ and Hμ are weight constants. In this way, the following 10-element simultaneous linear equation

[Equation 19] Is created. Here, 1 (lowercase letter L) is 1 to 10, and m = 1 to 10.

This 10-element simultaneous linear equation can be easily solved.

As described above, the parameter estimating section 21
, The change amount of each parameter is calculated by the least squares method. Based on these results, the camera behavior parameters (Dx, Dy, θ, φ, φ) and road shape parameters (a, b, c, d, e) is updated in the parameter update unit 23. The parameter at the previous time is represented by a subscript k-1.
, And the updated parameter is represented by a subscript k.

First, the road shape parameters a, b, c,
As for d and e, Δa to Δe may be added as they are. That is, the following equation is obtained.

A _(k) = a _(K−1) + Δa, b _(k) = b _(K−1) + Δb, c _(k) = c _(K−1) + Δc, d _(k) = d _{(K -1)} + Δd, e _(k) = e _(K-1) + Δe Further, the camera behavior parameters Dx, Dy, θ, φ, φ
Is updated as follows. The parameters (D _U , D _U , α, β, γ) in the equation (6) are all 0 at k−1.
D _U = ΔD _U , D _U = ΔD _U , α = Δ
α, β = Δβ, γ = Δγ. Substituting equation (2) into equation (6) gives

(Equation 20) Becomes By updating the behavior parameters of the camera, (U, V, W) = (U ′, V ′, W ′). By comparing the expressions (2) and (16),

(Equation 21) Is required. Note that a relational expression of R ^-1 = ^RT was used. θ _(k) , φ _(k) , φ _{(k )} are R _{31 (k)} ,
It is easily obtained from R32 _(k) and R12 _(k) .

As described above, each parameter is updated, but the output is the respective curvatures of the curve and gradient in the forward Z obtained from (4) ′, that is, the horizontal curvature (ρ) and the vertical curvature (μ), And the behavior of the vehicle, ie, the behavior parameters of the camera (Dx, Dy, θ, φ,
φ). Among the behavior parameters of the camera, the distance Dx from the road edge to the road and the yaw angle θ are important for lane departure warning in automatic driving and preventive safety.

FIG. 6 is a block diagram showing the configuration of a rear-end collision warning device using the above-described road shape measuring device of the present invention. The rear-end collision warning device shown in FIG.
Supply to 00. The road shape measuring device 100 includes a camera 1
The information on the road shape ahead of the vehicle and the position of the host vehicle is supplied to the determination unit 53 from the image information from the vehicle.

The device shown in FIG. 6 detects a vehicle ahead and detects the distance and direction to the vehicle.
The scanning laser radar 51 is used, and the position information of the preceding vehicle detected by the scanning laser radar 51 is supplied to the determination unit 53. The determination unit 53 determines whether the preceding vehicle and the own vehicle are in the same lane based on the road shape and the position information of the own vehicle supplied from the road shape measuring device 100 and the position information of the preceding vehicle supplied from the scanning laser radar 51. When the judgment is made, when the distance between the two vehicles is close to or less than a predetermined value in the same lane, an alarm signal is supplied to the alarm device 55, and an alarm signal is generated by, for example, a buzzer, a chime, a voice, or the like.

It is to be noted that whether or not the preceding vehicle is traveling in the same lane as the own vehicle is determined by assuming that the position of the preceding vehicle with respect to the own vehicle has been obtained as (Xs, Y = 0, Zs). information, (4) B = 0 in the equation, i.e. from Xp = f (Zs) = aZs 4 + bZs 3 + cZs 2 is a road center in the X-coordinate, in the case of Xp + B>Xs> Xp, left lane Xp-B < If Xs <Xp, it can be determined that the vehicle is traveling in the right lane. Position of the vehicle is given by D _x, in the case of D _x> 0, when the left lane D _x <0 is the right lane. Then, when the vehicle is traveling in the same lane and Zs approaches a predetermined distance or less, an alarm may be generated as described above. For example, in the case of Xs <Xp + B (lane width), it can be determined that the obstacle is a roadside obstacle. Accurate judgment can be made even on an S-shaped road.

FIG. 7 is a block diagram showing a configuration of a lane departure warning device using the above-described road shape measuring device of the present invention. The lane departure warning device shown in FIG. 1 supplies an image ahead of the vehicle in the traveling direction captured by the camera 1 to the road shape measuring device 100 of the present invention. The road shape measuring apparatus 100 supplies information about the road shape ahead of the vehicle and the position of the own vehicle to the determination unit 57 from the image information from the camera 1.
Also, a direction change signal is supplied to the determination unit 57 from a turn signal 59 which is a direction indicator of the vehicle. Now, for example, assuming that the vehicle is traveling in the left lane, the vehicle does not exit the turn signal 59 and the distance D
When x becomes larger than B (D _x > B) or becomes smaller than zero (D _x <0), it is considered that the vehicle has deviated from the own lane, and the determination unit 57 sends a warning signal to the warning device 55. And an alarm is generated from the alarm device 55.

Further, the automatic steering will be described. In the present road shape measuring apparatus, the curvature of the position of the own vehicle and the curvature in front of the own vehicle are obtained at the same time, so that the automatic steering for controlling the speed in advance is possible. For example, control can be performed as follows.

| Forward horizontal curvature |> | Current horizontal curvature | → deceleration | Forward horizontal curvature | <| Current horizontal curvature | → Acceleration Forward vertical curvature (gradient)> Current vertical curvature (gradient) → Acceleration (Throttle opening) Vertical curvature in front (gradient) <current vertical curvature (gradient) → deceleration (throttle closed) Also, for example, when the pitch angle φ, the roll angle φ, and the height Dy fluctuate greatly, decelerate. It is also conceivable. It can be said that it is feedback of ride quality.

The steering control includes, for example, (1) a steering angle Φ _k1 = K · ρ proportional to the current curvature, and (2) a left steering Φ _k2 = Φ _k−1 −δ when the yaw angle is positive. (3) Steering right when the yaw angle is negative Φ _k3 = Φ _k-1 + δ (4) Steering right when Dx is greater than a predetermined value Φ _k4 = Φ _k-1 + δ (5) Dx being a predetermined value If it is smaller, steering Φ _k5 = Φ _k-1 −δ may be set to the left, and the steering angle may be set to an average of Φ _{k1 to} Φ _k5 .

In the equation (4) defining the road model in the road coordinate system (X, Y, Z) described in the above embodiment, the road model approximates the range observable in the image, that is, up to about 100 meters ahead. It is assumed that a gentle curvature is used as much as possible.
It is expressed as the following general formula.

[0070]

(Equation 22) Further, the expression (6) may be expressed by the following expression.

[0071]

(Equation 23) Further, the evaluation error expressed by the equation (14) is expressed by a general equation as shown in the following equation, and the coefficients Aω and Bω are as shown in Table 1.

[0072]

(Equation 24)

[Table 1] Equations (16) and (17) are expressed as general equations using the following equations (16-T) and (17-T), respectively.

[0073]

(Equation 25)

(Equation 26)

[0074]

As described above, according to the present invention,
The road shape ahead of the vehicle in the traveling direction is detected from the image captured by the imaging unit, and a three-dimensional road model defined on three-dimensional coordinates is calculated based on the three-dimensional curve parameters.
The three-dimensional road model is converted into image coordinates based on the posture parameters of the imaging means, and the road shape is compared with the coordinate-converted three-dimensional road model to detect a displacement between the two. Since the amount of change in the dimension curve parameter and the amount of change in the posture parameter are estimated and each parameter is updated, it is represented by a relatively simple mathematical formula.
Estimate each parameter quickly and accurately with a small amount of calculation, accurately measure the road shape ahead of the vehicle, and find the road shape with high accuracy even on S-shaped curves and curves with changing curvature. Can be. Further, since the curvature and the road shape at the point of the vehicle are also estimated, the estimation result of the own vehicle position is also highly accurate.

[Brief description of the drawings]

FIG. 1 is a diagram corresponding to claims of the present invention.

FIG. 2 is a block diagram schematically showing an entire configuration of a road shape measuring device according to one embodiment of the present invention.

FIG. 3 is a block diagram illustrating a detailed configuration of an image processing unit used in the road shape measurement device illustrated in FIG. 2;

FIG. 4 is an explanatory diagram illustrating a coordinate system used in the image processing unit illustrated in FIG. 3;

FIG. 5 is an explanatory diagram illustrating line correspondence matching between a road model and a white line in a line correspondence matching unit illustrated in FIG. 3;

FIG. 6 is a block diagram showing a configuration of a rear-end collision warning device using the road shape measurement device of the present invention.

FIG. 7 is a block diagram illustrating a configuration of a lane departure warning device using the road shape measurement device of the present invention.

[Explanation of symbols]

 Reference Signs List 1 camera 3 image processing unit 5 control unit 11 image input unit 13 preprocessing unit 15 road model creation unit 17 coordinate conversion unit 19 line matching unit 21 parameter estimation unit 23 parameter update unit

Claims (1)

(57) [Claims] 1. An image pickup means for picking up an image of a road ahead in the traveling direction of a vehicle, a road shape detection means for detecting a road shape ahead of the vehicle in the traveling direction from an image picked up by the image pickup means, and a three-dimensional curve parameter. Road model calculating means for calculating a three-dimensional road model defined on three-dimensional coordinates, and converting the three-dimensional road model calculated by the road model calculating means into image coordinates based on the attitude parameters of the imaging means. Coordinate conversion means, and a displacement detection means for comparing the road shape detected by the road shape detection means with the three-dimensional road model coordinate-transformed by the coordinate conversion means and detecting a displacement between the two. And estimating the amount of change in the three-dimensional curve parameter and the amount of change in the posture parameter of the imaging means from the positional deviation detected by the positional deviation detecting means. A road shape measuring device comprising: an estimation updating unit that updates data.