A laser and visual SLAM system for indoor unmanned vehicles

Abstract

The invention discloses a laser and vision SLAM system for an indoor unmanned vehicle, comprising a sensor module, a front-end odometer, a back-end module, and a loop detection module. The sensor module is responsible for accessing various external sensors and inputting sensor data to the front-end odometer, the front-end odometer is responsible for completing the posture estimation and motion estimation of the unmanned vehicle, the back-end module is responsible for optimizing the posture estimated by the front-end odometer, and the loop detection module is responsible for detecting and correcting the loop to eliminate the accumulated error in the motion estimation. The invention realizes the fast and accurate positioning and reconstruction process of the indoor unmanned vehicle, and the efficient front-end odometer operation process, the back-end optimization module and the loop detection method ensure the execution efficiency of the algorithm and reduce the computing cost of the hardware processor.

Description

Technical Field

[0001] The invention relates to a laser and vision SLAM system for an indoor unmanned vehicle, belonging to the technical field of robot real-time positioning and mapping. Background Art

[0002] In the field of indoor autonomous positioning and mapping, due to the various noises generated during movement and the uncertainty of the indoor environment, the positioning and mapping effects of unmanned vehicles often deviate greatly from the ideal settings. Therefore, a robust real-time positioning and mapping system requires a good odometer module and an efficient back-end processing module to cope with the many interferences encountered by unmanned vehicles during indoor operation. However, it is often difficult to achieve a balance between the accumulated error of the odometer and the operating efficiency of the back-end algorithm. Therefore, in order to ensure the positioning accuracy and mapping effect of the unmanned vehicle, it is necessary to configure an efficient and feasible front-end odometer operation process, back-end optimization method and loop detection method in its SLAM system.

[0003] Under different indoor environments and different sensor data input conditions, there are large differences in the way of estimating the position and motion of unmanned vehicles in the SLAM process. At present, many SLAM systems still use a single front-end odometer method to estimate and calculate the state of the unmanned vehicle, so the positioning accuracy and mapping effect are often poor, and they cannot cope with complex and changeable indoor environments. Due to the inevitable motion noise and environmental interference, the cumulative error generated by the front-end odometer will become more and more obvious during the operation of the unmanned vehicle. In order to reduce the error, complex back-end optimization methods often optimize the landmarks and position at the same time, which seriously increases the calculation burden of the unmanned vehicle and leads to low execution efficiency of the algorithm. In addition, some SLAM systems still lack effective loop detection methods, resulting in the inability to correct the cumulative errors in the positioning and mapping process. Therefore, in order to improve the operating efficiency of the indoor unmanned vehicle SLAM system, and at the same time to reduce the SLAM system's dependence on sensors and the occupation of hardware processor computing resources, it is necessary to optimize the front-end odometer operation process, back-end optimization methods and loop detection methods in the SLAM system. Summary of the invention

[0004] In order to solve the problems existing in the prior art, the present invention proposes a laser and vision SLAM system for an indoor unmanned vehicle to achieve precise positioning and navigation functions of the indoor unmanned vehicle.

[0005] To achieve the above-mentioned purpose, the technical solution adopted by the present invention is: a laser and visual SLAM system for an indoor unmanned vehicle, including a sensor module, a front-end odometer, a back-end module and a loop detection module. The sensor module is responsible for connecting to various external sensors and inputting sensor data to the front-end odometer. The front-end odometer is responsible for completing the posture estimation and motion estimation of the unmanned vehicle. The back-end module is responsible for optimizing the posture estimated by the front-end odometer. The loop detection module is responsible for detecting and correcting loops to eliminate the accumulated errors in motion estimation.

[0006] Furthermore, the sensor module includes a laser radar, a camera and an inertial measurement unit that are connected to each other. The laser radar includes at least a single-line laser radar, a multi-line laser radar and a solid-state laser radar. The camera includes at least a monocular camera, a binocular camera and an RGB-D camera. The laser radar and the camera respectively input laser point cloud data and image data to the front-end odometer module. The inertial measurement unit includes at least a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer, which are used to assist the laser sensor in posture estimation.

