Cleaning robot environment obstacle marking method and system, equipment and medium

Abstract

The invention provides a cleaning robot environment obstacle marking method and system, equipment and a medium, and relates to the technical field of information, and the method comprises the steps: obtaining sensor data which comprises the position information of an obstacle; for sensor data, marking and clearing obstacle information in the map by adopting a mode of detection point marking, detection line clearing, blind area timeout clearing and vision field timeout clearing to obtain an obstacle grid map corresponding to a sensor; according to the sequence of priority of the obstacles, the second order of the removed areas and the third order of the unknown areas, the obstacle grid maps corresponding to the sensors are merged and compressed, and a comprehensive obstacle grid map is obtained; expansion processing is carried out, the obstacle points are expanded outwards by a preset grid number, exponential attenuation is carried out, and a cost map of the robot is obtained; and the robot autonomously navigates to the target position according to the global path and the local trajectory. The robot can flexibly deal with obstacles in a complex environment, and efficient autonomous navigation is achieved.

Description

Technical Field

[0001] The present invention relates to the field of information technology, and in particular to a method and system, equipment and medium for marking environmental obstacles of a cleaning robot. Background Art

[0002] The core technical challenge facing robot autonomous navigation is how to achieve efficient and accurate path planning and obstacle avoidance in complex and changing environments. Traditional methods often find it difficult to balance navigation efficiency and accuracy, especially in dynamic scenes. Specifically, the first is how to fuse multi-source sensor data to build an accurate environmental map. Sensor data has problems such as noise and blind spots, which directly affect the quality of the map. Secondly, how to compress map data while retaining key information to reduce storage and computing burdens. Furthermore, there is a contradiction between global path planning and local trajectory control. Global planning pursues efficiency but lacks accuracy, while local control has high accuracy but is difficult to grasp the overall situation. In addition, in a dynamic environment, the robot also needs to be able to perceive surrounding changes in real time and respond quickly, which requires the navigation algorithm to have good real-time and robustness. How to balance these conflicting requirements and design an autonomous navigation method that can adapt to various complex scenes and has both efficiency and accuracy is a key issue that needs to be solved urgently. Summary of the invention

[0003] The present invention provides a method for marking environmental obstacles of a cleaning robot, comprising:

[0004] Pass the low-resolution costmap to the global path planner and the high-resolution costmap of the area of ​​interest to the local trajectory controller;

[0005] The global path planner plans the global path based on the low-resolution cost map, and the local trajectory controller plans the local trajectory based on the high-resolution cost map and the global path;

[0006] The robot's obstacle avoidance control is achieved based on the global path and local trajectory.

[0007] The present invention provides a cleaning robot environment obstacle marking system, which mainly includes:

[0008] A planning module is used to pass the low-resolution cost map to the global path planner and pass the high-resolution cost map of the area of ​​interest to the local trajectory controller; the global path planner plans the global path according to the low-resolution cost map, and the local trajectory controller plans the local trajectory according to the high-resolution cost map and the global path;

[0009] The obstacle avoidance control module is used to implement obstacle avoidance control of the robot based on the global path and the local trajectory.

[0010] The present invention provides an electronic device, mainly comprising: one or more processors, and a memory; the processor and the memory are coupled; the memory is used to store computer program codes, the computer program codes including computer instructions;

[0011] When the processor executes the computer instructions, the electronic device executes the above-mentioned cleaning robot environment obstacle marking method.

[0012] The present invention provides a computer-readable storage medium, which stores computer instructions or programs. When the computer instructions or programs are run on a computer, the computer executes the above-mentioned cleaning robot environment obstacle marking method.

[0013] The technical solution provided by the embodiment of the present invention may have the following beneficial effects:

[0014] The present invention discloses a method and system, equipment and medium for marking obstacles in the environment of a cleaning robot. The method constructs an accurate obstacle grid map and cost map by fusing multiple sensor data, thereby realizing efficient navigation of the robot. First, the present invention adopts innovative data processing technologies, such as detection point marking and blind spot timeout clearing, to effectively improve the accuracy of the map. Secondly, a comprehensive obstacle grid map is generated through a priority merging and compression algorithm, which not only retains key information but also reduces data redundancy. Finally, the present invention adopts a hierarchical planning strategy, using low-resolution cost maps for global path planning and high-resolution cost maps for local trajectory control, which cleverly balances computational efficiency and navigation accuracy. This multi-level planning method enables the robot to flexibly respond to obstacles in complex environments and realize safe and efficient autonomous navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 The figure is a flow chart of the method for marking environmental obstacles of the cleaning robot of the present invention. DETAILED DESCRIPTION

