Mobile robot mixed path planning method and application

Abstract

The invention relates to the technical field of robots, in particular to a mobile robot mixed path planning method and application. A path planning algorithm is researched and improved according to a global path planning algorithm based on a static environment and a local path planning algorithm based on a dynamic environment, and the improved global path planning algorithm and the improved local path planning algorithm are fused. And generating a mobile robot path planning algorithm meeting the requirement of the mobile robot on the basis of the time elastic band model in a complex environment. Therefore, the mobile robot has the capability of autonomously planning an efficient and smooth path which avoids the collision risk and meets the requirements of an application scene in a changeable environment.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a hybrid path planning method and its application for mobile robots. Background Technology

[0002] With the rapid development of information science and technology, robots are no longer limited to industrial production applications. Robots with various functions are being used in people's daily work and lives, replacing or assisting humans in performing repetitive and dangerous tasks. Robots can autonomously complete pre-set tasks using their own sensors, greatly improving production efficiency while reducing production costs. As robot technology matures and commercializes, its use in various industries such as service, healthcare, logistics, and power systems is increasing. Consequently, more and more robot developers and university scholars worldwide are joining the research field of robot technology.

[0003] Currently, robots are gradually developing into third-generation intelligent robots. Sensors can accurately collect environmental information, greatly improving their adaptability and autonomy in complex and dynamic environments. The rapid expansion of robot applications means that the level of robot technology directly impacts national economic development.

[0004] Path planning is a core technology for autonomous navigation of mobile robots. Simply put, it enables robots to autonomously find an optimal path from a starting point to a target point, avoiding obstacles, much like human movement. Path planning uses different criteria depending on the robot's operational needs; different criteria result in different planned paths. Finding the path with the lowest cost, shortest time, shortest distance, and fastest obstacle avoidance has become a key research focus for many researchers. The sensors on a mobile robot are equivalent to the human sensory system, accurately grasping information about its surroundings and its own position. Path planning is like the human brain, using the sensory system to analyze environmental information, make decisions about task execution, and drive the mobile robot. However, current mobile robot path planning technologies suffer from low adaptability and safety in complex environments during practical applications. Summary of the Invention

[0005] To address the above problems, this invention provides a hybrid path planning method for mobile robots and its application.

[0006] A hybrid path planning method for a mobile robot, the method comprising:

[0007] Raster maps are constructed using the Cartographer algorithm;

[0008] In a grid map, after receiving the target point location information, a feasible, optimal, and smooth global path is planned.

[0009] When obstacle information is detected during driving, the global path is discretized to obtain local path nodes, and it is determined whether the robot has entered a dangerous area. The speed and turning angle of the mobile robot between local path nodes are controlled to avoid obstacles.

[0010] After obstacle avoidance is completed, return to the globally optimal path.

[0011] Furthermore, after receiving the target point location information, a feasible, optimal, and smooth global path is planned, including:

[0012] In the grid map, set the starting point and the target point, create a blocked list and an open list, and use the A* algorithm with an improved heuristic function and corner optimization strategy to calculate the cost value of the current search node; select the node with the minimum cost value and put it into the blocked list node, and continue to search for nodes in the open list until the target point is reached or there are no more nodes to search; output the nodes in reverse to obtain the planned global path;

[0013] The global path is planned in segments according to the turning nodes, and the trajectory is smoothed and optimized using Bézier curves. The smoothed segments are then merged and spliced ​​together to output the final global path.

[0014] Furthermore, when obstacle information is detected during travel, the global path is discretized to obtain local path nodes. It is then determined whether the robot has entered a dangerous area, and the robot's speed and steering angle between these local path nodes are controlled to avoid obstacles. This includes:

[0015] It receives global path information, travels along a predetermined route, and uses its own onboard sensors to perceive the environment and update the map in real time.

[0016] When encountering obstacles or emergencies, the obstacle expansion area strategy is adopted. The TEB algorithm is used to prioritize the selection of warning areas for path planning. If there is no searchable path, the search will switch to the safe area. Dangerous areas are not allowed to be searched.

