Robot mixed path planning simulation system and method

Abstract

The invention discloses a robot hybrid path planning simulation system and method. The system comprises a scene generation node, a visual node, a cost map node, a hybrid path planning node, a control instruction calculation node and an ROS message service. The method comprises the following five steps: initializing a path planning simulation starting system node and service, planning a mixed path, issuing a control instruction and displaying a path planning test result. The method has the advantages that: 1, the input and output of global and local path planning algorithms are unified by using a universal interface, and the defects of high coupling and difficulty in independent test of different level planning methods in the traditional method are overcome; 2, various basic methods for path planning are modularly designed by utilizing a base class-derived class architecture, and the bottlenecks of high scene adaptation cost and long algorithm iteration period in the prior art are broken through; and thirdly, a test node is planned by constructing a basic path, the new algorithm is integrated into the node, rapid test of the new algorithm is realized, and the algorithm test and optimization time is saved.

Description

Technical Field

[0001] The present invention relates to a robot hybrid path planning simulation system and method, and belongs to the field of automatic driving and robot control. Background Art

[0002] In the fields of intelligent transportation systems, industrial automation and service robots, path planning is the core link connecting environmental perception and motion control. Its technological innovation directly determines the practicality and reliability of intelligent systems in complex scenarios. As the application scenarios extend from structured environments to open dynamic environments, traditional path planning methods have gradually exposed systematic defects in terms of real-time performance and robustness, becoming a bottleneck restricting breakthroughs in key technologies such as autonomous driving vehicles. Especially in the context of accelerated urbanization and intelligent manufacturing upgrades, existing technologies are difficult to meet the high-dynamic and high-precision planning requirements. Therefore, the development of a hybrid path planning system has become a key technological breakthrough to promote the industrialization of intelligent equipment, and is of great strategic significance for improving the effectiveness of intelligent driving and intelligent equipment. Therefore, the present invention has developed a robot hybrid path planning simulation system and method, which can test the effectiveness of multiple path planning methods in the ROS simulation environment, and is of great significance for opening up the entire process of design, testing and application of path planning methods.

[0003] In the field of intelligent system path planning, the coordinated evolution of global and local path planning technologies has always been a core issue in technological development. Global path planning, based on a priori environmental models (such as high-precision maps or topological networks), generates a macro-optimal path from the starting point to the destination using graph search algorithms (A*, Dijkstra) or optimization methods (genetic algorithms). These methods demonstrate the advantage of ensuring path optimality in structured scenarios, but their rigid architecture struggles to cope with sudden environmental changes such as dynamic obstacles and real-time traffic flow, and their computational complexity increases exponentially with environmental scale. Local path planning focuses on dynamic obstacle avoidance driven by real-time perception data, employing algorithms such as model predictive control (MPC) and the dynamic windowing algorithm (DWA) to fine-tune the trajectory. While these technologies can effectively avoid unexpected obstacles, their lack of a global perspective can easily lead to local optimality traps. Whether global or local path planning, a single planning level struggles to balance the contradiction between "global optimality" and "local flexibility": global planning relies on static assumptions that are prone to failure, while local planning lacks long-term decision-making and is prone to short-term thinking. In this context, hybrid path planning technology has achieved breakthrough progress through architectural innovation and algorithm integration, demonstrating significant advantages in three aspects: adaptability to dynamic environments, multi-objective collaborative optimization, and algorithm scalability.

[0004] Existing hybrid algorithms generally suffer from architectural rigidity, high module coupling, and algorithm replacement requires restructuring the system framework, resulting in low algorithm reuse rates, high costs for functional expansion and cross-platform porting, and difficulty meeting the adaptation needs of diverse carriers and scenarios. These systemic flaws increase the difficulty of path planning algorithm design, testing, and simulation, severely restricting the deployment efficiency of intelligent equipment in open environments. Breakthroughs are urgently needed through architectural innovation and algorithm integration.

[0005] In response to the above difficulties, the present invention proposes a robot hybrid path planning simulation system and method. Based on the general posture state and path point position messages, a path planning simulation framework is established, and a path planning simulation system is built. The hybrid path planning algorithm simulation is performed with general structural design and simulation. It has good versatility and provides a set of general paradigms for the development and testing of new hybrid path planning algorithms, which can improve the development efficiency of path planning algorithms and save time and energy in developing general modules. Summary of the Invention

[0006] 1. Purpose of the invention:

