CN108052103B - Simultaneous localization and map construction method of inspection robot in underground space based on deep inertial odometry - Google Patents

Abstract

A method for simultaneous localization and map construction of an inspection robot in underground space based on depth inertial odometry, using a depth camera and an inertial measurement unit for loose coupling, obtaining point cloud information through a depth map collected by the depth camera, and extracting plane features; The collected RGB image is converted into a grayscale image and fused with plane features, and optimized by the iterative closest point algorithm; the data after the iterative closest point optimization is loosely coupled with the inertial measurement unit data, and the loopback detection is used to improve the accuracy of the pose graph and obtain the inspection result. The robot running track, point cloud map and tree jump table map can achieve the effect of simultaneous positioning and map construction of the inspection robot indoors. This method improves the simultaneous positioning accuracy and robustness of the inspection robot in the underground space, and achieves the effect of simultaneous positioning and map construction of the inspection robot in the underground space. When the inspection robot operates in the underground space, the method adopted in the present invention has good robustness in a strong rotation environment.

Description

Simultaneous localization and mapping of underground space for inspection robots based on deep inertial odometry build method

technical field

The invention relates to the field of simultaneous positioning of inspection robots, in particular to a method for simultaneous positioning and map construction of inspection robots in underground space based on a depth inertial odometer.

Background technique

With the advancement of science and technology, inspection robots are more and more widely used in industry, military and other fields. In many cases, the information of the inspection robot's work space is complex and unknown. In order to realize the functions of autonomous navigation, target recognition, and automatic obstacle avoidance in indoor inspection robots, its precise and simultaneous positioning is particularly important. Most of the traditional simultaneous positioning methods are based on the simultaneous positioning of global satellites such as GPS and Beidou, but the simultaneous positioning accuracy of ordinary GPS sensors is low, which cannot meet the precise and simultaneous positioning of inspection robots.

Although differential GPS has high positioning accuracy outdoors, it is expensive and cannot work in tunnels, roadways, and basements where GPS fails. Underground spaces such as tunnels, roadways, and basements cannot receive sunlight all year round, and the illumination is low. In terms of visual positioning, simple cameras are now generally used for positioning with low accuracy and cannot achieve the effect of effective positioning of inspection robots.

With the development of computer vision and image processing technology, machine vision methods navigate through the perception of the environment, and are widely used in real-time robot positioning. The principle of the simultaneous visual positioning method is that the camera installed on the robot body collects the images during the movement in real time, and extracts relevant information from the images, and then judges and calculates the robot's running posture and trajectory, and finally realizes navigation and real-time positioning. However, visual sensors are easily affected by light, and simultaneous localization is easily lost in situations such as strong exposure and low brightness. In addition, the pure monocular vision sensor has no scale information and cannot perceive the depth of the surrounding environment of the robot, and the features are lost when the robot turns in situ, which may easily lead to the failure of the real-time positioning of the robot.

The use of inertial measurement unit for positioning of inspection robots has been developed earlier. Inertial positioning is to use the linear acceleration and rotational angular rate measured by the inertial measurement unit to calculate the simultaneous positioning information of the six degrees of freedom of the carrier. The angular rate of the carrier is measured by the gyroscope, which is mainly used to calculate the rotation matrix of the robot, and provides the transformation relationship between the carrier coordinate system and the navigation coordinate system; the linear acceleration of the carrier is measured by the accelerometer, and the speed of the robot is calculated by integrating the obtained acceleration. information and displacement information, and finally complete the positioning by converting the six-degree-of-freedom information of the robot into the navigation coordinate system. However, the simple inertial measurement unit has a large error accumulation under repeated paths, and cannot perform effective loop closure detection. In addition, due to the random walk of the inertial measurement unit and other properties, a large amount of hysteresis errors will be generated when the inspection robot starts and the acceleration changes greatly.

Consumer machine depth cameras represented by ASUS xtion and Microsoft Kinect can acquire RGB images and depth maps, and are widely used in the field of indoor robotics. However, the field of view of this kind of depth camera is generally narrow, which makes the algorithm tracking target easy to lose. At the same time, there is often a lot of noise in the depth data, which even makes some data unusable. In traditional methods, the extraction algorithm of visual features is often based on the difference of pixels, but in the depth data measured by the depth camera, the points located at the corners are not easy to be identified. And in the case of large rotation, the positioning of the mobile robot using a separate depth camera is easy to lose.

Simultaneous location and mapping (SLAM) was originally applied in the field of robotics. Although the method of using a single sensor is less computationally intensive, the positioning accuracy is not high and the robustness is not strong. Simultaneous localization and map construction methods using multiple sensor fusions have become mainstream in development, and efficient simultaneous localization and map construction with fusion of depth cameras and inertial measurement units is lacking.

