JP7015506B2 - Positioning method using landmarks - Google Patents

Description

The present invention relates to a positioning method for measuring the current position of a moving object, and more particularly to a positioning method using landmarks.

Various positioning methods are known for measuring the current position of a moving object. For example, a positioning method for measuring a self-position by positioning using GPS (Global Positioning System) is known.

Further, as described in Patent Document 1, even in the positioning method using GPS, when the transmission wave from the positioning satellite cannot be received, the information obtained from the inertial device or the like is used. A positioning method is known in which the estimation accuracy of the self-position is improved by correcting the self-position. In the self-position estimation method, feature points are acquired from a DBS (Doppler Beam Sharpening) image obtained by radar, and the relationship between the acquired feature points and a SAR (Synthetic Aperture Radar) map obtained in advance is determined. By mutual reference, the self-position is estimated using the position information of the corresponding known landmark on the SAR map and the distance information to the feature point obtained on the DBS image corresponding to the known landmark. There is.

Japanese Unexamined Patent Publication No. 2014-174070

However, in the above-mentioned conventional positioning method, when a moving object sneaks into a pier or a pier, or when moving indoors, the position of the moving object is always in an environment where a positioning method using GPS or the like cannot be used. Since it is unknown, there is a problem that it is very difficult to guide a moving body safely and surely. In addition, when the moving body involves some work, the working position cannot be managed, which is a fatal problem for the utilization of the moving body. Such a problem is a big obstacle to the introduction of mechanization in an environment where the above-mentioned positioning method using GPS cannot be used.

Therefore, the present invention has been made in view of the above problems, and provides a positioning method capable of measuring the current position of a moving object even in an environment where an existing positioning method such as GPS cannot be used. The purpose is.

The positioning method according to the present invention is a positioning method in which the self-positioning of a moving body is performed by using the landmark information included in the map information, and the information on the surrounding environment of the moving body is accompanied by the movement of the moving body. The peripheral environment information acquisition means for continuously acquiring the above, the feature point extraction means for extracting one or two or more feature points from the peripheral environment information acquired at an arbitrary time by the peripheral environment information acquisition means, and the feature points. The moving body includes the landmark acquisition means corresponding to the landmark information included in the map information acquired in advance, and the moving body includes the surrounding environment information acquisition means, the direction acquisition means for acquiring its own direction, and the surrounding environment. It is provided with a relative motion information acquisition means for acquiring the relative motion information of the above, and when two of the feature points can be acquired at any time, the distance and the direction from each feature point obtained by the surrounding environment information acquisition means. The self-position is estimated from the first positioning means for positioning the self-position using the above, the distance from the feature point when one feature point can be acquired, and the self-direction obtained by the direction acquisition means. The second positioning means is provided, and the third positioning means for estimating the self-position from the relative motion information obtained by the relative motion information acquisition means and the previous measurement result when the feature point cannot be acquired is provided. The first positioning means, the first positioning means, comprising a switching means for switching between the first positioning means, the second positioning means, and the third positioning means according to the number of feature points acquired at an arbitrary time. Using the self-position at an arbitrary time obtained by the second positioning means or the third positioning means, the position information of the feature point at the time of the next positioning is estimated and the feature point is tracked. do.

Further, in the positioning method according to the present invention, the self-position information estimated by the second positioning means and the third positioning means becomes the self-positioning information when the first positioning means is executed. It is preferable to be corrected.

Further, in the positioning method according to the present invention, it is preferable to include an initial position acquisition means for acquiring the initial position of the moving body.

The outline of the above invention does not list all the necessary features of the present invention, and subcombinations of these feature groups can also be an invention.