[0007] Furthermore, the front-end odometer includes a visual odometer module and a laser odometer module. According to the differences in light, structure and composition of the indoor environment in which the indoor unmanned vehicle is located, the SLAM system uses an applicable odometer module to perform pose estimation.

[0008] Furthermore, the laser odometer module uses the iterative closest point ICP method to perform 3D-3D pose estimation, and uses the singular value decomposition SVD method to complete the ICP solution process. For the 3D point coordinates that have been registered:

[0009] P = {p 1 ,…,p n},P′={p 1 ′,…,p 2 ′}

[0010] Construct the least squares problem so that the error term matrix of the i-th pair of points is:

[0011] e i =p i -(Rp i ′+t)

[0012] The corresponding rotation matrix R and translation matrix t at the minimum:

[0013]

[0014] When solving, the error function is processed as:

[0015]

[0016] First, calculate the centroid position coordinates p, p′ of the two groups of points, and then calculate the centroid coordinates of each point:

[0017] q i =p i -p,q i ′=p i ′-p′.

[0018] Next, calculate the rotation matrix according to the optimization problem:

[0019]

[0020] Finally calculate the translation vector:

[0021] t * =p-Rp′

[0022] Finally, the pose transformation R,t is obtained.

[0023] Furthermore, the visual odometer module adopts different motion solving methods according to different input sensor data;

[0024] For monocular cameras, the visual odometer module solves the pose based on epipolar geometry constraints, that is, recovers R,t from the estimated essential matrix E through SVD; for binocular cameras, the visual odometer module uses PnP to solve the 3D to 2D point pair motion, and uses bundle adjustment BA to solve the PnP problem. For the spatial point coordinates P i =[X i ,Y i ,Z i ] T and its projected pixel coordinate u i =[u i ,v i ] T ,satisfy:

[0025] s i u i =KTP i

[0026] Among them, K is the camera intrinsic parameter matrix (a 3*3 dimensional matrix), T represents the Lie group value of the camera pose R, t;

[0027] The objective function corresponding to the reprojection error is:

[0028]

[0029] An unconstrained least squares optimization model is constructed using Lie algebra, and the Gauss-Newton method and Levenberg-Marquardt method are used to solve it. During the solution process, perturbations are introduced to differentiate the Lie algebra, and finally the observation matrix is ​​obtained.

[0030] For RGB-D cameras that can obtain depth data, the visual odometer module uses the ICP method to estimate the pose, and after completing the feature point matching, the pose is solved by the SVD method.

[0031] Furthermore, when the indoor brightness L <L t When L t is the illumination threshold, the SLAM system uses a laser odometer module for pose estimation; when the indoor brightness L≥L t The SLAM system uses a visual odometer module to estimate the pose. The laser point cloud data obtained by the lidar is used to supplement the depth information in the visual data. The key frame coordinates extracted by the visual odometer satisfy:

[0032]

[0033] Furthermore, for the feature points in the space of the acquired depth data in the visual odometer, 3D-3D pose estimation is adopted, and the ICP method is used to solve the pose. For the feature points of the unknown depth data, the 3D-2D minimum reprojection error is calculated to perform pose estimation.

[0034] Furthermore, the backend module adopts a pose graph optimization method, with the camera pose as the node and the relative motion between two pose nodes estimated by the odometer module as the edge. When optimizing the pose graph, Lie algebra is used to represent the pose. For the node T i and T j The Lie group value T of the motion between ij :

[0035]

[0036] Among them, T i and T j Represent the Lie group values ​​corresponding to the positions R, t at i and j respectively;

[0037] Construct the error matrix e ij :

[0038]

[0039] Among them, Δξ ij Indicates T i and T j The motion matrix of the inter-motion:

[0040]

[0041] After introducing the perturbation model, the error is differentiated and the overall objective function is:

[0042]

[0043] ε represents the pose set, which is an n*m matrix, where n is the number of pose samples and m is the position coordinate dimension.

