JP2016168905A - Road shape estimation unit and road shape estimation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To estimate a shape of a route in the extending direction of a road from among a group of observation data items obtained using sensing equipment mounted in a moving entity such as a vehicle.SOLUTION: A filter for use in route estimation is disposed in a virtual space in which a group of observation data items obtained by sensing road ends using sensors mounted in a moving entity is distributed. The filter is moved while being allowed to read the group of observation data items concerning the vicinity of a forward area. Positions represented by the group of observation data items read by the filter are regarded as indicating road ends. For example, an advancing direction of the filter is calculated so that the filter will pass positions separated by a half of a road width from each of the road ends. Moving trajectories of the filter are joined in order to estimate the shape of a route in the extending direction of the road.SELECTED DRAWING: Figure 21

Description

  The present invention relates to a technique for estimating a road shape from observation data groups obtained using a sensing device (laser, radar, camera, etc.) mounted on a moving body such as a vehicle.

  As conventional road shape estimation techniques, for example, there are techniques disclosed in Non-Patent Document 1 and Non-Patent Document 2 below. Non-Patent Document 1 and Non-Patent Document 2 disclose methods for estimating a road shape based on a sensed road edge observation data group.

  When a road is sensed by a sensing device, an observation data group including position information of objects (curbs, guardrails, walls, etc.) existing on the left and right sides of the road is obtained. The observation data group is obtained as a set of partially broken lines and points along the road extending direction (the traveling direction of the vehicle traveling on the road). In particular, FIG. 54A shows a state in which sensing is performed as a set of points. In FIG. 54A, the coordinates (observation points) of the sensed object are represented by the symbol “+”. In many cases, these are sensed from the vicinity of the vehicle to a distance (50 m to 150 m ahead) by a sensing device such as a camera, laser, or radar installed in the vehicle. In this specification, data obtained by sensing with a sensing device is referred to as observation data, a plurality of observation data is referred to as an observation data group, a sensed object and its position are referred to as observation points.

  In order to estimate the road shape, the partial observation data obtained must be connected and restored as lines on the left and right sides of the road. An example of the restored result is shown in FIG.

  In the conventional road shape estimation technology disclosed in Non-Patent Document 1 and Non-Patent Document 2, it is known which observation data belongs to the left end or the right end of the road among the observation data group at the road end. Only one of the sets is selected and fitted to a predetermined curve, and the shape of the road edge on one side is estimated. A clothoid curve or a polynomial is used as the curve to be fitted.

  Further, in Patent Document 1 below, in a system for estimating the shape of a road by applying a predetermined curve (a clothoid curve or a curve having a constant curvature radius) to an observation data group sensed by a sensor, A technique for acquiring a white line image and estimating a curvature radius of a road based on the acquired white line image is disclosed.

Republished patent WO2012 / 137354 (Claim 7)

IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, No.2 Feb 1992, “Recursive 3-D road and relative ego-state recognition”, Dickmanns, E.D. and Mysliwetz, B.D Sensors 2013, 13, 3270-3298; “Robust Lane Sensing and Departure Warning under Shadows and Occlusions” Rodolfo Tapia-Espinoza and Miguel Torres-Torriti

  As described above, in the disclosed techniques of Non-Patent Document 1 and Non-Patent Document 2, it is known whether the sensed observation data of the road edge belongs to the left edge or the right edge of the road. One of the road edge shapes can be estimated using either set of observation data. However, there may be a case where it is not known whether the observed observation data of the road edge belongs to the left edge or the right edge of the road.

  For example, if the sensed observation point is far away from the sensor (for example, a long distance of 100 m or more), whether the sensed observation data at the road edge is the observation data on the left side of the road or the observation data on the right side. It may not be easy to distinguish between the two. The situation is shown in FIG.

  Of the observation data group at the edge of the sensed road, it is possible to accurately estimate whether the observation data at the observation point existing at a short distance from the sensor belongs to the left end or the right end of the road. For example, the observation point A shown in FIG. 54C is detected from the left hand of the sensor (left side with respect to the traveling direction of the vehicle) and can be estimated as the one at the left end of the road.

  However, among the observation data group at the road edge, the observation data of the observation point that is located at a long distance from the sensor is affected by the shape of the entire road, and it is accurately determined whether it belongs to the left edge or the right edge of the road. It may not be possible to estimate. For example, the observation point B shown in FIG. 54C is detected from the right hand of the sensor (on the right side with respect to the traveling direction of the vehicle) because the entire road turns to the right. As a result, although the observation point B actually exists at the left end of the road, there is a possibility of misunderstanding that it is an observation point located at the right end of the road. Such misrecognition becomes more problematic as the observation point is farther from the sensor or the observation data contains more noise. In the present specification, the problem relating to the above-mentioned misrecognition is referred to as “observation data left / right determination problem”.

  According to the disclosed technique of Patent Document 1, it is possible to estimate the shape of a road at a long distance (curvature of a road) from a sensor using a white line image of the road in addition to the sensed observation data group. As a result, if the shape of the road can be estimated accurately, it is accurately estimated whether the sensed observation data belongs to the left end or the right end of the road. May be resolved. However, for example, when the white line of the road can only be detected as a partial point, the shape of the road cannot be accurately estimated, and the “observation data left / right determination problem” still cannot be solved.

  In consideration of the above problems, the present invention estimates the road shape more accurately by accurately estimating whether the sensed observation data belongs to the left end or the right end of the road. An object is to provide a shape estimation device and a road shape estimation method.

In order to achieve the above object, according to the present invention, there is provided a road shape estimation device for estimating the shape of a road using observation data groups sensed by a sensing device installed on a moving body traveling on the road. ,
An observation data group acquisition unit for acquiring an observation data group existing within a predetermined range in the sensing;
A process of arranging a filter having position coordinates and a traveling direction as parameters in a virtual space in which the observation data group is distributed, and calculating the traveling direction of the filter using the observation data group acquired by the observation data group acquisition unit A direction calculator,
A filter moving unit that moves the filter in the virtual space along the traveling direction of the filter calculated by the traveling direction calculating unit;
A road shape output unit that outputs the movement trajectory of the filter moved by the filter moving unit as a route along the extending direction of the road;
The traveling direction calculator is
A selection unit that selects the observation data existing within a predetermined range with reference to the current position of the filter;
A projection point calculator for calculating a projection point moved in a direction perpendicular to the current traveling direction of the filter by a predetermined distance from the position of the selected observation data;
A vector direction calculation unit for calculating a direction of a vector connecting the current position of the filter and the projection point;
A travel direction setting unit configured to set a new travel direction of the filter using the calculated vector direction;
The filter is moved by a predetermined moving distance toward a new traveling direction of the filter, and updated so that the current coordinates of the filter are set as the position of the filter after the movement, and the current traveling of the filter An update processing unit so that the direction is set as a new traveling direction of the filter,
A road shape estimation apparatus is provided.

In order to achieve the above object, according to the present invention, there is provided a road shape estimation method for estimating the shape of the road using observation data groups sensed by a sensing device installed on a moving body traveling on the road. ,
An observation data group acquisition step of acquiring an observation data group existing within a predetermined range in the sensing;
A process of arranging a filter having position coordinates and a traveling direction as parameters in a virtual space in which the observation data group is distributed, and calculating the traveling direction of the filter using the observation data group acquired in the observation data group acquisition step. A direction calculation step;
A filter moving step for moving the filter in the virtual space along the traveling direction of the filter calculated in the traveling direction calculating step;
A road shape output step for outputting the movement trajectory of the filter moved in the filter movement step as a route along the extending direction of the road;
The traveling direction calculation step includes:
A selection step of selecting the observation data existing within a predetermined range with reference to the current position of the filter;
A projection point calculating step of calculating a projection point moved in a direction perpendicular to the current traveling direction of the filter by a predetermined distance from the position of the selected observation data;
A vector direction calculation step of calculating a direction of a vector connecting the current position of the filter and the projection point;
A traveling direction setting step of setting a new traveling direction of the filter using the calculated vector direction;
The filter is moved by a predetermined moving distance toward a new traveling direction of the filter, and updated so that the current coordinates of the filter are set as the position of the filter after the movement, and the current traveling of the filter An update process step so that the direction is set as the new direction of travel of the filter,
A road shape estimation method is provided.

  The road shape estimation device and the road shape estimation method of the present invention have an effect that the road shape can be estimated more accurately. In particular, the shape of a route (for example, a route passing through the center of the road) along the road extending direction can be estimated as the road shape. Furthermore, it is possible to estimate a road shape having a road width by estimating the road width.