[0017] A dynamic danger zone and a safe distance value are set. If the distance between the robot and the obstacle is equal to the safe distance value, a dynamic prediction strategy is triggered. By calculating the current robot speed and the obstacle speed, a prediction is made. If either the robot or the obstacle passes through the danger zone first, it will travel along the predetermined route. If they may meet in the danger zone at the same time, an obstacle avoidance decision will be made.

[0018] Furthermore, using the A* algorithm with an improved heuristic function and corner optimization strategy, the cost value of the current search node is calculated, including:

[0019] We introduce a weighted search strategy and a corner optimization strategy based on path feature constraints into the heuristic function, and make secondary judgments on nodes with the same cost value.

[0020] Furthermore, it also includes: when there are nodes with the same cost value, using a corner optimization strategy based on path feature constraints to calculate the constraint function value, and selecting the node with the minimum value as the path point.

[0021] Furthermore, a weight search strategy is introduced for the heuristic function, including:

[0022] The heuristic function F(n) is shown in the following equation:

[0023] F(n) = G(n) + ω(n)H(n)

[0024] G(n) represents the path cost function value from the current node to the starting node, H(n) represents the evaluated path cost function value from the current node to the target node, and ω(n) is the weight coefficient, which is adjusted according to the working environment of the robot.

[0025] Furthermore, corner optimization strategies based on path feature constraints include:

[0026] The path feature constraint function α(n) is shown in the following equation:

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] T is the current node on the path, A is the parent node of node T, B is the child node of node T, θ is the turning angle, and D... AT For the length of AT, D TB For the length of TB, D AB Let AB be the length of AB, and let θ be the measure of the complementary angle obtained by the Law of Cosines.

[0033] Furthermore, an obstacle expansion region strategy is adopted, using the TEB algorithm to prioritize warning areas for path planning. If no searchable path is found, the search switches to safe areas, while dangerous areas are not allowed to be searched, including:

[0034] An expansion region is introduced, using the distance between the robot and the obstacle as a constraint. The outer perimeter of the obstacle is divided into a ring region with radii of distances from near to far, which is then divided into a danger zone, a warning zone, and a safe zone. The optimal path is searched first in the warning zone. If no path can be searched, the robot is moved to the safe zone. Searching is prohibited in the danger zone.

[0035] Furthermore, dynamic danger zones and safe distance values ​​are set. If the distance between the robot and an obstacle equals the safe distance value, a dynamic prediction strategy is triggered. By calculating the current robot speed and the obstacle speed, a prediction is made. If either the robot or the obstacle passes through the danger zone first, it continues along the predetermined route. If they may meet simultaneously in the danger zone, obstacle avoidance decisions are made, including:

[0036] Three obstacle avoidance modes are set according to the speed of the moving obstacle and the speed of the moving robot:

[0037] Mode 1: When the robot is about to reach the danger zone, if the obstacle is in the state of having already passed the danger zone, or if the following condition is met, it means that the obstacle has a higher priority than the moving robot and passes through the danger zone before the robot reaches it.

[0038] (x1-r1-r2) / v1>(x2+2r2) / v2

[0039] x1 and x2 represent the distance between the robot and the danger zone and the distance between the dynamic obstacle and the danger zone, respectively; r1 and r2 represent the radius of the robot's region and the radius of the expanded dynamic obstacle model, respectively; v1 and v2 represent the linear velocity of the robot and the average linear velocity of the dynamic obstacle, respectively.

[0040] Mode 2: When the robot is about to reach the danger zone at a distance x, and the obstacle has not yet reached the danger zone, if the following formula is satisfied, it means that the mobile robot has a higher priority than the obstacle and passes through the danger zone first before the obstacle reaches the danger zone;

[0041] (x1+r1+r2) / v1<(x2-2r2) / v2

[0042] Mode 3: The mobile robot and the obstacle may arrive at the danger zone simultaneously and collide, i.e., when the following equation is satisfied:

[0043] (x2-2r2) / v2<(x1-r1-r2) / v1<(x2+2r2) / v2

[0044] If the robot's speed satisfies the following formula, where α is the angle between the robot's x-axis in the robot coordinate system and the predicted collision point Q, it will detour around the danger zone from the right front. If the speed does not satisfy the following formula and the robot is in mode three, it is determined that the obstacle has entered the danger zone and has not yet left. In this case, the robot will detour around the obstacle from the left side based on the original algorithm.

