JP2017003561A - Device for estimating position and posture of mobile entity, and autonomous traveling system of mobile entity - Google Patents

Abstract

PROBLEM TO BE SOLVED: To greatly reduce a processing time until the position and posture of a mobile entity is estimated when estimating the current position and posture of a mobile entity by collating a movement candidate that is set on the basis of an environment map against sensing data.SOLUTION: Divide collation units 30a-30c are designed in such a way that when setting a movement candidate indicating a position and posture that become possible at next detection timing on the basis of an environment map that is sensing data at previous detection timing, estimating a movement candidate that becomes a maximum evaluation value by collating the sensing data and the movement candidate at latest detection timing, and specifying the amount of change in position and posture from the position and posture indicated by the environment map to the position and posture indicated by the movement candidate of the maximum evaluation value, two kinds of variables are fixed while only one kind of variable is varied, and a movement candidate that becomes a maximum evaluation value is estimated. As compared with a configuration in which all of movement candidates are collated, a processing time until the movement candidate that becomes the maximum evaluation value is estimated is significantly reduced.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a mobile object position and orientation estimation apparatus that estimates a current position and orientation of a moving object by comparing motion candidates set based on an environment map and sensing data, and a mobile object autonomous traveling system.

  When estimating the current position and orientation of a moving body such as a vehicle or a robot, generally, there are three types of position (x, y) and orientation θ as the position and orientation change amount of the moving body from the previous detection timing. It is necessary to specify the variables (x, y, θ). In order to obtain the amount of change of these three types of variables, the relative position and orientation of the moving body with respect to the external environment are detected by, for example, a distance sensor, and the sensing data acquired from the distance sensor and the sensing data at the previous detection timing are used. Check against the environmental map.

  As a collation method, a motion candidate indicating the position and orientation of the moving body at the next detection timing is set based on the environment map, and the motion candidate and the sensing data are collated. The motion candidates are positions and orientations that are possible at the next detection timing, and are set by moving the environmental map by a predetermined (x, y) and rotating it by a predetermined θ. Such collation is performed by the number of motion candidates, and the current position and orientation are specified by estimating the motion candidate having the maximum evaluation value indicating the collation accuracy.

JP 2009-109200 A

  When estimating a motion candidate having the maximum evaluation value by the collation method as described above, since the motion candidate is composed of three types of variables (x, y, θ), the number of motion candidates set based on the environment map Is enormous, and there is a problem that the amount of verification processing is large.

  In order to deal with such problems, Patent Document 1 proposes a method of reducing the processing amount of the means for estimating the position and orientation by separating the means for creating the environment map and the means for estimating the position and orientation. Further, since only effective pixels are extracted based on sensing data in the creation of the environment map, the means for estimating the position and orientation reduces the amount of verification processing by omitting the determination of invalid pixels.

However, in Patent Document 1, since it is necessary to collate with all the motion candidates, there is a disadvantage that the processing time for collation becomes enormous.
In addition, regarding the processing amount, in the process of creating the environment map, there is a process for determining invalid pixels, and therefore the entire processing amount including the data processing cannot be greatly reduced.
Furthermore, since the environment map is created by an external computer and acquired by communication, communication time with the external computer occurs, and this also requires processing time.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to estimate a current position and orientation of a moving body by comparing a motion candidate set based on an environmental map with sensing data. It is an object to provide a mobile object position and orientation estimation apparatus and a mobile object autonomous traveling system that can significantly reduce the processing time until the position and orientation of the mobile object are estimated.

  According to the first aspect of the present invention, when the detection unit detects sensing data, the sensing data is stored in the storage unit as an environmental map. Therefore, the motion candidate setting unit can be performed at the next detection timing based on the environmental map. A motion candidate composed of a plurality of variables indicating the position and orientation of the moving body is set. When the detection unit detects sensing data at the next detection timing, the collation unit collates the sensing data with the motion candidate and estimates the motion candidate that is the maximum evaluation value. Then, the estimated value output unit outputs a position / orientation estimated value indicating a position / orientation change amount from the position / orientation indicated by the environment map to the position / orientation indicated by the motion candidate estimated by the collation unit.

  Here, the division verification unit obtains an evaluation value by changing the first variable for estimating the motion candidate in a state where a predetermined initial value is set as the second variable for which the motion candidate is not estimated, thereby obtaining the maximum evaluation value. The first variable that is the maximum evaluation value is estimated. Then, the estimated value output unit outputs the first variable of the maximum evaluation value estimated by each of the division matching units as the position / orientation estimated value, so that the traveling locus of the vehicle is specified by accumulating and adding the position / orientation estimated values. can do.

Functional block diagram showing the autonomous traveling system in the first embodiment Functional block diagram showing the distance sensor Functional block diagram showing the configuration of the verification unit A diagram showing the position and orientation of the distance measurement sensor relative to the external environment Diagram showing the relationship between sensing data and environmental map The figure which shows the procedure which estimates the motion candidate used as the maximum evaluation value Diagram showing normal distribution centered on x1 Diagram showing verification procedure of sensing data and environmental map Functional block diagram showing the configuration of the verification unit in the second embodiment Functional block diagram showing the configuration of the matching unit in the third embodiment Functional block diagram showing the configuration of the matching unit in the fourth embodiment The figure which shows the procedure which estimates the motion candidate used as the maximum evaluation value Diagram showing the procedure of binary search Functional block diagram showing the configuration of the matching unit in the fifth embodiment Diagram showing the relationship between exercise candidates and evaluation values Diagram showing the relationship between exercise candidates and evaluation values Flow chart illustrating a method for determining an estimate Functional block diagram showing the configuration of the matching unit in the sixth embodiment Functional block diagram showing the configuration of the matching unit in the seventh embodiment The figure which shows the procedure which estimates the motion candidate used as the maximum evaluation value The figure which shows the procedure which estimates the exercise | movement candidate used as the maximum evaluation value in 8th Embodiment. The figure which shows the procedure which estimates the exercise | movement candidate used as the maximum evaluation value in 9th Embodiment. Functional block diagram showing the configuration of the matching unit in the tenth embodiment Functional block diagram showing the configuration of the matching unit in the eleventh embodiment Functional block diagram showing the configuration of the matching unit in the twelfth embodiment Diagram showing the relationship between exercise candidates and evaluation values The figure which shows the method for calculating | requiring the inclination angle of the rising or falling of evaluation value distribution