[0007] To address the challenges of multi-level coupling testing, slow algorithm development due to rigid system architecture, and long verification cycles in the development of robot hybrid path planning algorithms, the present invention provides a robot hybrid path planning simulation system and method. The core purpose of the invention is to achieve rapid adaptation and testing of new path planning algorithms by constructing a decoupled architecture between the global planning layer and the local optimization layer, using a common interface to standardize the input and output of heterogeneous algorithms, and establishing a modular path planning algorithm based on a base-derived class inheritance relationship. This overcomes the high migration and adaptation costs of existing systems due to their closed architecture, innovates the hybrid path planning system architecture, and provides an engineering support platform for the rapid R&D, debugging, and evolution of related fields.

[0008] 2. Technical solution:

[0009] The present invention provides a robot hybrid path planning simulation system, which includes: a scene generation node, a visualization node, a cost map node, a hybrid path planning node, a control instruction calculation node, and a ROS (Robot Operating System) message service.

[0010] The scene generation node loads the robot model and scene map, initializes the robot task scene and robot initial parameters, and provides scene conditions for the robot hybrid path planning simulation;

[0011] The visualization node realizes the visualization of scene and other related information based on RViz, and can display the cost map, global / local path, and robot motion trajectory to realize the observation of simulation results; at the same time, it generates waypoint messages through mouse click events for publishing first-level waypoints;

[0012] The costmap node uses the robot's location and environment map and the robot's equipment (such as lidar) to publish a global costmap and a local costmap of the robot.

[0013] The hybrid path planning node integrates the functions of the designed global planner and local path planner, coordinates the entire process of path planning, completes the combination of hybrid path planning methods, and publishes the next path point of the robot based on the results of path planning;

[0014] The control instruction calculation node calculates the control law of the robot by using the dual-modal strategy given the three-level path points of the robot to achieve control of the robot;

[0015] The ROS message service is a message service of the ROS operating system, which realizes message communication between various nodes.

[0016] Furthermore, as attached Figure 3 As shown in the figure, except for the ROS message service which is performed in real time, the other nodes follow a certain time sequence. The working relationship of each node in the system at different stages is as follows:

[0017] The scene generation node passes scene information, including map status and robot position, to the visualization node for display. It also passes the scene information to the costmap node, which processes it and receives robot control instructions to complete robot control.

[0018] The visualization node receives scene information and displays the scene content. Users can also publish first-level pathpoint messages from the visualization node to the hybrid path planning node using a mouse click. Furthermore, the visualization node can receive messages such as costmaps from costmap nodes and pathpoints from hybrid path planning nodes, displaying the real-time path planning scene. The costmap node receives scene information, publishes global and local costmaps to the hybrid path planning node, and sends the costmaps to the visualization node for display.

[0019] The hybrid path planning node receives the first-level path points and the cost map, performs hybrid path planning, and sends the third-level path points to the control instruction calculation node. At the same time, the path planning information is sent to the visualization node for display.

[0020] The control instruction calculation node receives the third-level path point message, calculates the robot control instruction, and sends it to the scene generation node to calculate the robot status.

[0021] Furthermore, the present invention addresses the problem of autonomous target finding by UAVs in an environment and invents a robot hybrid path planning simulation system and method. The specific implementation steps are as follows:

[0022] Step 1: Start the path planning simulation

[0023] Establish a URDF (Unified Robot Description Format) model for the robot and build a robot path planning task scenario based on the Gazebo simulation engine.

[0024] Based on the system's corresponding universal base class for path planning, design a global / local path planning algorithm, connect the algorithm to the hybrid path planning node, and complete the compilation. The global / local path planning algorithm of this system must inherit from the universal base class and utilize the universal coordinate conversion method and dynamic map processing module already written in the base class. It only needs to design the main module plan function for the path point sequence planning from the starting point to the end point based on the characteristics of the different algorithms to be adopted, using the existing map and current coordinates. Output a unified list based on the Posestamped type as the planned path, and call it in the interface corresponding to the hybrid path planning node to complete the design and use of the global / local path planning algorithm.

[0025] Start the path planning simulation system.

[0026] Step 2: System nodes and service initialization

[0027] The scene generation node loads the robot model file and task scene settings to initialize the scene. During system operation, it receives robot control instructions and realizes the robot's movement in the scene.

[0028] At each moment, the costmap node listens to the robot sensor messages, obtains and publishes the robot's global costmap and local costmap.

