Robot trajectory tracking and obstacle avoidance control method, system, equipment and medium

Abstract

The invention provides a robot trajectory tracking and obstacle avoidance control method, system and device and a medium, and belongs to the field of intelligent robot control, and the method comprises the steps: obtaining motion state information, obstacle information and a reference trajectory; calculating a look-ahead distance based on preset prediction time, a safety coefficient and a current maximum speed in the motion state information; selecting a locally planned target point from the reference trajectory according to the look-ahead distance; based on a dynamic window algorithm, generating a plurality of candidate tracks according to the motion state information and the obstacle information; based on the multi-target cost function and the target point, evaluating each candidate track to obtain a comprehensive cost value; and determining the candidate trajectory with the minimum comprehensive cost value as an optimal trajectory, obtaining an expected linear velocity and an expected angular velocity based on the optimal trajectory, and generating a control instruction to drive the robot to move along the optimal trajectory. According to the invention, the motion stability, path continuity and overall robustness of the robot in a continuous multi-section inspection task are improved.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot control, specifically to a robot trajectory tracking and obstacle avoidance control method, system, device, and medium. Background Technology

[0002] In fixed-route applications such as industrial inspection and security patrol, mobile robots need to strictly follow predetermined paths to ensure the integrity of the inspection coverage, while also possessing the ability to avoid sudden dynamic obstacles in the environment in real time. Therefore, how to enable robots to achieve stable and smooth real-time dynamic obstacle avoidance while ensuring high-precision trajectory tracking has become a core technical challenge for improving the reliability and efficiency of inspection tasks.

[0003] To address these challenges, existing technologies have proposed various path planning and control methods. For example, in specific fields such as navigational aid inspection and orchard operations, existing solutions employ path tracking based on predefined waypoints combined with reactive obstacle avoidance strategies, or utilize multi-sensor information fusion for environmental modeling and local path replanning. These methods have, to some extent, optimized path execution efficiency and improved obstacle avoidance capabilities.

[0004] However, these existing solutions still have significant limitations: First, it is difficult to effectively balance trajectory tracking accuracy and real-time dynamic obstacle avoidance, especially in scenarios with multiple connecting road segments or complex obstacles, where trajectory deviation, motion jitter, or decision-making stagnation are prone to occur. Second, there is a lack of smooth transition mechanisms for continuous path switching, resulting in discontinuous trajectories during road segment transitions and affecting operational stability. Third, in extremely congested or deadlocked scenarios, there is a lack of effective autonomous escape strategies, making the robot prone to getting stuck in local predicaments and causing task interruption. Therefore, there is an urgent need for a control system that can deeply integrate accurate tracking and flexible obstacle avoidance, and possess stronger environmental adaptability and robustness. Summary of the Invention

[0005] In view of this, it is necessary to provide a robot trajectory tracking and obstacle avoidance control method, system, device and medium to solve the technical problems existing in the prior art that make it difficult to achieve real-time and smooth dynamic obstacle avoidance while ensuring high-precision trajectory tracking, and that trajectory deviation, unsmooth switching or stagnation are prone to occur in complex scenarios.

[0006] To address the aforementioned technical problems, in a first aspect, the present invention provides a robot trajectory tracking and obstacle avoidance control method, comprising: Acquire motion status information, obstacle information, and reference trajectory; The look-ahead distance is calculated based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information. A target point for local planning is selected from the reference trajectory based on the forward distance; Based on the dynamic window algorithm, multiple candidate trajectories are generated according to the motion state information and the obstacle information; The lateral error is determined based on the motion state information, and the comprehensive cost value of each candidate trajectory is evaluated based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The candidate trajectory with the minimum comprehensive cost value is determined as the optimal trajectory, and the expected linear velocity and expected angular velocity are obtained based on the optimal trajectory. Based on the desired linear velocity and desired angular velocity, control commands are generated to drive the robot to move along the optimal trajectory.

[0007] In one possible implementation, the generation of multiple candidate trajectories based on the dynamic window algorithm according to the motion state information and the obstacle information includes: Based on the motion state information and the obstacle information, a dynamic window composed of the linear velocity range and the angular velocity range is determined; wherein, the boundary of the linear velocity range and the angular velocity range is determined by at least one of the following constraints: kinematic constraints based on the motion state information, dynamic constraints based on the robot, and safe distance constraints based on the obstacle information; Multiple candidate velocities are obtained by sampling velocities within the dynamic window range; Based on the candidate velocities and the current position and heading in the motion state information of each group, the multiple candidate trajectories are simulated and generated.

[0008] In one possible implementation, determining the lateral error based on the motion state information, and evaluating the comprehensive cost of each candidate trajectory based on a multi-objective cost function combining the lateral error, the obstacle information, and the target point, includes: Based on the motion state information, the lateral error, and the obstacle information, calculate the target distance cost between each candidate trajectory and the target point, the obstacle distance cost between each candidate trajectory and the obstacle, and the lateral error cost relative to the reference trajectory. Based on preset weighting coefficients, the target distance cost, obstacle distance cost, and lateral error cost are weighted and summed to obtain the comprehensive cost of the candidate trajectory. The weighting coefficients include at least a first coefficient, a second coefficient, and a third coefficient, which correspond to the target distance cost, the obstacle distance cost, and the lateral error cost, respectively; the second coefficient is greater than the first coefficient and the third coefficient.

[0009] One possible implementation also includes: For each candidate trajectory, based on the obstacle information, it is determined whether a collision will occur when the robot moves along the candidate trajectory; If a collision is determined to occur, the candidate trajectory is deemed to have a collision risk. If it is determined that no collision will occur, the candidate trajectory is deemed to have no collision risk, and the candidate trajectory with no collision risk is evaluated.

[0010] In one possible implementation, generating control commands based on the desired linear velocity and desired angular velocity to drive the robot to move along the optimal trajectory includes: Based on the real-time distance between the robot and the target point of the local planning, the desired linear velocity is adjusted, and the final linear velocity after adjustment is less than or equal to the upper limit value of the linear velocity, which is calculated based on the robot's minimum turning radius and the desired angular velocity. The control command is generated based on the final linear velocity and the desired angular velocity.

[0011] In one possible implementation, selecting a target point for local planning from the reference trajectory based on the look-ahead distance includes: Multiple candidate points are determined based on the reference trajectory; the candidate points are path points in the reference trajectory that are adjacent to the robot's historical path points. Determine whether each candidate point and the historical path point are on the same continuous path segment; If the candidate point and the historical path point are on the same continuous path segment, the candidate point is determined to be a connected candidate point; Based on the robot's historical movement direction, connected candidate points whose forward direction is consistent with the historical movement direction are selected as reference points; Based on the look-ahead distance and the preset spacing between path points in the reference trajectory, the offset index value is calculated; Starting from the reference point, the target point is determined by searching along the path segment where the reference point is located according to the offset index value.

[0012] One possible implementation also includes: Determine whether the combined cost of all candidate trajectories is higher than a preset congestion threshold; If the combined cost of all candidate trajectories is greater than or equal to a preset congestion threshold, it is determined that there is a local congestion in front of the robot. Determine whether the number of consecutive determinations of the local blockage exceeds a preset threshold. If the number of consecutive determinations is less than the preset threshold, a deceleration command or a stop command is generated, and a candidate trajectory is regenerated. If the number of consecutive determinations is greater than or equal to the preset threshold number, it is determined that the robot is surrounded by obstacles. The simulated trajectory cost of the robot performing a left turn and a right turn in place is calculated. A rotation control command is generated based on the rotation direction with the lowest simulated trajectory cost, so that the robot can perform the corresponding rotation to escape the surrounded state.