(First embodiment)
A first embodiment will be described with reference to FIGS.
As shown in FIG. 1, an autonomous traveling system 1 is mounted on the vehicle. The autonomous traveling system 1 includes an external environment acquisition unit 2, a traveling environment recognition unit 3 and an infrastructure information storage unit 4 that constitute a position and orientation estimation device, an action selection unit 5, a vehicle control unit 6, and a route plan that constitute an autonomous traveling device. Part 7.

The external environment acquisition unit 2 includes an image capturing unit 8, a distance calculation unit 9, a speed detection unit 10, and a direction detection unit 11.
The image capturing unit 8 and the distance calculating unit 9 constitute a main part of the distance measuring sensor unit 12 (corresponding to a detecting unit) shown in FIG. The image capturing unit 8 includes a light emitting unit 13 and a light receiving unit 14, and swings in a fan-shaped horizontal direction around the front direction of the vehicle. The light emitting unit 13 emits a laser beam in a pulse shape, and the light receiving unit 14 receives the laser beam from the light emitting unit 13 reflected in the external environment located in the detection region. The distance calculation unit 9 measures a time difference from when the light emitting unit 13 emits the laser light to when the light receiving unit 14 receives the light, and the distance to the external environment calculated based on the time difference corresponds to the swing angle. Stored in the distance image memory 15. The distance image memory 15 outputs distance data in which the distance and the swing angle are associated with each other. The intensity of light received by the light receiver 14 is stored in the intensity image memory 16 corresponding to the swing angle. The intensity image memory 16 outputs image data in which the received light intensity is associated with the swing angle. This image data is referred to in order to determine the reliability of the distance data output from the distance image memory 15.

  The speed detection unit 10 detects the vehicle speed based on a speed signal output from a speed sensor mounted on the vehicle. The direction detection unit 11 detects a turning angle of the vehicle based on an angle signal output from an angle sensor mounted on the vehicle.

  The infrastructure information storage unit 4 includes a map information storage unit 17 and a traffic information storage unit 18. The map information storage unit 17 receives and stores map information indicating a road on which the vehicle can travel, for example, from a map information center. The traffic information storage unit 18 receives and stores traffic information such as road congestion information and accident information from, for example, a traffic information center.

  The travel environment recognition unit 3 includes a host vehicle position estimation unit 19, a travelable region recognition unit 20, an obstacle recognition unit 21, and a road sign recognition unit 22. The own vehicle position estimating unit 19 is a main unit to which the present invention is applied, and will be described later. The travelable area recognition unit 20 calculates the distance between the stationary object, which is an external environment obtained from the image data and the distance data, and the vehicle position estimated by the vehicle position estimation unit 19 to obtain stationary object information. The travel lane that is the travelable area of the vehicle is recognized based on the stationary object information. The obstacle recognition unit 21 estimates the type of the moving body or the moving speed by detecting and tracking the moving body located around the vehicle from the image data, the distance data, and the stationary object information.

The road sign recognition unit 22 recognizes the road sign based on the information acquired by the image data and the map data external environment acquisition unit 2.
The action selection unit 5 includes a lane keeping unit 23, a lane change unit 24, and an obstacle avoiding unit 25. The lane keeping unit 23 performs a traveling action of traveling on a traveling lane recognized by the travelable region recognizing unit 20 or traveling following the preceding vehicle when the preceding vehicle is traveling. The lane change unit 24 performs a danger avoidance action for the front vehicle and the rear vehicle. The obstacle avoidance unit 25 determines danger when an obstacle exists on the lane, and performs an avoidance action.

  The route planning unit 7 includes a global route searching unit 26, and sets a destination and sets a route to the destination. That is, a route from the current position to the destination is set based on the map information stored in the map information storage unit 17 and the traffic information stored in the traffic information storage unit 18. For the route to the destination, the route cost is calculated for each grid at a predetermined interval, and the route having the minimum route cost is set.

The vehicle control unit 6 includes an actuator driving unit 27, and performs an autonomous running operation by reflecting the result of the action selecting unit 5 on an accelerator, a brake, a steering, and the like.
Now, since the present invention relates to the own vehicle position estimating unit 19 constituting the traveling environment recognizing unit 3, the own vehicle position estimating unit 19 will be described in detail below.