[0016] In order to further understand the content of the present invention, the present invention is described in detail in conjunction with the accompanying drawings and embodiments. The present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It is understood that the specific embodiments described herein are only used to explain the relevant inventions, rather than to limit the invention. It is also necessary to explain that, for ease of description, only the parts related to the invention are shown in the accompanying drawings.

[0017] like Figure 1 The method for marking environmental obstacles of the cleaning robot in this embodiment may specifically include:

[0018] Pass the low-resolution costmap to the global path planner and the high-resolution costmap of the area of ​​interest to the local trajectory controller;

[0019] The global path planner plans the global path based on the low-resolution cost map, and the local trajectory controller plans the local trajectory based on the high-resolution cost map and the global path;

[0020] The robot's obstacle avoidance control is achieved based on the global path and local trajectory.

[0021] Among them, before obtaining a low-resolution cost map and / or a high-resolution cost map, it includes: obtaining sensor data collected by a depth camera, a lidar, a line structured light and an ultrasonic sensor, and the sensor data includes the location information of obstacles.

[0022] Specifically, sensor data collected by depth cameras, laser radars, line structured light and ultrasonic sensors are obtained, and the sensor data includes the location information of obstacles. According to the obtained sensor data, the location information of obstacles is preprocessed, including data format conversion, coordinate system unification, etc. A multi-sensor data fusion algorithm is used to fuse the obstacle location information of different sensors after preprocessing to obtain unified obstacle location information. Through the obstacle recognition algorithm, the fused obstacle location information is analyzed to identify different obstacle types, such as pedestrians, vehicles, buildings, etc. According to the identified obstacle type, different obstacle tracking algorithms are used to track the movement trajectory of the obstacle in real time. The self-position information is obtained, and the relative position relationship between the obstacle and the self is determined by combining the position and movement trajectory information of the obstacle. If the relative position relationship indicates that there is a collision risk, the collision warning function is triggered and a warning signal is sent to the upper system. According to the preset safety distance threshold, the self-motion strategy is dynamically adjusted to generate an obstacle avoidance path. The generated obstacle avoidance path is passed to the motion control unit, and the motion parameters are adjusted to guide the intelligent body to safely avoid obstacles.

[0023] For example, the intelligent agent collects obstacle position information through depth cameras, 16-line laser radars, line structured light and ultrasonic sensors, and performs preprocessing such as format conversion and coordinate system unification on different sensor data. The Kalman filter multi-sensor fusion algorithm is used to fuse the preprocessed obstacle position information to obtain a unified obstacle position. The PointNet++ point cloud segmentation algorithm is used to identify the obstacle type, and the Hungarian algorithm is used to track the obstacle movement trajectory. The self-position of GPS / IMU fusion positioning is obtained, and the relative position relationship between the obstacle and the self is analyzed. If the relative distance is less than 3 meters, a collision warning signal is sent to the planning module. According to the preset safety distance threshold of 5 meters, the artificial potential field method is used to dynamically plan the obstacle avoidance path to guide the intelligent agent to safely avoid obstacles. Taking into account the obstacle avoidance path length, curvature and vehicle dynamics constraints, the Hybrid A* algorithm is used to solve the optimal obstacle avoidance path, and the path point set is transmitted to the motion control unit at a time interval of 0.05s. The vehicle speed and steering wheel angle are adjusted to make the intelligent agent drive smoothly along the obstacle avoidance path.