[0013] Secondly, the present invention also provides a robot trajectory tracking and obstacle avoidance control system, comprising: The acquisition module is used to acquire motion state information, obstacle information, and reference trajectory; The calculation module is used to calculate the look-ahead distance based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information; A selection module is used to select a target point for local planning from the reference trajectory based on the look-ahead distance; The generation module is used to generate multiple candidate trajectories based on the motion state information and the obstacle information, using a dynamic window algorithm. The evaluation module is used to determine the lateral error based on the motion state information, and evaluate the comprehensive cost value of each candidate trajectory based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The processing module is used to determine the candidate trajectory with the minimum comprehensive cost as the optimal trajectory, obtain the expected linear velocity and expected angular velocity based on the optimal trajectory, and generate control commands based on the expected linear velocity and expected angular velocity to drive the robot to move along the optimal trajectory.

[0014] Thirdly, the present invention also provides an electronic device, including a memory and a processor, wherein, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot trajectory tracking and obstacle avoidance control method described in any of the above implementations.

[0015] Fourthly, the present invention also provides a computer-readable storage medium for storing a computer-readable program or instruction, which, when executed by a processor, can implement the steps of the robot trajectory tracking and obstacle avoidance control method described in any of the above implementations.

[0016] The beneficial effects of this invention are as follows: The robot trajectory tracking and obstacle avoidance control method provided by this invention firstly transforms the global reference trajectory into a series of dynamically changing local target points through a dynamic look-ahead mechanism, providing clear tracking guidance for local planning and improving the robot's tracking accuracy and obstacle avoidance capability. Furthermore, based on a dynamic window combined with obstacle information and motion state information, multiple candidate trajectories are generated. A multi-objective cost function is used to calculate the comprehensive cost of each candidate trajectory according to the obstacle information and motion state information. This ensures that the trajectory planning process simultaneously considers multiple key performance indicators such as target guidance, obstacle avoidance safety, motion smoothness, and path tracking, achieving an optimal balance between tracking accuracy, safety, and efficiency. Attached Figure Description

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

[0018] Figure 1 A schematic flowchart of an embodiment of the robot trajectory tracking and obstacle avoidance control method provided by the present invention; Figure 2 For the present invention Figure 1 A schematic diagram of an embodiment of S104; Figure 3 For the present invention Figure 1 A schematic diagram of an embodiment of S105; Figure 4 A schematic flowchart of another embodiment of the robot trajectory tracking and obstacle avoidance control method provided by the present invention; Figure 5 For the present invention Figure 1 A schematic diagram of an embodiment of S107; Figure 6 For the present invention Figure 1 A schematic diagram of an embodiment of S103; Figure 7 A schematic flowchart of another embodiment of the robot trajectory tracking and obstacle avoidance control method provided by the present invention; Figure 8 A schematic diagram of an embodiment of the robot trajectory tracking and obstacle avoidance control system provided by the present invention; Figure 9 A schematic diagram of an embodiment of the electronic device provided by the present invention. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0020] In the description of the embodiments of the present invention, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can represent three situations: A exists alone, A and B exist simultaneously, and B exists alone.

[0021] The terms "first," "second," etc., used in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a technical feature defined with "first" or "second" may explicitly or implicitly include at least one of that feature.

[0022] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0023] This invention provides a robot trajectory tracking and obstacle avoidance control method, system, device and medium, which are described below.

[0024] Figure 1 This is a schematic flowchart of an embodiment of the robot trajectory tracking and obstacle avoidance control method provided by the present invention, as shown below. Figure 1 As shown, the robot trajectory tracking and obstacle avoidance control methods include: S101. Obtain motion status information, obstacle information, and reference trajectory.

[0025] It should be noted that upon system power-on initialization, a pre-set inspection route file (e.g., a list containing a series of global coordinate points) stored internally is loaded, constructing a reference trajectory data structure readable by the program. Simultaneously, through sensors such as LiDAR, IMU (Inertial Measurement Unit), and wheel encoders, the robot's current motion state information and obstacle information in its surrounding environment are acquired in real time via fusion. Motion state information includes pose (x, y, θ), linear velocity v, and angular velocity ω. Obstacle information is typically represented as a local cost map or a list of obstacle point clouds.

[0026] S102. The look-ahead distance is calculated based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information.

[0027] It should be noted that, based on the robot's physical performance and application scenario, two key parameters are set: a preset prediction time t_pred (e.g., 2.5 seconds) and a preset safety factor k_safe (range 1.1~1.5, preferably 1.2~1.3). The robot's current maximum permissible speed v_max is read. The look-ahead distance L is calculated using the formula: L = v_max × t_pred × k_safe.

[0028] S103. Select a target point for local planning from the reference trajectory based on the look-ahead distance.

[0029] It should be noted that, based on the dynamic look-ahead distance and the robot's real-time pose, a spatial coordinate point is selected from the reference trajectory as the target point for local navigation that drives subsequent trajectory optimization and decision-making.

[0030] S104. Based on the dynamic window algorithm, generate multiple candidate trajectories according to the motion state information and the obstacle information.

[0031] It should be noted that during the local trajectory planning phase, to explore feasible future motion paths while satisfying various physical constraints, a Dynamic Window Approach (DWA) is used to generate a set of candidate trajectories for evaluation. First, a dynamic sampling window is constructed. Second, velocity sampling and trajectory simulation are performed within the dynamic window. Finally, trajectory feasibility screening (collision detection) is performed. Then, by fusing robot motion state information and obstacle information, a safe and feasible velocity search space (i.e., the dynamic window) is dynamically defined, and multiple candidate trajectories for future short-term motion are simulated within this dynamic window.

[0032] S105. Determine the lateral error based on the motion state information, and evaluate the comprehensive cost value of each candidate trajectory based on the multi-objective cost function, combining the lateral error, the obstacle information, and the target point.

[0033] It should be noted that: by using a structured multi-objective cost function, the performance of each candidate trajectory in multiple key dimensions such as approaching the target, avoiding obstacles, maintaining smoothness and accurate tracking is simultaneously quantified, and a comparable comprehensive cost value is obtained by weighting according to the weight coefficients, thereby providing accurate and consistent numerical basis for subsequent optimal decision-making.

[0034] S106. Determine the candidate trajectory with the minimum comprehensive cost value as the optimal trajectory, and obtain the expected linear velocity and expected angular velocity based on the optimal trajectory.

[0035] It should be noted that: the comprehensive cost value of all valid candidate trajectories is compared, and the trajectory with the minimum comprehensive cost value is selected as the optimal trajectory for the current period. This optimal trajectory corresponds to a specific pair of sampling velocities (v_sample, ω_sample) during its generation, and this pair of sampling velocities (v_sample, ω_sample) is directly used as the current expected linear velocity v_best and expected angular velocity ω_best.

[0036] S107. Based on the desired linear velocity and desired angular velocity, generate control commands to drive the robot to move along the optimal trajectory.

[0037] It should be noted that: based on the given desired linear velocity and desired angular velocity, the motion control algorithm converts them into motor control quantities, and the robot's motion state is controlled in a closed loop to ensure that the robot can move accurately and stably along the pre-planned optimal trajectory, thereby achieving continuous and coordinated motion along the specified optimal trajectory.

