Voice interactive CT robot patient bed cooperation method

Abstract

The application provides a voice interactive CT robot sickbed cooperation method, based on an environment grid map, an initial global path is planned by using an improved algorithm, a cost function is fused with a geometric distance heuristic item to a target point, a risk potential field value in inverse proportion to a distance from an obstacle, and a path turning penalty item, a local trajectory is optimized by using a TEB algorithm, the robot is represented as a plurality of safe coverage circles, under the conditions of meeting space-time and kinematics constraints, a cost function containing a trajectory jerk smoothness penalty item and an obstacle safety gap item is minimized, when approaching the target, a cost function weight is reconstructed, a pose accuracy and an end zero speed requirement are enhanced, a time optimization target is weakened, if it is detected that a local trajectory and a global path are not consistent in a topological homotopy category around an obstacle, and a cost value exceeds a blocking threshold value, a risk potential field value is temporarily increased around the obstacle, a re-planning is triggered from a path point before topological deviation, and path reliability and environment adaptability are ensured.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mobile CT robots, and particularly relates to a voice interactive CT robot bed cooperation method. BACKGROUND

[0002] The mobile CT robot can autonomously navigate in a complex environment such as a hospital ward or a corridor to move beside a target bed. Bed cooperation means that the voice interactive CT robot moves beside the bed according to voice or order. At present, the mobile robot navigation system generally adopts a hierarchical planning architecture, that is, a global path planner is responsible for generating a macroscopic path from the starting point to the ending point, and a local path planner dynamically tracks and optimizes the global path according to real-time sensor information and kinematic constraints to realize real-time obstacle avoidance. However, the The cost function design of the algorithm is relatively single, usually only considering the path length, which leads to the planned path often being close to the obstacle, bringing a high collision risk to the bulky CT robot; the connection of the generated path points is harsh, containing a large number of sharp turns, which requires the robot to frequently accelerate, decelerate and turn in place when tracking, not only reducing the running efficiency, but also affecting the stability of the motion, which may cause damage to the precision instruments inside the device. In the aspect of local path planning and execution, the time-elastic band (TEB) algorithm is an excellent online trajectory optimization method, which can generate a smooth trajectory that meets the robot dynamics and time constraints. However, the weight setting of the optimization target of the standard TEB algorithm is fixed, and it is difficult to balance the different needs of the whole navigation. For example, in the middle of the path, the execution efficiency may be more important, but when approaching the target point for accurate docking, the accuracy of the final pose and the stability of the zero end speed should be the primary goal, and when the local environment changes or there is a narrow channel, the local trajectory optimized by the TEB algorithm may choose a different detour direction around the obstacle than the global path, that is, a topological homotopy category deviation occurs. This deviation is easy to make the local planner fall into a high-cost dilemma or fail to find a feasible solution, and without effective coordination and recovery mechanism, the robot may hesitate near the obstacle, repeatedly oscillate or even completely block. SUMMARY

[0003] In order to improve the execution efficiency of the voice interactive CT robot bed cooperation work, the application provides a voice interactive CT robot bed cooperation method, which comprises:

[0004] An environment grid map is acquired, and a algorithm is used to generate an initial global path, and the The cost function of the algorithm comprises at least a heuristic term obtained according to the distance between the grid center and the target point, a risk potential value in inverse proportion to the distance between the grid center and the nearest obstacle, and a steering penalty value determined according to the variation of the path expansion direction;

[0005] Based on the initial global path, a local trajectory is optimized by using a time-elastic band algorithm, and in the optimization process, the robot body is represented as a plurality of safety coverage circles; an execution trajectory is generated by minimizing a cost function of the time-elastic band algorithm under the premise of meeting time and kinematics constraints, wherein the cost function of the time-elastic band algorithm comprises a smoothness term for punishing the speed and acceleration of the trajectory and a gap term for maintaining a safe distance between the robot and the obstacle; when the predicted trajectory enters the terminal guidance area of the target pose, the weights of the cost function of the time-elastic band algorithm are reconstructed, the weights related to the pose accuracy and the terminal zero speed are increased, and the weight related to the path execution time is reduced;

[0006] When the local trajectory generated by the time-elastic band algorithm is inconsistent with the topological homotopy category of the initial global path around the obstacle, and the value of the local trajectory is higher than the blocking threshold, the risk potential value of the obstacle peripheral area on the grid map is temporarily increased, and the path point before the topological deviation occurs is triggered once path re-planning.

[0007] Optionally, the The cost function of the algorithm is wherein n is the current grid;

