Voice interactive CT robot sickbed cooperation method

Abstract

The invention provides a voice interactive CT robot sickbed cooperation method, which is based on an environment grid map, adopts an improved algorithm to plan an initial global path, fuses a geometric distance heuristic term to a target point, a risk potential field value inversely proportional to an obstacle distance and a path turning penalty term in a cost function, and performs local trajectory optimization by utilizing a TEB algorithm, so as to improve the robustness of the system. The method comprises the following steps: representing a robot as a plurality of safety coverage circles, minimizing a cost function comprising a trajectory jerk smoothness penalty term and an obstacle safety gap term under the condition of satisfying space-time and kinematics constraints, reconstructing the weight of the cost function, enhancing the pose precision and tail end zero speed requirement, weakening a time optimization target and solving the problem that the robot is close to a target when the target is close to the target. And if it is detected that the topological homotopy categories of the local trajectory and the global path around the obstacle are inconsistent and the cost value exceeds a blocking threshold value, a risk potential field value is temporarily increased around the obstacle, re-planning is triggered from path points before topological deviation, and the path reliability and the environmental adaptability are ensured.

Description

Technical Field

[0001] The present application belongs to the field of mobile CT robots, and in particular relates to a voice-interactive CT robot bed collaboration method. Background Art

[0002] Mobile CT robots can autonomously navigate and move to the target bed in complex environments such as hospital wards and corridors. Bed collaboration means that the voice-interactive CT robot can move to the bedside according to voice or queues. Currently, mobile robot navigation systems generally adopt a hierarchical planning architecture, that is, the global path planner is responsible for generating a macro path from the starting point to the end point, and the local path planner dynamically tracks and optimizes the global path based on real-time sensor information and kinematic constraints to achieve real-time obstacle avoidance. In terms of global path planning, the main method used is algorithm, but The algorithm's cost function design is relatively simplistic, typically considering only path length. This results in the planned paths often closely following obstacles, posing a high collision risk for large CT robots. The generated pathpoints are abruptly connected and contain numerous sharp turns, requiring the robot to frequently accelerate, decelerate, and turn on the spot while tracking. This not only reduces operational efficiency but also affects motion smoothness, potentially damaging delicate instruments within the device. At the local path planning and execution level, the Timed Elastic Band (TEB) algorithm is an excellent online trajectory optimization method that generates smooth trajectories that satisfy the robot's dynamic and temporal constraints. However, the standard TEB algorithm uses fixed weights for its optimization objectives, making it difficult to balance the diverse requirements of the entire navigation process. For example, efficiency may be more important mid-path, while accuracy of the final pose and stability of zero terminal velocity should be paramount when approaching the target for precise docking. Furthermore, when the local environment changes or narrow passages exist, the local trajectory optimized by the TEB algorithm may choose a different detour around obstacles than the global path, resulting in a deviation from the topological homotopy category. This deviation can easily cause the local planner to fall into an extremely costly dilemma or be unable to find a feasible solution. Without an effective coordination and recovery mechanism, the robot may hesitate, oscillate repeatedly, or even be completely blocked near obstacles. Summary of the Invention

[0003] In order to improve the execution efficiency of voice-interactive CT robot bed collaborative operations, this application proposes a voice-interactive CT robot bed collaborative method, including: Get the environment grid map and use The algorithm generates an initial global path, The cost function of the algorithm includes at least a heuristic term obtained based on the distance between the grid center and the target point, a risk potential field value inversely proportional to the distance from the grid center to the nearest obstacle, and a steering penalty value determined by the change in the path extension direction; Based on the initial global path, a temporal elastic band algorithm is used to perform local trajectory optimization. The optimization process represents the robot body as multiple safety cover circles. An execution trajectory is generated by minimizing a cost function of the temporal elastic band algorithm while satisfying time and kinematic constraints. The cost function of the temporal elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a clearance term for maintaining a safe distance between the robot and obstacles. When the predicted trajectory enters the terminal guidance area of ​​the target posture, the weight of the cost function of the temporal elastic band algorithm is reconstructed, increasing the weights related to posture accuracy and terminal zero velocity, and reducing the weight related to path execution time. When the local trajectory generated by the temporal elastic band algorithm is inconsistent with the topological homotopy category of the initial global path around the obstacle, and the cost value of the local trajectory is higher than the blocking threshold, the risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a topological deviation is triggered from the path point before the topological deviation occurs. Path replanning.