[0038] In summary, the robot trajectory tracking and obstacle avoidance control method provided in this embodiment of the invention firstly transforms the global reference trajectory into a series of dynamically changing local target points through a dynamic look-ahead mechanism, providing clear tracking guidance for local planning and improving the robot's tracking accuracy and obstacle avoidance capability. Furthermore, multiple candidate trajectories are generated based on a dynamic window combined with obstacle information and motion state information. A multi-objective cost function is then used to calculate the comprehensive cost of each candidate trajectory based on the obstacle information and motion state information. This ensures that the trajectory planning process simultaneously considers multiple key performance indicators such as target guidance, obstacle avoidance safety, motion smoothness, and path tracking, achieving an optimal balance between tracking accuracy, safety, and efficiency.

[0039] To efficiently generate a set of physically feasible, safe, and diverse short-term motion trajectory candidates while strictly satisfying the robot's own physical limits and real-time environmental safety constraints, thus providing a basis for subsequent optimal decision-making, in some embodiments of the present invention, such as... Figure 2 As shown, step S104 includes: S201. Based on the motion state information and the obstacle information, determine a dynamic window composed of the linear velocity range and the angular velocity range; wherein the boundary of the linear velocity range and the angular velocity range is determined by at least one of the following constraints: kinematic constraints based on the motion state information, dynamic constraints based on the robot, and safe distance constraints based on the obstacle information.

[0040] It should be noted that the dynamic window is defined by both the allowable linear velocity range and the allowable angular velocity range, and its boundary is precisely calculated by fusing at least three types of constraints, including: Kinematic constraints: Based on the robot's mechanical parameters (such as wheelbase), determine the minimum turning radius Rmin to prevent sideslip. From this, the coupling relationship between angular velocity and linear velocity is derived, that is, the linear velocity v and the angular velocity ω satisfy: v ≤ (ω × Rmin), ensuring that the trajectory corresponding to any velocity combination can be geometrically executed by the robot.

[0041] Dynamic constraints: Calculate the maximum acceleration 'a' based on the torque characteristics of the drive motor and the current speed state. max With maximum deceleration a min Within the sampling period Δt, the changes in linear velocity and angular velocity are limited to [a]. min ×Δt, a max Within the range of [×Δt], ensure that the speed command does not exceed the instantaneous response capability of the motor.

[0042] Safety distance constraint: Based on real-time obstacle information, calculate the safe distance required for the robot to brake suddenly at its current speed. For obstacles ahead, deduce the maximum permissible speed v to ensure a collision-free operation. safe .

[0043] S202. Speed ​​sampling is performed within the dynamic window range to obtain multiple candidate speeds.

[0044] It should be noted that within the rectangular dynamic window corresponding to the two-dimensional velocity space (v, ω) determined by S201, a set of discrete candidate velocities (vi, ωi) is generated by using a uniform sampling or adaptive sampling strategy. The sampling density of the candidate velocities can be configured according to the computing resources.

[0045] S203. Based on the candidate velocities and the current position and heading in the motion state information of each group, simulate and generate the multiple candidate trajectories.

[0046] It should be noted that for each candidate velocity (vi, ωi) obtained in S202, the robot's current motion state information is used as the initial state. It is assumed that the robot will move at a constant candidate velocity (vi, ωi) for a predetermined time interval T (e.g., 2.5 seconds) in the future. Using the robot's kinematic model (e.g., a differential driving model), the predicted poses at a series of discrete time points within the future time interval T are simulated and calculated through forward integration. These discrete poses are then connected in chronological order to generate a corresponding candidate trajectory. In this way, multiple candidate trajectories are simulated and generated. Each candidate trajectory describes the robot's complete motion path in the short future under the assumption of a constant candidate velocity.

[0047] In this embodiment, by pre-positioning and directly embedding three types of hard constraints—kinematics, dynamics, and safety distance—into the generation stage of the sampling space, it is ensured that all subsequent candidate trajectories are generated from a safe and feasible set of candidate velocities. This eliminates the possibility of the controller outputting dangerous commands that the robot cannot execute or that may lead to collisions, fundamentally guaranteeing the physical feasibility and safety of the candidate trajectories. Furthermore, the dynamic window mechanism reduces the velocity sampling space, transforming the infinite search problem into optimization within a strictly constrained, small, and precise feasible region. This significantly reduces invalid computations and ensures that the algorithm meets the real-time requirements of the control loop. Simultaneously, sampling within the feasible region ensures the diversity and quality of candidate trajectories, laying the foundation for selecting the optimal trajectory and achieving an optimized balance between planning efficiency and decision quality. Furthermore, since the safety distance constraint directly depends on real-time perceived obstacle information, the dynamic window can instantly shrink or expand according to environmental changes. For example, when approaching an obstacle, the window automatically shrinks to limit high-speed, dangerous sampling; when in an open area, the window expands to allow for more efficient motion options, making the robot's motion planning environmentally adaptable and improving the robot's tracking accuracy and obstacle avoidance capabilities.

[0048] In local trajectory planning, to scientifically and stably select the globally optimal candidate trajectory under multiple competing requirements such as target attraction, obstacle repulsion, and path tracking, in some embodiments of the present invention, such as... Figure 3 As shown, step S105 includes: S301. Based on the motion state information, the lateral error, and the obstacle information, calculate the target distance cost between each candidate trajectory and the target point, the obstacle distance cost between each candidate trajectory and the obstacle, and the lateral error cost relative to the reference trajectory.

[0049] It should be noted that for each candidate trajectory, its quantization cost is calculated in at least three key dimensions. The quantization cost in at least three key dimensions includes: Target distance cost: Calculate the Euclidean geometric distance between the predicted endpoint of the trajectory and the local planning target point. The target distance cost is calculated based on the Euclidean geometric distance; the closer the Euclidean geometric distance, the lower the target distance cost. For each candidate trajectory, obtain the predicted endpoint pose of the trajectory, calculate the Euclidean distance between the position coordinates in the predicted endpoint pose and the local planning target point, use this Euclidean distance as the base value of the target distance cost, and perform normalization processing.

[0050] Obstacle distance cost: Iterate through multiple predicted pose points on the candidate trajectory, calculate the nearest distance between each predicted pose point and all obstacles in the real-time obstacle information, and take the minimum of all nearest distance values ​​as the nearest obstacle distance for that trajectory; map the reciprocal of the nearest obstacle distance, or through a preset cost mapping table, to a cost value, which is used as the obstacle distance cost. For example, the obstacle distance cost (Cost) can be calculated using a mapping function. obs =η / dmin, where dmin is the nearest distance value and η is a configurable scaling factor used to adjust the conservatism of obstacle avoidance behavior. Alternatively, a predefined segmented penalty table can be used to convert the nearest distance value into an obstacle distance cost. The smaller the nearest distance value, the faster the obstacle distance cost increases non-linearly, thus imposing a strong penalty on trajectories that approach obstacles.

