Robot path planning control method based on map perception and construction

Abstract

The invention discloses a robot path planning control method based on map perception and construction, and relates to the technical field of mobile robots, and the method comprises the steps: collecting the information of the surrounding environment of a robot through a sensor carried on the robot; the robot performs map perception by performing preset simple mechanical motion; map construction is carried out, and the robot pose and the environment map are optimized through a Gauss-Newton method; comparing the current image with an image in a previously constructed map in real time to judge whether the robot returns to a previously visited position or not; converting the final environment map into a three-dimensional grid map; the sensor data is matched with the three-dimensional grid map, and the robot is positioned in real time; and performing global path planning by using an A * algorithm. Map perception is carried out through preset simple mechanical movement of the robot, and a safe and collision-free path is autonomously planned in a strange or complex environment.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robots, and in particular to a robot path planning and control method based on map perception and construction. Background Art

[0002] In the field of robotics, mobile robots have a wider range of applications and are more practical than fixed robots, and they occupy a very important position. Mobile robot systems include professional fields such as computers and communications, and have the ability to perceive their own information and move autonomously. They have been widely used in the military, catering and express delivery fields, and are meeting more human needs. For example, underwater robots explore the marine environment; service robots provide a variety of services to humans; medical robots are used in surgery to reduce surgical risks. It is a comprehensive research field with multiple aspects and technologies. However, from the development in recent years, it has been found that mobile robots have also had some defects. Due to the limitations of robot technology and the complex and changeable environment, mobile robots may encounter difficulties in completing tasks.

[0003] Mobile robot map construction and path planning technology are two important technologies of the robot autonomous mobile system. Map construction and path planning can ensure that when the robot is in an unfamiliar environment, it can use sensors to achieve self-positioning and autonomous movement while ensuring safety. The research and application of robots are receiving more and more attention. Autonomous mobile robots are one of the most common intelligent robots. They can best reflect their autonomy by solving the problem of "where am I" and are the prerequisite for achieving autonomy. When a mobile robot works in a certain environment, it must first accurately perceive the environment, then build a map, and navigate on this basis. How can a mobile robot autonomously plan a safe and collision-free path in an unfamiliar or complex environment and move to the end point? This is the goal of scientific researchers. Summary of the invention

[0004] In order to solve the above technical problems, a robot path planning and control method based on map perception and construction is provided. This technical solution solves the problems raised in the above background technology.

[0005] In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0006] A robot path planning control method based on map perception and construction, comprising:

[0007] The information of the robot's surrounding environment is collected through sensors carried by the robot, wherein the sensors include a binocular camera and a laser radar;

[0008] The robot perceives the map by performing preset simple mechanical movements, and the visual odometer uses the image feature matching method to calculate the robot's motion trajectory based on the image information of adjacent frames;

[0009] The back-end optimization end receives the camera posture information at different times from the visual odometer, builds the map, optimizes the robot posture and environment map through the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time;

[0010] Compare the current image with the image in the previously constructed map in real time to determine whether the robot has returned to the previously visited location. If so, modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made.

[0011] The robot completes the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map;

[0012] Match the sensor data with the three-dimensional grid map, locate the robot in real time, obtain the robot's real-time and accurate position, and determine the destination location in the three-dimensional grid map;

[0013] Use the A* algorithm to perform global path planning for the robot's real-time accurate position and destination location;

[0014] Perform path adjustment and obstacle avoidance in real time, and update the robot's position information in the three-dimensional grid map in real time.

[0015] Preferably, the collecting of information about the robot's surrounding environment by sensors carried by the robot specifically includes:

[0016] The binocular camera calculates the spatial information corresponding to the obstacle point based on the baseline, and the lidar uses the laser ranging method to emit and receive reflected light to obtain depth information;

[0017] When the laser radar is working, the internal laser emits a single line of laser, which is reflected when it reaches the surface of the obstacle. The reflected light is converged to the charge-coupled device through the lens. The laser, obstacle and lens form a triangle.

