Mobile robot for autonomously mapping in unknown closed space and working method

Abstract

The invention discloses a mobile robot for autonomously mapping in an unknown closed space and a working method. The mobile robot comprises a robot main body, a three-dimensional laser radar, a two-dimensional laser radar, an inertial measurement unit and a wheel type odometer, according to the method, a mathematical model of scene information density is constructed, so that the relationship between the information density and navigation and map construction performance of a robot main body is obtained, quantification of the sparse degree of point clouds in an unknown environment space is realized, and autonomous navigation and decision making of the mobile robot in the unknown environment are realized; then, determining a target point based on the point cloud information density, constructing a three-dimensional point cloud map of the unknown closed environment by using a three-dimensional laser radar, and performing path planning and navigation on a two-dimensional grid map based on the two-dimensional laser radar to realize autonomous mapping of the mobile robot in the unknown environment; according to the invention, full-automatic path planning and obstacle avoidance in an unknown closed environment can be realized without relying on manual intervention, so that accurate mapping is carried out on the unknown environment, and the efficiency is high.

Description

Technical Field

[0001] The present invention relates to a mobile robot and a working method for autonomously building a map in an unknown closed space, belonging to the technical field of point cloud data processing and environment mapping. Background Art

[0002] With the rapid development of artificial intelligence and robotics, autonomous navigation and decision-making systems are gradually showing their great application potential in many fields, especially in industry, mining and transportation. In mapping and navigation in unknown environments, it is crucial to make full use of the point cloud information of 3D radar. The point cloud information density refers to the amount of point cloud information contained in a unit space, which reflects the complexity and richness of the environment. In unknown and completely blind confined space scenes, when using lidar for mapping, the distribution of point cloud information density is often uneven. The point cloud information density in some key areas may be high, while the point cloud information density in some ordinary areas is relatively low. Therefore, how to accurately evaluate and utilize these information densities is crucial for the robot's autonomous navigation and decision-making in this scenario.

[0003] Most existing environmental mapping technologies rely on full manual control or semi-autonomous operation. Full manual control mapping refers to the operator manually controlling the mobile device to collect unknown environmental information and complete the mapping; semi-autonomous mapping is to use the device to autonomously plan the movement path, combined with manual intervention to improve the integrity of the map and ensure the mobile safety of the device. However, these methods have certain application limitations. For example, in closed underground environments, it is difficult to establish stable and reliable communication between mobile devices and the control end, which makes it difficult for humans to intervene, and thus it is impossible to smoothly perform environmental mapping under full manual or semi-autonomous conditions.

[0004] In summary, how to provide a fully autonomous mapping solution for mobile intelligent agents in unknown closed environments, so that they can accurately map unknown environments fully and automatically without relying on human intervention, is the research direction required by the present invention. Summary of the invention

[0005] In view of the problems existing in the above-mentioned prior art, the present invention provides a mobile robot and a working method for autonomously building maps in an unknown closed space, which can realize fully automatic path planning and obstacle avoidance in an unknown closed environment without relying on human intervention, thereby accurately building maps of the unknown environment.

[0006] In order to achieve the above-mentioned object, the technical solution adopted by the present invention is: a mobile robot for autonomously building a map in an unknown closed space, comprising a robot body, a three-dimensional laser radar, a two-dimensional laser radar, an inertial measurement unit (IMU), a wheel odometer and a central processor;

[0007] The three-dimensional laser radar is mounted on the top of the robot body through a bracket, and can rotate around the bracket to obtain 360° real-time laser scanning data of the environment in which the robot body is located;

[0008] The two-dimensional laser radar is installed on the top of the robot body through a turntable and in front of the bracket of the three-dimensional laser radar. The turntable can drive the two-dimensional laser radar to rotate 360 ​​degrees, so as to obtain real-time scanning data during the movement of the robot body, and feed it back to the central processor to construct a two-dimensional grid map for autonomous path planning and obstacle avoidance.

[0009] The inertial measurement unit is installed at the center of gravity of the robot body and is used to measure the acceleration information and angular velocity information of the robot body;

[0010] The wheel odometer is mounted on the driving wheel of the robot body and is used to obtain the wheel rotation information and travel distance information of the robot body;

[0011] The central processor is installed inside the robot body and is used to obtain monitoring data from three-dimensional laser radar, two-dimensional laser radar, inertial measurement unit and wheel speed odometer, and control the movement path of the robot body after analysis and processing until the autonomous mapping work is completed.

[0012] Furthermore, the robot body is a wheeled robot, and the central processor realizes the robot's forward, backward and turning movements by controlling the steering angle and speed of the front and rear wheels of the robot body; the three-dimensional laser radar is a 256-line laser radar.

[0013] The above-mentioned autonomous mapping method of the mobile robot in an unknown closed space has the following specific steps:

[0014] Step 1: Determine the real-time position and posture of the robot body: The central processor uses 3D laser radar and 2D laser radar to collect environmental information, and integrates the data fed back by the inertial measurement unit and wheel speed odometer for positioning. The extended Kalman filter (EKF) is used to combine the inertial measurement unit and odometer data, and the position and posture of the robot body is predicted and updated based on the motion model.