[0051] Lateral Error Cost: The vertical (normal) distance from the predicted trajectory endpoint to the global reference trajectory is calculated, and this vertical distance is used as the lateral deviation elat. When the absolute value of the lateral deviation is greater than a preset deviation tolerance threshold, a nonlinear penalty value is calculated based on the magnitude of the excess, and this nonlinear penalty value is used as the lateral error cost. When the absolute value of the lateral deviation is less than or equal to the deviation tolerance threshold, the lateral error cost is zero or a very small fixed value. For example, a deviation tolerance threshold eth is set. If |elat| ≤ eth, the lateral error cost is low; if |elat| > eth, the lateral error cost Cost is calculated according to the formula. lat =λ×(∣elat∣ eth) 2 The calculation is performed, where λ is the penalty coefficient, to impose severe penalties on behaviors that cause path deviations, thus constraining trajectory tracking accuracy.

[0052] S302. According to the preset weighting coefficients, the target distance cost, obstacle distance cost, and lateral error cost are weighted and summed to obtain the comprehensive cost of the candidate trajectory; wherein, the weighting coefficients include at least a first coefficient, a second coefficient, and a third coefficient, which correspond to the target distance cost, the obstacle distance cost, and the lateral error cost, respectively; the second coefficient is greater than the first coefficient and the third coefficient.

[0053] It should be noted that the system pre-sets and maintains a set of weighting coefficients, including at least a first coefficient α for target distance cost, a second coefficient β for obstacle distance cost, and a third coefficient for lateral error cost. Based on the principle of safety first, we set β > α and β > α. This makes obstacle avoidance safety the dominant and decisive factor in the assessment. The comprehensive cost (Cost) of a candidate trajectory is calculated using the following formula. total : Cost total =α×Cost goal +β×Cost obs + ×Cost lat +…(Formula 1) Cost goal The target distance cost is represented by "..." in Equation 1, which indicates other possible cost terms, such as speed cost, smoothing cost, etc. The weighting coefficients are not completely fixed and can be fine-tuned based on the prior characteristics of the current inspection segment. For example, in narrow passages, β is further increased; in long straight sections requiring precise tracking, it is appropriately increased. When approaching the finish line in an open area, α can be increased. This dynamic adjustment further optimizes the system's behavior in different scenarios.

[0054] In this embodiment, performance across different dimensions (approaching the target, avoiding obstacles, and tracking the path) is uniformly quantified into a calculable cost. A rigorous weighted summation is used for comprehensive evaluation, and the globally optimal trajectory is selected based on the same transparent and reproducible standard, significantly improving the stability and reliability of decision-making. Furthermore, an enhanced lateral error cost is introduced to ensure that the robot can be strongly pulled back to the predetermined path even during obstacle avoidance. This ensures that the robot can intelligently identify and lock onto the correct tracking segment in topologically complex paths such as intersections and parallel paths, avoiding trajectory jumps or discontinuities caused by misjudging the nearest point, and guaranteeing smooth and stable operation throughout the entire path. Furthermore, by explicitly setting β>α, When evaluating any trajectory, obstacle avoidance safety is given the highest priority. This means that a trajectory that can efficiently approach the target but is slightly close to obstacles will have a higher overall cost than a trajectory that may be slightly slower but is far away from all obstacles. This ensures that the robot prioritizes collision avoidance in all situations, greatly reducing the risk of accidents in complex and dynamic environments and enhancing the robot's operational safety. Furthermore, by modularizing the evaluation dimensions and supplementing them with configurable weights, the robot can exhibit the most suitable behavioral focus in different scenarios simply by adjusting the weight coefficients (such as dynamically adapting according to road segment type)—being more conservative in dangerous areas and more efficient in high-performance areas. This gives the system a high degree of flexibility and environmental adaptability, allowing it to adapt to diverse inspection scenarios through fine-tuning of weights.

[0055] In the trajectory planning process, to eliminate all invalid candidate trajectories that inevitably lead to collisions, limited computational resources are concentrated on a refined multi-objective optimization evaluation of safe and feasible trajectories, thereby improving the efficiency, real-time performance, and decision reliability of the planning algorithm. In some embodiments of the present invention, such as... Figure 4 As shown, it also includes: S401. For each candidate trajectory, determine whether a collision will occur when the robot moves along the candidate trajectory based on the obstacle information.

[0056] It should be noted that for each candidate trajectory generated in step S104, fast collision detection is performed. This involves geometrically comparing the continuous spatial envelope occupied by the robot as it moves along the candidate trajectory with real-time obstacle information. The continuous spatial envelope is typically the total area formed by scanning the robot's contour along the candidate trajectory, or the total area occupied by the robot at discrete key points on the candidate trajectory. It is then determined whether the robot's continuous spatial envelope intersects with the obstacle area generated based on the obstacle information. If there is an intersection, a collision is determined to occur; otherwise, no collision is determined to occur.

[0057] S402. If it is determined that a collision will occur, it is determined that the candidate trajectory has a collision risk. S403. If it is determined that no collision will occur, the candidate trajectory is determined to have no collision risk, and the candidate trajectory with no collision risk is evaluated.

[0058] It should be noted that, based on the judgment result of S401, candidate trajectories are classified and labeled using a binary classification method. Candidate trajectories with collision risk are marked as high-risk or invalid, while candidate trajectories without collision risk are marked as safe. All candidate trajectories marked as having collision risk will be directly excluded from the subsequent complex multi-objective cost function evaluation process (S105). Alternatively, a very large fixed cost value (such as infinity) can be assigned to candidate trajectories with collision risk to ensure that they will never be selected in subsequent comparisons. Only those candidate trajectories marked as having no collision risk will enter the subsequent multi-objective cost function evaluation process (S105). The system will fully calculate the cost values ​​of these candidate trajectories without collision risk across multiple dimensions, including target distance cost, obstacle distance cost (at this point, the closest distance to the obstacle is calculated, not whether a collision occurs), and lateral error cost, and perform weighted fusion to finally obtain a meaningful comprehensive cost value that reflects their overall performance.

[0059] In this embodiment, the evaluation of the multi-objective cost function involves a large number of mathematical operations, which is the main computational burden of the algorithm. By pre-screening a large number of obviously infeasible candidate trajectories through rapid collision detection (with relatively low computational cost), the number of candidate trajectories requiring fine evaluation is reduced, and the computation time of a single planning cycle is lowered. This allows the system to meet the requirements of shorter control cycles, enabling it to react and replan more promptly in the face of rapidly changing dynamic environments, significantly improving the computational efficiency and real-time performance of robot movement planning. Furthermore, freeing up limited computing resources (CPU time) from candidate trajectories with collision risks allows for the evaluation of remaining safe candidate trajectories without collision risks. This optimizes the allocation of decision-making resources, allowing for the exploration of more possibilities or more precise comparisons within the same time budget within a safe space. This improves the overall superiority of the final optimal trajectory selection, making the robot's movement smoother and more efficient. Furthermore, any candidate trajectory that poses a geometric risk of collision with an obstacle, regardless of how well it performs in other performance metrics (such as how close it is to the target or how smooth it is), will be excluded. This prevents the extreme danger of the algorithm accidentally selecting a high-performing but potentially dangerous trajectory due to improper weight configuration in the multi-objective cost function, sensor noise, or other evaluation errors. This greatly enhances the inherent safety and decision reliability of the entire control system.

[0060] To transform the desired velocity command output by the planning module, which may only consider optimality, into an optimal control command that can directly drive the actuator and ensure absolutely smooth and safe robot motion, adaptive adjustment and kinematic safety limiting are performed. In some embodiments of the present invention, such as Figure 5 As shown, step S107 includes: S501. Based on the real-time distance between the robot and the target point of the local planning, the desired linear velocity is adjusted, and the final linear velocity after adjustment is less than or equal to the upper limit value of the linear velocity, which is calculated based on the robot's minimum turning radius and the desired angular velocity. S502. Generate the control command based on the final linear velocity and the desired angular velocity.