In this invention, it is a figure which shows the filter (sweeper) for scanning the space where the observation data group is distributed. It is a flowchart which shows an example of the basic process in the 1st Embodiment of this invention. In the 1st Embodiment of this invention, it is a figure which shows an example of the reading range of an observation data group. It is a flowchart which shows an example of the sweeper calculation example 1 in the 1st Embodiment of this invention. In the 1st Embodiment of this invention, it is a figure which shows an example of the process which concerns on the observation point A by the sweeper calculation example 1. FIG. In the 1st Embodiment of this invention, it is a figure which shows an example of the process which concerns on the observation point B by the sweeper calculation example 1. FIG. In the 1st Embodiment of this invention, it is a figure which shows an example of the process which concerns on the observation point C by the sweeper calculation example 1. FIG. FIG. 7 is a diagram illustrating an example of a movement locus of a sweeper obtained by a sweeper calculation example 1 in the first embodiment of the present invention. It is a flowchart which shows an example of the sweeper calculation example 2 in the 1st Embodiment of this invention. In the 1st Embodiment of this invention, it is a figure which shows an example of the process which concerns on the observation points A, B, and C by the sweeper calculation example 2. FIG. In the 1st Embodiment of this invention, it is a figure which shows an example of the advancing direction of the sweeper obtained by the sweeper calculation example 2. FIG. In the 1st Embodiment of this invention, it is a figure which shows an example of the movement locus | trajectory of the sweeper obtained by the sweeper calculation example 2. FIG. It is a flowchart which shows an example of the sweeper calculation example 3 in the 1st Embodiment of this invention. In the 1st Embodiment of this invention, it is a figure which shows the example of the process (before sweeper rotation) concerning the observation point A by the sweeper calculation example 3 In the 1st Embodiment of this invention, it is a figure which shows the example of the process (after sweeper rotation) which concerns on the observation point A by the sweeper calculation example 3. FIG. In the 1st Embodiment of this invention, it is a figure which shows the example of the process (before sweeper rotation) concerning the observation point B by the sweeper calculation example 3 In the 1st Embodiment of this invention, it is a figure which shows the example of the process (after sweeper rotation) which concerns on the observation point B by the sweeper calculation example 3. FIG. In the 1st Embodiment of this invention, it is a figure which shows the example of the process (before sweeper rotation) regarding the observation point C by the sweeper calculation example 3. In the 1st Embodiment of this invention, it is a figure which shows an example of the advancing direction obtained by the sweeper calculation example 3, and the position after a movement. (A) is a figure which shows an example of the state in which the initial position of a sweeper is not settled in 1st Embodiment of this invention, (b) is a figure of sweeper in 1st Embodiment of this invention. It is a figure which shows an example of the method of determining an initial position. It is a block diagram which shows an example of a structure of the road shape estimation apparatus in the 1st Embodiment of this invention. In the 1st Embodiment of this invention, it is a figure which shows an example of the state in which an observation point exists right above a true road edge. In the 1st Embodiment of this invention, it is a figure which shows an example of the projection point obtained from the observation point which exists just above a true road edge. In the 1st Embodiment of this invention, it is a figure which shows an example of the movement locus | trajectory of a sweeper in case an observation point exists right above a true road edge. In the first embodiment of the present invention, it is an example of a graph plotting the traveling direction of the sweeper when the observation point is present directly above the true road edge. In the 1st Embodiment of this invention, it is a figure which shows an example in the state in which the observation point does not exist right above a true road edge. It is a figure which shows that an error is contained in the vector calculated in the state of FIG. In the 1st Embodiment of this invention, it is an example of the graph which plotted the advancing direction of the sweeper when the observation point does not exist directly above the true road edge. It is an example of the calculation formula of the Kalman filter used in the 2nd Embodiment of this invention. In the 2nd Embodiment of this invention, it is an example of the graph which plotted the advancing direction of the sweeper obtained using a Kalman filter. In the 2nd Embodiment of this invention, it is a figure which shows an example of the movement locus | trajectory of the supermarket obtained when the road is curved. In the 2nd Embodiment of this invention, it is the graph which plotted the advancing direction of the sweeper when the road is curved. (A)-(c) is a figure which shows each 1st-3rd example of the road shape decompress | restored by the process which concerns on the 2nd Embodiment of this invention. It is a figure which shows an example of the road direction vector in the 3rd Embodiment of this invention. (A)-(c) is a figure which shows each 1st-3rd example of the road shape decompress | restored by the process which concerns on the 2nd Embodiment of this invention. In the 3rd Embodiment of this invention, it is a figure which shows that an angle error becomes large when the distance of a sweeper and an observation point is long. In the 3rd Embodiment of this invention, it is a figure which shows that an angle error becomes small when the distance of a sweeper and an observation point is near. It is an example of the road model used for the statistical estimation in the 3rd Embodiment of this invention. 38 is a histogram showing an example of the frequency of angle errors obtained from the road model in FIG. 37 (when the distance between the sweeper and the observation point is short). It is a histogram which shows an example of the frequency of the angle error obtained from the road model of FIG. 38 is a histogram showing an example of the frequency of angular errors obtained from the road model in FIG. 37 (when the distance between the sweeper and the observation point is long). It is a graph which shows the dispersion value obtained from the distribution shown to the histogram of FIGS. It is a figure for demonstrating the problem which concerns on the 4th Embodiment of this invention, and is a figure which shows an example from which an exact road width is obtained from two observation points. It is a figure for demonstrating the problem which concerns on the 4th Embodiment of this invention, and is a figure which shows the shift | offset | difference of left-right observation data. It is a figure for demonstrating the problem which concerns on the 4th Embodiment of this invention, and is a figure which shows that the road width estimated when the road is curving includes an error. It is a figure for demonstrating the problem which concerns on the 4th Embodiment of this invention, and is a figure which shows that the road width estimated when the road width is changing contains an error. It is an example of the road model used for the statistical estimation in the 4th Embodiment of this invention. 47 is a histogram showing an example of the frequency of road width error obtained from the road model of FIG. 46 (when the deviation of the left and right observation data is small). 47 is a histogram showing an example of the frequency of angle errors obtained from the road model of FIG. 46. 47 is a histogram showing an example of the frequency of angular errors obtained from the road model of FIG. 46 (when the deviation between the left and right observation data is large). It is a graph which shows the dispersion value obtained from the distribution shown to the histogram of FIGS. It is an example of the calculation formula of the Kalman filter used in the 4th Embodiment of this invention. It is a flowchart which shows an example of the road width estimation process in the 4th Embodiment of this invention. It is a block diagram which shows an example of a structure of the road shape estimation apparatus in the 4th Embodiment of this invention. (A) is a figure which shows an example of the observation data group (several observation points) sensed by the sensor, (b) is a figure which shows an example of the road shape decompress | restored from the observation data group, (c) These are figures which show an example in which the observation data left-right determination problem occurs in the conventional technique.

  Hereinafter, first to fourth embodiments of the present invention will be described with reference to the drawings. In the first embodiment of the present invention, the basic concept of the present invention will be described. In the second embodiment of the present invention, a smoothing process that suppresses the influence of left-right displacement with respect to the road extending direction will be described. In the third embodiment of the present invention, a description will be given of a smoothing process that suppresses the influence of an error at a distant observation point. In the fourth embodiment of the present invention, processing for estimating the road width will be described.

