A mobile robot navigation system based on solid-state lidar and point cloud map

Abstract

The present invention discloses a mobile robot navigation system based on a solid-state lidar and a point cloud map, comprising: a point cloud map preprocessing module for generating an occupancy grid map for path planning; a lidar inertial odometer module for removing motion distortion from lidar point clouds and providing a relatively accurate odometer; a map matching and positioning module for matching the accumulated distortion-removed point clouds with the point cloud map to update the pose of the robot in the map system; and a path planning module for comprehensively planning a path to a target point based on map and positioning information. The present invention solves the contradiction that the two-dimensional grid map does not describe the complex environment completely, and the three-dimensional point cloud map is not convenient for path planning of mobile robots. By integrating the tightly coupled odometer of the solid-state lidar and the IMU and the method of matching and updating the pose with the point cloud map, the positioning and navigation of the robot have high precision and robustness in complex scenarios.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot navigation, and in particular to a mobile robot navigation system based on solid-state laser radar and point cloud map, which uses solid-state radar to locate in a three-dimensional point cloud map and processes the point cloud map into a grid map for path planning. Background Art

[0002] At present, the requirements for the intelligence of mobile robots in various application scenarios are constantly increasing. Many tasks require them to be able to identify surrounding obstacles, perform autonomous navigation based on environmental information, or accept remote commands to complete fixed-point navigation. Communication positioning methods such as WiFi and UWB are gradually unable to meet the requirements of complex operations. Among them, obtaining detailed information about the surrounding environment and its accurate position in it is the basis for solving the problem of autonomous navigation of mobile robots. Using point clouds to depict three-dimensional environments has high accuracy and is widely used in the fields of autonomous driving, surveying and mapping, and robotics.

[0003] For robot navigation in simple indoor environments, there are relatively mature open source solutions for the key steps of map building, relocalization in maps, and path planning. For example, a single-line laser radar can be used to draw a two-dimensional grid map of the environment through gmapping, cartographer, etc., and this grid map can be imported into the ROS-Navigation framework for relocalization and path planning in the map, thereby completing the complete robot navigation. Although such solutions can run in indoor structured scenes, on the one hand, in large outdoor scenes and complex or empty indoor scenes, due to the detection distance and installation height limitations of the single-line laser radar, all obstacles in the environment cannot be drawn into the grid map, and thus cannot work properly; on the other hand, the three-dimensional point cloud map is not convenient for path planning of mobile robots. Therefore, how to use the three-dimensional point cloud map for positioning while converting the point cloud map for path planning has become a key issue in proposing a robust navigation system. The present invention proposes a mobile robot navigation system based on solid-state laser radar and point cloud map, which solves the problem that many systems using single-line laser radar for mapping and repositioning have incomplete description of three-dimensional environment and few applicable scenarios. It can achieve higher positioning and navigation accuracy in both complex indoor scenes and open outdoor scenes. Summary of the invention

[0004] The purpose of the present invention is to overcome the shortcomings and deficiencies of the prior art, and proposes a mobile robot navigation system based on solid-state laser radar and point cloud map with high precision, strong robustness and wide application scenarios. It solves the contradiction that the two-dimensional grid map is not complete in describing the complex environment, and the three-dimensional point cloud map is not convenient for path planning of the mobile robot. It integrates the odometer tightly coupled with the solid-state laser radar and the IMU and the method of matching and updating the posture with the point cloud map, so that the robot's positioning and navigation are also highly accurate and robust in complex scenes.

[0005] To achieve the above purpose, the technical solution provided by the present invention is: a mobile robot navigation system based on solid-state laser radar and point cloud map, comprising:

[0006] Point cloud map preprocessing module, used to generate occupancy grid map for path planning and record the pose transformation between the two map coordinate systems in order to align the point cloud map coordinate system and the grid map coordinate system;

[0007] The LiDAR inertial odometer module is used to process solid-state LiDAR data and IMU data, eliminate motion distortion for the original radar point cloud through high-frequency IMU data, output the dedistorted point cloud, match the dedistorted point cloud with the accumulated point cloud, calculate the residual, and update the posture state to achieve an accurate odometer;

[0008] The map matching and positioning module is used to load the offline point cloud map, segment the local point cloud using the current robot's position in the map coordinate system and the radar's field of view, splice the dedistorted point cloud output by the lidar inertial odometer module into a sliding window structure, match the local point cloud with the point cloud in the sliding window, and finally update the robot's position in the map coordinate system;

[0009] The path planning module uses the grid map to load obstacle information, uses the robot posture output by the map matching positioning module, starts path planning after receiving the target location, and outputs motion control instructions to the robot's underlying drive to realize the navigation function.

[0010] Furthermore, the point cloud map preprocessing module is used to generate an occupancy grid map for path planning and record the pose transformation between the two map coordinate systems, and align the point cloud map coordinate system and the grid map coordinate system, including the following steps:

