JP2018152692A - Position estimation program and moving body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a position estimation program capable of precisely estimating the position of a moving body.SOLUTION: A vehicle 10 includes: a vehicle measurement unit 42 that acquires travel speed and direction of the vehicle 10 in which a stereo camera 46; an SfM calculation unit 58 that calculates an estimation position of the vehicle 10 based on images of the vicinity of the vehicle 10 picked up by the stereo camera 46 during travelling of the vehicle 10; and an FPS determination unit 56 that determines the imaging interval of the images in the vicinity of the vehicle 10 which are used by the SfM calculation unit 58 for estimating the position of the vehicle 10 based on the travel speed of the vehicle 10 or changing speed of the direction of the vehicle 10. The SfM calculation unit 58 calculates an estimation position of the vehicle 10 based on the images which are picked up at the imaging interval determined by the FPS determination unit 56.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a position estimation technique, and more particularly to a position estimation program for estimating the position of a moving body and a moving body using the position estimation program.

  Autonomous vehicles that can run autonomously have been developed. Level 3 and level 4 autonomous vehicles control all acceleration, steering, and braking, so it is necessary to accurately estimate their position during travel. For example, Patent Document 1 proposes a method in which a mobile object estimates its own position.

  Patent Document 1 discloses a technique for estimating a self-position with respect to an object around the moving body by detecting the position of the object around the moving body from a change in feature points on the image accompanying the movement of the moving body. Yes.

JP 2010-288112 A

  In such a self-position estimation method, since the feature points need to be captured in two or more images, there is a problem that if the feature points are lost, the self-position cannot be estimated accurately. Patent Document 1 describes a technique for controlling the orientation of an imaging device so that at least a part of feature points existing in an image acquired before the moving body rotates on the spot is captured. The inventor of the present invention has come up with a technique that can reduce the possibility that a feature point is lost and accurately estimate the self-position by a different method.

  This invention is made | formed in view of such a condition, The objective is to provide the technique which estimates the position of a moving body more correctly.

  In order to solve the above problems, a position estimation program according to an aspect of the present invention includes a computer, a speed acquisition unit that acquires the moving speed of a moving body provided with an imaging device, and a direction acquisition unit that acquires the direction of the moving body. A calculation unit that calculates an estimated position of the moving object based on an image of the surroundings of the moving object captured by the imaging device while the moving object is moving, based on the moving speed of the moving object, or the changing speed of the direction of the moving object The calculation unit functions as an interval determination unit that determines a shooting interval of an image around the moving body that is used to estimate the position of the moving body, and the calculation unit is the shooting interval determined by the interval determination unit. Based on the captured image, the estimated position of the moving object is calculated.

  It should be noted that any combination of the above-described constituent elements and a conversion of the expression of the present invention between a method, an apparatus, a system, a recording medium, a computer program, etc. are also effective as an aspect of the present invention.

  ADVANTAGE OF THE INVENTION According to this invention, the technique which estimates the position of a moving body more accurately can be provided.