[0018] The distance between the laser and the obstacle is calculated based on the geometric properties of the triangle to obtain the depth information of the obstacle;

[0019] The distance between the laser and the obstacle is:

[0020]

[0021] Where d is the distance between the laser and the obstacle, a is the distance between the laser and the lens, f is the focal length of the lens, x is the spot length on the charge-coupled device, and α is the angle between the laser emission route and the vertical direction.

[0022] Preferably, the robot performs map perception by performing a preset simple mechanical movement, and the visual odometer calculates the robot's motion trajectory according to image information of adjacent frames using an image feature matching method, specifically including:

[0023] Detecting points with significant features such as corners or edges in the image information as feature points;

[0024] For each feature point, calculate the Euclidean distance between it and all feature points of the adjacent frame image information;

[0025] Output the feature point with the smallest Euclidean distance as the matching result of the feature point, and set the two feature points as the same obstacle;

[0026] Based on the distance changes between these two feature points and the laser, the robot's motion trajectory during this period is calculated;

[0027] The Euclidean distance is:

[0028]

[0029] In the formula, ρ ij is the Euclidean distance between the i-th feature point in the image information and the j-th feature point in the adjacent frame image information, (x i ,y i , z i ) is the spatial coordinate of the i-th feature point in the image information, (x j ,y j , z j ) is the spatial coordinate of the jth feature point in the adjacent frame image information.

[0030] Preferably, the back-end optimization end receives the camera posture information at different times from the visual odometer, constructs a map, optimizes the robot posture and the environment map by the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time. Specifically, the steps include:

[0031] S101: The back-end optimization end receives the camera posture information of the visual odometer at different times, and fuses the two sensor calculation data;

[0032] S102: Converting the camera posture information at different times into the robot posture information;

[0033] S103: constructing a preliminary map based on the changes of feature points in each frame of image information to obtain a simplified environment map;

[0034] S104: Convert the pose graph optimization problem into a nonlinear least squares problem;

[0035] S105: Calculate the residual under the current parameter vector;

[0036] S106: Calculate the Jacobian matrix of the prediction value function under the current parameter vector, where the Jacobian matrix is ​​an n*m matrix, where n is the number of observations, m is the dimension of the parameter vector, and the matrix elements are partial derivatives of the prediction value function with respect to the current parameter vector;

[0037] S107: Based on the residual and Jacobian matrix, construct the Gauss-Newton equation and calculate the parameter update amount;

[0038] S108: adding the parameter vector and the parameter update amount as the parameter vector for the next iteration;

[0039] S109: Determine whether the number of iterations reaches the preset number. If so, terminate the iteration and output the final parameter vector as the optimized posture information and environment map. If not, return to step S105.

[0040] The nonlinear least squares problem is:

[0041]

[0042] Where f(x) is the objective function, x i is the parameter vector composed of the pose information to be optimized and the environment map, y i is the observed value, g(x i ) is the predicted value function, n is the number of observations;

[0043] The residual is:

[0044] r i =y i -g(x i ),

[0045] In the formula, r i is the residual;

[0046] The Gauss-Newton equation is:

[0047]

[0048] In the formula, J i is the Jacobian matrix, Δx i is the parameter update amount.

[0049] Preferably, the real-time comparison of the current image with the image in the previously constructed map to determine whether the robot has returned to the previously visited location specifically includes:

[0050] extracting image features from each image;

[0051] Classify image features to build a visual dictionary;

[0052] Using a set of words from a visual dictionary to represent each image;

[0053] Determine whether the repetition ratio of the word set in the current image and each image in the previously constructed map is greater than the preset threshold. If so, output the position as a visited position and modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made.

[0054] Preferably, the robot ends the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map, specifically comprising:

[0055] The three-dimensional continuous space is divided into a cube grid, and the side length of the cube grid is the resolution;

[0056] During the simple mechanical motion of the robot, the cubic grid covered by the obstacle points is set to the occupied state;