[0015] Step 2: Autonomous map construction: Extract feature points from the 3D point cloud data collected by the 3D laser radar. Feature points include corner points and plane points. Match feature points between each frame of the 3D laser radar scan, use corner points to match edge features, and plane points to match plane features to estimate the relative pose change between the current frame and the previous frame. According to the feature point matching results between multiple frames, optimize the global pose of the 3D laser radar through the Levenberg-Marquardt optimization algorithm, insert each frame of 3D laser radar data into the global point cloud map, build a complete 3D map, and update the map at a lower frequency. Convert each frame of local point cloud data into point cloud information in the global coordinate system and add it to the global point cloud map.

[0016] Step 3: Determine the target point for the robot to move: perform radius filtering on the point cloud data collected by the two-dimensional laser radar; construct the minimum bounding box of the current point cloud data and divide it into several voxel grids; calculate the point cloud information density of each voxel grid, select the voxel grid that does not reach the density threshold, and use its central projection point as the local path planning target point in the two-dimensional grid map;

[0017] Step 4: Robot movement and path optimization: Ensure that the movement path planned in step 3 meets the robot kinematics and obstacle avoidance requirements through kinematic constraints, time constraints, obstacle constraints and smoothness constraints; then optimize the path geometry and time parameters simultaneously through the global cost function, and minimize the cost function to obtain the optimal path; use two-dimensional laser radar for environmental perception and real-time obstacle avoidance until the robot body moves to the target point calculated in step 3;

[0018] Step 5: Repeat steps 1 to 4 to continuously update the global map until the autonomous mapping of the current environment is completed.

[0019] Further, the step 1 is specifically as follows:

[0020] Step 1.1: Preprocess the data. The wheel odometer obtains data from the encoder of the driving wheel and converts it into linear velocity and angular velocity. The inertial measurement unit obtains acceleration and angular velocity data and integrates them to estimate the velocity and attitude changes.

[0021] Step 1.2: Use the monitoring data of the IMU to predict the robot's position, speed and posture at the next moment, and use the measurement value of the wheel odometer to correct the predicted state; fuse the data of the wheel odometer and the IMU in the state space model, where the wheel odometer is regarded as an observation value, and update the state estimate of the Kalman filter;

[0022] Step 1.3: Based on Bayesian reasoning, combine the prior probability (prediction) and observation probability (measurement) to estimate the state vector and its covariance matrix; continue to receive inertial measurement unit and wheel odometer data, continuously perform prediction and update steps, correct the robot's state estimation in real time, and realize prediction and update of the robot's main body's posture based on the motion model.

[0023] Further, the step 2 is specifically as follows:

[0024] Step 2.1: Extract two types of feature points, corner points and surface points, from the 3D point cloud data. The curvature calculation formula is: Where k is a scanning cycle, L is the current coordinate system of the 3D laser radar, i represents the i-th point of the k-th scanning point cloud, represents the position, the coordinate system of the kth scan is L, the position of the i-th point in the point cloud in L, S is the point The specified points nearby include the front, back, left, and right points, and points at 0.25 degree intervals. is the vector norm, a measure of the length of the vector. The default is 2 norm. is a metric space, representing the Euclidean distance between two vectors;

[0025] Step 2.2: Based on the curvature calculation formula in step 2.1, filter the feature points and classify the points in the data according to the thresholds of c in different ranges into points with particularly large curvature, points with large curvature, and points with small curvature;

[0026] Step 2.3: Select the points with particularly large curvature as edge points and the points with smaller curvature as plane points among the classified points;

[0027] Step 2.4: Match the continuous feature frames, establish the error model, and construct the error function using the distance from point to line and the distance from point to surface. The problem becomes solving That is, the cumulative sum of all point-to-line and point-to-surface distances from the transformed point of the i-th point in the k+1-th frame point cloud to the reference point cloud is the problem model to be solved;

[0028] Step 2.5: Add rotation R and translation t. The relationship between the i-th point of the original data point cloud of the k+1th frame and the changed point cloud is:

[0029] Step 2.6: From steps 2.4 and 2.5, we can derive the Loss function as in is the position vector of the i-th point in the k+1-th scan point cloud, is the position vector of the i-th point in the k+1-th scan point cloud The position vector after rotation and translation;

[0030] Step 2.7: During the scanning process, the 3D lidar motion is modeled with constant angular velocity and linear velocity, and the attitude transformation of the points received at different times within the scanning range is linearly interpolated;

[0031] Step 2.8: Let t be the timestamp, record t k+1 is the start time of scan k+1, let is the laser radar at [t k+1 ,t], Includes rigid body motion of 3D LiDAR in 6 degrees of freedom, where t x ,t y ,t z are the translations along the x, y, and z axes of L, θ x ,θ y ,θ z is the rotation angle of the three corresponding axes, following the right-hand rule, given a point i, i∈P k+1 , let t i As its timestamp, and set for [t k+1 ,t i ], assuming that the radar moves at a constant angular velocity and linear velocity, then Can pass Linear interpolation formula calculate;