[0011] 1-1) Input the original point cloud, divide it into multiple regions of fixed size in the horizontal direction x, y, set one of them as the initial region, and give a rough ground equation;

[0012] 1-2) Determine whether the current area has been processed. If not, filter out the ground point cloud A according to the distance from the point to the currently set ground. The condition for each point p is:

[0013]

[0014] In the formula, are the current ground equations f i The normal vector in and the distance from the plane to the origin, ε is the set distance threshold; the second formula means that the difference between the surface normal vector Normal(p) of point p and the opposite direction of the gravity vector g does not exceed the angle The upper right subscript T is the matrix transpose operation;

[0015] 1-3) Determine whether the number of points in point cloud A is lower than the set threshold. If it is lower than the threshold, use point cloud segmentation based on region growing to check the plane equation h of the region with the most points in turn for the first r regions clustered. i Whether the parameters meet the following requirements:

[0016]

[0017] In the formula, The plane equations are h i The normal vector in and the distance from the plane to the origin, θ is the plane equation h i With the current ground equation f i The threshold of the normal vector angle difference, ε d Approximately, it is the distance threshold between the two planes; if it meets the requirements, let the reference ground equation f i+1 =h i , mark the points in the relevant area; if there is no h that meets the requirements in the first r areas i , then let f i+1 =f i , there are no marked points in this area;

[0018] When the number of points in point cloud A is higher than the threshold, the plane equation h is fitted to all points in A. i , and then h i Perform the same ground point cloud screening and record the result as point cloud B. When the number of points in point cloud B is greater than twice the number of points in point cloud A, let f i+1 =h i , mark the point in B as a ground point; otherwise, f i+1 =h i , mark the points in A as ground points;

[0019] 1-4) Remove the part marked as the ground, the part below the ground height, and the part above the ground and higher than the robot height; set the current area as processed, set f i+1 is the current ground equation, and then checks the four neighboring areas of the current area in turn, and performs the operations in steps 1-2) to 1-4) on them;

[0020] 1-5) After all areas are processed, merge the point clouds of all areas and project them to the z-axis. At this time, the ground areas in all point clouds have been deleted, and the remaining point clouds represent obstacles or areas where you cannot walk. Delete some distant boundary point clouds to control the map size. After statistical filtering of the point clouds to remove noise, divide the plane grid again. The occupancy value of this grid when generating the grid map is determined according to the number of points in the grid.

[0021] 1-6) Since some border trimming and rotation operations will be performed on the map in step 1-5), the pose transformation and scale between the raster map and the point cloud map coordinate systems are recorded during the operation, which will be used to unify the two map coordinate systems during subsequent path planning.

[0022] Further, the laser radar inertial odometer module performs the following operations:

[0023] 2-1) The robot remains stationary and collects solid-state lidar data to construct the initial cumulative point cloud for subsequent matching and residual calculation; collects stationary IMU data and uses the statistical mean to roughly estimate the measurement deviation b of the angular velocity meter and accelerometer ω ,b a and gravity vector g; wait for 2 to 3 seconds to complete the initialization of the mileage calculation method;

[0024] 2-2) Process the radar data and IMU data, and then a frame of solid-state lidar point cloud data is abbreviated as scan. The high-frequency IMU data is used to forward propagate the current posture until the next scan is input. All sampling points in this scan are projected to the scan end time using posture interpolation to remove motion distortion. The specific mathematical form of posture forward propagation is as follows:

[0025]

[0026]

[0027]

[0028]

[0029] In the formula, x i 、x i+1 are the states of the IMU at the current time i and the next time i+1, respectively; Δt is the interval between two IMU data; the lower right corner of each physical quantity is the object coordinate system described by the physical quantity, and the upper left corner is the coordinate system used to represent the physical quantity; the IMU coordinate system is recorded as I, and the IMU coordinate system at time i is recorded as I i , the first frame IMU coordinate system is the odometer coordinate system, which is abbreviated as the Odom coordinate system and recorded as O; where the state x iIncluding: the posture of the IMU coordinate system at time i Location speed IMU angular velocity sensor and accelerometer measurement deviation and the gravity vector O g i Representation in Odom coordinate system; is the generalized addition symbol. The physical quantities in the state quantity except the posture are linearly added during the generalized addition. The posture is operated according to the three-dimensional rotation group and its Lie algebra operation rules; u i is the motion input at time i, i.e., the angular velocity measurement from the IMU input Acceleration measurement w i is the system noise at time i, including the angular velocity measurement noise Acceleration measurement noise Angular velocity deviation noise and acceleration deviation noise 0 3×1 represents a zero vector with 3 rows and 1 column; F(x i ,u i ,w i ) is the state change per unit time;