[0024] In one embodiment, after obtaining sensor data, obstacle information in the map is marked and cleared by marking detection points, clearing detection lines, clearing blind spots after timeout, and clearing visual field after timeout, so as to obtain obstacle grid maps corresponding to the four sensors. Specifically, the location information of obstacles in the sensor data is obtained; according to the location information, detection points are marked at corresponding locations on the map, and detection lines are determined according to the detection points; it is determined whether there are detection points within a preset range on both sides of the detection line, and if not, the detection lines are cleared; it is determined whether obstacles have not been detected in the blind spots in the map for a long time, and if so, the obstacle information in the blind spots is cleared; it is determined whether the obstacle information in the area within the visual field of the sensor in the map has not been updated for a long time, and if so, the obstacle information in the area is cleared.

[0025] In a specific implementation, four types of sensor data are obtained, and the original obstacle information of the sensor is obtained through data preprocessing and fusion. According to the characteristics of the sensor data, the detection point marking method is used to preliminarily mark the obstacles to obtain the detection point marking obstacle map. On the basis of the detection point marking obstacle map, the detection line clearing method is used to remove the noise and false detection in the sensor data to obtain the obstacle map after the detection line is cleared. For the sensor blind area, the blind area timeout clearing method is used to determine whether the obstacle disappears. If the obstacle is not detected for a long time, it is cleared from the map to obtain the obstacle map after the blind area timeout clearing. Combined with the sensor field of view range, the field of view timeout clearing method is used to determine whether the obstacle leaves the sensor field of view. If the obstacle exceeds the sensor detection range, it is cleared from the map to obtain the obstacle map after the field of view timeout clearing. The obstacle maps after the detection point marking, detection line clearing, blind area timeout clearing and field of view timeout clearing are fused to obtain the comprehensive obstacle grid map corresponding to the four sensors. Post-process the comprehensive obstacle grid map, optimize the obstacle boundary and shape through filtering, clustering and other algorithms, and obtain a refined obstacle grid map. According to the obstacle grid map, extract the location, size, shape and other attribute information of the obstacle, and build a dynamic obstacle database to facilitate subsequent path planning and decision-making. Update sensor data in real time, dynamically update the obstacle grid map and database, ensure that the map information is consistent with the actual state of the environment, and provide reliable environmental perception support for autonomous navigation.

[0026] Exemplarily, the raw data of the laser radar, millimeter-wave radar, ultrasonic radar and visual sensor are first obtained. After preprocessing such as coordinate conversion and time synchronization, the obstacle information detected by different sensors is fused to obtain the original obstacle information in a unified coordinate system. Then, according to the detection characteristics of each sensor, the environment is modeled using a grid map, and the resolution of the grid is set to 0.2 meters. For each grid, the number of sensor detection points in the grid is counted. If it exceeds a threshold of 10, the grid is marked as occupied, and a preliminary obstacle map marked with detection points is obtained. Next, based on the detection point marking map, the sensor's detection line is analyzed. For the grid passed by the detection line, if its occupancy probability is lower than 0.2, it is cleared.

[0027] Remove noise interference and obtain the obstacle map after the detection line is cleared. At the same time, considering the influence of the sensor blind area, set the blind area timeout time to 5 seconds. If the grid where an obstacle is located is not detected by any sensor for 5 consecutive seconds, it is considered that the obstacle may have disappeared and it will be removed from the map. In addition, it is necessary to combine the field of view of the sensor and set the field of view timeout time to 3 seconds. If an obstacle exceeds the detection range of all sensors and is not detected for 3 consecutive seconds, it will also be removed. Finally, the obstacle maps obtained by the four sensors after detection point marking, detection line clearing, blind area timeout clearing and field of view timeout clearing are fused to obtain a comprehensive obstacle grid map. The map is morphologically filtered to smooth the obstacle boundary, and the DBSCAN clustering algorithm is used to extract independent obstacles to obtain a refined obstacle grid map. The location, length, width, outline and other attributes of each obstacle are extracted from the map and stored in the dynamic obstacle database. The sensor data and obstacle map are updated every 0.1 seconds, and the newly detected obstacle information is written into the database. The disappeared obstacles are deleted from the database to ensure that the map, database and environmental status are synchronized in real time, providing dynamic and reliable environmental perception for autonomous navigation.