[0032] Step 2.9: Use nonlinear optimization methods To solve;

[0033] Step 2.10: Start building a map after accumulating a certain amount of point cloud data, integrate the point cloud data into the world map at a lower frequency, and accurately estimate the position of the laser in the world coordinate system.

[0034] Further, the step three is specifically as follows:

[0035] Step 3.1: Construct a KD tree for the point cloud data collected by the current two-dimensional laser radar and establish the point cloud topological relationship; calculate the number of neighboring points within the neighborhood of any point in the point cloud, and determine whether the number of neighboring points is less than the judgment threshold. If it is less, it is considered a noise point and removed. Repeat the above steps until all points in the point cloud are processed;

[0036] Step 3.2: Using the map coordinate system as a reference, use PCL to traverse each point in the point cloud data and compare the X, Y, and Z values ​​in turn;

[0037] Step 3.3: The maximum value of the current local point cloud data in the three directions of X, Y, and Z obtained from step 3.2 is recorded as x max,y max and z max , the minimum value is recorded as x min ,y min and z min ;

[0038] Step 3.4: According to step 3.3, get the maximum and minimum values ​​on each coordinate axis and calculate the side length l of the minimum bounding box x ,l y ,l z , the calculation formula is

[0039] Step 3.5: Divide the lengths of the sides in the three coordinate axes into m1, m2, and m3 to divide the minimum bounding box into n individual cells. The volume of each voxel is V = a·b·c, where a, b, c are the lengths of the sides of the voxel in the x, y, and z directions;

[0040] Step 3.6: Let the coordinates of the pth point in the point cloud data be (x p ,y p ,z p ), label each voxel grid, let q=i·4+j+k, where Indicates the floor symbol, point (x p ,y p ,z p ) is located in the qth individual lattice;

[0041] Step 3.7: Traverse each point in the point cloud data and assign it to the corresponding voxel grid according to step 3.6. Let the total number of points in the qth voxel grid be W q , the point cloud density of the qth individual grid is Where V = a·b·c;

[0042] Step 3.8: By comparing with the set point cloud density threshold, assuming that the point cloud density of the qth area with the smallest point cloud density does not meet the threshold requirement, the center of the qth individual grid is used as the target point of this local path planning.

[0043] Further, the step 4 is specifically as follows:

[0044] Step 4.1: Detect the location of the obstacle through the two-dimensional laser radar and map it into the map, assigning a high-order value to the grid where the obstacle is located;

[0045] Step 4.2: A certain range around the obstacle will be marked as an expansion area; the location information of the obstacle is updated in real time using the local cost map;

[0046] Step 4.3: Take the initial trajectory generated by the global path planner as input, discretize the initial motion path into a series of waypoints, and assign a time segment to each waypoint;

[0047] Step 4.4: Each path point is represented by X i =(x i ,y i ,θ i ) T Indicates that x i and i is the spatial position of the waypoint, t i is the time information; the position and posture coordinate sequence of the robot body in the motion space is represented by Q = {X i} i＝0,…,n ,n∈N represents;

[0048] Step 4.5: The time interval Δt between two adjacent path points i =t i+1 -t i , represents the movement time of the robot between these two path points, and the time series is given by τ={Δt i} i＝0,…,n ,n∈N represents, N represents the number of control points;

[0049] Step 4.6: Combine the time series and position series of the above control points to obtain B = (Q, τ), where N represents the number of control points. Multi-objective optimization is performed in both the time series and the control point series using a weighted approach.

[0050] Step 4.7: Through the cost function Minimize the angle change between path points and constrain the smoothness between path points, where i = 0, ..., n, n∈N, N represents the number of control points;

[0051] Step 4.8: Through the cost function Minimize the total time of the path and keep the time uniformly distributed, where is the average time interval between path points, i = 0,…,n,n∈N, N represents the number of control points;

[0052] Step 4.9: Distance cost function from the minimum path point to the obstacle Make sure the waypoints are away from obstacles, where d(x i ,o) is the path point X i The distance to the obstacle, i = 0, ..., n, n∈N, N represents the number of control points;

[0053] Step 4.10: Through the cost function Keep the robot's velocity and acceleration on the path to satisfy its kinematic constraints, where i = 0, ..., n, n∈N, N represents the number of control points;

[0054] Step 4.11: Exploitation Get the optimal solution of B, find the optimal path point location and time distribution, where B * represents the optimal result, f(B) is the total cost considering multiple objectives, γ k Represents the weight of the cost function of each optimization objective, k = 1, 2, 3, 4;

[0055] Step 4.12: The robot body moves according to the path, speed and acceleration data determined above until the robot body moves to the target point calculated in step 3.

[0056] Compared with the prior art, the present invention has the following advantages:

[0057] 1. The present invention constructs a mathematical model of scene information density, obtains the relationship between information density and the navigation and map building performance of the robot body, realizes the quantification of the sparsity of the spatial point cloud of the unknown environment, and realizes the autonomous navigation and decision-making of the mobile robot in the unknown environment.