As shown in FIG. 3, the host vehicle position estimation unit 19 includes a position data memory 28 (corresponding to a storage unit), an environment map memory 29, and a collation unit 30 (corresponding to an exercise candidate setting unit and an estimated value output unit). ing. As shown in FIG. 4, the distance measurement sensor unit 12 is configured to detect the vehicle from an external environment located within a predetermined detection area (for example, −60 ° to 60 ° with respect to the vehicle front direction) with reference to the vehicle front direction. Sensing data indicating the relative position and orientation is stored in the position data memory 28. In this case, since the relative position of the vehicle with respect to the external environment located in the detection area can be represented by (x, y), the relative posture of the vehicle with respect to the external environment can be represented by the rotation angle θ. Sensing data can be represented by (x, y, θ).
When sensing data is stored in the position data memory 28 as described above, the sensing data is also stored in the environment map memory 29 as an environment map.

  The collation unit 30 sets a motion candidate (x, y, θ) composed of variables indicating positions and orientations that can be obtained at the next detection timing based on the environment map stored in the environment map memory 29. When the next detection timing is reached, the sensing data stored in the position data memory 28 and the motion candidate are collated.

  In order for the collation unit 30 to set a motion candidate, the motion candidate is set by changing each of the three types of variables (x, y, θ) constituting the environment map. This motion candidate is set in a position and orientation range that is possible at the next detection timing. Then, the sensing data at this detection timing and the motion candidate are collated, and the position / posture change amount from the position / posture indicated by the environment map to the position / posture indicated by the motion candidate having the maximum evaluation value is determined as a position / posture estimation value (hereinafter, estimated). Value). That is, when the estimated value is added to the environment map shown in FIG. 5, sensing data is obtained. Therefore, the estimated value can be specified by comparing the sensing data with the motion candidate.

  Now, when motion candidates are assigned to three axes of spatial coordinates, motion candidates can be represented three-dimensionally as indicated by circles in FIG. Among the motion candidates set in this way, there is a motion candidate with the maximum evaluation value that matches or approximates the sensing data, and the position and orientation change amount from the position and orientation indicated by the environment map to the position and orientation indicated by the motion candidate with the maximum evaluation value Is an estimated value.

  By the way, in the estimation method of the estimated value of the conventional method, the sensing data and all the motion candidates indicated by circles in FIG. 6B are collated three-dimensionally. There is a problem that the processing time is long.

  In addition, although it is desirable to estimate the position and orientation at the next detection timing and collate with the motion candidate around the estimated position and orientation, the position and orientation at the next detection timing is not estimated as in this embodiment. In this case, the environment map is set as an initial value.

  That is, among the three types of variables constituting the motion candidate, regarding two types of variables y and θ other than x, if y0 = 0 and θ0 = 0 ° are set as initial values and y and θ are fixed, the variables are x only. Assuming that x = x1 is the maximum evaluation value, the motion candidate for x has the maximum evaluation value for x = x1, as shown by the solid line in FIG. A normal distribution in which the evaluation value decreases with increasing distance from x = x1 is shown.

  Here, assuming a second horizontal axis parallel to the horizontal axis so as to be located in the search range of motion candidates, the motion candidates on the second horizontal axis also show a normal distribution in which x = x1 is the maximum evaluation value. Become. This is because the evaluation value shows a normal distribution with x = x1 as the center. Therefore, when the second horizontal axis passes through the normal distribution region of the evaluation value, as shown by a dotted line in FIG. The motion candidates on the horizontal axis also show a normal distribution in which x = x1 is the maximum evaluation value. Accordingly, by setting y = y0 and θ = θ0 (second variable) as initial values, a motion candidate x = x1 (first variable) that is the maximum evaluation value is obtained as shown in FIG. be able to. Similarly, by setting x = x0 and θ = θ0 (second variable) as initial values, a motion candidate y = y2 (first variable) that is the maximum evaluation value can be obtained. Further, by setting variables x = x0, y = y0 (second variable) as initial values, θ = θ3 (first variable) can be obtained as a motion candidate that becomes the maximum evaluation value. Then, the motion candidate (x1, y2, θ3) (first variable that becomes the maximum evaluation value) that becomes the maximum evaluation value obtained as described above can be specified as the estimated value (x1, y2, θ3). .

  In order to realize such an estimation method, the collation unit 30 includes a collation unit first axis 30a, a collation unit second axis 30b, and a collation unit third axis 30c (corresponding to a divided collation unit). It is designed to collate with motion candidates in a three-dimensional manner. The first axis corresponds to x, the second axis corresponds to y, and the third axis corresponds to θ. For x and y, the unit is distance. As for θ, the unit is an angle (−60 ° to + 60 °).

In order to simplify the description, it is assumed that three motion candidates for each axis are set as shown in FIG.
The collation unit first axis 30a sets y = 0 and θ = 0 ° as initial values, thereby determining an evaluation value with a motion candidate to be collated as one dimension having only x as a variable. In the example illustrated in FIG. 8, evaluation values of x = 1, x = 0, and x = −1 are determined. In this case, since the maximum evaluation value is obtained when x = 0, it is estimated that the motion candidate having the maximum evaluation value on one axis is x = 0.

  The collation unit second axis 30b determines the evaluation value by setting x = 0 and θ = 0 °, so that the motion candidate to be collated is one-dimensional with only y as a variable. In the example shown in FIG. 8, evaluation values of y = 1, y = 0, and y = −1 are obtained. In this case, since the maximum evaluation value is obtained when y = 0, it is estimated that the motion candidate having the maximum evaluation value on two axes is y = 0.

  The collation unit third axis 30c determines the evaluation value by setting x = 0 and y = 0 to make the motion candidate to be collated one-dimensional with only z as a variable. In the example shown in FIG. 8, evaluation values of θ = 90 °, θ = 0 °, and θ = −90 ° are determined. In this case, since the maximum evaluation value is obtained when θ = −90 °, the motion candidate having the maximum evaluation value on the three axes is estimated to be θ = −90 °.

  As described above, x = 0, y = 0, and θ = −90 ° are estimated as the motion candidates that are the maximum evaluation values by the collation units 1 to 3 (hereinafter referred to as division collation units) 30a to 30c. Therefore, (0, 0, −90 °) is specified as an estimated value indicating the position and orientation change amount.

  In short, when the motion candidates corresponding to each axis are as shown in FIG. 8, only 27 (= 3 × 3 × 3) times, which is the number obtained by three-dimensionally combining all the motion candidates for each axis in the conventional method. Although it was necessary to collate the sensing data with the motion candidate, in the present embodiment, it is 9 (= 3 + 3 + 3) times combined in a one-dimensional manner, and the processing time is reduced by reducing the number of times the sensing data is collated with the motion candidate. Can be greatly reduced. In this case, in the conventional method, since the number of exercise candidates is the number of collations, it can be said that the effect of reducing the processing time increases as the number of exercise candidates increases.

  In addition, when there are a plurality of motion candidates that will be the maximum evaluation value, if the number is an odd number, the center value (for example, the third if 5), the center value (for example, 6 for the even number) In this case, the third or fourth motion candidate is set as the estimated value. This is because the evaluation value is normally distributed around the maximum evaluation value, and the reliability of the maximum evaluation value becomes higher as the motion candidate is located at the center of the normal distribution.

  Since the vehicle travel locus can be specified by repeating the above-described processing, the current absolute position based on the absolute position and orientation (latitude, longitude, traveling direction) at the departure position of the vehicle. The posture (latitude, longitude, traveling direction) can be specified.

