JP6171849B2 - Moving body position / posture angle estimation apparatus and moving body position / posture angle estimation method - Google Patents

Description

  The present invention relates to a moving body position / posture angle estimation apparatus and method for estimating the position and posture angle of a moving body using a particle filter.

  Patent Document 1 discloses a technique for calculating the position of a moving object by comparing a three-dimensional map with a captured image of a camera. In this technology, 3D information of image-like feature points is generated within a range imaged by a camera mounted on a moving object, and 3D coordinates where the recording medium on which this 3D information is recorded and the actual image coincide with each other. The position and orientation angle of the moving object were obtained by finding the upper point and direction.

  In this conventional technique, a feature point group that increases the error distribution of the calculated position and posture angle of the moving object is extracted, and the remaining feature points excluding this feature point group are recalculated to perform position and orientation. The calculation accuracy of the corner was improved.

International Publication WO2005 / 038402A1

  However, in the above-described conventional technique, the position and posture angle of the moving body are estimated without considering the arrangement of the edge that is the feature point. There is a problem that the estimation result of the posture angle may include a large error.

Therefore, the present invention has been proposed in view of the above-described circumstances, and the mobile object position / posture angle estimation that can accurately estimate the position and posture angle of the mobile object even when the edges are unevenly present. It is an object of the present invention to provide an apparatus and a moving body position / posture angle estimation method.

  In order to solve the above-described problem, the present invention extracts a pair of edges that intersect in different directions from the edges set on the projection image, and sets the axis of the first direction in the traveling direction of the moving object. The axis of the second direction is set in the lateral direction of the moving body that is orthogonal to the traveling direction of the moving body. Then, the sum of the lengths of the first direction components of all edges on the projection image is compared with the sum of the lengths of the second direction components, and the first sum is obtained when the sum of the lengths of the first direction components is small. The evaluation value is corrected so that the evaluation value on the edge facing the direction increases.

  According to the present invention, the evaluation value can be corrected in consideration of the arrangement of the edges, so that the position and posture angle of the moving object can be accurately estimated even when the edges are unevenly present.

FIG. 1 is a block diagram showing a configuration of a mobile body position / posture angle estimation system including a mobile body position / posture angle estimation apparatus according to the first embodiment of the present invention. FIG. 2 is a flowchart showing a processing procedure of a mobile object position / posture angle estimation process by the mobile body position / posture angle estimation apparatus according to the first embodiment of the present invention. FIG. 3 is a diagram for explaining an evaluation value correction method by the moving object position / posture angle estimation apparatus according to the first embodiment of the present invention. FIG. 4 is a diagram for explaining a method for correcting an evaluation value by the moving body position / posture angle estimation apparatus according to the first embodiment of the present invention. FIG. 5 is a diagram for explaining a method for correcting an evaluation value by the moving body position / posture angle estimation apparatus according to the first embodiment of the present invention. FIG. 6 is a diagram for explaining a method of correcting an evaluation value by the moving body position / posture angle estimation apparatus according to the second embodiment of the present invention.

  Hereinafter, first and second embodiments to which the present invention is applied will be described with reference to the drawings.