The positioning method according to the present invention includes a landmark acquisition means for associating a feature point with a landmark information included in the map information acquired in advance, and the moving body acquires the surrounding environment information acquisition means and its own direction. It is equipped with a direction acquisition means and a relative motion information acquisition means for acquiring relative motion information with the surrounding environment, and when two feature points can be acquired at an arbitrary time, from each feature point obtained by the surrounding environment information acquisition means. The self-position is determined by the first positioning means for positioning the self-position using the distance and direction of, and the distance from the feature point and the self-direction obtained by the direction acquisition means when one feature point can be acquired. Since it is provided with a second positioning means for estimating and a third positioning means for estimating the self-position from the relative motion information obtained by the relative motion information acquisition means when the feature point cannot be acquired and the previous measurement result. Even in an environment where existing positioning methods such as GPS cannot be used, it is possible to perform positioning of a moving object with extremely high effectiveness.

The figure for demonstrating the outline of the positioning method which concerns on embodiment of this invention. The flow chart of the positioning method which concerns on embodiment of this invention. The figure for demonstrating the outline of the moving body which concerns on embodiment of this invention. The figure explaining the outline of the self-position estimation method. The flow chart of the first positioning means. The flow chart of the second positioning means. The flow chart of the third positioning means. The figure explaining the outline of the 3rd positioning means. The figure for demonstrating the positioning method of the moving body which concerns on embodiment of this invention.

Hereinafter, suitable embodiments for carrying out the present invention will be described with reference to the drawings. It should be noted that the following embodiments do not limit the invention according to each claim, and not all combinations of features described in the embodiments are essential for the means for solving the invention. ..

FIG. 1 is a diagram for explaining an outline of a positioning method according to an embodiment of the present invention, FIG. 2 is a flow chart of a positioning method according to an embodiment of the present invention, and FIG. 3 is a flow diagram of the present invention. It is a figure for demonstrating the outline of the moving body which concerns on embodiment, FIG. 4 is a figure explaining the outline of the self-position estimation method, and FIG. 5 is a flow diagram of the 1st positioning means. 6 is a flow chart of the second positioning means, FIG. 7 is a flow chart of the third positioning means, FIG. 8 is a diagram illustrating an outline of the third positioning means, and FIG. 9 is a diagram. It is a figure for demonstrating the positioning method of the moving body which concerns on embodiment of this invention.

As shown in FIG. 1, in the positioning method according to the present embodiment, measurement and observation are performed while the moving body 10 moves on the lower surface of a structure 1 such as a pier in an environment where positioning technology such as GPS cannot be used. It is preferable that it is used as a positioning method for positioning the position of the moving body by using the measurement result of the surrounding environment such as the pile 20 when performing such work.

As shown in FIG. 2, the positioning method according to the present embodiment includes peripheral environment information acquisition means for continuously acquiring information on the surrounding environment of the moving body 10 as the moving body 10 moves, and acquisition of the peripheral environment information. A feature point extraction means for extracting one or two or more feature points from the surrounding environment information acquired at an arbitrary time by means, and a landmark acquisition means for associating the feature points with the landmark information included in the map information acquired in advance. The moving body 10 is provided with a peripheral environment information acquisition means, a direction acquisition means for acquiring its own direction, and a relative motion information acquisition means for acquiring relative motion information with the surrounding environment, and a feature point is provided at an arbitrary time. When two can be acquired, the first positioning means for positioning the self-position using the distance and direction from each feature point obtained by the surrounding environment information acquisition means, and the case where one feature point can be acquired. The second positioning means that estimates the self-position based on the distance from the feature point and the self-direction obtained by the direction acquisition means, and the relative motion obtained by the relative motion information acquisition means when the feature point cannot be acquired. A third positioning means for estimating the self-position from the information and the previous measurement result is provided.

As shown in FIG. 3, the moving body 10 includes a surrounding environment information acquisition means 11, an orientation acquisition means 12 for acquiring its own direction, and a relative motion information acquisition means 13.

As shown in FIG. 4, the surrounding environment information acquisition means 11 may have any configuration as long as it can acquire information on the surrounding environment of the mobile body 10, but for example, the side such as an LRF (laser range finder). It is preferable to use a range sensor. The LRF is an optical device that can oscillate a laser, irradiate the periphery of the moving body 10 with the laser, and acquire / measure information on the surrounding environment based on the degree of reflection thereof. The surrounding environment information acquisition means 11 can scan the surrounding environment as shown in FIG. 4 by irradiating the moving body 10 with a laser or the like in two directions.