[0028] In one embodiment, after obtaining the obstacle grid maps corresponding to the four sensors, a comprehensive obstacle grid map is established based on the obstacle grid maps corresponding to the four sensors, including: merging and compressing the obstacle grid maps corresponding to the four sensors in the order of obstacle priority, cleared area second, and unknown area third, to obtain a comprehensive obstacle grid map.

[0029] Specifically, according to the principle of obstacle priority, the obstacle grid maps corresponding to the four sensors are obtained. If there are obstacles, they are marked as obstacles in the comprehensive obstacle grid map. According to the principle of clear area second, the clear area grid maps corresponding to the four sensors are obtained. If a grid is marked as a clear area in all sensor maps, it is marked as a clear area in the comprehensive obstacle grid map. According to the principle of unknown area second, the unknown area grid maps corresponding to the four sensors are obtained. If a grid is marked as an unknown area in any sensor map, it is marked as an unknown area in the comprehensive obstacle grid map. According to the distribution of obstacles, clear areas and unknown areas in the comprehensive obstacle grid map, the map is compressed by using an image compression algorithm to obtain a compressed comprehensive obstacle grid map. By analyzing the compressed comprehensive obstacle grid map, the proportion of obstacles, clear areas and unknown areas in the map is obtained to determine the passability of the map. According to the compressed comprehensive obstacle grid map, a path planning algorithm is used to obtain a feasible path from the starting point to the target point. By analyzing the distribution of obstacles on the feasible path, the safety of the path is judged. If the density of obstacles on the path exceeds the threshold, the path is replanned. According to the path planning results, the robot motion control instructions are obtained to control the robot to move along the planned path and update the robot position information in real time. The robot sensor obtains environmental information in real time to determine whether new obstacles appear or clear areas. If so, the comprehensive obstacle grid map is updated and the path is replanned.

[0030] Exemplarily, according to the principle of obstacle priority, the obstacle grid maps corresponding to the four sensors of laser radar, vision, ultrasound and infrared are obtained. For example, if the laser radar detects a wall 5 meters ahead, the area is marked as an obstacle in the corresponding grid map. If the four sensor maps all detect obstacles in a certain grid, the grid is marked as an obstacle in the comprehensive obstacle grid map. The clear area grid maps corresponding to the four sensors are obtained according to the principle of clear area second. If a grid is marked as a clear area in all sensor maps, for example, the visual sensor does not detect any obstacles within 2 meters, the grid is marked as a clear area in the comprehensive obstacle grid map. The unknown area grid maps corresponding to the four sensors are obtained according to the principle of unknown area second. If a grid is marked as an unknown area in any sensor map, such as the ultrasonic sensor has a limited detection range and the area beyond the range is marked as unknown, it is marked as an unknown area in the comprehensive obstacle grid map. According to the distribution of obstacles, cleared areas and unknown areas in the comprehensive obstacle grid map, the quadtree image compression algorithm is used to compress the map, and the compression threshold is set to 10%, that is, when the area of ​​similar areas exceeds 10%, they are merged to obtain the compressed comprehensive obstacle grid map. By analyzing the compressed map, the proportions of obstacles, cleared areas and unknown areas are 20%, 50% and 30% respectively, and the cleared area exceeds 50%, which judges that the overall passability of the map is good. According to the compressed map, the A* path planning algorithm is used to obtain the feasible path from the starting point to the target point, and the calculated path length is 15 meters. By analyzing the distribution of obstacles on the path, the obstacle density is 1 / meter, which is less than the safety threshold of 2 / meter. It is judged that the path safety is high and there is no need to re-plan the path. According to the path planning results, the path is discretized into a series of waypoints, and a waypoint is set every 1 meter. The robot motion control instructions are generated to control the robot to move along the waypoints at a speed of 0.5 meters per second, and the robot position is updated in real time through the odometer. The robot sensors acquire environmental information in real time at a frequency of 10 Hz. When the lidar detects a new obstacle at a distance of less than 1 meter, or the visual sensor finds a new clearance area with an area greater than 1 square meter, the comprehensive obstacle grid map is updated and the path planning is re-performed to ensure the safety and real-time performance of the robot's movement.

[0031] In one embodiment, after obtaining the comprehensive obstacle grid map, the obstacle points in the comprehensive obstacle grid map are expanded, the obstacle points are expanded outward by a preset number of grids, and exponential decay is performed to obtain a cost map of the robot.

