JP7429136B2 - Track estimation device and track estimation program - Google Patents

Description

The present invention relates to a running route estimation device and a running route estimation program.

In the route estimation device, it is first necessary to model the route. Conventionally, a method using B-splines has been known as one method for modeling a running route. B-splines, particularly B-splines of degree 3, have been commonly used in the field of CG (Computer Graphics) to express curves and curved surfaces. In recent years, B-splines have been increasingly used in the fields of automated driving and driving support to express road shapes, travel routes, and road alignment on maps.

For example, Non-Patent Document 1 is known as a document disclosing a track model using B-splines. A road model is expressed as a superposition of basis functions of one-dimensional order 3 B-splines (hereinafter sometimes referred to as "cubic B-splines"), and lane boundary data obtained from in-vehicle camera images is used to The model parameters (weights of the B-spline basis functions) are estimated.

H. Loose, U Franke, “B-spline-based road model for 3d recognition,” 13th International IEEE Conference on Intelligent Transportation System, 2010.

By the way, when a one-dimensional cubic B-spline is used as a track model, curves such as S-curves are easy to model and have good estimation accuracy and tracking performance, but estimation accuracy and tracking performance are poor at the connection between curves and straight lines. tends to decrease.

This is because in a one-dimensional cubic B-spline, it is extremely difficult to express a straight line with an angle using one basis function, and it must be approximated by superimposing multiple basis functions. ing. In particular, at connection points where a curve changes to a straight line or a straight line to a curve, the road shape captured by the in-vehicle camera appears to fluctuate greatly, but if the tracking performance of multiple basis functions is not consistent, estimation errors will occur due to tracking delays. occurs, leading to a decline in control and driving support performance.

Referring to FIG. 7, the problems when modeling a running route using cubic B-splines will be explained in more detail. Figure 7(a) shows a running track (hereinafter sometimes referred to as a "compound running track") that has continuous curves and straight roads along the traveling direction of a vehicle (denoted as a "car" in Fig. 7(a)). There is. In FIG. 7, the Z direction is the traveling direction of the vehicle. As mentioned above, it is extremely difficult to model a composite track using a track model using a cubic B-spline, and the estimation error is particularly large in a far distance (for example, an area about 100 to 200 meters in front of the vehicle). increase is inevitable.

FIG. 7(b) shows the arrangement of basis functions when modeling a compound travel path that continues from a curve to a straight line using a cubic B-spline. In the example shown in FIG. 7(b), modeling of the composite trajectory is attempted by adding weighted basis functions 1 and 2 of cubic B-splines. However, in the weighted addition of the basis functions of cubic B-splines, the connection part between a curve and a straight line, and the straight part are expressed by the sum of curves, so modeling along the travel path is not possible, and large An estimation error has occurred.

As described above, in recent travel route estimation devices, when a certain B-spline is assumed, a composite route in which a curved road and a straight road are combined can be expressed with one basis function, and multiple basis functions can be combined. Therefore, there was a need for a route estimation device that can accurately represent curved roads and straight roads with various curvatures and line segment lengths.

The present invention has been made in view of the above problems, and improves the estimation accuracy when estimating track parameters for tracks including various shapes, compared to the case where a track model using a cubic B-spline is used. The object of the present invention is to provide a running route estimation device and a running route estimation program with improved tracking performance.

In order to achieve the above object, the running route estimation device according to claim 1 includes: an imaging unit that is mounted on a moving body and captures a running route image that is an image of the running route of the moving body; a detection unit that detects a running state that is a state associated with running; an extraction unit that extracts boundary information corresponding to the boundary of the running track based on the running track image; and double integration of each of a plurality of predetermined functions. a storage unit that stores a function group that is a set of; a generation unit that performs predetermined processing on the function group based on the boundary information to generate a track model of a track including a straight road and a curved road; The estimation unit includes a driving state and an estimating unit that estimates running route parameters of the running route using the running route model.

The invention according to claim 2 is the invention according to claim 1, in which the plurality of predetermined functions are basis functions of a B-spline of degree 0 or basis functions of a B-spline of degree 1. It is something that is.