[0030] 2-3) Each point p in the dedistorted scan j The following formula is derived from the laser radar coordinate system L at time k: k Transform to Odom coordinate system:

[0031]

[0032] In the formula, I R L , I t L I is the pose transformation parameter between the pre-calibrated LiDAR coordinate system and the IMU coordinate system; k is the IMU coordinate system at time k; is the state quantity to be optimized O x k The posture and position items in , take the results after forward propagation as the initial values;

[0033] Then for each point in the scan in the Odom coordinate system O p j , find the nearest point using nearest neighbor search in the accumulated point cloud O q j And use the nearest 5 points to fit the plane, and record its normal vector as n j , then the residual res j The form is:

[0034]

[0035] By optimizing the state O x k Minimize the total residual of each point in the scan, the optimal after iterative convergence That is the robot state in the current Odom coordinate system;

[0036] 2-4) Based on the best estimate Posture and position in Add the dedistorted current scan to the accumulated point cloud.

[0037] Further, the map matching and positioning module performs the following operations:

[0038] 3-1) According to the robot's position in the point cloud map coordinate system and the radar's field of view angle α, the local point cloud in and near the field of view is selected. The point cloud map coordinate system is subsequently referred to as the Map coordinate system, and is denoted as M in the formula subscript. The specific operation is as follows: the local point cloud is composed of points that satisfy the following formula: M p composition:

[0039]

[0040] In the formula, L φ is the expression of the laser radar front direction vector in the laser radar coordinate system L, and δ is the allowable angle error; is the posture and position of the robot in the Map coordinate system, which is obtained from step 2-3) Pose transformation between Odom coordinate system and Map coordinate system M R O , M t O The obtained value is used as the initial value when the algorithm is initialized;

[0041] 3-2) To ensure that there are enough points for matching and to prevent the pose error in the Map coordinate system from causing the common view area with the local point cloud segmented in step 3-1) to be too small, the scan after dedistortion in step 2-2) is stored in a queue data structure, and the point cloud in the sliding window in the Odom coordinate system is composed of multiple frames of scan;

[0042] 3-3) Perform ICP matching on the local point cloud in the Map coordinate system in step 3-1) and the point cloud in the window in the Odom coordinate system in step 3-2), and solve and update the pose transformation between the Odom coordinate system and the Map coordinate system. M R O , M t O ;

[0043] 3-4) Use the pose transformation between the Odom coordinate system and the Map coordinate system obtained in step 2-3) M R O , M t O The robot pose estimation in the Odom coordinate system obtained in step 2-3) Get the robot pose in the Map coordinate system:

[0044]

[0045] After that, the robot's position in the Map coordinate system It can be used as input for subsequent path planning modules.

[0046] Further, the path planning module performs the following operations:

[0047] 4-1) Input the grid map in step 1-5), and regard the grids in the map that are higher than the set threshold as obstacles, and the grids that are lower than the threshold as passable areas;

[0048] 4-2) Continue to receive the robot pose in the Map coordinate system in step 3-4), and use the transformation relationship between the point cloud map and the grid map coordinate system recorded in step 1-6) to align the two coordinate systems. The resulting pose is the current position of the robot in the grid map.

[0049] 4-3) Input the coordinates of the navigation destination, combine the robot's current position and map obstacle information to make a path plan, and then convert it into motion control command output to complete the navigation.

[0050] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0051] 1. Solid-state laser radar is cheaper than mechanical rotating multi-line laser radar and can obtain denser three-dimensional point cloud information. However, due to the small viewing angle of most solid-state radars, the matching effect of single-frame solid-state radar point cloud and map is poor. The present invention uses a sliding window structure to accumulate multi-frame dedistorted point clouds and matches them with local maps segmented according to the solid-state radar posture and field of view, which greatly enhances the matching speed and posture accuracy, and gives full play to the performance advantages of solid-state radar.

[0052] 2. Compared with the grid map built by the single-line laser radar, the two-dimensional grid map converted from the three-dimensional point cloud map can obtain richer obstacle information and avoid blind spots and height restrictions. At the same time, the path planning algorithm on the two-dimensional grid map is more efficient and has more mature solutions for reference. The present invention combines the rich environmental information of the three-dimensional point cloud and the characteristics of the two-dimensional grid map that facilitates mobile robot navigation, improving the robustness and applicability of the entire navigation system.

[0053] 3. In the algorithm for converting a three-dimensional point cloud map into a two-dimensional grid map, the method of dividing multiple grids and fitting plane equations in sequence is adopted. This can cope with areas with slopes and scenes where the ground is not completely flat, thereby improving the robustness and applicability of the algorithm.

[0054] 4. A tightly coupled method is used to fuse the point cloud data of the solid-state lidar and the IMU measurement data, making full use of the IMU's ability to capture fast motion in a short period of time. While obtaining high-frequency odometer output, it can better remove the motion distortion of the lidar point cloud due to different sampling point times.