[0032] Specifically, according to the comprehensive obstacle grid map, the coordinate information of the obstacle point is obtained and stored in the obstacle point list. The obstacle point list is traversed, and each obstacle point is expanded, and the obstacle point is expanded outward by a preset number of grids to obtain the expanded obstacle area. The exponential decay function is used to calculate the cost value of each grid in the expanded obstacle area, and the cost value decays exponentially as the distance from the obstacle point increases. The calculated cost value is assigned to the corresponding grid to form a preliminary cost map. It is determined whether there is an area with too high cost value in the preliminary cost map. If so, the area is further smoothed to reduce the gradient change of the cost value. According to the smoothed cost map, combined with the size and motion constraints of the robot, the cost map is corrected to ensure that the robot can pass safely. By normalizing the corrected cost map, the cost value is mapped to the [0, 1] interval to obtain the final robot cost map. The generated robot cost map is merged with the original comprehensive obstacle grid map to update the cost information in the comprehensive obstacle grid map. The updated comprehensive obstacle grid map is passed to the robot's path planning module as input for path planning, guiding the robot to navigate safely and efficiently in the environment.

[0033] Exemplarily, by analyzing the comprehensive obstacle grid map, the coordinate information of the obstacle point is extracted and stored in a list. Then, each obstacle point is expanded, and 3 grid units are expanded outward to form an expanded obstacle area. The exponential decay function f(x) = e^(-x) is used to calculate the cost value of each grid in the expansion area. The farther away from the obstacle point, the lower the cost value. The cost value is assigned to the corresponding grid to generate a preliminary cost map. It is determined whether the cost value in the preliminary cost map exceeds 0.8, and the area is Gaussian smoothed. A 3x3 convolution kernel is used with a standard deviation of 1.5 to reduce the gradient change of the cost value. Considering that the robot radius is 0.3 meters, the cost value of the area less than 0.3 meters in the cost map is set to 1 to ensure that the robot passes safely. The cost map is normalized to the maximum and minimum values, and the cost value is mapped to the interval [0,1]. Finally, the generated robot cost map is fused with the original comprehensive obstacle grid map, and the cost value of the grid is updated as the input of path planning to guide the robot to navigate safely and efficiently.

[0034] In a specific implementation, after the cost map is obtained, the low-resolution cost map is passed to the global path planner, and the high-resolution cost map of the region of interest is passed to the local trajectory controller.

[0035] Specifically, a low-resolution cost map is initialized according to a static map, and the map is divided into grids using a rasterization method, and each grid is assigned a corresponding cost value. The information of the custom region of interest is obtained through the perception module, the location and range of the region of interest are determined, and the boundary coordinates of the region of interest are obtained. According to the boundary coordinates of the region of interest, the corresponding area is extracted from the low-resolution cost map to generate a high-resolution cost map of the region. The high-resolution cost map is optimized using an interpolation algorithm to improve the accuracy and smoothness of the map and determine the exact cost value of each grid. The optimized low-resolution cost map is passed to the global path planner, and the planner generates a global path based on the cost map and target point information. The high-resolution cost map of the region of interest is passed to the local trajectory controller, and the controller generates a local trajectory based on the high-precision map and vehicle status. The local trajectory controller optimizes the local trajectory based on obstacle information and vehicle dynamics constraints to obtain smooth and feasible local control instructions. If the local trajectory deviates significantly from the global path, the global path replanning is triggered, and the global path is regenerated on the low-resolution cost map based on the current vehicle position and target point. The local control instructions are passed to the execution module to control the vehicle to drive according to the local trajectory, while monitoring the vehicle status and environmental information to achieve autonomous navigation of the vehicle.