The invention according to claim 3 is the invention according to claim 1 or 2, wherein the predetermined process converts a weighted linear sum of each function of the function group into a predetermined model formula. This is a process to incorporate.

Further, the invention according to claim 4 is the invention according to any one of claims 1 to 3, in which the generating unit is configured to form a connecting portion between the straight path and the curved path into an arc shape or a clothoid shape. It is approximated by a curved shape.

Moreover, the invention according to claim 5 is the invention according to any one of claims 1 to 4, in which the traveling state is the speed and yaw rate of the moving body.

Further, the invention according to claim 6 is the invention according to any one of claims 1 to 5, in which the traveling path parameters include a lateral position of the moving body in a direction intersecting the traveling path, a yaw angle , pitch angle, and the shape of the running path.

In order to achieve the above object, the running route estimation program according to claim 7 includes an imaging unit that is mounted on a moving body and captures a running route image that is an image of the running track on which the moving body runs; A track estimation program using a track estimation device including a detection unit that detects a running state that is a state associated with running, the program causing a computer to extract boundary information corresponding to a boundary of the track based on the track image. a storage means for storing a function group that is a set of double integrals of each of a plurality of predetermined functions; and a generating means for generating a running route model of a running route including a curved road, and an estimating means for estimating running route parameters of the running route using the running condition and the running route model.

According to the present invention, estimation accuracy and tracking performance are improved when estimating track parameters for tracks including various shapes, compared to the case where a track model using a cubic B-spline is used. The present invention has the advantage of being able to provide a running route estimation device and a running route estimation program.

FIG. 1 is a block diagram showing an example of the configuration of a running route estimating device according to an embodiment. FIG. 2 is a diagram illustrating a track model according to an embodiment. (a) and (b) are diagrams illustrating modeling of a running route using a zero-order B-spline, and (c) and (d) are diagrams explaining modeling of a running route using a linear B-spline. (a) to (d) are diagrams illustrating arrays of basis functions that are fitted to the shapes of various travel paths, taking a linear B-spline as an example, and (e) is a diagram illustrating other forms of basis functions. (a) is a diagram explaining the effect of modeling a running route using a linear B-spline, and (b) is a diagram illustrating the estimation error of the running route estimation device according to the present embodiment using a cubic function and a cubic B-spline. This is a graph comparing the estimation error when It is a flowchart which shows the flow of processing of the course estimation program which the course estimation device concerning an embodiment executes. (a) is a diagram showing an example of a compound course in which a curve and a straight line are combined, and (b) is a diagram illustrating an estimation error when the course is modeled using a cubic B-spline.

Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings. In the following description, a form in which the running route estimation device and the running route estimation program according to the present invention are applied to a running route estimation device and a running route estimation program that are mounted on a vehicle and estimate a running route in an area including a driving lane will be exemplified. I will explain. The driving route estimation device and the driving route estimation program according to the present embodiment are capable of extracting boundary information such as white line lane marks from images taken by a monocular camera mounted on a vehicle, and accurately representing driving lanes over long distances as a model. configured so that it is possible. In addition, it is possible to reduce modeling errors, especially at the junction between a curved road and a straight road, resulting in route estimation that can accurately represent S-curves as well as junctions between curved roads and straight roads. A device and a route estimation program have been realized. Here, "far away" in this embodiment is assumed to be approximately 100 to 200 meters away. Note that in the following description, italicized characters in mathematical formulas may be represented by non-italicized characters.

As shown in FIG. 1, the driving route estimation device 10 according to the present embodiment includes a lane boundary feature extraction section 11, a parameter estimation section 12, a function storage section 13, a camera 14, a vehicle speed sensor 15, and a yaw rate sensor 16. It is configured.

The camera 14 is, for example, an imaging device that is disposed facing forward at a rear view mirror in the vehicle interior and captures an image of a road in front of the vehicle (hereinafter sometimes referred to as a "road image"). In this embodiment, it is assumed that the height and depression angle of the camera 14 with respect to the horizontal road surface are set to known values in advance. As shown in FIG. 1, the image captured by the camera 14 is output to the lane boundary feature extraction section 11. Note that although a monocular camera is assumed in this embodiment, the present invention is not limited to this, and a stereo camera may also be used.