[First Embodiment]
[Configuration of moving body position / posture angle estimation system]
FIG. 1 is a block diagram showing a configuration of a mobile body position / posture angle estimation system equipped with a mobile body position / posture angle estimation apparatus according to the present embodiment. However, this embodiment demonstrates the case where it applies to a vehicle as an example of a mobile body.

  As shown in FIG. 1, the moving body position / posture angle estimation system according to the present embodiment includes an ECU 1, a camera 2, a three-dimensional map database 3, and a vehicle sensor group 4.

  Here, the ECU 1 is an electronic control unit configured by a ROM, a RAM, an arithmetic circuit, and the like, and includes the moving body position / posture angle estimation device 10 according to the present embodiment. The ECU 1 may also be used as an ECU used for other controls.

  The camera 2 uses a solid-state imaging device such as a CCD, for example. In this embodiment, the camera 2 is installed on the front bumper of the vehicle so that the optical axis is horizontal and the front of the vehicle can be imaged. The captured image is transmitted to the ECU 1.

  The three-dimensional map database 3 stores three-dimensional position information, and stores, for example, an edge of the surrounding environment including a road surface display. In the present embodiment, at least three-dimensional information of the position and direction of lane markings and curbs indicating road edges is recorded, and in addition to road surface indications such as white lines, stop lines, pedestrian crossings, road marks, etc. Also includes edge information of structures such as buildings. As the data format of the three-dimensional map database 3, each map information such as a road edge is defined as an aggregate of edges. In the case where the edge is a long straight line, for example, since it is divided every 1 m, there is no extremely long edge. In the case of a straight line, each edge has three-dimensional position information indicating both end points of the straight line. In the case of a curve, each edge has three-dimensional position information indicating the end points and the center point of the curve.

  The vehicle sensor group 4 includes a GPS receiver 41, an accelerator sensor 42, a steering sensor 43, a brake sensor 44, a vehicle speed sensor 45, an acceleration sensor 46, a wheel speed sensor 47, and other sensors 48. Yes. The vehicle sensor group 4 is connected to the ECU 1 and supplies various detection values detected by the sensors 41 to 48 to the ECU 1. The ECU 1 uses the output value of the vehicle sensor group 4 to calculate the approximate position of the vehicle or to calculate odometry indicating the amount of movement of the vehicle per unit time.

  The moving body position / posture angle estimation device 10 is a device that estimates the position and posture angle of a vehicle by matching a three-dimensional map data with an image of the surrounding environment of the vehicle using a particle filter. Then, by executing a specific program, it operates as an edge image calculation unit 12, an odometry calculation unit 14, a particle presence distribution range setting unit 16, and a particle scattering unit 18. Further, it operates as a projection image creation unit 20, an evaluation point setting unit 22, a likelihood calculation unit 24, an evaluation value correction unit 25, and a position / orientation angle estimation unit 26.

  The edge image calculation unit 12 acquires an image obtained by capturing the surrounding environment of the vehicle from the camera 2, detects an edge from the image, and calculates an edge image. The image captured by the camera 2 captures at least a lane line and a curb indicating the road edge as road surface information necessary for estimating the position and posture angle of the host vehicle. In addition, a road surface display such as a white line, a stop line, a pedestrian crossing, and a road surface mark may be captured.

  The odometry calculation unit 14 calculates odometry, which is the amount of movement of the vehicle per unit time, using various sensor values obtained from the vehicle sensor group 4.

  The particle presence distribution range setting unit 16 sets the presence distribution range of particles within a predetermined range in the vicinity of the position and posture angle moved by the odometry calculated by the odometry calculation unit 14.

  The particle distribution unit 18 distributes particles within the particle presence distribution range set by the particle presence distribution range setting unit 16.

  The projection image creation unit 20 creates a projection image for each of the particles dispersed by the particle scattering unit 18. For example, a projection image is created by projecting and converting 3D position information such as edges stored in the 3D map database 3 so as to be an image captured by the camera 2 from the position and posture angle of each particle.

  The evaluation point setting unit 22 sets evaluation points for comparing the projection image and the edge image on the projection image.

  The likelihood calculating unit 24 compares the projection image with the edge image, sets an evaluation point on the projection image that matches the edge on the edge image as a target evaluation point, and sets a predetermined evaluation value for the target evaluation point. Then, the likelihood is calculated based on the evaluation value. Here, the likelihood is an index indicating how likely the position and posture angle of each particle is relative to the actual vehicle position and posture angle, and the degree of coincidence between the projection image and the edge image is high. The likelihood is set so as to increase.

  The evaluation value correction unit 25 corrects the evaluation value set by the likelihood calculation unit 24. More specifically, the evaluation value correction unit 25 extracts pairs of edges that intersect in different directions from the edges set on the projection image, and sets the axis in the first direction in the traveling direction of the vehicle. Then, an axis in the second direction is set in the lateral direction of the vehicle orthogonal to the traveling direction of the vehicle. Then, the sum of the lengths of the first direction components of all the edges on the projected image is compared with the sum of the lengths of the second direction components, and the direction in which the sum of the lengths of the pair of edges is smaller is directed. The evaluation value is corrected so that the evaluation value on the edge increases. In particular, in the present embodiment, the evaluation value is corrected so as to increase by increasing the number of evaluation points on the edge of the pair of edges facing the direction in which the total length is small.

  The position / orientation angle estimation unit 26 estimates the position and orientation angle of the vehicle based on the likelihood calculated by the likelihood calculation unit 24. For example, the position and posture angle of the particle having the highest likelihood are calculated as the estimation result of the actual position and posture angle of the vehicle.

  In this embodiment, the position of six degrees of freedom (front-rear direction, vehicle width direction, vertical direction) and posture angle (roll, pitch, yaw) of the vehicle are obtained. However, the present technology can be applied even when estimating a position (front-rear direction, lateral direction) and posture angle (yaw) with three degrees of freedom, such as an automatic guided vehicle used in a factory without a suspension or the like. Is possible. Specifically, in such a vehicle, the vertical position and posture angle roll and pitch are fixed, so these parameters may be measured and used in advance.