[0008] g(n) is the cumulative actual cost from the starting point to the current grid n, which is obtained by accumulating the movement cost of each step on the path; the movement cost of each step comprises a risk potential value in inverse proportion to the distance between the grid center and the nearest obstacle, a steering penalty value related to the path deflection angle, and a geometric distance between adjacent grids;

[0009] h(n) is the Euclidean distance between the current grid center and the target point.

[0010] Optionally, the robot body is represented as a plurality of safety coverage circles, comprising:

[0011] The robot body is represented as a combination of safety coverage circles with at least two different radii, thereby adapting to the asymmetric geometric shape and motion characteristics of the robot.

[0012] Optionally, the cost function of the time-elastic band algorithm comprises a smoothness term for punishing the speed and acceleration of the trajectory and a gap term for maintaining a safe distance between the robot and the obstacle, comprising:

[0013] The gap term is that when the distance d between any safety coverage circle of the robot and the nearest obstacle is less than a preset minimum safety distance a penalty value is generated, the penalty value being a monotonically increasing function of the distance exceeded by the trajectory;

[0014] The smoothness term punishes the non-smoothness of the trajectory by integrating or weighted summing the velocity and acceleration of the trajectory.

[0015] Optionally, the terminal guiding area is an area in which the geometric distance between the current pose of the robot and the target pose is less than a preset terminal distance threshold.

[0016] When entering the terminal guiding area, the coefficient value of the weight term related to the pose error and the terminal velocity 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 decreased.

[0017] Optionally, the blocking threshold is proportional to the cumulative risk value of the corresponding local trajectory segment on the initial global path.

[0018] When the total risk value of the local trajectory generated by the timing elastic band algorithm exceeds the blocking threshold, the re-planning condition is met.

[0019] Optionally, the risk potential value of the obstacle peripheral area on the grid map is temporarily increased once from the path point before the topological deviation occurs path re-planning, comprising:

[0020] Identifying the obstacle causing the topological deviation, multiplying the risk potential value of the obstacle and the grid in the adjacent specified range by a preset penalty factor;

[0021] The re-planning starts from the path point before the topological deviation occurs or the adjacent safe path point.

[0022] The present application counts the risk potential and the steering penalty in the cost function of the algorithm The generated global path can actively avoid potential collision risks and reduce unnecessary sharp turns from the source. In the local trajectory optimization phase, in view of the accurate docking requirement at the end of the navigation task, the weight of the TEB cost function is reconstructed to focus on improving the accuracy of the final pose and the stability of the terminal stop, ensuring that the medical device can be accurately positioned. Moreover, when the local planning produces a detour inconsistent with the global path due to environmental constraints and falls into a high-cost dilemma, the corresponding area can be increased in traffic cost and path re-planning can be triggered once to solve the blocking, shock or navigation failure problem that may occur to the robot, thereby enhancing the efficiency of the voice interactive CT robot in the bed cooperative work. BRIEF DESCRIPTION OF DRAWINGS

[0023] Figure 1 is a flowchart of a specific embodiment;

[0024] Figure 2 is a schematic diagram of risk potential field;

[0025] Figure 3 is a schematic diagram of double coverage circle;

[0026] Figure 4 is a schematic diagram of terminal guidance mechanism;

[0027] Figure 5 is a schematic diagram of risk potential field change. DETAILED DESCRIPTION

[0028] In order to make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present application.

[0029] The plurality in the present application refers to two or more. In addition, it should be understood that in the description of the present application, the words "first", "second", etc. are only used for the purpose of distinguishing the description, and cannot be understood as indicating or implying relative importance, nor can it be understood as indicating or implying order.

[0030] In specific embodiments, the present application proposes a voice interactive CT robot sickbed cooperation method, as shown in Figure 1 , which comprises:

[0031] S1, acquiring an environment grid map, using algorithm to generate an initial global path, wherein The cost function of the algorithm at least includes a heuristic term obtained according to 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 turning penalty value determined according to the path expansion direction change amount;

[0032] A two-dimensional grid map of the environment is established by a laser radar or a depth camera. An improved algorithm is used for global path search, and the 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) adopts 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, wherein the single-step cost from the parent node p to the child node n is composed of three parts: one is the movement cost between nodes; two is the risk potential field value obtained by the pre-computed distance transform field, which is inversely proportional to the distance from the center of the grid to the nearest obstacle, the closer the distance, the higher the cost value, and the relationship between the risk potential field and the obstacle is as shown in Figure 2 The third is the turning penalty value, which is obtained by comparing the vector from the grandparent node to the parent node p and the vector from the parent node p to the current node n. If the direction changes, a fixed penalty constant is applied, and if the direction does not change, the penalty value is zero.