[0061] It should be noted that the Euclidean distance between the robot's current position and the target point in the local planning is used as the real-time distance d. target，A progressive adjustment is performed based on the real-time distance between the robot and the target point. This involves dynamically scaling the desired linear velocity according to a preset speed adjustment function to obtain the adjusted linear velocity. This ensures the robot moves efficiently away from the target and automatically and smoothly decelerates as it approaches, eventually coming to a stop near the target point, effectively avoiding overshoot and end effector jitter. To ensure the robot's motion stability when executing turning commands (preventing sideslip or loss of steering ability), the theoretical maximum safe linear velocity v is calculated based on the robot's minimum turning radius Rmin and the current desired angular velocity ω_best. maxturn =∣ω_best∣×Rmin. Then, adjust the linear velocity v. adj With the upper limit of linear velocity v maxturn Compare the two values ​​and take the smaller value as the final linear velocity v. final That is, the final linear velocity v final =min(v adj v maxturn This limits the linear velocity of the robot's motion to within the kinematically permissible range. Then, the final linear velocity v is... final The desired angular velocity ω_best (which is usually kept at its original planned value because it is directly related to the steering intention) is combined with the control command and encapsulated into a standard instruction format that can be recognized by the underlying motion controller (such as a PID controller or a speed loop controller). The control command is sent to the robot's drive motor or actuator in real time via a communication bus (such as CAN or serial port), driving the robot to generate precise linear and angular velocity movements, thereby traveling along the planned optimal trajectory.

[0062] In this embodiment, the desired linear velocity is progressively adjusted based on the real-time distance between the robot's current position and the target point. The robot's linear velocity no longer changes abruptly but changes continuously and smoothly as the task progresses. This not only eliminates sudden stops and oscillations upon reaching the target point but also makes the acceleration / deceleration process throughout the inspection route smoother, significantly improving the robot's motion quality and ensuring extreme stability and riding / working comfort throughout the entire process. This is crucial for inspection tasks equipped with precision sensors or requiring stable operation. Furthermore, progressively adjusting the desired linear velocity based on the real-time distance between the robot's current position and the target point can bridge the gap between the planned ideal and the actual execution environment. This allows the robot to automatically adapt to special working conditions such as approaching the target or performing sharp turns, thereby improving the first-time success rate and overall robustness of complex continuous tasks (such as multi-segment inspection). Furthermore, by using a rigid speed limit constraint based on the minimum turning radius, it ensures that the linear velocity command issued under any circumstances is physically compatible with the current steering angular velocity command. This prevents the robot from deviating from its path, skidding, or even overturning due to excessively high command speeds that prevent it from turning in time, greatly improving the robot's safety in scenarios such as curves and obstacle avoidance. Moreover, adaptive speed adjustment ensures the robot maintains stable movement in different scenarios (straight paths, curves, approaching the target). In particular, the graded anomaly handling mechanism (from deceleration and waiting to active rotation for self-rescue) gives the robot the ability to autonomously escape when completely surrounded by dynamic or static obstacles, greatly reducing the need for human intervention and improving task completion rate and reliability in unpredictable environments.

[0063] To improve the path continuity of robots on fixed inspection routes with complex road sections (such as intersections, branches, and loops), in some embodiments of the present invention, such as... Figure 6 As shown, step S103 includes: S601. Determine multiple candidate points based on the reference trajectory; the candidate points are path points in the reference trajectory that are adjacent to the robot's historical path points.

[0064] It should be noted that: based on the robot's most recent historical path points (such as the pose sequence within the past second), a nearest neighbor search is performed on the global reference trajectory (usually stored as an ordered list of path points). Specifically, the Euclidean distance from each path point on the reference trajectory to the most recent historical path point is calculated, and the K path points with the smallest distances (e.g., K=3), or all path points with distances less than a set threshold, are selected as candidate points from the reference trajectory. These candidate points are geometrically closest to the robot, but they may represent path points on different branches or reverse paths.

[0065] S602. Determine whether each candidate point and the historical path point are on the same continuous path segment; S603. If the candidate point and the historical path point are on the same continuous path segment, the candidate point is determined to be a connected candidate point.

[0066] It should be noted that for each candidate point, it is checked whether it and the latest historical path point belong to the same continuous, uninterrupted path segment in the topology of the reference trajectory. Checking whether a candidate point and a historical path point are on the same continuous path segment can be done by querying a pre-built path connectivity graph or by checking whether the indices of the candidate point and the historical path point in the path point list are continuous and without branch markers. If the connectivity condition is met, the candidate point is marked as a connected candidate point. This step effectively eliminates branch points that are geometrically close but located on another unconnected parallel road or on the other side of an intersection.

[0067] S604. Based on the robot's historical movement direction, select the connected candidate points whose forward direction is consistent with the historical movement direction as reference points.

[0068] It should be noted that: The robot's historical motion direction is obtained (usually represented by a velocity vector or heading angle fitted from recent historical path points). Simultaneously, the forward direction of each connected candidate point on the reference trajectory is calculated (i.e., the tangent direction of the trajectory at that point). By calculating the cosine of the angle (dot product) between the historical motion direction and the forward direction of the candidate point, it is determined whether the directions are roughly consistent (e.g., the angle is less than 90 degrees). Connected candidate points with consistent directions are filtered out, and the connected candidate point with the closest geometric distance is selected as the reference point for the current path.

[0069] S605. Based on the look-ahead distance and the preset spacing between path points in the reference trajectory, the offset index value is calculated.

[0070] It should be noted that the calculated look-ahead distance is divided by the preset average spacing between path points in the reference path point list. The result is then rounded up to an integer value, which is used as the offset index value to quantify the continuous look-ahead distance into the number of path points that need to be traversed forward, ensuring that the selected target point at least meets the minimum look-ahead requirement.

[0071] S606. Starting from the reference point, the target point is determined by searching along the path segment where the reference point is located according to the offset index value.

[0072] It should be noted that: taking the index position of the reference point in the list of reference path points determined in S604 as the starting point, the offset index value calculated in S605 is used as the offset, and the count is performed forward (in the direction of increasing index) along the confirmed correct continuous path segment where the reference point is located. When the sum of the starting index value and the offset index value does not exceed (is less than or equal to) the endpoint index value of the path endpoint of the reference trajectory, the path point corresponding to the index sum value is determined as the target point for local trajectory planning; if the sum of the index values ​​exceeds (is greater than) the endpoint index value, the path endpoint of the reference trajectory is determined as the target point for local trajectory planning.

[0073] In this embodiment, traditional methods rely solely on the principle of geometric nearest point, which easily leads to incorrect path switching near intersections and loops, causing the robot to run the wrong path or oscillate in place. This application introduces two layers of logical judgment: path connectivity and motion direction consistency. This allows for intelligent identification and locking of the correct route the robot is actually traveling on, eliminating tracking ambiguity under complex path topologies, ensuring continuous and correct tracking of the global path, and ensuring that the inspection robot can independently and continuously complete inspection tasks with wide coverage and long duration without human intervention. Furthermore, when facing U-shaped bends, circular paths, or dense network routes, the robot's historical path points may be geometrically very close to multiple path branches. Filtering through historical path points can effectively resist this interference and stably output a unique correct reference point. This enables the robot to be reliably applied to real inspection scenarios with complex topologies, such as factory automated warehouses and large park loops, greatly improving the robot's robustness and adaptability in special scenarios. Furthermore, by selecting the reference points, the calculation of the forward target point heavily relies on the correct reference points, ensuring that the target point of the local planning is always ahead of the correct path that the robot should travel, thereby improving the global path tracking accuracy and local obstacle avoidance flexibility.