[0044] Furthermore, the loop detection module detects loops based on the bag-of-words model, creates a dictionary based on the ORB features of the image, calculates the similarity of image features using the TF-IDF method, determines whether a loop occurs based on the similarity results, and establishes a caching mechanism in time, stipulating that at a specific time t s The loops continuously detected within the loop are determined to be true loops, and finally the back-end module corrects the accumulated error according to the result of the loop detection.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] The laser and vision SLAM system for indoor unmanned vehicles proposed in the present invention ensures the independence and efficiency of sensor data input by reasonably configuring sensor modules, improves the compatibility of the odometer with the sensor by dividing the visual odometer and laser odometer processing modules, and improves the accuracy and speed of unmanned vehicle motion estimation by setting efficient solution methods in each odometer module. Through the pose graph optimization algorithm and loop detection method, the accumulated error in the unmanned vehicle motion process is significantly reduced and corrected, effectively reducing the computing cost of the unmanned vehicle hardware. Therefore, the laser and vision SLAM system for indoor unmanned vehicles has higher execution efficiency and operation effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a block diagram of the system composition of the present invention.

[0048] Figure 2 This is a flow chart of the front-end odometer operation of the present invention.

[0049] Figure 3 This is a flow chart of the back-end optimization algorithm execution of the present invention.

[0050] Figure 4 The flowchart of the loop detection algorithm of the present invention is shown in FIG. DETAILED DESCRIPTION

[0051] The embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention.

[0052] This embodiment proposes a laser and visual SLAM system for an indoor unmanned vehicle, including a sensor module, a front-end odometer, a back-end module and a loop detection module. Among them, the sensor module is responsible for connecting various external sensors and inputting sensor data to the front-end odometer, the front-end odometer is responsible for completing the posture estimation and motion estimation of the unmanned vehicle, the back-end module is responsible for optimizing the posture estimated by the front-end odometer, and the loop detection module is responsible for detecting and correcting loops to eliminate the accumulated errors in motion estimation.

[0053] In the specific implementation, in order to be compatible with various types of sensors and ensure the stability and independence of sensor data input, this embodiment independently sets up sensor modules, such as Figure 1 As shown, the sensor module includes a laser radar, a camera and an inertial measurement unit. The laser radar includes but is not limited to a single-line laser radar, a multi-line laser radar and a solid-state laser radar. The camera includes but is not limited to a monocular camera, a binocular camera and an RGB-D camera, which respectively input laser point cloud data and image data to the front-end odometer module. The inertial measurement unit includes but is not limited to a 3-axis gyroscope, a 3-axis accelerometer and a 3-axis magnetometer, which are used to assist the laser sensor in posture estimation.

[0054] At the same time, in order to ensure that the unmanned vehicle can achieve accurate motion estimation and autonomous positioning under different indoor environments and multiple sensor input conditions, such as Figure 1 As shown, the front-end odometer includes a visual odometer module and a laser odometer module. According to the differences in light, structure and composition of the indoor environment where the indoor unmanned vehicle is located, the SLAM system uses an applicable odometer module for pose estimation.

[0055] In order to ensure the accuracy of laser odometer in feature matching and pose estimation, Figure 2 As shown in the figure, the laser odometer module uses the iterative closest point ICP (Iterative Closest Point) method to estimate the 3D-3D pose and uses the singular value decomposition (SVD) method to complete the ICP solution process. For the 3D points that have been registered:

[0056] P = {p 1 ,…,p n},P′={p 1 ′,…,p 2 ′}

[0057] Construct the least squares problem so that the error term of the i-th pair of points is:

[0058] e i =p i -(Rp i ′+t)

[0059] The corresponding R,t at the minimum time:

[0060]

[0061] When solving, the error function is processed as:

[0062]

[0063] First, calculate the centroid positions p, p′ of the two sets of points, and then calculate the centroid coordinates of each point:

[0064] q i =p i -p,q i ′=p i ′-p′.

[0065] Next, calculate the rotation matrix according to the optimization problem:

[0066]

[0067] Finally calculate the translation vector:

[0068] t * =p-Rp′

[0069] Finally, the pose transformation R,t is obtained.