<First embodiment of the present invention>
First, the basic concept of the present invention will be described in the first embodiment of the present invention. The present invention does not perform the prior art procedure of selecting the observation data group of either the left end or the right end of the sensed road and estimating one road end shape first, instead of the sensed road. The observation data is read as needed, and the road shape (route along the road extending direction) is estimated while judging whether the observation data is the left end or the right end of the road. A route passing through the center of the road estimated according to the present invention can be restored to a road shape having a road width, for example, by giving a width to the left and right by the road width. Further, by estimating a route passing through the center of the road in real time in a traveling vehicle or the like, it can be used for information such as automatic driving or automatic lane keeping as information for predicting a safe driving route.

  Specifically, in the present invention, as schematically shown in FIG. 1, a filter for scanning a space (virtual space) in which observation data groups are distributed (in this specification, this filter is called a sweeper (Sweeper). : Scan)), and scan the observation data group by moving the filter. The sweeper reads the sensed observation data as needed, and moves along the route estimated to be the center of the road while determining whether the observation data belongs to the left end or the right end of the road. The sweeper used in the present invention samples the surrounding observation data from both the left and right sides of the road as needed to estimate the road shape, so even if there is a missing part in the information on one side of the road, The road shape can be estimated while supplementing with the information on one side.

  The sweeper referred to here is a filter that performs calculation processing by moving in a snapshot space where observation data groups sensed by sensors installed in the vehicle are distributed. In the figure, a box-shaped sweeper is schematically illustrated, but the sweeper has position coordinates and a traveling direction in a space where observation data groups are distributed as parameters. The angle indicating the direction of travel of the sweeper is represented by a counterclockwise polar coordinate in which the rightward direction on the paper is 0 ° and the upward direction is 90 °.

  As shown in the flowchart of FIG. 2, the basic process of the present invention includes an observation data group reading process (step S11) and a sweeper calculation process (step S12). By repeatedly performing these processes, the road The route through the center of is gradually estimated.

  In the observation data group reading process in step S11, observation data existing at a position far away from the current position coordinates of the sweeper is not forcibly read, and an observation data group closer than a predetermined distance (based on the current position coordinates of the sweeper) ( Only the observation data group (existing within a predetermined range) is sampled. This is intended to solve the “observation data left / right determination problem” that may occur due to observation point data that exists in the distance. For example, as shown in FIG. 3, an observation data group distributed within a certain range in front of the sweeper (in the example of FIG. 3, a sector having θ = 90 ° ± 50 ° and a radius = 10 m) is sampled.

  In the sweeper calculation process in step S12, a process of calculating the traveling direction of the sweeper using the sampled observation data group and a process of moving the sweeper along the calculated traveling direction of the sweeper are performed. In the sweeper calculation process, the sweeper may be moved while determining the traveling direction of the sweeper for each sampled observation data (Sweeper calculation example 1 described later). In addition, the sweeper may be moved while determining the traveling direction of the sweeper based on a plurality of sampled observation data. In this case, by combining vectors obtained from each of a plurality of sampled observation data groups. The sweeper travel direction may be calculated (Sweeper calculation example 2 described later), and the final sweeper travel direction is calculated while tentatively calculating the sweeper travel direction for each of a plurality of sampled observation data groups. You may calculate (Sweeper calculation example 3 mentioned later).

  Hereinafter, sweeper calculation examples 1 to 3 will be described. In these sweeper calculation examples 1 to 3, as shown in FIG. 3, there are three observation points A to C within a predetermined range in front of the sweeper, and the position coordinates of the sweeper (initial position coordinates) and the current A description will be given assuming that the traveling direction (initial traveling direction) is set.

(Sweeper calculation example 1)
As a method of calculating the traveling direction of the sweeper, for example, a method of moving the sweeper while determining the traveling direction of the sweeper for each observation point can be adopted. Specifically, for example, using the observation point that is closest to the sweeper, the traveling direction of the sweeper is calculated, and the operation of moving the sweeper along the calculated traveling direction is repeatedly performed.

  FIG. 4 is a flowchart illustrating an example of the sweeper calculation example 1 according to the first embodiment of the invention. The process according to the flowchart of FIG. 4 corresponds to the sweeper calculation process of step S12 of FIG. In the sweeper calculation example 1, first, the nearest observation point in front of the sweeper is selected (step S1211). For example, in the example of FIG. 3, the observation point A exists on the right side as viewed from the traveling direction of the sweeper as the observation point closest to the sweeper. It can be determined whether the observation point belongs to the left end or the right end of the road depending on whether the observation point exists on the left side or the right side with respect to the current traveling direction of the sweeper.

  Next, using the current traveling direction of the sweeper and the observation data (positional coordinates of the observation point) to be processed, a projection point corresponding to the observation point is calculated (step S1212), and the current position of the sweeper is calculated. The direction of the vector connecting the coordinates and the projection point is calculated (step S1213). For example, as shown in FIG. 5, a projection point A projected from the observation point A by a distance (W / 2) half the road width W perpendicular to the current direction of travel of the sweeper (reference direction) is calculated. The direction of the vector connecting the current position coordinates of the sweeper and the projection point A is calculated.

  In the sweeper calculation example 1, the direction of the vector connecting the current position coordinate of the sweeper and the projection point is set as a new traveling direction of the sweeper, and the sweeper is moved by a predetermined distance along this direction (step S1214). ). Note that the moving distance of the sweeper at this time can be arbitrarily set. For example, the sweeper may be moved by a predetermined movement distance, or the sweeper may be moved to the projection point. After the sweeper is moved, the new travel direction of the sweeper is the current travel direction of the sweeper, and the position coordinate after the sweeper is moved is the current position coordinate of the sweeper.

  As described above, when the sweeper is moved along the new traveling direction calculated using the observation point A, another observation point is set as a processing target (step S1215). As shown in FIG. 6, the same processing is performed on the observation point B, a new traveling direction is calculated, and the sweeper is moved. Further, after the sweeper is moved along the new traveling direction calculated using the observation point B, the same processing is performed on the observation point C as shown in FIG. Calculate the direction and move the sweeper.

  When the processing for all the observation points existing within a predetermined range in front of the sweeper is completed, the sweeper calculation processing (that is, the sweeper calculation processing in step S12 in FIG. 2) ends. Thereafter, returning to the observation data group reading process in step S11 of FIG. 2, the observation data group in front of the sweeper is newly sampled, and the same sweeper calculation process is repeated again.

  In the sweeper calculation example 1, the sweeper is moved by the number of times equal to the number of observation points existing within a predetermined range, and a series of movement trajectories of the sweeper as shown in FIG. 8 is obtained. When the observation point is the road edge, the sweeper moves so as to keep a position (W / 2) that is half the road width W from the road edge. Therefore, the sweeper moves along the center of the road. It can be regarded as a route through.

(Sweeper calculation example 2)
In addition, as a method of calculating the traveling direction of the sweeper, for example, the traveling direction of the sweeper is calculated for each of a plurality of observation points existing within a predetermined range, and a summary of these calculation results is used as a new traveling direction. It is also possible to adopt a setting method. Specifically, the traveling direction is calculated from the observation data group distributed within a predetermined range, and the operation of moving the sweeper along the calculated traveling direction is repeatedly performed.

  FIG. 9 is a flowchart illustrating an example of the sweeper calculation example 2 according to the first embodiment of this invention. The process according to the flowchart of FIG. 9 corresponds to the sweeper calculation process of step S12 of FIG. In the sweeper calculation example 2, the position coordinates of an arbitrary observation point are selected (step S1221), and the current traveling direction of the sweeper and the observation data (observation point position coordinates) to be processed are used as processing targets. A projection point corresponding to a certain observation point is calculated (step S1222), and a direction of a vector connecting the current position coordinates of the sweeper and the projection point is calculated (step S1223). These processes are performed for all observation points (step S1224). As a result, for example, as shown in FIG. 10, the direction of the vector connecting the current position coordinates of the sweeper and the projection points A to C corresponding to the observation points A to C is calculated.

  When processing has been completed for all observation points within the specified range in front of the sweeper, the direction of the newly calculated vector from each observation point is set as the new direction of travel of the sweeper, and this direction The sweeper is moved by a predetermined distance along (step S1225). Arbitrary calculation methods can be used to calculate the direction of vectors newly calculated from each observation point. For example, these vectors (or vectors after normalization) may be combined. Further, the moving distance of the sweeper can be arbitrarily set. For example, the sweeper may be moved by a predetermined moving distance, or the moving distance according to the predetermined range used when acquiring the observation point (for example, when the predetermined range is a sector having a radius = 10 m) The moving distance may be 10 m). For example, as illustrated in FIG. 11, the vectors calculated from the observation points A to C (illustrated by dotted lines in FIG. 11) are combined, and the position moved by 10 m in the direction of the combined vector is moved. It can be the position of the sweeper.

  When the sweeper is moved, the sweeper calculation process (that is, the sweeper in step S12 of FIG. 2) is performed by setting the new sweeper direction as the sweeper's current travel direction and using the position coordinate after the sweeper as the sweeper's current position coordinate. The calculation process is finished. Thereafter, the process returns to the observation data group acquisition process in step S11 of FIG. 2 to acquire a new observation data group, and the same sweeper calculation process is repeated again.

  In the sweeper calculation example 2, even when there are a plurality of observation points within a predetermined range, the sweeper moves only once. However, by repeatedly performing the process of calculating the traveling direction from the observation data group within the predetermined range and moving the sweeper, a series of sweeping trajectories of the sweeper as shown in FIG. 12 is obtained. When the observation point is the road edge, the sweeper is calculated to move while maintaining a position that is half the distance (W / 2) of the road width W from the road edge. It can be regarded as a route passing through the center.