[0058] 2. The present invention determines the target point based on the point cloud information density, and uses the three-dimensional laser radar to construct a three-dimensional point cloud map of the unknown closed environment. It performs path planning and navigation on the two-dimensional grid map based on the two-dimensional laser radar, so as to realize autonomous mapping of the mobile robot in the unknown environment. It can realize fully automatic path planning and obstacle avoidance in the unknown closed environment without relying on human intervention, so as to accurately map the unknown environment with high efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a framework diagram of the autonomous mapping process of the present invention;

[0060] Figure 2 It is a schematic diagram of the relative position relationship of each sensor in the present invention;

[0061] Figure 3 A schematic diagram of the process of generating a three-dimensional point cloud map in the present invention;

[0062] Figure 4 A schematic diagram of the process of determining a target point by point cloud information density in the present invention;

[0063] Figure 5 It is a schematic diagram of the path planning process in the present invention.

[0064] In the figure: 1. 3D laser radar; 2. Bracket; 3. 2D laser radar; 4. Robot body; 5. Inertial measurement unit. DETAILED DESCRIPTION

[0065] The present invention will be further described below.

[0066] like Figure 2 As shown, a mobile robot for autonomously building a map in an unknown closed space includes a robot body 4, a three-dimensional laser radar 1, a two-dimensional laser radar 3, an inertial measurement unit 5, a wheel odometer and a central processor;

[0067] The three-dimensional laser radar 1 is mounted on the top of the robot body 4 through the bracket 2, and can rotate around the bracket 2 to obtain 360° real-time laser scanning data of the environment in which the robot body 4 is located;

[0068] The two-dimensional laser radar 3 is installed on the top of the robot body 4 through a turntable and is located in front of the bracket of the three-dimensional laser radar 1. The turntable can drive the two-dimensional laser radar to rotate 360°, which is used to obtain real-time scanning data during the movement of the robot body 4, and feed it back to the central processor to construct a two-dimensional grid map for autonomous path planning and obstacle avoidance;

[0069] The inertial measurement unit 5 is installed at the center of gravity of the robot body 4 and is used to measure the acceleration information and angular velocity information of the robot body 4;

[0070] The wheel odometer is mounted on the driving wheel of the robot body 4 and is used to obtain the wheel rotation information and travel distance information of the robot body 4;

[0071] The central processor is installed inside the robot body 4, and is used to obtain the monitoring data of the three-dimensional laser radar 1, the two-dimensional laser radar 3, the inertial measurement unit 5 and the wheel speed odometer, and control the moving path of the robot body 4 after analysis and processing until the autonomous mapping work is completed;

[0072] The robot body 4 is a wheeled robot, and the central processor realizes the robot's forward, backward and turning movements by controlling the steering angle and speed of the front and rear wheels of the robot body 4; the three-dimensional laser radar 1 is a 256-line laser radar.

[0073] like Figure 1 As shown in the figure, the autonomous mapping method of the mobile robot in the unknown closed space completes the autonomous mapping and positioning of the unknown closed space through the mapping algorithm, exploration strategy and path planning algorithm based on radar odometer and nonlinear optimization; the sparsity of the spatial point cloud is quantified by constructing a point cloud information density model, and the voxel grid division of the local point cloud map is realized, so as to provide the mobile robot with path planning target points and realize autonomous exploration of the unknown environment. The specific steps are as follows:

[0074] Step 1: Determine the real-time position and posture of the robot body: The central processor uses the three-dimensional laser radar 1 and the two-dimensional laser radar 3 to collect environmental information, and integrates the data fed back by the inertial measurement unit 5 and the wheel speed odometer for positioning. The extended Kalman filter (EKF) is used to combine the inertial measurement unit 5 and the odometer data, and the position and posture of the robot body 4 is predicted and updated based on the motion model. Specifically:

[0075] Step 1.1: Preprocess the data. The wheel odometer obtains data from the encoder of the driving wheel and converts it into linear velocity and angular velocity. The inertial measurement unit 5 obtains acceleration and angular velocity data and integrates them to estimate the velocity and attitude changes.

[0076] Step 1.2: Use the monitoring data of the inertial measurement unit 5 to predict the position, speed and posture of the robot at the next moment, and use the measurement value of the wheel odometer to correct the predicted state; fuse the data of the wheel odometer and the inertial measurement unit 5 in the state space model, where the wheel odometer is regarded as an observation value, and update the state estimation of the Kalman filter;

[0077] Step 1.3: Based on Bayesian reasoning, the state vector and its covariance matrix are estimated by combining the prior probability (prediction) and the observation probability (measurement); the inertial measurement unit 5 and the wheel odometer data are continuously received, and the prediction and update steps are continuously performed to correct the robot's state estimation in real time, so as to predict and update the posture of the robot body 4 based on the motion model.