[0057] There are no obstacles in the starting and ending areas of the scanning beam, and the cubic grid covered by the area is set to a non-occupied state;

[0058] The cube grid behind the obstacle and not covered by the scan point is set to unknown state;

[0059] Summarize the status of each cube grid in the final environment map and output it as a three-dimensional grid map.

[0060] Preferably, the use of the A* algorithm to perform global path planning on the robot's real-time accurate position and destination position specifically includes:

[0061] S201: Preset the movement cost of the robot moving in each direction;

[0062] S202: Setting the current position of the robot to a search completed state;

[0063] S203: Calculate the sum of the movement cost and the Euclidean distance of each path cube grid around the search completion state, and output the total cost required for each path cube grid;

[0064] S204: setting the path cube grid with the minimum total cost to a search completed state;

[0065] S205: Determine whether the destination position is in a search-completed state. If so, end the search and output the path cube grid connection between the robot's current position and the destination position in a search-completed state as the global shortest path. If not, return to step S203.

[0066] Preferably, the real-time path adjustment and obstacle avoidance, and real-time updating of the robot's position information in the three-dimensional grid map specifically include:

[0067] The robot moves according to the planned global shortest path;

[0068] While moving, the robot continuously acquires data about its surroundings through sensors;

[0069] Real-time path adjustment and obstacle avoidance;

[0070] Update the robot's position information in the three-dimensional grid map in real time.

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

[0072] The robot performs preset simple mechanical movements to perceive the map. It first accurately perceives the environment, then builds a map, and navigates on this basis to autonomously plan a safe and collision-free path in an unfamiliar or complex environment, obtain higher positioning accuracy and mapping accuracy, and move to the end point. It also integrates global path planning with local path planning, improving local obstacle avoidance capabilities while ensuring path optimality. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 A flow chart of a robot path planning control method based on map perception and construction according to the present invention;

[0074] Figure 2 A flow chart of the method for collecting information about the robot's surrounding environment of the present invention;

[0075] Figure 3 is a flow chart of the map perception method of the present invention;

[0076] Figure 4 A flow chart of the method for optimizing the robot posture and environment map of the present invention;

[0077] Figure 5 A flow chart of a method for determining whether a robot has returned to a previously visited location according to the present invention;

[0078] Figure 6 A flow chart of a method for converting a final environment map into a three-dimensional grid map according to the present invention;

[0079] Figure 7 A* algorithm method flow chart of the present invention;

[0080] Figure 8 This is a flow chart of the real-time path adjustment and obstacle avoidance method of the present invention. DETAILED DESCRIPTION

[0081] The following description is used to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are only examples, and those skilled in the art may think of other obvious variations.

[0082] Reference Figure 1 As shown, a robot path planning control method based on map perception and construction includes:

[0083] The information of the robot's surrounding environment is collected through sensors carried by the robot, wherein the sensors include a binocular camera and a laser radar;

[0084] The robot perceives the map by performing preset simple mechanical movements, and the visual odometer uses the image feature matching method to calculate the robot's motion trajectory based on the image information of adjacent frames;

[0085] The back-end optimization end receives the camera posture information at different times from the visual odometer, builds the map, optimizes the robot posture and environment map through the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time;

[0086] Compare the current image with the image in the previously constructed map in real time to determine whether the robot has returned to the previously visited location. If so, modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made.

[0087] The robot completes the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map;

[0088] Match the sensor data with the three-dimensional grid map, locate the robot in real time, obtain the robot's real-time and accurate position, and determine the destination location in the three-dimensional grid map;

[0089] Use the A* algorithm to perform global path planning for the robot's real-time accurate position and destination location;

[0090] Perform path adjustment and obstacle avoidance in real time, and update the robot's position information in the three-dimensional grid map in real time.

[0091] Reference Figure 2 As shown, the information about the robot's surrounding environment collected by the sensors on the robot specifically includes:

[0092] The binocular camera calculates the spatial information corresponding to the obstacle point based on the baseline, and the lidar uses the laser ranging method to emit and receive reflected light to obtain depth information;