(Sweeper calculation example 3)
In addition, as a method of calculating the traveling direction of the sweeper, for example, a method of obtaining a new traveling direction by sequentially calculating the traveling direction of the sweeper for each of a plurality of observation points existing within a predetermined range is adopted. Is also possible. In the above-described sweeper calculation example 2, the vectors corresponding to each observation point are calculated independently, and then the calculated vectors are collected, whereas in the sweeper calculation example 3, calculation is performed using a certain observation point. The vector direction corresponding to another observation point is calculated using the vector direction.

  FIG. 13 is a flowchart illustrating an example of the sweeper calculation example 3 according to the first embodiment of this invention. The process according to the flowchart of FIG. 13 corresponds to the sweeper calculation process of step S12 of FIG. In the sweeper calculation example 3, the position coordinates of an arbitrary observation point are selected (step S1231), and the observation data (to be processed) using the current traveling direction of the sweeper and the position coordinates of the observation point to be processed ( The projection point corresponding to the observation point position coordinate) is calculated (step S1232), and the direction of the vector connecting the current position coordinate of the sweeper and the projection point is calculated (step S1233).

  Here, when the processing related to all observation points is not completed, the process proceeds to processing for newly calculating unprocessed observation points (step S1234). At this time, the calculation is performed in step S1233. The direction of the selected vector is set to the current traveling direction of the sweeper (step S1235). This process can be said to be an operation of rotating the sweeper on the spot and changing the reference direction used when calculating the projection point from the next selected observation point.

  When processing for all observation points existing within a predetermined range in front of the sweeper is completed, the direction of the finally calculated vector is set as a new traveling direction of the sweeper, and the sweeper is moved by a predetermined distance. Note that the moving distance of the sweeper can be arbitrarily set for the example of the sweeper calculation. The sweeper calculation process (that is, the sweeper calculation process in step S12 in FIG. 2) is completed. Thereafter, the process returns to the observation data group acquisition process in step S11 of FIG. 2 to acquire a new observation data group, and the same sweeper calculation process is repeated again.

  Hereinafter, the operation of the sweeper calculation example 3 will be specifically described with reference to FIGS. For example, when the observation point A is first selected, as shown in FIG. 14, the distance (W / W) of the road width W is perpendicular to the current traveling direction (reference direction) of the sweeper from the observation point A. 2) The projection point A corresponding to the observation point A is set as the projected point, and the direction of the vector connecting the current position coordinate of the sweeper and the projection point A is calculated. For example, the sweeper in the sweeper calculation example 3 can be rotated around the current position coordinate of the sweeper. When the process of step S1235 in FIG. 13 is performed, the current direction of travel of the sweeper is new as shown in FIG. Rotate the sweeper around the current position coordinate so that it matches the calculated vector direction.

  Subsequently, in this state, as shown in FIG. 16, a projection point B corresponding to another observation point (for example, observation point B) is calculated. Then, the process of step S1235 in FIG. 13 is performed again, and as shown in FIG. 17, the sweeper is centered on the current position coordinate so that the current traveling direction of the sweeper coincides with the direction of the newly calculated vector. Rotate.

  Further, in this state, as shown in FIG. 18, a projection point C corresponding to another observation point (for example, observation point C) is calculated. Here, since the processing for all the observation points A to C existing within a predetermined range in front of the sweeper is completed, the processing in step S1236 in FIG. 13 is performed, and the direction of the finally calculated vector (FIG. 18). Is set as a new direction of travel of the sweeper, and the sweeper is moved by a predetermined distance. For example, as illustrated in FIG. 19, the position of the sweeper moved 10 m in the new traveling direction can be set as the position of the sweeper after the movement.

  Similar to the above-described sweeper calculation example 2, in the sweeper calculation example 3, even when there are a plurality of observation points within a predetermined range, the number of times the sweeper moves is only one. However, by repeatedly performing the process of calculating the traveling direction from the observation data group within the predetermined range and moving the sweeper, a series of sweeping trajectories of the sweeper as shown in FIG. 12 is obtained. When the observation point is the road edge, the sweeper is calculated to move while maintaining a position that is half the distance (W / 2) of the road width W from the road edge. It can be regarded as a route passing through the center.

  In the sweeper calculation example 1, every time the sweeper moves, an operation of determining a new traveling direction of the sweeper based on the next observation point is performed. Further, in the sweeper calculation example 2 and the sweeper calculation example 3, every time the sweeper moves, an operation of determining a new traveling direction of the sweeper based on a plurality of observation points existing within a predetermined range is performed. All of these actions are as if walking in a corridor with a width W, sometimes touching the left and right walls, keeping the position half the width W (W / 2) of the corridor from the wall as your own position. It can be likened to a state of tracing in the center of. In the present specification, a route passing through the center of the road is estimated as a route along the extending direction of the road. This is because the calculation is performed by tracing an equal distance (half the road width) from the left and right road edges (left and right observation points), but it is necessary to use half the road width for the calculation. There is no. For example, even if the road width is divided unevenly, such as a distance of a quarter of the road width as viewed from the observation point on the right side, a distance of a quarter of the road width as viewed from the observation point on the left side, etc. Good.

  In any of the sweeper calculation examples 1 to 3, the road width W is used when calculating the traveling direction of the sweeper. In the present invention, the method for acquiring or estimating the road width is not limited, and an arbitrary method (for example, a mechanism for estimating the road width is separately provided, or an appropriate value is set as the road width in advance. Etc.) can be adopted. Further, as described in the fourth embodiment of the present invention, the road width may be estimated using the observation data group.

  In the above description of the sweeper calculation examples 1 to 3, it is assumed that the position coordinates (initial position coordinates) of the sweeper and the current traveling direction (initial traveling direction) are set. At the moment, the position and direction of travel of the sweeper has not yet been calculated, and it is not known where is the center position of the road and in which direction the sweeper should be moved. Precisely, with respect to the vertical axis (ie, the axis parallel to the road extension direction), the position of the sensor (front surface of the sensor surface) can be regarded as the start point of the sweeper, while the horizontal axis (ie, road extension). For the axis perpendicular to the direction), it is not possible to specify where the sweeper should be placed as the initial position. This is shown in FIG.

  First of all, it is assumed that the sensor or the mobile body on which the sensor is mounted always faces the road extending direction, and the front surface of the sensor surface is perpendicular to the vertical axis (that is, the axis parallel to the road extending direction). Suppose there is. Thereby, the initial traveling direction of the sweeper can be determined to be a direction perpendicular to the front surface of the sensor surface. Under this assumption, we read several observation data groups near the sensor, calculate a set of points projected from the observation points perpendicular to the front of the sensor, and set these points to the two left and right. By dividing into groups and assuming that the average position of each group is the initial position of the left and right ends of the road, the position of the initial left and right ends of the sweeper is specified. Thereby, the position which equally divides the line segment which connects the initial right and left ends of a sweeper can be defined as the initial position of the sweeper (initial position of the center of the road). This is shown in FIG.

  Even if the initial conditions are slightly deviated, the position and traveling direction of the sweeper converges to the center of the road as it moves, so it is not always necessary to set accurate initial conditions. Even if initial conditions are roughly set, a desired calculation result can be obtained.

  In addition, when the inventor verified the operation of actually estimating the road shape using each method of the above-described sweeper calculation examples 1 to 3, a highly accurate result can be efficiently obtained. The result that the method 3 is relatively useful can be obtained.

  Next, the configuration of the road shape estimation apparatus according to the first embodiment of the present invention will be described. FIG. 21 is a block diagram showing an example of the configuration of the road shape estimation apparatus according to the first embodiment of the present invention. The road shape estimation apparatus 10 illustrated in FIG. 21 includes an observation data group reading unit 20 that reads an observation data group, and an observation data group read by the observation data group reading unit 20 in a virtual space in which the observation data group is distributed. From the calculation result by the route estimation filter processing unit 30 for calculating the traveling direction of the sweeper (filter) while moving the sweeper in (1), the road shape (the sweeping trajectory of the sweeper that can be regarded as a route passing through the center of the road) ( A road shape restoration unit 40 for restoring a route passing through the center of the road). The route estimation filter processing unit 30 can perform any one of the above-described sweeper calculation examples 1 to 3, and further uses the Kalman filter in the second and third embodiments of the present invention described later. Calculation processing may be performed.

  The observation data group storage unit 50 stores an observation data group in which the road edge is sensed by a sensor mounted on a moving body (for example, a vehicle such as an automobile, a motorcycle, or a bicycle). The road shape estimation device 10 performs processing by sampling observation data to be processed from these observation data groups. The road shape estimation device 10 is installed in a mobile body on which a sensor is mounted, and is an observation data group output from a sensor that performs sensing in accordance with the movement of the mobile body (temporarily stored in the observation data group storage unit 50). The observation data group) may be read to restore the road shape in real time. Alternatively, the mobile body may be provided with a data communication function, and the observation data group output from the sensor may be transmitted to the remote road shape estimation apparatus 10. Further, the observation data group output from the sensor when the moving body is moved is accumulated in the observation data group storage unit 50, and after the movement of the moving body is completed, the observation data group storage unit 50 is collected to obtain the road shape. You may make the estimation apparatus 10 read an observation data group.

  In FIG. 21, the device configuration of the road shape estimation device 10 is represented using functional blocks, but this is realized by creating a program related to each functional block and causing it to be executed by a CPU (Central Processing Unit) of the computer. Each functional block may be constituted by an integrated circuit. Further, using a display device (such as a display) or an input device (such as a mouse or a keyboard) connected to the road shape estimation device 10, the operator can appropriately control settings related to each function, processing timing in each function, and the like. You may do it.

  Next, in the first embodiment of the present invention, a description will be given of left-right swinging of the sweeper movement locus. First, as a simple example, consider a case where the road is straight and the road width W is known, and the sensed observation data group has no noise or error. In FIG. 22, lines indicating the left and right ends of the true road are shown by dotted lines, and three observation points A to C existing within a predetermined range (a sector having θ = 90 ° ± 50 ° and a radius = 10 m). Is represented by the symbol “+”. Since the sensed observation data group has no noise or error, each observation point A to C is located directly above the true road edge. Here, the initial position coordinates of the sweeper are placed directly above the center of the true road, and the initial traveling direction of the sweeper is the true road extending direction (θ = 90 °).

  In this case, as shown in FIG. 23, the projection points A to C corresponding to the observation points A to C are all located right above the center of the true road. Therefore, in any of the methods of the above sweeper calculation examples 1 to 3, the traveling direction of the sweeper is always directed to the true road extending direction (θ = 90 °). When drawing in the same manner as in FIG. 12 described above, the movement trajectory when the traveling direction of the sweeper is calculated according to the sweeper calculation example 3 can be expressed as shown in FIG.

  FIG. 25 is a graph plotting the angle θ in the traveling direction when the sweeper moves. The vertical axis of the graph represents the angle θ in the traveling direction of the sweeper, and the horizontal axis of the graph represents the number of movements (number of turns). Here, since the road is straight ahead, the road width W is known, and there is no noise or error in the sensed observation data group, the angle θ in the traveling direction of the sweeper is always 90 °. The sweeping trajectory of the sweeper does not fluctuate.

  Next, consider a case where the road is straight and the road width W is known, but the sensed observation data group is deviated from the true road edge. For example, as shown in FIG. 26, the observation data group actually sensed by the sensor is often deviated from the true road edge due to the influence of noise or error.

  In FIG. 26, unlike the state shown in FIG. 22, the observation point “+” is not directly above the true road edge. Therefore, based on the processing according to the first embodiment described above, a distance of half the road width W (W / 2) is projected perpendicularly to the vector of the traveling direction of the sweeper from the sensed observation point. When the calculated points are calculated, as shown in FIG. 27, the current vector of the sweeper and the vector from the position of the sweeper to the projected point are shifted by the angle dθ with respect to the current direction of travel of the sweeper. It becomes the state.

  Similarly, for other observation points that exist within a given range, there is a shift in the angle between the current vector of the sweeper's current direction and the vector from the position of the sweeper to the projected point. The new direction of travel of the sweeper calculated from all the observation points is in a state of being out of alignment with the true road extending direction (θ = 90 °). As a result, as shown in FIG. 28, the sweeper's trajectory is largely dispersed from side to side about the true center of the road (θ = 90 °), and the sweeper moves while being swung from side to side.

  In this way, even if discontinuous fluctuations occur in the sweeper's trajectory, the sweeper's trajectory transitions across the true center of the road (θ = 90 °), so that it can be used for observation data groups with errors. It can be said that it is useful to apply the processing according to the first embodiment of the present invention. However, in order to obtain a more accurate result, it is possible to perform the smoothing process described as the second embodiment of the present invention.