The direction acquisition means 12 may have any configuration as long as it can acquire its own direction, but for example, it is preferable to use a gyro sensor. Further, the relative motion information acquisition means 13 is provided with a camera for image processing the lower surface of the measurement target such as a pier or the sea surface.

Further, as shown in FIG. 2, the positioning method according to the present embodiment includes an initial position acquisition means for acquiring the initial position of the moving body 10 when the positioning is determined to be the first time by the loop determination. ing. Since this initial position acquisition means is usually performed before moving to a measurement target such as a pier, the moving body 10 has not yet entered the lower surface of the measurement target, and position information such as GPS can be used. Therefore, it may be executed by acquiring the position information from GPS. Further, instead of acquiring the position information from the GPS, the initial position of the moving body 10 in the map information may be manually input.

Next, the feature point extraction means and the landmark acquisition means will be described with reference to FIG. The feature point extraction means selects an obstacle from the scanning result 31 obtained by the surrounding environment information acquisition means 11 and observes the selected obstacle as the feature point 32. The scanning of the surrounding environment is continuously performed with the movement of the moving body 10, and the observation of the feature point 32 acquires the information of the surrounding environment acquired at an arbitrary time in chronological order.

The landmark acquisition means matches the map information 30 including the landmark information 33, which is the position information of obstacles such as stakes, with the scanning result 31 obtained by the surrounding environment information acquisition means 11 by performing correlation processing or the like. Therefore, the scanning result 31 is mapped to the map information 30. As a result, the feature point 32 and the landmark information 33 are associated with each other.

The switching means 14 is any of the first positioning means, the second positioning means, and the third positioning means according to the number of feature points 32 acquired at an arbitrary time obtained by the surrounding environment information acquisition means 11. The positioning means is being switched. Specifically, when two or more feature points 32 are acquired, the first positioning means is switched to, and when one feature point 32 is acquired, the second positioning means is switched to, and the feature point 32 is switched to. If it cannot be obtained, it switches to the third positioning means.

As shown in FIG. 5, the first positioning means performs positioning by using a so-called lighthouse method. Specifically, two features are obtained in order from the obtained feature points 32 to those closest to the moving body 10. A point is selected, and the observation direction of one of the most recent feature points 32 (the direction of the most recent feature point seen from the moving body 10) and the length of the line segment connecting that point to the moving body 10 (feature point 32). The distance from to the moving body 10) is obtained. Further, the angle formed by the X-axis in the coordinate system of the moving body (hereinafter referred to as the R coordinate system) and the line segment connecting the above-mentioned two feature points is calculated.

Next, the absolute orientation of the moving body 10 in the world coordinate system (hereinafter referred to as W coordinate system) is calculated. Specifically, the difference between the azimuth angle of the line segment connecting the feature point 32 to the moving body 10 in the W coordinate system and the angle formed by the line segment calculated as described above is obtained with respect to the W coordinate system. The angle formed by the R coordinate system is calculated to calculate the absolute orientation of the moving body 10.

Next, the position of the moving body 10 in the W coordinate system is calculated by converting from the R coordinate system to the W coordinate system. Specifically, by back-calculating the absolute position of the moving body 10 from the relative position between the angle formed by the R coordinate system (absolute orientation of the moving body 10) and the nearest feature point with respect to the already calculated W coordinate system. The position of the moving body 10 is calculated.

As described above, since the first positioning means can calculate its own position information from the position of the feature point 32 in the R coordinate system, the positioning error is small, and the feature point is continuously observed and the positioning is continued. Even in the case, the cumulative error is zero in principle.

