JP2019074532A - Method for giving real dimensions to slam data and position measurement using the same - Google Patents

Abstract

To provide a new technique that does not generate drift with respect to the time axis in the self-position estimation and environmental map by SLAM (three-dimensional point cloud data) in a real dimension estimation using an IMU and to provide a position measurement system capable of carrying out a process in a small place in a short processing time which does not depend on the GNSS using the technique.SOLUTION: In the method of providing real dimensions to the self-position estimation and environmental map by an SLAM, a camera and an IMU are installed in a form where the relative position is fixed, the acceleration is measured by a video shooting with the camera and the IMU in the movement, the captured video is exported as a still image for each frame, the captured image is analyzed by SfM, the self-relative position of the camera is determined to obtain the acceleration of the camera by a second-order differentiation and to obtain the acceleration of the camera, and the scale coefficient is calculated by synchronizing the acceleration of the camera and the acceleration information obtained from the IMU and by multiplying the self-relative position of the camera by the scale factor.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a technique for applying actual dimensions to self-location and environmental map (three-dimensional point group data) information estimated by SLAM (Simultaneous Localization and Mapping), and position measurement using the same.

  SLAM (Simultaneous Localization and Mapping) is a technology that simultaneously estimates self-location and creates an environmental map from images and sensor information, etc., and important technologies such as autonomous mobile robots, autonomous vehicles, and augmented reality. It has become to. In order to recognize an unknown environment, a robot moving autonomously grasps the shape of the surrounding environment by an image or a sensor, and estimates its own position based on the shape data. It will make an environmental map and move while estimating your position and making corrections. One of the methods to estimate the movement distance is odmetry. A method of estimating the position of a robot by obtaining the amount of movement from the number of rotations and the angle of rotation of a wheel-type mobile robot, and also called dead reckoning. However, it can not be detected when the wheel idles, and the movement amount includes an error, and there is a problem that the error is further added on the error, and the robot can move for a long distance with odometry alone. In such a case, the position shifts as the vehicle travels, and environmental maps such as surrounding obstacles are also affected by the accumulated error.

  GNSS, that is, "Global Navigation Satellite System (s)" "as a method to add actual position and absolute position information on the earth coordinates to the self position and environmental map (three-dimensional point group data) information estimated by SLAM described above. It is well known to use information obtained from the Global Navigation Satellite System. The position information of a plurality of specific points of the point cloud data is obtained by GNSS, and is associated with the earth coordinate position. However, since this method can not be adopted where radio waves from satellites do not reach, there is also known a method of giving actual dimensions by an inertial measurement unit (hereinafter abbreviated as IMU) under such conditions. . The inertial device comprises an accelerometer and a gyro, and the acceleration is a rate of change of velocity, which occurs whenever an object moves. Integration of this acceleration once results in velocity, and integration once more results in distance. Therefore, by arranging the accelerometers in three axes of horizontal (X, Y) and vertical (Z) and integrating the outputs twice, it is possible to know the movement distance (position) from the start point of each axis. The inclination of the moving axis is measured by a gyro. Similar to the accelerometers, the gyros are also arranged in three axes, and the inclination (attitude angle) of each axis can be known by integrating the outputted angular velocity once. However, this IMU method has a problem that the drift error is integrated and becomes large as time is accumulated.

  Patent Document 1 discloses a SLAM mapping technique that detects together the position of a feature point in an image input from a camera and the position and orientation of the camera. In this document, camera position and orientation information having an existence probability distribution according to a normal distribution is generated based on an acquired image of the camera, and further, a feature point tracking error distribution according to the normal distribution based on the acquired image It generates the existence probability distribution in a three-dimensional space of feature points by applying the camera position and posture information having the existence probability distribution according to the normal distribution and the feature point tracking error distribution information according to the normal distribution An approach is disclosed.

  Patent Document 2 discloses a method of specifying overall position information of a route traveled by the measurement vehicle by completely correcting the sequentially calculated positional information by a three-dimensional SLAM algorithm. This document is a method to calculate the position and attitude of a measuring vehicle traveling in a pipeline, using odometry and IMU (Inertial Device) and SLAM, which calculate the distance from the number of rotations of the tire. IMU analysis is considered to be integral. Since the analysis of IMU is by integration, errors accumulate over time and there is no accuracy. Correcting the IMU error with odometry does not improve accuracy much.

  In addition, the applicant attached a camera and an inertial device to a traveling body, and provided a method of giving actual dimensions to three-dimensional point group data obtained from a plurality of photographs having different viewing angles, and a position of a conduit etc. The measurement was filed earlier (Japanese Patent Application No. 2016-80882). According to the present invention, the camera and the IMU are installed at a fixed relative position, and the moving image shooting with the camera and the measurement of the acceleration by the IMU are performed with movement, and the captured moving image is written and recorded as a still image for each frame. Of the image by SfM to determine the camera's own relative position, second-order differentiation of the camera's own relative position to determine the camera's acceleration, and synchronization of the camera's acceleration with the acceleration information obtained from the IMU The scale factor is obtained by multiplying the scale factor by the self relative position of the camera. Since the method of the present invention compares the measured data directly with the camera acceleration without integrating the acceleration data into the second order, the drift error is extremely small compared to that by the conventional IMU. Although the effect is achieved, there is a problem that the processing time becomes long when data processing is performed on the entire three-dimensional point cloud data.