[0029] The visualization node loads the mission scenario, receives the robot status, and uses RViz to visualize the robot mission scenario. Based on the visualization node, the user publishes the robot's desired coordinates. These coordinates are called first-level path points.

[0030] The hybrid path planning node always monitors the coordinate transformation relationship and receives the first-level path points to complete the initialization of the hybrid path planning node.

[0031] The control instruction calculation node completes initialization, monitors relevant data, and prepares to calculate the robot control instructions.

[0032] The ROS message service enables communication between different nodes. The service is started when ROS is started.

[0033] Step 3: Hybrid Path Planning

[0034] Using the robot's current position and posture, the global cost map, and the local cost map, the hybrid path planning node calls the global path planner and local path planner designed in step 1 to plan the next path coordinates of the robot. The coordinates of these path points are called third-level path points and are published to the control instruction calculation node for control law calculation. The specific steps are as follows:

[0035] S31. System status pre-check

[0036] Checks whether the global planner, local planner, and robot pose in the hybrid path planning node have been initialized. If all are ready, proceed to the next path planning cycle; otherwise, terminate the current planning cycle immediately to ensure system robustness. Simultaneously, monitors for first-level waypoint messages. If a first-level waypoint message is received, invokes the global path planner to perform global path planning.

[0037] S32. Planning Termination Judgment

[0038] When the global path planned in step S31 exists, the following steps are performed in sequence:

[0039] a. Extract the end point of the path as the global target point.

[0040] b. Convert the current robot pose to the global coordinate system.

[0041] c. Calculate the Euclidean distance between the robot and the target point.

[0042] d. If the distance is less than the 0.01-meter threshold, the current robot pose is published as a Level 3 waypoint and returned, marking the task complete and clearing the global path data to avoid duplicate planning.

[0043] S33, motion state update and path preprocessing

[0044] Get the robot's current linear and angular velocity and calculate the composite velocity based on them. Update the global waypoint timestamp to the current time to ensure timing consistency.

[0045] S34, path tracking deviation detection

[0046] Based on the robot's current position and global path, calculate the deviation between the robot and the currently planned path and determine whether path replanning is necessary. The specific steps of this step are as follows:

[0047] a. Convert the global path and the robot's current pose to the local coordinate system.

[0048] b. For the global path P in the local coordinate system local ={p1,p2,…,p n}(where p i =(x i ,y i ) represents the coordinates of the path points), and processes the line segments formed by each pair of adjacent path points in turn

[0049] c. For a certain line segment According to the current position of the robot p cur =(x cur ,y cur ), define the geometric vectors v and w:

[0050]

[0051] And calculate the projection parameter t:

[0052]

[0053] Then determine the nearest point q based on the t value:

[0054]

[0055] d. Calculate the current point p cur Euclidean distance to the nearest point q:

[0056]

[0057] e. Traverse all global path segments and output the minimum value d of all path distances min As the deviation of the current point from the global path.

[0058] f. According to the current speed v of the robot l Calculate the speed factor and limit the speed factor to a certain range. The calculation formula is as follows:

[0059]

[0060] And generate a dynamic deviation threshold h=0.5k according to the speed factor.

[0061] g. If the minimum path deviation d is detected min If the dynamic deviation threshold h is exceeded, global re-planning is triggered, with the robot's current coordinates as the starting point and the latest first-level path point as the end point, and the global path planner is called to perform global path planning. Otherwise, S35 is executed.

[0062] S35. Local path planning and path point generation

[0063] The latest global path P currently maintained by the hybrid path planning node local ={p1,p2,…,p n} (This path is initially generated by step S31 and may be updated in step S34), call the local planner to perform local path planning:

[0064] a. Calculate the distance between the robot's current coordinates and all global path points to obtain the shortest distance point p j ;

[0065] b. Taking the shortest point as the reference, select the 15th path point p in the direction of the target point (i.e. the direction of increasing index) j+15 As the local navigation target (if it exceeds the end of the path, the end point is taken). This point is called a secondary waypoint;

[0066] c. Input the starting pose, secondary path points, and the converted global path in the local coordinate system, and use the local planning method of the local planner to perform local path planning to generate a local trajectory containing multiple path points;

[0067] d. Based on the local trajectory containing multiple path points, find the nearest reference point in the trajectory according to the robot's current position, then dynamically calculate the foresight distance based on the current speed. Select the point along the trajectory toward the target point (i.e., the direction of increasing index) that is closest to the foresight distance from the nearest reference point as the target position that meets the robot's motion capabilities;