[0070] In the specific implementation, in order to make the visual odometer compatible with multiple cameras and accurately complete the pose estimation, such as Figure 2 As shown in the figure, the visual odometer module adopts different motion solving methods according to the different input sensor data. For the monocular camera, the visual odometer module solves the pose based on the epipolar geometry constraint, that is, recovers R,t from the estimated essential matrix E through SVD; for the binocular camera, the visual odometer module adopts PnP to solve the 3D to 2D point pair motion, and uses the bundle adjustment BA (Bundle Adjustment) to solve the PnP problem. Specifically, for the spatial point coordinate P i =[X i ,Y i ,Z i ] T and its projected pixel coordinate u i =[u i ,v i ] T ,satisfy:

[0071] s i u i =KTP i

[0072] The objective function corresponding to the reprojection error is:

[0073]

[0074] An unconstrained least squares optimization model is constructed using Lie algebra, and the Gauss-Newton method and Levenberg-Marquardt method are used to solve it. During the solution process, perturbations are introduced to differentiate the Lie algebra, and finally the observation matrix is ​​obtained.

[0075] For RGB-D cameras that can obtain depth data, the visual odometer module uses the ICP method to estimate the pose, and after completing the feature point matching, the pose is solved by the SVD method.

[0076] In the specific implementation, in order for the SLAM system to achieve accurate motion estimation under different indoor brightness conditions, such as Figure 2 As shown, when the indoor brightness L <L t When L t is the illumination threshold, the SLAM system uses a laser odometer module for pose estimation; when the indoor brightness L≥L t The SLAM system uses a visual odometer module to estimate the pose. The laser point cloud data obtained by the lidar is used to supplement the depth information in the visual data. The key frames extracted by the visual odometer satisfy:

[0077]

[0078] In the specific implementation, in order for the visual odometer to effectively utilize the depth data, such as Figure 2 As shown in the figure, for the feature points in the space of the acquired depth data in the visual odometer, 3D-3D pose estimation is adopted, and the ICP method is used to solve the pose. For the feature points of the unknown depth data, the 3D-2D minimum reprojection error is calculated to perform pose estimation.

[0079] In the specific implementation, in order to reduce the calculation error of the front-end odometer in motion estimation, the present invention makes relevant designs for the back-end optimization algorithm in the SLAM system, such as Figure 3 As shown in the figure, the backend module adopts the pose graph optimization method, with the camera pose as the node and the relative motion between the two pose nodes estimated by the odometer module as the edge. When optimizing the pose graph, Lie algebra is used to represent the pose. For the node T i and T j Movement between T ij:

[0080]

[0081] Build Error:

[0082]

[0083] After introducing the perturbation model, the error is differentiated and the overall objective function is:

[0084]

[0085] Finally, the Gauss-Newton method and Levenberg-Marquardt method are used to solve the pose optimization.

[0086] In the specific implementation, in order to eliminate the cumulative error caused by the front-end odometer under environmental noise and interference, the present invention makes relevant designs for the back-end detection algorithm in the SLAM system, such as Figure 4 As shown in the figure, the loop detection module detects loops based on the bag-of-words model, creates a dictionary based on the ORB features of the image, calculates the similarity of image features using the TF-IDF method, determines whether a loop occurs based on the similarity results, and establishes a caching mechanism in time, stipulating that at a specific time t s The loops that are continuously detected are real loops. Finally, the back-end module corrects the accumulated error according to the results of the loop detection.

[0087] It should be noted that the above-mentioned embodiments illustrate rather than limit the invention and that those skilled in the art will be able to design alternative embodiments without departing from the scope of the appended claims.

Claims