[Procedure for estimating the position and orientation angle of a moving object]
Next, the procedure of the position / orientation angle estimation process of the moving body according to the present embodiment will be described with reference to the flowchart of FIG. In the present embodiment, the position and posture angle of a total of six degrees of freedom of roll, pitch, and yaw are estimated as traveling direction, lateral direction (vehicle width direction), vertical direction, and posture angle information as vehicle position information. However, the roll is a rotation direction with the traveling direction of the vehicle as an axis, the pitch is a rotation direction with the lateral direction of the vehicle as an axis, and the yaw is a rotation direction with the vertical direction of the vehicle as an axis.

  Moreover, the position / orientation angle estimation process of the moving body described below is continuously performed at intervals of, for example, about 100 msec.

  As shown in FIG. 2, first, in step S <b> 110, the edge image calculation unit 12 calculates an edge image from the image of the camera 2. An edge in the present embodiment refers to a portion where the luminance of a pixel changes sharply. As an edge detection method, for example, the Canny method can be used. In addition, various methods such as differential edge detection may be used.

  In addition, it is desirable that the edge image calculation unit 12 also extracts the brightness change direction of the edge, the color near the edge, and the like from the image of the camera 2. Thereby, in step S160 and step S170, which will be described later, it is possible to set the likelihood using information other than these edges recorded in the 3D map database 3 and calculate the position and attitude angle of the host vehicle. It becomes possible.

  Next, in step S120, the odometry calculation unit 14 calculates odometry, which is the amount of movement of the vehicle from the time calculated in step S120 one loop before, based on the sensor value obtained from the vehicle sensor group 4. . In the case of the first loop after starting the program, the odometry is calculated as zero.

  The odometry calculation unit 14 calculates the turning amount (turning angle) in the yaw angle direction from the difference between the encoder values of the wheel speed sensors 47 for the left and right wheels, while limiting the vehicle motion to a plane as a method for calculating the odometry. The moving amount in the traveling direction and the lateral direction is obtained by calculating an average moving amount from the encoder value of the wheel speed sensor 47 of each wheel, and taking the cosine and sine of the turning angle in the yaw angle direction with respect to this moving amount. Can be sought. Further, the movement amount and the rotation amount per unit time may be calculated from the wheel speed and yaw rate of each wheel. Further, the wheel speed may be substituted by the difference between the vehicle speed and the positioning value of the GPS receiver 41, and the yaw rate may be substituted by the steering angle. Various calculation methods can be considered as the odometry calculation method, but any method may be used as long as odometry can be calculated.

  In particular, in order to calculate the amount of movement in the lateral direction and yaw angle direction more accurately, for example, at very low speeds of about 10 km / h or less, Ackerman steering geometry (automobile motion and control Chapter 3, written by Masato Abe, Sankaido Odometry may be obtained according to (Issuance). In addition, the equation of motion of a linear two-wheel model that can take into account the skid angle of the tire at higher vehicle speeds (motor motion and control, Chapter 3, Section 3.2.1, pages 56 (3.12) and (3.13), written by Masato Abe, It is even better to calculate using Sankai-do. However, when using the equation of motion of the linear two-wheel model, it is necessary to measure tire parameters (cornering parameters) of each wheel in advance.

  Next, in step S130, the particle presence distribution range setting unit 16 moves the position and posture angle of each particle estimated in step S170 one loop before by the odometry calculated in this step S120. However, in the case of the first loop after starting the program, there is no previous vehicle position information, so the data from the GPS receiver 41 included in the vehicle sensor group 4 is used as the initial position information. Also, the vehicle position and posture angle calculated last when the vehicle stopped last time may be stored, and the initial position and posture angle information may be stored.

  Then, a particle existence distribution range is set in the vicinity of the position and posture angle of the vehicle moved by the odometry in consideration of the dynamic characteristics and the running state of the vehicle.

  The particle presence distribution range setting unit 16 can take the true value of the vehicle position and posture angle in consideration of the measurement error of the vehicle sensor group 4, the odometry error caused by the communication delay, and the vehicle dynamic characteristics. A plurality of possible prediction positions and posture angle candidates are generated. The predicted position and posture angle candidates are set at random using a random number table or the like within the upper and lower limits of the error for the six-degree-of-freedom parameters of position and posture angle. To do. In the present embodiment, 500 candidates for the predicted position and posture angle are created. And the upper and lower limits of the error for the parameters of 6 degrees of freedom of predicted position and posture angle are ± 0.05 [m], ± 0.05 [m], ± 0.05 [m], ± 0.5 [deg], ± 0.5 [deg] in this order. , ± 0.5 [deg].

  The number of the predicted position and attitude angle candidates to be created and the upper and lower limits of the error for the six-degree-of-freedom parameters of position and attitude angle are changed as appropriate by detecting or estimating the driving state of the vehicle and the road surface condition. . For example, when a sudden turn or slip occurs, the error in the plane direction (traveling direction, lateral direction, yaw) is likely to increase, so the upper and lower limits of the error of these three parameters are increased. In addition, the number of predicted positions to be created is increased.

  Next, in step S140, the particle distribution unit 18 distributes particles within the presence distribution range set in step S130. As a spraying method, random parameters are set using a random number table or the like within the range (upper and lower limits) set in step S130 for the six-degree-of-freedom parameters that define the position and posture angle of particles. In the present embodiment, 500 particles are always created.

  Note that particles may be dispersed using the technique disclosed in Japanese Patent Application Laid-Open No. 2010-224755. In this case, it is calculated how much the position and posture angle of the vehicle moved by the odometry in step S130 is corrected by the position and posture angle calculated in step S170, and particles are determined according to this correction amount. What is necessary is just to set and distribute the average and distribution to spread. At this time, the range in which the particles are dispersed is the presence distribution range set in step S130. Also, the existence distribution range may be obtained using the technique disclosed in Japanese Patent Application Laid-Open No. 2010-224755 and ORed with the existence distribution range set in step S130. Further, the number of particles to be dispersed may be determined dynamically according to the presence distribution range set in step S130 using the technique disclosed in Japanese Patent Application Laid-Open No. 2010-224755.

  Next, in step S150, the projection image creation unit 20 creates a projection image (virtual image) for each of the plurality of particles dispersed in step S140. At this time, the evaluation point setting unit 22 sets the evaluation point by projecting and converting, for example, 3D position information such as an edge stored in the 3D map database 3 into a camera image at each predicted position and posture angle candidate. To do. Thereby, a projection image for evaluation is created. The evaluation point group projected on the projection image is compared with the edge on the edge image calculated in step S110 in step S160 described later.

  In addition, the method of extracting edge information around the host vehicle calculates the distance D (unit: m) between each edge recorded in the three-dimensional map database 3 and the camera 2, and this distance is equal to or less than a threshold value Dth (m). Extract edge information only. Strictly speaking, it is desirable to calculate the distance by using the center point of each edge and the position of the camera 2. However, the calculation cost may be reduced by substituting the end point of the edge and the vehicle position. Further, since only the imaging range of the camera 2 is matched, only the edge of the range that can be captured by the camera 2 may be extracted. However, the position and posture angle obtained in step S130 are used as the vehicle position and posture angle used at this time. That is, the position and posture angle that are moved by the odometry calculated in step S120 from the vehicle position estimated in step S170 one loop before are used.

  Further, in the projection conversion, an external parameter indicating the position of the camera 2 and an internal parameter of the camera 2 are required. The external parameter may be calculated from the predicted position and the posture angle candidate by measuring the relative position from the vehicle to the camera 2 in advance. The internal parameters may be calibrated in advance.

  In step S110, when the edge luminance change direction, the color near the edge, and the like are extracted from the camera image, it is desirable to create a projection image using them.

  Next, in step S160, the likelihood calculating unit 24 compares the edge image calculated in step S110 with the projection image created in step S150 for each of the plurality of particles scattered in step S140. Then, based on the comparison result, the likelihood is calculated for each particle that is a predicted position and posture angle candidate. The likelihood is an index indicating how likely each predicted position and posture angle candidate is to the actual vehicle position and posture angle, and the higher the degree of coincidence between the projection image and the edge image, the higher the likelihood. The likelihood is set to be high. An example of how to obtain this likelihood will be described below.

  First, an evaluation point is specified on the projection image, and it is determined whether or not an edge on the edge image exists at this evaluation point. If the presence or absence of an edge matches, an evaluation score is set as a target evaluation score, and an evaluation value 1 is set for the target evaluation score. On the other hand, if the presence or absence of an edge does not match, 0 is set as the evaluation value. Next, the set evaluation value is corrected by the evaluation value correction unit 25, and such processing is performed on all evaluation points, and the number of coincidence evaluation points, which is the sum of the corrected evaluation values, is obtained. After the number of coincidence evaluation points is calculated for each particle that is a candidate for all predicted positions and posture angles, the likelihood is calculated by normalization so that the total value is 1. In addition, since many other calculation methods can be considered as the calculation method of likelihood, you may use those methods.

  Here, a method of correcting the evaluation value by the evaluation value correcting unit 25 will be described. The evaluation value correction unit 25 extracts a pair of edges that intersect in different directions from the edges set on the projection image, sets an axis in the first direction in the traveling direction of the vehicle, and sets the traveling direction of the vehicle The axis of the second direction is set in the lateral direction of the vehicle orthogonal to Then, the sum of the lengths of the first direction components of all the edges on the projected image is compared with the sum of the lengths of the second direction components, and the direction in which the sum of the lengths of the pair of edges is smaller is directed. Correction is performed so that the evaluation value on the edge increases. In particular, in the present embodiment, the evaluation value is corrected so as to increase by increasing the number of evaluation points on the edge of the pair of edges facing the direction in which the total length is small.

  However, in the example shown below, when the sum of the lengths of the first direction components is small, the evaluation value on the edge facing the first direction is corrected so as to increase, and particularly on the edge facing the first direction. By increasing the number of evaluation points, the evaluation value is corrected so as to increase.

  Here, with reference to FIG. 3, the evaluation value correction method by the evaluation value correction unit 25 will be specifically described. As illustrated in FIG. 3, a case where the host vehicle 31 travels on a road and edges E1 to E5 exist within the imaging range W of the camera 2 will be described.

  The evaluation value correction unit 25 extracts a pair of edges that intersect in different directions from the edges set in the imaging range W of the projection image. For example, E1 and E3, E2 and E4, etc. are extracted as edge pairs. Here, the two edges constituting the pair of edges need not be parallel but may be linearly independent. Therefore, the edge formed by the white line at the left end of the road and the edge formed by the white line at the right end of the road do not form an edge pair.

  In FIG. 3, an axis in the first direction is set in the X-axis direction that is the traveling direction of the vehicle, and an axis in the second direction is set in the Y-axis direction that is the lateral direction (vehicle width direction) of the vehicle. However, the north-south direction parallel to the ground may be used as the first direction axis, and the east-west direction parallel to the ground may be used as the second direction axis, or other directions may be used.

Then, the total length Xa (unit: m) of the lengths of the first direction components of all edges in the projection image and the total length Ya (unit: m) of the lengths of the second direction components are obtained. In FIG. 3, the edges in the edge image are E1 to E5.
Xa = X1 + X2
Ya = Y1 + Y2 + Y3
It becomes.

  Then, the evaluation value correction unit 25 sets the number of evaluation points Ni (rounded up after the decimal point) of the i-th edge Ei based on the following equation (1).

Ni = max [{Xi / (Xa / Na)}, {Yi / (Ya / Na)}, 2]
= Max [{(Xi · Na) / Xa}, {(Yi · Na) / Ya}, 2] (1)
Here, in the formula (1), Na is the total number of evaluation points in each direction, and is 1000 as a standard in this embodiment. Xi is the length component (unit: m) of the i-th edge Ei in the X-axis direction, and Yi is the length component (unit: m) of the i-th edge Ei in the Y-axis direction. The number Ni of evaluation points is set to 2 or more in order to always set evaluation points at the end points of the edge.

Specifically, if the number N1 of evaluation points is calculated for the edge E1 in FIG. 3 using the equation (1), Xi = X1 and Yi = 0, so that N1 is N1 = max [{(X1 · Na) / Xa}, {(0 · Na) / Ya}, 2]
= (X1 · Na) / Xa
It becomes. However, in the case of (X1 · Na) / Xa <2, N1 = 2.

  As described above, among the total length Xa of the first direction component and the total length Ya of the second direction component, the smaller value of Xa, Ya is (Xi · Na) / Xa, (Yi · Na) / The value of Ya increases. Therefore, the number Ni of evaluation points is based on a smaller value of the total lengths Xa and Ya by comparing the total length Xa of the first direction components with the total length Ya of the second direction components. Will be set.

  As a result, many evaluation points are set in the direction in which the total length is smaller, and fewer evaluation points are set in the direction in which the total length is large. It is possible to increase the number of evaluation points on the edge. Thereby, it can correct | amend so that the evaluation value on the edge which faced the direction where the sum total of length is small may increase.

  In the example of FIG. 3, since the total sum of the lengths of the first direction components is small, in order to correct the evaluation value on the edge facing the first direction to increase, in particular on the edge facing the first direction. Increase the number of evaluation points. Instead of increasing the number of evaluation points in the direction in which the total length is small, the number of evaluation points in the direction in which the total length is large may be decreased.

  By setting the number of evaluation points in this way, the number of evaluation points can be equally arranged on the edge in the X-axis direction and the edge in the Y-axis direction regardless of the length.

  However, without using the above-described formula (1), for example, when Xa is smaller than Ya, Xi increases the number of evaluation points of edge Ei longer than Yi by one, or increases by a predetermined number. The number of evaluation points may be increased by a simple method.

  And when setting an evaluation point on the edge Ei of length li (unit: m) by the number Ni of the evaluation points calculated by the equation (1), an interval Spi (unit :) is expressed by the following equation (2). m) is obtained, and evaluation points are arranged on the edge Ei at the interval Spi.

Spi = li / (Ni-1) (2)
In this way, the evaluation value correction unit 25 can set an evaluation point on the edge Ei.

  In addition to the direction parallel to the ground, another axis (Z axis) is set in the vertical direction of the vehicle so that the number of evaluation points is uniform in the X axis, Y axis, and Z axis directions. May be. In this case, the number of evaluation points may be obtained by replacing equation (1) as shown in equation (3).

Ni = max [{Xi ÷ (Xa ÷ Na)}, {Yi ÷ (Ya ÷ Na)}, {Zi ÷ (Za ÷ Na)}, 2] (3)
Here, Za is the total length (unit: m) of the Z-axis direction components of all edges in the projection image, and Zi is the length component (unit: m) of the i-th edge Ei in the Z-axis direction.

  In the above-described embodiment, the case where the first direction is the X axis and the second direction is the Y axis has been described. However, the first direction and the second direction may be the Y axis and the Z axis, respectively. An axis may be used.

  If the density of evaluation points exceeds the resolution of the projected image, evaluation points that exceed the resolution may be thinned out.

  Furthermore, since the edge closer to the own vehicle (camera 2) is shown larger on the screen, the number of evaluation points obtained by the equation (1) or (3) is increased for the edge near the own vehicle, and the distant The edges may be reduced. For example, for an edge 10 m ahead, the number of evaluation points may be set to 1/10, which is the reciprocal thereof, the magnification of the increase / decrease is taken on the vertical axis, and the distance may be calculated with reference to a map on the horizontal axis. .

  Next, with reference to FIGS. 4 and 5, effects of the moving body position / posture angle estimation apparatus 10 according to the present embodiment will be described.

  FIG. 4 shows an example in an outdoor parking lot where there are many white line edges in the lateral direction of the host vehicle and few edges exist in the traveling direction of the host vehicle. In this case, evaluation points are set at equal intervals on the edge on the white line, and the position and posture angle are matched by matching the projected image and the edge image each time while appropriately changing the position and posture angle of the host vehicle. Is estimated.

  At this time, if the matched evaluation points are indicated by black circles and the unmatched evaluation points are indicated by white circles, as shown in FIG. 4A, the projection image and the edge image are superimposed with the position and posture angle being close to true values. In this case, it can be seen that matching is made at all evaluation points.

  On the other hand, when the position of the host vehicle deviates from the true value by the same distance in the forward direction or the right direction, it changes as shown in FIGS. 4B and 4C, respectively. In other words, in spite of being shifted by the same distance, not all the evaluation points are matched in FIG. On the other hand, in FIG. 4C shifted in the right direction, matching is performed at most evaluation points.

  This is because an extremely large number of edges are present in the lateral direction and hardly exist in the traveling direction. Therefore, if the evaluation points are set evenly with respect to the length of the edge, the position and posture angle of the host vehicle can be accurately estimated in the situation where the edges are unevenly present as shown in FIG. I can't. In particular, in FIG. 4, even if there is an error in the horizontal direction, it is difficult to detect, so it is difficult to accurately detect the position in the horizontal direction.

  Therefore, in the present embodiment, the evaluation value correction unit 25 compares the sum of the lengths of the first direction components of all the edges with the sum of the lengths of the second direction components, and the direction in which the sum of the lengths is small. The evaluation value is corrected so as to increase by increasing the number of evaluation points. For example, as shown in FIG. 4, when the sum of the lengths of the traveling direction components of the vehicle is small, correction is made to increase the number of evaluation points in the traveling direction of the vehicle.

  More specifically, as shown in the enlarged view of FIG. 5A, evaluation points exist only at the end points of the edges in the traveling direction of the vehicle. In the present embodiment, the evaluation points are shown in FIG. 5B. Thus, two evaluation points are increased with respect to the edge facing the traveling direction of the vehicle.

  As a result, even when the edges are biased in a certain direction, it is possible to accurately estimate the position and posture angle of the moving body by adjusting the number of evaluation points. Further, since the unevenness of the evaluation points can be reduced, the sensitivity of matching with respect to a change in the lateral position of the vehicle approaches the sensitivity in the traveling direction of the vehicle, and the accuracy and stability of matching can be improved.

  Next, in step S170, the position / orientation angle estimation unit 26 calculates the final position and orientation angle of the vehicle using the plurality of predicted position and attitude angle candidates having the likelihood information calculated in step S160. For example, the position / orientation angle estimation unit 26 calculates the predicted position and attitude angle candidate with the highest likelihood as the actual position and attitude angle of the vehicle. Further, a weighted average of the predicted position and posture angle may be obtained using the likelihood of each predicted position and posture angle candidate, and the values may be used as the final vehicle position and posture angle. When the estimation result of the position and posture angle of the vehicle is thus calculated, the position / posture angle estimation process of the moving body according to the present embodiment is terminated.

[Effect of the first embodiment]
As described above in detail, in the mobile object position / posture angle estimation device according to the present embodiment, the sum of the lengths of the first direction components and the length of the second direction components of all edges on the projection image Compare Then, the evaluation value is corrected so that the evaluation value on the edge facing the direction in which the total length of the pair of edges is small is increased. As a result, the evaluation value can be corrected in consideration of the arrangement of the edges, so that the position and posture angle of the moving body can be accurately estimated even when the edges are biased.

  Further, in the mobile object position / posture angle estimation apparatus according to the present embodiment, the evaluation value increases by increasing the number of evaluation points on the edge facing the direction in which the total length of the pair of edges is small. The evaluation value is corrected. This makes it possible to equalize the number of evaluation points in the direction with the larger total length and the number of evaluation points in the smaller direction, so that the position and posture angle of the moving object can be accurately estimated even when the edges are biased. can do.

[Second Embodiment]
Next, a moving body position / posture angle estimation apparatus according to a second embodiment of the present invention will be described with reference to the drawings.

[Configuration of moving body position / posture angle estimation system]
Since the configuration of the mobile object position / orientation angle estimation system equipped with the mobile object position / orientation angle estimation device according to this embodiment is the same as that of the first embodiment shown in FIG. Omitted.

  However, in the present embodiment, the function of the evaluation value correction unit 25 is different from that of the first embodiment. The evaluation value correction unit 25 according to the present embodiment sets an addition point for an evaluation point on an edge that faces a direction in which the sum of lengths is small among pairs of edges that intersect in different directions. The evaluation value is corrected so that the value increases. This addition point is set based on the reciprocal of the sum of the edge lengths.

[Procedure for estimating the position and orientation angle of a moving object]
In the position / orientation angle estimation process of the moving body according to the present embodiment, the evaluation value correction method executed in step S160 of FIG. 2 is different from that of the first embodiment. In the first embodiment, the evaluation value correction unit 25 increases the number of evaluation points. However, in this embodiment, an additional point is set for the evaluation points on the edge facing the direction in which the total length is small. To do.

  In step S150 of FIG. 2, the evaluation point setting unit 22 first sets an evaluation point on the edge of the projection image. At this time, in this embodiment, the case where the number of evaluation points is set equally according to the length of the edge will be described. However, as described in the first embodiment, the direction in which the sum of the lengths of the edges is small is described. It may be set to increase the number of evaluation points.

  After the evaluation points are set, in step S160, the likelihood calculating unit 24 compares the projection image with the edge image, and sets an evaluation value for the evaluation point on the projection image that matches the edge on the edge image. Set. For example, 1 is set when the presence or absence of an edge matches, and 0 is set when they do not match.

  Here, the evaluation value correction unit 25 corrects the set evaluation value. More specifically, the evaluation value correction unit 25 compares the sum of the lengths of the first direction components of all the edges on the projection image with the sum of the lengths of the second direction components, and determines the pair of edges. An addition point is set for an evaluation point on an edge facing a direction in which the total length is small.

  However, in the example shown below, when the sum of the lengths of the first direction components is small, the evaluation value on the edge facing the first direction is corrected so as to increase, and particularly on the edge facing the first direction. By setting an addition point for the evaluation point, the evaluation value is corrected so as to increase.

  This adding point setting method will be described with reference to FIG. 3 of the first embodiment as an example. The evaluation value correcting unit 25 sets the likelihood adding point Δi based on the following equation (4).

Δi = max [(Xi ÷ Xa), (Yi ÷ Ya)] (4)
However, Xa is the total length (unit: m) of the first direction components of all edges in the projection image, and Ya is the total length (unit: m) of the second direction components. Xi is the length component (unit: m) of the i-th edge Ei in the X-axis direction, and Yi is the length component (unit: m) of the i-th edge Ei in the Y-axis direction.

  As described above, since the addition point is set according to the reciprocal of the total length Xa of the first direction component and the total length Ya of the second direction component, the addition point Δi is the sum of the lengths Xa and Ya. It is set based on the smaller value. As a result, the addition points in the direction in which the total length is small are increased, so that the evaluation value on the edge facing the direction in which the total length is small in the pair of edges can be increased.

  In the example of FIG. 3, since the total sum of the lengths of the first direction components is small, in order to correct the evaluation value on the edge facing the first direction to increase, in particular on the edge facing the first direction. Add points for evaluation points.

  However, even if Equation (4) described above is not used, for example, when Xa is smaller than Ya, a simple method of doubling the addition point at the evaluation point on edge Ei where Xi is longer than Yi. The evaluation value may be corrected.

  When the addition point Δi is thus set, the likelihood calculating unit 24 calculates the likelihood by adding the addition point Δi to each evaluation point.

  Further, another axis (Z axis) may be set not only in the direction parallel to the ground but also in the vertical direction of the vehicle. In this case, the addition point Δi may be obtained by replacing equation (4) as shown in equation (5).

Δi = max [(Xi ÷ Xa), (Yi ÷ Ya), (Zi ÷ Za)] (5)
Here, Za is the total length (unit: m) of the Z-axis direction components of all edges in the projection image, and Zi is the length component (unit: m) of the i-th edge Ei in the Z-axis direction.

  Here, a method of setting the above-described addition point will be described with reference to FIG. As described with reference to FIG. 4 of the first embodiment, when there are an extremely large number of edges in the lateral direction of the vehicle and few in the traveling direction of the vehicle, the length of the edge is evaluated equally. If a point is set, the position and posture angle of the host vehicle cannot be accurately estimated.

  Therefore, in this embodiment, the evaluation value correction unit 25 sets the addition point so that the evaluation value on the edge facing the direction in which the total length is small increases.

  For example, as shown in the enlarged view of FIG. 6, only the evaluation points P1 and P2 are present at the end points of the edge facing the traveling direction of the vehicle. In the embodiment, the addition point Δi is set based on the above-described equation (4). The likelihood calculating unit 24 calculates the likelihood by adding the addition point Δi when calculating the likelihood.

  By doing so, the likelihood is calculated to be large at the evaluation point on the edge facing the direction in which the total length is small. Therefore, even when the edge is biased in one direction, the position and posture angle of the moving body can be accurately estimated.

[Effects of Second Embodiment]
As described above in detail, in the mobile object position / orientation angle estimation device according to the present embodiment, an addition point is set for an evaluation point on an edge that faces a direction in which the total sum of lengths of the pair of edges is small. Thus, the evaluation value is corrected so as to increase. As a result, the likelihood of the evaluation points on the edges facing the direction in which the sum of the lengths is small increases, so that the position and posture angle of the moving object can be accurately estimated even when the edges are biased. it can.

  Further, in the mobile object position / posture angle estimation device according to the present embodiment, since the addition point is set based on the reciprocal of the sum of the lengths of the edges, the evaluation points on the edges facing the direction in which the sum of the lengths is small The likelihood of can be increased. As a result, the position and posture angle of the moving body can be accurately estimated even when the edges are unevenly present.

  The above-described embodiment is an example of the present invention. For this reason, the present invention is not limited to the above-described embodiment, and even if it is a form other than this embodiment, as long as it does not depart from the technical idea of the present invention, it depends on the design and the like. Of course, various modifications are possible.

1 ECU
2 camera 3 3D map database 4 vehicle sensor group 10 moving body position / posture angle estimation device 12 edge image calculation unit 14 odometry calculation unit 16 particle presence distribution range setting unit 18 particle scattering unit 20 projection image generation unit 22 evaluation point setting unit 24 Likelihood calculation unit 25 Evaluation value correction unit 26 Position / attitude angle estimation unit 41 GPS receiver 42 Accelerator sensor 43 Steering sensor 44 Brake sensor 45 Vehicle speed sensor 46 Acceleration sensor 47 Wheel speed sensor 48 Other sensors

Claims (5)

An edge image is calculated from an image of the surrounding environment of the moving object, a projection image is created for each of the dispersed particles using a particle filter, and the likelihood is calculated by comparing the projection image with the edge image. A mobile body position / posture angle estimation device that estimates the position and posture angle of the mobile body by:
An evaluation point setting unit for setting an evaluation point on the projection image when creating the projection image;
The projection image is compared with the edge image, and an evaluation point on the projection image that matches the edge on the edge image is set as a target evaluation point, and a predetermined evaluation value is set for the target evaluation point. A likelihood calculating unit for calculating likelihood based on the evaluation value;
An evaluation value correction unit that corrects the evaluation value set by the likelihood calculation unit,
The evaluation value correction unit extracts a pair of edges that intersect in different directions from the edges set on the projection image, sets an axis in a first direction as the traveling direction of the moving body, and An axis in the second direction is set in the lateral direction of the moving body perpendicular to the traveling direction of the moving body, and the sum of the lengths of the first direction components and the lengths of the second direction components of all edges on the projection image The evaluation value is corrected so that the evaluation value on the edge facing the first direction increases when the total sum of the lengths of the first direction components is small. Mobile body position / posture angle estimation device.   The mobile object according to claim 1, wherein the evaluation value correction unit corrects the evaluation value so as to increase by increasing the number of evaluation points on the edge facing the first direction. Position and orientation angle estimation device.   The evaluation value correction unit corrects the evaluation value so as to increase by setting an addition point with respect to an evaluation point on an edge facing the first direction. The moving body position / posture angle estimation device according to claim 1.   The mobile object position / posture angle estimation apparatus according to claim 3, wherein the addition point is set based on a reciprocal of a total sum of edge lengths. An edge image is calculated from an image of the surrounding environment of the moving object, a projection image is created for each of the dispersed particles using a particle filter, and the likelihood is calculated by comparing the projection image with the edge image. A mobile body position / posture angle estimation method of a mobile body position / posture angle estimation apparatus for estimating the position and posture angle of the mobile body by
When creating the projection image, set an evaluation point on the projection image,
The projection image is compared with the edge image, and an evaluation point on the projection image that matches the edge on the edge image is set as a target evaluation point, and a predetermined evaluation value is set for the target evaluation point. Correct,
When correcting the evaluation value, a pair of edges that intersect in different directions is extracted from the edges set on the projection image, and an axis in the first direction is set in the traveling direction of the moving body, An axis in the second direction is set in the lateral direction of the moving body orthogonal to the traveling direction of the moving body, and the sum of the lengths of the first direction components of all edges on the projection image and the length of the second direction component are set. The evaluation value is corrected so that the evaluation value on the edge facing the first direction increases when the total length of the first direction component is small. A moving body position / posture angle estimation method.