JP2013186718A - Mobile object position attitude estimation device and method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a mobile object position attitude estimation device and method capable of accurately estimating a position of a mobile object and an attitude angle.SOLUTION: The mobile object position attitude estimation device detects a traveling state of its own vehicle V (S1); when the traveling state becomes an acceleration state or a constant speed state, enhances an image likelihood for attitude angle estimation indicating a suitable degree as an image for estimating an attitude angle of a mobile object in a front photographed image, and enhances an image likelihood for position estimation indicating a suitable degree as an image for estimating a position of the mobile object in a lateral photographed image (S6); on the basis of a coincidence degree between a photographed image where the image likelihood for position estimation, out of the front photographed image and the lateral photographed image, becomes a predetermined value or more and a virtual image which is projected in the same direction as the photographed image, estimates an actual position of the mobile object, and on the basis of a coincidence degree between a photographed image where the image likelihood for attitude angle estimation, out of the front photographed image and the lateral photographed image, becomes a predetermined value or more and a virtual image which is projected in the same direction as the photographed image, estimates the actual attitude angle of the mobile object (S7-S10).

Description

  The present invention relates to a moving object position / posture estimation apparatus and method for estimating the position of a moving object.

  As a technique for calculating the position of a moving object by comparing a three-dimensional map with a captured image of a camera, for example, a technique described in Patent Document 1 below is known. This patent document 1 describes an edge image extracted from an actual image obtained by an in-vehicle camera provided in a vehicle and a three-dimensional map in which the position and shape of the edge of the surrounding environment are recorded in three dimensions. The position and posture angle of the in-vehicle camera are adjusted so that the virtual image when projected onto the camera matches the virtual image. As a result, the position and posture angle of the in-vehicle camera in the three-dimensional space are estimated.

JP 2009-199572

  However, in Patent Document 1, even if the actual video and the virtual video match, if the matching location is far from the in-vehicle camera, there may be a large error in the position of the in-vehicle camera. Conversely, if the matched position is close to the in-vehicle camera, the error in the attitude angle of the in-vehicle camera may be large.

  Therefore, the present invention has been proposed in view of the above-described circumstances, and an object thereof is to provide a moving object position / posture estimation apparatus and method that can accurately estimate the position and posture angle of a moving object. .

  In the present invention, when the traveling state is acceleration or constant speed, the posture angle estimation image likelihood is increased for the front captured image, the position estimation image likelihood is increased for the side captured image, and the position estimation image likelihood is increased. The position is estimated based on the degree of coincidence between the captured image whose degree is equal to or greater than a predetermined value and the virtual image, and based on the degree of coincidence between the captured image whose attitude angle estimation image likelihood is equal to or greater than the predetermined value and the virtual image, The posture angle of the moving object is estimated.

  According to the present invention, the posture angle of the host vehicle can be estimated using the front captured image, and the position of the host vehicle can be estimated using the side captured image. Therefore, the position and posture angle of the moving object can be estimated with high accuracy. Can do.