1. A laser and visual SLAM system for an indoor unmanned vehicle, characterized in that: It includes a sensor module, a front-end odometer, a back-end module and a loop detection module. The sensor module is responsible for connecting various external sensors and inputting sensor data to the front-end odometer. The front-end odometer is responsible for completing the posture estimation and motion estimation of the unmanned vehicle. The back-end module is responsible for optimizing the posture estimated by the front-end odometer. The loop detection module is responsible for detecting and correcting loops to eliminate the accumulated errors in motion estimation. The sensor module includes a laser radar, a camera and an inertial measurement unit that are connected to each other. The laser radar includes at least a single-line laser radar, a multi-line laser radar and a solid-state laser radar. The camera includes at least a monocular camera, a binocular camera and an RGB-D camera. The laser radar and the camera input laser point cloud data and image data to the front-end odometer respectively. The inertial measurement unit includes at least a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer, which are used to assist the laser sensor in posture estimation; The front-end odometer includes a visual odometer module and a laser odometer module. According to the different light, structure and composition of the indoor environment where the indoor unmanned vehicle is located, the SLAM system uses an applicable odometer module to estimate the posture; The laser odometer module uses the iterative closest point ICP method to perform 3D-3D pose estimation and uses the singular value decomposition SVD method to complete the ICP solution process. For the 3D point coordinates that have been registered: P={p1,…,p n },P′={p1′,…,p2′} Construct the least squares problem so that the error term matrix of the i-th pair of points is: e i =p i -(Rp i ′+t) The corresponding rotation matrix R and translation matrix t at the minimum: When solving, the error function is processed as: First, calculate the centroid position coordinates p, p of the two sets of points ′ , and then calculate the centroid coordinates of each point: q i =p i -p,q i ′=p i ′-p′ Next, calculate the rotation matrix according to the optimization problem: Finally calculate the translation vector: t * =p-Rp′ Finally, we get the pose transformation R,t; The visual odometer module adopts different motion solving methods according to different input sensor data; For monocular cameras, the visual odometer module solves the pose based on epipolar geometry constraints, that is, recovers R,t from the estimated essential matrix E through SVD; for binocular cameras, the visual odometer module uses PnP to solve the 3D to 2D point pair motion, and uses bundle adjustment BA to solve the PnP problem. For the spatial point coordinates P i =[X i ,Y i ,Z i ] T and its projected pixel coordinate u i =[u i ,v i ] T ,satisfy: s i u i =KTP i Among them, K is the camera intrinsic parameter matrix (a 3*3 dimensional matrix), T represents the Lie group value of the camera pose R, t; The objective function corresponding to the reprojection error is: An unconstrained least squares optimization model is constructed using Lie algebra, and the Gauss-Newton method and Levenberg-Marquardt method are used to solve it. During the solution process, perturbations are introduced to differentiate the Lie algebra, and finally the observation matrix is ​​obtained. For RGB-D cameras that can obtain depth data, the visual odometer module uses the ICP method to estimate the pose, and after completing the feature point matching, the pose is solved by the SVD method; The backend module adopts the pose graph optimization method, with the camera pose as the node and the relative motion between two pose nodes estimated by the odometer module as the edge. When optimizing the pose graph, Lie algebra is used to represent the pose. For the node T i and T j The Lie group value T of the motion between ij : Among them, T i and T j Represent the Lie group values ​​corresponding to the positions R, t at i and j respectively; Construct the error matrix e ij : Among them, Δξ ij Indicates T i and T j The motion matrix of the inter-motion: After introducing the perturbation model, the error is differentiated and the overall objective function is: ε represents the pose set, which is an n*m matrix, where n is the number of pose samples and m is the position coordinate dimension; The loop detection module detects loops based on the bag-of-words model, creates a dictionary based on the ORB features of the image, calculates the similarity of image features using the TF-IDF method, determines whether a loop occurs based on the similarity results, and establishes a cache mechanism in time, stipulating that at a specific time t s The loops continuously detected within the loop are determined to be true loops, and finally the back-end module corrects the accumulated error according to the result of the loop detection.

2. The laser and visual SLAM system for indoor unmanned vehicles according to claim 1, characterized in that: When the indoor brightness L <L t When L t is the illumination threshold, the SLAM system uses a laser odometer module for pose estimation; when the indoor brightness L≥L t The SLAM system uses a visual odometer module to estimate the pose. The laser point cloud data obtained by the lidar is used to supplement the depth information in the visual data. The key frame coordinates extracted by the visual odometer satisfy:

3. The laser and visual SLAM system for indoor unmanned vehicles according to claim 1, characterized in that: For the feature points in the space of the acquired depth data in the visual odometer, 3D-3D pose estimation is adopted, and the pose is solved by using the ICP method. For the feature points of the unknown depth data, the 3D-2D minimum reprojection error is calculated to perform pose estimation.

Images