[0033] S2, based on the initial global path, a local trajectory optimization is performed using a time-elastic band algorithm, in which the robot body is represented as a plurality of safety covering circles; an execution trajectory is generated by minimizing a cost function of the time-elastic band algorithm under the premise of satisfying time and kinematics constraints, the cost function of the time-elastic band algorithm including a smoothness term for penalizing trajectory speed and acceleration, and a gap term for maintaining a safe distance between the robot and the obstacle; when the predicted trajectory enters the terminal guidance region of the target pose, the weights of the cost function of the time-elastic band algorithm are reconstructed, the weights related to pose accuracy and terminal zero speed are increased, and the weight related to path execution time is reduced;

[0034] In The global path points generated by the algorithm are used as the initial trajectory guide of the time-elastic band TEB algorithm. In the optimization process, the rectangular profile of the CT robot is abstracted as two or more overlapping circles in front and back to simplify the collision detection calculation. The TEB algorithm solves a cost function composed of multiple cost objectives weighted through a g2o graph optimization framework. The cost function includes a smoothness term for penalizing sharp changes in the trajectory by calculating the changes in velocity and acceleration between points on the trajectory to ensure smooth motion, and a gap term whose cost value is inversely proportional to the distance from the robot covering circle to the nearest obstacle to maintain a safe distance. When the robot is less than a preset distance threshold, for example, 1.5 meters, from the final target point, it is determined to enter the terminal guidance region. At this time, the weights of the cost function are adjusted, the weight coefficients of the target point pose error term and the terminal speed error term are increased by an order of magnitude, and the weight coefficient of the penalty path execution time is reduced by an order of magnitude, so that the robot switches from pursuing travel efficiency to prioritizing accurate and smooth berthing.

[0035] S3, when the local trajectory generated by the timing elastic band algorithm is inconsistent with the initial global path in the homotopy class of topology around the obstacle, and the value of the local trajectory is higher than the blocking threshold, temporarily increase the risk potential field value of the obstacle surrounding area on the grid map, trigger once from the path point before the topological deviation occurs path re-planning.

[0036] In the navigation process, the local trajectory generated by TEB and the corresponding global path segment are continuously detected for the topological relationship around the obstacle. 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 homotopy class. If it is determined that they are inconsistent, for example, the global path planning is from the left side of the obstacle, TEB selects the right side, and at this time the total value calculated by the TEB algorithm exceeds the blocking threshold value calibrated according to the experiment, the recovery strategy is started. The obstacle causing the topological deviation is identified, and the obstacle is removed from the global cost map used by the algorithm. The risk potential field value of the grid within a certain range around the obstacle is multiplied by a larger penalty coefficient, for example, 10. A safe distance is backtracked from the current robot position along the original global path, and the backtracking point is selected as the new starting point. The updated high-cost cost map is used to re-execute the TEB algorithm to generate a new global path that can bypass the high-risk area and guide the robot out of the blocked state.

[0037] In an optional embodiment, the cost function of the algorithm is where n is the current grid;

[0038] g(n) is the cumulative actual cost from the starting point to the current grid n, which is obtained by accumulating the movement cost of each step on the path. The movement cost of each step includes the risk potential field 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;

[0039] h(n) is the Euclidean distance from the current grid center to the target point.

[0040] ​​​When the robot plans a path, the cost of each potential next step n in the grid map is calculated. Suppose the cumulative cost of moving from the start to the previous grid n is 15.0, and now consider moving to n. The cost of moving to n is calculated as 2.75, where the center of grid n is 0.8 meters away from the nearest shelf, the risk potential value is set to 1.25, a turning penalty of 0.5 is applied because this move involves a 45 degree turn compared to the previous step, and the geometric distance is 1.0 if n is a neighboring grid. The total cumulative actual cost g(n) from the start to grid n is then updated to 17.75. The heuristic h(n) is the straight-line distance from the center of grid n to the final goal, which is assumed to be 20.5 meters. The total cost f(n) of grid n is then g(n) plus h(n) which equals 38.25. The grid with the lowest f(n) is selected for expansion in each iteration, The algorithm can preferentially explore areas that are both close to the start and close to the goal, far from obstacles, and have smooth paths, finding the optimal global path.

[0041] In an optional embodiment, the robot body is represented as a plurality of safety coverage circles, comprising:

[0042] The robot body is represented as a combination of at least two safety coverage circles of different radii, thereby accommodating the asymmetric geometry and motion characteristics of the robot.