[0045]

[0046] A mobile robot uses the aforementioned hybrid path planning method for path planning.

[0047] This invention has at least the following beneficial effects:

[0048] To improve the search efficiency and trajectory smoothness of the A* algorithm in the path planning process, this invention proposes a dynamic weight strategy, a redundant corner optimization strategy, and a trajectory smoothing strategy, and integrates these three improved strategies into the traditional A* algorithm. The improved global path planning algorithm is applicable to environments of varying sizes and with different obstacle distributions, and to a certain extent eliminates or reduces factors that are irrelevant, unreliable, or unstable to actual work requirements, greatly improving the speed and practicality of global path planning.

[0049] This invention integrates the improved A* algorithm with the improved TEB algorithm to construct a hybrid path planning algorithm for both static and dynamic environments. The mobile robot and path planning algorithm designed for both static and dynamic environments, while ensuring optimal path selection, can achieve strong real-time dynamic obstacle avoidance and multi-target point path planning capabilities, and possesses adaptive path planning capabilities for complex environments.

[0050] Other features and advantages of the invention will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures pointed out in the description and the drawings. Attached Figure Description

[0051] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0052] Figure 1 A flowchart illustrating a hybrid path planning method for autonomous mobile robots that integrates the improved A* algorithm and the improved TEB algorithm;

[0053] Figure 2A schematic diagram of a path planned using a hybrid path planning algorithm in a static environment where all environmental conditions are known.

[0054] Figure 3 A schematic diagram of a path planned using a hybrid path planning algorithm in a dynamic environment. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] To address the issues of low adaptability and safety in complex environments during practical applications of mobile robot path planning technology, this invention researches and improves path planning algorithms based on both global path planning algorithms for static environments and local path planning algorithms for dynamic environments. It then integrates these improved global and local path planning algorithms to generate a mobile robot path planning algorithm based on a time-elastic band model that satisfies the requirements of mobile robots in complex environments. This enables mobile robots to autonomously plan a collision-avoiding, efficient, and smooth path that meets application scenario requirements in changing environments.

[0057] A hybrid path planning method for a mobile robot, the method comprising:

[0058] Raster maps are constructed using the Cartographer algorithm;

[0059] In a grid map, after receiving the target point location information, a feasible, optimal, and smooth global path is planned.

[0060] When obstacle information is detected during driving, the global path is discretized to obtain local path nodes, and it is determined whether the robot has entered a dangerous area. The speed and turning angle of the mobile robot between local path nodes are controlled to avoid obstacles.

[0061] After obstacle avoidance is completed, return to the globally optimal path.

[0062] The specific implementation includes the following steps:

[0063] (1) Obtain environmental map information and use the Cartographer algorithm to construct a raster map, and construct a large-scale two-dimensional map with relatively high accuracy for the working environment under limited computing power.

[0064] (2) Specify the target point for the mobile robot. Improve the traditional heuristic function by introducing a weighted search strategy and a corner optimization strategy based on path feature constraints. That is, propose a new constraint function based on path features, improve the prediction function, and make a second judgment on nodes with the same cost value. When the cost function values ​​are the same, add a path feature constraint function for comparison, and improve the A* algorithm (referred to as the improved A* algorithm) by introducing a high-order Bézier curve smoothing trajectory strategy. Use segmented optimization to cut the global path into multiple segments and quickly search for an optimal global path.

[0065] (3) During the process of the mobile robot traveling along the global path, it continuously updates the surrounding environment information by relying on its own onboard sensors. If it encounters an obstacle, it improves the TEB algorithm by introducing a danger zone strategy for static obstacles and a prediction strategy for dynamic obstacles (hereinafter referred to as the improved TEB algorithm), thereby achieving obstacle avoidance and searching for the local optimal path.

[0066] (4) After obstacle avoidance is completed, determine whether there are still obstacles. If there are obstacles, continue to use the improved TEB algorithm, which introduces a danger zone strategy for static obstacles and a prediction strategy for dynamic obstacles, to avoid obstacles in real time. If there are no obstacles, return to the optimal path until the mobile robot safely reaches the target point.

[0067] This invention is practical and can help users apply it to various mobile robot usage scenarios.