[0068] e. Convert the target position in the local coordinate system back to the global coordinate system and publish it as the target point for robot control. This target point is the third-level path point.

[0069] Step 4: Control instruction release

[0070] The robot control instruction calculation node receives the robot's three-level path points and calculates the robot's control law based on the robot's current posture. First, the three-level path points are converted to the body coordinate system. In the local map coordinate system, let the desired posture corresponding to the three-level path points be p d =(x d ,y d ,θ d ), the current posture of the robot is p0=(x0,y0,θ0), then the transformed three-level path point posture e=(e x ,e y ,e θ )for:

[0071]

[0072] Where Δx = x d -x0, Δy=y d -y0 is the lateral and longitudinal deviation of the robot in the local coordinate system, e θ Normalize to (-π,π].

[0073] Then a dual-modal error compensation strategy is adopted:

[0074] When the lateral error e y When the error is greater than 0.1m, the mixed error drive control law is adopted:

[0075]

[0076] When the lateral error e y When the distance is ≤0.1m, it switches to pure angle tracking mode and controls the residual position error with a small linear velocity compensation.

[0077]

[0078] The calculated desired linear velocity and angular velocity are then sent to the robot for solution and drive the robot to move.

[0079] Step 5: Path planning test results display

[0080] The robot starts to move, and the visualization node displays the cost map, global path, and local path obtained during the robot's movement, and displays the robot's motion control instructions.

[0081] 3. Advantages and effects:

[0082] The present invention proposes a robot hybrid path planning simulation system and method, which mainly has the following advantages: First, by constructing a dynamic collaborative architecture of global planning and local optimization, and using a universal interface to unify the input and output of the global and local path planning algorithms, the shortcomings of traditional methods in which different levels of planning methods are highly coupled and difficult to test individually are solved; Second, by utilizing the base class-derived class architecture, various basic methods of path planning are modularly designed, breaking through the bottleneck of high scene adaptation cost and long algorithm iteration cycle caused by the rigid architecture of the existing technology; Third, by constructing basic path planning test nodes, the new algorithm is integrated into the node to achieve rapid testing of the new algorithm, saving time for algorithm testing and optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 This is a schematic diagram of a robot hybrid path planning simulation system and method.

[0084] Figure 2 This is the architecture diagram of the path planning simulation system.

[0085] Figure 3 This is a timing diagram of each node system outside the message service.

[0086] Figure 4 Simulate scenarios for path planning.

[0087] Figure 5 Two-level path planning for robots.

[0088] Figure 6 Test results for robot path planning. DETAILED DESCRIPTION

[0089] To make the objectives, technical solutions, and advantages of the present invention more clearly understood, the present invention is further described in detail below by combining a specific path planning method and embodiments. It should be noted that the path planning method and embodiments described herein are merely for the purpose of explaining the present invention and are not intended to limit the present invention.

[0090] Step 1: Start the path planning simulation

[0091] Create a path planning task scenario with multiple obstacles and construct the differential robot physical model and kinematic model URDF files as the robot model. Equip the robot with a lidar and place it at the origin of the path planning task scenario.

[0092] Based on the system's corresponding universal path planning base class, we designed an improved A* algorithm as the global path planning algorithm and a dynamic window method as the local path planning algorithm. These two path planning algorithms were used as the global path planner and the local path planner, respectively. These algorithms were connected to the hybrid path planning node and compiled.

[0093] Start the simulation system, start the scene generation node, visualization node, cost map node, hybrid path planning node, control instruction calculation node, and ROS message service.

[0094] Step 2: System nodes and service initialization

[0095] The scene generation node loads the robot model URDF file and the task scene settings to initialize the scene. During system operation, it receives robot control commands and realizes the robot's movement in the scene.

[0096] Based on the Gazebo simulation platform, the robot uses a lidar to detect its surroundings and publishes lidar detection data in real time. At each moment, the costmap node directly generates a local costmap based on the lidar detection data and publishes it. Simultaneously, it uses SLAM (Simultaneous Localization and Mapping) to construct a global map and publishes global costmap messages based on the global map.

[0097] The visualization node uses RViz for visualization. Figure 4 Click the 2D Nav Goal button and select the desired target point and desired direction. This path point is the robot's desired coordinate point, the robot's first-level path point, and is published.