As shown in FIG. 6, the second positioning means acquires the estimated direction of the moving body 10 from the direction acquisition means 12 when one feature point 32 is acquired. Next, the absolute orientation of the moving body 10 in the W coordinate system is calculated. Specifically, the angle formed by the R coordinate system with respect to the W coordinate system is calculated from the orientation acquisition means 12, and the absolute orientation of the moving body 10 in the W coordinate system is calculated from the angle formed by the R coordinate system with respect to the W coordinate system. When the direction acquisition means 12 is a device that outputs a relative direction such as a gyro, the absolute direction at the time when the feature point becomes one point or less is held as the initial value of the estimated direction and acquired here. The orientation of the moving body 10 is estimated by adding it to the initial value of the estimated orientation that holds the relative orientation.

Next, the estimated position of the moving body 10 in the W coordinate system is calculated by converting from the R coordinate system to the W coordinate system. Specifically, the absolute position of the moving body 10 is calculated back from the relative position between the angle formed by the R coordinate system (absolute orientation of the moving body 10) and the nearest feature point with respect to the already calculated W coordinate system. Estimate the position of body 10.

As described above, the second positioning means can acquire the estimated direction by the direction acquisition means 12 even when only one feature point is obtained, so that the second positioning means is the latest as in the first positioning means. The self-position can be estimated from the relative position of the feature points of.

As shown in FIG. 7, when the feature points cannot be obtained at all, the third positioning means acquires the estimated directional of the moving body 10 from the azimuth acquiring means 12 in the same manner as the second positioning means, and in the W coordinate system. The absolute orientation of the moving body 10 is calculated. In addition, the absolute position at the time when no feature points are obtained is retained as the initial value of the estimated position.

Next, the absolute position of the moving body 10 is switched to the estimated position by dead reckoning using inertial navigation, relative motion information with respect to the surrounding environment, and the like. The relative motion information acquisition means is a moving body 10 by performing image processing using optical flow on an image obtained from a camera provided in the moving body 10 as shown in Doppler velocity log, odometry, or FIG. The translational speed and turning speed of the camera are estimated, and the relative motion information with the surrounding environment is acquired. The optical flow is a vector representation of the movement of an object (lower part of a floor plate such as a pier) in images continuously acquired by a camera, and the translational speed and turning speed of the moving body 10 are calculated from this vector. .. As a result, the relative motion information with respect to the surrounding environment such as the obtained translational speed and turning speed is accumulated, and the position of the moving body 10 is estimated by adding the accumulated value to the initial value of the estimated position.

After that, the positioning data is updated. To update the positioning data, the landmark database of the R coordinate system is updated by converting all the landmark coordinates in the database held in the W coordinate system into the R coordinate system.

Further, when the first positioning means is executed, the estimated orientation and the estimated position obtained by the second positioning means or the third positioning means are updated with the current absolute direction and the absolute position, so that the self can be used. It is corrected to the positioning information. Since the estimated azimuth and the estimated position are usually cumulative values of observed values such as rate and relative velocity, they include a cumulative error. Therefore, this correction can eliminate the cumulative error and improve the accuracy of positioning.

Next, with reference to FIG. 9, the positioning method of the mobile body 10 according to the present embodiment will be described in detail. As described above, the moving body 10 observes the feature point 32 and acquires it in time series as information on the surrounding environment acquired at an arbitrary time. That is, the observations shown in the positioning steps T to T + 2 in FIG. 9 are continuously performed. In the following description, an example in which landmarks in the W coordinate system are included as map information from (1) to (4) will be described.

First, in the positioning step T, since two or more feature points 32 are observed, the moving body 10 is positioned by the first positioning means described above, and the feature points 32 and the landmark coordinates are collated. , The landmarks in the W coordinate system are collated. That is, the observed feature points 32 and the landmarks (1) to (3) in the W coordinate system are collated.

Next, in the positioning step T + 1 performed after a minute time from the positioning step T, two feature points 32 are observed. Since these two points are landmarks (1) and (2) that overlap with the collation result of the previous positioning step T, they were observed in the positioning step T + 1 with the landmarks (1) and (2) collated in the positioning step T. It is presumed that the landmarks (1) and (2) are the same, and the landmarks (1) and (2) can be tracked in the positioning step T + 1.