[0078] Step 2: Autonomous map construction: Extract feature points from the 3D point cloud data collected by the 3D laser radar 1. Feature points include corner points and plane points. Match feature points between each frame scanned by the 3D laser radar 1. Use corner points to match edge features and plane points to match plane features to estimate the relative pose change between the current frame and the previous frame. According to the feature point matching results between multiple frames, optimize the global pose of the 3D laser radar 1 through the Levenberg-Marquardt optimization algorithm, insert each frame of the 3D laser radar 1 data into the global point cloud map, construct a complete 3D map, and update the map at a lower frequency. Convert each frame of local point cloud data into point cloud information in the global coordinate system and add it to the global point cloud map, such as Figure 3 As shown, specifically:

[0079] Step 2.1: Extract two types of feature points, corner points and surface points, from the 3D point cloud data. The curvature calculation formula is: Where k is a scanning cycle, L is the current coordinate system of the 3D laser radar, i represents the i-th point of the k-th scanning point cloud, represents the position, the coordinate system of the kth scan is L, the position of the i-th point in the point cloud in L, S is the point The specified points nearby include the front, back, left, and right points, and points at 0.25 degree intervals. is the vector norm, a measure of the length of the vector. The default is 2 norm. is a metric space, representing the Euclidean distance between two vectors;

[0080] Step 2.2: Based on the curvature calculation formula in step 2.1, filter the feature points and classify the points in the data according to the thresholds of c in different ranges into points with particularly large curvature, points with large curvature, and points with small curvature;

[0081] Step 2.3: Select the points with particularly large curvature as edge points and the points with smaller curvature as plane points among the classified points;

[0082] Step 2.4: Match the continuous feature frames, establish the error model, and construct the error function using the distance from point to line and the distance from point to surface. The problem becomes solving That is, the cumulative sum of all point-to-line and point-to-surface distances from the transformed point of the i-th point in the k+1-th frame point cloud to the reference point cloud is the problem model to be solved;

[0083] Step 2.5: Add rotation R and translation t. The relationship between the i-th point of the original data point cloud of the k+1th frame and the changed point cloud is:

[0084] Step 2.6: From steps 2.4 and 2.5, we can derive the Loss function as in is the position vector of the i-th point in the k+1-th scan point cloud, is the position vector of the i-th point in the k+1-th scan point cloud The position vector after rotation and translation;

[0085] Step 2.7: During the scanning process, the motion of the 3D laser radar 1 is modeled with constant angular velocity and linear velocity, and the attitude transformation of the points received at different times within the scanning range is linearly interpolated;

[0086] Step 2.8: Let t be the timestamp, record t k+1 is the start time of scan k+1, let is the laser radar at [t k+1 ,t], Includes rigid body motion of 3D LiDAR in 6 degrees of freedom, where t x ,t y ,t z are the translations along the x, y, and z axes of L, θ x ,θ y ,θz is the rotation angle of the three corresponding axes, following the right-hand rule, given a point i, i∈P k+1 , let t i As its timestamp, and set for [t k+1 ,t i ], assuming that the radar moves at a constant angular velocity and linear velocity, then Can pass Linear interpolation formula calculate;

[0087] Step 2.9: Use nonlinear optimization methods To solve;

[0088] Step 2.10: Start building a map after accumulating a certain amount of point cloud data, integrate the point cloud data into the world map at a lower frequency, and accurately estimate the position of the laser in the world coordinate system.

[0089] Step 3: Determine the target point for the robot to move: perform radius filtering on the point cloud data collected by the two-dimensional laser radar 3; construct the minimum bounding box of the current point cloud data and divide it into several voxels; calculate the point cloud information density of each voxel, select the voxel that does not reach the density threshold, and use its central projection point as the local path planning target point in the two-dimensional grid map, such as Figure 4 As shown, specifically:

[0090] Step 3.1: Construct a KD tree for the point cloud data collected by the current two-dimensional laser radar 3 to establish the point cloud topological relationship; calculate the number of neighboring points within the neighborhood of any point in the point cloud, and determine whether the number of neighboring points is less than the judgment threshold. If it is less, it is considered as a noise point and removed. Repeat the above steps until all points in the point cloud are processed;

[0091] Step 3.2: Using the map coordinate system as a reference, use PCL to traverse each point in the point cloud data and compare the X, Y, and Z values ​​in turn;

[0092] Step 3.3: The maximum value of the current local point cloud data in the three directions of X, Y, and Z obtained from step 3.2 is recorded as x max ,y max and z max , the minimum value is recorded as x m i n ,y min and z min ;

[0093] Step 3.4: According to step 3.3, get the maximum and minimum values ​​on each coordinate axis and calculate the side length l of the minimum bounding box x ,l y ,lz , the calculation formula is

[0094] Step 3.5: Divide the lengths of the sides in the three coordinate axes into m1, m2, and m3. In this embodiment, m1=4, m2=4, and m3=1 to divide the minimum bounding box into n individual cells. The volume of each voxel is V = a·b·c, where a, b, c are the lengths of the sides of the voxel in the x, y, and z directions;