According to such an embodiment, the following effects can be produced.
The collation unit 30 three-dimensionally sets motion candidates including a plurality of variables indicating positions and orientations that are possible at the next detection timing based on the environmental map that is sensing data at the previous detection timing, and performs sensing. By comparing the data and the motion candidate, the motion candidate that is the maximum evaluation value is estimated, and the estimated value indicating the amount of change in position and orientation from the position and orientation indicated by the environment map to the position and orientation indicated by the motion candidate having the maximum evaluation value is specified In this case, since the motion candidate that becomes the maximum evaluation value is estimated one-dimensionally by fixing only two types of variables and fixing only one type of variable, all the motion candidates are three-dimensionally estimated. Compared with the structure to collate, the processing time until the motion candidate used as the maximum evaluation value is estimated can be significantly reduced.

  Since only the environmental map that is sensing data at the previous detection timing needs to be stored, the storage capacity of the environmental map memory 29 can be greatly reduced as compared to the configuration storing a large number of environmental maps. The time required for storing and reading can be greatly reduced.

(Second Embodiment)
A second embodiment will be described with reference to FIG. In the first embodiment, the division verification units 30a to 30c fix two types of variables and obtain evaluation values of one type of variable. However, in this embodiment, one type of variable is fixed and two types of variables are calculated. It is characterized by obtaining an evaluation value of a variable.

  As shown in FIG. 9, instead of the collation unit first axis 30a and the collation unit second axis 30b of the first embodiment, the collation unit first axis / second axis 30d (corresponding to the division collation unit) are integrated. Is provided. The collation unit first axis / second axis 30d performs two-dimensional collation with the motion candidate.

  That is, since the two types of variables (x, y) indicate the position of the vehicle two-dimensionally and are related to each other, the two types of variables (x, y) are two types compared to checking for each type of variable. It can be said that the accuracy of the estimated value is higher when two-dimensional matching is performed by combining the variables (x, y).

  Therefore, θ = 0 ° is set as an initial value, and evaluation values of two types of variables x and y are obtained. In this case, since the number of motion candidates collated by the collation unit 1st axis / second axis 30d is 3 × 3, the number of collation is 9, and the collation number of the collation unit 3rd axis 30c is 3 times. In addition, the total number of collations is 12. Thereby, although it will increase rather than the frequency | count of collation of 1st Embodiment, it can reduce with respect to the conventional frequency | count of collation.

  According to such an embodiment, in the case where the accuracy of the estimated value is reduced unless the combination of a plurality of variables among the variables constituting the motion candidate is considered, while suppressing the reduction of the estimation accuracy, Similar to the first embodiment, the processing time until the estimated value is specified can be reduced.

(Third embodiment)
A third embodiment will be described with reference to FIG. The present embodiment is characterized in that the estimated value specified by the matching unit 30 is reset in the matching unit 30 as an initial value.

  As an initial value, it is important to set a value as close as possible to the position and orientation at the next detection timing. That is, the closer the initial value is to the position and orientation at the next detection timing, the higher the accuracy of the normal distribution and the higher the accuracy of the specified estimated value (x1, y2, θ3).

  Therefore, as shown in FIG. 10, the estimated values output from the divided matching units 30 a to 30 c are reset to the divided matching units 30 a to 30 c as initial values. In this case, the division verification units 30a to 30c specify the estimated value by setting the environmental map stored in the environmental map memory 29 as the first initial value, and the division verification units 30a to 30c corresponding to the initial value. The estimated value from 30c is reset as the second initial value.

According to such an embodiment, since the estimated value from the division collating units 30a to 30c corresponding to the initial value is reset as the initial value, a value closer to the position and orientation at the next detection timing is set to the initial value. And the accuracy of the estimated values by the division verification units 30a to 30c can be increased.
The initial value may be reset twice or more.

(Fourth embodiment)
A fourth embodiment will be described with reference to FIGS. In the first embodiment, the environment map is set as an initial value. However, the present embodiment is characterized in that a predicted value by a time series filter such as a Kalman filter or a particle filter is set as an initial value.

As described above, as the initial value, it is important to set a value as close as possible to the position and orientation at the next detection timing.
Therefore, in the present embodiment, the position and orientation at the next detection timing is predicted by the time series filter. Since the time series filter has no characteristics, its description is omitted.

  As shown in FIG. 11, the host vehicle position estimation unit 19 is provided with a time series filter unit 31 subsequent to the matching unit 30. The time series filter unit 31 is provided to input the estimated values from the division matching units 30a to 30c, and stores the input past estimated values in time series. Since such time-series estimated values are statistical information, it is possible to accurately predict the movement of the vehicle (assuming an acceleration motion when turning) by removing noise from the statistical information. That is, as shown in FIG. 12, the position and orientation at the next detection timing can be accurately estimated.