SUMMARY OF THE INVENTION

According to the deficiencies of the prior art, the present invention provides a simultaneous positioning and map construction method for an inspection robot in the underground space based on a depth inertial odometry, through which the positioning accuracy and robustness of the inspection robot in the underground space are improved, and the inspection robot is Check the effect of simultaneous positioning and map construction of the robot in the underground space. When the inspection robot operates in the underground space, the method adopted by the present invention has good robustness in a strong rotation environment.

To achieve the above object, the technical scheme adopted in the present invention is:

A method for simultaneous localization and map construction of inspection robot underground space based on deep inertial odometry, the method is as follows:

The depth camera and the inertial measurement unit are used for loose coupling, the point cloud information is obtained through the depth map collected by the depth camera, and the plane features are extracted; the RGB image collected by the depth camera is converted into a grayscale image and plane feature fusion, and the iterative closest point algorithm is used to perform Optimization: Loosely couple the data after iterative closest point optimization and the inertial measurement unit data, use loop detection to improve the accuracy of the pose graph, and obtain the running trajectory, point cloud map and tree jump table map of the inspection robot, so that the inspection robot can be indoors at the same time. Effects of positioning and map building.

Preferably, the depth camera collects two adjacent frames as the scene S and the model M, and the two sets of matching points are respectively recorded as P={pi | _i =1,...,n} and Q={q _i |i =1,...,n};

where _pi and _qi both represent three-dimensional space points, which can be parameterized as

Preferably, the depth camera model is:

Where (u, v) is the pixel position corresponding to the spatial point (x, y, z) ^T , d is the depth value, and C is the camera internal parameter.

Preferably, the motion of M to S is described by rotation R and translation t, solved using an iterative closest point algorithm:

Preferably, plane features are extracted from the point cloud obtained from the depth map, and four parameters are used to describe the plane of the three-dimensional space:

p=(a,b,c,d)={x,y,z|ax+by+cz+d=0};

Let d=0, project each fitted plane on the imaging plane to obtain the imaging position (u, v) of the plane point, and use the projection equation to solve:

where f _x , f _y , c _x , and c _y are the internal parameters of the depth camera, and s is the scaling factor of the depth data.

It is preferable to perform gray histogram normalization on each plane image to enhance its contrast, and then extract feature points and calculate the depth of feature points:

Preferably, too many keyframes will bring extra computation to the backend and loop closure detection, while too few keyframes will result in too much motion between keyframes and insufficient number of feature matches, resulting in easy loss. In the plane where the image is extracted, the relative motion between it and the previous keyframe is calculated to exceed a certain threshold, and it is considered a new keyframe.

Preferably, the calculation of the threshold is performed by evaluating translations and Euler rotations:

Here (Δx, Δy, Δz) is the relative translation, and (α, β, γ) is the relative Euler angle;

w ₁ =(m,m,m),m∈(0.6,0.7); w _2∈ (0.95,1.05).

Beneficial effects of the present invention:

Through this method, the positioning accuracy and robustness of the inspection robot in the underground space are improved, and the effect of simultaneous positioning and map construction of the inspection robot in the underground space environment is achieved. When the inspection robot operates in the underground space, the method adopted by the present invention has good robustness in a strong rotation environment.

Description of drawings

1 is a schematic diagram of a method framework of the present invention;

2 is a schematic diagram of a grayscale image converted from an RGB image acquired by a depth camera of the present invention;

3 is a schematic diagram of a depth image collected by a depth camera of the present invention;

4 is a three-dimensional point cloud map of the construction environment of the present invention;

Fig. 5 is the construction environment three-dimensional tree skip table map of the present invention;

Fig. 6 is the running track of the robot of the present invention.

Detailed ways

The present invention will be further described below through specific embodiments in conjunction with the accompanying drawings.

As shown in Figures 1 to 6, a simultaneous localization and map construction method for an inspection robot in the underground space based on a depth inertial odometry. The depth map to obtain point cloud information and extract plane features. Integrate the RGB images collected by the depth camera with the plane features, and use the iterative closest point (ICP) algorithm to optimize; the ICP-optimized data and the inertial measurement unit (IMU) data are loosely coupled, and loop closure is used to improve the pose. The accuracy of the map is obtained, and the running trajectory, point cloud map and tree jump table map of the inspection robot are obtained.

The depth camera collects two adjacent frames as scene S and model M, and the two sets of matching points are respectively recorded as P={pi | _i =1,...,n} and Q={q _i |i=1,. ..,n}. where _pi and _qi both represent three-dimensional space points, which can be parameterized as

The depth camera model is:

Where (u, v) is the pixel position corresponding to the spatial point (x, y, z) ^T , d is the depth value, and C is the camera internal parameter.