[0004] Optionally, the The cost function of the algorithm is , where n is the current grid; g(n) is the cumulative actual cost from the starting point to the current grid n, obtained by accumulating the movement cost of each step on the path. The movement cost of each step includes the risk potential value inversely proportional to the distance from the grid center to the nearest obstacle, the steering penalty value related to the path deflection angle, and the geometric distance between adjacent grids. h(n) is the Euclidean distance from the current grid center to the target point.

[0005] Optionally, representing the robot body as a plurality of safety coverage circles includes: The robot body is represented as a combination of at least two safety coverage circles of different radii, thereby adapting to the asymmetric geometry and movement characteristics of the robot.

[0006] Optionally, the cost function of the temporal elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a clearance term for maintaining a safe distance between the robot and obstacles, including: The gap term is when the distance d between any safety coverage circle of the robot and the nearest obstacle is less than the preset minimum safety distance When a penalty value is generated, the penalty value is about the distance exceeded A monotonically increasing function of The smoothness term penalizes the non-smoothness of the trajectory by integrating or weighted summing the velocity and acceleration of the trajectory.

[0007] Optionally, the terminal guidance area is an area where the geometric distance between the robot's current posture and the target posture is less than a preset terminal distance threshold; After entering the terminal guidance area, the coefficient value of the weight term related to the posture error and the terminal speed in the cost function of the timing elastic band algorithm is increased, and the coefficient value of the weight term related to the path execution time is reduced.

[0008] Optionally, the blocking threshold is proportional to the cumulative risk cost value of the corresponding local trajectory segment on the initial global path; When the total cost value of the local trajectory generated by the temporal elastic band algorithm exceeds the blocking threshold, the replanning condition is met.

[0009] Optionally, the risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a trigger is triggered once from the path point before the topological deviation occurs. Path replanning, including: Identify obstacles that cause topological deviation, and multiply the risk potential field values ​​of the obstacles and the grids within the specified range by a preset penalty factor; The replanning starts from the path point before the topology deviation occurs or an adjacent safe path point.

[0010] The present invention proposes The algorithm's cost function takes into account risk potential fields and steering penalties. The generated global path can proactively avoid potential collision risks and reduce unnecessary sharp turns at the source. In the local trajectory optimization stage, the algorithm focuses on improving the accuracy of the final posture and the stability of the end stop by reconstructing the weights of the TEB cost function to meet the precise docking requirements at the end of the navigation task, ensuring that the medical equipment can be accurately positioned. Moreover, when the local planning results in a detour inconsistent with the global path due to environmental constraints and falls into a high-cost dilemma, it can solve the possible blockage, oscillation or navigation failure problems of the robot by increasing the travel cost of the corresponding area and triggering a path replanning, thereby enhancing the efficiency of the voice-interactive CT robot in collaborative bed operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 is a flow chart of a specific embodiment; Figure 2 is a schematic diagram of the risk potential field; Figure 3 is a schematic diagram of a double covering circle; Figure 4 This is a schematic diagram of the terminal guidance mechanism; Figure 5Schematic diagram of the change of risk potential field. DETAILED DESCRIPTION

[0012] To make the purpose, technical solutions, and advantages of the embodiments of this application more clear, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the drawings in the embodiments of this application. Obviously, the described embodiments are part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0013] In this application, the term "plurality" refers to two or more. In addition, it should be understood that in the description of this application, the terms "first" and "second" are used only for the purpose of distinguishing descriptions and should not be understood as indicating or implying relative importance or order.