[0043] 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 is used to completely enclose the robot, a large amount of unnecessary safety clearance would be left on the sides of the robot, preventing it from passing through some channels that are actually wide enough. To more accurately fit the shape of the robot, two coverage circles can be used to represent the robot, Figure 3 A diagram of two safety coverage circles. For example, a large circle with a radius of 0.7 meters covers the front half of the robot, 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.

[0044] The double-circle model is more accurate when performing collision detection, for example when the robot needs to make a large-angle turn. The area swept by the front of the robot is much larger than the area swept by the rear of the robot. Placing the larger coverage circle at the front of the robot more accurately reflects the space occupied by the robot when turning, ensuring safety when turning in a narrow space. When driving straight through a narrow channel, the double-circle model allows the robot to use its narrow body to get closer to the obstacle, as long as neither coverage circle collides with the obstacle.

[0045] 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 gap term for maintaining a safe distance between the robot and obstacles, including:

[0046] The gap term is defined as the distance d between any safe coverage circle of the robot and the nearest obstacle being less than a preset minimum safe distance. At that time, a penalty value is generated, which is related to the distance exceeded. A monotonically increasing function;

[0047] The smoothness term penalizes the non-smoothness of the trajectory by integrating or weighting the velocity and acceleration of the trajectory.

[0048] The cost function here is used to optimize the local trajectory. For the gap term, a minimum safe distance is assumed. The distance is 0.4 meters. When the robot moves, the distance d between the circle covering its front end and the wall is 0.6 meters. At this time, d is greater than... The penalty value for the gap term is 0. If the robot continues to approach the wall, the distance d decreases to 0.25 meters, which is less than the minimum safe distance and exceeds the limit by 0.15 meters. At this point, a penalty value is activated, the size of which can be the square of the excess distance multiplied by a coefficient. The penalty value will prompt the optimizer to adjust the trajectory, moving the robot away from the wall.

[0049] The smoothness term focuses on the comfort and mechanical losses of robot movement. It primarily penalizes changes in acceleration, i.e., impact. For example, a trajectory might require the robot to suddenly change from uniform acceleration to uniform deceleration at a certain point in time, resulting in a large acceleration value. Another trajectory, however, gradually changes acceleration through a smooth transition zone. By weighted summing the acceleration values ​​at all points along the entire trajectory, the former will have a much higher smoothness score than the latter. In minimizing the total cost, the optimizer will naturally tend to choose the trajectory with smaller acceleration, ensuring smooth robot operation and avoiding abrupt movements and jitter.

[0050] In an optional embodiment, the terminal guidance area is the area where the geometric distance between the robot's current pose and the target pose is less than a preset terminal distance threshold.

[0051] Upon entering the terminal guidance area, the coefficient values ​​of the weight terms related to pose error and end-effector velocity in the cost function of the time-series elastic band algorithm are increased, while the coefficient values ​​of the weight terms related to path execution time are decreased.

[0052] Suppose a robot needs to dock precisely 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 away from the center of the bed, the main goal of the path planner is to quickly and safely reach the vicinity of the target, and the weight coefficient of the path execution time can be 2.0 and the weight coefficient of the pose error can be 0.5.

[0053] Once the robot enters the terminal guidance zone 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 jumps from 0.5 to 50.0, and the weight of the end velocity error also increases significantly, while the weight of the path execution time decreases from 2.0 to 0.1, as shown in Figure 4 The weight changes cause the path optimization algorithm to shift its primary focus from pursuing speed to pursuing extreme alignment accuracy and zero speed stop. The robot will thus slow down and make subtle position and attitude adjustments to ensure that its docking position and attitude error are as small as possible, successfully completing the docking task.

[0054] In an optional embodiment, the jamming threshold is proportional to the cumulative risk cost value of the corresponding local trajectory segment on the initial global path;

[0055] The re-planning condition is satisfied when the total cost of the local trajectory generated by the time-elastic band algorithm exceeds the jamming threshold.

[0056] For example, the initial In the global path planned by the algorithm, there is a 5-meter-long path segment that passes through a wide corridor, and the cumulative risk cost value of this segment is relatively low, such as 20. According to this risk value, a proportional jamming threshold is set. When the robot is traveling along this segment, a pedestrian suddenly appears in front of it. The TEB algorithm generates a local trajectory to avoid the pedestrian, and the cost of this local trajectory is calculated to be 45. Since 45 exceeds the dynamic threshold of 36, it is determined that local adjustment is not sufficient to solve the problem, and there may be a serious jam, so global path re-planning is immediately triggered.