Here, as shown in FIG. 13, it is effective to limit the search range for motion candidates to a predetermined range centered on the prediction value by the time series filter unit 31, and therefore, the limit value of acceleration / deceleration of the vehicle. It is set to a certain 9.8 m / s ² or the maximum change width obtained from the running result. By limiting the search range in this way, it is possible to reduce the processing time until searching for a motion candidate that is the maximum evaluation value.

  Furthermore, when obtaining a motion candidate that will be the maximum evaluation value, it is not necessary to collate the sensing data with all motion candidates in the search range, and it is possible to further increase the processing time by detecting the motion candidate that is the maximum evaluation value by binary search. Reduction can be achieved.

  Specifically, when the time-series filter prediction value is a shown in FIG. 13, the motion candidate having the maximum evaluation value exists on either the right side or the left side of a. Assuming that there is a motion candidate having the maximum evaluation value on the right side of a, the motion candidate indicated by b obtained by adding a predetermined value from the time series filter prediction value is evaluated. In this case, since the evaluation value decreases, it is determined that the exercise candidate having the maximum evaluation value does not exist on the right side of a but on the left side, and the exercise candidate indicated by c obtained by subtracting a predetermined value from a is evaluated. . In this case, since the evaluation value rises, it is determined that there is a possibility that the maximum evaluation value exists further on the left side, and the motion candidate indicated by d obtained by subtracting a predetermined value is evaluated. Since the evaluation value further increases, it is determined that there is a possibility that the exercise candidate having the maximum evaluation value exists on the left side of “a”, and the exercise candidate indicated by “e” subtracted by a predetermined value is evaluated. Since the evaluation value decreases this time, it is determined that the exercise candidate having the maximum evaluation value exists in the range from the right side to c with respect to e. First, the exercise candidate indicated by f that is intermediate between c and d is evaluated. . Then, since it falls below the evaluation value of d, it is determined that the exercise candidate having the maximum evaluation value exists between d and e, and the exercise candidate indicated by g which is between d and e is evaluated.

  As described above, the search range of the motion candidates is gradually limited, and when the search range falls below a predetermined limit value or when the search is executed a specified number of times, the motion candidate that is the maximum evaluation value is searched and estimated. Output as a value.

According to such an embodiment, since the predicted value by the time series filter unit 31 is set as the initial value, the accuracy of the estimated value can be increased.
Since the search range of the motion candidates is limited to a predetermined range centered on the prediction value by the time series filter unit 31 and the motion candidate that becomes the maximum evaluation value is detected by binary search, the motion candidate that becomes the maximum evaluation value is selected. The processing time until the search can be further reduced.

(Fifth embodiment)
A fifth embodiment will be described with reference to FIGS. In actual driving scenes, the movement of the vehicle changes due to accelerator control or steering operation, and the arrangement of stationary objects in the surrounding environment also dynamically changes. However, in the third and fourth embodiments described above, the number of loops for resetting the estimated value as the initial value for the division matching units 30a to 30c is fixed, and it is predicted that it is not possible to appropriately cope with the fluctuation. Is done.

  In view of this, the present embodiment is characterized in that the number of loops for resetting the estimated value as an initial value can be changed with respect to the division matching units 30a to 30c, and resetting is performed as shown in FIG. A control unit 32 is provided. The reset control unit 32 inputs the estimated value and the evaluation value from the division matching units 30a to 30c, determines whether or not to reset the initial value based on them, and determines to reset the initial value. In this case, the initial value is reset in the division verification units 30a to 30c.

  A method for determining whether or not to reset the initial value will be described. When the evaluation value distribution for the motion candidate as shown in FIG. 15 is obtained, the number of convergence is different for each axis, but by repeating the resetting of the initial value, the true value is reached in the order indicated by the circled numbers in the figure. It is thought to converge.

  Here, as shown in FIG. 16, if the fluctuation range of the obtained evaluation value (Δ evaluation value shown in FIG. 16) is within a predetermined value as a result of resetting the initial value, the exercise having the maximum evaluation value Can be confirmed as a candidate. Therefore, as shown in FIG. 17, in the division verification units 30a to 30c, when the fluctuation range of the evaluation value exceeds the predetermined value, the initial value is reset, and the fluctuation range of the evaluation value is within the predetermined value in all the axes. If this happens, the resetting of the initial value is terminated and the estimated value is finally determined.

  According to such an embodiment, since the number of loops for resetting the initial value can be changed until the exercise candidate having the maximum evaluation value is specified, the loop number for resetting the initial value is fixed. In comparison, it becomes possible to respond dynamically to changes in the surrounding environment, and the accuracy of the estimated value can always be improved.

(Sixth embodiment)
A sixth embodiment will be described with reference to FIG. The present embodiment is characterized in that initial values are set based on position and orientation information from the vehicle control ECU 33.

  As shown in FIG. 18, the collation unit 30 receives position and orientation information based on the vehicle speed and rotation angle from the vehicle control ECU 33 (equivalent to an information output unit and control unit). Is set as the initial value.

  That is, the collation unit 30 determines the x-direction position x = x0, the y-direction position y = y0, and the rotation angle θ = θ0 at the next detection timing based on the position and orientation information from the vehicle control ECU 33. An exercise candidate having the maximum evaluation value in the state set as the initial value at 30c is estimated.

  According to such an embodiment, since the initial value can be appropriately set based on the position and orientation information from the vehicle control ECU 33, the accuracy of the estimated value can be increased.

(Seventh embodiment)
A seventh embodiment will be described with reference to FIGS. 19 and 20. In the first embodiment, the division verification units 30a to 30c are configured to operate in parallel, but in this embodiment, the division verification units 30a to 30c are configured to operate in series.

  As shown in FIG. 19, the collation unit first axis 30 a inputs sensing data from the position data memory 28 and an environment map from the environment map memory 29. The collation unit second axis 30b receives the sensing data from the position data memory 28, the environment map from the environment map memory 29, and the first axis estimated value from the collation unit first axis 30a. The collation unit third axis 30c inputs the sensing data from the position data memory 28, the environment map from the environment map memory 29, and the second axis estimated value from the collation unit second axis 30b. In this case, when both the environment map from the environment map memory 29 and the estimated value from the preceding stage are input in the division matching units 30a to 30c, the estimated value from the preceding stage is set as the initial value. It has become. This is because the estimated value from the previous stage is more accurate than the environmental map.

  As shown in FIG. 20, the collation unit first axis 30a sets y = y0 and θ = θ0 based on the environment map as initial values, fixes y and θ motion candidates, and changes only x to maximize X = x1 is estimated as a motion candidate as an evaluation value.

  Next, the collation unit second axis 30b sets θ = θ0 based on the environment map as an initial value and the first axis estimated value x1 from the collation unit first axis 30a to fix the motion candidates for x and θ, and y Only y = y2 is estimated as the motion candidate that becomes the maximum evaluation value.

  Finally, the collation unit third axis 30c sets the first axis estimated value x = x1 from the collation unit first axis 30a and the second axis estimated value y = y2 from the collation unit second axis 30b as initial values. Then, the motion candidates for x and y are fixed, and only θ is changed to estimate θ = θ3 as the motion candidate having the maximum evaluation value.

  According to such an embodiment, when both the environment map from the environment map memory 29 and the estimated value from the preceding stage are input in the division matching units 30a to 30c, the estimated value from the preceding stage is set as the initial value. Therefore, the accuracy of the estimated value can be improved as compared with the case where the environmental map is set as the initial value.