It is a figure for demonstrating the imaging | photography space | interval when the vehicle is going straight ahead. It is a figure for demonstrating the imaging | photography space | interval when a vehicle is turning the curve. It is a figure showing roughly composition of vehicles concerning an example.

  Embodiments of the present invention provide a technique for estimating the position of a moving body based on images taken at two or more different viewpoint positions in a moving body equipped with an imaging device. As such a position estimation technique, for example, SfM (Structure from Motion) can be used. SfM is a technique that makes it possible to restore a three-dimensional shape and a viewpoint position from a plurality of two-dimensional images with different viewpoints. In order to use SfM, it is necessary to capture a plurality of images with different viewpoints while the moving body is moving. However, the change in the viewpoint position (position of the imaging apparatus) or the line-of-sight direction (direction of the imaging apparatus) is large. If it is too long, the number of feature points included in a plurality of captured images may be reduced, and the accuracy of detecting correspondence between feature points may be reduced, and the viewpoint position and the line-of-sight direction are almost changed. Otherwise, the parallax between the feature points becomes small, and there is a possibility that the accuracy of detecting the relative position between the feature points and the imaging device may be reduced. Therefore, in this embodiment, the position of the moving body is estimated by SfM using a plurality of images with appropriate changes in the viewpoint position and the line-of-sight direction. Thereby, the position of a mobile body can be estimated more correctly.

  Hereinafter, as an embodiment, an autonomous driving vehicle equipped with a stereo camera will be described. In self-driving cars, the orientation of the stereo camera is usually used to prevent errors in various information obtained by analyzing the images captured by the stereo camera from increasing due to a deviation in the orientation of the stereo camera. Is fixed to the vehicle. Therefore, in the present embodiment, the shooting interval of images used for SfM is dynamically adjusted so that the amount of change in viewpoint position and line-of-sight direction in a plurality of images shot by a stereo camera is appropriate. . Hereinafter, the term “photographing interval” simply refers to a time interval, not an interval between positions where images are taken.

  Note that the time interval at which the stereo camera actually captures images may be dynamically changed according to the time interval that is set as the appropriate shooting interval, or if it is shorter than the time interval that is set as the appropriate shooting interval. The time interval may be different from the appropriate shooting interval. As an example of the latter, the stereo camera may capture images periodically at regular time intervals. In this case, images to be used are selected from among images taken at regular time intervals so that the shooting interval of images used for SfM is appropriate. In short, it is sufficient that images taken at an appropriate shooting interval are used in SfM, and even if the time interval at which the stereo camera actually takes an image is the same as the appropriate shooting interval, May be different. In the former case, since it is not necessary to dynamically control the frame rate of the stereo camera, the configuration can be simplified. In the latter case, it is possible to reduce the data volume of the image to be shot, the communication amount between the stereo camera and the configuration for estimating the position, the processing load required for shooting and position estimation of the stereo camera, and the like.

  FIG. 1 is a diagram for explaining a shooting interval when the vehicle is traveling straight. FIG. 1 schematically shows an imaging range of a stereo camera mounted on a vehicle 10 traveling on a straight road 12a. A sign 14 is included in the image taken by the stereo camera at the position of the vehicle 10a. If the sign 14 is also included in images taken from different viewpoint positions, the sign 14 is associated as a feature point between the images by SfM using those images, and between the sign 14 and the vehicle 10. Relative position can be calculated. Further, if the map data including the position information of the sign 14 is further referred to, the position of the vehicle 10 can be estimated. Therefore, in the example of FIG. 1, an image may be taken with a stereo camera even at the position of the vehicle 10b. If the position of the vehicle 10b is exceeded, the sign 14 is not included in the image captured by the stereo camera, and the sign 14 is lost as a feature point. As described above, if the image around the vehicle 10 is continuously captured by the stereo camera at an appropriate capturing interval such that a sufficient amount of feature points are included in the plurality of captured images while the vehicle 10 is traveling, the vehicle The 10 positions can be estimated accurately.