[Patent Document 1] Japanese Patent Application Publication No. 2008-304269 "Information processing apparatus, information processing method, and computer program" released on December 18, 2008 JP, 2015-25753, A "measurement device of buried pipeline, and measurement method of buried pipeline" February 5, 2015 release

  An object of the present invention is to provide a new method which does not generate a drift with respect to a time axis in self-localization by SLAM and in adding actual dimensions to an environmental map (three-dimensional point group data) using IMU. Another object of the present invention is to provide a position measurement system which is not dependent on GNSS and can be implemented in a narrow place using the method.

  A further object of the present invention is to shorten the time for position measurement combined with IMU by adopting a rapid data processing method by SLAM as self-position estimation information.

  The method of self-position estimation by SLAM according to the present invention and the method of giving actual dimensions to an environmental map (three-dimensional point group data) is performed by installing a camera and an IMU in a fixed relative position form and capturing moving images with the camera And IMU measure the acceleration, write the captured moving image as a still image for each frame, analyze the captured image by SfM to determine the camera's relative position, and second-order differentiate the relative position of the camera The acceleration of the camera is determined, the acceleration of the camera and the acceleration information obtained from the IMU are synchronized to obtain a scale factor, and the self relative position of the camera is multiplied by the scale factor.

  The method of estimating actual position by SLAM according to the present invention and assigning actual dimensions to an environmental map (three-dimensional point group data) is characterized in that the traveling body performs measurement while repeating increase and decrease of acceleration.

  In the position measurement method of the present invention, the form in which the relative position between the camera and the IMU is fixed is installation on a traveling body, and the movement is made to travel in the target area by the traveling body. The pipeline position is to be grasped from the position estimation information.

The method of self-position estimation by SLAM according to the present invention and the method of giving actual dimensions to an environmental map (three-dimensional point group data) is to compare measured data with camera's acceleration as it is without integrating acceleration data into second order. Therefore, the drift error is extremely small compared to that by the conventional IMU.
The method of self-position estimation by SLAM according to the present invention and the method of giving actual dimensions to an environmental map (three-dimensional point group data) performs feature point tracking without applying the SfM method to all point group data. The data processing time can be significantly shortened by reducing the number of operations.
The synchronization method of the present invention makes it possible to synchronize the camera and the IMU without using a conventionally required synchronization device.
The measurement method of the present invention is highly practical because it does not need to be stopped halfway as in conventional inertial photogrammetry if appropriate acceleration and deceleration are obtained.
This method also contributes to the improvement of the positioning accuracy in the inertial measurement in which the drift phenomenon is a drawback.