(Eighth embodiment)
The eighth embodiment will be described with reference to FIG. This embodiment is characterized in that a predicted value from the time series filter unit 31 is input to the matching unit 30 shown in the seventh embodiment instead of the environment map from the environment map memory 29. In this case, when both the predicted value from the time series filter unit 31 and the estimated value from the preceding stage are input in the division matching units 30a to 30c, the estimated value from the preceding stage is set as the initial value. ing. This is because the estimated value from the previous stage side is more accurate than the predicted value by the time series filter unit 31.

  As shown in FIG. 21, the collation unit first axis 30a sets the predicted values y = y0 and θ = θ0 by the time-series filter unit 31 as initial values, fixes y and θ motion candidates, and sets only x. X = x1 is estimated as a motion candidate that is changed to be the maximum evaluation value.

  Next, the collation unit second axis 30b sets the predicted value θ = θ0 by the time series filter unit 31 and the first axis estimated value x = x1 from the collation unit first axis 30a as initial values, and sets x and θ. The motion candidate is fixed, and only y is changed, and y = y2 is estimated as the motion candidate having the maximum evaluation value.

  Finally, the collation unit third axis 30c sets the first axis estimated value x = x1 from the collation unit first axis 30a and the second axis estimated value y = y2 from the collation unit second axis 30b as initial values. The motion candidates for x and y are fixed, and only θ is changed, and θ = θ3 is estimated as the motion candidate having the maximum evaluation value.

  According to such an embodiment, when the predicted value from the time series filter unit 31 and the estimated value from the preceding stage are input in the division matching units 30a to 30c, the estimated value from the preceding stage is set as the initial value. Thus, the accuracy of the estimated value can be increased as compared with the configuration in which the predicted value by the time series filter unit 31 is set as the initial value.

(Ninth embodiment)
A ninth embodiment will be described with reference to FIG. This embodiment is characterized in that position and orientation information is input from the vehicle control ECU 33 in place of the environment map from the environment map memory 29 to the matching unit 30 shown in the seventh embodiment. In this case, when both the position and orientation information from the vehicle control ECU 33 and the estimated value from the preceding stage are input in the division verification units 30a to 30c, the estimated value from the preceding stage is set as the initial value. ing. This is because the estimated value from the preceding stage is more accurate than the position and orientation information from the vehicle control ECU 33.

  As shown in FIG. 22, the collating unit first axis 30a sets the y-direction value y = y0 and the rotation angle value θ = θ0 obtained from the vehicle control ECU 33 as initial values, and moves y and θ. The candidate is fixed, and only x is changed, and x = x1 is estimated as the motion candidate that becomes the maximum evaluation value.

  Next, the collation unit second axis 30b sets the rotation angle value θ = θ0 obtained from the vehicle control ECU 33 as an initial value and the first axis estimated value x = x1 from the collation unit first axis 30a to x. The motion candidate for θ is fixed, and only y is changed, and y = y 2 is estimated as the motion candidate having the maximum evaluation value.

  Finally, the collation unit third axis 30c sets the first axis estimated value x = x1 from the collation unit first axis 30a and the second axis estimated value y = y2 from the collation unit second axis 30b as initial values. The motion candidates for x and y are fixed, and only θ is changed, and θ = θ3 is estimated as the motion candidate having the maximum evaluation value.

  According to such an embodiment, when both the position and orientation information from the vehicle control ECU 33 and the estimated value from the preceding stage are input in the division matching units 30a to 30c, the estimated value from the preceding stage is set as the initial value. Since the position and orientation information from the vehicle control ECU 33 is set as an initial value, the accuracy of the estimated value can be increased.

(10th Embodiment)
A tenth embodiment will be described with reference to FIG. The present embodiment is characterized in that a plurality of collation units 30 in which collation orders of the division collation units 30a to 30c that operate in series are different from each other are provided and operated in parallel.

  When the collation order is set in the division verification units 30a to 30c by operating the division verification units 30a to 30c in series as in the seventh embodiment, when the accuracy of the estimated value (initial value) from the previous stage is low This may affect the result of matching on the subsequent stage.

  Therefore, in this embodiment, as shown in FIG. 23, the first verification unit 30A set so that the division verification units 30a to 30c operate in series and the division verification units 30a to 30c of the first verification unit are as follows. A second verification unit 30B having a different verification order is provided, and the first verification unit 30A and the second verification unit 30B are configured to operate in parallel.

  That is, as the second collation unit 30B, the collation unit third axis 30c, the collation unit first axis 30a, and the collation unit second axis 30b are different from the collation order of the divided collation units 30a to 30c of the first collation unit 30A. Match in order. The selection unit 34 compares the estimated value from the first matching unit 30A and the estimated value from the second matching unit 30B for each variable, and specifies the estimated value with the larger evaluation value as the estimated value corresponding to the variable. .

  According to such an embodiment, the first collation unit 30A and the second collation unit 30B are set so that the collation orders of the division collation units 30a to 30c are different from each other and operate in parallel. The accuracy of the estimated value can be increased as compared with the configuration in which the estimated value is specified only by the matching units 30A and 30B.