[0068] The application environment of mobile robots is complex and ever-changing due to the presence of static, dynamic, and unknown obstacles. Using only a single path planning algorithm cannot meet the requirements of mobile robots for optimal path selection, high efficiency, and strong safety. The hybrid path planning algorithm decomposes path planning into global path planning and local path planning, and optimizes both algorithms. Its features include an environment map construction algorithm, an improved A* algorithm for global path planning, and an improved TEB algorithm for local path planning, wherein:

[0069] The environmental map construction module is used to process data from various sensors such as lidar and IMU to accurately construct an environmental map.

[0070] The global path planning module improves the efficiency and smoothness of the mobile robot's path search by utilizing an improved heuristic function, introducing strategies to reduce redundant corners and smooth the trajectory. The speed and practicality of the global path planning are verified using grid maps of different scales and with varying obstacle ratios.

[0071] The local path planning module uses an improved TEB algorithm to perform local path planning, which enhances the obstacle avoidance capability and the ability to search for local optimal paths in both static and dynamic obstacle environments. The module demonstrates good obstacle avoidance and local optimal path search capabilities in both static and dynamic obstacle environments.

[0072] In step (2), the traditional global path planning algorithm searches indiscriminately for nodes, covering a large number of useless nodes. The improved A* algorithm introduces weight coefficients into its heuristic function, and the evaluation function of the improved A* is shown in Equation 1:

[0073] F(n)=G(n)+ω(n)H(n)⑴

[0074] G(n) represents the path cost function value from the current node to the starting node, and H(n) represents the evaluated path cost function value from the current node to the target node. ω(n) is a weighting coefficient that can be adjusted according to the robot's working environment. The principle of the search algorithm has changed from 1:1 to 1:ω(n). The larger the value of ω(n), the more the path planning search method will favor the estimated function H(n), and the search nodes will quickly expand towards the target node, greatly improving the search speed. Conversely, the smaller the value of ω(n), the less it will favor expanding towards the target node, and the more it will favor searching for the shortest path rather than reducing search time. Therefore, a search method introducing dynamic weights is proposed, which greatly improves search efficiency and reduces the number of search nodes.

[0075] To reduce the number of redundant turns in the global path, a path feature constraint function α(n) is added as shown in Equation 2. When the cost function values ​​are the same, the path feature constraint function is added for comparison, and too many redundant turns are eliminated.

[0076] F′(n)=G(n)+ω′(n)H(n)+α(n) ⑵

[0077] F′(n) is the new total algebraic value obtained by proposing a path feature constraint-based strategy based on the addition of a dynamic weight search strategy, while ω′(n) is the dynamic weight coefficient designed in this paper.

[0078] The steps for calculating α(n) are shown in equations (3) to (7) below:

[0079]

[0080]

[0081]

[0082]

[0083]

[0084] T is the current node on the path, A is the parent node of node T, B is the child node of node T, θ is the turning angle, and D... AT For the length of AT, D TB For the length of TB, D AB Let AB be the length of AB, and let θ be the measure of the complementary angle obtained by the Law of Cosines.

[0085] Considering that the robot's trajectory needs to be smooth and stable in practical applications, a global path planning algorithm is used to generate the path. The path is divided into segments based on the time interval set according to the path nodes. A higher-order Bezier curve is introduced to smooth each segment of the trajectory and merge them into a global path.

[0086] Introducing dynamic weight coefficients to change the search ratio, reducing redundant corners to optimize path length, and smoothing straight-line trajectories.

[0087] The steps of the global path planning algorithm in searching for a path are as follows:

[0088] Step 1: Obtain environmental map information, and use the Cartographer algorithm to fuse sensor information to construct a high-precision two-dimensional global environmental map.

[0089] Step 2: Set the starting and target points, create a blocked list and an open list, and use the improved heuristic function and corner optimization strategy to calculate the cost value of the current search node. Select the node with the minimum cost value and add it to the blocked list. Continue searching for nodes in the open list until the target point is reached or there are no more nodes to search. Output the nodes in reverse order to plan the global path.

[0090] Step 3: Segment the planned path according to the turning nodes and use Bézier curves to smooth the trajectory. Merge and splice the smoothed path segments to output the final path.