An example of feature point detection is shown, and the figure shown on the left is a normal captured image, and the figure on the right is an image in which detected feature point sites are plotted. It is the image by which the feature point site | part which imaged the underground tunnel and was detected was plotted. It is a figure which shows the operation | movement flow of this invention. A of a figure is a graph which shows the change of the acceleration value of the planar direction by the time difference of a camera and IMU, B is a graph which shows calculation of a synchronous position. It is a graph which shows the example of a waveform containing the noise before filter passing. It is a graph which shows the example of a waveform which removed the noise after filter passing. It is a graph explaining the determination method of the scale by correlation. It is a measuring device in which a camera, an IMU and a laptop computer are fixedly installed on the wagon. It is SLAM data which showed the locus | trajectory of the camera position estimated as the feature point group which SLAM sampled. The locus | trajectory of the camera position which gave real dimension to SLAM data is shown by the top view and the graph of a side view. It is what wrote the result which gave the actual size to SLAM data on the map. The measurement apparatus which fixedly attached the camera and IMU on the notebook personal computer, and the measurement form which a person holds and performs it are shown. The underground passage which carried out experiment and the feature point which SLAM sampled are shown. The horizontal locus of the camera center position obtained by this experiment is obtained from the x-axis information and y-axis information of the earth coordinate system, and is superimposed and displayed on the map of this area where the building information is written.

  The self-positioning technology for mobiles, which has been performed in the field of position measurement and robot position control by mapping technology in the field of surveying, and in the field of automatic operation of automobiles, has made its own progress. However, both techniques are closely related in terms of position measurement using mapping technology. The present invention is based on the application of SLAM technology, which has not been widely used in the field of surveying, to position measurement of the surrounding environment based on the moving position of a traveling body.

There are various algorithms such as PTAM, LSD-SLAM, ORB-SLAM, etc., although there are various algorithms such as PTAM, LSD-SLAM, ORB-SLAM, and several kinds of algorithms for detection of feature points in photographed images are known. In the present embodiment of the invention presented here, ORB-SLAM was used, and feature point detection in ORB-SLAM was made to use the FAST algorithm.
An experimental example of feature point detection is shown in FIG. The figure shown on the left is a normal captured image, the figure shown on the left is a normal captured image, and the figure on the right is an image in which detected feature point sites are plotted. The present invention gives a real scale to this group of points. Moreover, what is shown in FIG. 2 is an image of an underground water channel, and the characteristic points can be detected even if the shape is a flat wall-like surface.

The theory of the present invention is the theory of the method of giving an actual dimension to this point group, and FIG. 3 shows the flow of analysis in the present invention.
In the present invention, it is essential that the camera and the inertial unit (IMU) be in a form in which the relative position is fixed to each other, and it is not essential to place the camera on the traveling vehicle. An example in which an IMU equipped with a camera, an accelerometer, and a gyro is mounted on the body will be described as an example. The shooting and measurement are performed. First, moving image shooting with a camera and measurement of acceleration with an IMU are performed. Since a change in acceleration does not occur during constant-speed linear traveling, the traveling body travels at an equal speed and performs measurement while obtaining acceleration information. Captured moving images are exported as still images frame by frame. The detected point group is analyzed by SfM to obtain relative three-dimensional point group data of the object and the position and tilt of the camera. The external orientation element of the camera according to SfM will be described. Expression (1) is an expression of perspective transformation representing the relationship between a two-dimensional point (left side) shown in the image and an actual three-dimensional point (right side). The 3 × 3 matrix including the focal length f on the right side is an internal measurement element (lens distortion, principal point position, etc.) of the camera. The parameters are determined by performing camera calibration. The 3 × 4 matrix including the camera position t and the inclination r on the right side is an external orientation element of the camera.
When SfM is analyzed without giving reference coordinate values as initial conditions, the obtained external orientation factor of the camera and the 3D point group data are similar to the original model because the scale factor s is uncertain. Is generated. Here, the second derivative of the calculated position of the camera with respect to time is defined in alignment with the acceleration of the camera. In order to obtain the scale factor, it is necessary to synchronize the camera and the IMU.