[0014] In a specific embodiment, the present application proposes a voice interactive CT robot bed collaboration method, such as Figure 1 Shown, including: S1, obtain the environmental grid map, use The algorithm generates an initial global path, The cost function of the algorithm includes at least a heuristic term obtained based on the distance between the grid center and the target point, a risk potential field value inversely proportional to the distance from the grid center to the nearest obstacle, and a steering penalty value determined by the change in the path extension direction; Create a 2D grid map of the environment using LiDAR or depth cameras. The algorithm performs a global path search. The cost function F(n) of the algorithm is equal to the sum of the actual cost G(n) and the heuristic cost H(n). The heuristic cost H(n) uses the Euclidean distance from the center of the current grid n to the center of the target point. The actual cost G(n) is the cumulative cost from the starting point to the current grid n, where the single-step cost from the parent node p to the child node n is composed of three weighted parts: one is the movement cost between nodes; the other is the risk potential field value obtained by the pre-calculated distance transformation field. The risk potential field value is inversely proportional to the distance from the grid center to the nearest obstacle. The closer the distance, the higher the cost value. The relationship between the risk potential field and the obstacle is as follows: Figure 2 As shown; the third is the steering penalty value, by comparing the vector from the grandparent node to the parent node p with the vector from the parent node p to the current node n, if the direction changes, a fixed penalty constant is applied, if the direction remains unchanged, the penalty value is zero.

[0015] S2, based on the initial global path, using the time series elastic band algorithm to perform local trajectory optimization, the optimization process represents the robot body as multiple safety coverage circles; generating an execution trajectory by minimizing the cost function of the time series elastic band algorithm under the premise of satisfying time and kinematic constraints, the cost function of the time series elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a gap term for maintaining a safe distance between the robot and obstacles; when the predicted trajectory enters the terminal guidance area of ​​the target posture, the weight of the cost function of the time series elastic band algorithm is reconstructed, increasing the weight related to posture accuracy and terminal zero speed, and reducing the weight related to path execution time; by The global pathpoints generated by the algorithm serve as the initial trajectory guidance for the Timed Elastic Band (TEB) algorithm. During the optimization process, the CT robot's rectangular outline is abstracted into two or more overlapping circles to simplify collision detection calculations. The TEB algorithm, using a G2O graph optimization framework, solves a cost function composed of multiple weighted cost objectives. This cost function includes a smoothness term, which penalizes abrupt trajectory changes by calculating the change in velocity and acceleration between points on the trajectory to ensure smooth motion. It also includes a gap term, whose cost is inversely proportional to the distance from the robot's coverage circle to the nearest obstacle to maintain a safe distance. When the robot's distance from the final target point falls within a preset distance threshold, such as 1.5 meters, it is determined to have entered the terminal guidance area. At this point, the cost function weights are adjusted, increasing the weight coefficients for the target point pose error and the terminal velocity error by an order of magnitude, while reducing the weight coefficient for penalizing path execution time by an order of magnitude. This shifts the robot's focus from efficiency to ensuring accurate and smooth berthing.

[0016] S3: When the local trajectory generated by the temporal elastic band algorithm is inconsistent with the topological homology category of the initial global path around the obstacle, and the cost value of the local trajectory is higher than the blocking threshold, the risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a topological deviation is triggered from the path point before the topological deviation occurs. Path replanning.

[0017] During the navigation process, the topological relationship between the local trajectory generated by TEB and the corresponding global path segment with respect to the obstacle is continuously detected. By comparing the position relationship sequence of the two paths relative to the local obstacle reference point, it is determined whether they belong to the same homology class. If it is determined to be inconsistent, for example, the global path planning detours from the left side of the obstacle, and TEB chooses the right side, and the total cost value calculated by the TEB algorithm exceeds the blocking threshold calibrated according to the experiment, the recovery strategy is initiated. The obstacle that causes the topological deviation is identified and the On the global cost map used by the algorithm, the grid risk potential field value within a certain range around the obstacle is multiplied by a large penalty coefficient, such as 10. From the current robot position, trace back a safe distance along the original global path, select the backtracking point as the new starting point, and re-execute using the updated high-cost cost map. The algorithm generates a new global path that can bypass high-risk areas and guide the robot out of the blocked state.