[0093] When the laser radar is working, the internal laser emits a single line of laser, which is reflected when it reaches the surface of the obstacle. The reflected light is converged to the charge-coupled device through the lens. The laser, obstacle and lens form a triangle.

[0094] The distance between the laser and the obstacle is calculated based on the geometric properties of the triangle to obtain the depth information of the obstacle;

[0095] The distance between the laser and the obstacle is:

[0096]

[0097] Where d is the distance between the laser and the obstacle, a is the distance between the laser and the lens, f is the focal length of the lens, x is the spot length on the charge-coupled device, and α is the angle between the laser emission route and the vertical direction.

[0098] There are two ranging principles used by lidar: time-of-flight method and triangulation ranging method. The principle of time-of-flight method is to record the time interval taken for the laser to return from the transmitting end to the receiving end, and calculate the distance between the radar and the object being measured based on the speed of light. The method used in this article is triangulation ranging method.

[0099] Reference Figure 3 As shown in the figure, the robot perceives the map by performing a preset simple mechanical movement. The visual odometer uses the image feature matching method to calculate the robot's motion trajectory based on the image information of adjacent frames. Specifically, it includes:

[0100] Detecting points with significant features such as corners or edges in the image information as feature points;

[0101] For each feature point, calculate the Euclidean distance between it and all feature points of the adjacent frame image information;

[0102] Output the feature point with the smallest Euclidean distance as the matching result of the feature point, and set the two feature points as the same obstacle;

[0103] Based on the distance changes between these two feature points and the laser, the robot's motion trajectory during this period is calculated;

[0104] The Euclidean distance is:

[0105]

[0106] In the formula, ρ ij is the Euclidean distance between the i-th feature point in the image information and the j-th feature point in the adjacent frame image information, (x i ,y i , z i ) is the spatial coordinate of the i-th feature point in the image information, (xj ,y j , z j ) is the spatial coordinate of the jth feature point in the adjacent frame image information.

[0107] The brute force matching algorithm is the most direct and basic feature point matching method. For each feature point in the feature point set to be matched, it calculates the matching degree with all the feature points in another feature point set in turn (such as calculating the Euclidean distance or Hamming distance between descriptors), and finds the feature point pair with the highest matching degree as the matching result.

[0108] Reference Figure 4 As shown in the figure, the back-end optimization end receives the camera pose information at different times from the visual odometer, builds the map, optimizes the robot pose and environment map through the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time. Specifically, it includes:

[0109] S101: The back-end optimization end receives the camera posture information of the visual odometer at different times, and fuses the two sensor calculation data;

[0110] S102: Converting the camera posture information at different times into the robot posture information;

[0111] S103: constructing a preliminary map based on the changes of feature points in each frame of image information to obtain a simplified environment map;

[0112] S104: Convert the pose graph optimization problem into a nonlinear least squares problem;

[0113] S105: Calculate the residual under the current parameter vector;

[0114] S106: Calculate the Jacobian matrix of the prediction value function under the current parameter vector, where the Jacobian matrix is ​​an n*m matrix, where n is the number of observations, m is the dimension of the parameter vector, and the matrix elements are partial derivatives of the prediction value function with respect to the current parameter vector;

[0115] S107: Based on the residual and Jacobian matrix, construct the Gauss-Newton equation and calculate the parameter update amount;

[0116] S108: adding the parameter vector and the parameter update amount as the parameter vector for the next iteration;

[0117] S109: Determine whether the number of iterations reaches the preset number. If so, terminate the iteration and output the final parameter vector as the optimized posture information and environment map. If not, return to step S105.

[0118] The nonlinear least squares problem is:

[0119]

[0120] Where f(x) is the objective function, x i is the parameter vector composed of the pose information to be optimized and the environment map, y i is the observed value, g(x i ) is the predicted value function, n is the number of observations;

[0121] The residual is:

[0122] r i =y i -g(x i ),