(Eleventh embodiment)
The eleventh embodiment will be described with reference to FIG. The present embodiment is characterized in that the collation order of the division collation units 30a to 30c is set in accordance with the order in which the variables constituting the prediction value by the time series filter unit 31 have high accuracy.

  Although the prediction value by the time series filter unit 31 is relatively high in accuracy but low in robustness, once the prediction value deviates greatly, the prediction value thereafter is greatly affected. For this reason, it is necessary to determine that the predicted value by the time series filter unit 31 is significantly different from the original predicted value.

  From such a situation, in order to determine the accuracy of the predicted value by the time series filter unit 31, it is compared with the position and orientation information from the vehicle control ECU 33. That is, although the accuracy of the position / orientation information from the vehicle control ECU 33 is lower than the predicted value by the time series filter unit 31, the robustness is remarkably high, so the predicted value by the time series filter unit 31 and the position / orientation from the vehicle control ECU 33 are It can be estimated that the accuracy of the predicted value by the time-series filter unit 31 has decreased as the difference from the information increases.

  Therefore, in the present embodiment, the predicted value by the time series filter unit 31 and the position / orientation information from the vehicle control ECU 33 are compared for each variable, and the order of the variables is determined in ascending order of the difference (in order of accuracy). The collation order of the division collation units 30a to 30c is set according to the order of the variables.

  That is, when it is assumed that the accuracy of the predicted value set as the initial value by the collation unit first axis 30a is the highest and the accuracy of the prediction value set as the initial value by the collation unit third axis 30c is the lowest, the order selection unit 35 As shown in FIG. 24, the collation order of the collation unit first axis 30a, the collation unit second axis 30b, and the collation unit third axis 30c is determined.

  In the first collation unit first axis 30a, the first axis estimated value is specified in a state where the second axis predicted value and the third axis predicted value with high accuracy of the predicted value by the time series filter unit 31 are set as initial values. That is, since the accuracy of the second axis predicted value and the third axis predicted value is higher than the first axis predicted value, the collation unit first axis 30a uses the second axis predicted value and the third axis predicted value as initial values. Set the value.

  Next, in the collation unit second axis 30b, the second axis estimation value is specified in a state where the predicted value of the third axis from the time series filter unit 31 and the first axis estimation value from the collation unit first axis 30a are set as initial values. To do.

  Finally, in the collation unit third axis 30c, the first axis estimated value from the collation unit first axis 30a and the second axis estimated value from the collation unit second axis 30b are set as initial values. Is identified.

  According to such an embodiment, since the collation order of the division collation units 30a to 30c is set according to the order in which the variables constituting the prediction value by the time series filter unit 31 are highly accurate, the accuracy of the estimated value Can be increased.

(Twelfth embodiment)
A twelfth embodiment will be described with reference to FIGS. In actual driving scenes, the movement of the host vehicle fluctuates due to accelerator control or steering operation, and the arrangement of stationary objects in the surrounding environment also fluctuates dynamically. However, in the above seventh to eleventh embodiments, the collation order of the division collation units 30a to 30c is fixed, and it is predicted that the fluctuation cannot be appropriately handled.

  Therefore, the present embodiment is characterized in that the collation order of the division collation units 30a to 30c can be changed. As a means for that purpose, an order control unit 36 is provided as shown in FIG. The order control unit 36 inputs the evaluation value and the estimated value from the division collation units 30a to 30c, determines the collation order of the division collation units 30a to 30c based on them, and sets the collation order to the division collation units 30a to 30a. 30c.

  A method for determining the collation order will be described. As shown in FIG. 26, when an evaluation value distribution indicating a distribution of evaluation values for a motion candidate is obtained, the rising or falling inclination angle of the evaluation value distribution differs for each axis. In this case, the smaller the inclination angle, the more easily the maximum evaluation value is affected by noise, so it can be said that the accuracy of the motion candidate that is the maximum evaluation value is low. In the example shown in FIG. 26, since the inclination angle of the x-axis is the smallest, the accuracy of the motion candidate that is the maximum evaluation value is the lowest. For this reason, when the motion candidate having the maximum evaluation value is specified from the x-axis, the influence on the subsequent stage is increased. It is desirable to set.

  Here, in order to determine the inclination angle, as shown in FIG. 27, the inclination angle is determined based on the inclination of the hypotenuse of the right triangle having the maximum evaluation value and the predetermined motion candidate as a vertex. The height dimension of the right triangle may be set to 1/2 of the maximum evaluation value, for example, but may be set to 1/3, 2/3, or the like. In this case, since the accuracy of the maximum evaluation value increases as the slope of the right side of the hypotenuse increases, the collation order of the division collation units 30a to 30c is set so that the slope of the hypotenuse has the larger order.

  According to such an embodiment, since the collation order of the division collation units 30a to 30c is set in descending order of the rising or falling inclination angle of the evaluation value distribution, the division collation units 30a to 30c on the subsequent stage side are set. On the other hand, a highly accurate initial value can be set, and the accuracy of the estimated value can always be improved.

(Other embodiments)
The present invention is not limited to the above embodiment, and can be modified or expanded as follows.
In the above embodiment, the sensing data, the environment map, and the motion candidate are configured with three types of variables, but may be configured with four or more types of variables. In this case, if the number of variables is N, the variable to be fixed (second variable) is in the range of 1 to N-1.
When the accuracy of the position and orientation information by GPS is extremely high, the current absolute position and orientation (latitude, longitude, traveling direction) may be corrected based on the position and orientation information.

  In the drawing, 12 is a distance measuring sensor unit (detection unit), 28 is a position data memory (storage unit), 30 is a verification unit (motion candidate setting unit, estimated value output unit), and 30a is a verification unit first axis (division verification) Part), 30b is the collation part 2 axis (division collation part), 30c is the collation part 3 axis (division collation part), 31 is a time series filter part (time series filter part), 32 is a reset control part, 33 Is a vehicle control ECU (detection unit, control unit), 35 is an order selection unit, and 36 is an order control unit.