[0055] 5. The tightly coupled odometer that uses IMU pose recursion and lidar point cloud as the observation correction and update state can output the pose of the odometer coordinate system with higher accuracy, thereby providing a good pose initial value for the subsequent matching point cloud map for ICP algorithm, increasing the average number of successful matching points, reducing the probability of failure of the ICP algorithm, and improving the accuracy and robustness of the positioning algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is the overall logic flow chart of the present invention.

[0057] Figure 2 This is a flow chart of a two-dimensional grid map algorithm for path planning using a point cloud map preprocessing module in the present invention.

[0058] Figure 3 This is a flow chart of the positioning algorithm using solid-state radar in a point cloud map in the present invention.

[0059] Figure 4 The top view comparison is between the results obtained in the embodiment without removing the ground point cloud by region and the results obtained by using the method of the present invention.

[0060] Figure 5 It is a raster map obtained by converting the point cloud map in the embodiment.

[0061] Figure 6 This is a diagram showing the effect of aligning the point cloud map with the grid map in the embodiment.

[0062] Figure 7 This is a diagram showing the effect of using the solid-state radar field of view angle to segment the point cloud map in the embodiment.

[0063] Figure 8 This is a diagram showing the actual operation effect of the repositioning and navigation system proposed in the present invention in the embodiment. DETAILED DESCRIPTION

[0064] The present invention is further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0065] As attached Figure 1 As shown, this embodiment provides a mobile robot navigation system based on solid-state laser radar and point cloud map, including the following modules:

[0066] Point cloud map preprocessing module, used to generate occupancy grid map for path planning and record the pose transformation between the two map coordinate systems in order to align the point cloud map coordinate system and the grid map coordinate system;

[0067] The LiDAR inertial odometer module is used to process solid-state LiDAR data and IMU data. The high-frequency IMU data is used to eliminate motion distortion for the original radar point cloud and output the dedistorted point cloud. At the same time, the dedistorted point cloud is matched with the accumulated point cloud, and the residual is calculated to update the posture state, thus achieving a more accurate odometer.

[0068] The map matching and positioning module is used to load the offline point cloud map, segment the local point cloud using the current robot's position in the map coordinate system and the radar's field of view, splice the dedistorted point cloud output by the lidar inertial odometer module into a sliding window structure, match the local point cloud with the point cloud in the sliding window, and finally update the robot's position in the map coordinate system.

[0069] The path planning module uses the grid map to load obstacle information; it uses the robot posture output by the map matching positioning module to start path planning after receiving the target location, and outputs motion control instructions to the robot's underlying drive to realize the navigation function.

[0070] Furthermore, as attached Figure 3 As shown, the point cloud map preprocessing module specifically performs the following operations:

[0071] 1-1) Input the original point cloud, divide it into multiple regions of fixed size in the horizontal direction x, y, set one of them as the initial region, and give a rough ground equation f 0 In this embodiment, the scene of the original point cloud map is an outdoor square of about 180m×240m. After removing some boundaries, it can be divided into 64 regions according to the grid size of 20×25m. The initial region is selected as the region where the origin in the original point cloud is located. Since the z-axis of the point cloud map is the vertical direction and the height of the mapping device from the ground is 0.6m, the initial ground equation is set to In the formula is the normal vector of the initial ground equation, is the distance from the initial ground to the origin;

[0072] 1-2) Determine whether the current area has been processed. If not, filter out the ground point cloud A according to the distance from the point to the currently set ground. The condition for each point p is:

[0073]

[0074] In the formula, are the current ground equations f i The normal vector in and the distance from the plane to the origin, ε is the set distance threshold; the second formula means that the difference between the surface normal vector Normal(p) of point p and the opposite direction of the gravity vector g does not exceed the angle The upper right corner subscript T is a matrix transposition operation. In this embodiment, the distance threshold is set to 10 cm, and the angle threshold is set to 30°.

[0075] 1-3) Determine whether the number of points in point cloud A is lower than the set threshold. If it is lower than the threshold, use point cloud segmentation based on region growing to check the plane equation h of the region with the most points in the first r regions clustered. i Whether the parameters meet the following requirements:

[0076]

[0077] In the formula, The plane equations are h i The normal vector in and the distance from the plane to the origin, θ is the plane equation h i With the current ground equation f i The threshold of the normal vector angle difference, ε d Approximately, it is the distance threshold between the two planes; if it meets the requirements, let the reference ground equation f i+1 =h i , mark the points in the relevant area; if there is no h that meets the requirements in the first r areas i , then let f i+1 =f i , there are no marked points in this area;

[0078] When the number of points in point cloud A is higher than the threshold, the plane equation h is fitted to all points in A. i , and then h i Do the same ground point cloud screening again, and record the result as point cloud B. When the number of points in point cloud B is greater than twice the number of points in point cloud A, let f i+1 =h i , mark the point in B as a ground point; otherwise, f i+1 =h i , mark the points in A as ground points;