[0091] In step (3), the traditional TEB algorithm is prone to inaccurate obstacle avoidance angle and speed control, resulting in vehicle shaking or accidental collisions when unknown obstacles appear. An expansion region and dynamic obstacle prediction strategy are introduced, wherein:

[0092] An expansion region is introduced to improve the ability to respond to changes in the position of static obstacles. The expansion model is used as a constraint on the distance between the robot and the obstacle. The outer ring of the obstacle is divided into danger zone, warning zone and safe zone according to the radius of the distance from the nearest to the farthest obstacle. The optimal path is searched first in the warning zone. If no path can be searched, it is switched to the safe zone. Searching is prohibited in the danger zone, which greatly improves the ability to search for local optima.

[0093] To enhance the obstacle avoidance capability against dynamic obstacles and strengthen the control of the mobile robot's travel angle and speed, a dynamic obstacle prediction strategy is introduced. This strategy sets three obstacle avoidance modes based on the speed of the moving obstacle and the speed of the mobile robot:

[0094] Mode 1: When the robot is about to reach the danger zone, the obstacle is in the state of having passed the danger zone, or satisfies equation (9), which means that the obstacle has a higher priority than the mobile robot and passes through the danger zone first before the robot reaches the danger zone.

[0095] (x1-r1-r2) / v1>(x2+2r2) / v2⑼

[0096] x1 and x2 represent the distance between the robot and the danger zone and the distance between the dynamic obstacle and the danger zone, respectively; r1 and r2 represent the radius of the robot's region and the radius of the expanded dynamic obstacle model, respectively; v1 and v2 represent the linear velocity of the robot and the average linear velocity of the dynamic obstacle, respectively.

[0097] Mode 2: When the robot is about to reach the danger zone x, the obstacle has not yet reached the danger zone, satisfying equation (10), which means that the mobile robot has a higher priority than the obstacle and passes through the danger zone first before the obstacle reaches the danger zone.

[0098] (x1+r1+r2) / v1<(x2-2r2) / v2⁰

[0099] Mode 3: The mobile robot and the obstacle may arrive at the danger zone at the same time and collide, thus satisfying the mathematical relationship of equation (11):

[0100] (x2-2r2) / v2<(x1-r1-r2) / v1<(x2+2r2) / v2⑾

[0101] If the robot's speed satisfies equation (12), where α is the angle between the robot's x-axis in the robot coordinate system and the predicted collision point Q, the decision is to remove the left region trajectory and detour around the danger zone from the right front. If the speed does not satisfy this mathematical expression and is in mode three, it is determined that the obstacle has entered the danger zone and has not yet left, so the robot detours around the obstacle from the left side based on the original algorithm.

[0102]

[0103] Based on priority classification, the system ensures optimal path selection and accurate obstacle avoidance in real time. The dynamic obstacle prediction strategy enables precise obstacle avoidance and better control of vehicle angle and speed, preventing vehicle vibration caused by large fluctuations in values.

[0104] The steps of the local path planning algorithm in searching for a path are as follows:

[0105] Step 1: Receive global path information, travel along the predetermined route, and use the robot's onboard sensors to perceive the environment and update the map in real time.

[0106] Step 2: If obstacles or emergencies are encountered, the obstacle expansion area strategy will prioritize the selection of warning areas for path planning. If no search path is available, the search will switch to the safe area. Searching is not allowed in dangerous areas.

[0107] Step 3: Set dynamic danger zones and safe distance values. If the distance between the robot and the obstacle equals the safe distance value, a dynamic prediction strategy is triggered. By calculating the current robot speed and the obstacle speed, a prediction is made. If either the robot or the obstacle passes through the danger zone first, it will continue along the predetermined route. If they may meet simultaneously in the danger zone, an obstacle avoidance decision will be made.

[0108] To enable those skilled in the art to better understand the present invention, the principles of the present invention are explained below in conjunction with the accompanying drawings:

[0109] This invention utilizes LiDAR and IMU sensors to map the robot's working environment; an improved A* algorithm enables the mobile robot to autonomously and quickly plan a smooth and optimal global path; when encountering obstacles, an improved TEB algorithm is used for obstacle avoidance, planning a locally optimal obstacle avoidance path adaptable to various obstacles, including static, dynamic, known, and unknown ones. After obstacle avoidance, the path returns to the globally optimal path. This application, employing the above scheme, can be applied to mobile robot path planning under any working condition. Considering the variations in the workspace in practical applications, this application can adapt to dynamic environments with varying obstacle arrangements and variable environments, while improving the efficiency of the mobile robot in searching for the optimal path and its obstacle avoidance capabilities, thus enabling autonomous navigation of the mobile robot.

[0110] (1) When searching for a global path in mobile robot, the traditional A* algorithm has problems such as long search time, many redundant nodes in the path, and the path trajectory does not conform to the motion characteristics of the mobile robot. To improve the A* algorithm, we propose a dynamic weight search strategy based on the improved heuristic function as shown in Equation 1, a node optimization strategy based on path feature constraints as shown in Equation 2, and a trajectory smoothing strategy based on Bézier curve.

[0111] F(n)=G(n)+ω(n)H(n) ⑴

[0112] G(n) represents the path cost function value from the current node to the starting node, and H(n) represents the evaluated path cost function value from the current node to the target node. ω(n) is a weighting coefficient, which can be adjusted according to the robot's working environment to improve search efficiency.

[0113] F′(n)=G(n)+ω′(n)H(n)+α(n) ⑵

[0114] F′(n) is the new total algebraic value obtained by proposing a path feature constraint-based strategy based on the addition of a dynamic weight search strategy, while ω'(n) is the dynamic weight coefficient designed in this paper.

[0115] (2) To address the problem of incomplete obstacle avoidance caused by large fluctuations in speed and angle when mobile robots encounter obstacles during movement, acceleration constraints are added, and a danger zone strategy for dealing with static obstacles and a prediction strategy for dealing with dynamic obstacles are introduced to improve the TEB algorithm.

[0116] For static obstacles, a danger coefficient strategy based on an obstacle expansion model is introduced. A circle is drawn with the obstacle center as the origin and the maximum radius of the obstacle plus the expansion distance as the radius. Non-convex expansion processing is applied to the obstacle edges, and an expansion model is constructed around the obstacle circle. Using sensors onboard the robot, the number of obstacles and their offset angles and distances are perceived. The area is divided into danger zones, warning zones, and safe zones, with the robot's distance from the obstacles as a constraint. To improve the local optimality of local path planning under the premise of safety and no collisions, the improved TEB algorithm prioritizes paths planned in warning zones. If a path is impassable in a warning zone, it switches to searching for a passable path in a safe zone. To further enhance robot safety, path searching is not allowed in danger zones.

[0117] When a dynamic obstacle is detected in the current environment, its speed and position information are monitored in real time, and the distance between the obstacle and the mobile robot is updated accordingly. Point Q is the danger point of collision, and its position is updated in real time as the mobile robot's position changes. A circle with point Q as the center and an expanded dynamic obstacle model radius of r2 represents the danger zone, which changes continuously with point Q. For the safety distance, r1 is the radius of the robot's expansion area. When the robot is x away from the danger zone, the danger zone prediction strategy is triggered, and the priority level is set according to the current speed of the robot and the dynamic obstacle.

[0118] (3) The improved A* algorithm is combined with this algorithm to obtain a mobile robot path planning algorithm based on the time elastic band model that satisfies the requirements of mobile robots in complex environments.

[0119] Reference Figure 1 The following is a further description of the implementation flowchart and effects of the improved A* global path planning algorithm of this invention:

[0120] (1) Obtain environmental map information and use the Cartographer algorithm to construct a raster map, and construct a large-scale two-dimensional map with relatively high accuracy for the working environment under limited computing power.

[0121] (2) Set the initial pose and target point, create open and close lists, and put the remaining nodes into the open list, waiting for the search.

[0122] (3) Find reachable nodes around the current node, skipping points in the blocked set. The current node can be the parent node of these nodes.

[0123] (4) By utilizing an improved heuristic function, calculate the new node search cost value, select the node with the lowest cost value, remove it from the open list, and add it to the closed list. If there are nodes with the same cost value, calculate the constraint function value using a corner optimization strategy based on path feature constraints, and select the node with the minimum value as the path point.