[0074] To intelligently identify obstacle blockage states ranging from momentary obstruction to complete entrapment during robot inspection tasks, and to automatically trigger a progressive response strategy from conservative waiting to active escape, thereby maintaining mobility and ultimately completing the task in dynamic and complex environments, as described in some embodiments of the present invention, such as... Figure 7 As shown, it also includes: S701. Determine whether the combined cost of all candidate trajectories is higher than a preset congestion threshold; S702. If the combined cost value of all the candidate trajectories is greater than or equal to a preset blockage threshold, it is determined that there is a local blockage in front of the robot.

[0075] It's important to note that a high congestion threshold is set, meaning any trajectory exceeding this threshold represents an extremely poor motion state (e.g., extremely close to an obstacle or severely deviating from the path). At the end of each planning cycle, the combined cost of all candidate trajectories is checked. If the combined cost of all candidate trajectories is greater than or equal to this congestion threshold, it's determined that there is a local congestion ahead of the robot at the current moment, indicating that within the current sensor field of view and motion capabilities, the robot cannot find a safe and effective feasible path.

[0076] S703. Determine whether the number of consecutive determinations of the local blockage is higher than a preset threshold number; S704. If the number of consecutive determinations is less than the preset number threshold, generate a deceleration command or a stop command, and regenerate the candidate trajectory.

[0077] It should be noted that when a partial obstruction is detected in front of the robot, a duration counter is started. If the number of consecutive detections of partial obstruction is less than a preset threshold (e.g., 3-5 consecutive control cycles), it is considered to be a dynamic, temporary obstacle (such as a pedestrian crossing or temporary debris). In this case, a deceleration command (gradually reducing the linear velocity until it reaches zero) or a stop command is generated to pause the robot and maintain observation. Simultaneously, candidate trajectories are regenerated and evaluated based on updated environmental information (dynamic obstacles may have moved away), to handle anticipated temporary obstructions with minimal energy consumption and risk.

[0078] S705. If the number of consecutive determinations is greater than or equal to the preset number threshold, it is determined that the robot is in a state surrounded by obstacles. The simulated trajectory cost of the robot performing a left rotation and a right rotation is calculated. A rotation control command is generated based on the rotation direction with the lowest simulated trajectory cost, so that the robot performs the corresponding rotation to escape the surrounded state.

[0079] It should be noted that if the number of consecutive judgments of partial blockage reaches or exceeds a preset threshold, it is determined that the robot is not experiencing a temporary blockage, but may be completely surrounded by static obstacles, resulting in a deadlock. At this point, conventional forward / backward trajectory planning fails. The robot needs to switch to self-rescue mode, which involves simulating two actions: a leftward rotation and a rightward rotation (with a fixed angular velocity and zero linear velocity), generating corresponding rotational trajectories, and evaluating the obstacle distance in the new orientation after rotation. The simulated trajectory cost for each action is calculated based on the obstacle distance. Then, the simulated trajectory cost of the leftward and rightward rotations is compared, and the rotation direction with the lowest simulated trajectory cost is selected. This lowest cost indicates fewer or farther obstacles, offering a greater chance of escape. A rotation control command is then generated and issued based on the lowest cost rotation direction, driving the robot to rotate in place according to the selected direction. During the rotation, sensors continuously monitor the environment. After rotating a certain angle (such as 90 degrees or 180 degrees) or until a feasible path is detected in the new orientation, the self-rescue mode exits, the system resets to the normal S101 state perception steps, and restarts the global perception-planning-control loop.

[0080] In this embodiment, intelligent classification and precise response to congestion scenarios are achieved, significantly improving task completion rate and operational efficiency in complex dynamic environments. The system effectively distinguishes between dynamic temporary congestion and static encirclement deadlock by introducing a time-duration criterion. For the former, a conservative strategy of deceleration-waiting-replanning is adopted to avoid unnecessary aggressive actions, ensuring energy efficiency and safety. For the latter, a proactive strategy of rotational self-rescue is decisively triggered to actively seek a breakthrough. This differentiated intelligent response ensures that the robot can patiently cope with temporary interference and decisively escape fatal traps, thus ultimately completing inspection tasks in various unpredictable scenarios.

[0081] This approach provides a path-dependent, proactive perception-based ultimate escape method, fundamentally enhancing the system's robustness and autonomous survival capabilities. When trapped in a complete encirclement that traditional path planning cannot resolve, the rotational self-rescue strategy offered by this solution is a metacognitive behavior. It temporarily abandons the inherent goal of proceeding along the path, prioritizing instead the alteration of its posture to acquire new environmental information. This method of actively changing sensor perspectives to find a way out mimics the exploratory behavior of organisms in distress, endowing the robot with algorithmic adaptability and resilience. It addresses the weakness of traditional navigation algorithms in extreme situations, providing the ultimate guarantee of system robustness.

[0082] This approach optimizes system resource allocation and behavioral decision-making, balancing a conservative yet proactive strategy. Instead of blindly taking aggressive actions every time a poor path is detected, it uses a brief observation period to confirm the severity of the problem. This prevents erroneous actions caused by transient sensor noise or poor single-cycle planning, making the system more stable and reliable. Simultaneously, it concentrates computational resources (frequent replanning) on ​​the most likely times to resolve the issue (when a brief blockage may be cleared), and upon confirming a deadlock, quickly switches to a computationally simple but goal-oriented escape algorithm, achieving an efficient match between decision-making logic and computational resources.