[0095] Step 3.6: Let the coordinates of the pth point in the point cloud data be (x p ,y p ,z p ), label each voxel grid, let q=i·4+j+k, where Indicates the floor symbol, point (x p ,y p ,z p ) is located in the qth individual lattice;

[0096] Step 3.7: Traverse each point in the point cloud data and assign it to the corresponding voxel grid according to step 3.6. Let the total number of points in the qth voxel grid be W q , the point cloud density of the qth individual grid is Where V = a·b·c;

[0097] Step 3.8: By comparing with the set point cloud density threshold, assuming that the point cloud density of the qth area with the smallest point cloud density does not meet the threshold requirement, the center of the qth individual grid is used as the target point of this local path planning.

[0098] Step 4: Robot movement and path optimization: Ensure that the movement path planned in step 3 meets the robot kinematics and obstacle avoidance requirements through kinematic constraints, time constraints, obstacle constraints and smoothness constraints; then optimize the path geometry and time parameters simultaneously through the global cost function, minimize the cost function to obtain the optimal path; use the two-dimensional laser radar 3 for environmental perception and real-time obstacle avoidance until the robot body 4 moves to the target point calculated in step 3, such as Figure 5 As shown, specifically:

[0099] Step 4.1: The position of the obstacle is detected by the two-dimensional laser radar 3 and mapped into the map, and the grid where the obstacle is located is assigned a high value;

[0100] Step 4.2: A certain range around the obstacle will be marked as an expansion area; the location information of the obstacle is updated in real time using the local cost map;

[0101] Step 4.3: Take the initial trajectory generated by the global path planner as input, discretize the initial motion path into a series of waypoints, and assign a time segment to each waypoint;

[0102] Step 4.4: Each path point is represented by X i =(x i ,y i ,θ i ) T Indicates that x i and i is the spatial position of the waypoint, t i is the time information; the position and posture coordinate sequence of the robot body in the motion space is represented by Q = {X i} i＝0,…,n ,n∈N represents;

[0103] Step 4.5: The time interval Δt between two adjacent path points i =t i+1 -t i , represents the movement time of the robot between these two path points, and the time series is given by τ={Δt i} i＝0,…,n ,n∈N represents, N represents the number of control points;

[0104] Step 4.6: Combine the time series and position series of the above control points to obtain B = (Q, τ), where N represents the number of control points. Multi-objective optimization is performed in both the time series and the control point series using a weighted approach.

[0105] Step 4.7: Through the cost function Minimize the angle change between path points and constrain the smoothness between path points, where i = 0, ..., n, n∈N, N represents the number of control points;

[0106] Step 4.8: Through the cost function Minimize the total time of the path and keep the time uniformly distributed, where is the average time interval between path points, i = 0,…,n,n∈N, N represents the number of control points;

[0107] Step 4.9: Distance cost function from the minimum path point to the obstacle Make sure the waypoints are away from obstacles, where d(x i ,o) is the path point X i The distance to the obstacle, i = 0, ..., n, n∈N, N represents the number of control points;

[0108] Step 4.10: Through the cost function Keep the robot's velocity and acceleration on the path to satisfy its kinematic constraints, where i = 0, ..., n, n∈N, N represents the number of control points;

[0109] Step 4.11: Exploitation Get the optimal solution of B, find the optimal path point location and time distribution, where B * represents the optimal result, f(B) is the total cost considering multiple objectives, γ k Represents the weight of the cost function of each optimization objective, k = 1, 2, 3, 4;

[0110] Step 4.12: The robot body 4 moves and avoids obstacles in real time according to the path, speed and acceleration data determined above, until the robot body 4 moves to the target point calculated in step 3.

[0111] Step 5: Repeat steps 1 to 4 to continuously update the global map until the point cloud information density values ​​of all divided areas reach the threshold requirement, completing the autonomous mapping of the current environment.

[0112] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A mobile robot capable of autonomously mapping an unknown enclosed space, characterized in that: It includes a robot body, a 3D laser radar, a 2D laser radar, an inertial measurement unit, a wheel odometer and a central processor; The three-dimensional laser radar is mounted on the top of the robot body through a bracket, and can rotate around the bracket to obtain 360° real-time laser scanning data of the environment in which the robot body is located; The two-dimensional laser radar is installed on the top of the robot body through a turntable and in front of the bracket of the three-dimensional laser radar. The turntable can drive the two-dimensional laser radar to rotate 360 ​​degrees, so as to obtain real-time scanning data during the movement of the robot body, and feed it back to the central processor to construct a two-dimensional grid map for autonomous path planning and obstacle avoidance. The inertial measurement unit is installed at the center of gravity of the robot body and is used to measure the acceleration information and angular velocity information of the robot body; The wheel odometer is mounted on the driving wheel of the robot body and is used to obtain the wheel rotation information and travel distance information of the robot body; The central processor is installed inside the robot body and is used to obtain monitoring data from three-dimensional laser radar, two-dimensional laser radar, inertial measurement unit and wheel speed odometer, and control the movement path of the robot body after analysis and processing until the autonomous mapping work is completed.