The vehicle speed sensor 15 is a part that measures the speed of the own vehicle relative to the road surface, and the yaw rate sensor 16 is a part that measures the yaw rate of the own vehicle. The speed measured by the vehicle speed sensor 15 and the yaw rate measured by the yaw rate sensor 16 are output to the parameter estimation unit 12 via, for example, an in-vehicle CAN (Controller Area Network) (not shown). Note that the "vehicle speed sensor 15" and the "yaw rate sensor 16" are examples of the "detection unit" according to the present invention. Further, "vehicle speed" and "yaw rate" are examples of "driving state" according to the present invention.

Based on the road image captured by the camera 14, the lane boundary feature extraction unit 11 extracts observation points (hereinafter referred to as Obtain the coordinate values (camera coordinate values) (ix _obs , iy _obs ) of the camera image (sometimes referred to as "white line point"). Here, the lane boundary is extracted by an edge extraction filter such as a Sobel filter, assuming that there is a brightness difference on the image at the boundary between the road surface and the lane boundary. Furthermore, within a small area in the image, the lane boundary is assumed to be a straight line, and a white line corresponding to the straight line is selected by Hough transform or the like. Alternatively, lane boundaries may be extracted by learning lane boundary features through deep learning.

The function storage unit 13 stores functions related to the B-spline used in the parameter estimating unit 12 (basis function b _i , one-time integration of basis function b _i , and two-time integration g _i of basis function b _i ). There is. The double integral g _i of the basis function b _i is expressed by (Equation 1) shown below.

As will be described later, each subscript i (corresponding basis function number) is specified corresponding to which section on the Z-axis each lane boundary point falls.

The parameter estimating unit 12 uses the camera coordinate values of the left and right lane boundary points as observed values, and uses the least squares method, RANSAC (Random Sample Consensus), extended Kalman filter, particle filter, etc. to calculate (Equation 2) and ( The parameter values of the track model expressed by Equation 3) are determined. At this time, the Z coordinate value of the lane boundary point is calculated based on the assumption of a flat road surface or the assumption of constant lane width. Alternatively, it may be calculated from parallax based on stereoscopic vision using a stereo camera.


At this time,
G(Z): Planar linearity (travel shape) expressed as the linear sum of the two-time integral g _i of the B-spline,
X ₀ : Offset of lane center at Z=0,
θ ₀ : Yaw angle at Z=0,
w: lane width,
θα: The tangent slope of the plane linearity of the running path at Z=Z _j , that is,

c _i : Weight of the basis function of the i-th B-spline, that is, a parameter representing the trajectory shape;
n: Number of basis functions of B-spline.

Regarding the sign of ± in (Equation 2), a positive sign is used in the case of a right lane boundary, and a negative sign is used in the case of a left lane boundary. B-splines used in CG are generally vectors, but in this embodiment, in consideration of the ease of processing inverse perspective transformation of camera images, we use one-dimensional B-splines with X as the domain and Z as the range. Considering it as a function, treat c _i as a scalar value representing a weight. Here, "reverse perspective transformation" refers to transformation from an image in camera coordinates to an image in world coordinates. On the other hand, "perspective transformation" refers to transformation from an image in world coordinates to an image in camera coordinates.

FIG. 2 is a running plan view of a running path showing the correspondence with the above (Formula 2) and (Formula 3). As shown in FIG. 2, the running route model according to the present embodiment expresses the running route as a curved line on a two-dimensional orthogonal coordinate system XZ with the camera position as the origin. At this time, the Z axis indicates the optical axis direction of the camera (vehicle traveling direction), and the X axis indicates the lateral position direction. In the coordinate system shown in FIG. 2, the direction of gravity is the Y axis. It is also assumed that the height of the camera 14 from the road surface is known. In Fig. 2, w represents the width of the road (lane width), X ₀ represents the offset of the vehicle center at Z=0, θ ₀ represents the yaw angle of the vehicle at Z = 0, and θα represents the coordinates of the road (X _j , Z _j ). The term 1/cosθα represents the apparent variation in lane width due to the tangent angle. Further, G(Z) represents a plane linear shape (travel shape), and is expressed as a linear sum of two-time integrals g _i of the basis functions of the B-spline weighted by a weight c _i . Here, the "plane linear" of the basis function in this embodiment refers to a transition part and a straight line part where there is a transition from a certain straight line to a straight line with a different slope. Further, the yaw angle of the vehicle refers to the angle of the moving body in plan view with respect to a predetermined reference. The rate at which the yaw angle changes is the yaw rate. On the other hand, "pitch angle", which will be described later, refers to the angle of the moving body in a cross-sectional view with respect to a predetermined reference.

Referring again to FIG. 1, the alarm/control unit 20 issues an alarm or controls a predetermined target based on the parameters c _i , X ₀ , and θ ₀ estimated by the parameter estimation unit 12 .

As mentioned above, especially when cubic B-splines are applied to the track model, curves such as S-curves are easy to express and the estimation accuracy and tracking performance are good, but the estimation accuracy is poor at the connection between the curve and the straight line. There is a tendency for tracking performance to deteriorate. This is because it is extremely difficult to express a straight line with an angle using one basis function of a cubic B-spline, and it must be approximated by superimposing a plurality of basis functions.

Therefore, in the present invention, a change in the slope (curvature) of the plane linearity of the running track is achieved using a B-spline of degree 0 or degree 1 (hereinafter sometimes referred to as "0th order B-spline" or "1st order B-spline", respectively). If it is small, the approximate value of curvature) is expressed, and the plane linearity (travel shape, offset component) is expressed by its double integration. As a result, a straight line section (change in slope is 0) at any point can be expressed with one basis function, which improves estimation accuracy and tracking performance compared to the case of using conventional cubic B-splines. It has become possible to improve the