[0123] In the formula, r i is the residual;

[0124] The Gauss-Newton equation is:

[0125]

[0126] In the formula, J i is the Jacobian matrix, Δx i is the parameter update amount.

[0127] The Gauss-Newton method is sensitive to the choice of initial points. Different initial points may lead to convergence to different local optimal solutions. The inverse matrix needs to be calculated in each iteration, which may lead to high computational complexity, especially when the dimension of the Jacobian matrix is ​​large. In some cases, the Gauss-Newton method may not converge or converge to an unstable solution. Therefore, in practical applications, it is necessary to carefully select parameters such as the iteration step size and convergence conditions to ensure the stability and convergence of the algorithm.

[0128] Reference Figure 5 As shown, real-time comparison of the current image with the image in the previously constructed map determines whether the robot has returned to the previously visited location, specifically including:

[0129] extracting image features from each image;

[0130] Classify image features to build a visual dictionary;

[0131] Using a set of words from a visual dictionary to represent each image;

[0132] Determine whether the repetition ratio of the word set in the current image and each image in the previously constructed map is greater than the preset threshold. If so, output the position as a visited position and modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made.

[0133] Loop detection is an indispensable part of the map construction algorithm. It is of great significance to improve the positioning accuracy of the robot and the accuracy of map construction. Through continuous loop detection and pose optimization, the long-term stability of the map construction system and the accuracy of the map can be improved.

[0134] Reference Figure 6 As shown, the robot ends the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map, which specifically includes:

[0135] The three-dimensional continuous space is divided into a cube grid, and the side length of the cube grid is the resolution;

[0136] During the simple mechanical motion of the robot, the cubic grid covered by the obstacle points is set to the occupied state;

[0137] There are no obstacles in the starting and ending areas of the scanning beam, and the cubic grid covered by the area is set to a non-occupied state;

[0138] The cube grid behind the obstacle and not covered by the scan point is set to unknown state;

[0139] Summarize the status of each cube grid in the final environment map and output it as a three-dimensional grid map.

[0140] Environmental modeling is to describe the state of the environment, that is, to build an environmental map. There are many types of environmental maps, such as feature maps, point cloud maps, geometric maps, raster maps, topological maps, etc. Visual map construction usually focuses on building feature maps and point cloud maps, while laser map construction focuses on building raster maps.

[0141] Reference Figure 7 As shown in the figure, the global path planning of the robot's real-time accurate position and destination position using the A* algorithm specifically includes:

[0142] S201: Preset the movement cost of the robot moving in each direction;

[0143] S202: Setting the current position of the robot to a search completed state;

[0144] S203: Calculate the sum of the movement cost and the Euclidean distance of each path cube grid around the search completion state, and output the total cost required for each path cube grid;

[0145] S204: setting the path cube grid with the minimum total cost to a search completed state;

[0146] S205: Determine whether the destination position is in a search-completed state. If so, end the search and output the path cube grid connection between the robot's current position and the destination position in a search-completed state as the global shortest path. If not, return to step S203.

[0147] Common forms of cost functions include Manhattan distance, diagonal distance, Euclidean distance, etc. When the grid map only allows movement in four directions (up, down, left, and right), Manhattan distance is selected; when the grid map allows movement in eight directions (up, down, left, right, upper left, lower left, upper right, and lower right), diagonal distance is selected; when the grid map allows movement in any direction, Euclidean distance is selected.

[0148] Reference Figure 8 As shown, real-time path adjustment and obstacle avoidance, and real-time updating of the robot's position information in the three-dimensional grid map specifically include:

[0149] The robot moves according to the planned global shortest path;

[0150] While moving, the robot continuously acquires data about its surroundings through sensors;

[0151] Real-time path adjustment and obstacle avoidance;

[0152] Update the robot's position information in the three-dimensional grid map in real time.

[0153] The integration of global path planning and local path planning improves the local obstacle avoidance capability while ensuring the optimality of the path.