2. The mobile robot according to claim 1, characterized in that: The robot body is a wheeled robot, and the central processor realizes the robot's forward, backward and turning movements by controlling the steering angle and speed of the front and rear wheels of the robot body; the three-dimensional laser radar is a 256-line laser radar.

3. An autonomous mapping method for a mobile robot according to claim 1 or 2, characterized in that: The specific steps are: Step 1: Determine the real-time position and posture of the robot body: The central processor uses 3D laser radar and 2D laser radar to collect environmental information, and integrates the data fed back by the inertial measurement unit and wheel speed odometer for positioning. The extended Kalman filter is used to combine the inertial measurement unit and odometer data, and the position and posture of the robot body is predicted and updated based on the motion model. Step 2: Autonomous map construction: Extract feature points from the 3D point cloud data collected by the 3D laser radar. Feature points include corner points and plane points. Match feature points between each frame of the 3D laser radar scan, use corner points to match edge features, and plane points to match plane features to estimate the relative pose change between the current frame and the previous frame. According to the feature point matching results between multiple frames, optimize the global pose of the 3D laser radar through the Levenberg-Marquardt optimization algorithm, insert each frame of 3D laser radar data into the global point cloud map, build a complete 3D map, and update the map at a lower frequency. Convert each frame of local point cloud data into point cloud information in the global coordinate system and add it to the global point cloud map. Step 3: Determine the target point for the robot to move: perform radius filtering on the point cloud data collected by the two-dimensional laser radar; construct the minimum bounding box of the current point cloud data and divide it into several voxel grids; calculate the point cloud information density of each voxel grid, select the voxel grid that does not reach the density threshold, and use its central projection point as the local path planning target point in the two-dimensional grid map; Step 4: Robot movement and path optimization: Ensure that the movement path planned in step 3 meets the robot kinematics and obstacle avoidance requirements through kinematic constraints, time constraints, obstacle constraints and smoothness constraints; then optimize the path geometry and time parameters simultaneously through the global cost function, and minimize the cost function to obtain the optimal path; use two-dimensional laser radar for environmental perception and real-time obstacle avoidance until the robot body moves to the target point calculated in step 3; Step 5: Repeat steps 1 to 4 to continuously update the global map until the autonomous mapping of the current environment is completed.

4. The autonomous mapping method according to claim 3, characterized in that: The step 1 is specifically as follows: Step 1.1: Preprocess the data. The wheel odometer obtains data from the encoder of the driving wheel and converts it into linear velocity and angular velocity. The inertial measurement unit obtains acceleration and angular velocity data and integrates them to estimate the velocity and attitude changes. Step 1.2: Use the monitoring data of the IMU to predict the robot's position, speed and posture at the next moment, and use the measurement value of the wheel odometer to correct the predicted state; fuse the data of the wheel odometer and the IMU in the state space model, where the wheel odometer is regarded as an observation value, and update the state estimate of the Kalman filter; Step 1.3: Based on Bayesian reasoning, combine the prior probability and the observed probability to estimate the state vector and its covariance matrix; Continuously receive inertial measurement unit and wheel odometer data, continuously perform prediction and update steps, correct the robot's state estimation in real time, and predict and update the robot's main body's position and posture based on the motion model.

5. The autonomous mapping method according to claim 3, characterized in that: The step 2 is specifically as follows: Step 2.1: Extract two types of feature points, corner points and surface points, from the 3D point cloud data. The curvature calculation formula is: Where k is a scanning cycle, L is the current coordinate system of the 3D laser radar, i represents the i-th point of the k-th scanning point cloud, represents the position, the coordinate system of the kth scan is L, the position of the i-th point in the point cloud in L, S is the point The specified points nearby include the front, back, left, and right points, and points at 0.25 degree intervals. is the vector norm, a measure of the length of the vector. The default is 2 norm. is a metric space, representing the Euclidean distance between two vectors; Step 2.2: Based on the curvature calculation formula in step 2.1, filter the feature points and classify the points in the data according to the thresholds of c in different ranges into points with particularly large curvature, points with large curvature, and points with small curvature; Step 2.3: Select the points with particularly large curvature as edge points and the points with smaller curvature as plane points among the classified points; Step 2.4: Match the continuous feature frames, establish the error model, and construct the error function using the distance from point to line and the distance from point to surface. The problem becomes solving That is, the cumulative sum of all point-to-line and point-to-surface distances from the transformed point of the i-th point in the k+1-th frame point cloud to the reference point cloud is the problem model to be solved; Step 2.5: Add rotation R and translation t. The relationship between the i-th point of the original data point cloud of the k+1th frame and the changed point cloud is: Step 2.6: From steps 2.4 and 2.5, we can derive the Loss function as in is the position vector of the i-th point in the k+1-th scan point cloud, is the position vector of the i-th point in the k+1-th scan point cloud The position vector after rotation and translation; Step 2.7: During the scanning process, the 3D lidar motion is modeled with constant angular velocity and linear velocity, and the attitude transformation of the points received at different times within the scanning range is linearly interpolated; Step 2.8: Let t be the timestamp, record t k+1 is the start time of scan k+1, let is the laser radar at [t k+1 ,t], Includes rigid body motion of 3D LiDAR in 6 degrees of freedom, where t x ,t y ,t z are the translations along the x, y, and z axes of L, θ x ,θ y ,θ z is the rotation angle of the three corresponding axes, following the right-hand rule, given a point i, i∈P k+1 , let t i As its timestamp, and set for [t k+1 ,t i ], assuming that the radar moves at a constant angular velocity and linear velocity, then Can pass Linear interpolation formula calculate; Step 2.9: Use nonlinear optimization methods To solve; Step 2.10: Start building a map after accumulating a certain amount of point cloud data, integrate the point cloud data into the world map at a lower frequency, and accurately estimate the position of the laser in the world coordinate system.