<Second Embodiment>
Hereinafter, a second embodiment of the present invention will be described. In the second embodiment of the present invention, smoothing using a Kalman filter is performed when calculating a new traveling direction of the sweeper so that the locus of the sweeper does not fluctuate greatly from side to side. Smoothing using the Kalman filter can absorb the angular jump due to the error of each observation point when calculating the new direction of travel of the sweeper, compared with the case where smoothing processing is performed after obtaining the sweeper's trajectory. Thus, it is possible to suppress large fluctuations in the direction of travel of the sweeper in real time.

  In the second embodiment of the present invention, for example, with respect to the direction of a vector (represented by an angle value) that connects the current position coordinates of the sweeper and the projection point so that the movement locus of the sweeper does not greatly shake from side to side Then, smoothing using a Kalman filter is performed. Smoothing using the Kalman filter can absorb angular jumps due to errors of individual observation points when a vector connecting the current position coordinates of the sweeper and the projection point is calculated. In addition, it is possible to suppress large fluctuations in the traveling direction of the sweeper in real time with higher accuracy than in the case where the smoothing process is performed after obtaining the movement locus of the sweeper.

  The direction of the vector connecting the current position coordinate of the sweeper and the projection point is, for example, the process of step S1213 in FIG. 4 in the above-described sweeper calculation example 1, the process of step S1223 in FIG. In the first sweeper calculation example 1, the calculation is performed by the process of step S1233 of FIG. In the second embodiment of the present invention, the Kalman filter process is performed on the direction (angle value) obtained by these processes.

An example of a specific calculation formula for Kalman filter processing is as shown in FIG. 29, for example. In the Kalman filter used in the second embodiment of the present invention, the only variable of the state x is the angle θ representing the direction of the vector connecting the current position coordinate of the sweeper and the projection point, so that it becomes a scalar Kalman filter and is simply predicted. The state x is updated through the update step. Further, since the Kalman filter is used for the purpose of the smoothing process, there is no control operation of the Kalman filter, and the control matrix F = 1 and the observation matrix H = 1 can be set. Further, a value other than 0 is used for the control error variance Q, and the observation error R can be set as R = (π / 2) ² .

  When the road is straight and the road width W is known, but the sensed observation data group is deviated from the true road edge (example shown in FIG. 26), the above Kalman filter processing is applied. The graph obtained is shown in FIG. Compared to the case where the Kalman filter processing is not applied (graph shown in FIG. 28), the angle θ in the direction of travel of the sweeper is smoothed, and the value is not greatly dispersed to the left and right, and the movement of the sweeper trajectory is suppressed. I understand that.

  FIG. 31 is a diagram showing an example of the movement trajectory of the sweeper obtained by applying the Kalman filter processing according to the second embodiment of the present invention, and FIG. 32 is a diagram of the second embodiment of the present invention. It is the graph which plotted angle (theta) of the advancing direction of the sweeper obtained by applying the Kalman filter process which concerns on a form. FIG. 31 shows the movement trajectory of the sweeper calculated when the observation data group sensed from the road edge of the curved road is deviated from the true road edge. In the graph of FIG. 32, the angle θ in the traveling direction of the sweeper calculated using the Kalman filter in the situation shown in FIG. 31 is plotted with black circles. In the graph of FIG. 32, the angle θ in the traveling direction of the sweeper calculated without using the Kalman filter is plotted as a white triangle as a comparison target.

  The advantage of processing using the Kalman filter is that, for example, when the road is curved as shown in FIGS. 31 and 32, the jump of the angle due to the error of the individual observation data is absorbed, while the road The rough change in the angle of bending is that it is saved by filtering. That is, the Kalman filter has a role as a low-pass filter.

  FIGS. 33A to 33C show a route (trajectory) passing through the center of the road actually obtained by the method using the Kalman filter processing according to the second embodiment of the present invention, and a known road width. A road having a road width restored using is shown. In the reconstruction of the road shape in FIG. 33A, an observation data group by an actual millimeter wave radar is used. Further, in FIGS. 33B and 33C, the sweeper is moving upward from the bottom of the page in the space where the observation point data is distributed, and the road edge is estimated along with the movement of the sweeper. Recognize. It can also be seen that the estimated road edge almost coincides with the correct road edge, and the estimation is performed with high accuracy.