[0083] For example, the optimal trajectory is calculated by the DWA algorithm, and the optimal control speed is obtained by combining the PID control algorithm. The input messages include preset trajectory point information, grid_map obstacle information, target point information, and robot current motion state information. The robot trajectory tracking and obstacle avoidance control process includes: (1) Reference trajectory initialization: Load the preset inspection route file and construct the reference trajectory data structure, including the target line, historical line, and charging line. (2) Trajectory planning and evaluation: The DWA algorithm is used for local trajectory planning. Based on the robot's current state and target point position, sampling is performed in the velocity space to predict multiple possible motion trajectories, and evaluation is performed through a multi-objective cost function. The multi-objective cost function includes heading cost, velocity cost, obstacle distance cost, target distance cost, lateral error cost, and angle cost. The multi-objective cost function is defined as: total cost = α × target distance cost + β × obstacle distance cost + γ × velocity cost + δ × heading cost + ε × lateral error cost + ζ × angle cost, where α, β, γ, δ, ε, and ζ are the weight coefficients of each cost item. The initial values ​​of the weight coefficients are determined through experimental trial and error to determine typical value ranges (e.g., β is the highest, γ and ζ are lower). Based on preset rules, the values ​​can be dynamically and adaptively adjusted according to the characteristics of different inspection sections such as straight roads, curves, and narrow passages to optimize performance indicators such as tracking accuracy, obstacle avoidance safety, and motion stability. Specifically, the value ranges of each weight coefficient are as follows: obstacle distance cost weight β is the highest (0.8~1.2), target distance cost weight α and lateral error cost weight ε are in the middle (0.3~0.6, 0.2~0.5), heading cost weight δ is next (0.1~0.4), while speed cost weight γ and angle cost weight ζ are the lowest (0.05~0.2, 0.02~0.1). This weight configuration strategy achieves optimal decision-making for trajectory planning through a multi-objective cost function. (3) Dynamic selection of lookahead: Based on the robot's current maximum speed v_max, prediction time t_pred (i.e., the time length for the algorithm to simulate the robot's future trajectory, set to 2.5 seconds), trajectory point spacing d_step, and safety factor k_safe (a scaling factor set based on robot braking performance, sensor accuracy, and environmental complexity, used to introduce a safety buffer in the lookahead distance, with a value range of 1.1 to 1.5, preferably 1.2 to 1.3), the lookahead index L_lookahead= is dynamically calculated. (v_max×t_pred×k_safe) / d_step And select a suitable target point as the local planning target from the current road segment trajectory points; then, directly determine the path point with index number L_lookahead on the reference trajectory starting from the current position and moving backward as the target point of the current local trajectory planning. This ensures that the robot's "field of vision" can be dynamically adjusted with speed, focusing on the distance at high speed to ensure stability, and focusing on the nearby area at low speed to facilitate flexible obstacle avoidance. (4) Lateral error feedback control: calculate the lateral deviation e between the robot's current position and the reference trajectory in real time. lat In trajectory evaluation, a lateral error cost term is introduced. When the lateral error exceeds a threshold e, th At that time, a high penalty is imposed on the angular velocity direction that may lead to increased error; through P=λ×max(0,e lat -e th(5) Speed ​​control: Speed ​​control adopts a hierarchical strategy: The DWA algorithm outputs the desired linear velocity v_best and angular velocity ω_best that satisfy multi-objective optimization. The PID controller is used to accurately track and adjust the desired velocity output by DWA to overcome dynamic delay and disturbance and ensure the execution of control commands. (6) Path point continuity guarantee: Record the historical trajectory and calculate multiple candidate path points closest to the robot. Determine whether the candidate point is on the robot's current route or a reasonable route to eliminate disconnected branch points. Determine whether the forward direction of the candidate point is roughly consistent with the robot's historical motion direction to eliminate points with excessive directional deviation. Lock the correct target point from multiple candidate points with similar distances to ensure the continuity of the trajectory and smooth transition when switching between multiple road segments. (7) Adaptive speed adjustment: The robot's speed is dynamically adjusted according to the distance d_target between the robot and the target point. When approaching the target point, a gradual deceleration strategy v=k×d_target (k is a speed proportional coefficient with a value of 0.8) is adopted. At the same time, the linear velocity v is limited according to the magnitude of the angular velocity ω to ensure smooth motion, satisfying v≤ω×R_min, where R_min is the minimum turning radius. (8) Rotation self-rescue strategy: When the trajectory planning cost cost continuously exceeds the threshold cost_threshold, it is determined that the robot is surrounded by obstacles. By calculating the rotation trajectory in the left and right directions, the robot selects the obstacle-free direction to rotate in place to find a way out. (9) Output trajectory message: The trajectory points obtained by the planning are retained according to the original coordinate system and converted into the result relative to the robot coordinate system as the output. This application combines DWA local trajectory planning with PID control and introduces strategies such as dynamic look-ahead point selection, lateral error feedback, and adaptive speed adjustment to enable a fixed-route inspection robot to achieve real-time obstacle avoidance of dynamic obstacles while ensuring high-precision trajectory tracking. This solves the problem that traditional methods are prone to stalling or deviating from the route in complex scenarios, and improves the robot's motion stability, path continuity, and overall robustness in continuous multi-segment inspection tasks.

[0084] To better implement the robot trajectory tracking and obstacle avoidance control method in the embodiments of the present invention, based on the robot trajectory tracking and obstacle avoidance control method, correspondingly, as follows: Figure 8 As shown, this embodiment of the invention also provides a robot trajectory tracking and obstacle avoidance control system 800, which includes: The acquisition module 801 is used to acquire motion state information, obstacle information, and reference trajectory. The calculation module 802 is used to calculate the look-ahead distance based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information; The selection module 803 is used to select a target point for local planning from the reference trajectory based on the look-ahead distance; The generation module 804 is used to generate multiple candidate trajectories based on the motion state information and the obstacle information according to the dynamic window algorithm. Evaluation module 805 is used to determine the lateral error based on the motion state information, and evaluate the comprehensive cost value of each candidate trajectory based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The processing module 806 is used to determine the candidate trajectory with the minimum comprehensive cost as the optimal trajectory, obtain the expected linear velocity and expected angular velocity based on the optimal trajectory, and generate control commands based on the expected linear velocity and expected angular velocity to drive the robot to move along the optimal trajectory.

[0085] The robot trajectory tracking and obstacle avoidance control system 800 provided in the above embodiments can realize the technical solutions described in the above embodiments of the robot trajectory tracking and obstacle avoidance control method. The specific implementation principles of each module or unit can be found in the corresponding content in the above embodiments of the robot trajectory tracking and obstacle avoidance control method, and will not be repeated here.

[0086] like Figure 9 As shown, the present invention also provides an electronic device 900. The electronic device 900 includes a processor 901, a memory 902, and a display 903. Figure 9 Only some components of the electronic device 900 are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0087] In some embodiments, processor 901 may be a central processing unit (CPU), microprocessor, or other data processing chip, used to run program code stored in memory 902 or process data, such as the robot trajectory tracking and obstacle avoidance control method of the present invention.

[0088] In some embodiments, processor 901 may be a single server or a group of servers. The server group may be centralized or distributed. In some embodiments, processor 901 may be local or remote. In some embodiments, processor 901 may be implemented on a cloud platform. In one embodiment, the cloud platform may include a private cloud, public cloud, hybrid cloud, community cloud, distributed cloud, intranet, multi-cloud, etc., or any combination thereof.

[0089] In some embodiments, memory 902 may be an internal storage unit of electronic device 900, such as a hard disk or memory of electronic device 900. In other embodiments, memory 902 may also be an external storage device of electronic device 900, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc. equipped on electronic device 900.

[0090] Furthermore, the memory 902 may include both internal storage units of the electronic device 900 and external storage devices. The memory 902 is used to store application software and various types of data installed on the electronic device 900.

[0091] In some embodiments, display 903 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, or an OLED (Organic Light-Emitting Diode) touchscreen. Display 903 is used to display information from electronic device 900 and to display a visual user interface. Components 901-903 of electronic device 900 communicate with each other via a system bus.

[0092] In one embodiment, when the processor 901 executes the robot trajectory tracking and obstacle avoidance control program in the memory 902, the following steps can be implemented: Acquire motion status information, obstacle information, and reference trajectory; The look-ahead distance is calculated based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information. A target point for local planning is selected from the reference trajectory based on the forward distance; Based on the dynamic window algorithm, multiple candidate trajectories are generated according to the motion state information and the obstacle information; The lateral error is determined based on the motion state information, and the comprehensive cost value of each candidate trajectory is evaluated based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The candidate trajectory with the minimum comprehensive cost value is determined as the optimal trajectory, and the expected linear velocity and expected angular velocity are obtained based on the optimal trajectory. Based on the desired linear velocity and desired angular velocity, control commands are generated to drive the robot to move along the optimal trajectory.