[0154] Furthermore, the present solution also proposes a computer-readable storage medium on which a computer-readable program is stored. When the computer-readable program is called, the above-mentioned robot path planning and control method based on map perception and construction is executed.

[0155] It is understandable that the storage medium may be a magnetic medium, such as a floppy disk, a hard disk, or a magnetic tape; an optical medium, such as a DVD; or a semiconductor medium, such as a solid state drive (SSD).

[0156] To sum up, the advantages of the present invention are: map perception is performed by the robot performing preset simple mechanical movements, first accurately perceiving the environment, then building a map, and navigating on this basis, so as to realize autonomous planning of a safe and collision-free path in an unfamiliar or complex environment, obtain higher positioning accuracy and mapping accuracy, and move to the end point, and integrate global path planning with local path planning, thereby improving local obstacle avoidance capability while ensuring path optimality.

[0157] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions only describe the principles of the present invention. The present invention may be subject to various changes and improvements without departing from the spirit and scope of the present invention. These changes and improvements fall within the scope of the present invention. The scope of protection claimed by the present invention is defined by the attached claims and their equivalents.

Claims

1. A robot path planning and control method based on map perception and construction, characterized in that: include: The information of the robot's surrounding environment is collected through sensors carried by the robot, wherein the sensors include a binocular camera and a laser radar; The robot perceives the map by performing preset simple mechanical movements, and the visual odometer uses the image feature matching method to calculate the robot's motion trajectory based on the image information of adjacent frames; The back-end optimization end receives the camera posture information at different times from the visual odometer, builds the map, optimizes the robot posture and environment map through the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time; Compare the current image with the image in the previously constructed map in real time to determine whether the robot has returned to the previously visited location. If so, modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made. The robot completes the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map; Match the sensor data with the three-dimensional grid map, locate the robot in real time, obtain the robot's real-time and accurate position, and determine the destination location in the three-dimensional grid map; Use the A* algorithm to perform global path planning for the robot's real-time accurate position and destination location; Perform path adjustment and obstacle avoidance in real time, and update the robot's position information in the three-dimensional grid map in real time.

2. A robot path planning and control method based on map perception and construction according to claim 1, characterized in that: The information about the robot's surrounding environment collected by the sensors carried by the robot specifically includes: The binocular camera calculates the spatial information corresponding to the obstacle point based on the baseline, and the lidar uses the laser ranging method to emit and receive reflected light to obtain depth information; When the laser radar is working, the internal laser emits a single line of laser, which is reflected when it reaches the surface of the obstacle. The reflected light is converged to the charge-coupled device through the lens. The laser, obstacle and lens form a triangle. The distance between the laser and the obstacle is calculated based on the geometric properties of the triangle to obtain the depth information of the obstacle; The distance between the laser and the obstacle is: Where d is the distance between the laser and the obstacle, a is the distance between the laser and the lens, f is the focal length of the lens, x is the spot length on the charge-coupled device, and α is the angle between the laser emission route and the vertical direction.

3. A robot path planning and control method based on map perception and construction according to claim 2, characterized in that: The robot perceives the map by performing a preset simple mechanical movement, and the visual odometer calculates the robot's motion trajectory according to the image information of adjacent frames using an image feature matching method, specifically including: Detecting points with significant features such as corners or edges in the image information as feature points; For each feature point, calculate the Euclidean distance between it and all feature points of the adjacent frame image information; Output the feature point with the smallest Euclidean distance as the matching result of the feature point, and set the two feature points as the same obstacle; Based on the distance changes between these two feature points and the laser, the robot's motion trajectory during this period is calculated; The Euclidean distance is: In the formula, ρ ij is the Euclidean distance between the i-th feature point in the image information and the j-th feature point in the adjacent frame image information, (x i ,y i , z i ) is the spatial coordinate of the i-th feature point in the image information, (x j ,y j , z j ) is the spatial coordinate of the jth feature point in the adjacent frame image information.