Next, a method of synchronizing the camera and the IMU will be described. In the present invention, synchronization is achieved by the acceleration of the camera and the IMU without using the synchronization device. FIG. 4 is a graph showing the acceleration of the camera and the IMU. In A, the broken line is the camera and the solid line is the acceleration of the IMU. Two are two graphs with different scales, and the acceleration of the camera and IMU used for synchronization is the acceleration in the plane direction of equation (2).
Here, assuming that the camera is not synchronized, the camera acceleration is shown as being obtained with a delay of 15 seconds. B of FIG. 4 is the calculation result calculated | required by Formula (3). In the figure, Σ (camera × IMU) is obtained by multiplying the acceleration in the plane direction of the IMU by the acceleration in the plane direction of the camera. The time when the value At is the largest is determined by equation (3). The time is determined by polynomial approximation as shown in B of FIG. 4 and the time calculated in more detail is determined as the synchronization position. In the graph, the symbol ◆ indicates the original data obtained by multiplying the camera and the acceleration of the IMU, and the symbol ○ indicates data used for the polynomial.
Specifically, looking at B in FIG. 4, from the convex vertex of the polynomial, the time difference between the start of activation of the camera and the IMU is 15 seconds. This indicates that since the delay of activation of the camera initially set is 15 seconds, the time obtained by the present method is correct, and it is possible to obtain an accurate synchronization position.
In order to perform synchronization, it is preferable to use as noiseless data as possible. Therefore, Fourier transform is performed to remove noise. FIG. 5 is waveform data including noise. FIG. 6 shows waveform data from which noise components are removed by fast Fourier transform. In this method, synchronization processing is performed while changing the adopted Fourier spectrum, and a waveform with the largest value of camera × IMU is adopted.

Next, to explain the determination of the scale factor for achieving absolute dimensioning, in order to obtain the scale factor using the synchronized camera and the acceleration of the IMU, the magnification at which the camera acceleration is equal to the acceleration of the IMU may be determined. Therefore, if correlation is taken using the camera and the acceleration of IMU, the scale factor can be considered as a polynomial of equation (4). The scale factor (s) has the same meaning as the term a, in order to find the slope of y = x. Therefore, s can be obtained by obtaining a.
In the correlation, noise greatly influences the determination of the scale factor. FIG. 5 is a correlation diagram in which an acceleration of a camera including an outlier and an IMU are assumed. The small points are randomly generated as outliers, and the circled points are correct observation values. The broken line is a linear regression obtained from data including outliers. Originally, I would like to find an approximate curve with a slope of 1: 1. However, due to the influence of outliers, an accurate slope can not be determined.