[0057] In contrast, another segment of the global path needs to pass through a narrow area filled with static shelves. The cumulative risk cost value of this 5-meter-long path segment is already high, such as 150. The corresponding dynamic jamming threshold is 270. In the narrow area, even if the TEB generates a local trajectory to avoid a small obstacle with a cost of 200, since it does not exceed the threshold of 270, it is considered to be within the expected difficult passage range, and local planning is effective, so global re-planning is not triggered. This avoids unnecessary global re-planning due to minor disturbances in an already complex area.

[0058] In an optional embodiment, the temporarily increasing the risk potential field value of the obstacle perimeter region on the grid map is triggered once from the path point before the topological deviation occurs path re-planning, comprising:

[0059] identifying the obstacle causing the topological deviation, multiplying the risk potential field value of the obstacle and the grids within a specified range adjacent to the obstacle by a preset penalty factor;

[0060] The re-planning starts from the path point before the topological deviation occurs or a safe path point adjacent thereto.

[0061] Suppose the global path planner robot passes from the right side of a column. But when the robot approaches, it finds that the right side is blocked by a temporarily placed cleaning vehicle, and the TEB local planner can only generate a trajectory that detours from the left side of the column. It is detected that this local trajectory crosses the obstacle column compared with the global path, and a topological deviation occurs. The column is identified as the core obstacle causing the deviation.

[0062] On the internal cost map, the risk values of the grid representing the column and all the grids within a range of 0.6 meters around the column are temporarily multiplied by a penalty factor, such as 8.0, as shown in Figure 5 In the next few seconds, the path cost from the right side of the column will become extremely high. The global re-planning is started, but the starting point is not the current position of the robot, but a path point back to before the topological deviation occurs, for example, a point 2 meters in front of the column. From this safe point, the new planning naturally selects a new path that detours from the left side due to the high cost on the right side of the column, generating a global path that is consistent with the current actual situation and conflict-free.

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

[0064] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing 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, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0065] The method and electronic device for providing product object information provided in this application have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the method and its core ideas. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A voice interactive CT robot patient bed cooperation method, characterized in that, The method comprises the following steps: Obtain an environmental raster map and utilize The algorithm generates an initial global path, the The cost function of the algorithm includes at least a heuristic term obtained from the distance between the grid center and the target point, a risk potential field value that is inversely proportional to the distance from the grid center to the nearest obstacle, and a turning penalty value determined based on the change in the path expansion direction. based on the initial global path, a local trajectory is optimized by using a time elastic band algorithm, the robot body is represented as a plurality of safety coverage circles in the optimization process; an execution trajectory is generated by minimizing a cost function of the time elastic band algorithm under the premise of meeting time and kinematics constraints, the cost function of the time elastic band algorithm comprises a smoothness term for punishing the speed and acceleration of the trajectory, and a gap term for keeping a safe distance between the robot and the obstacle; when the predicted trajectory enters a terminal guidance area of the target pose, the weights of the cost function of the time elastic band algorithm are reconstructed, the weights related to the pose accuracy and the terminal zero speed are increased, and the weight related to the path execution time is reduced; temporarily increase the risk potential field value of the obstacle surrounding area on the grid map, and trigger once from the path point before the topological deviation occurs path re-planning.

2. The method of claim 1, wherein, 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, which is obtained by accumulating the movement cost of each step on the path; the movement cost of each step comprises a risk potential field value inversely proportional to the distance from the center of the grid to the nearest obstacle, a turning penalty value related to the path deflection angle, and a geometric distance between adjacent grids; h(n) is the Euclidean distance from the center of the current grid to the target point.

3. The method of claim 1, wherein, The robot body is represented as a plurality of safety coverage circles, comprising: The robot body is represented as a combination of safety coverage circles with at least two different radii, thereby adapting to the asymmetric geometric shape and motion characteristics of the robot.

4. The method of claim 1, wherein, The cost function of the time elastic band algorithm comprises a smoothness term for punishing the speed and acceleration of the trajectory, and a gap term for keeping a safe distance between the robot and the obstacle, comprising: The gap term is a penalty value generated when the distance d of any safety coverage circle of the robot to the nearest obstacle is less than a preset minimum safety distance The penalty value is a monotonically increasing function of the distance exceeded. The smoothness term punishes the non-smoothness of the trajectory by integrating or weighted summing the speed and acceleration of the trajectory.

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

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

7. The method of claim 1, wherein, The risk potential field value of the obstacle peripheral region on the grid map is temporarily increased once triggered from the path point before the topological deviation occurs Path re-planning, comprising: The obstacle causing the topological deviation is identified, and the risk potential field value of the obstacle and the grid in the adjacent specified range is multiplied by a preset penalty factor; The replanning starts from the path point before the topological deviation occurs.

Images