6. The autonomous mapping method according to claim 3, characterized in that: The step three is specifically as follows: Step 3.1: Construct a KD tree for the point cloud data collected by the current two-dimensional laser radar and establish the point cloud topological relationship; calculate the number of neighboring points within the neighborhood of any point in the point cloud, and determine whether the number of neighboring points is less than the judgment threshold. If it is less, it is considered a noise point and removed. Repeat the above steps until all points in the point cloud are processed; Step 3.2: Using the map coordinate system as a reference, use PCL to traverse each point in the point cloud data and compare the X, Y, and Z values ​​in turn; Step 3.3: The maximum value of the current local point cloud data in the three directions of X, Y, and Z obtained from step 3.2 is recorded as x max ,y max and z max , the minimum value is recorded as x min ,y min and z min ; Step 3.4: According to step 3.3, get the maximum and minimum values ​​on each coordinate axis and calculate the side length l of the minimum bounding box x ,l y ,l z , the calculation formula is Step 3.5: Divide the lengths of the sides in the three coordinate axes into m1, m2, and m3 to divide the minimum bounding box into n individual cells. The volume of each voxel is V = a·b·c, where a, b, c are the lengths of the sides of the voxel in the x, y, and z directions; Step 3.6: Let the coordinates of the pth point in the point cloud data be (x p ,y p ,z p ), label each voxel grid, let q=i·4+j+k, where Indicates the floor symbol, point (x p ,y p ,z p ) is located in the qth individual lattice; Step 3.7: Traverse each point in the point cloud data and assign it to the corresponding voxel grid according to step 3.

6. Let the total number of points in the qth voxel grid be W q , the point cloud density of the qth individual grid is Where V = a·b·c; Step 3.8: By comparing with the set point cloud density threshold, assuming that the point cloud density of the qth area with the smallest point cloud density does not meet the threshold requirement, the center of the qth individual grid is used as the target point of this local path planning.

7. The autonomous mapping method according to claim 3, characterized in that: The step 4 is specifically as follows: Step 4.1: Detect the location of the obstacle through the two-dimensional laser radar and map it into the map, assigning a high-order value to the grid where the obstacle is located; Step 4.2: A certain range around the obstacle will be marked as an expansion area; the location information of the obstacle is updated in real time using the local cost map; Step 4.3: Take the initial trajectory generated by the global path planner as input, discretize the initial motion path into a series of waypoints, and assign a time segment to each waypoint; Step 4.4: Each path point is represented by X i =(x i ,y i ,θ i ) T Indicates that x i and i is the spatial position of the waypoint, t i It is time information; The position and posture coordinate sequence of the robot body in the motion space is Q = {X i } i＝0,,n ,n∈N represents; Step 4.5: The time interval Δt between two adjacent path points i =t i+1 -t i , represents the movement time of the robot between these two path points, and the time series is given by τ={Δt i } i＝0,…,n ,n∈N represents, N represents the number of control points; Step 4.6: Combine the time series and position series of the above control points to obtain B = (Q, τ), where N represents the number of control points. Multi-objective optimization is performed in both the time series and the control point series using a weighted approach. Step 4.7: Through the cost function Minimize the angle change between path points and constrain the smoothness between path points, where i = 0, ..., n, n∈N, N represents the number of control points; Step 4.8: Through the cost function Minimize the total time of the path and keep the time uniformly distributed, where is the average time interval between path points, i = 0,…,n,n∈N, N represents the number of control points; Step 4.9: Distance cost function from the minimum path point to the obstacle Make sure the waypoints are away from obstacles, where d(x i ,o) is the path point X i The distance to the obstacle, i = 0, ..., n, n∈N, N represents the number of control points; Step 4.10: Through the cost function Keep the robot's velocity and acceleration on the path to satisfy its kinematic constraints, where i = 0, ..., n, n∈N, N represents the number of control points; Step 4.11: Exploitation Get the optimal solution of B, find the optimal path point location and time distribution, where B * represents the optimal result, f(B) is the total cost considering multiple objectives, γ k Represents the weight of the cost function of each optimization objective, k = 1, 2, 3, 4; Step 4.12: The robot body moves according to the path, speed and acceleration data determined above until the robot body moves to the target point calculated in step 3.

Images