It is a block diagram which shows the structure of the moving object position and orientation estimation apparatus shown as embodiment of this invention. It is a top view which shows the direction of the own vehicle. It is a figure which shows the angle of view and imaging direction of a camera. (A) is a top view of the host vehicle on which the front camera and the left side camera are mounted, and (b) is a top view of the host vehicle on which the front camera and the right side camera are mounted. (A) is a top view of the host vehicle on which the front camera, left side camera, and rear camera are mounted, and (b) is a top view of the host vehicle on which the front camera, right side camera, and rear camera are mounted. It is a top view of the own vehicle carrying a front camera, a left side camera, and a right side camera. (A) is a top view of the host vehicle on which a front camera, a left side camera, a right side camera, and a rear camera are mounted, and (b) is a top view of the host vehicle on which an omnidirectional camera is mounted. (A) is a captured image, (b) is an edge image of (a), (c) is a virtual image, and (d) is a virtual image when the virtual position of (c) is shifted rightward. It is a flowchart which shows the operation | movement procedure of the moving object position and orientation estimation apparatus shown as embodiment of this invention. In the moving object position and orientation estimation apparatus shown as an embodiment of the present invention, it is a diagram showing the relationship between the vehicle speed and the angle of view of the front-rear camera. In the moving object position and orientation estimation apparatus shown as an embodiment of the present invention, it is a diagram showing the relationship between the vehicle speed and the angle of view of the left and right lateral camera. In the moving object position and orientation estimation apparatus shown as an embodiment of the present invention, it is a diagram showing the posture estimation image likelihood of the front captured image and the left captured image with respect to the running state. In the moving object position and orientation estimation apparatus shown as an embodiment of the present invention, it is a diagram showing position estimation image likelihood of the front captured image and the left side captured image with respect to the running state. In the moving object position / posture estimation apparatus shown as the embodiment of the present invention, it is a diagram illustrating posture likelihood estimation image likelihood of a front captured image, a rear captured image, a left captured image, and a right captured image with respect to a running state. In the moving object position and orientation estimation apparatus shown as an embodiment of the present invention, it is a diagram showing position estimation image likelihood of the front captured image, the rear captured image, the left side captured image and the right side captured image with respect to the running state.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

  A moving object position and orientation estimation apparatus shown as an embodiment of the present invention is configured as shown in FIG. This moving object position / posture estimation apparatus includes an ECU 1, a camera group 2, a three-dimensional map database 3, and a vehicle sensor group 4. 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 such as a yaw rate sensor. The ECU 1 is actually composed of a ROM, a RAM, an arithmetic circuit, and the like. The ECU 1 performs processing according to the moving object position / posture estimation program stored in the ROM, thereby realizing functions (virtual image acquisition means, image likelihood setting means, moving object position / posture estimation means) described later.

  The camera group 2 includes a plurality of imaging means (cameras). Each camera in the camera group 2 uses a solid-state image sensor such as a CCD. The camera group 2 includes at least a front camera (front imaging unit) of the vehicle and a side camera (side imaging unit) that images either the left side or the left side.

  Here, as shown in FIG. 2, the forward direction of the host vehicle V is the main traveling direction of the host vehicle V. The rear direction of the host vehicle V is the direction opposite to the front direction. The right direction of the host vehicle V is the right-hand direction when viewing the front direction. The left direction of the host vehicle V is the left hand direction when the front direction is viewed. Further, as shown in FIG. 3, the cameras included in the camera group 2 have a predetermined imaging angle of view θ and an imaging range 21 centered on the imaging direction of the camera.

  An in-vehicle example of the camera group 2 is shown in FIG. 4A and 4B show the minimum configuration in the present embodiment. The camera group 2 in FIG. 4A includes a front camera 2f in which the front side of the host vehicle V is an imaging range 21f and a left-side camera 2l in which the left direction of the host vehicle V is an imaging range 21l. The camera group 2 in FIG. 4B includes a front camera 2f in which the front of the host vehicle V is an imaging range 21f and a right-side camera 2r in which the right direction of the host vehicle V is an imaging range 21r.

  The camera group 2 may have any one of the configurations shown in FIGS. The camera group 2 shown in FIGS. 5A and 5B includes a rear camera 2b in which the rear direction of the host vehicle V is an imaging range 21b in addition to the configurations of FIGS. 4A and 4B. . The camera group 2 shown in FIG. 6 includes a front camera 2f and both a left side camera 21 and a right side camera 2r so that both the right direction and the left direction of the host vehicle V can be imaged. The camera group 2 shown in FIGS. 7A and 7B has an imaging range in all directions of the host vehicle V. The camera group 2 shown in FIG. 7A includes a front camera 2f, a left side camera 2l, a right side camera 2r, so that all of the front direction, right direction, left direction, and rear direction of the host vehicle V can be imaged. And the rear camera 2b is included. The camera group 2 shown in FIG. 7B includes only the omnidirectional camera 2a in which the imaging range 21a is seamless in all directions of the host vehicle V. If the angle of view that can capture the front or side of the host vehicle V is covered instead of the camera group 2 shown in FIGS. May be installed.

  Cameras included in the camera group 2 capture images at predetermined time intervals to obtain captured images and supply them to the ECU 1.

  The 3D map database 3 stores, for example, 3D position information such as edges of the surrounding environment including road surface display. In the present embodiment, the 3D map database 3 includes edge information of structures such as curbs and buildings, in addition to road surface displays such as white lines, stop lines, pedestrian crossings, and road surface marks. Each map information such as a white line is defined by 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 both end points and a center point of the curve.

  The vehicle sensor group (running state detection means) 4 is connected to the ECU 1. The vehicle sensor group 4 supplies various sensor 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 host vehicle V and the odometry that indicates the amount of movement of the host vehicle V per unit time. In particular, the vehicle sensor group 4 detects the traveling state of the host vehicle V.