Therefore, outliers are rejected using RANSAC (RANdom SAmple Consensus) to provide robustness. Unlike the least squares method, RANSAC is a robust estimate that can also be used on data that includes outliers. A tentative slope is determined based on randomly extracted data, and if the tentative slope and the extracted two-point slope are below the threshold, it is taken as an inlier, and if it exceeds the threshold, the data is rejected as an outlier Do. Repeat this for all patterns. And a temporary inclination is changed and the said process is repeated.
The solid line in FIG. 7 is a linear regression using RANSAC. Since the outlier data is rejected by RANSAC, the slope excluding the outlier can be determined.
By multiplying the obtained scale factor by the value of SfM, it is possible to obtain the camera external orientation element and the three-dimensional point group without integrating IMU. From the graph of FIG. 7, the scale factor (s) can be identified as 0.9274.

  In the above embodiment, the camera and the IMU are installed in a form in which the relative position is fixed, and the moving image shooting with the camera and the measurement of the acceleration by the IMU are performed. There is also known one equipped with a laser transmission / reception system (rider). In the present invention, not only the camera but also a rider having a function of scanning the radiation direction of the laser beam and the IMU are installed in a form in which the relative position is fixed, and time series data by the rider accompanying movement and acceleration by the IMU Measured and photographed time-series shooting data is written as a still image for each data, and the image is analyzed by the SLAM method to obtain the relative position of the rider, and second-order differentiation of the relative position of the rider is performed. Three-dimensional point cloud data in which the acceleration of the camera is determined, the acceleration of the rider and the acceleration information obtained from the IMU are synchronized to obtain the scale coefficient, and the self relative position of the rider is multiplied by the scale coefficient It is also possible to take a method of giving actual dimensions to

  The measurement data which implemented this invention in Miyaga-dong No. 3 Sakai in the Kawanishi Ryuji land improvement zone in Iida-shi, Nagano is shown below. The inner diameter of this tunnel was 1 m in width and 1.6 m in height, and it was a channel with a structure that people can pass through. As shown in FIG. 8, the measuring device fixedly mounted a camera with a light on the front of the wagon, and fixedly mounted an IMU and a notebook computer for signal processing on a loading platform. While pushing this wagon, people passed through the trail and acquired experimental data.

  From the captured image data of the camera, the feature points are poured out by SLAM, and the map of the inner wall of the shinji and the locus of the self position of the camera are sequentially obtained. Also, on the other hand, the data of IMU is obtained in synchronization with this data, and the data is accumulated. The SLAM data obtained at this time is shown in FIG. The black points in the figure are the feature points poured out by SLAM, and the rectangular frame indicates the image forming plane of the camera, and the intersection of oblique lines indicates the camera center. Therefore, the line connecting the centers of the cameras is the trajectory of the camera's own position estimated by SLAM.

  The result of combining the SLAM data and the IMU data and adding actual dimensions to the SLAM data is shown in FIG. Data are shown with the traveling direction as the Z axis, the vertical direction as the Y axis, and the horizontal direction as the X axis. From the map information of the Shinto inner wall obtained by SLAM, it was confirmed that the Shinto inner wall was designed in a straight line and formed as a straight water channel. The plan view in the drawing shows the locus of the horizontal position at the center of the camera, and the side view shows the locus of the vertical position at the center of the camera. Since the change in the horizontal position is a measurement while pushing the wagon, it indicates the change in the position of the wagon. The change in vertical position is due to the wagon moving on the sediment deposited on the bottom of the ridge. This positional variation at the center of the camera has no effect on the measurement of the inner wall of the canal which is the measurement object. It was confirmed that the distance from the entrance to the exit of the tunnel was 95.300 m, the height position of the exit was 0.819 m lower and the deviation of the horizontal position was 0.378 m based on the position of the entrance.

  The result of writing the actual dimensions to the SLAM data is shown on a map as shown in FIG. As can be seen from this result, it was found that the Miyag-dong 3 Jong-do was a straight Jang-do, the slope was low and almost horizontal.

Since Miyaga-dong No.3 Shindo was a simple straight tunnel, in order to carry out the measurement according to the present invention for a more complicated route, the measurement was carried out on the underground road in the city (Toei Asakusa Line East Japan Bridge Station underground passage). Since this is an experiment on a public road including stairs, without using a wagon as a measuring device, as shown in FIG. 12, a camera-equipped SLAM and an IMU are fixedly mounted on a notebook computer to form an integral structure. The effect confirmation experiment of the present invention was conducted in the form of holding and walking in the underground road (see FIG. 13). The plot points in the figure indicate sampling feature points extracted by the SLAM device. Specific numerical data obtained in this experiment are shown in Table 1.
In this table, data in the left four columns (A to D) are raw data obtained from the SLAM apparatus used, and the right four columns (E to H) are data subjected to arithmetic processing according to the present invention.

  Unix_time in column A is the time information indicated by the SLAM device, and the one in which this is converted to standard time is shown in column E. Column B is a value showing the x-axis position information of the lens center of this device in the SLAM coordinate system, and column C and D are values showing the y-axis position information of the lens center and z-axis position information in the SLAM coordinate system It is. Based on this value, second-order differentiation is performed to calculate the camera acceleration, synchronized with the acceleration information obtained from the IMU, the scale factor is obtained, and the position information of the SLAM coordinate system is determined by GSI. Converts to Tokyo coordinate system information. The data in column F is the x axis information of the earth coordinate system, and the data in column G is the y axis information in the earth coordinate system. Although the H-column data is height position information, the value of the measurement start position is calculated as zero. The unit of numbers is meters. The underground passage in the loop was moved and measured, and the original starting position was returned to complete the experiment. The data in Table 1 is continuous data from the start of the experiment to the middle of the experiment. The midway data is omitted, and the data in the lowermost row is data when the departure point returns. Although there is a measurement error such as drift when the values are different from the initial data, this experiment was carried out by a person holding the measuring device in hand, so the camera center position is exactly at the origin It is because it does not come.