[0124] (5) The planned path is segmented according to the turning nodes, and the trajectory is smoothed by Bézier curves. The smoothed paths are then merged and spliced ​​together to output the final path and quickly search for an optimal global path.

[0125] Reference Figure 2 The following is a further description of the implementation flowchart and effects of the improved TEB local path planning algorithm of this invention:

[0126] (1) Obtain the global path and the robot's current pose, extract a portion of the global path near the robot to obtain local path nodes, and adaptively adjust the path nodes and time intervals based on the temporal resolution of adjacent nodes.

[0127] (2) Based on constraints such as obstacle constraints, velocity constraints, acceleration constraints, and time-optimal constraints, non-convex expansion processing is performed on the obstacle edges. After introducing a danger zone strategy, local path planning is performed. When the mobile robot enters the safe distance value of the danger zone, a hypergraph is constructed using decision-making strategies oriented towards static and dynamic obstacles.

[0128] (3) Select a local path driving area. If the optimized path is in a drivable area excluding the danger zone, calculate the robot's driving route and front wheel steering angle based on the path nodes and the robot's kinematic characteristics. Otherwise, re-extract the global path to obtain local path nodes.

[0129] Reference Figure 3 The following is a further description of the specific implementation methods and effects of the overall flowchart of the present invention:

[0130] (1) The mobile robot perceives and maps the environment and locates its position.

[0131] (2) An improved A* algorithm is adopted as the global path planning algorithm. After receiving the target point location information, a feasible, optimal and smooth global path is planned.

[0132] (3) If obstacle information is detected during driving, the TEB algorithm is used to discretize the global path to obtain local path nodes, and it is determined whether the robot has entered a dangerous area. The speed and steering angle of the mobile robot between local path nodes are controlled by the new evaluation function and the kinematic constraints of the Ackerman chassis.

[0133] (4) After obstacle avoidance is completed, the robot returns to the global optimal path, enabling the mobile robot to reach the target location safely and quickly.

[0134] Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A hybrid path planning method for a mobile robot, characterized in that, The method includes: Raster maps are constructed using the Cartographer algorithm; In a grid map, after receiving the target point location information, a feasible, optimal, and smooth global path is planned. When obstacle information is detected during driving, the global path is discretized to obtain local path nodes, and it is determined whether the robot has entered a dangerous area. The speed and turning angle of the mobile robot between local path nodes are controlled to avoid obstacles. After obstacle avoidance is completed, return to the globally optimal path.

2. The mobile robot hybrid path planning method according to claim 1, characterized in that, After receiving the target point location information, a feasible, optimal, and smooth global path is planned, including: In the grid map, set the starting point and the target point, create a blocked list and an open list, and use the A* algorithm with an improved heuristic function and corner optimization strategy to calculate the cost value of the current search node; select the node with the minimum cost value and put it into the blocked list node, and continue to search for nodes in the open list until the target point is reached or there are no more nodes to search; output the nodes in reverse to obtain the planned global path; The global path is planned in segments according to the turning nodes, and the trajectory is smoothed and optimized using Bézier curves. The smoothed segments are then merged and spliced ​​together to output the final global path.

3. The mobile robot hybrid path planning method according to claim 1, characterized in that, When obstacle information is detected during movement, the global path is discretized to obtain local path nodes. It is then determined whether the robot has entered a dangerous area, and the robot's speed and steering angle between these local path nodes are controlled to avoid obstacles. This includes: It receives global path information, travels along a predetermined route, and uses its own onboard sensors to perceive the environment and update the map in real time. When encountering obstacles or emergencies, the obstacle expansion area strategy is adopted. The TEB algorithm is used to prioritize the selection of warning areas for path planning. If there is no searchable path, the search will switch to the safe area. Dangerous areas are not allowed to be searched. A dynamic danger zone and a safe distance value are set. If the distance between the robot and the obstacle is equal to the safe distance value, a dynamic prediction strategy is triggered. By calculating the current robot speed and the obstacle speed, a prediction is made. If either the robot or the obstacle passes through the danger zone first, it will travel along the predetermined route. If they may meet in the danger zone at the same time, an obstacle avoidance decision will be made.