[0036] For example, the static map can be divided into 10cm×10cm grids by rasterization method, and each grid is assigned a cost value of 0-255. Obstacle information within 30m ahead is obtained through sensors such as lidar to determine the boundary coordinates of the area of ​​interest. The system extracts the area from the low-resolution cost map, increases the resolution to 2cm, smoothes the map using the cubic spline interpolation algorithm, and increases the corresponding grid cost value by 20% according to the obstacle position. The global path planner uses the A* algorithm, takes the low-resolution cost map as input, the current position of the vehicle as the starting point, and the target position as the end point to search for the path with the lowest cost. The local trajectory controller takes the high-resolution map as input, takes the preview point of the vehicle within the next 3s as the target, and uses the polynomial curve fitting method to plan a smooth driving trajectory, taking into account the vehicle's minimum turning radius, maximum speed, comfort and other constraints. If it is detected that the local trajectory deviates from the global path by more than 0.5m, the global path replanning is triggered. Finally, the planned local trajectory is discretized into 0.1s control instructions and passed to the chassis for execution. At the same time, the controller monitors the vehicle position and obstacle information at a frequency of 50Hz to achieve real-time obstacle avoidance and trajectory tracking.

[0037] In another specific implementation, based on the static map information, a low-resolution cost map is initialized and generated, and the map is divided into grids, and each grid is assigned a corresponding cost value. The generated low-resolution cost map is passed to the global path planner as the input data of the planner. In the global path planner, the Dijkstra algorithm is used to search the low-resolution cost map to find the optimal path from the starting point to the target point. The Dijkstra algorithm selects the node with the smallest cost as the next expansion node by comparing the cost from the current node to the starting point and the cost from the neighboring node to the starting point. If the current expansion node is the target node, the algorithm terminates and the optimal path from the starting point to the target point is obtained; otherwise, the next node with the smallest cost is continued to be expanded. The optimal path obtained by the Dijkstra algorithm is used as the output result of the global path planner to guide the global navigation of the robot. At the same time, according to the custom area of ​​interest, a high-resolution map in the area is generated to provide more detailed environmental information. The generated high-resolution map is passed to the local controller for fine navigation control of the robot in the local area. The local controller adjusts the movement of the robot in real time based on the high-resolution map and combined with the sensor data to avoid obstacles and reach the target position.

[0038] Exemplarily, according to the static map information, the map is divided into 0.5m × 0.5m grids, the cost value of the obstacle grid is set to 1000, and the cost value of the free space is set to 10. The Dijkstra algorithm is used to search and generate a low-resolution cost map. By comparing the cost from the current node to the starting point and the neighboring node, for example, the cost from the current node to the starting point is 100, and the cost from the neighboring node to the starting point is 150, the current node is selected as the next expansion node. After searching the target point, the global optimal path is obtained to guide the robot navigation. At the same time, a 0.1m × 0.1m high-resolution map is generated for the custom 10m × 10m area of ​​interest, which is passed to the local controller for fine obstacle avoidance. The controller combines the data of sensors such as lidar to adjust the robot speed and direction in real time to reach the target position safely and efficiently in the local area.

[0039] The local trajectory controller uses the dynamic window method to plan the local trajectory according to the high-resolution cost map and the global path, including establishing a high-resolution dynamic local use map based on the low-resolution dynamic initial map and obtaining a high-resolution cost map. The global path is planned on the low-resolution dynamic initial map through the global path planning algorithm. The global path is projected onto the high-resolution dynamic local use map to obtain the local path. According to the high-resolution cost map and the local path, the dynamic window method is used to search the trajectory on the local map. Within the dynamic window, the cost of each sampled trajectory is evaluated to calculate the navigation cost, speed cost and distance cost of the trajectory. Through the cost function, the optimal local trajectory is determined by comprehensively considering various costs. If the local trajectory meets the constraints, it is used as the output of the local controller to control the robot to move along the trajectory. If the local trajectory does not meet the constraints, it returns to search the local trajectory again. As the robot moves, the high-resolution dynamic local use map and the local path are continuously updated, and the steps are repeated to achieve real-time local trajectory planning and control.