[0098] In the hybrid path planning simulation node, the first-level path points, local cost maps, and global cost maps are always monitored. After receiving the local cost map, if it is the first time to receive it, the local path planner is initialized and the local map is stored, otherwise only the local path planner is updated. The global path planner also performs the same steps. After receiving the first-level path point, if the global path planner has been initialized, the global cost map is used to call the global path planner for planning and obtain the global path coordinate point data P of the Path type. global .

[0099] The control instruction calculation node completes initialization, monitors relevant data, and prepares to calculate the robot control instructions.

[0100] The ROS message service starts when the system starts.

[0101] Step 3: Hybrid Path Planning

[0102] Using the robot's current position and posture, global cost map, and local cost map, and utilizing the hybrid path planning node, the global path planner and the local path planner are called to plan the next path coordinates of the robot. The coordinates of these path points are called third-level path points and are published to the control instruction calculation node for control law calculation.

[0103] S31. System status pre-check

[0104] Checks whether the global planner, local planner, and robot pose in the hybrid path planning node have been initialized. If they are, proceed with subsequent path planning; otherwise, terminate the current planning cycle immediately. Simultaneously, monitors for first-level waypoint messages. If a first-level waypoint message is received, invokes the global path planner and performs global path planning.

[0105] S32. Planning Termination Judgment

[0106] When the global path planned in step S31 exists, the following steps are performed in sequence:

[0107] a. Extract the end point of the path as the global target point, as point p glo_end =(x glo_end ,y glo_end ).

[0108] b. Convert the current robot pose to the global coordinate system as point p glo_cur =(x glo_cur ,y glo_cur ).

[0109] c. Calculate the distance between two points

[0110] d. If the distance is less than the 0.01 meter threshold, publish point p glo_cur Return as a third-level waypoint, mark the task completed, and clear the global path data.

[0111] S33, motion state update and path preprocessing

[0112] Get the current linear velocity v l =(v x ,v y ) and angular velocity, and calculate the resultant velocity based on this Update the global path timestamp to the current time.

[0113] S34, path tracking deviation detection

[0114] Based on the robot's current position and global path, the deviation between the robot and the currently planned path is calculated to determine whether path replanning is needed.

[0115] a. Convert the global path and the robot's current pose to the local coordinate system.

[0116] b. For the global path P in the local coordinate system local ={p1,p2,…,p3}(where p i =(x i ,y i ) represents the coordinates of the path points), and processes the line segments formed by each pair of adjacent path points in turn

[0117] c. For a certain line segment Calculate the nearest point q according to equations (1)-(3);

[0118] d. Calculate the Euclidean distance from the current point to the nearest point according to formula (4);

[0119] e. Traverse all global path segments and output the minimum value d of all path distances min as the deviation of the current point from the global path;

[0120] f. According to formula (5) and the dynamic deviation threshold calculation method to obtain the dynamic deviation threshold;

[0121] g. If the minimum path deviation d is detected minIf the dynamic threshold h is exceeded, global re-planning is triggered, with the robot's current coordinates as the starting point and the latest first-level path point as the end point, and the global path planner is called to perform global path planning. Otherwise, the process continues with S35.

[0122] S35. Local path planning and path point generation

[0123] The latest global path P currently maintained by the hybrid path planning node local ={p1,p2,…,p n} (This path is initially generated by step S31 and may be updated in step S34), call the local planner to perform local path planning:

[0124] a. Calculate the distance between the robot's current coordinates and all global path points to obtain the shortest distance point p j ;

[0125] b. Taking the shortest point as the reference, select the 15th path point p in the direction of the target point (i.e. the direction of increasing index) j+15 As the local navigation target (if it exceeds the end of the path, the end point is taken). This point is called a secondary waypoint;

[0126] c. Input the starting pose, secondary path points, and the converted global path in the local coordinate system, and use the local planning method of the local planner to perform local path planning to generate a local trajectory containing multiple path points;

[0127] d. Based on this local trajectory containing multiple path points, the robot finds the nearest reference point in the trajectory according to its current position. The robot then dynamically calculates the foresight distance based on its current speed. The robot then selects the point closest to the nearest reference point along the trajectory toward the target point (i.e., in the direction of increasing index) as the target position that meets the robot's motion requirements.

[0128] e. Convert the target position in the local coordinate system back to the global coordinate system and publish it as the target point for robot control. This target point is the third-level waypoint.