[0018] In an optional embodiment, the The cost function of the algorithm is , where n is the current grid; g(n) is the cumulative actual cost from the starting point to the current grid n, obtained by accumulating the movement cost of each step on the path. The movement cost of each step includes the risk potential value inversely proportional to the distance from the grid center to the nearest obstacle, the steering penalty value related to the path deflection angle, and the geometric distance between adjacent grids. h(n) is the Euclidean distance from the current grid center to the target point.

[0019] When the robot plans its path, the cost is calculated for each potential next step grid n in the grid map. Assume that the cumulative cost of moving from the starting point to the previous grid of the current grid n is 15.0, and now consider moving to n. Calculate the cost of moving to n. If the center of grid n is 0.8 meters away from the nearest shelf, the risk potential field value can be set to 1.25; if this movement has a 45-degree turn compared to the previous step, a turning penalty of 0.5 can be imposed; if n is an adjacent grid, the geometric distance is 1.0. Then the movement cost of this step is 2.75. The total accumulated actual cost g(n) from the starting point to grid n is updated to 17.75. The heuristic term h(n) is the straight-line distance from the center of grid n to the final target point. Assume that the straight-line distance is 20.5 meters. The total cost f(n) of grid n is g(n) plus h(n), which is equal to 38.25. By selecting the grid with the smallest f(n) for expansion in each iteration, The algorithm can prioritize exploring those areas that are generally close to the starting point, close to the end point, far away from obstacles and have smooth paths, and find the optimal global path.

[0020] In an optional embodiment, the robot body is represented as a plurality of safety coverage circles, including: The robot body is represented as a combination of at least two safety coverage circles of different radii, thereby adapting to the asymmetric geometry and movement characteristics of the robot.

[0021] For a CT robot that is 1.5 meters long and 0.9 meters wide, using a single circle for collision detection would be very inaccurate. If a large circle with a radius of 0.85 meters were used to completely surround it, a large amount of unnecessary safety gaps would be left on the sides of the robot, preventing it from passing through some passages that are actually wide enough. In order to fit its shape more accurately, two covering circles can be used to represent it. Figure 3 A diagram of two safety circles. For example, a large circle with a radius of 0.7 meters covers the front half of the vehicle, including the drive wheels and the main body, and a small circle with a radius of 0.5 meters covers the relatively narrow rear half.

[0022] The dual-circle model provides more precise collision detection. For example, when a robot needs to make a sharp turn, the front of the vehicle sweeps a much larger area than the rear. Placing the larger coverage circle at the front of the vehicle more realistically reflects the space occupied during the turn, ensuring safety in tight spaces. When driving straight through narrow passages, the dual-circle model allows the robot to use its narrower body to get closer to obstacles, as long as neither coverage circle collides with the obstacle.

[0023] In an optional embodiment, the cost function of the temporal elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a clearance term for maintaining a safe distance between the robot and obstacles, including: The gap term is when the distance d between any safety coverage circle of the robot and the nearest obstacle is less than the preset minimum safety distance When a penalty value is generated, the penalty value is about the distance exceeded A monotonically increasing function of The smoothness term penalizes the non-smoothness of the trajectory by integrating or weighted summing the velocity and acceleration of the trajectory.

[0024] The cost function here is used to optimize the local trajectory. For the gap term, assuming the minimum safe distance set When the robot moves, the distance d between its front end and the wall is 0.6 meters. At this time, d is greater than , the penalty for the gap term is 0. If the robot continues to approach the wall, the distance d decreases to 0.25 meters, which is now less than the minimum safe distance and the excess distance is 0.15 meters. At this point, a penalty is activated, which is calculated by multiplying the square of the excess distance by a coefficient. This penalty prompts the optimizer to adjust the trajectory to keep the robot away from the wall.

[0025] The smoothness term focuses on the comfort and mechanical losses of the robot's motion. This term primarily penalizes changes in acceleration, or shock. For example, one trajectory might require the robot to abruptly change from uniform acceleration to uniform deceleration at a certain point in time, resulting in a large acceleration value. Another trajectory gradually changes acceleration through a smooth transition zone. By taking a weighted sum of the acceleration values ​​at each point along the entire trajectory, the smoothness term score for the former will be much higher than the latter. In the process of minimizing the total cost, the optimizer will naturally tend to choose the trajectory with the lowest acceleration, ensuring smooth robot operation and avoiding jerkiness and jitter.