[0079] 1-4) Remove the part marked as the ground, the part below the ground height, and the part above the ground and higher than the robot height, and set the current area as processed. Set f i+1 is the current ground equation, and then checks the four neighboring areas of the current area in turn, and performs steps 1-2) to 1-4) on them;

[0080] 1-5) After all areas have been processed, merge the point clouds of all areas and project them to the z-axis. At this point, the ground areas in all point clouds have been deleted, and the remaining point clouds represent obstacles or inaccessible areas. After the point cloud is statistically filtered to remove noise, the plane grid is divided again, and the occupancy value of this grid when generating the grid map is determined based on the number of points in the grid. In this embodiment, the resolution is set to 4cm / grid according to the size of the scene. In the area represented by each grid, if the number of points is less than 50, the occupancy value is set to 0, otherwise it is set to 255. The final result is as follows Figure 5 Raster map shown.

[0081] 1-6) Since some operations such as border trimming and rotation will be performed on the map in step 1-5), the pose transformation and scale between the raster map and the point cloud map coordinate system during the operation are recorded for the unification of the two map coordinate systems during subsequent navigation.

[0082] As attached Figure 4 As shown, in this embodiment, since the scene is large and there are several small-angle slopes, directly removing the ground point cloud according to the same plane equation cannot meet the requirements of distinguishing obstacles from walkable areas (see Appendix Figure 4 The point cloud map processing method proposed by the present invention can achieve the expected effect (see attached Figure 4 Down).

[0083] Attached Figure 6 The effect of aligning the three-dimensional point cloud map with the two-dimensional grid map in the embodiment is demonstrated.

[0084] Furthermore, regarding the hardware selection of the laser radar inertial odometer module, in this embodiment, Livox Mid-70 is selected as the solid-state laser radar, and its field of view angle is 70.4°. The selected mobile robot platform is a four-wheel AGV car, and the industrial computer used is Advantech MIC-7700H. The selected IMU is the built-in IMU on Intel-Realsense d435i. Figure 2 As shown, the specific process is:

[0085] 2-1) The robot remains stationary and collects solid-state lidar data to construct the initial cumulative point cloud for subsequent matching and residual calculation; collects stationary IMU data and uses the statistical mean to roughly estimate the measurement deviation b of the angular velocity meter and accelerometer ω ,ba and gravity vector g. Wait for 2 to 3 seconds to complete the initialization of the mileage calculation method;

[0086] 2-2) Process radar data and inertial sensor (IMU) data, and use high-frequency IMU data (including three-axis acceleration and three-axis angular velocity) to forward propagate the current posture until the next frame of radar point cloud data is input (a frame of lidar point cloud data is abbreviated as scan below), and use posture interpolation to project all sampling points in this scan to the end time of the scan to remove motion distortion. The specific mathematical form of posture forward propagation is as follows:

[0087]

[0088]

[0089]

[0090]

[0091] In the formula, x i 、x i+1 are the states of the IMU at the current time i and the next time i+1, respectively; Δt is the interval between two IMU data; the lower right corner of each physical quantity is the object coordinate system described by the physical quantity, and the upper left corner is the coordinate system used to represent the physical quantity; the IMU coordinate system is recorded as I, and the IMU coordinate system at time i is recorded as I i , the first frame IMU coordinate system is the odometer coordinate system, which is abbreviated as the Odom coordinate system and recorded as O; where the state x i Including: the posture of the IMU coordinate system at time i Location speed IMU angular velocity sensor and accelerometer measurement deviation and the gravity vector O g i Representation in Odom coordinate system; is the generalized addition symbol. The physical quantities in the state quantity except the posture are linearly added during the generalized addition. The posture is operated according to the three-dimensional rotation group and its Lie algebra operation rules; u i is the motion input at time i, i.e., the angular velocity measurement from the IMU input Acceleration measurement w i is the system noise at time i, including the angular velocity measurement noise Acceleration measurement noise Angular velocity deviation noise and acceleration deviation noise 0 3×1represents a zero vector with 3 rows and 1 column; F(x i ,u i ,w i ) is the state change per unit time.

[0092] It is worth noting that the entire mileage calculation method in this embodiment outputs the robot posture at a frequency of 200Hz of the IMU, and performs state updates based on radar point cloud observations at a frequency of 10Hz of the radar frames transmitted by Livox.

[0093] 2-3) Each point p in the dedistorted scan j The following formula is derived from the laser radar coordinate system L at time k: k Transform to the odometry coordinate system O:

[0094]

[0095] in I R L , I T L It is the pose transformation parameter between the pre-calibrated LiDAR coordinate system and the IMU coordinate system; is the state quantity to be optimized O x k The posture and position items in are initialized with the results after forward propagation.