Claims (15)

A detection unit (12) for detecting sensing data composed of a plurality of variables indicating a relative position and orientation of the moving body with respect to the external environment at a predetermined detection timing;
A storage unit (28) for storing sensing data at the previous detection timing as an environmental map composed of a plurality of variables;
An exercise candidate setting unit (30) for setting an exercise candidate composed of a plurality of variables indicating a position and orientation at which the moving body is enabled at the next detection timing based on the environment map;
A collation unit (30) that collates the sensing data with the motion candidate and estimates a motion candidate that has a maximum evaluation value that indicates the maximum collation accuracy;
An estimated value output unit (30) for outputting, as a position and orientation estimated value, a change amount from the position and orientation indicated by the environment map to the position and orientation indicated by the motion candidate that is the maximum evaluation value;
The verification unit includes a plurality of divided verification units (30a to 30d) in which a first variable that estimates the motion candidate and a second variable that does not estimate the motion candidate are different from each other among the plurality of variables that configure the motion candidate. Configured,
The division verification unit estimates a first variable that is a maximum evaluation value by obtaining an evaluation value by changing the first variable in a state where a predetermined initial value is set as the second variable,
The estimated value output unit is a position / posture estimation apparatus for a moving body that outputs, as the estimated position / posture value, a first variable that is a maximum evaluation value estimated by the division verification unit.   The position / posture estimation apparatus for a moving body according to claim 1, wherein the division verification unit sets a second variable constituting the environmental map as the initial value. A time series filter unit (31) for estimating a predicted value indicating a position and orientation at the next detection timing based on past time series statistical information,
The position / posture estimation apparatus for a moving body according to claim 1, wherein the division verification unit sets a second variable constituting the predicted value as the initial value. An information output unit (33) for outputting position and orientation information indicating the position and orientation at the next detection timing;
The position / posture estimation apparatus for a moving body according to claim 1, wherein the division verification unit sets a second variable indicated by the position / posture information as the initial value.   The position / posture estimation apparatus for a moving body according to claim 1, wherein the division verification unit is provided so as to operate in parallel.   The position / posture estimation apparatus for a moving body according to claim 5, wherein the division verification unit resets a second variable constituting the position / posture estimation value as the initial value.   When the difference between the maximum evaluation value and the previous maximum evaluation value when the division verification unit resets the second variable exceeds a predetermined value, the division verification unit is reset to reset the second variable. The position / orientation estimation apparatus for a moving body according to claim 6, further comprising a reset control unit (32) for instructing. The division verification unit is provided to operate in series,
5. The position / posture estimation apparatus for a moving body according to claim 1, wherein the subsequent-stage division verification unit sets, as the initial value, the first variable estimated by the previous-stage division verification unit.   The division collation unit sets one of the first variable estimated by the division collation unit on the preceding stage and the environment map as the initial value, and sets one of the variables. The position / orientation estimation apparatus for a moving body according to claim 8, wherein when both the estimated first variable and the environmental map are input, the first variable is set. A time-series filter unit (31) for estimating a predicted value indicating a position and orientation at the next detection timing based on past time-series statistical information;
The division verification unit sets one of the first variable estimated by the division verification unit on the previous stage and the prediction value estimated by the time series filter unit as the initial value, The position / orientation estimation apparatus for a moving body according to claim 8, wherein when both the first variable and the predicted value are input, the first variable is set. An information output unit (33) for outputting position and orientation information indicating the position and orientation at the next detection timing;
The division collation unit sets one of the first variable and the position / orientation information estimated by the division collation unit on the previous stage as the initial value, and sets the one variable, The position / posture estimation apparatus for a moving body according to claim 8, wherein the first variable is set when both of the position / posture information are input. A plurality of the collation units are provided, and the order of the second variables set in the division collation unit as the initial value is set to be different from other collation units,
The position of the moving body according to claim 8, wherein the estimated value output unit compares the evaluation value of the position / orientation estimated value for each variable, and outputs a variable having a maximum evaluation value in one variable as the position / orientation estimated value. Posture estimation device. A time series filter unit (31) for estimating a predicted value indicating a position and orientation at the next detection timing based on past time series statistical information set as the initial value;
An information output unit (33) for outputting position and orientation information indicating the position and orientation at the next detection timing;
An order selection unit (35) that compares the predicted value and the position / orientation information for each variable, and selects a collation order in order from a division collation unit in which a variable having a small difference is set as a second variable;
The mobile body position and orientation estimation apparatus according to claim 8, comprising:   The position and orientation of the moving body according to claim 8, further comprising an order control unit (36) that controls a collation order of the division collation units in descending order of the rising or falling inclination angle of the evaluation value distribution with respect to the motion candidate. Estimating device. A detection unit (12) for detecting sensing data composed of a plurality of variables indicating a relative position and orientation of the moving body with respect to the external environment at a predetermined detection timing;
A storage unit (28) for storing sensing data at the previous detection timing as an environmental map composed of a plurality of variables;
An exercise candidate setting unit (30) for setting an exercise candidate composed of a plurality of variables indicating a position and orientation at which the moving body is enabled at the next detection timing based on the environment map;
A collation unit (30) that collates the sensing data with the motion candidate and estimates a motion candidate that has a maximum evaluation value that indicates the maximum collation accuracy;
An estimated value output unit (30) that outputs a change amount from the position and orientation indicated by the environment map to the position and orientation indicated by the motion candidate that is the maximum evaluation value, as a position and orientation estimated value;
A control unit (33) for controlling the movement of the moving body while estimating the current position based on the estimated position and orientation value,
The division verification unit estimates a first variable that is a maximum evaluation value by obtaining an evaluation value by changing the first variable in a state where a predetermined initial value is set as the second variable,
The estimated value output unit is a self-supporting traveling system for a moving body that outputs, as the position and orientation estimated value, a first variable that is a maximum evaluation value estimated by the division verification unit.

Images