[0026] In an optional embodiment, the terminal guidance area is an area where the geometric distance between the robot's current posture and the target posture is less than a preset terminal distance threshold; After entering the terminal guidance area, the coefficient value of the weight term related to the posture error and the terminal speed in the cost function of the timing elastic band algorithm is increased, and the coefficient value of the weight term related to the path execution time is reduced.

[0027] Suppose a robot needs to precisely dock in front of a hospital bed that is only 0.2 meters wider than the robot's body. The terminal distance threshold is set to 1.2 meters. When the robot is more than 1.2 meters from the center of the bed, its path planning objective is to quickly and safely reach the target. In this case, the weight coefficient for path execution time might be 2.0, and the weight coefficient for pose error might be 0.5.

[0028] Once the robot enters the terminal guidance area within 1.2 meters of the target, the control system immediately adjusts the weights in the cost function. The weight coefficient of the pose error increases sharply from 0.5 to 50.0, the weight of the terminal velocity error also increases significantly, and the weight of the path execution time decreases from 2.0 to 0.1. Figure 4 As shown in the figure, this weight change shifts the path optimization algorithm's priority from speed to extreme alignment accuracy and zero-speed stopping. The robot decelerates and makes subtle position and posture adjustments to minimize errors in its docking position and posture, ensuring successful docking.

[0029] In an optional embodiment, the blocking threshold is proportional to the cumulative risk cost value of the corresponding local trajectory segment on the initial global path; When the total cost value of the local trajectory generated by the temporal elastic band algorithm exceeds the blocking threshold, the replanning condition is met.

[0030] For example, initially The algorithm's planned global path includes a 5-meter-long section that passes through a wide corridor. Its cumulative risk cost is low, for example, 20. A proportional blocking threshold is set based on this risk value. While the robot is traveling along this section, if a pedestrian suddenly appears ahead, the TEB algorithm calculates a local trajectory cost of 45 to circumvent the pedestrian. Because 45 exceeds the dynamic threshold of 36, the algorithm determines that local adjustments are insufficient to resolve the problem and suggests a serious blocking situation. Therefore, global path replanning is immediately triggered.

[0031] In contrast, the other section of the global path needs to pass through a narrow area full of static shelves. The cumulative risk cost of this 5-meter path is already very high, for example, 150. The corresponding dynamic blocking threshold is 270. In this narrow area, even if the local trajectory cost generated by TEB to avoid small obstacles reaches 200, since it does not exceed the threshold of 270, it is considered that this is still within the expected difficult passage range, and the local planning is valid, so no global replanning is triggered. This avoids frequent and unnecessary global replanning due to small disturbances in an already complex area.

[0032] In an optional embodiment, the risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a time delay is triggered from the path point before the topological deviation occurs. Path replanning, including: Identify obstacles that cause topological deviation, and multiply the risk potential field values ​​of the obstacles and the grids within the specified range by a preset penalty factor; The replanning starts from the path point before the topology deviation occurs or an adjacent safe path point.

[0033] Suppose a robot's global path planning involves passing to the right of a pillar. However, as the robot approaches, it discovers that the right side is blocked by a temporarily placed cleaning cart. The TEB local planner can only generate a trajectory that circumvents the pillar's left side. This local trajectory is detected as a topological deviation from the global path, crossing the pillar. The pillar is identified as the core obstacle causing this deviation.

[0034] On the internal cost map, the risk values ​​of the grid representing the pillar and all grids within 0.6 meters around it are temporarily multiplied by a penalty factor, such as 8.0, as shown in Figure 5 As shown. In the next few seconds, the cost of the path to the right of the pillar will become extremely high. Start global replanning, but the starting point is not the current position of the robot, but a path back to the point before the topological deviation occurred, such as a point 2 meters in front of the pillar. Starting from this safe point, the new The planner will naturally choose a new path around the left side due to the high cost on the right side of the pillar, generating a conflict-free global path that is consistent with the current actual situation.