According to the experiments of the present inventor, when the vehicle 10 is traveling on the straight road 12, the change amount x of the viewpoint position between images suitable for estimating the position of the vehicle 10 with high accuracy by SfM. [M] is 0.5 to 3.5 m, preferably 1 to 3 m, more preferably 1.5 to 2.5 m, and still more preferably about 2 m. When the amount of change in the viewpoint position is x [m] and the speed of the vehicle 10 is v [km] per hour, the shooting interval is expressed by the following equation.
(Shooting interval) = 3600 × x / (v × 1000) (Formula 1)
In addition, the number of shootings per unit time by the stereo camera, that is, FPS (Frames per second) representing the frame rate is given by the reciprocal of the shooting interval. The photographing interval is calculated by substituting any value in the above-described x range and the vehicle speed v of the vehicle 10 into Equation 1. Therefore, as the speed of the vehicle 10 increases, the shooting interval is shortened and the FPS is increased.

  Any value included in the above range may be assigned as a constant to x in Equation 1, or a value that is dynamically changed according to the state of the vehicle, the situation around the vehicle, or the like. May be. As an example of the latter, a parameter serving as an index of SfM accuracy, such as a position error estimated by SfM, may be calculated, and if the value exceeds a predetermined value, the value of x may be made smaller. . Further, in a region where there are many stationary objects that can be feature points, the value of x may be larger than in a region where there are few objects that can be feature points. Whether it is such a region may be determined with reference to map data, or may be determined with reference to the calculation result of SfM. Also, on roads where there is unevenness on the road surface and vertical acceleration occurs while traveling, the position of the stereo camera may shift in the vertical direction, which may reduce the overlap of shooting ranges between images. Therefore, the value of x may be made smaller than on a road with less road surface unevenness. Whether or not the road is such a road may be determined with reference to map data, or may be determined with reference to an acceleration sensor value, an analysis result of an image taken by a stereo camera, or the like. Further, in a situation where other vehicles or the like existing in the vicinity enter the imaging range of the stereo camera and the object that can be a feature point is shielded and cannot be sufficiently imaged, the value of x may be made smaller. Whether such a situation is present may be determined with reference to map data and traffic jam information, or may be determined with reference to an analysis result of an image taken by a stereo camera.

  FIG. 2 is a diagram for explaining a photographing interval when the vehicle is turning a curve. FIG. 2 schematically shows a photographing range of a stereo camera mounted on the vehicle 10 traveling on the curved road 12b. A sign 16 is included in the image captured by the stereo camera at the position of the vehicle 10c. If the sign 16 is also included in images taken from different viewpoint positions, the sign 16 is associated as a feature point between the images by SfM using those images, and between the sign 16 and the vehicle 10. Relative position can be calculated. Further, if the map data including the position information of the sign 16 is further referred to, the position of the vehicle 10 can be estimated. Therefore, in the example of FIG. 2, it is only necessary to take an image with a stereo camera even at the position of the vehicle 10d. If the vehicle turns further beyond the position of the vehicle 10d, the sign 16 is not included in the image captured by the stereo camera, and the sign 16 is lost as a feature point. Thus, when the vehicle 10 is traveling on the curved road 12b, not only the position of the stereo camera changes in the traveling direction of the vehicle, but also the direction of the stereo camera changes with the change in the yaw angle of the vehicle. Therefore, images of the surroundings of the vehicle 10 are continuously captured by the stereo camera at an appropriate capturing interval such that a sufficient amount of feature points are included in a plurality of captured images on the basis of the amount of change in the line-of-sight direction. Is desirable.

  When the line-of-sight direction changes in the left-right direction, in order to ensure that a sufficient amount of feature points are included in both the images taken before and after the change, the shooting range and change of the image taken before the change are changed. It is desirable that the ratio (hereinafter referred to as “remaining rate”) of the range where the shooting ranges of the images shot later overlap, that is, the range 18a remaining as the shooting range, to the entire shooting range 18 is equal to or greater than a predetermined value. For example, when the angle of view of a stereo camera is 30 degrees and the afterimage ratio is 80% or more, the next image is displayed until the line-of-sight direction changes by 6 degrees, that is, until the yaw angle of the vehicle tilts by 6 degrees. It is desirable to shoot. When the yaw angular velocity of the vehicle 10 is 20 degrees per second, the shooting interval is 6/20 = 0.3 [seconds], and the FPS is 20/6 = 3.3 [frames / second]. If the angle of view of the stereo camera is constant, the shooting interval is determined based on the afterimage rate and the yaw angular velocity of the vehicle. The afterimage rate may be 70% or more, preferably 75% or more, more preferably 80% or more. The higher the yaw angular velocity of the vehicle 10, the shorter the shooting interval and the FPS.

  As for the afterimage rate, as in the case of x in Equation 1 described above, any value included in the above range may be substituted as a constant, and may vary depending on the state of the vehicle, the situation around the vehicle, and the like. Changed values may be substituted. As an example of the latter, a parameter that is an index of the accuracy of SfM, such as a position error estimated by SfM, is calculated, and when the value exceeds a predetermined value, the residual rate value is increased. Also good. Further, in a region where there are many stationary objects that can be feature points, the value of the afterglow ratio may be made smaller than in a region where there are few objects that can be feature points. Whether it is such a region may be determined with reference to map data, or may be determined with reference to the calculation result of SfM. Also, on roads where there is unevenness on the road surface and vertical acceleration occurs while traveling, the position of the stereo camera may shift in the vertical direction, which may reduce the overlap of shooting ranges between images. Therefore, the value of the afterimage rate may be increased as compared with a road with less road surface unevenness. Whether or not the road is such a road may be determined with reference to map data, or may be determined with reference to an acceleration sensor value, an analysis result of an image taken by a stereo camera, or the like. Further, in a situation where other vehicles or the like that exist in the vicinity enter the shooting range of the stereo camera and an object that can be a feature point is shielded and is not sufficiently shot, the remaining rate value may be increased. Whether such a situation is present may be determined with reference to map data and traffic jam information, or may be determined with reference to an analysis result of an image taken by a stereo camera. Even when the vehicle is traveling on a straight road, the shooting interval may be determined based on the afterimage rate.

  As described above, in order not to lose sight of the feature points in SfM, it is necessary to determine the shooting interval by the stereo camera so as to satisfy both the reference for the change speed of the viewpoint position and the reference for the change speed of the line-of-sight direction. There is. Therefore, in this embodiment, the imaging interval determined based on the change speed of the viewpoint position, that is, the vehicle speed of the vehicle, and the imaging interval determined based on the change speed of the line-of-sight direction, that is, the yaw angular velocity of the vehicle, are short. Adopt one of the shooting intervals. In terms of FPS, the higher one is adopted.

  FIG. 3 schematically illustrates the configuration of the vehicle 10 according to the embodiment. The vehicle 10 detects a vehicle drive system 40 including a configuration for driving the vehicle 10, such as a tire, an engine, a motor, a gear, and a brake of the vehicle 10, and a state of the vehicle 10 and a situation around the vehicle. Vehicle measurement for measuring the vehicle speed and yaw angular velocity of the vehicle 10 based on the vehicle information sensor 44 and information on the driving state of the vehicle 10 acquired from the vehicle drive system 40 and various sensor values acquired from the vehicle information sensor 44. Unit 42, stereo camera 46 that captures the surroundings of vehicle 10, and vehicle 10 from an image captured by stereo camera 46 in an appropriate manner according to the vehicle speed and yaw angular velocity of vehicle 10 measured by vehicle measurement unit 42. A position estimation unit 50 that estimates the position of the vehicle 10, and the action of the vehicle 10 is determined based on the position of the vehicle 10 estimated by the position estimation unit 50. And a control unit 20 for driving the vehicle drive system 40, a storage unit 30 for storing information used by the control unit 20 and the position estimation unit 50 according to behavior. The vehicle measurement unit 42 may measure the vehicle speed from information such as the rotation speed of the tire acquired from the vehicle drive system 40, or the vehicle speed from a sensor value such as a vehicle speed sensor or an acceleration sensor acquired from the vehicle information sensor 44. May be measured. Further, the yaw angular velocity may be measured from information such as the steering angle of the steering wheel acquired from the vehicle drive system 40, or a sensor such as a gyro sensor, geomagnetic sensor, acceleration sensor, or tilt sensor acquired from the vehicle information sensor 44. The yaw angular velocity may be measured from the value.

  The position estimation unit 50 includes a speed reference FPS calculation unit 52 that calculates the FPS of the stereo camera 46 based on the vehicle speed of the vehicle 10 as in the example illustrated in FIG. 1, and a vehicle as in the example illustrated in FIG. 2. Among the FPS calculated by the angular velocity reference FPS calculation unit 54, the FPS calculated by the velocity reference FPS calculation unit 52, and the FPS calculated by the angular velocity reference FPS calculation unit 54. The FPS determination unit 56 that determines the larger one as the FPS, and the vehicle 10 and the surrounding object by SfM using the image of the periphery of the vehicle 10 captured by the stereo camera 46 with the FPS determined by the FPS determination unit 56 And an SfM calculation unit 58 that calculates a relative position between the two.

  These configurations can be realized in terms of hardware by a CPU, memory, or other LSI of any computer, and in terms of software, they are realized by programs loaded into the memory. It depicts the functional blocks that are realized. Accordingly, those skilled in the art will understand that these functional blocks can be realized in various forms only by hardware, or by a combination of hardware and software.

  The control unit 20 acquires the relative position between the vehicle 10 and the surrounding object from the position estimation unit 50, and includes map data including the 3D shape and 3D position information of the object stored in the storage unit 30. The position of the vehicle 10 is calculated with reference to FIG. As a result, the position of the vehicle 10 can be estimated with high accuracy even in a tunnel or indoor location where a GNSS (Global Navigation Satellite System (s)) receiver cannot receive a signal.

  The control unit 20 includes the position of the vehicle 10 estimated by the position estimation unit 50, the position of the vehicle 10 measured from sensor values such as a speed sensor, an acceleration sensor, a geomagnetic sensor, a gyro sensor, and a tilt sensor of the vehicle 10. The position of the vehicle 10 may be estimated by comprehensively considering the position of the vehicle 10 measured from the signal received by the GNSS receiver. For example, the position of the vehicle 10 may be estimated by a weighted average of the respective measurement results. The weight given to each measurement result may be determined according to the state of the vehicle 10 and the situation around the vehicle 10. For example, in a situation where the accuracy of the signal received by the GNSS receiver is insufficient, such as when passing through a tunnel, the weight of the measurement result by the signal received by the GNSS receiver may be reduced, or the road surface may be uneven. In certain road conditions, the weight of the measurement result by the acceleration sensor may be lowered.

  According to the present embodiment, since the surrounding image of the vehicle photographed at an appropriate photographing interval is used according to the vehicle speed or the yaw angular velocity, it is possible to improve the accuracy of detecting the correspondence between the feature points in SfM. And the position of the vehicle can be estimated more accurately. Moreover, since a useless calculation with low feature point detection accuracy can be omitted, the amount of calculation for estimating the position of the moving body can be reduced. In addition, when the image capturing apparatus captures only images used for calculation, the data capacity and the communication amount can be reduced.

  The outline of one embodiment of the present invention is as follows. A position estimation program according to an aspect of the present invention is a computer that captures a computer while a moving body provided with an imaging device acquires a moving speed, a direction acquiring unit that acquires a moving body direction, and the moving body moving. The calculation unit calculates the estimated position of the moving body based on the image of the surroundings of the moving body photographed by the device, the moving speed of the moving body, or the changing speed of the direction of the moving body. It functions as an interval determination unit that determines an imaging interval of an image around the moving body used for estimating the position, and the calculation unit moves based on an image captured at the imaging interval determined by the interval determination unit. Calculate the estimated position of the body.

  According to this aspect, since the estimated position of the moving body is calculated based on images taken at appropriate shooting intervals, the position of the moving body can be estimated more accurately. Moreover, since useless calculation with low accuracy can be omitted, the amount of calculation for estimating the position of the moving body can be reduced. In addition, when the image capturing apparatus captures only images used for calculation, the data capacity and the communication amount can be reduced.

  The interval determination unit may adopt the shorter one of the shooting interval calculated based on the moving speed of the moving body and the shooting interval calculated based on the changing speed of the direction of the moving body. According to this aspect, since a more appropriate shooting interval is adopted according to the movement mode of the moving body, the position of the moving body can be estimated more accurately.

  Another embodiment of the present invention is a mobile object. The moving body includes a photographing device for photographing the periphery of the moving body, a position estimating unit that estimates the position of the moving body, a control unit that controls the moving body based on the position estimated by the position estimating unit, Is provided. The position estimation unit includes a speed acquisition unit that acquires the moving speed of the moving body provided with the imaging device, a direction acquisition unit that acquires the orientation of the moving body, and a moving body that is imaged by the imaging device while the moving body is moving. A calculation unit that calculates the estimated position of the moving body based on the surrounding image, and the calculation unit to estimate the position of the moving body based on the moving speed of the moving body or the changing speed of the direction of the moving body An interval determination unit that determines an imaging interval of an image around the moving body to be used. The computing unit computes the estimated position of the moving object based on the images taken at the photographing interval determined by the interval determining unit.

  Also according to this aspect, since the estimated position of the moving body is calculated based on images captured at appropriate shooting intervals, the position of the moving body can be estimated more accurately. Moreover, since useless calculation with low accuracy can be omitted, the amount of calculation for estimating the position of the moving body can be reduced. In addition, when the image capturing apparatus captures only images used for calculation, the data capacity and the communication amount can be reduced.

  In the above, this invention was demonstrated based on the Example. This embodiment is an exemplification, and it will be understood by those skilled in the art that various modifications can be made to each of those constituent elements or combinations of processing processes, and such modifications are also within the scope of the present invention. .

  In the embodiment shown in FIG. 1 and FIG. 2, the stereo camera is directed and fixed in front of the front of the vehicle. However, in another example, the stereo camera is in a direction other than the front of the vehicle, for example, slightly to the left. It may be directed and fixed. In this case, even if there is an oncoming vehicle on the right side of the vehicle, the target existing on the left side of the road can be captured more reliably, but between the images captured every unit time The amount of displacement of the target existing on the left side of the road is larger than when the stereo camera is directed toward the front of the vehicle. Therefore, images may be taken with a stereo camera at shorter shooting intervals. The shooting interval in this case may be determined so that the afterimage rate is equal to or greater than a predetermined value, as in the case where the vehicle turns a curve. In a country or region where the vehicle is regulated to travel on the right side of the road, the stereo camera may be directed slightly to the right. A camera with a variable orientation may be mounted on the vehicle. Even in this case, the position of the vehicle can be estimated more accurately while suppressing the amount of calculation by using images taken at an appropriate shooting interval.

  In the embodiment, both the shooting interval determined based on the vehicle speed of the vehicle and the shooting interval determined based on the yaw angular velocity of the vehicle are calculated, and the shorter one of them is adopted. Even when the vehicle is traveling autonomously, both the vehicle speed and the yaw angular velocity are very high, that is, it is usually not considered to bend at a high speed. Therefore, in another example, when the vehicle speed is equal to or higher than a predetermined threshold, the calculation of the shooting interval based on the yaw angular velocity is omitted, and the shooting interval is determined based only on the vehicle speed, and the vehicle speed is less than the predetermined threshold. Only in this case, the photographing interval determined with reference to the yaw angular velocity of the vehicle may be considered. In addition, lost feature points due to changes in yaw angle typically change significantly when the vehicle is turning around a curve or intersection, or when making a U-turn or parking. Therefore, when the yaw angular velocity is less than the predetermined threshold, the calculation of the shooting interval based on the yaw angular velocity is omitted, and the shooting is performed based only on the vehicle speed. The shooting interval determined based on the yaw angular velocity of the vehicle may be considered only when the interval is determined and the yaw angular velocity is equal to or greater than a predetermined threshold.

  DESCRIPTION OF SYMBOLS 10 Vehicle, 20 Control part, 30 Storage part, 40 Vehicle drive system, 42 Vehicle measurement part, 44 Vehicle information sensor, 46 Stereo camera, 50 Position estimation part, 52 Speed reference FPS calculation part, 54 Angular speed reference FPS calculation part, 56 FPS determination unit, 58 SfM calculation unit.