Next, referring to FIG. 3, the B-spline employed in this embodiment will be described in more detail. FIGS. 3(a) and 3(b) are diagrams for explaining a zero-order B-spline, and FIGS. 3(c) and (d) are diagrams for explaining a first-order B-spline. 3(a) and 3(c) show the arrangement of basis functions of each B-spline, and FIGS. 3(b) and 3(d) show each basis function b _i and its one-time integration and two-time integration. It shows the relationship between g _i .

FIG. 3(a) shows basis functions of a zero-order B-spline. As shown in FIG. 3(a), the shape of the basis functions of the zero-order B-spline is rectangular, and FIG. 3(a) shows six basis functions b ₀ to b ₅ as examples. Using this rectangular basis function group, a pattern of changes in slope (approximately curvature component) of the set section of the travel route on the Z-axis is expressed. That is, each subscript i (corresponding basis function number) is specified in accordance with which section on the Z axis each lane boundary point falls.

FIG. 3(b) shows the shape of the basis function of the zero-order B-spline (<1>), the shape of one-time integration (<2>), and the shape of two-time integration. The basis function expresses the change in slope, the single integral expresses the slope (yaw angle), and the double integral expresses the plane linearity (trajectory shape).
As shown in Figure 3(b), in the case of a zero-order B-spline, the plane linearity of the basis function representing the trajectory is a quadratic curve (<3>), and its slope is a straight line (<2>). . In the case of the example in FIG. 3(b), as shown in FIG. 3(b)<3>, the planar shape, that is, the runway shape, is divided into three regions I, II, and III, where region I is a straight line, and region II is a straight line. is a quadratic curve, and region III is a straight line. The vertical axis shown in Figure 3(b) exemplifies the case where the horizontal linear vertical axis is expressed by X (<3>), so the vertical axis of the slope is the first partial differential of X with respect to Z. (<2>), and the vertical axis of the change in slope is expressed by the second partial differential of X with respect to Z (<1>).

On the other hand, FIG. 3(c) shows the basis functions of a linear B-spline. As shown in FIG. 3(b), the shape of the basis functions of the linear B-spline is triangular, and FIG. 3(b) illustrates six basis functions b ₀ to b ₅ . This triangle is used to express a pattern of changes in inclination (approximately curvature component) of the set section of the running route on the Z-axis. That is, each subscript i (corresponding basis function number) is specified in accordance with which section on the Z axis each lane boundary point falls.