[0035] Through the description of the above embodiments, it can be seen that those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a software product, which can be stored in a storage medium such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in various embodiments of the present application or certain parts of the embodiments.

[0036] Each embodiment in this specification is described in a progressive manner. The same or similar parts between the embodiments can be referred to each other. Each embodiment focuses on the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple. For relevant parts, refer to the partial description of the method embodiment. The system and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without expending creative work.

[0037] The above describes in detail the method and electronic device for providing commodity object information provided by this application. Specific examples are used herein to illustrate the principles and implementation methods of this application. The description of the above embodiments is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the contents of this specification should not be construed as limiting this application.

Claims

1. A voice-interactive CT robot bed collaboration method, characterized in that: include: Get the environment grid map and use The algorithm generates an initial global path, The cost function of the algorithm includes at least a heuristic term obtained based on the distance between the grid center and the target point, a risk potential field value inversely proportional to the distance from the grid center to the nearest obstacle, and a steering penalty value determined by the change in the path extension direction; Based on the initial global path, a temporal elastic band algorithm is used to perform local trajectory optimization. The optimization process represents the robot body as multiple safety cover circles. An execution trajectory is generated by minimizing a cost function of the temporal elastic band algorithm while satisfying time and kinematic constraints. The cost function of the temporal elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a clearance term for maintaining a safe distance between the robot and obstacles. When the predicted trajectory enters the terminal guidance area of ​​the target posture, the weight of the cost function of the temporal elastic band algorithm is reconstructed, increasing the weights related to posture accuracy and terminal zero velocity, and reducing the weight related to path execution time. When the local trajectory generated by the temporal elastic band algorithm is inconsistent with the topological homotopy category of the initial global path around the obstacle, and the cost value of the local trajectory is higher than the blocking threshold, the risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a topological deviation is triggered from the path point before the topological deviation occurs. Path replanning.

2. The method according to claim 1, characterized in that described The cost function of the algorithm is , where n is the current grid; g(n) is the cumulative actual cost from the starting point to the current grid n, obtained by accumulating the movement cost of each step on the path. The movement cost of each step includes the risk potential value inversely proportional to the distance from the grid center to the nearest obstacle, the steering penalty value related to the path deflection angle, and the geometric distance between adjacent grids. h(n) is the Euclidean distance from the current grid center to the target point.

3. The method according to claim 1, characterized in that The robot body is represented as a plurality of safety coverage circles, including: The robot body is represented as a combination of at least two safety coverage circles of different radii, thereby adapting to the asymmetric geometry and movement characteristics of the robot.

4. The method according to claim 1, wherein The cost function of the temporal elastic band algorithm includes a smoothness term for penalizing trajectory velocity and acceleration, and a clearance term for maintaining a safe distance between the robot and obstacles, including: The gap term is when the distance d between any safety coverage circle of the robot and the nearest obstacle is less than the preset minimum safety distance When a penalty value is generated, the penalty value is about the distance exceeded A monotonically increasing function of The smoothness term penalizes the non-smoothness of the trajectory by integrating or weighted summing the velocity and acceleration of the trajectory.

5. The method according to claim 1, wherein The terminal guidance area is an area where the geometric distance between the robot's current posture and the target posture is less than a preset terminal distance threshold; After entering the terminal guidance area, the coefficient value of the weight term related to the posture error and the terminal speed in the cost function of the timing elastic band algorithm is increased, and the coefficient value of the weight term related to the path execution time is reduced.

6. The method according to claim 1, characterized in that The blocking threshold is proportional to the cumulative risk cost value of the corresponding local trajectory segment on the initial global path; When the total cost value of the local trajectory generated by the temporal elastic band algorithm exceeds the blocking threshold, the replanning condition is met.

7. The method according to claim 1, characterized in that The risk potential field value of the area around the obstacle is temporarily increased on the grid map, and a path point before the topological deviation occurs is triggered once Path replanning, including: Identify obstacles that cause topological deviation, and multiply the risk potential field values ​​of the obstacles and the grids within the specified range by a preset penalty factor; The replanning starts from the path point before the topology deviation occurs.

Images