<Third Embodiment>
Hereinafter, a third embodiment of the present invention will be described. As described above, in the present invention, in order to solve the “observation data left / right determination problem”, observation data existing at a position far away from the current position coordinate of the sweeper is not forcibly read, and the current position coordinate of the sweeper is used as a reference As described above, only an observation data group (an observation data group existing within a predetermined range) closer than a predetermined distance is read. In the third embodiment of the present invention, it is further developed, and it is not a “two-selection” determination as to whether it is within a predetermined range, but “gradually” as the observation data is farther away from the sweeper. The meaning that it is difficult to determine the left and right is statistically estimated and incorporated into the Kalman filter as a variance value.

  FIG. 34 shows a vector (“road direction vector”) indicating the degree of bending of a road around a certain observation data on a certain road. The “road direction vector” is a local vector facing the extending direction of the road, and can be defined as a tangent line that touches the route curve at a point that intersects the route curve that passes through the center of the road from the observation point. It is exactly this tangent that Sweeper is trying to predict by calculation. The observation data is read, and update calculation is performed so that the “road direction vector” of the observation data becomes the traveling direction vector of the sweeper.

  In FIGS. 35 and 36, as the position of the sweeper is closer to the observation data, the vector connecting the current position coordinates of the sweeper and the projection point approaches the correct value “direction vector representing the road shape” and becomes parallel. Show how it goes. That is, compared with FIG. 35, the distance between the sweeper and the observation point is closer in FIG. 36, and the vector connecting the current position coordinate of the sweeper and the projection point and the “direction vector representing the road shape” are It can be seen that the angle difference dθ is small.

  Also, using a road shape model as shown in FIG. 37, statistically estimated whether the “road direction vector” can be calculated using observation data related to an observation point as the sweeper approaches the observation point. The results are shown in FIGS. FIG. 37 is a diagram showing a road shape used for statistical data acquisition in the third embodiment of the present invention. In this statistical estimation, a reverse-winding road model is used in addition to the spiral road model shown in FIG. Further, FIGS. 38 to 40 are calculated from the observation data and the vector connecting the current position coordinates of the sweeper and the projection point when the infinite number of observation data is read for each of the infinite number of sweepers that can exist on the road. The angle difference with the calculated “road direction vector” is calculated as an error, and the calculation result is expressed as a histogram distribution. In this calculation, a large number of observation points are set at each of a large number of points located at the center of the road, and at each of the large number of observation points, a vector connecting the current position coordinates of the sweeper and the projection point, The angle difference with the “road direction vector” calculated from the observation point data is calculated. The horizontal axis of FIGS. 38 to 40 represents the magnitude of the calculated angle difference (angle error), and the vertical axis of FIGS. 38 to 40 represents the frequency of error.

  FIG. 38 shows a case where the parameter of the distance r set in the calculation is within the range of 0 to 10 (when the distance between the sweeper and the observation point is short), and FIG. 39 shows a case where the parameter is within the range of 20 to 30. , Each representing an error distribution in the range of 60 to 70 (when the distance between the sweeper and the observation point is long).

  In the histogram of FIG. 38 in which the distance between the sweeper and the observation point is close, there are many cases where the angle difference between the vector that connects the current position coordinate of the sweeper and the projection point and the “road direction vector” is close to zero. That is, as the distance value r between the sweeper and the observation point is smaller, it means that many of the calculated vectors are correctly oriented in the direction of the “road direction vector” that is the direction along the road shape.

  On the other hand, in the histogram of FIG. 40 in which the angle difference between the vector that connects the current position coordinates of the sweeper and the projection point and the “road direction vector” is large, the distance r between the sweeper and the observation point is large and the distance from the sweeper is large. Since the vector is calculated based on the observation point at the position, a case where the angle difference (angle error) is large is often seen as a distribution, which indicates that the vector calculated from the observation data is not always accurate.

  From the result of this statistical estimation, it can be assumed that the distribution (error distribution) of the angular difference represented by the histograms of FIGS. 38 to 40 has the shape of a Gaussian function, and the function f ( r) can be calculated. FIG. 41 is a graph showing the function f (r) of the variance value of the angle difference distribution. The horizontal axis in FIG. 41 represents the distance r from the sweeper to the observation point, and the vertical axis in FIG. 41 represents the variance value. As shown in FIG. 41, the function f (r) of the dispersion value represents a tendency that the dispersion value increases as the distance r increases.

  As described above, it is possible to assume that the angle difference between the vector that connects the current position coordinates of the sweeper and the projection point and the “road direction vector” calculated from the observation point data follows a Gaussian distribution. From this, the function f (r) of the variance value of the angular difference distribution can be taken into the Kalman filter so that the error can be calculated to be minimized.

As a specific example of the calculation formula for the Kalman filter processing, the formula in the fourth row of the Kalman filter calculation shown in FIG. 29 is replaced as follows, for example.
S = H * P * H '+ R + f (r) (Formula 1)
The Kalman filter has a property (addition of dispersion) that can be simply added when different dispersions are independent. The above newly set equation is obtained by adding the term of the function f (r) in parallel to the observation error variance R using the additive property of the variance.

  In this way, by adding the term f (r) to the fourth row expression of the Kalman filter calculation, when the distance r between the sweeper and the observation point is short, the variance f (r) becomes small and the update On the other hand, when the distance r between the sweeper and the observation point is far, the variance f (r) is increased and the reliability is decreased, and the update value is not greatly affected.

<Fourth embodiment>
Hereinafter, a fourth embodiment of the present invention will be described. In the first to third embodiments of the present invention described above, the method for acquiring or estimating the road width is not particularly limited and the road width is known. However, the fourth embodiment of the present invention is described. In the embodiment, a case where the road width is estimated using the observation data group will be described.

  First, the problem when the road width is estimated using the observation data group will be described.

  FIG. 42 shows a method for estimating the road width when an observation point exists on a line perpendicular to the road extending direction. The straight line connecting the observation points on both the left and right sides of the road is directed to the straight line of the road (tangent tangent to the route curve at the point where it intersects perpendicularly to the route curve passing through the center of the road from the observation point) When vertical, the road width is simply equal to the distance between these two observation points.

  However, it is rare for two observation points to exist on the same vertical line as shown in FIG. 42. In fact, as shown in FIG. 43, two observation points do not exist on the same vertical line. It is often in a state of being shifted to. In this specification, this shift is referred to as “shift between left and right observation data”. A point dropped perpendicularly to a straight line facing the road extending direction from each of the two observation points (hereinafter referred to as a perpendicular drawn from the observation point). This is expressed by a distance d between the two legs. For example, when the distance d is 0, there is no “left-right observation data shift”, and when the distance d is greater than 0, there is a “left-right observation data shift”.

  The road width can be estimated by using, for example, the distance between the observation point and the foot of a perpendicular line perpendicular to the extending direction of the road from the observation point. For example, in the example of FIG. 43, when the distance between the observation point A and the perpendicular foot a is WA and the distance between the observation point B and the perpendicular foot b is WB, the road width is the sum of these distances WA + WB. Can be estimated.

  According to this road width estimation method, a value close to the true road width can be obtained in the case of a straight road having a constant road width. However, when the road shape is curved or when the road width is changed, an error from the true road width becomes large.

  For example, when the road shape is curved as shown in FIG. 44, the road width WA + WB estimated using the two observation points A and B having the “deviation of left and right observation data” and the true road An error occurs between the width. This error increases as the distance “d” of “left-right observation data shift” increases, or as the road curvature (curvature) increases.

  For example, when the road width is changed as shown in FIG. 45, similarly, the road width WA + WB estimated using the two observation points A and B having the “deviation of the left and right observation data”. There is an error between the actual road width. This error increases as the distance “d” of “left-right observation data shift” increases or the change rate of the road width increases.

  In consideration of the above-described problems, in the fourth embodiment of the present invention, smoothing using a Kalman filter is performed on the road width to be estimated so that the road width changes smoothly. Smoothing using the Kalman filter can suppress large fluctuations in the road width in real time, and can absorb jumps due to errors of individual observation points used in the calculation of the road width. Furthermore, it is proposed to statistically estimate how the estimation result of the road width becomes inaccurate as the “deviation of left and right observation data” increases, and to import the estimation result as a variance value into the Kalman filter.

  The following shows statistical results showing how the estimated road width differs from the true road width as the “deviation of left and right observation data” increases for a certain road shape.

  FIG. 46 is a diagram showing the road shape used for examining the influence of the “deviation of left and right observation data” on the estimation result of the road width in the fourth embodiment of the present invention. 47 to 49 calculate the error between the road width WA + WB calculated by the above-described simple road width estimation method and the true road width with respect to the observation data that can be read from the center of the road shown in FIG. It is a histogram which shows a result. In this calculation, for each of a large number of points located at the center of the road, a set of two observation points on the left and right across the center of the road is set, and the road width WA + WB estimated in each set and the true road The error with the width is calculated. The horizontal axis in FIGS. 47 to 49 represents the magnitude of error between the estimated road width WA + WB and the true road width, and the vertical axis in FIGS. 47 to 49 represents the frequency of errors.

  47 shows the case where the parameter of the distance d set in the calculation is in the range of 0 to 1.25 (when “the deviation of the left and right observation data” is small), and FIG. In this case, FIG. 49 represents error distributions in the range of 3.75 to 5 (when the “deviation of left and right observation data” is large).

  In the histogram of FIG. 47 in which the “left-right observation data shift” is small, there are many cases where the error between the estimated road width and the true road width is close to zero. That is, as the distance d of the “left-right observation data shift” is smaller (when the two sampled left and right observation points are close to each other on the same line perpendicular to the road extension direction), the road width is more accurately determined. It means that it can be estimated.

  On the other hand, in the histogram of FIG. 49 where the “left-right observation data deviation” is large, the error value between the estimated road width and the true road width may be large because the distance value d of “left-right observation data deviation” is large. It is often seen as a distribution, indicating that the estimated road width is not always accurate.

  From the result of this statistical estimation, it can be assumed that the error distribution of the road width estimation represented by the histograms of FIGS. 47 to 49 is half the shape of the Gaussian function, and the function g (d) of its variance value is Can be calculated. FIG. 50 is a graph showing the function g (d) of the variance value of the error distribution of the road width estimation. The horizontal axis of FIG. 50 represents the distance d of “deviation of left and right observation data”, and the vertical axis of FIG. 50 represents the variance value. As shown in FIG. 50, the function g (d) of the dispersion value represents a tendency that the dispersion value increases as the distance d increases.

  As described above, since it is possible to assume that the error between the estimated road width and the true road width follows a Gaussian distribution, the variance of the error distribution related to “deviation of left and right observation data” in the road width estimation By incorporating the value function g (d) into the Kalman filter, it is possible to calculate so that this error is minimized.