[0129] Step 4: Control instruction release

[0130] The robot control command calculation node receives the robot's three-level path points and calculates the robot's control law based on the robot's current position. The robot control command is calculated according to equations (6)-(8), and v and ω are used as the robot's linear velocity and angular velocity, respectively, and sent to the cmd_vel topic to drive the robot movement.

[0131] Step 5: Path planning test results display

[0132] The robot starts to move, and the visualization node displays the cost map, global path, and local path during the robot's movement, and displays the robot's motion control instructions.

[0133] Figures 4 to 6 This is the simulation result diagram of this embodiment. Figure 4 Simulate scenarios for robot path planning; Figure 5 are the global and local waypoints of the robot during path planning; Figure 6 At the end of the simulation, the robot is able to reach the given first-level path points according to the planned path, completing the path planning task of this embodiment.

Claims

1. A robot hybrid path planning simulation system, characterized by: The system includes: scene generation node, visualization node, cost map node, hybrid path planning node, control instruction calculation node, and ROS message service; The scene generation node loads the robot model and scene map, initializes the robot task scene and robot initial parameters, and provides scene conditions for the robot hybrid path planning simulation; The visualization node visualizes scene-related information based on RViz, and can display cost maps, global / local paths, and robot motion trajectories to observe simulation results. At the same time, it generates waypoint messages through mouse click events to publish first-level waypoints. The costmap node uses the robot's location and environment map to publish a global costmap and a local costmap of the robot based on the robot's device; The hybrid path planning node integrates the functions of the designed global planner and local path planner, coordinates the entire process of path planning, completes the combination of hybrid path planning methods, and publishes the next path point of the robot based on the results of path planning; The control instruction calculation node calculates the control law of the robot by using the dual-modal strategy given the three-level path points of the robot to achieve control of the robot; The ROS message service is a message service of the ROS operating system, which realizes message communication between various nodes.

2. The system according to claim 1, wherein: Except for the ROS message service, which is performed in real time, all other nodes follow a certain time sequence. The working relationship of each node in the system at different stages is as follows: The scene generation node passes the scene information, including the map status and robot position information, to the visualization node for display. At the same time, the scene information is passed to the cost map node for processing, and the cost map node receives the robot control instructions to complete the robot control. The visualization node receives scene information and displays the scene content. At the same time, the user uses the mouse to click to publish the first-level path point message in the visualization node to the hybrid path planning node. In addition, the visualization node can also receive the cost map node's cost map node and the path point message from the hybrid path planning node to display the real-time scene of the path planning. The cost map node receives scene information, publishes global and local cost maps to the hybrid path planning node, and sends the cost map to the visualization node for display; The hybrid path planning node receives the first-level path points and cost map, performs hybrid path planning, and sends the third-level path points to the control instruction calculation node; at the same time, the path planning information is sent to the visualization node for display; The control instruction calculation node receives the third-level path point message, calculates the robot control instruction, and sends it to the scene generation node to calculate the robot status.

3. A robot hybrid path planning simulation method, characterized by: The specific implementation steps are as follows: Step 1: Start the path planning simulation Establish the robot's URDF model and construct the robot path planning task scenario; Based on the system's corresponding path planning universal base class, design the global / local path planning algorithm, connect the algorithm to the hybrid path planning node, and complete the compilation; Start the path planning simulation system; Step 2: System nodes and service initialization The scene generation node loads the robot model file and task scene settings to initialize the scene; during system operation, it receives robot control instructions to realize the robot's movement in the scene; At each moment, the costmap node listens to the robot sensor messages, obtains and publishes the robot's global costmap and local costmap; The visualization node loads the task scenario, receives the robot status, and uses RViz to visualize the robot task scenario. The user publishes the robot's expected coordinate points based on the visualization node. This coordinate point is called a first-level path point; The hybrid path planning node constantly monitors the coordinate transformation relationship and receives the first-level path points to complete the initialization of the hybrid path planning node; The control instruction calculation node completes initialization, monitors relevant data, and prepares to calculate the robot control instructions; The ROS message service enables communication between different nodes. This service is started when ROS is started; Step 3: Hybrid Path Planning Using the robot's current position and posture, the global cost map, and the local cost map, the hybrid path planning node calls the global path planner and local path planner designed in step 1 to plan the next path coordinates of the robot. The coordinates of these path points are called third-level path points and are published to the control instruction calculation node for control law calculation. Step 4: Control instruction release The robot control instruction calculation node receives the robot's three-level path points and calculates the robot's control law based on the robot's current posture; Step 5: Path planning test results display The robot starts to move, and the visualization node displays the cost map, global path, and local path obtained during the robot's movement, and displays the robot's motion control instructions.

