CN109358342B - Three-dimensional laser SLAM system based on 2D laser radar and control method - Google Patents
Three-dimensional laser SLAM system based on 2D laser radar and control method Download PDFInfo
- Publication number
- CN109358342B CN109358342B CN201811186896.1A CN201811186896A CN109358342B CN 109358342 B CN109358342 B CN 109358342B CN 201811186896 A CN201811186896 A CN 201811186896A CN 109358342 B CN109358342 B CN 109358342B
- Authority
- CN
- China
- Prior art keywords
- map
- laser radar
- point cloud
- steering engine
- dimensional
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000011161 development Methods 0.000 claims abstract description 54
- 230000018109 developmental process Effects 0.000 claims description 53
- 238000005457 optimization Methods 0.000 claims description 21
- 230000006870 function Effects 0.000 claims description 17
- 230000001360 synchronised effect Effects 0.000 claims description 15
- 238000004891 communication Methods 0.000 claims description 11
- 241000282979 Alces alces Species 0.000 claims description 9
- 101100389234 Arabidopsis thaliana ELC gene Proteins 0.000 claims description 9
- 238000009434 installation Methods 0.000 claims description 8
- 238000007781 pre-processing Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 238000004804 winding Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 abstract description 7
- 238000010276 construction Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000630 rising effect Effects 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 1
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010862 gear shaping Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Abstract
The invention provides a three-dimensional laser SLAM system and a method based on a 2D laser radar, which specifically comprise the following steps: the device comprises a device main body, a router and a computer; the device main body includes: the system comprises an embedded development board, a direct-current power supply, a 2D laser radar, a steering engine and a conductive slip ring. The system theory of the invention is combined with the reality, the structure is reasonable, simple and convenient, the invention can quickly establish and realize the three-dimensional map and the positioning, the cost is low, the operation is simple, and the use is convenient. The whole system can be used in a handheld mode or carried on a mobile robot, a three-dimensional reconstruction map can be established in real time in indoor and outdoor environments, the pose of a sensor is calculated through an SLAM algorithm, and the real-time positioning function is achieved. Compared with a three-dimensional laser radar, the three-dimensional laser radar has a large visual field, is free of a dead angle in 360 degrees, is more suitable for drawing construction in an indoor environment, and is lower in cost. In addition, the whole system is convenient to operate and is not influenced by ambient light.
Description
Technical Field
The invention belongs to the technical field of three-dimensional simultaneous positioning and mapping, and particularly relates to a three-dimensional laser SLAM system based on a 2D laser radar and a control method thereof.
Background
With the continuous progress of computer technology, mobile robot technology has been increasingly applied to the fields of lunar exploration, civil robots, and robotics and operations. Robots, also known as robotics, focus on the relationship between control of a handling robot and a target to be handled. Robotics encompasses many subjects, mainly involving kinematics and dynamics, system architecture, perception techniques and control techniques. Wherein simultaneous localization and mapping (SLAM) of the robot is a key technology of mobile robots. The SLAM system of the high-precision mobile robot is the premise of automatic control and path planning and is also a difficult problem of academic discussion and engineering application.
Currently mainstream SLAM systems are largely classified into laser SLAM and visual SLAM according to the difference of sensors used. The vision SLAM adopts a system of vision sensors or RGB-D type sensors, and is divided into a monocular SLAM, a binocular SLAM and an RGBD SLAM according to the different selected vision sensors. Although the visual SLAM is rich in data information, it is dependent on visual features, which makes it less effective in many cases where visual features are lacking. Furthermore, monocular and binocular SLAMs cannot be used in dark environments; the RGBD SLAM is easily affected by solar illumination, and is only suitable for indoor environment and cannot be used outdoors. Therefore, autonomous navigation of an actual robot mainly uses laser SLAM. The traditional three-dimensional laser SLAM system usually adopts a large-scale 3D laser scanner system, and although a compact three-dimensional map with higher precision can be obtained, the three-dimensional map is expensive and large in size, long-time scanning needs to be carried out at a fixed point, offline processing is carried out, manpower and time are consumed, and large-scale application of the three-dimensional laser SLAM system in reality is greatly hindered. The three-dimensional laser radar velodyne16 is limited by a scanning pitch angle, can generate a large scanning blind area, and is not beneficial to the motion decision of the autonomous robot. Most of self-made three-dimensional scanning systems based on the single-line laser and the transmission device can only support off-line data processing at present, three-dimensional environment construction cannot be carried out in real time at present, and reconstruction efficiency is low.
Disclosure of Invention
In order to overcome the defects of the SLAM system, the invention provides a real-time portable three-dimensional laser SLAM system based on a 2D laser radar and a control method thereof. The whole system is convenient to operate, low in cost, free from the influence of ambient light and large in visual angle.
Three-dimensional laser SLAM system based on 2D laser radar specifically includes: the device comprises a device main body, a router and a computer;
wherein, the device main part includes: the system comprises an embedded development board, a direct-current power supply, a 2D laser radar, a steering engine and a conductive slip ring;
the conductive slip ring includes: the conductive slip ring rotor and the conductive slip ring stator;
the embedded development board is connected with a router, and the router is connected with a computer; the direct current power supply is respectively connected with the conductive slip ring stator, the embedded development board and the steering engine; the conductive slip ring stator is connected with the conductive slip ring rotor, the conductive slip ring rotor is connected with the 2D laser radar, and the 2D laser radar is connected with the embedded development board; the steering engine is connected with the embedded development board;
the router realizes real-time communication between the embedded development board and the computer;
the computer is used for receiving the data specially transmitted by the embedded development board, displaying the three-dimensional point cloud map and transmitting a control instruction to the embedded development board;
the embedded development board: all programs run on the embedded development board, the programmed programs are used for realizing the establishment and the positioning of a three-dimensional graph, sending a motion control command to the steering engine to enable the steering engine to rotate at a constant speed of 180 degrees/s, and simultaneously reading data of an encoder inside the steering engine and data of a 2D laser radar to realize data synchronization of two sensors;
the power supply voltage of the direct current power supply is 12V, and the direct current power supply provides a power supply for the embedded development board, the 2D laser radar and the steering engine;
when the direct-current power supply supplies power to the embedded development board, a 12V power supply is converted into a 5V power supply to supply power;
the 2D laser radar is connected with the embedded development board through a USB line, and transmits a sensor signal acquired by the 2D laser radar to the embedded development board;
the steering engine provides rotation for the 2D laser radar, generates three-dimensional point cloud by matching with the 2D laser radar, is internally provided with an encoder, can provide rotation angle information for a system, and transmits the angle information to the embedded development board;
the conductive slip ring avoids winding caused by rotation of the 2D laser radar;
the steering engine comprises a power supply device special for the steering engine and a communication module, the power supply device special for the steering engine is connected with a direct-current power supply, and the direct-current power supply supplies power for the power supply device special for the steering engine; the communication module is connected with the embedded development board through a USB wire;
the direct current power supply is connected with the conductive slip ring stator through a lead; the conductive slip ring stator is connected with the conductive slip ring rotor by using a spring piece; the conductive slip ring rotor is connected with the 2D laser radar through a lead;
and the 2D laser radar data is read by adopting SCIP2.0 protocol.
The embedded development board and the steering engine form a bottom layer control unit.
The control method of the three-dimensional laser SLAM system based on the 2D laser radar is realized by using the three-dimensional laser SLAM system based on the 2D laser radar, and specifically comprises a bottom layer motion control process and an upper layer map reconstruction process:
(1) The bottom layer motion control process comprises the following specific flows:
step 1: initialization: making the steering engine angle value zero;
and 2, step: if a starting instruction transmitted from the computer is obtained, starting a bottom layer motion control process, and controlling the steering engine to rotate at a constant speed, otherwise, continuing the initialization process of the step 1;
and step 3: reading synchronous data of an encoder inside the steering engine and the 2D laser radar;
and 4, step 4: generating three-dimensional point cloud by using synchronous data of an encoder inside the steering engine and the 2D laser radar;
and 5: sending the result of point cloud generation to a computer;
and 6: if the computer transmits an ending instruction, ending the bottom layer motion control process, otherwise returning to the step 3, and continuously reading the synchronous data of the encoder inside the steering engine and the 2D laser radar;
(2) The upper map reconstruction process comprises the following specific processes:
step 1: data preprocessing: taking a point cloud calculation result transmitted to a computer in the bottom layer motion control process as a data preprocessing object, and filtering the three-dimensional point cloud by adopting a method in a PCL (personal computer) library to remove outliers to obtain an undistorted three-dimensional point cloud;
step 2: based on the undistorted three-dimensional point cloud, map positioning is carried out by using a calibration algorithm, the influence of installation errors on the three-dimensional point cloud is eliminated, and rough positioning information is obtained;
and step 3: obtaining rough map information by using a map reconstruction algorithm;
and 4, step 4: and eliminating accumulated drift by using a map optimization algorithm to obtain a final map and accurate positioning information.
The calibration algorithm comprises the following specific processes:
step 2.1: initializing a calibration angle: setting an initial value { alpha 0 ,β 0 Angle θ 0 Setting to a fixed value;
step 2.2: acquiring new three-dimensional point cloud coordinates under a Cartesian coordinate system:
c (-) and S (-) respectively represent cos and sin functions, alpha, beta and theta are offset angles of x, y and z axes of the laser radar due to installation errors, rho is the distance from a laser point to a laser head and is data obtained from the laser radar;
step 2.3: randomly sampling the three-dimensional point cloud by using a consistency algorithm, and carrying out segmentation to extract plane points to obtain P planes with the maximum number of inner points and a plane equation;
step 2.4: calculating a cost function E:
where N is the total number of data points at a valid depth, a valid point refers to a data point that has been discarded with an error sum exceeding a specified distance, N j Is the number of inner points of the jth plane, d j,i Representing the distance from the ith inner point to the jth plane corresponding to the ith inner point; p is the number of planes;
step 2.5: if the cost function E is smaller than the threshold value, outputting a calibration angle, ending the calibration algorithm, otherwise, taking the current calibration angle as an initialization angle, re-acquiring a new three-dimensional point cloud coordinate under the Cartesian coordinate system, and repeating the step 2.2-the step 2.6;
the map reconstruction algorithm comprises the following specific processes:
step 3.1: establishing an octree model, establishing a map storage container Cloud, initializing, and setting a grid resolution parameter m, wherein m =0.05;
step 3.2: acquiring point cloud data at the moment k, and recording the point cloud data as n points;
step 3.3: if the point Cloud is at the time of 0, namely the first time point Cloud, directly inputting the point Cloud into the octree model, and then storing the point Cloud in the octree into the Cloud;
step 3.4: if the point Cloud is not the point Cloud at the time of 0, inputting the point Cloud at the time into an octree model, calling an isVoxelOccupriedAtPoint function of the octree, judging whether n point clouds exist, if so, not keeping the ith point Cloud, i =1,2 \8230n; if not, storing the ith point into the Cloud;
step 3.5: k = k +1, point Cloud data at the next moment is obtained, the steps 3.2-3.4 are repeated, and when the point clouds at all the moments are obtained, the point clouds stored in the Cloud are the global map.
The map optimization algorithm comprises the following specific processes:
step 4.1: dividing the European space into a plurality of sub-areas according to the rough map information obtained in the step 3, and then establishing a local map for each sub-area; the Euclidean space is the maximum three-dimensional volume of the established map;
step 4.2: an ELCH method is used as a rough graph optimization method: optimizing the local map of each sub-area by using an ELCH method;
step 4.3: when a potential closed loop is formed after optimization of the ELCH method, matching a current local map with a historical local map of the potential closed loop through an ICP method, and if matching is successful, considering that the current local map and the historical local map are in the same scene, and calculating a pose transformation matrix between the current local map and the historical local map;
step 4.4: and establishing a g2o graph model by using a g2o graph optimization tool and using the pose as a point and the pose transformation matrix as an edge, and optimizing the graph model by using a DCS robust kernel function and an LM optimization method to obtain a final map and accurate positioning information after global drift is eliminated.
The beneficial technical effects are as follows:
the invention combines the system theory with the reality, has reasonable, simple and convenient structure, can quickly establish and realize the three-dimensional map and the positioning, and has low cost, simple operation and convenient use. The steering engine with the encoder is adopted in hardware, so that the system has high rotation precision, the encoder does not need to be added, and the overall volume of the equipment is reduced; by adopting the PCD disc type conductive slip ring, the axial space of the steering engine is saved, and the system is reasonable, simple and convenient to construct. In the algorithm, the hardware synchronization means of the sensor data is adopted, so that the timestamps of the sensor data are strictly aligned, and the accuracy is higher than that of the traditional software synchronization; the influence of installation errors on the accuracy of the obtained point cloud can be effectively eliminated through a calibrated algorithm; the accumulated error in the mapping process is eliminated by using g2o and other tools. The whole system can be used in a handheld mode or carried on a mobile robot, a three-dimensional reconstruction map can be established in real time in indoor and outdoor environments, the pose of a sensor is calculated through an SLAM algorithm, and the real-time positioning function is achieved. Compared with a three-dimensional laser radar, the three-dimensional laser radar has a large visual field, is free of a dead angle in 360 degrees, is more suitable for drawing construction in an indoor environment, and is lower in cost. In addition, the whole system is convenient to operate and is not influenced by ambient light.
Drawings
Fig. 1 is a general structural block diagram of a real-time portable three-dimensional laser SLAM system based on a 2D laser radar according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of the hardware connections of the underlying motion control unit according to an embodiment of the present invention;
FIG. 3 is a structural diagram of a PCB disc type conductive slip ring according to an embodiment of the invention;
FIG. 4 is a software framework diagram of an embodiment of the present invention;
FIG. 5 is a flowchart of a bottom layer motion control process according to an embodiment of the present invention;
FIG. 6 is a flowchart of an upper map reconstruction process according to an embodiment of the present invention;
FIG. 7 is a flow chart of a calibration algorithm according to an embodiment of the present invention;
FIG. 8 is a flowchart of a map reconstruction algorithm according to an embodiment of the present invention;
fig. 9 is a flowchart of a map optimization algorithm according to an embodiment of the present invention.
Detailed Description
The invention is further described with reference to the accompanying drawings and specific embodiments, and a three-dimensional laser SLAM system based on a 2D laser radar specifically includes: a device body, as shown in the solid line block of fig. 1, a router, a computer; wherein, the device main part includes: an embedded development board, a direct current power supply, a 2D laser radar, a steering engine and a conductive slip ring, as shown in figure 1;
the conductive slip ring comprises: the conductive slip ring rotor and the conductive slip ring stator;
the embedded development board is connected with a router, and the router is connected with a computer; the direct current power supply is respectively connected with the conductive slip ring stator, the embedded development board and the steering engine; the conductive slip ring stator is connected with the conductive slip ring rotor, the conductive slip ring rotor is connected with the 2D laser radar, and the 2D laser radar is connected with the embedded development board; the steering engine is connected with the embedded development board;
the router realizes real-time communication between the embedded development board and the computer;
the computer is used for receiving the data specially delivered by the embedded development board, displaying the three-dimensional point cloud map and delivering a control instruction to the embedded development board;
all programs of the embedded development board are operated on the embedded development board, the programmed programs are used for realizing the establishment and the positioning of a three-dimensional graph, sending a motion control command to the steering engine to enable the steering engine to rotate at an angular speed of 180 degrees/s at a constant speed, and simultaneously reading data of an encoder inside the steering engine and data of the 2D laser radar to realize data synchronization of the two sensors;
the power supply voltage of the direct current power supply is a 12V lithium battery, and the direct current power supply provides a power supply for the embedded development board, the 2D laser radar and the steering engine;
when the direct-current power supply supplies power to the embedded development board, a 12V to 5V module is used for converting a 12V power supply into a 5V power supply to supply power;
the 2D laser radar is connected with the embedded development board through a USB line, and the model UTM-30LX transmits a sensor signal acquired by the 2D laser radar to the embedded development board;
the steering engine is of a model Dynamixel MX-28, provides rotation for the 2D laser radar, is matched with the 2D laser radar to generate three-dimensional point cloud, and is internally provided with an encoder, so that rotation angle information can be provided for the system, and the angle information is transmitted to the embedded development board;
the conductive slip ring avoids winding caused by rotation of the 2D laser radar, and is a PCB disc type conductive slip ring;
the steering engine comprises a power supply device special for the steering engine and a communication module, the power supply device special for the steering engine is SMPS2Dynamixel, the power supply device special for the steering engine is connected with a direct-current power supply, and the direct-current power supply supplies power to the power supply device special for the steering engine; the communication module is connected with the embedded development board through a USB (universal serial bus) line, the communication module is connected with the embedded development board through the USB line, and the model of the RS 485-to-USB module of the steering engine is USB2Dynamixel;
the direct current power supply is connected with the conductive slip ring stator through a lead; the conductive slip ring stator is connected with the conductive slip ring rotor by using a spring piece; the conductive slip ring rotor is connected with the 2D laser radar through a lead;
the embedded development board and the steering engine form a bottom layer control unit, as shown by a dotted line square frame in fig. 1.
The 2D laser radar is fixed on the radar support frame through screws, and the radar support frame is connected with a rudder plate on an output shaft of the Dynamixel MX28 through screws; the rudder disc is connected with an output shaft of the Dynamixel MX28 through gear shaping; dynamixel MX28 is connected with a fixed round plate assembled on the Dynamixel MX28 through screws; the PCB disc type conductive slip ring is assembled between the rudder disc and the steering engine, and has the advantages of lightness, thinness, axial space saving and small device body volume. The structure diagram of the PCB disc type conductive slip ring is shown in figure 3 and is divided into a stator and a rotor, wherein the stator is connected with a fixed circular plate assembled in Dynamixel MX28 through screws, and the rotor is connected with a steering wheel through screws; a data line and a power supply line of the laser radar are connected with the rotor end of the PCB disc type conductive slip ring; the port of the stator output end corresponding to the data line is connected with the ODROID XU4 development board through a USB interface, and the port of the stator output end corresponding to the power supply line is connected with a 12V power supply through a lead, as shown in FIG. 2.
The software framework of this embodiment is shown in fig. 5, and the whole software system is based on the ROS operating system in the Linux environment and is divided into two parts, namely a bottom-layer motion control module and an upper-layer map reconstruction and positioning module. Fig. 6 is a bottom layer motion control flow chart, which includes control of the motion of the steering engine, reading of the data of the coded disc of the steering engine, reading of the data of the laser radar, and synchronization of the two data. The SCIP2.0 protocol is adopted for reading the 2D laser radar data, and the Dynamixel MX28 communication protocol is adopted for reading the steering engine data. In the two data synchronization processes, a GPIO 21 pin of the ODROID XU4 is set to be in an external rising edge trigger interrupt mode, and the pin is connected with a synchronization signal of the laser radar; the laser radar synchronous signal sends a rising edge signal every 25ms and triggers interruption, when the interruption occurs, ODROID XU4 reads data of an encoder inside the steering engine and data of the laser radar to obtain two types of sensor data with synchronous time, and preparation is made for generating 3D point cloud. Fig. 7 is a flowchart of upper map reconstruction. Firstly, preprocessing is carried out, a method in a PCL library is adopted to carry out filtering on the three-dimensional point cloud to remove outliers, and then the installation error is calibrated. And then circularly executing the map positioning, map reconstruction and map optimization programs which respectively correspond to the laser positioning algorithm, the laser map reconstruction algorithm and the map optimization algorithm.
The control method of the three-dimensional laser SLAM system based on the 2D laser radar is realized by using the three-dimensional laser SLAM system based on the 2D laser radar, and specifically comprises a bottom layer motion control process and an upper layer map reconstruction process, as shown in fig. 4: controlling the motion of the steering engine: the embedded development board odroid XU4 sends a motion instruction to the steering engine through a communication protocol of Dynamixel MX28, so that the steering engine rotates at a uniform speed of 180 degrees/s. Reading data of a code wheel of the steering engine: the embedded development board odroid XU4 sends an instruction of reading coded disc data to the steering engine through a communication protocol of Dynamixel MX28, and obtains the steering engine coded disc data. Laser radar data reading: and the embedded development board odroid XU4 sends a data reading instruction to the laser radar through the SCIP2.0 protocol of the laser radar to acquire the data of the laser radar. Data preprocessing: 1. converting coded disc data of a steering engine into angle value data taking degrees as a unit; 2. and filtering the laser radar data, and keeping the data within the range of 0.5m to 30m away from the laser radar.
(1) As shown in fig. 5, the bottom layer motion control process includes the following specific steps:
step 1: initialization: making the angle value of the steering engine zero;
step 2: if a starting instruction transmitted from the computer is obtained, starting a bottom layer motion control process, and controlling the steering engine to rotate at a constant speed, otherwise, continuing the initialization process of the step 1;
and step 3: the laser radar is provided with a synchronous signal line, a synchronous signal is sent out every 25ms in the laser radar, if the laser synchronous signal is received, an encoder in the steering engine is synchronous with the 2D laser radar, and synchronous data of the encoder in the steering engine and the 2D laser radar are read;
and 4, step 4: generating three-dimensional point cloud by using synchronous data of an encoder inside the steering engine and the 2D laser radar;
and 5: sending the result of point cloud generation to a computer;
and 6: if the computer transmits an ending instruction, ending the bottom layer motion control process, otherwise returning to the step 3, and continuously reading the synchronous data of the encoder inside the steering engine and the 2D laser radar;
(2) As shown in fig. 6, the upper map reconstruction process includes the following specific steps:
step 1: data preprocessing: taking a point cloud calculation result transmitted to a computer in the bottom layer motion control process as a data preprocessing object, filtering the three-dimensional point cloud by adopting a method in a PCL (personal computer) library to remove outliers, and obtaining the undistorted three-dimensional point cloud, wherein depth data and angle data in the graph are read 2D laser radar data and steering engine data respectively;
step 2: based on the undistorted three-dimensional point cloud, map positioning is carried out by using a calibration algorithm, the influence of installation errors on the three-dimensional point cloud is eliminated, and rough positioning information is obtained;
and 3, step 3: obtaining rough map information by using a map reconstruction algorithm;
and 4, step 4: and eliminating accumulated drift by using a map optimization algorithm to obtain a final map and accurate positioning information.
As shown in fig. 7, the calibration algorithm has the following specific flow:
step 2.1: initializing a calibration angle: setting an initial value { alpha 0 ,β 0 Angle θ 0 Set to a fixed value, { α in the example 0 ,β 0 }θ 0 All initialized to 0 degrees;
step 2.2: acquiring new three-dimensional point cloud coordinates under a Cartesian coordinate system:
wherein, C (-) and S (-) represent cos and sin functions respectively, alpha, beta and theta are offset angles of x, y and z axes of the laser radar caused by installation errors respectively, rho is the distance from a laser point to a laser head and is data obtained from the laser radar;
step 2.3: randomly sampling the three-dimensional point cloud by using a consistency algorithm, and carrying out segmentation to extract plane points to obtain P planes with the maximum number of interior points and a plane equation;
step 2.4: calculating a cost function E:
where N is the total number of data points at a valid depth, a valid point refers to a data point that has been discarded with an error sum exceeding a specified distance, and N is j Is the number of inner points of the jth plane, d j,i Representing the distance from the ith inner point to the jth plane corresponding to the ith inner point; p is the number of planes
Step 2.5: if the cost function E is smaller than the threshold value, taking the cost function E as 0.1, outputting a calibration angle, ending the calibration algorithm, otherwise, taking the current calibration angle as an initialization angle, re-acquiring a new three-dimensional point cloud coordinate under the Cartesian coordinate system, and repeating the step 2.2-the step 2.6;
as shown in fig. 8, the map reconstruction algorithm specifically includes the following steps:
step 3.1: establishing an octree model, establishing a map storage container Cloud, initializing and setting a grid resolution parameter m of the map storage container Cloud;
step 3.2: acquiring point cloud data at the moment k, and recording the point cloud data as n points;
step 3.3: if the point Cloud at the time of 0 is the first point Cloud, directly inputting the point Cloud into the octree model, and storing the point Cloud in the octree into the Cloud;
step 3.4: if the point Cloud is not the point Cloud at the time of 0, inputting the point Cloud at the time into an octree model, calling an isVoxelOccupriedAtPoint function of the octree, judging whether n point clouds exist, if so, not keeping the ith point Cloud, i =1,2 \8230n; if not, storing the ith point into the Cloud;
step 3.5: k = k +1, point Cloud data at the next moment is obtained, the steps 3.2-3.4 are repeated, and when the point clouds at all the moments are obtained, the point clouds stored in the Cloud are the global map.
As shown in fig. 9, the map optimization algorithm specifically includes the following steps:
step 4.1: dividing the European space into a plurality of sub-areas according to the rough map information obtained in the step 3, and then establishing a local map for each sub-area; the Euclidean space is the maximum three-dimensional volume of the established map;
step 4.2: an ELCH method is used as a rough graph optimization method: optimizing the local map of each sub-area by using an ELCH method;
step 4.3: when a potential closed loop is formed after optimization of the ELCH method, matching a current local map with a historical local map of the potential closed loop through an ICP method, and if matching is successful, considering that the current local map and the historical local map are in the same scene, and calculating a pose transformation matrix between the current local map and the historical local map;
step 4.4: and establishing a g2o graph model by using a g2o graph optimization tool and using the pose as a point and the pose transformation matrix as an edge, and optimizing the graph model by using a DCS robust kernel function and an LM optimization method to obtain a final map and accurate positioning information after global drift is eliminated.
Claims (3)
1. A control method of a three-dimensional laser SLAM system based on a 2D laser radar is realized by using the three-dimensional laser SLAM system based on the 2D laser radar, and the system comprises the following steps: the device comprises a device main body, a router and a computer;
wherein, the device main part includes: the system comprises an embedded development board, a direct-current power supply, a 2D laser radar, a steering engine and a conductive slip ring;
the conductive slip ring includes: the conductive slip ring rotor and the conductive slip ring stator;
the embedded development board is connected with a router, and the router is connected with a computer; the direct current power supply is respectively connected with the conductive slip ring stator, the embedded development board and the steering engine; the conductive slip ring stator is connected with the conductive slip ring rotor, the conductive slip ring rotor is connected with the 2D laser radar, and the 2D laser radar is connected with the embedded development board; the steering engine is connected with the embedded development board;
the router realizes real-time communication between the embedded development board and the computer;
the computer is used for receiving the data specially transmitted by the embedded development board, displaying the three-dimensional point cloud map and transmitting a control instruction to the embedded development board;
all programs of the embedded development board run on the embedded development board, and the programmed programs are used for realizing the creation and positioning of a three-dimensional graph, sending a motion control command to the steering engine to enable the steering engine to rotate at a constant speed, and simultaneously reading data of an encoder inside the steering engine and data of a 2D laser radar to realize data synchronization of two sensors;
the direct current power supply provides a power supply for the embedded development board, the 2D laser radar and the steering engine;
the 2D laser radar is connected with the embedded development board through a USB line, and transmits a sensor signal acquired by the 2D laser radar to the embedded development board;
the steering engine provides rotation for the 2D laser radar, the steering engine is matched with the 2D laser radar to generate three-dimensional point cloud, and an encoder is arranged in the steering engine and can provide rotation angle information for a system and transmit the angle information to the embedded development board;
the conductive slip ring avoids winding caused by rotation of the 2D laser radar;
the system control method is characterized by specifically comprising a bottom layer motion control process and an upper layer map reconstruction process:
(1) The bottom layer motion control process comprises the following specific flows:
step 1: initialization: making the steering engine angle value zero;
step 2: if a starting instruction transmitted from the computer is obtained, starting a bottom layer motion control process, and controlling the steering engine to rotate at a constant speed, otherwise, continuing the initialization process in the step 1;
and step 3: reading synchronous data of an encoder inside the steering engine and the 2D laser radar;
and 4, step 4: generating three-dimensional point cloud by using synchronous data of an encoder inside the steering engine and the 2D laser radar;
and 5: sending the result of point cloud generation to a computer;
and 6: if the computer transmits an ending instruction, ending the bottom layer motion control process, otherwise returning to the step 3, and continuously reading the synchronous data of the encoder inside the steering engine and the 2D laser radar;
(2) The upper map reconstruction process comprises the following specific procedures:
step 1: data preprocessing: taking a point cloud calculation result transmitted to a computer in the bottom layer motion control process as a data preprocessing object, and filtering the three-dimensional point cloud by adopting a method in a PCL (personal computer) library to remove outliers to obtain an undistorted three-dimensional point cloud;
step 2: based on the undistorted three-dimensional point cloud, map positioning is carried out by using a calibration algorithm, the influence of installation errors on the three-dimensional point cloud is eliminated, and rough positioning information is obtained;
the calibration algorithm in the upper map reconstruction process comprises the following specific processes:
step 2.1: initializing a calibration angle: setting an initial value { alpha 0 ,β 0 Angle of the angleθ 0 Setting to a fixed value;
step 2.2: acquiring new three-dimensional point cloud coordinates under a Cartesian coordinate system:
c (-) and S (-) respectively represent cos and sin functions, alpha, beta and theta are offset angles of x, y and z axes of the laser radar due to installation errors, rho is the distance from a laser point to a laser head and is data obtained from the laser radar;
step 2.3: randomly sampling the three-dimensional point cloud by using a consistency algorithm, and carrying out segmentation to extract plane points to obtain P planes with the maximum number of inner points and a plane equation;
step 2.4: calculating a cost function E:
where N is the total number of data points at a valid depth, a valid point refers to a data point that has been discarded with an error sum exceeding a specified distance, and N is j Is the number of inner points of the jth plane, d j,i Representing the distance from the ith inner point to the jth plane corresponding to the ith inner point; p is the number of planes;
step 2.5: if the cost function E is smaller than the threshold value, outputting a calibration angle, ending the calibration algorithm, otherwise, taking the current calibration angle as an initialization angle, re-acquiring a new three-dimensional point cloud coordinate under the Cartesian coordinate system, and repeating the step 2.2-the step 2.6;
and 3, step 3: obtaining rough map information by using a map reconstruction algorithm;
and 4, step 4: and eliminating accumulated drift by using a map optimization algorithm to obtain a final map and accurate positioning information.
2. The control method of the 2D lidar-based three-dimensional laser SLAM system as defined in claim 1, wherein the specific flow of the map reconstruction algorithm in the upper map reconstruction process is as follows:
step 1: establishing an octree model, establishing a map storage container Cloud, initializing and setting a grid resolution parameter m of the map storage container Cloud;
step 2: acquiring point cloud data at the moment k, and recording the point cloud data as n points;
and 3, step 3: if the point Cloud at the time of 0 is the first point Cloud, directly inputting the point Cloud into the octree model, and storing the point Cloud in the octree into the Cloud;
and 4, step 4: if the point Cloud is not the point Cloud at the time 0, inputting the point Cloud at the time into an octree model, calling an isVoxelOccupriedAtPoint function of the octree, judging whether n point clouds exist, if yes, not retaining the ith point Cloud, i =1,2 \ 8230n; if not, storing the ith point into Cloud;
and 5: k = k +1, point Cloud data at the next moment is obtained, the steps 3.2-3.4 are repeated, and when the point clouds at all the moments are obtained, the point clouds stored in the Cloud are the global map.
3. The control method of the 2D lidar based three-dimensional laser SLAM system according to claim 1, wherein the map optimization algorithm in the upper map reconstruction process comprises the following specific steps:
step 1: dividing the European space into a plurality of sub-regions according to the rough map information obtained in the step 3, and then establishing a local map for each sub-region; the Euclidean space is the maximum three-dimensional volume of the built map;
and 2, step: an ELCH method is used as a rough graph optimization method: optimizing the local map of each sub-area by using an ELCH method;
and step 3: when a potential closed loop is formed after optimization of the ELCH method, matching a current local map with a historical local map of the potential closed loop through an ICP method, and if matching is successful, considering that the current local map and the historical local map are in the same scene, and calculating a pose transformation matrix between the current local map and the historical local map;
and 4, step 4: and establishing a g2o graph model by using a g2o graph optimization tool and using the pose as a point and the pose transformation matrix as an edge, and optimizing the graph model by using a DCS robust kernel function and an LM optimization method to obtain a final map and accurate positioning information after global drift is eliminated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811186896.1A CN109358342B (en) | 2018-10-12 | 2018-10-12 | Three-dimensional laser SLAM system based on 2D laser radar and control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811186896.1A CN109358342B (en) | 2018-10-12 | 2018-10-12 | Three-dimensional laser SLAM system based on 2D laser radar and control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109358342A CN109358342A (en) | 2019-02-19 |
| CN109358342B true CN109358342B (en) | 2022-12-09 |
Family
ID=65348821
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811186896.1A Active CN109358342B (en) | 2018-10-12 | 2018-10-12 | Three-dimensional laser SLAM system based on 2D laser radar and control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109358342B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110851556B (en) * | 2019-11-20 | 2023-02-17 | 苏州博众智能机器人有限公司 | Mobile robot mapping method, device, equipment and storage medium |
| CN111251271B (en) * | 2020-03-17 | 2023-02-21 | 青岛聚远网络科技有限公司 | SLAM robot for constructing and positioning rotary laser radar and indoor map |
| CN111830529B (en) * | 2020-07-09 | 2023-04-14 | 武汉理工大学 | Laser SLAM method and device based on lamplight calibration information fusion |
| CN112630795B (en) * | 2020-12-24 | 2023-05-26 | 浙江大学滨海产业技术研究院 | Three-dimensional point cloud data synthesis system based on 2D laser radar |
| CN112748445A (en) * | 2020-12-28 | 2021-05-04 | 芜湖哈特机器人产业技术研究院有限公司 | Intelligent radar system |
| CN112799096B (en) * | 2021-04-08 | 2021-07-13 | 西南交通大学 | Map construction method based on low-cost vehicle-mounted two-dimensional laser radar |
| CN113570721B (en) * | 2021-09-27 | 2021-12-21 | 贝壳技术有限公司 | Method and device for reconstructing three-dimensional space model and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN107167141A (en) * | 2017-06-15 | 2017-09-15 | 同济大学 | Robot autonomous navigation system based on double line laser radars |
| CN108303710A (en) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar |
| CN108401461A (en) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | Three-dimensional mapping method, device and system, cloud platform, electronic equipment and computer program product |
-
2018
- 2018-10-12 CN CN201811186896.1A patent/CN109358342B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN107167141A (en) * | 2017-06-15 | 2017-09-15 | 同济大学 | Robot autonomous navigation system based on double line laser radars |
| CN108401461A (en) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | Three-dimensional mapping method, device and system, cloud platform, electronic equipment and computer program product |
| CN108303710A (en) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar |
Non-Patent Citations (1)
| Title |
|---|
| "A Real-time and Low-cost 3D SLAM System Based on a Continuously Rotating 2D Laser Scanner";Zheng Fang et al.;《2017 IEEE 7th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems (CYBER)》;20180826;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109358342A (en) | 2019-02-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109358342B (en) | Three-dimensional laser SLAM system based on 2D laser radar and control method | |
| CN107167141B (en) | Robot autonomous navigation system based on double laser radars | |
| CN106199626B (en) | Based on the indoor three-dimensional point cloud map generation system and method for swinging laser radar | |
| CN109238240A (en) | A kind of unmanned plane oblique photograph method that taking landform into account and its camera chain | |
| Schäfer et al. | Multicopter unmanned aerial vehicle for automated inspection of wind turbines | |
| CN108535736A (en) | Three dimensional point cloud acquisition methods and acquisition system | |
| CN109541630A (en) | A method of it is surveyed and drawn suitable for Indoor environment plane 2D SLAM | |
| CN111998772A (en) | Pixel-level target positioning method based on laser and monocular vision fusion | |
| CN101493526A (en) | Lunar vehicle high speed three-dimensional laser imaging radar system and imaging method | |
| CN111337037B (en) | Mobile laser radar slam drawing device and data processing method | |
| CN111077907A (en) | Autonomous positioning method of outdoor unmanned aerial vehicle | |
| CN201293837Y (en) | Moonmobile high speed three-dimensional laser imaging radar system | |
| Deng et al. | Long-range binocular vision target geolocation using handheld electronic devices in outdoor environment | |
| CN111640177B (en) | Three-dimensional modeling method based on underwater sonar detection and unmanned submersible | |
| CN113570715B (en) | Sensor fusion-based rotary laser real-time positioning modeling system and method | |
| CN107941167B (en) | Space scanning system based on unmanned aerial vehicle carrier and structured light scanning technology and working method thereof | |
| CN112034484B (en) | Modeling system and method based on hemispherical laser radar | |
| CN111251271B (en) | SLAM robot for constructing and positioning rotary laser radar and indoor map | |
| CN113777580A (en) | Three-dimensional laser scanner based on rotatable single line laser radar | |
| Nikolov et al. | LiDAR-based 2D localization and mapping system using elliptical distance correction models for UAV wind turbine blade inspection | |
| CN203550917U (en) | Mobile three-dimensional laser scanning device based on four shaft aircraft | |
| CN206388067U (en) | Unmanned plane and system of taking photo by plane | |
| CN112254653B (en) | Program control method for 3D information acquisition | |
| Fang et al. | A real-time and low-cost 3D SLAM system based on a continuously rotating 2D laser scanner | |
| CN207882763U (en) | A kind of multi-rotor unmanned aerial vehicle automatic obstacle avoiding device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20231213 Address after: Room 4X-139, No. 96 Sanhao Street, Heping District, Shenyang City, Liaoning Province, 110057 Patentee after: Shenyang Ruige Holdings Co.,Ltd. Address before: No.11, Wenhua Road, Sanxiang, Heping District, Shenyang City, Liaoning Province Patentee before: Fang Zheng Patentee before: Shenyang Ruige Holdings Co.,Ltd. Effective date of registration: 20231213 Address after: No.11, Wenhua Road, Sanxiang, Heping District, Shenyang City, Liaoning Province Patentee after: Fang Zheng Patentee after: Shenyang Ruige Holdings Co.,Ltd. Address before: No. 195, Chuangxin Road, Hunnan District, Shenyang City, Liaoning Province Patentee before: Northeastern University |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240106 Address after: No. 94-2 Sanhao Street, Heping District, Shenyang City, Liaoning Province, 110057 (3008) Patentee after: Ruige Intelligent Technology (Shenyang) Co.,Ltd. Address before: Room 4X-139, No. 96 Sanhao Street, Heping District, Shenyang City, Liaoning Province, 110057 Patentee before: Shenyang Ruige Holdings Co.,Ltd. |