4. A robot path planning and control method based on map perception and construction according to claim 3, characterized in that: The back-end optimization end receives the camera posture information at different times from the visual odometer, constructs a map, optimizes the robot posture and the environment map by the Gauss-Newton method, and updates the globally consistent robot trajectory and environment map in real time. Specifically, it includes: S101: The back-end optimization end receives the camera posture information of the visual odometer at different times, and fuses the two sensor calculation data; S102: Converting the camera posture information at different times into the robot posture information; S103: constructing a preliminary map based on the changes of feature points in each frame of image information to obtain a simplified environment map; S104: Convert the pose graph optimization problem into a nonlinear least squares problem; S105: Calculate the residual under the current parameter vector; S106: Calculate the Jacobian matrix of the prediction value function under the current parameter vector, where the Jacobian matrix is ​​an n*m matrix, where n is the number of observations, m is the dimension of the parameter vector, and the matrix elements are partial derivatives of the prediction value function with respect to the current parameter vector; S107: Based on the residual and Jacobian matrix, construct the Gauss-Newton equation and calculate the parameter update amount; S108: adding the parameter vector and the parameter update amount as the parameter vector for the next iteration; S109: Determine whether the number of iterations reaches the preset number. If so, terminate the iteration and output the final parameter vector as the optimized posture information and environment map. If not, return to step S105. The nonlinear least squares problem is: Where f(x) is the objective function, x i is the parameter vector composed of the pose information to be optimized and the environment map, y i is the observed value, g(x i ) is the predicted value function, n is the number of observations; The residual is: r i =y i -g(x i ), In the formula, r i is the residual; The Gauss-Newton equation is: In the formula, J i is the Jacobian matrix, Δx i is the parameter update amount.

5. A robot path planning and control method based on map perception and construction according to claim 4, characterized in that: The real-time comparison of the current image with the image in the previously constructed map to determine whether the robot has returned to the previously visited location specifically includes: extracting image features from each image; Classify image features to build a visual dictionary; Using a set of words from a visual dictionary to represent each image; Determine whether the repetition ratio of the word set in the current image and each image in the previously constructed map is greater than the preset threshold. If so, output the position as a visited position and modify the robot's preset simple mechanical motion parameters to prevent the robot from entering a loop motion. If not, no output is made.

6. A robot path planning and control method based on map perception and construction according to claim 5, characterized in that: The robot ends the preset simple mechanical movement and converts the final environment map into a three-dimensional grid map, specifically including: The three-dimensional continuous space is divided into a cube grid, and the side length of the cube grid is the resolution; During the simple mechanical motion of the robot, the cubic grid covered by the obstacle points is set to the occupied state; There are no obstacles in the starting and ending areas of the scanning beam, and the cubic grid covered by the area is set to a non-occupied state; The cube grid behind the obstacle and not covered by the scan point is set to unknown state; Summarize the status of each cube grid in the final environment map and output it as a three-dimensional grid map.

7. A robot path planning and control method based on map perception and construction according to claim 6, characterized in that: The use of the A* algorithm to perform global path planning for the robot's real-time accurate position and destination position specifically includes: S201: Preset the movement cost of the robot moving in each direction; S202: Setting the current position of the robot to a search completed state; S203: Calculate the sum of the movement cost and the Euclidean distance of each path cube grid around the search completion state, and output the total cost required for each path cube grid; S204: setting the path cube grid with the minimum total cost to a search completed state; S205: Determine whether the destination position is in a search-completed state. If so, end the search and output the path cube grid connection between the robot's current position and the destination position in a search-completed state as the global shortest path. If not, return to step S203.

8. A robot path planning and control method based on map perception and construction according to claim 7, characterized in that: The real-time path adjustment and obstacle avoidance and real-time updating of the position information of the robot in the three-dimensional grid map specifically include: The robot moves according to the planned global shortest path; While moving, the robot continuously acquires data about its surroundings through sensors; Real-time path adjustment and obstacle avoidance; Update the robot's position information in the three-dimensional grid map in real time.

Images