4. The method according to claim 3, wherein: The specific process of step three is as follows: S31. System status pre-check Check whether the global planner, local planner, and robot pose in the hybrid path planning node have been initialized. If they are all ready, proceed to subsequent path planning. Otherwise, terminate the current planning cycle immediately to ensure system robustness. At the same time, monitor the first-level waypoint message. If a first-level waypoint message is received, call the global path planner. Perform global path planning; S32. Planning Termination Judgment When the global path planned in step S31 exists, the following steps are performed in sequence: a. Extract the end point of the path as the global target point; b. Convert the current robot pose to the global coordinate system; c. Calculate the Euclidean distance between the robot and the target point; d. If the distance is less than the 0.01m threshold, the current robot pose is published as a Level 3 waypoint and returned, marking the task complete and clearing the global path data to avoid duplicate planning. S33, motion state update and path preprocessing Get the robot's current linear velocity and angular velocity, and calculate the composite velocity based on them; update the global path point timestamp to the current moment to ensure timing consistency; S34, path tracking deviation detection Based on the robot's current position and global path, calculate the deviation between the robot and the currently planned path and determine whether path replanning is necessary. The specific steps of this step are as follows: a. Convert the global path and the robot's current pose to the local coordinate system; b. For the global path P in the local coordinate system local ={p1,p2,…,p n }, where p i =(x i ,y i ) represents the coordinates of the path points, and processes the line segments formed by each pair of adjacent path points in turn c. For a certain line segment According to the current position of the robot p cur =(x cur ,y cur ), define the geometric vectors v and w: And calculate the projection parameter t: Then determine the nearest point q based on the t value: d. Calculate the current point p cur Euclidean distance to the nearest point q: e. Traverse all global path segments and output the minimum value d of all path distances min as the deviation of the current point from the global path; f. According to the current speed v of the robot l Calculate the speed factor and limit the speed factor to a certain range. The calculation formula is as follows: And generate a dynamic deviation threshold h=0.5k based on the speed factor; g. If the minimum path deviation d is detected min If the dynamic deviation threshold h is exceeded, global re-planning is triggered, with the robot's current coordinates as the starting point and the latest first-level path point as the end point, and the global path planner is called to perform global path planning; otherwise, S35 is executed; S35. Local path planning and path point generation The latest global path P currently maintained by the hybrid path planning node local ={p1,p2,…,p n }, call the local planner to perform local path planning: a. Calculate the distance between the robot's current coordinates and all global path points to obtain the shortest distance point p j ; b. Based on the shortest point, select the 15th path point p towards the target point. j+15 As a local navigation target; this point is called a secondary waypoint; c. Input the starting pose, secondary path points, and the converted global path in the local coordinate system, and use the local planning method of the local planner to perform local path planning to generate a local trajectory containing multiple path points; d. Based on the local trajectory containing multiple path points, find the nearest reference point in the trajectory according to the robot's current position, then dynamically calculate the foresight distance based on the current speed. Select the point along the trajectory toward the target point that is closest to the foresight distance from the nearest reference point as the target position that meets the robot's motion capabilities; e. Convert the target position in the local coordinate system back to the global coordinate system and publish it as the target point for robot control; this target point is the third-level path point.

5. The method according to claim 3, wherein: The specific process of step 4 is as follows: first, transform the three-level path points into the body coordinate system; in the local map coordinate system, let the desired position corresponding to the three-level path points be p d =(x d ,y d ,θ d ), the current posture of the robot is p0=(x0,y0,θ0), then the transformed three-level path point posture e=(e x ,e y ,e θ )for: Where Δx = x d -x0, Δy=y d -y0 is the lateral and longitudinal deviation of the robot in the local coordinate system, e θ Normalize to (-π,π]; Then a dual-modal error compensation strategy is adopted: When the lateral error e y When the error is greater than 0.1m, the mixed error drive control law is adopted: When the lateral error e y When the distance is ≤0.1m, it switches to pure angle tracking mode and controls the residual position error with a small linear velocity compensation. The calculated desired linear velocity and angular velocity are then sent to the robot for solution and drive the robot to move.

Images