4. The mobile robot hybrid path planning method according to claim 2, characterized in that, The A* algorithm, employing an improved heuristic function and corner optimization strategy, calculates the cost of the current search node, including: We introduce a weighted search strategy and a corner optimization strategy based on path feature constraints into the heuristic function, and make secondary judgments on nodes with the same cost value.

5. The mobile robot hybrid path planning method according to claim 2, characterized in that, It also includes: when there are nodes with the same cost value, a corner optimization strategy based on path feature constraints is used to calculate the constraint function value, and the node with the minimum value is selected as the path point.

6. The mobile robot hybrid path planning method according to claim 4, characterized in that, Introducing weight search strategies for heuristic functions, including: The heuristic function F(n) is shown in the following equation: F(n) = G(n) + ω(n)H(n) G(n) represents the path cost function value from the current node to the starting node, H(n) represents the evaluated path cost function value from the current node to the target node, and ω(n) is the weight coefficient, which is adjusted according to the working environment of the robot.

7. The mobile robot hybrid path planning method according to claim 4, characterized in that, Corner optimization strategies based on path feature constraints include: The path feature constraint function α(n) is shown in the following equation: T is the current node on the path, A is the parent node of node T, B is the child node of node T, θ is the turning angle, and D... AT For the length of AT, D TB For the length of TB, D AB Let AB be the length of AB, and let θ be the measure of the complementary angle obtained by the law of cosines.

8. The mobile robot hybrid path planning method according to claim 3, characterized in that, An obstacle expansion region strategy is adopted, and the TEB algorithm is used to prioritize the selection of warning areas for path planning. If no searchable path is found, the search switches to a safe area. Dangerous areas are not allowed to be searched, including: An expansion region is introduced, using the distance between the robot and the obstacle as a constraint. The outer perimeter of the obstacle is divided into a ring region with radii of distances from near to far, which is then divided into a danger zone, a warning zone, and a safe zone. The optimal path is searched first in the warning zone. If no path can be searched, the robot is moved to the safe zone. Searching is prohibited in the danger zone.

9. The mobile robot hybrid path planning method according to claim 3, characterized in that, A dynamic danger zone and safe distance value are set. If the distance between the robot and the obstacle equals the safe distance value, a dynamic prediction strategy is triggered. By calculating the current robot speed and the obstacle speed, a prediction is made. If either the robot or the obstacle passes through the danger zone first, the robot continues along the predetermined route. If they may meet simultaneously in the danger zone, obstacle avoidance decisions are made, including: Three obstacle avoidance modes are set according to the speed of the moving obstacle and the speed of the moving robot: Mode 1: When the robot is about to reach the danger zone, if the obstacle is in the state of having already passed the danger zone, or if the following condition is met, it means that the obstacle has a higher priority than the moving robot and passes through the danger zone before the robot reaches it. (x1-r1-r2) / v1>(x2+2r2) / v2 x1 and x2 represent the distance between the robot and the danger zone and the distance between the dynamic obstacle and the danger zone, respectively; r1 and r2 represent the radius of the robot's region and the radius of the expanded dynamic obstacle model, respectively; v1 and v2 represent the linear velocity of the robot and the average linear velocity of the dynamic obstacle, respectively. Mode 2: When the robot is about to reach the danger zone at a distance x, and the obstacle has not yet reached the danger zone, if the following formula is satisfied, it means that the mobile robot has a higher priority than the obstacle and passes through the danger zone first before the obstacle reaches the danger zone; (x1+r1+r2) / v1<(x2-2r2) / v2 Mode 3: The mobile robot and the obstacle may arrive at the danger zone simultaneously and collide, i.e., when the following equation is satisfied: (x2-2r2) / v2<(x1-r1-r2) / v1<(x2+2r2) / v2 If the robot's speed satisfies the following formula, where α is the angle between the robot's x-axis in the robot coordinate system and the predicted collision point Q, it will detour around the danger zone from the right front. If the speed does not satisfy the following formula and the robot is in mode three, it is determined that the obstacle has entered the danger zone and has not yet left. In this case, the robot will detour around the obstacle from the left side based on the original algorithm.

10. A mobile robot, characterized in that, The path planning is performed using the hybrid path planning method for mobile robots as described in any one of claims 1-9.

Images