The motion of M to S is described by rotation R and translation t, which is solved by the ICP algorithm:

The plane features are extracted from the point cloud obtained from the depth map, and four parameters are used to describe the plane of the three-dimensional space:

p=(a,b,c,d)={x,y,z|ax+by+cz+d=0}

Let d=0, project each fitted plane on the imaging plane to obtain the imaging position (u, v) of the plane point, and use the projection equation to solve:

where f _x , f _y , c _x , and c _y are the internal parameters of the depth camera, and s is the scaling factor of the depth data.

Do a grayscale histogram normalization for each plane image to enhance its contrast, then extract feature points and calculate the depth of the feature points:

The camera pose in three-dimensional space, expressed by translation and unit quaternion: x={ _x , _y , _z ,qx,qy,qz, _qw };

The plane group P={P _i } extracted from this frame, each plane contains its plane parameters and the feature points to which it belongs.

Too many keyframes will bring extra computation to the backend and loop closure detection, while too few keyframes will result in too much motion between keyframes and insufficient number of feature matches, resulting in easy loss. In the plane where the image is extracted, the relative motion between it and the previous keyframe is calculated to exceed a certain threshold, and this is considered a new keyframe. The threshold is calculated by evaluating translations and Euler rotations:

Here (Δx,Δy,Δz) are relative translations and (α,β,γ) are relative Euler angles.

w ₁ =(m,m,m), m∈(0.6,0.7), w _2∈ (0.95,1.05).

The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in other forms. Any person skilled in the art may use the technical content disclosed above to change or change the form to be equivalent implementation of equivalent changes. example. However, any simple modifications, equivalent changes and modifications made to the above embodiments according to the technical essence of the present invention without departing from the solution content of the present invention still belong to the protection scope of the present invention.

Claims (6)

1. a simultaneous positioning and map construction method based on the inspection robot underground space of depth inertial odometer, is characterized in that, this method is: The depth camera and the inertial measurement unit are used for loose coupling, and the point cloud information is obtained through the depth map collected by the depth camera, and the plane features are extracted; Convert the RGB image collected by the depth camera into a grayscale image and fuse plane features, and use the iterative closest point algorithm for optimization; The data after iterative closest point optimization and the inertial measurement unit data are loosely coupled, and the loopback detection is used to improve the accuracy of the pose graph, and the running trajectory, point cloud map and tree jump table map of the inspection robot are obtained, so as to achieve simultaneous positioning of the inspection robot in the underground space and the effect of map construction; The depth camera collects two adjacent frames as scene S and model M, and the two sets of matching points are respectively recorded as P={pi | _i =1,...,n} and Q={q _i |i=1,. ..,n}; where _pi and _qi both represent three-dimensional space points, which can be parameterized as

The motion from M to S is described by a rotation R and a translation t, solved using the iterative closest point algorithm:

2. the simultaneous positioning and map construction method of the inspection robot underground space based on depth inertial odometer according to claim 1, is characterized in that: The depth camera model is:

Where (u, v) is the pixel position corresponding to the spatial point (x, y, z) ^T , d is the depth value, and C is the camera internal parameter. 3. the simultaneous positioning and map construction method of the inspection robot underground space based on the depth inertia odometer according to claim 1, is characterized in that: The plane features are extracted from the point cloud obtained from the depth map, and four parameters are used to describe the plane of the three-dimensional space: p=(a,b,c,d)={x,y,z|ax+by+cz+d=0}; Let d=0, project each fitted plane on the imaging plane to obtain the imaging position (u, v) of the plane point, and use the projection equation to solve:

d=z·s where f _x , f _y , c _x , and c _y are the internal parameters of the depth camera, and s is the scaling factor of the depth data. 4. the simultaneous positioning and map construction method of the inspection robot underground space based on depth inertial odometer according to claim 3, is characterized in that: Do a grayscale histogram normalization for each plane image to enhance its contrast, then extract feature points and calculate the depth of the feature points:

5. the simultaneous positioning and map construction method of the inspection robot underground space based on depth inertial odometer according to claim 1, is characterized in that: Too many keyframes will bring extra computation to the backend and loop closure detection, while too few keyframes will result in too much movement between keyframes and insufficient feature matching, resulting in easy loss; , calculate the relative motion between it and the previous keyframe exceeds a certain threshold, consider this a new keyframe. 6. the simultaneous positioning and map construction method of the inspection robot underground space based on depth inertial odometer according to claim 5, is characterized in that: The threshold is calculated by evaluating translations and Euler rotations:

Here (Δx, Δy, Δz) is the relative translation, and (α, β, γ) is the relative Euler angle; w ₁ =(m,m,m),m∈(0.6,0.7); w _2∈ (0.95,1.05).

Images