FIG. 3(d) shows the shape of the basis function of the linear B-spline (<1>), the shape of one-time integration (<2>), and the shape of two-time integration. The basis function expresses the change in slope, the single integral expresses the slope (yaw angle), and the double integral expresses the plane linearity (trajectory shape).
As shown in Fig. 3(d), in the case of a linear B-spline, the plane linearity of the basis function representing the trajectory becomes two cubic curves (<3>), and the slope thereof becomes two quadratic curves. It becomes (<2>).
In the case of the example in FIG. 3(d), as shown in FIG. 3(d) <3>, the planar shape, that is, the runway shape, is divided into four regions I, II, III, and IV, and region I is a straight line. , regions II and III are cubic curves, and region IV is a straight line. The vertical axis shown in Figure 3(d) exemplifies the case where the horizontal linear vertical axis is expressed by X (<3>), so the vertical axis of the slope is the first partial differential of X with respect to Z. (<2>), and the vertical axis of the change in slope is expressed by the second partial differential of X with respect to Z (<1>).

FIGS. 4(a) to 4(d) show examples of basis function weighting when modeling each plane line (travel shape) using a linear B-spline as an example. Figure 4(a) shows the weight of a circular arc, Figure 4(b) shows the weight of a clothoid curve, Figure 4(c) shows the weight of a connection from a straight line to a circular arc, and Figure 4(d) shows the weight of a connection from a circular arc to a straight line. The arrangement of basis functions c _i b _i (i=0 to 5) is shown. In this way, the route estimating device 10 according to the present embodiment can express a curved road and a straight road connected thereto using one basis function.

FIG. 4(e) shows an example of a basis function having a shape other than a rectangle or a triangle. FIG. 4E shows an example of a trapezoidal basis function. According to the trapezoidal basis function as shown in FIG. 4(e), there is a possibility that the estimation accuracy of the stationary curve section can be improved. It is based on the idea that a steady curve reflects the fact that the curvature is constant over a certain period, and also represents a clothoid curve section. Thus, in this embodiment, the shape of the basis functions is not limited to rectangular or triangular shapes.

As described above, each function shown in FIG. 3 is stored in advance in the function storage section 13, and is read out and used by the parameter estimating section 12 as needed.

Next, with reference to FIG. 5, the effects of the running route estimating device 10 according to the present embodiment will be described in more detail. FIG. 5(a) is an example of modeling in which a composite track having a curved road and a straight road is expressed linearly (road shape) using double integration of a basis function of a linear B-spline.
As shown in FIG. 5(a), by weighting and adding the twice integrals of basis functions, it is possible to accurately and easily model a composite trajectory.

FIG. 5(b) shows the estimation error (m) at 180 m ahead when modeling using a cubic function, when modeling using a cubic B-spline, and when modeling according to this embodiment. shows the simulation results. In FIG. 5(b), the vertical axis represents the estimation error (m) 180 m ahead, and the horizontal axis represents the radius of curvature, showing the estimation errors when the radius of curvature is 1000 m, 500 m, 200 m, and 100 m. As is clear with reference to FIG. 5(b), the estimation error when modeling using a cubic function is much larger than when using other functions. Although the modeling according to this embodiment is generally comparable to modeling using cubic B-splines, it has an advantage over cubic B-splines in areas where the radius of curvature of the running track is small, where estimation errors are generally considered to increase. confirmed. This is because the route estimation device 10 according to the present embodiment can express a curved road and a straight road connected to it using one basis function, and can express various curvatures and line segment lengths by combining multiple one basis functions. This is because curved roads and straight roads can be expressed with high accuracy. Furthermore, since the route estimating device 10 according to the present embodiment uses basis functions having relatively simple shapes such as rectangular or triangular shapes, it has the advantage that the calculation load can be reduced compared to cubic B-splines.

Next, the estimation of track parameters using a track model defined by a B-spline, which is executed by the parameter estimation unit 12, will be described in more detail.