[0040] Exemplarily, based on a low-resolution 10-meter dynamic initial map, a high-resolution 1-meter dynamic local usage map is established by bilinear interpolation to obtain a high-resolution cost map. The A* algorithm is used to plan a global path on the low-resolution dynamic initial map, and the global path is projected onto the high-resolution dynamic local usage map to obtain a local path. According to the high-resolution cost map and the local path, the cost of each sampled trajectory is evaluated in a dynamic window of 5 meters x 5 meters, including the navigation cost of the trajectory (such as obstacle distance), speed cost (such as speed change smoothness) and distance target point cost (such as the distance between the end point of the trajectory and the target point). Through the weighted sum of the cost function J = w1*J navigation + w2*J speed + w3*J distance, the various costs are comprehensively considered to determine the optimal local trajectory. If the local trajectory meets the constraints such as the maximum speed of 1m / s and the maximum acceleration of 0.5m / s^2, it is used as the output of the local controller to control the robot to move along the trajectory. If the constraints are not met, the local trajectory is searched again in the dynamic window. As the robot moves, it updates the high-resolution dynamic local usage map and local path within the range of 10m x 10m at a frequency of 10Hz to achieve real-time local trajectory planning and control. By continuously sensing environmental changes and integrating them into the map, while quickly responding to changes in local trajectories, the robot can flexibly respond to dynamic unknown environments and reach the target point safely and efficiently.

[0041] The obstacle avoidance control of the robot is realized based on the global path and the local trajectory, including the robot autonomously navigating to the target position according to the global path and the local trajectory.

[0042] Specifically, the obstacle points in the obstacle grid map are expanded to obtain the expanded obstacle grid map. According to the expanded obstacle grid map, the cost map of the robot is obtained by exponential decay. Through the cost map, the global path of the robot is planned to determine the approximate direction of the robot's navigation. According to the current position and target position of the robot, combined with the global path, the local trajectory of the robot is generated. The perception information of the robot is integrated to determine whether there are dynamic obstacles in the local trajectory. If so, the local trajectory is replanned. According to the planned local trajectory, the robot is controlled to move along the trajectory. Through sensors such as odometers and vision, the real-time position and posture information of the robot is obtained to determine whether it deviates from the trajectory. If the robot deviates from the trajectory, the local trajectory is adjusted according to the degree of deviation and the cost map to make the robot return to the correct trajectory. The above process is repeated until the robot reaches the target position and the autonomous navigation process is completed.

[0043] Exemplarily, when the obstacle grid map is expanded, the obstacle point can be expanded using a 3x3 structural element, that is, the 8 neighborhoods of each obstacle point are marked and set as obstacles. In this way, a certain safety distance can be formed around the obstacle. According to the expanded obstacle map, the exponential decay function f(d) = e^(-d / d0) can be used to generate a cost map, where d represents the distance to the nearest obstacle, and d0 is the decay constant, which can be 2-3 times the grid size. On the cost map, the A* algorithm is used to search for the optimal path from the starting point to the target point as the global path. According to the current position, the target position and the global path, a local trajectory can be generated within 5 meters in front of the robot. The obstacle information within 10 meters in front is sensed by sensors such as laser radar to determine whether the local trajectory has a collision risk. If so, the local trajectory can be adjusted online using methods such as the dynamic window method. According to the position and speed information of the sampling points on the trajectory, the speed and steering of the robot are controlled using kinematic and dynamic models. Real-time positioning is performed through sensors such as encoders and IMUs, and the distance and angle of the robot's deviation from the trajectory are calculated. If the deviation exceeds 10cm or 5°, the trajectory adjustment is triggered, and the DWA algorithm is used to perform a local search on the cost map to generate a path back to the original trajectory. By continuously executing the above perception, decision-making, planning, and control steps, the robot can achieve autonomous navigation in a dynamic environment.

[0044] The present invention provides a cleaning robot environment obstacle marking system, which mainly includes:

[0045] A planning module is used to pass the low-resolution cost map to the global path planner and pass the high-resolution cost map of the area of ​​interest to the local trajectory controller; the global path planner plans the global path according to the low-resolution cost map, and the local trajectory controller plans the local trajectory according to the high-resolution cost map and the global path;

[0046] The obstacle avoidance control module is used to implement obstacle avoidance control of the robot based on the global path and the local trajectory.

[0047] The present invention provides an electronic device, mainly comprising: one or more processors, and a memory; the processor and the memory are coupled; the memory is used to store computer program codes, the computer program codes including computer instructions;

[0048] When the processor executes the computer instructions, the electronic device executes the above-mentioned cleaning robot environment obstacle marking method.