[0096] Then for each point in the scan in the Odom coordinate system O p j , find the nearest point using nearest neighbor search in the accumulated point cloud O q j And use the nearest 5 points to fit the plane, and record its normal vector as n j , then the residual res j The form is:

[0097]

[0098] By optimizing the state O x k Minimize the total residual of each point in the scan, the optimal after iterative convergence That is the robot state in the current Odom coordinate system;

[0099] 2-4) Based on the best estimate Posture and position in Add the dedistorted current scan to the accumulated point cloud.

[0100] Furthermore, the map matching and positioning module performs ICP matching on the point cloud in the corresponding range of the odometer (Odom) coordinate system and the point cloud map (Map) coordinate system to estimate the robot's state in the Odom coordinate system. Convert to the Map coordinate system to provide the robot's position in the map for navigation tasks. Figure 2 As shown, the specific process is:

[0101] 3-1) According to the robot's position in the point cloud map coordinate system and the radar's field of view angle α, the local point cloud in and near the field of view is selected. The point cloud map coordinate system is subsequently referred to as the Map coordinate system, and is denoted as M in the formula subscript. The specific operation is as follows: the local point cloud is composed of points that satisfy the following formula: M p composition:

[0102]

[0103] in, L φ is the expression of the laser radar front direction vector in the laser radar coordinate system L, and δ is the allowable angle error. In this embodiment, the x-axis of Livox Mid70 is its front direction, so L φ=(1,0,0) T , the field of view angle α=70.4°, and δ is set to 5°. is the posture and position of the robot in the Map coordinate system, which is obtained from step 2-3) Pose transformation between Odom coordinate system and Map coordinate system M R O , M t O The obtained value is used as the initial value when the algorithm is initialized.

[0104] Attached Figure 7 The figure shows the effect of segmenting the local point cloud that may be within the field of view according to the robot's position in the map. The brighter three-dimensional point cloud (i.e., local point cloud) in the figure is cone-shaped, corresponding to the field of view of the solid-state laser radar. The white breakpoints on the local point cloud are the currently incoming scans, and the white endpoints fit the surface shape of the point cloud, showing the good operation of the positioning algorithm.

[0105] 3-2) To ensure that there are enough points for matching and to prevent the pose error in the Map coordinate system from causing the common view area with the local point cloud segmented in step 3-1) to be too small, the scan after dedistortion in step 2-2) is stored in a queue data structure, and the point cloud in the sliding window in the Odom coordinate system is composed of multiple frames of scan;

[0106] 3-3) Perform ICP matching on the local point cloud in the Map coordinate system in step 3-1) and the point cloud in the window in the Odom coordinate system in step 3-2), and solve and update the pose transformation between the Odom coordinate system and the Map coordinate system. M R O , M t O , where the ICP matching is in the form of solving the least squares problem of the following formula:

[0107]

[0108] where p i ,p' i are the i-th point in the local point cloud and the point cloud in the window respectively, and n is the total number of points in the point cloud with fewer points in the two point clouds;

[0109] 3-4) Use the pose transformation between the Odom coordinate system and the Map coordinate system obtained in step 3-3) M R O , M t O The robot pose estimation in the Odom coordinate system obtained in step 2-3) Get the robot pose in the Map coordinate system:

[0110]

[0111] After that, the robot's position in the Map coordinate system It can be used as input for subsequent path planning modules.

[0112] Furthermore, the path planning module in this embodiment uses ROS-Navigation as the framework of the path planning algorithm, and the specific implementation steps are:

[0113] 4-1) Input the 2D grid map in step 1-5) to the ROS-map_server node, load the map according to the set resolution and coordinate origin, and treat the grids in the map that are higher than the set threshold as obstacles, and the grids that are lower than the threshold as passable areas.

[0114] 4-2) Continue to receive the robot pose in the Map coordinate system in step 3-4). Since the point cloud map and grid map coordinate systems have been aligned in step 1-6), the obtained pose is the current position of the robot in the grid map;

[0115] 4-3) Input the navigation destination coordinates, the robot's current position and map obstacle information to the ROS-move_base node, use the A* algorithm to do global path planning, and the DWA algorithm to do local path planning, and then convert it into motion control command output to complete navigation.

[0116] Note that in this embodiment, the robot posture in the Map coordinate system in the laser radar inertial odometer module In fact, it is the position and posture of the IMU sensor in the Map coordinate system. However, since the IMU is fixed on the robot, the two coordinate systems only differ by a pre-calibrated position and posture parameter, so no distinction is made here.

[0117] Attached Figure 8 The effect of the robot running the navigation system in the scene in this embodiment is shown. The left part of the figure is a screenshot of the operation of the path planning part of the navigation system, where the white point group with the robot as the vertex is the current scan, and the curve in front of the robot is the planned path. The upper right area of ​​the figure is the picture of the camera in front of the robot. The lower right area of ​​the figure is a photo of the actual position of the robot at this time. It can be seen that the positional relationship between the robot and the surrounding objects in the actual scene corresponds accurately to each item in the grid map.