As described above, the parameter estimation unit 12 uses the camera coordinate values (ix _obs , iy _obs ) of the left and right lane boundary points as observed values to estimate the parameters of the road model using the least squares method, RANSAC, extended Kalman filter, particle filter, etc. Calculate the data value (travel parameter). At this time, the coordinate transformation from the camera coordinate values (ix _obs , iy _obs ) to the corresponding world coordinate values (X, Z) can be performed by assuming that the road surface is flat (flat road surface assumption) or by keeping the lane width constant. (assuming constant lane width). Note that the traveling route parameters according to this embodiment include pitch angle, yaw angle (corresponding to the inclination shown in FIG. 3(b) <2> and FIG. 3(d) <2>), and the lateral position of the moving object (traveling Changes in the relative position of the vehicle and lanes, etc. (also called "offset"), horizontal alignment (road shape, see Figure 3 (b) <3> and Figure 3 (d) <3>), lane width, and slope (See FIG. 3(b) <1> and FIG. 3(d) <1>). The road parameters estimated by the parameter estimating section 12 are output to the warning/control section 20 as described above, and are used for issuing a predetermined warning or controlling a predetermined vehicle.

By assuming a flat road surface and perspective transformation, the j-th point (X _j , Y _j ) on the track model (world coordinates) is transferred to the point (ix _j , iy _j ) on the camera image (camera coordinates) as shown below. It is projected using (Equation 4) and (Equation 5).

When the coordinate values (ix _obsj , iy _obsj ) of m white line points from the left and right white lines of the lane are obtained as observed values based on the road image captured by an on-vehicle monocular camera, the observed value vector of the Kalman filter is It is expressed by (Equation 6) shown below.



Using the inverse perspective transformation and the assumption of a flat road surface, the distance Z _j of the white line point in the traveling direction is calculated from iy _obs using (Equation 7) shown below.

Based on (Formula 2) and (Formula 4), calculate the predicted value of ix from Z _j calculated using (Formula 7), and from the difference between the predicted value and the observed value, use a known Kalman filter. Update the estimate vector x.

From the above, the estimated value vector of the Kalman filter is expressed by (Equation 8) shown below. In (Formula 8), the symbol T indicates a transposition symbol.

In addition, if a more accurate value than an approximate value is required as the curvature c _d in front of Z (m) for control or alarm purposes, use the following (Equation 9) based on known geometry knowledge. It may be calculated.

Next, with reference to FIG. 6, the route estimation process executed in the route estimation device 10 according to the present embodiment will be described. FIG. 6 is a flowchart showing the process flow of the route estimation program executed in the route estimation process. The main course estimation program is stored in a storage means such as a ROM (not shown) of the course estimating device 10, and when an instruction to start execution is received via a user interface etc., the CPU (not shown) starts the main course estimation program from a storage means such as a ROM. Load the program, expand it to RAM, etc., and execute it.

In step S100, a track image is acquired from the camera 14. The acquired road image is sent to the lane boundary feature extraction unit 11.

In step S101, sensor information is acquired. That is, the vehicle speed is acquired from the vehicle speed sensor 15 and the yaw rate is acquired from the yaw rate sensor 16. The acquired vehicle speed and yaw rate are sent to the parameter estimation section 12.

In step S102, lane boundary candidates (white line points) are extracted from the road image acquired in step S100. That is, the lane boundary feature extraction unit 11 is controlled to extract lane boundary candidates.

In step S103, an appropriate basis function b _i and a double integral of the basis function b _i are read from the function storage unit 13 in consideration of the white line points extracted in step S102. The double integral of the read basis functions is incorporated into the model equations shown in (Equation 2) and (Equation 3) to generate a track model.

At step S104, the route parameters are updated using the route model generated at step S103. That is, the parameter estimation unit 12 is controlled to receive the vehicle speed, yaw rate, and lane boundary candidates as input, and to update the road parameters using a Kalman filter or the like.

In step S105, the running route parameters are output. That is, the parameter estimation unit 12 is controlled to output the updated running route parameters to the warning/control unit 20.

In step S106, it is determined whether there is an instruction to end the process. If the determination is negative, the process returns to step S100 to continue the route estimation process. On the other hand, if the determination is affirmative, the main running route estimation program is ended. The determination as to whether an instruction to end has been given may be made, for example, by detecting whether or not the user has turned off the power to the route estimating device 10 .

The operation of the route estimating device 10 according to the present embodiment described above can be summarized as follows. That is, the running route estimating device 10 receives as input a running route image obtained by the camera 14, a vehicle speed obtained by the vehicle speed sensor 15, a yaw rate obtained by the yaw rate sensor 16, and the like. Then, the lane boundary feature extraction unit 11 obtains white line points (lane boundary points) based on the road image. Further, the parameter estimating section 12 reads an appropriate function from the function storage section 13 in consideration of the white line point. Subsequently, the parameter estimating unit 12 generates a running track model by incorporating the read function into the running track model formula ((Formula 2), (Formula 3)), and uses the generated running track model, vehicle speed, and yaw rate, Outputs road parameters, ie, lateral position (offset), yaw angle, pitch angle, lane width, etc. of the own vehicle.

As described in detail above, according to the running route estimation device and the running route estimation program according to the present embodiment, data from a camera image and an on-vehicle external sensor such as a lidar (LiDAR) are input to calculate the running route shape and the relationship between the running route and the vehicle. When estimating a relative position and orientation, it is possible to provide a running route estimation device and a running route estimation program that have improved estimation accuracy and tracking performance, particularly at the connection between a curved road and a straight road.

In addition, although the above embodiment has been described by exemplifying a mode in which white line lane marks (white line points) on both sides of the road are observed, the present invention is not limited to this, and may also be applied to a mode in which white line lane marks on one side are observed. good. In the case of observing a white lane mark on one side, the lane width w may not be used as a model parameter, or the lane width w may be a fixed value. In addition, in the case of observing the white lane mark on one side, the height and depth of the road surface are calculated based on SFM (Structure From Motion) of the feature points on the road surface, and the distance information is used to estimate the driving route. Good too. On the other hand, in the case of observing the white line lane marks on both the left and right sides, the depth distance to the white line may be calculated based on the assumption of a local road surface plane and the assumption of constant lane width.

10 Route estimation device 11 Lane boundary feature extraction unit 12 Parameter estimation unit 13 Function storage unit 14 Camera 15 Vehicle speed sensor 16 Yaw rate sensor 20 Warning/control unit

Claims (6)

an imaging unit that is mounted on a moving body and captures a track image that is an image of a track on which the moving body runs;
a detection unit that detects a running state that is a state accompanying the running of the moving body;
an extraction unit that extracts boundary information corresponding to the boundary of the runway based on the runway image;
a storage unit that stores a function group that is a set of double integrals of each of a plurality of predetermined functions;
a generation unit that performs predetermined processing on the function group based on the boundary information to generate a track model of a track including a straight road and a curved road;
an estimation unit that estimates a running route parameter of the running route using the running state and the running route model ,
The plurality of predetermined functions are basis functions of a B-spline of degree 0 or basis functions of a B-spline of degree 1,
Route estimation device. The route estimation device according to claim 1 , wherein the predetermined process is a process of incorporating a weighted linear sum of each function of the function group into a predetermined model formula. The travel route estimation device according to claim 1 or 2 , wherein the generation unit approximates a connecting portion between the straight road and the curved road with a circular arc shape or a clothoid curve shape. The running route estimating device according to any one of claims 1 to 3 , wherein the running state is a speed and a yaw rate of the moving object. The running route parameter is at least one of a lateral position of the moving body in a direction intersecting the running route, a yaw angle, a pitch angle, and a shape of the running route. route estimation device. A route estimation device is used that is mounted on a moving body and includes an imaging unit that captures a route image, which is an image of the route traveled by the moving body, and a detection unit that detects a running state that is a state accompanying the travel of the moving body. It is a running route estimation program,
computer,
Extracting means for extracting boundary information corresponding to the boundary of the running track based on the running track image;
storage means for storing a function group that is a set of double integrals of each of a plurality of predetermined functions;
generating means for generating a track model of a track including a straight road and a curved road by subjecting the function group to predetermined processing based on the boundary information;
Estimating means for estimating running route parameters of the running route using the running state and the running route model;
function as
The plurality of predetermined functions are basis functions of a B-spline of degree 0 or basis functions of a B-spline of degree 1,
Track estimation program.