Claims (3)

Computer
A speed acquisition unit for acquiring the moving speed of the moving body provided with the imaging device;
A direction acquisition unit for acquiring the direction of the moving body;
A computing unit that computes an estimated position of the moving body based on an image of the surroundings of the moving body captured by the imaging device during the movement of the moving body;
Based on the moving speed of the moving body or the changing speed of the direction of the moving body, the calculation unit determines an imaging interval of an image around the moving body used for estimating the position of the moving body. Interval determination unit,
Function as
The position calculation program characterized in that the calculation unit calculates an estimated position of the moving body based on images taken at a shooting interval determined by the interval determination unit.   The interval determination unit adopts a shorter one of the shooting interval calculated based on the moving speed of the moving body and the shooting interval calculated based on the changing speed of the direction of the moving body. The position estimation program according to claim 1. A photographing device for photographing the surroundings of the moving object;
A position estimation unit for estimating the position of the moving body;
A control unit that controls the moving body based on the position estimated by the position estimation unit;
With
The position estimation unit
A speed acquisition unit that acquires the moving speed of the moving object provided with the imaging device;
A direction acquisition unit for acquiring the direction of the moving body;
A calculation unit that calculates an estimated position of the moving body based on an image of the surroundings of the moving body captured by the imaging device during the movement of the moving body;
Based on the moving speed of the moving body or the changing speed of the direction of the moving body, the calculation unit determines an imaging interval of an image around the moving body used for estimating the position of the moving body. An interval determination unit;
With
The said calculating part calculates the estimated position of the said moving body based on the image image | photographed with the imaging | photography interval determined by the said space | interval determination part.

Images