[0118] It should be understood that various parts of the present application can be implemented by hardware, software or a combination thereof, and in the above-mentioned embodiments, multiple steps or methods can be implemented by other appropriate hardware or software.

[0119] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A mobile robot navigation system based on solid-state laser radar and point cloud map, It is characterized in that include: Point cloud map preprocessing module, used to generate occupancy grid map for path planning and record the pose transformation between the two map coordinate systems in order to align the point cloud map coordinate system and the grid map coordinate system; The LiDAR inertial odometer module is used to process solid-state LiDAR data and IMU data. It uses high-frequency IMU data to eliminate motion distortion for the original radar point cloud, outputs the dedistorted point cloud, matches the dedistorted point cloud with the accumulated point cloud, calculates the residual, and updates the posture state to achieve an accurate odometer. The map matching and positioning module loads the offline point cloud map, uses the current robot's position in the map coordinate system and the radar's field of view to segment the local point cloud, uses the dedistorted point cloud output by the lidar inertial odometer module to stitch into a sliding window structure, and matches the local point cloud with the point cloud in the sliding window, and finally updates the robot's position in the map coordinate system; The path planning module uses the grid map to load obstacle information, uses the robot posture output by the map matching positioning module, starts path planning after receiving the target location, and outputs motion control instructions to the robot's underlying drive to realize the navigation function.

2. A mobile robot navigation system based on solid-state laser radar and point cloud map according to claim 1, Features: The point cloud map preprocessing module is used to generate an occupied grid map for path planning and record the pose transformation between the two map coordinate systems, and align the point cloud map coordinate system and the grid map coordinate system, including the following steps: 1-1) Input the original point cloud, divide it into multiple regions of fixed size in the horizontal direction x, y, set one of them as the initial region, and give a rough ground equation; 1-2) Determine whether the current area has been processed. If not, filter out the ground point cloud A according to the distance from the point to the currently set ground. The condition for each point p is: In the formula, are the current ground equations f i The normal vector in and the distance from the plane to the origin, ε is the set distance threshold; the second formula means that the difference between the surface normal vector Normal(p) of point p and the opposite direction of the gravity vector g does not exceed the angle The upper right subscript T is the matrix transpose operation; 1-3) Determine whether the number of points in point cloud A is lower than the set threshold. If it is lower than the threshold, use point cloud segmentation based on region growing to check the plane equation h of the region with the most points in turn for the first r regions clustered. i Whether the parameters meet the following requirements: In the formula, The plane equations are h i The normal vector in and the distance from the plane to the origin, θ is the plane equation h i With the current ground equation f i The threshold of the normal vector angle difference, ε d Approximately, it is the distance threshold between the two planes; if it meets the requirements, let the reference ground equation f i+1 =h i , mark the points in the relevant area; if there is no h that meets the requirements in the first r areas i , then let f i+1 =f i , there are no marked points in this area; When the number of points in point cloud A is higher than the threshold, the plane equation h is fitted to all points in A. i , and then h i Perform the same ground point cloud screening and record the result as point cloud B. When the number of points in point cloud B is greater than twice the number of points in point cloud A, let f i+1 =h i , mark the point in B as a ground point; otherwise, f i+1 =h i , mark the points in A as ground points; 1-4) Remove the part marked as the ground, the part below the ground height, and the part above the ground and higher than the robot height; set the current area as processed, set f i+1 is the current ground equation, and then checks the four neighboring areas of the current area in turn, and performs the operations in steps 1-2) to 1-4) on them; 1-5) After all areas are processed, merge the point clouds of all areas and project them to the z-axis. At this time, the ground areas in all point clouds have been deleted, and the remaining point clouds represent obstacles or areas where you cannot walk. Delete some boundary point clouds to control the map size. After statistical filtering of the point clouds to remove noise, divide the plane grid again. The occupancy value of this grid when generating the grid map is determined according to the number of points in the grid. 1-6) Since some border trimming and rotation operations will be performed on the map in step 1-5), the pose transformation and scale between the raster map coordinate system and the point cloud map coordinate system are recorded during the operation, which will be used to unify the two map coordinate systems during subsequent path planning.