The ECU 1 is an electronic control unit that estimates the position and posture angle of the host vehicle V using the captured image captured by the camera group 2 and the three-dimensional position information stored in the three-dimensional map database 3. In particular, the ECU 1 estimates the position and posture angle of the host vehicle V based on the traveling state of the host vehicle V detected by the vehicle sensor group 4. The ECU 1 may also be used as an ECU used for other controls.
In particular, the moving object position / orientation estimation apparatus compares a captured image captured by the camera group 2 with a virtual image obtained by converting 3D map data into an image captured from a virtual position and a virtual attitude angle, Is estimated. Here, it is assumed that a captured image as shown in FIG. 8A is obtained and an edge image as shown in FIG. 8B is obtained. On the other hand, assume that a virtual image obtained by projecting the three-dimensional position information onto the position and posture angle of the camera group 2 is as shown in FIG. When the captured image of FIG. 8B and the virtual image of FIG. 8C are compared, both the far position (A) and the near position (B) match, so the virtual position where the virtual image is generated and It can be estimated that the virtual posture angle corresponds to the position and posture angle of the host vehicle V. However, when the virtual position is displaced in the right direction, the virtual image is as shown in FIG. In this case, when the captured image of FIG. 8A and the captured image of FIG. 8D are compared, the distant position (A) matches, but the nearby position (B) is greatly shifted. Conversely, when the virtual posture angle of the virtual image in FIG. 8C is shifted (not shown) and compared with the captured image in FIG. 8A, the near position (B) matches, but the far position (A) Deviates greatly.

  Focusing on such a phenomenon, the moving object position / posture estimation apparatus determines that the virtual position of the virtual image is likely when the neighboring position pixel in the captured image matches the neighboring position pixel in the virtual image. . Conversely, the moving object position / posture estimation apparatus determines that the virtual posture angle of the virtual image is likely when the far position pixel in the captured image matches the far position pixel in the virtual image.

  Hereinafter, the operation of this moving object position / orientation estimation apparatus will be described with reference to the position / orientation estimation algorithm of FIG. In the present embodiment, it is assumed that the position (front-rear direction, horizontal direction, vertical direction) and posture angle (roll, pitch, yaw) of the host vehicle V are estimated. Further, the position / orientation estimation algorithm of FIG. 9 is continuously performed by the ECU 1 at intervals of, for example, about 100 msec.

  In step S1, the ECU 1 detects the vehicle speed V (unit: m / s) and the yaw rate γ (unit: rad / s) from the sensor value information obtained from the vehicle sensor group 4. The ECU 1 obtains the turning radius ρ (unit: m) by calculation of ρ = V / γ. Furthermore, the movement amount of the host vehicle V is calculated as odometry from the time when step S1 in the position and orientation estimation algorithm before one loop is executed. In the case of the first loop after starting the position / orientation estimation algorithm, the odometry is calculated as zero. When the yaw rate γ is sufficiently small, the turning radius ρ is set to 1000 [m], for example, to avoid dividing the vehicle speed by substantially zero.

  As the odometry calculation method, for example, the movement amount and the rotation amount per unit time may be calculated from the wheel speed of each wheel and the yaw rate sensor after limiting the vehicle motion to a plane. Further, the ECU 1 may substitute the wheel speed by the difference between the vehicle speed and the positioning value of the GPS receiver 41, or may substitute the yaw rate sensor by the steering angle. Various calculation methods can be considered as a method for calculating odometry, but any method may be used as long as odometry can be calculated.

  In the next step S2, the ECU 1 sets a captured image acquisition range in the front-rear direction of the host vehicle V based on the vehicle speed V detected in step S1. In the present embodiment, the acquisition range of the imaging range is set to the horizontal field angle αf (unit: deg) of the front camera 2f or the rear camera 2b (front / rear direction camera) that images the front / rear direction of the host vehicle V. The angle of view of the front-rear camera is set according to the vehicle speed V as shown in FIG. The higher the vehicle speed V, the narrower the angle of view of the front-rear camera. In the present embodiment, the front camera 2f or the rear camera 2b that is mounted on the host vehicle V and captures the front-rear direction preferably has a maximum horizontal angle of view of 90 [deg] or more.

  Of the captured images in the front-rear and left-right directions of the host vehicle V, the captured image in the front-rear direction is an image in which the imaging range becomes narrower as the vehicle speed V increases. This is because captured images in the front-rear direction are more susceptible to lateral movement as the speed of the host vehicle V decreases, so it is necessary to adjust the image to be captured in a wider range and match the three-dimensional map data. Because there is.

  In the next step S3, the ECU 1 extracts a forward captured image that is a forward image over the acquisition range of the captured image in the front-rear direction set in step S2 from the captured images captured by the camera group 2. To do. Moreover, ECU1 extracts a back captured image over the acquisition range of the captured image of the front-back direction set in step S2 among captured images. Thereby, ECU1 extracts the back-and-front captured image group. As will be described later, the ECU 1 may extract one of the front and rear captured images according to the imaging direction of the image selected as the position estimation image or the posture angle estimation image, or may extract both captured images. . When the rear captured image is not used in the processing described later, it is not necessary to extract the rear captured image. In addition, the image to extract is a part image | photographed within the range of angle (theta) of horizontal view angle (alpha) f / 2 respectively from the front direction of the own vehicle V to the right and left like FIG.

  In step S4, the ECU 1 sets the left and right image acquisition ranges based on the turning radius ρ calculated in step S1. In the present embodiment, the left and right image acquisition ranges are set to the horizontal angle of view αs (unit: deg) of the left-side camera 21 or the right-side camera 2r that captures the lateral direction of the host vehicle V. The angles of view of the left and right direction cameras are set with reference to the turning radius ρ as shown in FIG. As the turning radius ρ increases, the angle of view of the left and right cameras becomes narrower. The left side camera 21 and the right side camera 2r preferably have a maximum horizontal angle of view of 90 [deg] or more, for example.

  Of the captured images in the front / rear / left / right direction of the host vehicle V, the captured image in the left / right / horizontal direction is an image in which the imaging range becomes narrower as the turning radius increases. This is because the image of the left and right lateral direction has a turning radius that decreases, that is, the sharper the curve, the wider the range of the state of the curve needs to be captured and the three-dimensional map data must be adjusted to match. Because.

  In the next step S5, the ECU 1 performs either the left direction or the right direction over the acquisition range of the left direction and right direction captured images set in step S4 among the captured images captured by the camera group 2. Images (side captured images) are extracted. As will be described later, the ECU 1 may extract one of the left and right side captured images according to the imaging direction of the image used as the position estimation image or the posture angle estimation image, or extracts both side captured images. May be. Thereby, ECU1 extracts a side captured image group. In addition, the image to extract is a part image | photographed within the range of angle (theta) of horizontal image (alpha) s / 2 from the horizontal direction of the own vehicle V to the left and right, respectively, like FIG.

  In step S6, the ECU 1 determines the image likelihood for position estimation and the image likelihood for posture angle estimation for each of the captured images acquired in step S3 and step S5 based on the running state detected by the vehicle sensor group 4. The degree is set (image likelihood setting means). The image likelihood for position estimation represents a degree suitable for an image for estimating the position of the host vehicle V. The posture angle estimation image likelihood represents a degree suitable for an image for estimating the posture angle of the host vehicle V. The position estimation image likelihood and the posture angle estimation image likelihood are referred to when obtaining the position estimation likelihood and the posture angle estimation likelihood, as will be described later.

  When the ECU 1 is in a normal running state, the ECU 1 increases the posture angle estimation image likelihood for the front captured image in the front and rear captured image group. Further, the ECU 1 increases the image likelihood for position estimation for the side captured image group in the normal traveling state. Here, the normal running state is a situation where the host vehicle V is accelerated or at a constant speed.

  Since the front of the host vehicle V has better visibility than in the lateral direction, the forward captured image is often taken far from the host vehicle V. On the other hand, the side-captured image is often taken near the host vehicle V. Therefore, as described with reference to FIG. 8 described above, the moving object position / posture estimation apparatus uses posture angle estimation in order to use a front captured image capturing a distance from the host vehicle V as a posture angle estimation image. Increase image likelihood. On the other hand, the moving object position / posture estimation apparatus increases the position estimation image likelihood in order to use the side captured image capturing the vicinity from the host vehicle V as the position estimation image.

  For this process, the ECU 1 may refer to table data as shown in FIGS. 12 and 13, for example. 12 and 13 are set for the host vehicle V on which only the front camera 2f and the left side camera 21 are mounted, as shown in FIG. 4A. Further, this table data is obtained by setting the image likelihood for position estimation and the image likelihood for posture angle estimation in the range of 0 to 10, and of course is not limited to these numerical values.

  The table data shown in FIG. 12 is data for setting the posture angle estimation image likelihood according to the running state. According to FIG. 12, in any driving state, the ECU 1 generally has a posture angle estimation image likelihood of the front captured image rather than a posture angle estimation image likelihood of the side captured image (left image). The degree can be increased. The table data shown in FIG. 13 is data for setting the image likelihood for position estimation according to the running state. According to FIG. 13, in any driving state, the ECU 1 generally uses the position estimation image likelihood of the side captured image (left image) rather than the position estimation image likelihood of the front captured image. Can be increased.

  Further, when the host vehicle V is decelerating, that is, when the vehicle speed V is continuously decreasing, the ECU 1 increases the position estimation image likelihood because the front captured image is also suitable for position estimation. May be. As shown in FIG. 13, when the host vehicle V is decelerating and traveling in a straight traveling direction, the image likelihood for position estimation of the front captured image is increased as compared with other traveling states. Yes. This is because when the host vehicle V decelerates, there are stop lines, intersections, and curves, which are targets to be decelerated forward, and it is often possible to capture nearby structures with the front captured image.

  Furthermore, when the host vehicle V is decelerating, that is, when the vehicle speed V is continuously decreasing, the ECU 1 desirably increases the posture angle estimation image likelihood for the rear captured image. The fact that the host vehicle V decelerates is likely to be in a state where the front cannot be seen, and therefore the rear is more suitable for estimating the posture angle than the front. For this process, the ECU 1 may refer to table data as shown in FIGS. 14 and 15, for example. FIGS. 14 and 15 are set for the host vehicle V on which the front camera 2f, the right side camera 2r, the left side camera 21 and the rear camera 2b are mounted as shown in FIG. The table data shown in FIG. 14 is data for setting the posture angle estimation image likelihood according to the running state. The table data shown in FIG. 15 is data for setting the image likelihood for position estimation according to the running state. According to FIG. 14, when the host vehicle V is decelerating and traveling in the straight direction, the posture angle estimation image likelihood of the front captured image is set to “3”, and the posture of the rear captured image The image likelihood for angle estimation is set to “7”.

  Further, when the turning radius ρ of the host vehicle V decreases, the ECU 1 desirably increases the posture angle estimation image likelihood for the rear captured image. Further, when the turning radius ρ of the host vehicle V increases, the ECU 1 desirably increases the posture angle estimation image likelihood for the front captured image. This is because the turning radius ρ decreases at the beginning of the turn of the host vehicle V, and the rear of the host vehicle V can be viewed far away. This is because there is a high possibility of being overlooked. For example, in FIG. 14, when the host vehicle V is traveling forward and the turning radius ρ is decreasing, the posture likelihood estimation image likelihood of the front captured image is set to “3”, and the rear captured image The attitude likelihood estimation image likelihood is set to “6”. Conversely, when the turning radius ρ is increased, the posture angle estimation image likelihood of the front captured image is set to “6”, and the posture angle estimation image likelihood of the rear captured image is set to “3”. It is desirable for the ECU 1 to set a threshold for determining the previous radius ρ. This threshold is set by the imaging range of the rear captured image and the front captured image corresponding to the turning radius ρ of the host vehicle V.

  Furthermore, when the turning radius of the host vehicle V decreases beyond a certain threshold TH (unit: m), the ECU 1 desirably increases the position estimation image likelihood for the side captured image in the turning inner direction. . The ECU 1 determines the turning direction of the host vehicle V based on the sign of the output value from the yaw rate sensor detected in step S1. For example, the threshold value TH is set to 100 [m]. This is because there is a structure in which the captured image of the road edge or the like can be compared with the virtual image continuously inside the turn of the host vehicle V, and the side captured image inside the turn is suitable for position estimation. . For example, in FIG. 15, when the host vehicle V is traveling forward and is turning left, the position estimation image likelihood of the left captured image is set to “7”, and the position estimation image likelihood of the right captured image is set. Set the degree to "2". On the other hand, when the vehicle is turning right, the position estimation image likelihood of the right side captured image is set to “6”, and the position estimation image likelihood of the left side captured image is set to “3”.

  Furthermore, it is desirable that the ECU 1 increases the image likelihood for position estimation for the right-side captured image or the left-side captured image capturing the moving direction of the host vehicle V in the lateral direction. When the moving speed in the horizontal direction increases and exceeds a certain threshold TH (unit: m / s), it is assumed that the horizontal image in the moving direction is suitable for position estimation. For example, the threshold value TH is set to 0.5 [m / s]. For example, when there is a lane change, there is a high possibility that there is a structure or road marking that can match the captured image and the virtual image in the lane change direction, and the side-side captured image on the moving side is suitable for position estimation. For example, in FIG. 15, when the host vehicle V is traveling forward and the lane is changed to the left, the left-side captured image position estimation image likelihood is set to “7”, and the right-side captured image position estimation is performed. The image likelihood is set to “2”. Conversely, when the lane is changed to the right, the position estimation image likelihood of the right captured image is set to “7” and the position estimation image likelihood of the left captured image is set to “2”.

  In the next step S7, the ECU 1 estimates the position estimation image likelihood or the posture angle that is equal to or larger than a predetermined value among the captured images in which the position estimation image likelihood and the posture angle estimation image likelihood are set in step S6. An edge image is created from the captured image of the image likelihood. This predetermined value may be a value greater than 0 or an arbitrarily set value. Thereby, the ECU 1 can create the posture angle estimation edge image Ea and the position estimation edge image Ep.

  For example, as shown in FIG. 4A, when the host vehicle V equipped with the front camera 2f and the left side camera 21 is moving forward at a constant speed, the edge image of the front captured image is an attitude angle as shown in FIG. Edge image Ep. Further, as shown in FIG. 13, the left-side captured image becomes the position estimation edge image Ep. In another example, as shown in FIG. 7A, when the host vehicle V including the front camera 2f, the left side camera 21, the right side camera 2r, and the rear camera 2b is moving forward at a constant speed, FIG. As illustrated, the edge image of the front captured image and the edge image of the rear captured image are respectively the posture angle edge images Ep. Further, when the host vehicle V is moving forward at a constant speed, as shown in FIG. 15, the edge image of the left side captured image and the edge image of the right side captured image are respectively the position estimation edge images Ep.

  An edge in the present embodiment refers to a location where the luminance of a pixel changes sharply. For example, the Canny method can be used as the edge detection method. The edge detection method is not limited to this, and various other methods such as differential edge detection may be used. Further, it is desirable that the ECU 1 extracts the brightness change direction of the edge, the color near the edge, and the like from the captured image of the camera group 2. Thereby, in step S5 and step S6 described later, the position estimation likelihood and the posture angle estimation likelihood are set using information other than the edges recorded in the three-dimensional map database 3, The position and posture angle of the host vehicle may be estimated.

  In step S8 following step S1, the ECU 1 moves the odometry calculated in step S1 from the vehicle position estimated in step S6 one loop before. The ECU 1 calculates a plurality of virtual (predicted) position and virtual (predicted) attitude angle candidates in the vicinity of the moved vehicle position. However, in the case of the first loop after starting the position / orientation estimation algorithm, the ECU 1 does not have the previous vehicle position information. Therefore, the ECU 1 uses the data from the GPS receiver 41 included in the vehicle sensor group 4 as initial position information. Or ECU1 memorize | stores the vehicle position and attitude | position angle which were last calculated at the time of a stop last time, and may make it into initial position and attitude | position angle information.

  The ECU 1 takes into account measurement errors of the vehicle sensor group 4, odometry errors caused by communication delays, and vehicle dynamic characteristics that cannot be taken into account by the odometry, so that there is a possibility that the true values of the vehicle position and attitude angle can be obtained. A plurality of virtual position and virtual attitude angle candidates are generated. The candidates for the virtual position and the virtual attitude angle set the upper and lower limits of the error with respect to the 6-degree-of-freedom parameters of the position and attitude angle, and randomly use a random number table or the like within the upper and lower limits of the error. Set.

  In the present embodiment, 500 candidates for the virtual position and the virtual posture angle are created. The upper and lower limits of the error for the 6-degree-of-freedom parameters of position and posture angle are ± 0.05 [m], ± 0.05 [m], ± 0.05 [ m], ± 0.5 [deg], ± 0.5 [deg], ± 0.5 [deg]. The number of virtual position and virtual attitude angle candidates to be created and the upper and lower limits of the error with respect to the 6-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. It is desirable. For example, when a sudden turn or slip occurs, the error in the plane direction (front / rear 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, it is desirable to increase the number of virtual position and virtual attitude angle candidates created.

  In the next step S9, the ECU 1 creates a virtual (projection) image for each of all the virtual positions and virtual attitude angle candidates created in step S8. At this time, the ECU 1 creates a virtual image by projecting and converting, for example, three-dimensional position information such as an edge stored in the three-dimensional map database 3 into a captured image captured from the virtual position and the virtual attitude angle. This virtual image is an image for evaluation that evaluates whether each virtual position and virtual posture angle candidate matches the actual position and posture angle of the host vehicle. In the projection conversion, an external parameter indicating the position of the camera group 2 and an internal parameter of the camera group 2 are required. The external parameter may be calculated from the virtual position and virtual attitude angle candidates by measuring in advance the relative position from the vehicle position (for example, the center position) to the camera group 2. The internal parameters may be calibrated in advance.

  The ECU 1 creates a virtual image that becomes a captured image captured from the camera group 2 mounted on the host vehicle V for each virtual position and virtual attitude angle candidate. For example, as shown in FIG. 4A, when the camera group 2 includes a front camera 2f and a left side camera 21, a virtual front image and a virtual left side image are created for each virtual position and virtual attitude angle candidate. 7A, when the camera group 2 includes a front camera 2f, a left side camera 21, a right side camera 2r, and a rear camera 2b, a virtual front image for each virtual position and virtual posture angle candidate, A virtual right side image, a virtual left side image, and a virtual rear image are created. Thereby, the ECU 1 creates a posture estimation virtual image Aa projected in the same direction as the imaging direction of the posture estimation edge image Ea, and projects it in the same direction as the imaging direction of the position estimation edge image Ep. An image Ap is created. Note that this virtual image may be calculated only in the range of the angle of view of αf and αs set in step S2 and step S4.

  In addition, when it is possible to extract from the captured image the direction of the luminance change of the edge, the color near the edge, and the like from the captured image captured by the camera 2 in step S1, the three-dimensional position information is also extracted from the captured image. It is better to create a virtual image in advance.

  Step S10 is performed after the process of step S7 and step S9. In step S10, the ECU 1 performs the edge image Ea, Ep created in step S7 and the imaging direction of the edge image for each of the virtual position and virtual attitude angle candidates set in step S8, in the same step S9. The created virtual images Aa and Ap are respectively compared. The ECU 1 sets a posture angle estimation likelihood (unit: none) having a value corresponding to the degree of coincidence between the posture angle estimation edge image Ea and the posture estimation virtual image Aa. Further, the ECU 1 sets a position estimation likelihood (unit: none) having a value corresponding to the degree of coincidence between the position estimation edge image Ep and the position estimation virtual image Ap. Thus, the ECU 1 can set the posture angle estimation likelihood and the position estimation likelihood for each virtual position and virtual posture angle candidate point.

  Specifically, when there are pixels of the posture angle estimation edge image Ea and the position estimation edge image Ep at the pixel position in the virtual image, the posture angle estimation likelihood and the position estimation likelihood are added. . Conversely, when there is no pixel of the posture angle estimation edge image Ea and the position estimation edge image Ep at the pixel position in the virtual image, the posture angle estimation likelihood and the position estimation likelihood are not added. Thereby, the posture angle estimation likelihood and the position estimation likelihood can be set for each imaging direction of each virtual position and virtual posture angle candidate point.

  In addition, when the posture estimation edge image Ea and the posture estimation virtual image Aa are compared, and there is a pixel matching the same position, 1.0 is added to the posture estimation likelihood and the position estimation likelihood is increased. You may add 0.2 to the degree. On the other hand, when the position estimation edge image Ep and the position estimation virtual image Ap are compared and there is a pixel that matches the same position, 0.2 is added to the posture estimation likelihood, and the position estimation likelihood is increased. You may add 1.0 to the degree.

  Further, the position estimation likelihood and the posture angle estimation likelihood may be respectively multiplied by the position estimation image likelihood and the posture angle estimation likelihood of the image used for comparison.

  For example, as shown in FIG. 4A, when the host vehicle V equipped with the front camera 2f and the left side camera 21 is moving forward at a constant speed, the posture angle edge image Ep obtained from the front captured image is projected forward. The posture angle estimation likelihood is set according to the degree of coincidence with the virtual image. Further, the position estimation likelihood is set according to the degree of coincidence between the position estimation edge image Ep obtained from the left side captured image and the virtual image projected to the left side.

  In another example, as shown in FIG. 7A, when the host vehicle V including the front camera 2f, the left side camera 21, the right side camera 2r, and the rear camera 2b is moving forward at a constant speed, a front captured image The posture angle estimation likelihood according to the degree of coincidence between the posture angle edge image Ep obtained from the above and the virtual image projected forward is set. Further, the posture angle estimation likelihood is set according to the degree of coincidence between the posture angle edge image Ep obtained from the rear captured image and the virtual image projected rearward. Further, the likelihood for posture angle estimation is set according to the degree of coincidence between the position estimation edge image Ep obtained from the left side captured image and the virtual image projected to the left side. Further, a posture angle estimation likelihood is set according to the degree of coincidence between the position estimation edge image Ep obtained from the right side captured image and the virtual image projected to the right side. Note that the ECU 1 may average the two posture angle estimation likelihoods obtained from the two posture angle estimation edge images Ea for one virtual position and virtual posture angle, and the value of the posture angle estimation image likelihood Depending on, a weighted average may be used. Similarly, the ECU 1 may average two position estimation likelihoods obtained from the two position estimation edge images Ep for one virtual position and virtual attitude angle, depending on the value of the position estimation image likelihood. Weighted average.

  In the next step S11, the ECU 1 uses the plurality of virtual position and virtual attitude angle candidates for which the position estimation likelihood and the attitude angle estimation likelihood are obtained in step S10, to determine the final vehicle V. The position and posture angle are calculated. The ECU 1 estimates the actual posture angle of the host vehicle V with respect to the virtual posture angle of the virtual image based on the likelihood for posture angle estimation of the virtual image (moving object position and posture estimation means). The ECU 1 estimates the actual position of the host vehicle V with respect to the virtual position of the virtual image based on the position estimation likelihood of the virtual image (moving object position / posture estimation means).

  At this time, the ECU 1 calculates the virtual position and the virtual attitude angle as the actual position and attitude angle of the host vehicle V using, for example, the virtual position and virtual attitude angle candidate having the highest position estimation likelihood. May be. Alternatively, the ECU 1 may calculate the virtual position and the virtual posture angle as the actual position and posture angle of the host vehicle V using the virtual position and virtual posture angle candidate having the highest posture angle estimation likelihood. Good. Alternatively, the ECU 1 uses the virtual position and the virtual attitude angle candidate having the highest sum of the position estimation likelihood and the attitude angle estimation likelihood, and uses the virtual position and the virtual attitude angle as the actual position of the host vehicle V. You may calculate as a position and a posture angle. Alternatively, the ECU 1 calculates the actual position and attitude angle of the host vehicle V by taking a weighted average according to the position estimation likelihood of each virtual image for a plurality of virtual position and virtual attitude angle candidates. May be. Alternatively, the ECU 1 calculates the actual position and posture angle of the host vehicle V by taking a weighted average according to the posture angle estimation likelihood of each virtual image for a plurality of virtual position and virtual posture angle candidates. May be. Alternatively, the ECU 1 may calculate the actual position and posture angle of the host vehicle V by taking a weighted average corresponding to the sum of the position estimation likelihood and the posture angle estimation likelihood of each virtual image.

  The ECU 1 can sequentially calculate the position and posture angle of the host vehicle V by repeatedly performing the above steps S1 to S11.

  As described above in detail, according to the moving object position / posture estimation apparatus shown as the present embodiment, when the running state is the normal state of acceleration or constant speed, the posture angle estimation image for the front captured image The likelihood is increased, and the position estimation image likelihood for the side captured image is increased. Thereby, the moving object position / posture estimation apparatus can estimate the posture angle of the host vehicle V using the front captured image, and can estimate the position of the host vehicle V using the side captured image. Therefore, according to this moving object position / posture estimation apparatus, it is possible to perform estimation by separately adjusting the position and posture angle, considering different likelihoods for the position and posture angle. Specifically, if these two images match at a place far from the host vehicle V, the attitude angle of the virtual image is close to the true value, but the error in the position information of the virtual image may be large. The likelihood for posture angle estimation can be set larger. Conversely, the position estimation likelihood is not set too large. On the other hand, if the two images match in a place close to the host vehicle V, the position estimation likelihood is set larger, and conversely, the posture angle estimation likelihood is not set too large.

  When the place where the two images match is far from the host vehicle V, the position error may be large. Conversely, when the position where the two images match is close to the host vehicle V, the posture angle error is large. In some cases, the moving object position / posture estimation apparatus can accurately estimate the position and the posture angle of the moving object.

  Further, according to this moving object position / posture estimation apparatus, it is possible to omit processing that matches a virtual image for a captured image that is unnecessary for obtaining the position and posture angle of the host vehicle V, thereby realizing a reduction in calculation load. it can.

  Further, according to this moving object position / posture estimation apparatus, when the host vehicle V is decelerating, the posture angle estimation image likelihood for the rear captured image is increased, and therefore an obstacle is present in front of the host vehicle V. The posture angle of the host vehicle V can be estimated using the rear captured image that can be widely viewed. Thus, according to the moving object position / posture estimation apparatus, the posture angle of the host vehicle V can be estimated using a rear-view captured image with good visibility, so that the posture angle estimation accuracy can be improved.

  Furthermore, according to this moving object position and orientation estimation apparatus, when the turning radius of the moving object decreases, the posture angle estimation image likelihood for the rear captured image is increased, and the turning radius of the moving object increases. Increases the image likelihood for posture angle estimation of the forward captured image. Thus, according to this moving object position and orientation estimation apparatus, when the turning radius starts and the turning radius decreases, the posture angle of the host vehicle V can be estimated using a rearward captured image with good visibility, and the turning radius at which the turning ends. When the angle increases, the attitude angle of the host vehicle V can be estimated using a forward captured image with good visibility. Therefore, even if the turning radius of the host vehicle V changes, the estimation accuracy of the posture angle can be improved.

  Furthermore, according to this moving object position / posture estimation apparatus, when the host vehicle V turns, the image likelihood for posture angle estimation for the right-side captured image or the left-side captured image capturing the inner direction of the turning direction is increased. As a result, the moving object position / posture estimation apparatus can estimate the posture angle using the captured images in the direction of the inside of the turn that can continuously image road edges, lane markings, and the like that often exist at a short distance inside the turn. Therefore, it is possible to improve the estimation accuracy of the posture angle even when the host vehicle V is turning.

  Furthermore, according to the moving object position / posture estimation apparatus, when the host vehicle V moves in the horizontal direction, the image likelihood for position estimation with respect to the right-side captured image or the left-side captured image capturing the moving direction in the horizontal direction. To increase. As a result, the moving object position / posture estimation apparatus estimates the position by using a captured image in the lateral movement direction that can be imaged such as a road edge or a lane line that is often present at a short distance when the host vehicle V changes lanes, for example. it can. Therefore, the position estimation accuracy can be improved even when the host vehicle V is moving in the lateral direction.

  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 various modifications can be made depending on the design and the like as long as the technical idea according to the present invention is not deviated from this embodiment. Of course, it is possible to change.

  In the above-described embodiment, a vehicle is taken as an example. However, any vehicle that has at least one camera mounted thereon can be applied to an aircraft, a ship, and the like.

  In the above-described embodiment, the position (front-rear direction, lateral direction, vertical direction) and posture angle (roll, pitch, yaw) of the six degrees of freedom of the vehicle are obtained, but the present invention is not limited to this. For example, this embodiment is also used 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 applicable. Specifically, in such a vehicle, the vertical position and the posture angle such as roll and pitch are fixed. Therefore, these parameters are measured in advance or the 3D map database 3 is referred to. And ask for it.

1 ECU
2 camera group 2a omnidirectional camera 2b rear camera 2f front camera 2l left side camera 2r right side camera 3D map database 4 vehicle sensor group 21 imaging range 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 (6)

A moving object position and orientation estimation apparatus for estimating a position and an attitude angle of a moving object,
Forward imaging means for capturing the front of the moving object and obtaining a forward captured image;
A side imaging means for capturing a right side or a left side of a moving object to obtain a side captured image;
The three-dimensional map data is converted into an image virtually captured by the front imaging means to acquire a virtual front image, and the three-dimensional map data is converted into an image virtually captured by the side imaging means to be virtual Virtual image acquisition means for acquiring side images;
Traveling state detection means for detecting the traveling state of the moving object;
Based on the traveling state of the moving object detected by the traveling state detection unit, for each of the front captured image acquired by the front imaging unit and the side captured image acquired by the side imaging unit, Sets the image likelihood for position estimation representing the degree suitable for an image for estimating the position and the image likelihood for posture angle estimation representing the degree suitable for an image for estimating the posture angle of the moving object. When the traveling state is acceleration or constant speed, the posture angle estimation image likelihood for the front captured image is increased, and the position estimation image likelihood for the side captured image is increased. Image likelihood setting means;
Of the forward captured image and the lateral captured image, a captured image whose position estimation image likelihood set by the image likelihood setting means is a predetermined value or more and a virtual image projected in the same direction as the captured image Based on the degree of coincidence, the position of the actual moving object is estimated, and the posture angle estimation image likelihood set by the image likelihood setting means of the front captured image and the side captured image is greater than or equal to a predetermined value A moving object position and orientation estimation means for estimating the orientation angle of the actual moving object based on the degree of coincidence between the captured image and the virtual image projected in the same direction as the captured image. Position and orientation estimation device. It further has a rear imaging means for imaging the rear of the moving object and obtaining a rear captured image,
The image likelihood setting means increases image likelihood for posture angle estimation for the rear captured image when the traveling state detecting means detects that the moving object is decelerating. Item 4. A moving object position / posture estimation apparatus according to Item 1.   The image likelihood setting means increases the posture angle estimation image likelihood for the rear captured image when the running state detecting means detects that the turning radius of the moving object decreases, and the running state detection 3. The moving object position / posture estimation apparatus according to claim 2, wherein when the turning radius of the moving object is detected to be increased by the means, the posture likelihood estimation image likelihood of the front captured image is increased. The side imaging means captures both the right side and the left side of the moving object to obtain the right side captured image and the left side captured image,
The image likelihood setting means increases the image likelihood for posture angle estimation for a right-side captured image or a left-side captured image that captures the inner direction of the turning direction of the moving object detected by the traveling state detection means. The moving object position and orientation estimation apparatus according to any one of claims 1 to 3.   The image likelihood setting means increases the image likelihood for position estimation for the right-side captured image or the left-side captured image that captures the lateral movement direction of the moving object detected by the running state detection means. The moving object position and orientation estimation apparatus according to claim 4. A moving object position / posture estimation method for estimating a position and a posture angle of a moving object,
Detecting a traveling state of the moving object;
When the traveling state is acceleration or constant speed, the posture angle estimation image likelihood representing the degree of suitability as an image for estimating the posture angle of the moving object with respect to the front captured image is increased, Increasing the image likelihood for position estimation representing the degree of suitability as an image for estimating the position of the moving object with respect to the captured image;
Based on the degree of coincidence between the captured image having the position estimation image likelihood of a predetermined value or more and the virtual image projected in the same direction as the captured image, of the front captured image and the lateral captured image, actual movement is performed. While estimating the position of an object, between the front captured image and the side captured image, a captured image whose posture angle estimation image likelihood is a predetermined value or more and a virtual image projected in the same direction as the captured image And a step of estimating an actual posture angle of the moving object based on the degree of coincidence.

Images