Next, a case where there are no overlapping landmarks such as the positioning step T + 1 and the positioning step T + 2 will be described. In the positioning method according to the present embodiment, the geometrical positional relationship between the landmarks (3) and (4) that were not detected in the positioning step T + 1 on the map via the map information in each positioning step. Is estimated. Therefore, in the positioning step T + 2, the observed feature points 32 can be collated with the landmarks (3) and (4), and the landmarks (1) to (4) can be tracked.

As described above, since the positioning method according to the present embodiment estimates the positions of all the landmarks in the map information in each positioning step, the landmarks other than the landmarks that were successfully collated in the immediately preceding positioning step Can also be tracked, and a wide range of positioning can be realized. That is, the positioning method according to the present embodiment realizes wide-range positioning by performing positioning such as walking around while handing over a plurality of existing landmarks.

As described above, according to the positioning method according to the present embodiment, even in an environment where positioning technology such as GPS cannot be used, positioning is possible as long as the map information including landmark information is provided, and GPS and the like can be used. Even in an environment where the positioning technology of is not available, it is possible to mechanize many tasks.

Further, according to the positioning method according to the present embodiment, the map information having the landmark information with respect to the position of the landmark installed in advance is possessed, and the position of the landmark is based on the geometrical positional relationship between the landmarks. Since the above are distinguished, it is possible to perform positioning without separately preparing a feature for distinguishing between individual landmarks.

Further, according to the positioning method according to the present embodiment, since the first to third positioning means are switched according to the number of available landmarks, it is possible to flexibly change the positioning method from the obtained information. In addition, even if the number of available landmarks decreases, it can be compensated for by other upwards, so that positioning can be performed more flexibly.

The positioning method according to the present embodiment described above has been described, for example, in the case where image processing is performed while photographing the floor plate surface of the pier with the relative motion information acquisition means, but the relative motion information acquisition means captures the images. The object to be processed is not limited to the floor plate surface, and image processing may be performed while photographing, for example, the sea floor. Further, a program for executing the positioning method according to the present embodiment can be loaded into a computer and executed. It is clear from the description of the claims that the form with such changes or improvements may be included in the technical scope of the present invention.

10 Moving object, 11 Surrounding environment information acquisition means, 12 Direction acquisition means, 13 Relative motion information acquisition means, 20 Pile, 30 Map information, 31 Scanning result, 32 Feature point, 33 Landmark information.

Claims (3)

It is a positioning method that uses landmark information included in map information to determine the self-position of a moving object.
Peripheral environment information acquisition means for continuously acquiring information on the surrounding environment of the moving body as the moving body moves, and
A feature point extraction means for extracting one or more feature points from the surrounding environment information acquired at an arbitrary time by the surrounding environment information acquisition means, and a feature point extraction means.
It is provided with a landmark acquisition means for associating the feature point with the landmark information included in the map information acquired in advance.
The moving body includes the peripheral environment information acquisition means, the direction acquisition means for acquiring its own direction, and the relative motion information acquisition means for acquiring relative motion information with the surrounding environment.
When two of the feature points can be acquired at an arbitrary time, the first positioning means for positioning the self-position using the distance and the direction from the respective feature points obtained by the surrounding environment information acquisition means. When one of the feature points can be acquired, the second positioning means for estimating the self-position based on the distance from the feature points and the self-direction obtained by the direction acquisition means, and the feature points cannot be acquired. In this case, a third positioning means for estimating the self-position from the relative motion information obtained by the relative motion information acquisition means and the previous measurement result is provided.
A switching means for switching between the first positioning means, the second positioning means, and the third positioning means according to the number of feature points acquired at an arbitrary time is provided.
Using the self-position at an arbitrary time obtained by the first positioning means, the second positioning means, or the third positioning means, the position information of the feature point at the time of the next positioning is estimated and the feature point . A positioning method characterized by tracking . In the positioning method according to claim 1,
A positioning method characterized in that its own position information estimated by the second positioning means and the third positioning means is corrected to its own positioning information when the first positioning means is executed. .. In the positioning method according to claim 1 or 2,
A positioning method including an initial position acquisition means for acquiring the initial position of the moving body.

Images