A specific calculation example of the Kalman filter processing is as shown in FIG. 51, for example. In the Kalman filter used in the fourth embodiment of the present invention, since the variable of the state x _W is only the road width W, it becomes a scalar Kalman filter, and the state x _W is simply updated through the prediction and update steps. . Since there is no control operation of the Kalman filter, the control matrix F = 1 and the observation matrix H = 1 can be set. Further, the control error variance Q and the observation error R can be set to 1.

The function g (d) of the variance value of the road width estimation error is reflected as follows in the expression of the fourth row of the Kalman filter calculation shown in FIG.
S = H * P * H '+ R + g (d) (Formula 2)
This formula also uses the additiveness of variance referred to in the third embodiment of the present invention, and the term of the function g (d) of the variance value of the error distribution of the road width estimation is added to the observation error variance R. It was added in parallel.

  In this way, by adding the term g (d) to the expression of the fourth row of the Kalman filter calculation, the variance g (d) becomes small when the distance d of “left-right observation data shift” is small, On the other hand, it greatly affects the update value. On the other hand, when the distance “d” between the left and right observation data is large, the variance g (d) increases and the reliability decreases, and the update value is not significantly affected. It becomes like this.

  Next, processing in the fourth exemplary embodiment of the present invention will be described. FIG. 52 is a flowchart showing an example of a road width estimation process according to the fourth embodiment of the present invention.

  In the road width estimation processing according to the fourth embodiment of the present invention, the sweeper scans the observation data group while moving in the space where the observation data group is distributed. The reading of the observation data group by the sweeper may also serve as the reading process of the observation data in the process for estimating the route passing through the center of the road described above. For example, a predetermined data as shown in FIG. By reading the observation data group within the range, it is possible to prevent the “observation data left / right determination problem” that may occur due to observation point data existing in the distance.

  The sweeper scans the observation data group existing within a predetermined range from the front (position close to the sweeper), and samples and records the observation data estimated to be located on the right side of the road (step S21). In addition, the observation data estimated to be located on the left side of the road is similarly sampled and recorded (step S22). Note that the sweeper estimates the observation data present on the right side of the traveling direction as observation data located on the right side of the road, and the observation data present on the left side of the traveling direction is estimated as observation data located on the left side of the road. can do. If the sampled observation data does not include observation data located on both the left and right sides of the road (that is, observation data in both the left and right sets), the above-described steps S21 and S21 are performed until observation data in both the left and right sets are obtained. The process of S22 is repeated (step S23).

  On the other hand, if the sampled observation data includes observation data located on both the left and right sides of the road, the road width W is calculated from the left and right observation data of the most recently observed road (step S24). In this calculation, for example, it is possible to use a distance between the observation point and a perpendicular foot drawn perpendicularly to the road extending direction from the observation point. For example, in the example of FIG. 43, the distance WA between the observation point A and the vertical foot a and the distance WB between the observation point B and the vertical foot b are calculated, and the sum WA + WB is calculated as the road width (provisional). Typical road width).

  Further, the deviation d between the observation data on the left and right of the most recently observed road is calculated. In this calculation, it is only necessary to obtain the distance between the feet of the perpendicular line dropped from the observation point perpendicular to the extending direction of the road. For example, in the example of FIG. 43, the distance between the vertical foot a and the vertical foot b (the difference d between the left and right observation data) is calculated (step S25).

Then, the road width W (provisional road width) calculated in step S24 and the shift d between the left and right observation data calculated in step S25 are input to a Kalman filter as shown in FIG. 51 (step S26). Incidentally, the road width W calculated at step S24 is input to the state variables x _W, displacement of the right and left observation data calculated in step S25 d is a function of the variance of the error of the road width estimated incorporated into the Kalman filter g Used in the calculation of (d). When processed by the Kalman filter, an updated state variable (ie, updated road width) is output from the Kalman filter. The road width output from the Kalman filter is smoothed so as to suppress the influence of errors and noise included in the measurement of the observation data, and reflects the influence of the deviation d of the left and right observation data. Then, all the observation data groups existing within the predetermined range are returned to step S21 again, and the calculation of the road width using the Kalman filter is repeated for all the observation data groups existing within the predetermined range. When the processing is completed for all observation data groups existing within the predetermined range, the sweeper moves by a predetermined distance in the road extending direction, and moves the sweeper by performing the processing according to the flowchart of FIG. Thus, the road width can be estimated little by little along the route of the road.

  FIG. 53 is a block diagram showing an example of the configuration of the road shape estimation apparatus according to the fourth embodiment of the present invention. In addition to the configuration of the road shape estimation apparatus 10 shown in FIG. 21, the road width while moving the sweeper in the virtual space in which the observation data group is distributed with respect to the observation data group read by the observation data group reading unit 20 The road width estimation filter processing unit 50 is calculated.

  The road width estimation filter processing unit 50 can perform the above-described road width estimation processing. The road width estimated by the road width estimation filter processing unit 50 is used for processing such as calculation of the sweeper traveling direction in the route estimation filter processing unit 30 and restoration of a wide road shape in the road shape restoration unit 40. Is possible.

  In FIG. 53, it is possible to perform the Kalman filter process in the fourth embodiment of the present invention independently of the Kalman filter process performed in the second and third embodiments of the present invention described above. Therefore, the route estimation filter processing unit 30 and the road width estimation filter processing unit 50 are illustrated by different blocks. However, since the road width estimated by the road width estimation filter processing unit 50 can be used in, for example, calculation processing by the route estimation filter processing unit 30 (processing for estimating a route passing through the center of the road), the two filter processing units It is good also as an inseparable structure.

  INDUSTRIAL APPLICABILITY The present invention is applicable to a road shape estimation technique for estimating a road shape, and particularly for estimating the shape of a route (for example, a route passing through the center of the road) along the road extending direction as the road shape. It is applicable to other technologies. In addition, by mounting on a moving body, it is possible to estimate a road shape in front of or around the moving moving body, and it can be applied to a technique for safely moving the moving body.

DESCRIPTION OF SYMBOLS 10 Road shape estimation apparatus 20 Observation data group reading part 30 Path estimation filter process part 40 Road shape restoration part 50 Road width estimation filter process part

Claims (18)