3. A mobile robot navigation system based on solid-state laser radar and point cloud map according to claim 2, Features: The LiDAR inertial odometer module performs the following operations: 2-1) The robot remains stationary and collects solid-state lidar data to construct the initial cumulative point cloud for subsequent matching and residual calculation; collects stationary IMU data and uses the statistical mean to roughly estimate the measurement deviation b of the angular velocity meter and accelerometer ω ,b a and gravity vector g; wait for 2 to 3 seconds to complete the initialization of the mileage calculation method; 2-2) Process the radar data and IMU data, and then a frame of solid-state lidar point cloud data is abbreviated as scan. The high-frequency IMU data is used to forward propagate the current posture until the next scan is input. All sampling points in this scan are projected to the scan end time using posture interpolation to remove motion distortion. The specific mathematical form of posture forward propagation is as follows: In the formula, x i 、x i+1 are the states of IMU at the current time i and the next time i+1, respectively, and Δt is the interval between two IMU data; The lower right corner of each physical quantity is the object coordinate system described by the physical quantity, and the upper left corner is the coordinate system used to represent the physical quantity; the IMU coordinate system is denoted as I, and the IMU coordinate system at time i is denoted as I i , the first frame IMU coordinate system is the odometer coordinate system, which is abbreviated as the Odom coordinate system and is recorded as O; The state x i Including: the posture of the IMU coordinate system at time i Location speed IMU angular velocity and accelerometer measurement bias and the gravity vector O g i Representation in Odom coordinate system; is the generalized addition symbol. The physical quantities in the state quantity except the posture are linearly added during the generalized addition. The posture is operated according to the three-dimensional rotation group and its Lie algebra operation rules; u i is the motion input at time i, i.e., the angular velocity measurement from the IMU input Acceleration measurement w i is the system noise at time i, including the angular velocity measurement noise Acceleration measurement noise Angular velocity deviation noise and acceleration deviation noise 0 3×1 represents a zero vector with 3 rows and 1 column; F(x i ,u i ,w i ) is the state change per unit time; 2-3) Each point p in the dedistorted scan j The following formula is derived from the laser radar coordinate system L at time k: k Transform to Odom coordinate system: In the formula, I R L , I t L I is the pose transformation parameter between the pre-calibrated LiDAR coordinate system and the IMU coordinate system; k is the IMU coordinate system at time k; is the state quantity to be optimized O x k The posture and position items in , take the results after forward propagation as the initial values; Then for each point in the scan in the Odom coordinate system O p j , find the nearest point using nearest neighbor search in the accumulated point cloud O q j And use the nearest 5 points to fit the plane, and record its normal vector as n j , then the residual res j The form is: By optimizing the state O x k Minimize the total residual of each point in the scan, the optimal after iterative convergence That is the robot state in the current Odom coordinate system; 2-4) Based on the best estimate Posture and position in Add the dedistorted current scan to the accumulated point cloud.

4. A mobile robot navigation system based on solid-state laser radar and point cloud map according to claim 3, Features: The map matching and positioning module performs the following operations: 3-1) According to the robot's position in the point cloud map coordinate system and the radar's field of view angle α, the local point cloud in and near the field of view is selected. The point cloud map coordinate system is subsequently referred to as the Map coordinate system, and is denoted as M in the formula subscript. The specific operation is as follows: the local point cloud is composed of points that satisfy the following formula: M p composition: In the formula, L φ is the expression of the laser radar front direction vector in the laser radar coordinate system L, and δ is the allowable angle error; is the posture and position of the robot in the Map coordinate system, which is obtained from step 2-3) Pose transformation between Odom coordinate system and Map coordinate system M R O , M t O The obtained value is used as the initial value when the algorithm is initialized; 3-2) To ensure that there are enough points for matching and to prevent the pose error in the Map coordinate system from causing the common view area with the local point cloud segmented in step 3-1) to be too small, the scan after dedistortion in step 2-2) is stored in a queue data structure, and the point cloud in the sliding window in the Odom coordinate system is composed of multiple frames of scan; 3-3) Perform ICP matching on the local point cloud in the Map coordinate system in step 3-1) and the point cloud in the window in the Odom coordinate system in step 3-2), and solve and update the pose transformation between the Odom coordinate system and the Map coordinate system. M R O , M t O ; 3-4) Use the pose transformation between the Odom coordinate system and the Map coordinate system obtained in step 2-3) M R O , M t O The robot pose estimation in the Odom coordinate system obtained in step 2-3) Get the robot pose in the Map coordinate system: After that, the robot's position in the Map coordinate system It can be used as input for subsequent path planning modules.

5. A mobile robot navigation system based on solid-state laser radar and point cloud map according to claim 4, Features: The path planning module performs the following operations: 4-1) Input the grid map in step 1-5), and regard the grids in the map that are higher than the set threshold as obstacles, and the grids that are lower than the threshold as passable areas; 4-2) Continue to receive the robot pose in the Map coordinate system in step 3-4), and use the transformation relationship between the point cloud map and the grid map coordinate system recorded in step 1-6) to align the two coordinate systems. The resulting pose is the current position of the robot in the grid map. 4-3) Input the coordinates of the navigation destination, combine the robot's current position and map obstacle information to make a path plan, and then convert it into motion control command output to complete the navigation.

Images