FIG. 14 shows the horizontal locus of the camera center position obtained by this experiment, obtained from the x-axis information and y-axis information of the earth coordinate system, superimposed on the map of this area where the building information is written and displayed. is there. From this figure, it can be confirmed that a loop-like underground passage is installed in a triangular area in the vicinity of East Japan Bridge Station.
Although this data is position information of the camera center obtained by a person moving in the passage, the floor position of the underground passage, the wall positions on both sides, and the ceiling position information should be calculated based on the position information of the camera center. Is also possible. In the first place, since the SLAM apparatus is capable of obtaining a quaternion (quaternion number: x, y, z, w) indicating the direction of the camera in addition to the SLAM coordinate system position information of the camera center described above, It can be said that it is self-evident in principle that the position information of the feature points sampled by the SLAM can be converted to the earth coordinate system by giving an actual dimension to the position information of the system. Therefore, it is possible to show passage information in the form of floor surface position, wall positions on both sides, ceiling position information, and as central position information of the passage.

The present invention can be realized by three-dimensional point cloud data obtained by camera photographing without satellite radio waves and an accelerometer although there is no restriction on the location, so it is effective to apply to underground or in-facility areas where radio waves do not reach is there. Underground underground pipes such as sewers, tunnels, tunnels such as mines (these are generically referred to as underground closed circuits in this specification), as well as position measurement and verification at rubble disaster site Furthermore, application in a wide field is also expected for inspection in the human body.
In the position measurement of the buried pipeline, a video camera and an IMU are mounted on the traveling body, and by sequentially acquiring a moving image and acceleration data while driving the inside of the pipeline at an equal speed, the camera position is determined by the method of the present invention. You can get the trajectory. The locus of this camera position means the pipe position. When the buried pipeline is hollow, it is suitable to use a traveling vehicle as the traveling body, and when there is a flow like a water channel, it can be installed in a float or small boat and used while changing the moving speed. In any case, highly accurate position measurement can be performed even in an area without a reference point and an area not reached by satellite radio waves.

Claims (4)

  The camera and the IMU are installed in a fixed relative position, and the moving image shooting with the camera and the measurement of the acceleration by the IMU are performed, the captured moving image is written as a still image for each frame, and the captured image is captured The self relative position of the camera is determined by analysis using the SLAM method, the second relative position of the camera is differentiated, the acceleration of the camera is determined, and the acceleration of the camera and the acceleration information obtained from the IMU are synchronized. A method of obtaining a scale factor and giving actual dimensions to three-dimensional point cloud data, wherein the scale factor is multiplied by the self relative position of the camera.   The method according to claim 1, wherein the movement is measured while repeating increase and decrease of acceleration.   The form in which the relative position between the camera and the IMU is fixed is installation on a traveling body, and movement is performed by the traveling body within the target area, and from the position information of the camera obtained by the method according to claim 1 How to measure the position of an object. A rider with a function to scan the radiation direction and an IMU are installed in a fixed relative position configuration, and time series data by the rider with movement and acceleration by the IMU are measured, and the time series photographed data is Each data is written out as a still image, and the image is analyzed by the SLAM method to determine the rider's own relative position, and the rider's own relative position is second-order differentiated to determine the camera acceleration, the rider's acceleration and A method of obtaining a scale factor in synchronization with acceleration information obtained from the IMU, and applying actual dimensions to three-dimensional point group data in which the scale factor is multiplied by the relative position of the rider.