[0093] It should be understood that when the processor 901 executes the robot trajectory tracking and obstacle avoidance control program in the memory 902, in addition to the functions mentioned above, it can also perform other functions, as can be found in the description of the corresponding method embodiments above.

[0094] Furthermore, this embodiment of the invention does not specifically limit the type of electronic device 900 mentioned. Electronic device 900 can be a mobile phone, tablet computer, personal digital assistant (PDA), wearable device, laptop computer, or other portable electronic device. Exemplary embodiments of portable electronic devices include, but are not limited to, portable electronic devices running iOS, Android, Microsoft, or other operating systems. The aforementioned portable electronic device can also be other portable electronic devices, such as a laptop computer with a touch-sensitive surface (e.g., a touch panel). It should also be understood that in some other embodiments of the invention, electronic device 900 may not be a portable electronic device, but rather a desktop computer with a touch-sensitive surface (e.g., a touch panel).

[0095] Accordingly, this application also provides a computer-readable storage medium for storing computer-readable programs or instructions. When the programs or instructions are executed by a processor, they can implement the steps or functions of the robot trajectory tracking and obstacle avoidance control methods provided in the above-described method embodiments.

[0096] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware (such as a processor, controller, etc.), and the computer program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0097] The robot trajectory tracking and obstacle avoidance control method, system, device and medium provided by the present invention have been described in detail above. Specific examples have been used to illustrate the principle and implementation of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation and application scope based on the idea of ​​the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A robot trajectory tracking and obstacle avoidance control method, characterized in that, include: Acquire motion status information, obstacle information, and reference trajectory; The look-ahead distance is calculated based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information. A target point for local planning is selected from the reference trajectory based on the forward distance; Based on the dynamic window algorithm, multiple candidate trajectories are generated according to the motion state information and the obstacle information; The lateral error is determined based on the motion state information, and the comprehensive cost value of each candidate trajectory is evaluated based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The candidate trajectory with the minimum comprehensive cost value is determined as the optimal trajectory, and the expected linear velocity and expected angular velocity are obtained based on the optimal trajectory. Based on the desired linear velocity and desired angular velocity, control commands are generated to drive the robot to move along the optimal trajectory.

2. The robot trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The dynamic window algorithm generates multiple candidate trajectories based on the motion state information and the obstacle information, including: Based on the motion state information and the obstacle information, a dynamic window composed of the linear velocity range and the angular velocity range is determined; wherein, the boundary of the linear velocity range and the angular velocity range is determined by at least one of the following constraints: kinematic constraints based on the motion state information, dynamic constraints based on the robot, and safe distance constraints based on the obstacle information; Multiple candidate velocities are obtained by sampling velocities within the dynamic window range; Based on the candidate velocities and the current position and heading in the motion state information of each group, the multiple candidate trajectories are simulated and generated.

3. The robot trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The step of determining the lateral error based on the motion state information, and evaluating the comprehensive cost value of each candidate trajectory based on a multi-objective cost function, combining the lateral error, the obstacle information, and the target point, includes: Based on the motion state information, the lateral error, and the obstacle information, calculate the target distance cost between each candidate trajectory and the target point, the obstacle distance cost between each candidate trajectory and the obstacle, and the lateral error cost relative to the reference trajectory. Based on preset weighting coefficients, the target distance cost, obstacle distance cost, and lateral error cost are weighted and summed to obtain the comprehensive cost of the candidate trajectory. The weighting coefficients include at least a first coefficient, a second coefficient, and a third coefficient, which correspond to the target distance cost, the obstacle distance cost, and the lateral error cost, respectively; the second coefficient is greater than the first coefficient and the third coefficient.

4. The robot trajectory tracking and obstacle avoidance control method according to claim 3, characterized in that, Also includes: For each candidate trajectory, based on the obstacle information, it is determined whether a collision will occur when the robot moves along the candidate trajectory; If a collision is determined to occur, the candidate trajectory is deemed to have a collision risk. If it is determined that no collision will occur, the candidate trajectory is deemed to have no collision risk, and the candidate trajectory with no collision risk is evaluated.

5. The robot trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The step of generating control commands based on the desired linear velocity and desired angular velocity to drive the robot to move along the optimal trajectory includes: Based on the real-time distance between the robot and the target point of the local planning, the desired linear velocity is adjusted, and the final linear velocity after adjustment is less than or equal to the upper limit value of the linear velocity, which is calculated based on the robot's minimum turning radius and the desired angular velocity. The control command is generated based on the final linear velocity and the desired angular velocity.

6. The robot trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The step of selecting a target point for local planning from the reference trajectory based on the look-ahead distance includes: Multiple candidate points are determined based on the reference trajectory; the candidate points are path points in the reference trajectory that are adjacent to the robot's historical path points. Determine whether each candidate point and the historical path point are on the same continuous path segment; If the candidate point and the historical path point are on the same continuous path segment, the candidate point is determined to be a connected candidate point; Based on the robot's historical movement direction, connected candidate points whose forward direction is consistent with the historical movement direction are selected as reference points; Based on the look-ahead distance and the preset spacing between path points in the reference trajectory, the offset index value is calculated; Starting from the reference point, the target point is determined by searching along the path segment where the reference point is located according to the offset index value.

7. The robot trajectory tracking and obstacle avoidance control method according to any one of claims 1 to 6, characterized in that, Also includes: Determine whether the combined cost of all candidate trajectories is higher than a preset congestion threshold; If the combined cost of all candidate trajectories is greater than or equal to a preset congestion threshold, it is determined that there is a local congestion in front of the robot. Determine whether the number of consecutive determinations of the local blockage exceeds a preset threshold. If the number of consecutive determinations is less than the preset threshold, a deceleration command or a stop command is generated, and a candidate trajectory is regenerated. If the number of consecutive determinations is greater than or equal to the preset threshold number, it is determined that the robot is surrounded by obstacles. The simulated trajectory cost of the robot performing a left turn and a right turn in place is calculated. A rotation control command is generated based on the rotation direction with the lowest simulated trajectory cost, so that the robot can perform the corresponding rotation to escape the surrounded state.

8. A robot trajectory tracking and obstacle avoidance control system, characterized in that, include: The acquisition module is used to acquire motion state information, obstacle information, and reference trajectory; The calculation module is used to calculate the look-ahead distance based on the preset prediction time, the preset safety factor, and the current maximum speed in the motion state information; A selection module is used to select a target point for local planning from the reference trajectory based on the look-ahead distance; The generation module is used to generate multiple candidate trajectories based on the motion state information and the obstacle information, using a dynamic window algorithm. The evaluation module is used to determine the lateral error based on the motion state information, and evaluate the comprehensive cost value of each candidate trajectory based on a multi-objective cost function that combines the lateral error, the obstacle information, and the target point. The processing module is used to determine the candidate trajectory with the minimum comprehensive cost as the optimal trajectory, obtain the expected linear velocity and expected angular velocity based on the optimal trajectory, and generate control commands based on the expected linear velocity and expected angular velocity to drive the robot to move along the optimal trajectory.

9. An electronic device, characterized in that, Including memory and processor, among which, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot trajectory tracking and obstacle avoidance control method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Used to store computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the robot trajectory tracking and obstacle avoidance control method according to any one of claims 1 to 7.

Images