A road shape estimation device that estimates the shape of the road using observation data groups sensed by a sensing device installed on a moving body traveling on the road,
An observation data group acquisition unit for acquiring an observation data group existing within a predetermined range in the sensing;
A process of arranging a filter having position coordinates and a traveling direction as parameters in a virtual space in which the observation data group is distributed, and calculating the traveling direction of the filter using the observation data group acquired by the observation data group acquisition unit A direction calculator,
A filter moving unit that moves the filter in the virtual space along the traveling direction of the filter calculated by the traveling direction calculating unit;
A road shape output unit that outputs the movement trajectory of the filter moved by the filter moving unit as a route along the extending direction of the road;
The traveling direction calculator is
A selection unit that selects the observation data existing within a predetermined range with reference to the current position of the filter;
A projection point calculator for calculating a projection point moved in a direction perpendicular to the current traveling direction of the filter by a predetermined distance from the position of the selected observation data;
A vector direction calculation unit for calculating a direction of a vector connecting the current position of the filter and the projection point;
A travel direction setting unit configured to set a new travel direction of the filter using the calculated vector direction;
The filter is moved by a predetermined moving distance toward a new traveling direction of the filter, and updated so that the current coordinates of the filter are set as the position of the filter after the movement, and the current traveling of the filter An update processing unit so that the direction is set as a new traveling direction of the filter,
A road shape estimation apparatus having The projection point calculation unit calculates a first projection point using a position of the first observation data among a plurality of observation data groups existing within the predetermined range;
The vector direction calculation unit calculates a direction of a first vector connecting the current position of the filter and the first projection point, and determines the direction of the first vector as the first observation data. Is configured to set as the current traveling direction of the filter for performing calculations using different second observation data,
A series of processing by the projection point calculation unit and the vector direction calculation unit is sequentially repeated for each of the plurality of observation data, and when the calculation is completed for all observation points, the traveling direction setting unit is configured to display the vector direction. The road shape estimation apparatus according to claim 1, configured to set a direction of a vector finally calculated by a calculation unit as a new traveling direction of the filter.   3. The smoothing processing unit according to claim 1, further comprising: a smoothing processing unit configured to perform a smoothing processing that suppresses a deviation of an angle between the direction of the vector calculated by the vector direction calculation unit and the current traveling direction of the filter. Road shape estimation device.   The smoothing processing unit increases the distance between the current direction of the filter and the direction of the vector calculated by the vector direction calculation unit as the distance from the current position of the filter to the position of the observation data increases. The road shape estimation apparatus according to claim 3, wherein the road shape estimation apparatus is configured to suppress angular deviation.   The distribution of the accuracy rate distribution of the route along the road extending direction is statistically obtained as a function of the distance between the filter position and the observation data, and the observation data is calculated from the current position of the filter. The road shape estimation apparatus according to claim 4, wherein a variance value corresponding to a distance to the position of the road is used for calculation by the smoothing processing unit.   The road shape estimation device according to any one of claims 3 to 5, wherein the smoothing processing unit is configured to perform a smoothing process using a Kalman filter. A road width calculation unit that calculates the road width using the observation data group acquired by the observation data group acquisition unit;
The road width calculation unit
Based on the traveling direction of the filter, the left and right sorting unit for sorting the observation data located on the right side of the road and the observation data located on the left side of the road;
Calculating a first projection point on a path along the extending direction of the road, projected in a direction perpendicular to the traveling direction of the filter from the position of the observation data located on the right side of the road; A road width projection point for calculating a second projection point on a path along the road extending direction, which is projected in a direction perpendicular to the traveling direction of the filter from the position of observation data located on the left side of the road A calculation unit;
As a provisional road width, a distance between the position of observation data located on the right side of the road and the first projection point, a position of observation data located on the right side of the road, and the second projection point A provisional road width calculation unit for calculating the sum with the distance;
A projection point shift calculation unit for calculating a distance between the first and second projection points calculated by the projection point calculation unit;
A road that calculates the road width by a smoothing process while suppressing a deviation from the road width calculated before the movement of the filter with respect to the direction of the temporary road width vector calculated by the temporary road width calculation unit. A width smoothing processing unit,
When calculating the road width using the road width calculated before the movement of the filter and the provisional road width in the road width smoothing processing unit, between the first and second projection points. The road shape estimation device according to any one of claims 1 to 6, wherein the road shape estimation device is configured to suppress the influence of the temporary road width on the road width that is the estimated result as the distance increases.   The distribution of the accuracy ratio distribution of the road width is statistically obtained as a function of the distance between the two projection points located on the right and left sides of the road, and between the first and second projection points. The road shape estimation apparatus according to claim 7, wherein a variance value corresponding to a distance is used for calculation of the road width by the road width smoothing processing unit.   The road shape estimation apparatus according to claim 7 or 8, wherein the road width smoothing processing unit is configured to perform a smoothing process using a Kalman filter. A road shape estimation method for estimating the shape of the road using an observation data group sensed by a sensing device installed on a moving body traveling on the road,
An observation data group acquisition step of acquiring an observation data group existing within a predetermined range in the sensing;
A process of arranging a filter having position coordinates and a traveling direction as parameters in a virtual space in which the observation data group is distributed, and calculating the traveling direction of the filter using the observation data group acquired in the observation data group acquisition step. A direction calculation step;
A filter moving step for moving the filter in the virtual space along the traveling direction of the filter calculated in the traveling direction calculating step;
A road shape output step for outputting the movement trajectory of the filter moved in the filter movement step as a route along the extending direction of the road;
The traveling direction calculation step includes:
A selection step of selecting the observation data existing within a predetermined range with reference to the current position of the filter;
A projection point calculating step of calculating a projection point moved in a direction perpendicular to the current traveling direction of the filter by a predetermined distance from the position of the selected observation data;
A vector direction calculation step of calculating a direction of a vector connecting the current position of the filter and the projection point;
A traveling direction setting step of setting a new traveling direction of the filter using the calculated vector direction;
The filter is moved by a predetermined moving distance toward a new traveling direction of the filter, and updated so that the current coordinates of the filter are set as the position of the filter after the movement, and the current traveling of the filter An update process step so that the direction is set as the new direction of travel of the filter,
A method for estimating a road shape. In the projection point calculation step, a first projection point is calculated using a position of the first observation data among a plurality of observation data groups existing within the predetermined range,
In the vector direction calculation step, a direction of a first vector connecting the current position of the filter and the first projection point is calculated, and the direction of the first vector is determined as the first observation data. Is set as the current direction of travel of the filter for performing calculations using different second observation data,
A series of processing in the projection point calculation step and the vector direction calculation step is sequentially repeated for each of the plurality of observation data, and when the calculation is completed for all observation points, the vector direction is determined in the advancing direction setting step. The road shape estimation method according to claim 10, wherein the direction of the vector finally calculated in the calculation step is set as a new traveling direction of the filter.   The smoothing process step according to claim 10 or 11, further comprising a smoothing process step for performing a smoothing process for suppressing a deviation of an angle between the direction of the vector calculated in the vector direction calculation step and a current traveling direction of the filter. Road shape estimation method.   In the smoothing processing step, the greater the distance from the current position of the filter to the position of the observation data, the greater the distance between the direction of the vector calculated in the vector direction calculation step and the current traveling direction of the filter. The road shape estimation method according to claim 12, wherein an angle deviation is suppressed.   The distribution of the accuracy rate distribution of the route along the road extending direction is statistically obtained as a function of the distance between the filter position and the observation data, and the observation data is calculated from the current position of the filter. The road shape estimation method according to claim 13, wherein a variance value corresponding to a distance to the position is used for calculation in the smoothing processing step.   The road shape estimation method according to any one of claims 12 to 14, wherein in the smoothing processing step, smoothing processing is performed using a Kalman filter. A road width calculation step of calculating the road width using the observation data group acquired in the observation data group acquisition step;
In the road width calculating step,
A left and right sorting step for sorting observation data located on the right side of the road and observation data located on the left side of the road with reference to the traveling direction of the filter;
Calculating a first projection point on a path along the extending direction of the road, projected in a direction perpendicular to the traveling direction of the filter from the position of the observation data located on the right side of the road; A road width projection point for calculating a second projection point on a path along the road extending direction, which is projected in a direction perpendicular to the traveling direction of the filter from the position of observation data located on the left side of the road A calculation step;
As a provisional road width, a distance between the position of observation data located on the right side of the road and the first projection point, a position of observation data located on the right side of the road, and the second projection point A provisional road width calculating step for calculating the sum with the distance;
A projection point shift calculating step for calculating a distance between the first and second projection points calculated in the projection point calculating step;
A road for calculating the road width by smoothing processing while suppressing a deviation from the road width calculated before moving the filter with respect to the direction of the temporary road width vector calculated in the temporary road width calculation step. Width smoothing processing step,
When the road width is calculated using the road width calculated before the movement of the filter and the provisional road width in the road width smoothing processing step, between the first and second projection points. The road shape estimation method according to any one of claims 10 to 15, wherein the larger the distance is, the more the influence of the temporary road width on the estimated road width is suppressed.   The distribution of the accuracy ratio distribution of the road width is statistically obtained as a function of the distance between the two projection points located on the right and left sides of the road, and between the first and second projection points. The road shape estimation method according to claim 16, wherein a variance value corresponding to a distance is used for calculation of the road width in the road width smoothing processing step.   The road shape estimation method according to claim 16 or 17, wherein in the road width smoothing processing step, smoothing processing is performed using a Kalman filter.

Images