[0049] The present invention provides a computer-readable storage medium, which stores computer instructions or programs. When the computer instructions or programs are run on a computer, the computer executes the above-mentioned cleaning robot environment obstacle marking method.

[0050] Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present application without departing from the spirit and scope of the embodiments of the present application. Thus, if these modifications and variations of the embodiments of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A method for marking obstacles in a cleaning robot environment, characterized in that: include: Pass the low-resolution costmap to the global path planner and the high-resolution costmap of the area of ​​interest to the local trajectory controller; The global path planner plans a global path according to the low-resolution cost map, and the local trajectory controller plans a local trajectory according to the high-resolution cost map and the global path; The obstacle avoidance control of the robot is achieved based on the global path and the local trajectory.

2. The method according to claim 1, characterized in that Before obtaining a low-resolution costmap and / or high-resolution costmap, include: Obtain the location information of obstacles in sensor data; According to the position information, a detection point is marked at a corresponding position on the map, and a detection line is determined according to the detection point; Determine whether there are detection points within a preset range on both sides of the detection line, and if not, clear the detection line; Determine whether an obstacle has not been detected in the blind area in the map for a long time. If so, clear the obstacle information in the blind area. Determine whether the obstacle information in the area within the sensor's field of view in the map has not been updated for a long time. If so, clear the obstacle information in the area.

3. The method according to claim 1, characterized in that Before obtaining a low-resolution costmap and / or high-resolution costmap, include: Obtain obstacle grid maps corresponding to depth cameras, lidars, line lasers, and ultrasonic sensors; Determine the obstacle information of each grid in each obstacle grid map; If obstacles exist in the same grid of all four maps, the obstacles are marked in the corresponding grid of the comprehensive obstacle grid map; If there are no obstacles in the same grid of the four maps, the corresponding grid of the comprehensive obstacle grid map is marked as a clear area; In other cases, the corresponding grids in the comprehensive obstacle grid map are marked as unknown areas.

4. The method according to claim 3, characterized in that After determining the obstacle information of each grid in each obstacle grid map, including: Obtaining the position of each obstacle point in the comprehensive obstacle grid map; Taking each obstacle point as the center, expand outward by a preset number of grids, assign exponential decay to the grid cost values ​​in the expanded area, and obtain a cost map.

5. The method according to claim 1, characterized in that The low-resolution cost map is passed to the global path planner, and the high-resolution cost map of the region of interest is passed to the local trajectory controller, including: Downsample the cost map to obtain a low-resolution cost map; Pass the low-resolution costmap to the global path planner; Determine the area within a preset range around the robot as the area of ​​interest; Obtaining a cost map within the region of interest as a high-resolution cost map; The high-resolution costmap is passed to the local trajectory controller.

6. The method according to claim 1, characterized in that The global path planner plans a global path according to the low-resolution cost map, including: Get a low-resolution costmap; The Dijkstra algorithm is used to search the low-resolution cost map to find the path with the minimum cost from the starting point to the target point as the global path.

7. The method according to claim 1, characterized in that The local trajectory controller plans the local trajectory according to the high-resolution cost map and the global path, including: Get high-resolution costmap and global path; The dynamic window method is used to sample on the high-resolution cost map, and the trajectory with the minimum cost is selected from the sampled trajectories as the local trajectory under the guidance of the global path.

8. Cleaning robot environment obstacle marking system, characterized in that, The system comprises: A planning module is used to pass the low-resolution cost map to the global path planner and pass the high-resolution cost map of the area of ​​interest to the local trajectory controller; the global path planner plans the global path according to the low-resolution cost map, and the local trajectory controller plans the local trajectory according to the high-resolution cost map and the global path; The obstacle avoidance control module is used to implement obstacle avoidance control of the robot based on the global path and the local trajectory.

9. An electronic device, characterized in that: include: One or more processors, and a memory; the processor and the memory are coupled; the memory is used to store computer program code, the computer program code includes computer instructions; When the processor executes the computer instructions, the electronic device executes the cleaning robot environment obstacle marking method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions or programs, and when the computer instructions or programs are executed on a computer, the computer executes the cleaning robot environment obstacle marking method as described in any one of claims 1-7.

Images