Mobile robot adaptive following method and system based on uwb and lidar

Abstract

The present application relates to the technical field of robot autonomous following, and particularly relates to a mobile robot adaptive following method and system based on UWB and laser radar. The present application takes a fixed control cycle as a core reference, preferentially relies on UWB pose information effectiveness determination to ensure core positioning reliability, combines with a de-bouncing processing to filter out laser radar interference signals, cools the timing constraint three-state switching rhythm, only enables the corresponding controller at state switching time, disables other controllers and synchronously clears the internal state, and then assists with speed and acceleration double-amplitude to prevent motion mutation, so as to effectively realize smooth following of the robot to the target. In actual diversified following scenes such as home, hospital ward and indoor warehouse, the problem of insufficient adaptability and safety caused by frequent switching of following and obstacle avoidance and prominent speed peak is solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot autonomous following, in particular to a mobile robot adaptive following method and system based on UWB and laser radar. BACKGROUND

[0002] In recent years, with the continuous integration and innovation of artificial intelligence and sensor technology, autonomous mobile robots are increasingly widely used, covering logistics and warehousing, intelligent medical care, home services and other scenarios. In core scenarios such as elderly care and postoperative rehabilitation, the stable following and dynamic obstacle avoidance capabilities of robots for target personnel have become the core demand of human-machine collaboration. For example, in a home use environment, it is necessary to both accurately track the target moving trajectory and to avoid dynamic obstacles such as furniture and pedestrians in real time, while ensuring the smoothness and safety of the movement process. However, most of the existing researches are focused on human following technology in obstacle-free areas, which is far from the actual operational requirements in complex indoor environments; and in the design of mobile robot systems, the complexity and variability of indoor environments make it impossible for mobile robot systems relying on a single sensor to robustly complete the following obstacle avoidance function.

[0003] To overcome the defects of a single sensor, current mobile robot systems optimize performance through multi-sensor fusion solutions, such as the multi-modal following method based on the fusion of UWB and laser radar data disclosed in Chinese patent application file CN116087972A, which solves the problem that a single sensor cannot simultaneously achieve target positioning and obstacle detection through multi-sensor fusion, fills the gap of single-mode perception, and improves the basic environmental perception capability of the robot.

[0004] However, the above multi-sensor fusion scheme still has obvious limitations, and its technical design focuses on simple scenes without obstacles or low disturbance, ignoring the multiple challenges of sensing uncertainty, dynamic obstacles and control switching in real scenarios. The UWB pose information of the target object is frequently invalid due to the influence of multipath effect and the challenge of dynamic obstacles. Although the multi-sensor fusion scheme can improve the perception coverage, there are still two types of bottlenecks: first, the control response is lagging, typically manifested as the use of traditional pose PID control (i.e. a traditional scheme that uses PID algorithm to perform closed-loop control based on the pose deviation of the target and the robot), integral saturation leads to the robot colliding when the target suddenly slows down (for example, in a home scenario, when an elderly target suddenly stops, the mobile robot maintains the original acceleration trend and cannot quickly respond to the deceleration instruction, resulting in a forward collision safety risk), and the large differential noise leads to the left and right swing of the robot during turning (for example, in a hospital scenario, when a patient turns in the corridor of the ward, the mobile robot is disturbed by the corner facility, resulting in frequent fluctuations in the turning angular velocity and the trajectory deviation of the left and right swing); second, the decision mechanism is rigid, and the current common practice is to have multiple controllers output in parallel, and the downstream takes the latest data or the data with the highest priority to arbitrate (this way is easy to write in parallel when the timing jitter occurs), or only uses a single threshold event to trigger state jump (this way is easy to oscillate near the threshold and cannot guarantee the stability of a period of time after switching). These problems are particularly prominent in the frequent switching scenarios of following and obstacle avoidance, and in actual diversified following scenarios such as home environments with dense furniture and medical scenarios with flowing personnel, the adaptability and safety are obviously insufficient. SUMMARY

[0005] The application provides a mobile robot adaptive following method and system based on UWB and laser radar to solve the problem of low adaptability and safety caused by frequent switching between following and obstacle avoidance of mobile robots in real scenarios.

[0006] To solve the above technical problems, the application provides the following technical solutions:

[0007] The mobile robot adaptive following method based on UWB and laser radar comprises the following steps:

[0008] S10: setting an idle state, a following state and an obstacle avoidance state; powering on the robot and loading parameters, the parameters including a UWB timeout threshold, a debouncing time of obstacle appearance and disappearance, a switching cooling time, speed and acceleration limit values, initializing the current state as the idle state, and clearing the internal state of the controller, the internal state including the following state and the obstacle avoidance state;

[0009] S20: Collect UWB pose information and LiDAR scanning data; perform jitter removal processing on the scanning data according to the jitter removal time of obstacle appearance and disappearance to obtain smooth pose and Boolean values ​​representing whether there are obstacles in front and the nearest forward distance;

[0010] S30: Within each preset control cycle, determine whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If invalid or timed out, switch the robot's current state to the idle state. After the robot enters the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooling timer ends, switch the current state to the following state or the obstacle avoidance state according to the Boolean value.

[0011] S40: Enables the controller corresponding to the current state after the switch only when switching states, and disables other controllers; the internal state of the controller is cleared once when it is enabled.

[0012] S50: The enabled controller takes the smoothed UWB pose information or the nearest forward distance as input, calculates the linear velocity or angular velocity, constrains the changes in linear velocity and angular velocity within the control cycle according to the velocity and acceleration limit values, and outputs the constrained data to the robot for execution.

[0013] Furthermore, a mobile robot adaptive following system based on UWB and LiDAR, which utilizes a mobile robot adaptive following method based on UWB and LiDAR, includes:

[0014] The preprocessing module powers on the robot and loads parameters, including UWB timeout threshold, de-jittering time for obstacle appearance and disappearance, switching cooling time, speed and acceleration limit values.

[0015] The data acquisition and obstacle detection module acquires UWB pose information and LiDAR scanning data; it smooths the UWB pose information and performs anti-jitter processing on the scanning data based on the anti-jitter time of obstacle appearance and disappearance to obtain smooth pose and Boolean values ​​representing whether there is an obstacle in front and the nearest forward distance.

[0016] The state machine decision module sets up an idle state, a follow state, and an obstacle avoidance state. After the robot is powered on, the current state is initialized to the idle state, and the internal state of the controller is cleared, including the follow state and the obstacle avoidance state. Within each preset control cycle, the module determines whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If invalid or timed out, the robot's current state is switched to the idle state. After the robot enters the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooling timer ends, the current state is switched to the follow state or the obstacle avoidance state according to the Boolean value.

[0017] When the controller is enabled, its internal state is cleared to zero instantly. When disabled, it immediately releases zero velocity and enters the disabled state. It acquires smoothed UWB pose information and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution.

[0018] The obstacle avoidance controller, when enabled, instantly clears its internal state to zero; when disabled, it immediately releases zero speed and enters the disabled state. It acquires Boolean values ​​representing whether there is an obstacle ahead and the nearest forward distance, and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution.

[0019] The state machine decision module enables the controller corresponding to the current state after the switch only when switching states, and disables other controllers.

[0020] The principles and beneficial effects of this invention are as follows: This invention uses a fixed control cycle as a stable benchmark. This design allows core decision-making processes such as UWB pose information validity determination, obstacle avoidance status verification, and state switching judgment to proceed in an orderly manner at a unified pace. This avoids coordination disorder between different modules due to misaligned decision-making timing, ensuring the entire control process is coherent and predictable. Based on this, this invention prioritizes ensuring the reliability of core positioning data through UWB pose information validity and timeout determination. For example, in a home environment, UWB pose information is a key basis for the robot to lock onto the elderly person's location. The validity of this data directly determines the following accuracy and safety. If the UWB pose information signal is invalid because the elderly person passes by metal furniture, or if the transmission delay is too long to reflect the elderly person's current location, safety is prioritized immediately, and the robot immediately switches to an idle state to avoid loss of robot control due to inaccurate positioning.

[0021] Meanwhile, by combining the obstacle avoidance state after jigging with the switching between idle, following, and obstacle avoidance states, the jigging process can filter out instantaneous interference in the LiDAR scan, such as non-real obstacle signals like tissues temporarily falling on the floor or brief reflections on furniture surfaces in a home environment, ensuring accurate obstacle judgment. The cooling timer further constrains the state switching rhythm, starting the timer after each state switch and prohibiting new state switches during this period. This prevents the robot from repeatedly switching between following and obstacle avoidance states due to small fluctuations in the obstacle judgment threshold (such as an elderly person slightly moving a dining chair), which would disrupt operational stability.

[0022] Furthermore, this invention enables the corresponding controller and disables other controllers only once during state switching, simultaneously clearing the internal state of the controllers. This method fundamentally eliminates the possibility of multiple controllers concurrently writing speed commands, preventing speed conflicts caused by the simultaneous output of commands from the following controller and the obstacle avoidance controller. For example, in a home environment for the elderly, when the elderly person is moving around indoors wearing a UWB tag, the robot can reliably handle signal fluctuations in UWB pose information caused by metal furniture obstruction, eliminating interference from temporary debris on the ground through de-jittering. It also avoids sudden activation or deactivation, or abrupt changes in direction due to repeated state switching or controller conflicts, ensuring the robot consistently and smoothly follows the elderly person's steps, providing a safe and consistent following service without concerns about safety hazards caused by data fluctuations or command conflicts.

[0023] Finally, the enabled controller calculates the speed based on the reliable data processed earlier. Specifically, in follow mode, the follow controller uses smoothed UWB pose data and a preset safe following distance to calculate the linear velocity that maintains a stable distance, as well as the angular velocity to align with the UWB pose information. In obstacle avoidance mode, the obstacle avoidance controller uses obstacle data after LiDAR de-jittering to calculate the turning speed and deceleration magnitude to avoid obstacles. Simultaneously, speed limiting ensures the robot does not exceed the safe speed adapted to the current scene, preventing excessively fast movement; acceleration limiting controls the rate of change of speed, preventing sudden acceleration or deceleration. In this way, even if the UWB pose information fluctuates, the robot can smoothly adjust its motion state, completely avoiding safety hazards caused by sudden changes in motion.

[0024] In summary, this invention uses a fixed control cycle as its core benchmark, prioritizes the validity of UWB pose information to ensure reliable core positioning, combines jitter reduction processing to filter out LiDAR interference signals, and uses cooling timing to constrain the three-state switching rhythm. It enables the corresponding controller only once during state switching, disables other controllers, and simultaneously resets their internal states. Furthermore, it employs dual limiting of speed and acceleration to prevent sudden motion changes, effectively achieving smooth robot following of the target. In diverse real-world following scenarios such as homes, hospital wards, and indoor warehouses, it solves the problem of insufficient adaptability and safety caused by frequent switching between following and obstacle avoidance and prominent speed peaks.

[0025] Furthermore, in step S20, the UWB pose information is preprocessed, that is, the UWB pose information is filtered. When filtering, the distance, orientation, and validity obtained from the UWB pose information are used as the original data. The original data are subjected to median filtering and low-pass filtering to obtain smoothed distance, orientation, and validity as smoothed data. Then, the validity of the smoothed data is verified.

[0026] Filtering before verification avoids noise in the original data from interfering with the verification results, reduces the number of invalid verifications, and improves verification accuracy. Median filtering can specifically remove sudden outliers in the original data, while low-pass filtering can smooth continuous fluctuations in the data. The combined effect of these two methods makes the output smooth data more closely match the actual pose, reducing the impact of data jumps on subsequent control. Furthermore, the smooth data obtained by processing the same variable through different methods has a dual function: it serves as the object of validity verification, ensuring that all data entering downstream has undergone reliability verification, and it is also the core input for subsequent calculations, realizing a closed-loop reliable transmission of data processing, verification, and application.

[0027] Furthermore, in step S50, when constraining the angular velocity, the actual distance obtained from the filtered UWB pose information is acquired in real time. The actual distance is compared with the preset target distance to obtain the distance error. The azimuth angle of the filtered UWB pose information is converted into radians to obtain the angle between the robot's orientation and the target object's direction as the heading error. Based on the heading error and the actual distance, the vertical distance from the robot's central axis to the target is calculated as the lateral error. Stanley-like control logic is used to calculate the desired front wheel steering angle by combining the heading error and the lateral error, while limiting the front wheel steering angle within the robot's mechanical structure parameters. The front wheel steering angle is converted into angular velocity by combining the robot's physical parameters.

[0028] Converting the azimuth angle to radians eliminates calculation biases caused by differences in angle units, making the heading error value more compatible with the control algorithm requirements and improving error calculation accuracy. Combining heading and lateral errors to calculate the steering angle simultaneously corrects the robot's orientation deviation and lateral position offset, ensuring steering control considers both direction alignment and position correction, avoiding steering yaw caused by a single error calculation. Mechanical structural parameter limitations prevent the steering angle from exceeding hardware limits, avoiding damage to steering components or robot rollover. The filtered UWB pose information provides both actual distance to support distance and lateral error calculations and azimuth angle to support heading error derivation, providing fundamental data for the coherent calculation of steering angle and angular velocity.

[0029] Furthermore, after calculating the heading error and lateral error, the method also includes: inputting the distance error into a preset position-type PID controller, and calculating the speed control quantity by combining preset proportional, integral, and derivative gains; when the actual distance is less than the preset target distance, dynamically reducing the speed through a preset distance proportional coefficient, and limiting the maximum linear speed through the speed control quantity.

[0030] The positional PID controller can dynamically adjust the speed control quantity according to the changes in distance error, so that the following distance between the robot and the target continuously approaches the preset value, avoiding the long-term accumulation of distance deviation. The application of the distance proportional coefficient can actively slow down the robot when it approaches the target, rather than relying on the lag adjustment of the PID, reducing the risk of collision from the source. The speed control quantity has the dual attributes of PID calculation results and maximum linear speed limit basis, realizing dynamic adjustment of speed by a single parameter and safety backup. The distance error not only indirectly supports the relevant calculation of heading error and lateral error, but also participates in the derivation of speed control quantity as a core input, connecting steering control and speed control.

[0031] Further, in step S50, during obstacle avoidance, the controller constructs an attraction vector using UWB pose information and a repulsion vector using obstacles obtained from scan data within a preset safe distance. The attraction and repulsion vectors are added component-by-component in the robot coordinate system to obtain the resultant force. The resultant force direction angle is calculated as a reference for angular velocity, and the resultant force magnitude is calculated as a reference for linear velocity. When the resultant force direction angle is within a preset negligible range, the angular velocity is set to zero. When the nearest forward distance is less than a preset emergency stop threshold, the velocity is immediately set to zero. When the nearest forward distance is between a preset slow stop threshold and an emergency stop threshold, the controller stops outputting longitudinal linear velocity control values ​​to the robot, while retaining the output of lateral angular velocity control values. Preset acceleration upper limits are applied to both linear velocity and angular velocity to limit their variation within adjacent control cycles. Simultaneously, the upper limit of the amplitude constrains both linear velocity and angular velocity to prevent exceeding the robot's hardware allowable range.

[0032] The combined force of attraction and repulsion allows the robot to actively avoid obstacles while maintaining its target-following tendency, balancing task continuity and obstacle avoidance safety. The direction angle and magnitude of the resultant force correspond to angular velocity and linear velocity, respectively, deeply binding speed adjustment with obstacle avoidance direction and preventing disconnection between direction and speed control. Differentiated processing of different distance thresholds (e.g., emergency stop at zero speed, slow stop with linear velocity restriction and angular velocity preservation) can dynamically adjust motion permissions according to the obstacle risk level, both blocking emergency risks and preserving adjustment space. The acceleration upper limit can prevent sudden jumps in linear velocity and angular velocity, protecting hardware such as drive motors and preventing robot posture oscillations. The resultant force also provides a reference for angular velocity and linear velocity, realizing multi-dimensional control of the robot's motion state by a single physical quantity.

[0033] Furthermore, in step S50, the robot's motion is driven by velocity and angular velocity only when the controller is enabled and the UWB pose information is valid; if the UWB pose information is detected to be invalid in the current control cycle, the angular velocity and linear velocity are set to zero in the current control cycle, and the resultant force calculation and linear velocity and angular velocity constraint steps are skipped, and the subsequent state is processed in the next control cycle; at the same time, the visual markers of attractive force, repulsive force and resultant force are output.

[0034] The dual determination of enable signal and UWB pose information can filter invalid drive commands at the source, avoiding erroneous robot actions when enable fails or data is abnormal, thus improving control reliability. Zero-speed return to zero speed in the current control cycle when UWB pose information fails significantly shortens safety response time, eliminating the need to wait for the next control cycle of the state machine and reducing the risk of uncontrolled robot movement during the failure period. Skipping the resultant force calculation step avoids erroneous outputs caused by invalid data participating in calculations, while also saving computational resources of the following system. Visual markers intuitively present the force field state, facilitating real-time monitoring of the robot's obstacle avoidance logic and providing a clear basis for troubleshooting and parameter tuning. Zero-speed zeroing covers two scenarios: emergency safety handling when UWB pose information fails, and routine safety management when enable signals are invalid, achieving multi-scenario adaptation of a single safety mechanism.

[0035] Furthermore, in step S10, after power-on initialization, the robot's historical motion trajectory is obtained based on the robot's historical operation data, as well as multi-frame LiDAR scanning data collected during the historical motion process. The relative positions of obstacles and the robot are obtained through the scanning data. Combined with the robot's historical motion trajectory, an environmental map consisting of the robot's movable range during operation is constructed. At the same time, the continuously collected UWB pose information of the target object is combined with the collection time to form the target object's historical movement trajectory. The historical movement trajectory of the target object and the environmental map are used to construct and train a trajectory prediction model. The trajectory prediction model takes the current movement trajectory of the target object as input and outputs the target object's subsequent movement trajectory.

[0036] In step S30, when the current state is the following state, the current movement trajectory is generated based on the currently continuously acquired effective smooth UWB pose information. The current movement trajectory is input into the trajectory prediction model to obtain the subsequent movement trajectory of the target object as the predicted movement trajectory. The current movement speed of the target object is calculated based on the acquisition time of the UWB pose information. In step S50, when calculating the speed, the predicted movement trajectory, the environment map, and the current movement speed are combined to generate the optimal pursuit trajectory for the robot to chase the target object in the environment map under the speed and acceleration limits.

[0037] In this scheme, the environmental map not only avoids fixed obstacles by clearly defining the movable range, but also provides spatial constraints for the generation of the optimal trajectory, preventing the trajectory from exceeding the safe area; UWB pose information is used to construct the historical movement trajectory of the target object to support model training, and to generate the current movement trajectory as prediction input. It can also be combined with the acquisition time to calculate the target's current speed, improving the utilization rate of a single data dimension; the trajectory prediction model can not only output the target's subsequent trajectory in advance to reduce following lag, but also adapt to the target's movement habits based on historical data, reducing the adjustment range during sudden turns.

[0038] Further, in step S50, after generating the optimal pursuit trajectory and before outputting the speed command, the latest frame of effective smoothed UWB pose information is obtained as the real-time position, and the deviation value between the real-time position and the predicted movement trajectory is compared; the Boolean value and the forward nearest distance obtained from the current LiDAR scanning data processing are obtained; if the deviation value is less than the preset deviation threshold and there are no new obstacles, the linear velocity and angular velocity are calculated using the optimal pursuit trajectory; if the deviation value is not less than the preset deviation threshold or the forward nearest distance is less than the preset slow stop distance threshold, the coverage area of ​​the predicted movement trajectory in the environment map is reduced according to the preset prediction ratio as the replanning range, the current movement trajectory is updated with the real-time position of the target object, the current movement trajectory is input into the trajectory prediction model to obtain the updated predicted movement trajectory, and the predicted movement trajectory is constrained according to the replanning range.

[0039] The latest UWB pose information provides real-time position data that can be compared with the predicted trajectory to identify deviations and promptly detect sudden changes in the target's position. It also serves as a benchmark for updating the current movement trajectory, ensuring the accuracy of the trajectory input. Simultaneously, this invention uses Boolean values ​​to determine the presence of new obstacles, and the forward nearest distance to trigger a replanning threshold. The combination of these two methods prevents trajectory failure due to misjudgment of obstacles. The replanning range plays a dual role: by reducing the coverage area, it decreases the trajectory adjustment amplitude, preventing frequent large-scale changes that could cause robot instability; and it also constrains the updated predicted trajectory, ensuring it falls within a safe and movable area.

[0040] Furthermore, after obtaining the predicted movement trajectory in step S50, the speed of the target object at each point on the predicted movement trajectory is extracted as the predicted speed. Points on the predicted movement trajectory whose predicted speed is less than the preset static speed threshold are selected as static points. The time period of the target object at the static point is obtained by combining the predicted speed as the static time period.

[0041] When generating the optimal pursuit trajectory, the start time of the next stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is determined by combining the velocity and acceleration limit values. If it is not later than the pursuit deadline, a speed priority instruction is generated. If it is later than the pursuit deadline, the end time of the stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is determined again. If it is not later than the pursuit deadline, a smoothness priority instruction is generated. If it is later than the pursuit deadline, the start time of the next stationary time period or the time when the target object reaches the end of the predicted movement trajectory is used as the pursuit deadline.

[0042] The time it takes for the robot to move from the starting point to the ending point of the optimal pursuit trajectory is taken as the pursuit time. After determining the pursuit deadline, if a speed priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum pursuit time. If a smoothness priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum number of changes in linear velocity and angular velocity on the optimal pursuit trajectory, provided that the pursuit time is not later than the pursuit deadline.

[0043] The predicted velocity is used to filter out stationary periods where the predicted velocity is lower than the preset value, identifying the target's stationary state, and also helps to determine the target's movement rhythm, avoiding misjudging brief slow movements as stationary periods. The stationary period plays a crucial role, serving both as a basis for determining the pursuit deadline, preventing the robot from meaninglessly pursuing the target object at high speed, and providing a time constraint for trajectory generation, preventing a misalignment between the pursuit time and the target's stationary time. The pursuit deadline has a two-way adjustment function: it determines whether to adopt a speed-first strategy by comparing it with the fastest pursuit time, improving following efficiency in high-efficiency scenarios; and it can switch to a smoothness-first strategy when fast pursuit is not possible, reducing frequent changes in velocity and angular velocity. Attached Figure Description

[0044] Figure 1 This is a flowchart of the adaptive following method for a mobile robot based on UWB and LiDAR in Example 1;

[0045] Figure 2 This is a flowchart of the decision-making process of the state machine decision-making module in Example 1;

[0046] Figure 3 This is a flowchart of the state machine decision module switching states in Example 1;

[0047] Figure 4 This is a schematic diagram of the functional modules of the mobile robot adaptive following system based on UWB and LiDAR in Example 1.

[0048] Figure 5 This is a flowchart of the calculation of speed and acceleration by the follower controller in Example 1.

[0049] Figure 6 This is a flowchart of the dual-layer anti-concurrency mechanism of the mobile robot adaptive following system based on UWB and LiDAR in Example 1.

[0050] Figure 7 This is a schematic diagram illustrating the calculation of attractive force, repulsive force, and resultant force in Example 1.

[0051] Figure 8 This is a flowchart of the obstacle avoidance controller calculating linear velocity and angular velocity in Example 1. Detailed Implementation

[0052] The following will describe the concept and technical effects of the present invention clearly and completely with reference to embodiments, so as to fully understand the purpose, features and effects of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are all within the scope of protection of the present invention.

[0053] Example 1

[0054] Ultra-Wideband (UWB) is a carrier-free wireless communication and positioning technology that transmits narrow pulse signals in the nanosecond to picosecond range, combining high-precision positioning with low-power communication capabilities. In this invention, the target object carries a UWB tag, and the robot carries a UWB antenna. The UWB tag continuously sends the robot real-time UWB pose information of the target object (including the target object's identifier, hereinafter referred to as "UWB target"; the target object's current position, hereinafter referred to as "orientation"; and the distance between the target object and the robot, hereinafter referred to as "smooth distance"). The robot receives the UWB pose information through the UWB antenna. After preprocessing and filtering by the preprocessing module, the UWB pose information becomes the core data source for the state machine decision module to make decisions about the current state (including following, obstacle avoidance, and idle), and for the controller to calculate speed. If the robot does not receive UWB pose information within a preset UWB timeout threshold, the timed-out UWB pose information is considered invalid.

[0055] In this invention, "enabling" refers to actively granting a controller the authority to operate; it is a signal command to start or activate the controller. "Being enabled" means that the controller successfully receives the enable signal, and after being enabled (receiving the enable signal), the controller enters an active, operational state. When the state machine decision module switches states (e.g., from idle to follow, or from follow to obstacle avoidance), it sends an enable signal to the corresponding controller, allowing it to participate in speed calculation and output control commands (e.g., linear velocity, angular velocity). After receiving the enable signal, the controller calculates the speed according to preset logic (e.g., the PID algorithm of the follow controller) and outputs the constrained commands to the robot actuator.

[0056] In this invention, "the moment the controller is enabled" refers to the first moment when the controller receives the enable signal, and "the enabled controller" refers to the controller that receives the enable signal.

[0057] like Figure 1 As shown, the adaptive following method for mobile robots based on UWB and LiDAR includes the following steps:

[0058] S10: Set the idle state, follow state, and obstacle avoidance state; power on the robot and load parameters, including the UWB timeout threshold (set by the administrator to compare and judge whether the UWB pose information has timed out or is valid), the de-jittering time for the appearance and disappearance of obstacles, the switching cool-down time, the speed and acceleration limit values, initialize the current state to the idle state, and clear the internal state of the controller, which includes the follow state and obstacle avoidance state;

[0059] S20: Collect UWB pose information and LiDAR scanning data; perform jitter removal processing on the scanning data according to the jitter removal time of obstacle appearance and disappearance to obtain smooth pose and Boolean values ​​representing whether there are obstacles in front and the nearest forward distance;

[0060] S30: Within each preset control cycle, determine whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If invalid or timed out, switch the robot's current state to the idle state. After the robot enters the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooling timer ends, switch the current state to the following state or the obstacle avoidance state according to the Boolean value.

[0061] S40: Enables the controller corresponding to the current state after the switch only when switching states, and disables other controllers; the internal state of the controller is cleared once when it is enabled.

[0062] S50: The enabled controller takes the smoothed UWB pose information or the nearest forward distance as input, calculates the linear velocity or angular velocity, constrains the changes in linear velocity and angular velocity within the control cycle according to the velocity and acceleration limit values, and outputs the constrained data to the robot for execution.

[0063] This embodiment also includes a mobile robot adaptive following system based on UWB and LiDAR (hereinafter referred to as the "following system") that uses a mobile robot adaptive following method based on UWB and LiDAR.

[0064] like Figure 4 As shown, the following system includes:

[0065] The preprocessing module powers on the robot and loads parameters, including UWB timeout threshold, de-jittering time for obstacle appearance and disappearance, switching cooling time, speed and acceleration limit values.

[0066] The data acquisition and obstacle detection module includes a UWB tag on the target object and a UWB antenna on the robot, used to acquire UWB pose information and LiDAR scanning data; the UWB pose information is smoothed, and the scanning data is de-jittered according to the de-jittering time of the appearance and disappearance of obstacles, so as to obtain a smooth pose and a Boolean value representing whether there are obstacles in front and the nearest forward distance.

[0067] The state machine decision module sets up an idle state, a follow state, and an obstacle avoidance state. After the robot is powered on, the current state is initialized to the idle state, and the internal state of the controller is cleared, including the follow state and the obstacle avoidance state. Within each preset control cycle, the module determines whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If invalid or timed out, the robot's current state is switched to the idle state. After the robot enters the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooling timer ends, the current state is switched to the follow state or the obstacle avoidance state according to the Boolean value.

[0068] When the controller is enabled, its internal state is cleared to zero instantly. When disabled, it immediately releases zero velocity and enters the disabled state. It acquires smoothed UWB pose information and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution.

[0069] The obstacle avoidance controller, when enabled, instantly clears its internal state to zero; when disabled, it immediately releases zero speed and enters the disabled state. It acquires Boolean values ​​representing whether there is an obstacle ahead and the nearest forward distance, and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution.

[0070] The state machine decision module enables the controller corresponding to the current state after the switch only when switching states, and disables other controllers.

[0071] Specifically, such as Figure 3 As shown, the workflow of the following system includes the following steps:

[0072] After the robot is powered on, the following system runs. The preprocessing module loads parameters, including UWB timeout threshold, de-jittering time for obstacle appearance and disappearance, switching cooldown time, detection threshold, control gain, dead zone, and amplitude limiting. The state machine decision module initializes the current state to idle and clears the controller's internal state. The controller includes a following controller and an obstacle avoidance controller.

[0073] The data acquisition and obstacle detection module controls the parallel acquisition of UWB pose information (by the robot) and LiDAR scanning data from the sensors. Specifically, the UWB acquisition module provides distance, orientation, and validity indicators at a fixed frequency; the LiDAR provides single-frame scan data at a fixed control cycle.

[0074] like Figure 3 As shown, the acquisition and obstacle detection modules perform preprocessing of UWB pose information and LiDAR scanning data, respectively. The UWB pose information preprocessing follows a sequence: first, median processing removes impulse noise; then, low-pass processing filters out high-frequency interference; finally, order-preserving processing maintains the signal timing, outputting smoothed distance, azimuth, and validity. The LiDAR side only processes the forward sector. After coordinate alignment, it first performs clustering to obtain obstacle clusters; if clustering is unstable, it then selects the nearest forward obstacle point to fill the gap. This yields a Boolean value indicating whether there is a forward obstacle and the nearest forward distance. Simultaneously, simple visual markers are generated for easy debugging.

[0075] Next, the state machine decision module performs state determination. This involves reading and judging whether the UWB pose information within each control cycle is valid or has timed out, thus obtaining the robot's current state. Based on the state determination result for this control cycle, the state machine decision module switches the robot's current state to idle, following, or obstacle avoidance. If the UWB pose information is invalid or has timed out, the robot immediately enters the idle state, the currently enabled controller issues a zero speed (i.e., the robot's movement speed is reduced to zero), the state machine decision module performs a one-time reset (i.e., clears all data from this control cycle), and begins a cooldown timer. If the UWB pose information is valid and the current state is not in the cooldown period, the state machine decision module selects between a pure following state and an obstacle avoidance state based on the presence of obstacles. If the current state is in the cooldown period, the state machine decision module maintains the previous state.

[0076] The state machine decision module sends a control command to the controller only once when the state changes within the current control cycle. This enables the controller corresponding to the new state (the switched state) while simultaneously disabling the other controller. The control command is sent only once throughout the entire control cycle to avoid both controllers writing speeds simultaneously. If the state result of the current control cycle is a change to an idle state, a zero-speed command is immediately issued to each controller after receiving the state result, and a one-time reset is performed. If the state result of the current control cycle is a change to a pure follower state, the outer layer executes a single-time enable of the follower controller and disables the obstacle avoidance controller within the current control cycle. If the state result of the current control cycle is a change to an obstacle avoidance state, the outer layer executes a single-time enable of the obstacle avoidance controller and disables the follower controller within the current control cycle.

[0077] After the state machine decision module enables the follower controller, the follower controller controls the longitudinal speed (including forward or backward speed) through the PID algorithm to stably maintain the following distance from the target; it plans the lateral adjustment set (including left or right position adjustment) using geometric relationships; and it guides the wheel angle allocation when the robot turns through the Ackerman algorithm to make the turning smoother.

[0078] After the state machine decision module enables the obstacle avoidance controller, the obstacle avoidance controller combines the attractive force of the target object's UWB pose information on the robot with the repulsive force of the obstacle on the robot to guide the robot's movement.

[0079] When the follow system receives a stop command (i.e., a command to stop the robot from running), it enters the inner zero-speed interlock, forcibly stopping the robot and ensuring safety.

[0080] Once the enabled controller is activated, the outputs of both the follower controller and the obstacle avoidance controller are uniformly fed into the same limiting module. This module constrains the speed and acceleration within each control cycle, ensuring that the chassis acceleration and maximum speed limits are met. The limited speed command is then sent to the chassis, i.e., the robot's actuators. Simultaneously, the current operating status is published, and the monitoring data from the follower system and the robot are stored as log data for online observation and offline verification. The next control cycle then begins. In this way, the robot can stably follow the target object when it is visible and the path to follow it is feasible; it can promptly avoid obstacles on the path; and it can quickly stop and remain safe when UWB pose information is abnormal.

[0081] In practical, diverse following scenarios, such as Figure 2 As shown, for each control cycle, the decision-making process of the state machine decision module includes the following steps:

[0082] At the beginning of each control cycle, the state machine decision module reads whether the UWB pose information is valid or has timed out; it also reads whether there are obstacles ahead and the duration of the obstacle avoidance state.

[0083] If the UWB pose information is valid, first check if it is within the cooldown timer. If it is in the cooldown period, maintain the original state and directly enter the control execution of this control cycle; if it is not in the cooldown period, determine whether the obstacle avoidance state has continuously reached the set duration.

[0084] If the UWB pose information is invalid or has timed out, immediately set the current state to idle, then send a zero-speed command to the robot, record the switching time, mark a one-time reset, and start the cooling timer; after the cooling is completed, determine whether the UWB pose information is valid and has not timed out. If not, maintain the idle state until the end of the current control cycle; if so, determine whether the obstacle avoidance state has continuously reached the set duration.

[0085] If the obstacle avoidance state continues for a set duration, the state machine decision module will set the current state to obstacle avoidance state. In this control cycle, only the obstacle avoidance controller will be enabled and the follower controller will be disabled. Then, the cooling timer will start, and it will be determined whether a stop command has been received. Otherwise, the current state will be set to pure follower state. In this control cycle, only the follower controller will be enabled and the obstacle avoidance controller will be disabled. Cooling will be enabled, and it will be determined whether a stop command has been received.

[0086] If a stop command is received, zero-speed interlock is executed, the current control cycle immediately reaches zero speed and stops writing speed, waiting for the follow system to be reactivated. After the follow system is reactivated, a new control cycle begins. If no stop command is received, speed is calculated based on the unified amplitude limit, and the linear velocity and angular velocity are published after the calculation until a new control cycle begins.

[0087] In the decision-making process of the aforementioned state machine decision module, the following system determines the state based on data collected by multiple sensors. When switching the current state, the arrival frequency and delay of UWB pose information and LiDAR scanning data are different. If the state is switched immediately based on the result of a single frame of data, it is easy to cause back-and-forth jitter, where the previous frame determines the state to be set to the following state and the next frame determines the state to be set to the obstacle avoidance state. Furthermore, each state switch must be completed in one go, and there must be a cooling-off period after completion. No new state switch will be triggered before the cooling-off period ends; otherwise, continuous switching and jitter will occur. In this embodiment, the following system uses different minimum durations for the de-jittering time of obstacle appearance and disappearance. The state change is only recognized when the minimum duration is continuously met. After each switch, a fixed cooling-off period is entered, and the state does not switch again within the cooling-off window, thereby achieving the de-jittering effect and avoiding repeated switching.

[0088] In complex real-world environments, indoor multipath propagation and occlusion can cause UWB pose information to become temporarily invalid or arrive late. If these anomalies are not handled preferentially, the following system will frequently switch between following and obstacle avoidance states (or following and idle states). In this embodiment, within any control cycle, as soon as invalid or timed-out UWB pose information is detected, the following system is immediately set to an idle or safe state (i.e., the distance between the robot and the obstacle is not less than a preset safe distance), and a one-time reset is triggered. This determination has higher priority than the obstacle avoidance state, preventing the robot from frequently switching between following and obstacle avoidance states.

[0089] If the follow controller and obstacle avoidance controller write speed simultaneously during the same period, a conflict will occur, manifesting as speed spikes or sudden changes in direction, posing a safety hazard. In this embodiment, during the control cycle in which the state change is confirmed, only one control switch signal is sent (i.e., the state machine decision module only sends one enable signal). This means that the controller enabled in the previous state is disabled, and the other controller is enabled; the controller enabled in other control cycles remains enabled. This ensures that only one controller has write speed authority at any given time, avoiding safety hazards caused by speed spikes or sudden changes in direction.

[0090] The azimuth angle of the UWB pose information and the forward sector of the radar must fall within the same vehicle coordinate system (i.e., the positive x-axis points forward and the positive y-axis points to the left). If the extrinsic parameter calibration is inaccurate, yaw is not compensated, or the angle sign convention is inconsistent, contradictions will occur (e.g., the UWB pose information points forward, but the radar interprets it as lateral or rearward), leading to incorrect switching and jitter. In this embodiment, to ensure the consistency of the state machine decision module's judgment results, all sensor-collected data has been converted to the vehicle coordinate system before entering the state machine decision module, and yaw compensation and extrinsic parameter calibration have been completed. This ensures that the azimuth angle of the UWB pose information and the determination result of forward obstacles are defined under the same coordinate semantics, avoiding incorrect switching due to coordinate inconsistencies.

[0091] Before activating the following system, install it in the robot. After activating the robot, power on the following system and initialize all functional modules. First, with the robot as the origin, x is the front of the robot and y is the left, ensuring the positive azimuth directions are consistent; load parameters such as UWB timeout threshold, obstacle appearance and disappearance de-jitter time, switching cooldown time, and speed and acceleration limits; align the received UWB pose information, LiDAR scan data, and robot coordinates to avoid inconsistencies between the scan data and UWB pose information in subsequent judgments, completing the external participant yaw alignment.

[0092] When the preprocessing module loads and processes UWB pose information, it obtains the raw distance (distance between the target object and the robot without smoothing), orientation, and validity flag (used to indicate whether the UWB pose information is valid) from the UWB pose information. First, it uses median filtering to remove abnormal data from the UWB pose information, and then uses a first-order low-pass filter for smoothing. If the UWB pose information in the current frame is invalid, it temporarily uses the most recently acquired valid UWB pose information, and then obtains the smoothed distance, orientation, and validity (of the UWB pose data). In this way, the state machine decision module determines whether the UWB pose information is invalid or has timed out.

[0093] When loading and processing LiDAR data, single-frame scan LiDAR data is acquired. Only the forward sector is retained from the acquired single-frame scan data. After coordinate alignment, clustering is performed to obtain the shortest distance from the target object to its corresponding forward cluster center. Then, the straight-line distance between the nearest point to the target object and the reference point within the forward sector is calculated using geometric methods as a fallback. The minimum of the two is taken. This yields the information on whether there are obstacles ahead (represented by Boolean values) and the nearest forward distance (in meters).

[0094] The state machine decision module sets the initial state to idle. Each control cycle reads two types of information: whether the UWB pose information is valid or has timed out, and whether there are obstacles ahead (for duration determination). If the UWB pose information is invalid or has timed out, it immediately enters idle mode and sends a zero-speed signal. If the UWB pose information is valid and not in the cooldown period, it chooses between a pure following state and an obstacle avoidance state. If it is in the cooldown period, it maintains the previous state. Only when the obstacle avoidance state changes within the current control cycle is an enable signal sent only once to the controller, i.e., enabling the target controller and disabling the other controller, to avoid both controllers writing speeds simultaneously.

[0095] When the follower controller is enabled, the linear velocity and angular velocity are calculated by controlling the longitudinal speed using the PID algorithm, planning the lateral adjustment set, and guiding the steering angle allocation using the Ackerman algorithm. When the follower controller is disabled, zero speed is output immediately within the current control cycle and the operation stops.

[0096] When the obstacle avoidance controller is enabled, it generates speed by calculating the direction and magnitude of the resultant force by combining the attractive force of the target object on the robot and the repulsive force of the obstacle on the robot, and executes angle dead zone and slow or emergency stop; when the obstacle avoidance controller is disabled, it immediately outputs zero speed and stops within the current control cycle.

[0097] When the follower controller and obstacle avoidance controller execute the above process, the output results must be subject to the same speed limit and acceleration limit constraint method before being sent to the robot chassis to ensure smooth switching and meet physical constraints.

[0098] As the system runs, it publishes its current running status (such as FOLLOW / AVOID / IDLE), records timestamps and key quantities (status, speed, nearest distance), which facilitates subsequent review and parameter tuning.

[0099] The goal of follow control is to maintain the desired following distance for the robot and reduce lateral and heading deviations. In step S20, as... Figure 5 The UWB data preprocessing process is shown in the dashed box. The preprocessing module preprocesses the UWB pose information (including the original distance between the target object and the robot, the orientation of the target object, and the validity of the UWB pose information). This involves filtering the UWB pose information. The filtering process uses the original distance, orientation, and validity (of the UWB pose information) obtained from the UWB pose information (without smoothing) as the raw data. Median filtering and low-pass smoothing are applied to the raw data. When the UWB pose information in the current frame is invalid, the previously valid UWB pose information is temporarily called to maintain output continuity (e.g., ...). Figure 5As shown in "Reuse the most recent dropped frame"), when the deviation between the current valid frame and the historical smoothing data exceeds the limit, the filter's historical buffer is cleared and reinitialized with the current frame's UWB pose information (e.g., Figure 5 The "Reset when the deviation between valid frames and historical smoothed data exceeds the limit" mechanism is shown to ensure that downstream modules (acquisition and fault diagnosis modules) obtain reliable UWB pose information. This yields smoothed distance, orientation, and validity as smoothed data, which is then validated for validity.

[0100] If the enable signal disables the controller, the inner layer of the controller issues a zero speed for zero speed interlock, and the state machine decision module (outer layer) issues a zero speed, and the controller does not enter the speed calculation process; if the enable signal enables the controller, the state machine decision module determines whether the UWB pose information is valid. If it is invalid, it issues a zero speed; if it is valid, it calculates the control error.

[0101] In step S20, UWB pose information is acquired asynchronously. In step S50, when constraining the angular velocity, the actual distance obtained from the filtered UWB pose information is acquired in real time. The actual distance is compared with the preset target distance to obtain the distance error. The control error is calculated, including heading error and lateral error. The azimuth angle (in degrees) of the filtered UWB pose information is converted to radians to obtain the angle between the robot's orientation and the target object's direction, which is used as the heading error. Based on the heading error and the actual distance, the vertical distance from the robot's central axis to the target is calculated as the lateral error. Then, the angular velocity and linear velocity are calculated using lateral control and longitudinal control methods respectively.

[0102] When calculating angular velocity, a Stanley-like control logic is used, combining heading and lateral errors to calculate the desired front wheel steering angle. Simultaneously, the front wheel steering angle is limited within the robot's mechanical structural parameters (i.e., the steering angle is limited to physical limits) to avoid exceeding the robot's steering capabilities (i.e., preventing sharp turns). The front wheel steering angle is converted into angular velocity using the robot's physical parameters. Specifically, the administrator-preset Ackerman algorithm converts the front wheel steering angle into angular velocity (formula: angular velocity = linear velocity × tan(steering angle) / wheelbase), where the wheelbase is a robot physical parameter that needs to be accurately measured beforehand. If the wheelbase is too small (less than the wheelbase threshold set by the administrator), the angular velocity is automatically set to 0 to avoid division by zero errors.

[0103] When calculating linear velocity, the distance error is input into a preset positional PID controller, which, in conjunction with preset proportional, integral, and derivative gains, calculates the speed control quantity. The integral term output is limited to ±2 to prevent speed overshoot caused by integral saturation (i.e., overshoot prevention). When the actual distance is less than the preset target distance set by the administrator, the speed is dynamically reduced through the distance proportional coefficient to prevent the vehicle from getting too close to the target; at the same time, the maximum linear velocity is limited to ensure smooth driving.

[0104] Then, based on the results of lateral and longitudinal control, the speed or acceleration is limited, and a speed command is generated and issued to the robot.

[0105] The follower controller outputs velocity and angular velocity commands to the / cmd_vel topic to drive the robot's movement only when the enable signal is valid and the UWB pose information is valid. If the enable signal or data is invalid, a stop command is immediately issued to the robot. Upon receiving an external reset signal, the PID controller's historical error and integral term are cleared, restoring the initial control state. If the follow timeout occurs, a stop command is triggered, and the follower waits for repositioning before restarting.

[0106] like Figure 6 As shown, the state within the current control cycle is obtained. When the outer layer is deactivated (i.e., the state machine decision module switches from a pure follower state to an obstacle avoidance state or an idle state), the state machine decision module sends an enable signal only once within the control cycle of the state change, enabling the target controller and disabling the other controller. Once the follower controller receives the disabled enable signal, it immediately issues a zero-speed and stops writing speed within the control cycle; speed calculation and output only resume after the state machine decision module re-enables a controller (i.e., single enable). This inner zero-speed interlock and the outer single enable together form a two-layer anti-concurrency mechanism. The outer layer ensures which controller can write linear velocity or angular velocity, while the inner layer ensures that the controller whose enable has been deactivated will no longer write linear velocity or angular velocity. The two layers work together to ensure that only one speed output source is maintained within a control cycle; there is no concurrent writing, no speed spikes, and the trajectory transitions smoothly.

[0107] The obstacle avoidance controller aims to decelerate or detour around obstacles in a timely manner while maintaining general directional tracking. The controller's inputs include smoothed distance and orientation data from UWB pose information, an enable flag from the state machine decision module, and single-frame point cloud data from the LiDAR. If the enable flag is false, the UWB pose information is invalid, or a timeout occurs, the current control cycle directly outputs zero speed and returns without entering the calculation process.

[0108] In step S50, such as Figure 7 and Figure 8 As shown, in obstacle avoidance mode (when only the obstacle avoidance controller is enabled), the obstacle avoidance controller constructs a vector representing the attractive force using UWB pose information, and constructs a vector representing the repulsive force using obstacles obtained from LiDAR scanning data within a preset safe distance (forming a set of forward obstacles within the safe radius). The attractive force vector and the repulsive force vector are added component by component in the robot coordinate system to obtain the resultant force, thereby completing the vector synthesis. The direction angle of the resultant force is calculated as a reference quantity for angular velocity, and the magnitude of the resultant force is calculated as a reference quantity for linear velocity.

[0109] To ensure stability and safety, the controller introduces three constraint methods based on the obtained forward nearest distance:

[0110] Firstly, the angular dead zone. When the direction angle of the resultant force is within a preset negligible range, the angular velocity is set to zero (corresponding to...). Figure 8 The "immediate zero speed" shown avoids slight back-and-forth oscillations near zero.

[0111] Secondly, such as Figure 8 As shown, slow stop and emergency stop gating. When the nearest forward distance is less than a preset emergency stop threshold, the speed is immediately set to zero; when the nearest forward distance is between the preset slow stop threshold and the emergency stop threshold, the output of longitudinal linear velocity control to the robot stops, while the output of lateral angular velocity control to the robot is retained (corresponding to...). Figure 8 The "turn only, don't move forward" approach demonstrates a progressive safety strategy when approaching obstacles.

[0112] Thirdly, smoothing and limiting. Preset acceleration upper limits (administrator settings) are applied to both linear velocity and angular velocity to limit their variation within adjacent control cycles. Simultaneously, the upper limit value constrains the linear velocity and angular velocity (corresponding to...) Figure 8 The “Speed / Acceleration Limit” shown above generates speed commands from the obtained linear and angular velocities and issues them to the robot to prevent the speed or acceleration from exceeding the robot’s hardware limits.

[0113] The above three constraint methods work together with the cooling window (cooling timer) of the state machine decision module to make the speed transition when the state changes from the following state to the obstacle avoidance state smooth and controllable, protecting the actuator and reducing attitude oscillation.

[0114] like Figure 7 As shown, the repulsive force is centered on the robot and points in the opposite direction to the obstacle within the safe radius; the attractive force is centered on the robot and points in the direction of the target object; the resultant force is the vector formed by the fusion of the attractive and repulsive forces, with the direction angle of the resultant force serving as a reference for the angular velocity and the magnitude of the resultant force serving as a reference for the linear velocity.

[0115] Only when the controller is enabled and the UWB pose information is valid will the enabled controller output velocity and angular velocity commands to the / cmd_vel topic in ROS, using the output velocity and angular velocity to drive the robot's movement. If the UWB pose information is detected as invalid in the current control cycle, the angular velocity and linear velocity will be set to zero in the current control cycle, and the resultant force calculation and linear and angular velocity constraint steps will be skipped. The state machine decision module will then handle the subsequent states in the next control cycle.

[0116] Once the state machine decision module receives a deactivation instruction, i.e., the state transitions from obstacle avoidance to pure follow or idle, it immediately sends a zero-speed signal to the controller and stops writing speed within the current control cycle, while simultaneously clearing locally stored data. Calculation and output only resume after the controller is re-enabled. The obstacle avoidance and pure follow states are mutually exclusive. The state machine decision module only allows a switch back from obstacle avoidance to pure follow when obstacles in the forward sector have continuously disappeared for the required clearing time and the cooldown timer has ended. The control cycle in which the state transition occurs sends only one enable signal, enabling the follow controller and disabling the obstacle avoidance controller. Furthermore, the same speed and acceleration limiting method is used before and after the state transition, thus preventing speed spikes or jumps.

[0117] In practice, after the robot is powered on, parameters adapted for home safety are loaded: maximum linear velocity is set to 0.3m / s (to avoid excessive collisions), acceleration limit is 0.1m / s² (to prevent sudden acceleration from startling the elderly), UWB timeout threshold is 500ms (due to the presence of walls and furniture in homes, which can cause rapid response signals to fail), obstacle appearance and disappearance de-jitter time is 200ms (to filter temporary obstacles such as slippers and fallen tissues), and switching cooldown time is 1s (to avoid frequent switching affecting stability); the initial state is "idle", and the internal state of the follower or obstacle avoidance controller is cleared.

[0118] Elderly person A wears a UWB tag. The robot collects UWB pose information (smoothing distance, orientation, UWB target) through a preprocessing module. The preprocessing of UWB pose information follows the sequence of first removing impulse noise through median filtering, then filtering high-frequency interference through low-pass filtering, and finally maintaining the signal timing. Specifically, median filtering removes outliers caused by reflections from metal furniture, and low-pass filtering smooths the pose fluctuations of the elderly person's slow movement. If the current frame's UWB pose information is invalid (e.g., the elderly person enters the bedroom and is blocked by a wall), the most recently acquired valid UWB pose information is temporarily used to maintain the output. Assuming that the LiDAR scan data preprocessing only processes the forward 60° sector (the home space is narrow, and a wide-range scan is not required), after coordinate alignment, clustering is performed (e.g., identifying fixed obstacles such as sofas and coffee tables). If the clustering is unstable, the nearest obstacle point in the forward sector is used as a fallback. After de-jittering, a Boolean value indicating whether there is an obstacle is output (e.g., if there are slippers near the elderly person's feet, it is determined that there is no obstacle after de-jittering) and the nearest forward distance (e.g., 1.2m from the coffee table).

[0119] When switching states, only one command is issued. When switching to the pure follow state, the follow controller is enabled and the obstacle avoidance controller is disabled, and the historical PID error of the follow controller is instantly cleared. When switching to the "obstacle avoidance" state, the opposite is true: the follow controller is disabled and it immediately outputs zero speed.

[0120] In pure follow mode, assuming the follower controller uses a target distance of 1m as a reference, the longitudinal PID calculates the linear velocity (e.g., 0.2m / s when the elderly walk slowly, and the speed is reduced to 0.1m / s by the proportional coefficient when the distance is less than 0.8m), and the lateral geometry calculates the turning angle based on the azimuth angle of the UWB pose information (e.g., when the elderly turn to the kitchen, the turning angle is limited to ±15° to prevent sudden turns), and converts it into angular velocity through the Ackerman algorithm to ensure smooth turning.

[0121] In obstacle avoidance mode, the obstacle avoidance controller constructs an attractive force pointing towards the elderly person using UWB pose information and a repulsive force away from the obstacle using radar obstacle information. The resultant force direction guides the elderly person to detour (such as avoiding the dining table). At the same time, constraints are triggered. The constraints are assumed to be: when the nearest forward distance is <0.5m (emergency stop threshold), the speed immediately drops to zero; when 0.5m < distance <1m (slow stop threshold), the linear velocity stops, but the angular velocity is maintained (only turning without moving forward). The change in linear velocity or angular velocity does not exceed the acceleration limit to avoid sudden changes.

[0122] In addition, the system publishes "FOLLOW / AVOID / IDLE" status and force field visualization markers in real time, making it easy for family members to view remotely. The log records data such as the elderly person's location and the robot's speed for subsequent parameter fine-tuning.

[0123] In another warehouse scenario, the robot loads parameters adapted to warehouse efficiency, assuming the following: maximum linear velocity of 0.8 m / s (warehouse trolleys move quickly, requiring efficiency matching), acceleration limit of 0.3 m / s², UWB timeout threshold of 1 s (open warehouse space, minimal obstruction, stable signal), obstacle de-jitter time of 500 ms (filtering interference from temporary forklift passage and identifying fixed shelves), and switching cooldown time of 0.5 s (for rapid obstacle switching response). The robot initializes to an "idle" state and clears the controller's internal state.

[0124] Warehouse manager A wears a UWB tag. The preprocessing module filters out interference from the metal frame of the shelves and outputs a smooth distance (assuming the target following distance is 1.5m) and orientation. The LiDAR scan data is processed in a forward 120° sector (the specific angle is determined based on the width of the covered warehouse aisle), and obstacles such as shelves and stacked boxes are identified by clustering (clustering is stable, and the closest distance to the cluster center is preferred). After jitter removal, the Boolean value of the obstacle and its forward distance (e.g., 2.5m from the shelf) are output.

[0125] Assuming a control cycle of 50ms, when UWB pose information is valid (the warehouse loader is moving in the aisle without obstruction), the system operates in pure following mode when there are no obstacles. The following controller outputs a longitudinal PID speed of 0.6-0.8m / s and quickly aligns the warehouse loader laterally (e.g., when the loader turns towards the shelf, the Ackerman algorithm calculates a turning angle of ±25° to suit the warehouse aisle width). Upon encountering a shelf obstacle, the system switches to obstacle avoidance mode. The obstacle avoidance controller combines attractive force (towards the warehouse loader) and repulsive force (away from the shelf), guiding the loader around the shelf (assuming an emergency stop threshold of 1m and a slow stop threshold of 2m to accommodate high-speed buffering). Velocity changes are constrained by acceleration to avoid sudden braking. During state transitions, the corresponding controller is enabled only once, while the other controller is disabled to ensure no speed conflicts. If the UWB pose information is temporarily invalidated due to forklift obstruction, the system immediately switches to idle zero speed, cools down, and resumes following, balancing efficiency with the risk of shelf collisions.

[0126] Example 2

[0127] The only difference between this embodiment and Embodiment 1 is that, in step S10, after power-on initialization, the robot's historical motion trajectory is obtained based on the robot's historical operation data, as well as multi-frame LiDAR scanning data collected during the historical motion process. The relative positions of obstacles and the robot are obtained through the scanning data. Combined with the robot's historical motion trajectory, an environmental map consisting of the robot's movable range during operation is constructed. At the same time, the continuously collected UWB pose information of the target object is combined with the collection time to form the target object's historical movement trajectory. The historical movement trajectory of the target object and the environmental map are used to construct and train a trajectory prediction model. The trajectory prediction model takes the current movement trajectory of the target object as input and outputs the subsequent movement trajectory of the target object.

[0128] When constructing the trajectory prediction model, UWB pose information (e.g., 10Hz acquisition, filtered by median, low-pass, and order-preserving filters) and LiDAR scan data (e.g., constructing a grid environment map) are preprocessed to generate trajectory samples containing input sequences, output labels, and environmental features. The trajectory prediction model employs a two-branch LSTM: the temporal branch extracts trajectory features from the UWB pose information, and the environmental branch extracts grid features; these are then fused to output the predicted movement trajectory. During training, historical robot operation data is obtained based on the training time set by the administrator (e.g., 1 month, 6 months, 1 year, etc.) to train the trajectory prediction model.

[0129] In step S30, when the current state is the following state, the current movement trajectory is generated based on the currently continuously acquired effective smooth UWB pose information. The current movement trajectory is input into the trajectory prediction model to obtain the subsequent movement trajectory of the target object as the predicted movement trajectory. The current movement speed of the target object is calculated based on the acquisition time of the UWB pose information.

[0130] In step S50, when calculating the speed, the predicted movement trajectory, the environment map, and the current movement speed are combined to generate the optimal pursuit trajectory for the robot to chase the target object in the environment map under the speed and acceleration limits.

[0131] In step S50, after generating the optimal tracking trajectory and before outputting the speed command, the latest frame of effective smoothed UWB pose information is obtained as the real-time position, and the deviation value between the real-time position and the predicted movement trajectory is compared; the Boolean value and the forward nearest distance obtained from the current LiDAR scan data processing are obtained; if the deviation value is less than the preset deviation threshold (set by the administrator according to the area and dimensions of the movable range in the environment map) and there are no new obstacles, the linear velocity and angular velocity are calculated using the optimal tracking trajectory; if the deviation value is not less than the preset deviation threshold or the forward nearest distance is less than the preset slow stop distance threshold (set by the administrator according to the robot's physical braking parameters and the de-jittering time of obstacle appearance and disappearance), the coverage area of ​​the predicted movement trajectory in the environment map is reduced according to the preset prediction ratio as the replanning range, the current movement trajectory is updated with the real-time position of the target object, the current movement trajectory is input into the trajectory prediction model to obtain the updated predicted movement trajectory, and the predicted movement trajectory is constrained according to the replanning range.

[0132] After obtaining the predicted movement trajectory in step S50, the speed of the target object at each point on the predicted movement trajectory is extracted as the predicted speed. Points on the predicted movement trajectory whose predicted speed is less than the preset stationary speed threshold (set by the administrator according to the individual situation of the target object, for example, in a warehouse environment, the worker moves faster, and in a home environment, the elderly move slower. The administrator needs to set different stationary speed thresholds according to the target object using the following system) are selected as stationary points. The time period of the target object at the stationary point is obtained by combining the predicted speed as the stationary time period.

[0133] When generating the optimal pursuit trajectory, the start time of the next stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is determined by combining the velocity and acceleration limits. If it is not later than the pursuit deadline, a speed priority instruction is generated. If it is later, the end time of the stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is then determined again. If it is not later than the pursuit deadline, a smoothness priority instruction is generated. If it is later than the pursuit deadline, the start time of the next stationary time period or the time when the target object reaches the end of the predicted trajectory is used as the pursuit deadline.

[0134] The time it takes for the robot to move from the starting point to the ending point of the optimal pursuit trajectory is taken as the pursuit time. After determining the pursuit deadline, if a speed priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum pursuit time. If a smoothness priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum number of changes in linear velocity and angular velocity on the optimal pursuit trajectory, provided that the pursuit time is not later than the pursuit deadline.

[0135] When generating the optimal pursuit trajectory based on the minimum pursuit time, the following formula (1) is used.

[0136] (1),

[0137] t is the pursuit time (the time difference between the starting point and the finish line); As the starting point, The final moment; For linear velocity, These are the upper and lower limits of linear velocity, respectively. , Linear acceleration, angular acceleration , These are the upper and lower limits of the acceleration limit, respectively; This is the deadline for the pursuit.

[0138] Formula (1) minimizes the pursuit time t by constraining the speed and acceleration within a safe range, ensuring that the robot reaches the destination as quickly as possible without exceeding the deadline.

[0139] When generating the optimal pursuit trajectory, the minimum number of changes in linear velocity and angular velocity is used as the criterion, as shown in the following formula (2).

[0140] (2),

[0141] N represents the total number of changes; The number of changes in linear velocity; This represents the number of changes in angular velocity (counted when velocity or angular velocity increases or decreases).

[0142] Formula (2) minimizes the number of changes in linear velocity and angular velocity, reduces sudden motion changes, and ensures a smooth trajectory, provided that the pursuit time t does not exceed the deadline. It is suitable for scenarios that are sensitive to stability.

[0143] In practical implementation, in the home environment of the elderly, the robot needs to prioritize obstacle avoidance and smoothly follow the target object, adapting to the characteristics of the elderly who move slowly and have variable paths (such as stopping midway and making temporary changes). Take elderly person B as an example.

[0144] The elderly person B wears a wristband with a built-in UWB tag (assuming a sampling frequency of 10Hz). A LiDAR is installed on the top of the robot (assuming a 180° scanning range, sampling at 5Hz), and a wheeled drive module is mounted on the bottom (assuming a linear velocity range of -0.1~0.5m / s and an angular velocity range of ±1.5rad / s). During the environmental map construction phase, the LiDAR scans areas such as the living room, bedroom, and kitchen, marking fixed obstacles such as sofas (e.g., x∈[2.5,3.5]m, y∈[1.5,2.5]m) and coffee tables (e.g., x∈[2,2.5]m, y∈[1,1.5]m) as permanently occupied areas, and passageways such as corridors (e.g., x∈[1,4]m, y∈[0.5,1]m) as movable areas, stored in a grid map (assuming a precision of 0.1m×0.1m).

[0145] During trajectory prediction model training, the daily trajectories of elderly person B over the past 30 days are collected (e.g., from the bedroom to the kitchen at 7:00 AM, from the living room to the balcony at 10:00 AM). Assuming 20 input frames and 10 output frames are generated, a dual-branch LSTM is used to train the trajectory prediction model, enabling it to predict the elderly person's habitual paths (e.g., pausing for 2 seconds near the coffee table before walking towards the sofa). In the following phase, if elderly person B walks from the bedroom (assumed to be x=1m, y=3m) to the sofa (assumed to be x=3m, y=2m), the real-time pose of the target is obtained through UWB pose information, and the presence of temporary obstacles (e.g., a water cup casually placed by the elderly person) is monitored using LiDAR scanning data.

[0146] When it is detected that elderly person B is about to stop next to the sofa (assuming a stationary deadline T=5s), a speed-priority command is triggered. This involves calculating the minimum pursuit time using a quadratic programming model. Assuming a linear velocity of 0, the speed accelerates to 0.3m / s (to avoid sudden movements), moving along a cubic trajectory (assumed to be x=-5s³+7s²+1, y=-3s³+4s²+3). When passing a corner in the corridor (assumed curvature κ=0.2rad / m), the linear velocity decreases to 0.2m / s, ensuring a safe distance (assumed to be 0.8m) is reached in 4.8s. For daily following, a smoothness-priority command is used to reduce changes in speed or angular velocity (e.g., only two turns from the living room to the balcony) to avoid startling the elderly person.

[0147] In a warehouse environment, robots need to focus on rapid pursuit and obstacle avoidance under speed constraints, adapting to scenarios with fast-moving staff, dense shelving, and numerous dynamic obstacles (such as forklifts). Let's take warehouse manager B as an example.

[0148] In terms of hardware, the wearable vest of warehouse manager B integrates UWB tags (assuming a sampling frequency of 20Hz, resistant to metal interference), and the robot is equipped with a 360° LiDAR (assuming 10Hz sampling, ranging range of 0.5~20m). The drive module supports linear velocity of 0~1.5m / s and angular velocity of ±3rad / s. When constructing the environmental map, the LiDAR scans fixed obstacles such as shelves (assuming x∈[0,10]m,y∈[2,3]m; x∈[0,10]m,y∈[7,8]m), and the passage (assuming y∈[4,6]m) is marked as a high-priority movable area, which is stored as a grid map (assuming 0.2m×0.2m accuracy).

[0149] The trajectory prediction model is trained using the operational trajectories of warehouse manager B over the past 5 days (e.g., a straight-line journey from shelf A to shelf B, or a 10-second stop in front of shelf C for barcode scanning). The input sample consists of 30 frames of UWB pose information (assuming a 3-second span), and the output is 20 frames of predicted trajectory. The trajectory prediction model is optimized for scenarios involving rapid turns (e.g., workers suddenly turning back). During the follow-up phase, the position of warehouse manager B is tracked in real time using UWB pose information, and the LiDAR updates the obstacle avoidance status every second (e.g., marking temporary obstacles when a forklift enters the aisle).

[0150] When warehouse manager B moves from shelf A (assuming x=2m, y=5m) to shelf B (assuming x=8m, y=5m), with a stationary deadline of T=8s, a speed priority command is triggered. Assuming the trajectory prediction model sets the upper limit of linear velocity to 1.2m / s and the acceleration limit to 0.5m / s² during quadratic programming, the B moves along a straight path (assuming x=6s+2, y=5). When the lidar detects a gap in the shelf (assuming a width of 1.5m), the B quickly turns at an angular velocity of 2rad / s, with a total pursuit time of 7.2s (≤8s). If a forklift temporarily occupies the lane, a smoothing priority command is triggered: the B moves around the forklift with three turns within 8s, maintaining a linear velocity of 0.8~1.0m / s to ensure efficiency and avoid collisions, adapting to the fast-paced operation requirements of the warehouse.

[0151] The above are merely embodiments of the present invention. The invention is not limited to the fields covered by these embodiments. Commonly known structures and characteristics in the solutions are not described in detail here. Those skilled in the art are aware of all common technical knowledge in the field prior to the application date or priority date, are able to access all existing technologies in that field, and have the ability to apply conventional experimental methods prior to that date. Those skilled in the art can, based on the guidance provided in this application, improve and implement the invention in combination with their own capabilities. Some typical known structures or methods should not be obstacles for those skilled in the art to implement this application. It should be noted that those skilled in the art can make several modifications and improvements without departing from the structure of the invention. These should also be considered within the scope of protection of the invention, and will not affect the effectiveness of the invention or the practicality of the patent. The scope of protection claimed in this application should be determined by the content of its claims. The specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. An adaptive following method for mobile robots based on UWB and LiDAR, characterized in that, Includes the following steps: S10: Set the idle state, follow state, and obstacle avoidance state; power on the robot and load parameters, including UWB timeout threshold, obstacle appearance and disappearance de-jittering time, switching cooldown time, speed and acceleration limit value; initialize the current state to the idle state and clear the internal state of the controller, which includes the follow state and obstacle avoidance state. S20: Collect UWB pose information and LiDAR scanning data; smooth the UWB pose information; perform jitter removal on the scanning data based on the jitter removal time of obstacle appearance and disappearance to obtain smooth pose and Boolean values ​​representing whether there are obstacles in front and the nearest forward distance. S30: Within each preset control cycle, determine whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If it is invalid or has timed out, switch the robot's current state to the idle state. After the robot switches to the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooldown timer has ended, the current state is switched to either the follow state or the obstacle avoidance state according to the Boolean value. S40: Enable the controller corresponding to the current state after the switch only when switching states, and disable other controllers; The controller's internal state is instantly cleared upon being enabled. S50: The enabled controller takes the smoothed UWB pose information or the nearest forward distance as input, calculates the linear velocity or angular velocity, constrains the changes in linear velocity and angular velocity within the control cycle according to the preset velocity and acceleration limits, and outputs the constrained data to the robot for execution.

2. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 1, characterized in that: In step S20, the UWB pose information is preprocessed, that is, the UWB pose information is filtered. When filtering, the distance, orientation, and validity obtained from the UWB pose information are used as the original data. The original data are subjected to median filtering and low-pass filtering to obtain smoothed distance, orientation, and validity as smoothed data. Then, the validity of the smoothed data is verified.

3. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 2, characterized in that: In step S50, when constraining the angular velocity, the actual distance obtained from the filtered UWB pose information is acquired in real time. The actual distance is compared with the preset target distance to obtain the distance error. The azimuth angle of the filtered UWB pose information is converted into radians to obtain the angle between the robot's orientation and the target object's direction as the heading error. Based on the heading error and the actual distance, the vertical distance from the robot's central axis to the target is calculated as the lateral error. Stanley-like control logic is used to calculate the desired front wheel steering angle by combining the heading error and the lateral error, while limiting the front wheel steering angle within the robot's mechanical structure parameters. The front wheel steering angle is converted into angular velocity by combining the robot's physical parameters.

4. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 3, characterized in that: After calculating the heading error and lateral error, the method also includes: inputting the distance error into a preset position-type PID controller, and calculating the speed control quantity by combining preset proportional, integral, and derivative gains; when the actual distance is less than the preset target distance, dynamically reducing the speed through a preset distance proportional coefficient, and limiting the maximum linear speed through the speed control quantity.

5. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 1, characterized in that: In step S50, during obstacle avoidance, the controller constructs an attractive force vector using UWB pose information and a repulsive force vector using the obstacles obtained from the scan data within a preset safe distance. The attractive force vector and the repulsive force vector are added component by component in the robot coordinates to obtain the resultant force. The direction angle of the resultant force is calculated as a reference quantity for angular velocity, and the magnitude of the resultant force is calculated as a reference quantity for linear velocity. When the resultant force direction angle is within a preset negligible range, the angular velocity is set to zero; when the nearest forward distance is less than a preset emergency stop threshold, the velocity is immediately set to zero; when the nearest forward distance is between a preset slow stop threshold and an emergency stop threshold, the output of longitudinal linear velocity control to the robot is stopped, while the output of lateral angular velocity control to the robot is retained; preset acceleration upper limits are applied to the linear velocity and angular velocity respectively to limit the variation of linear velocity and angular velocity in adjacent control cycles, while the upper limit of the amplitude value constrains the linear velocity and angular velocity to avoid exceeding the robot's hardware allowable range.

6. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 5, characterized in that: In step S50, the robot is driven to move using velocity and angular velocity only when the controller is enabled and the UWB pose information is valid. If the UWB pose information is detected to be invalid in the current control cycle, the angular velocity and linear velocity are set to zero in the current control cycle, and the resultant force calculation and linear velocity and angular velocity constraint steps are skipped. The subsequent state is processed in the next control cycle. At the same time, the visual markers of attraction, repulsion and resultant force are output.

7. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 1, characterized in that: In step S10, after power-on initialization, the robot's historical motion trajectory is obtained based on the robot's historical operation data, as well as multi-frame LiDAR scanning data collected during the historical motion. The relative positions of obstacles and the robot are obtained through the scanning data. Combined with the robot's historical motion trajectory, an environmental map consisting of the robot's movable range during operation is constructed. At the same time, the continuously collected UWB pose information of the target object is combined with the collection time to form the target object's historical movement trajectory. The historical movement trajectory and environmental map are used to construct and train a trajectory prediction model. The trajectory prediction model takes the current movement trajectory of the target object as input and outputs the target object's subsequent movement trajectory. In step S30, when the current state is the following state, the current movement trajectory is generated based on the currently continuously acquired effective smooth UWB pose information. The current movement trajectory is input into the trajectory prediction model to obtain the subsequent movement trajectory of the target object as the predicted movement trajectory. The current movement speed of the target object is calculated based on the acquisition time of the UWB pose information. In step S50, when calculating the speed, the predicted movement trajectory, the environment map, and the current movement speed are combined to generate the optimal pursuit trajectory for the robot to chase the target object in the environment map under the speed and acceleration limits.

8. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 7, characterized in that: In step S50, after generating the optimal pursuit trajectory and before outputting the speed command, the latest frame of effective smoothed UWB pose information is obtained as the real-time position, and the deviation value between the real-time position and the predicted movement trajectory is compared; the Boolean value and the forward nearest distance obtained from the processing of the current lidar scanning data are obtained. If the deviation value is less than the preset deviation threshold and there are no new obstacles, the linear velocity and angular velocity are calculated using the optimal pursuit trajectory. If the deviation value is not less than the preset deviation threshold or the forward nearest distance is less than the preset slow stop distance threshold, the coverage area of ​​the predicted movement trajectory in the environmental map is reduced according to the preset prediction ratio as the replanning range. The current movement trajectory is updated with the real-time position of the target object. The current movement trajectory is input into the trajectory prediction model to obtain the updated predicted movement trajectory. The predicted movement trajectory is constrained according to the replanning range.

9. The adaptive following method for mobile robots based on UWB and LiDAR according to claim 8, characterized in that: After obtaining the predicted movement trajectory in step S50, the speed of the target object at each point on the predicted movement trajectory is extracted as the predicted speed. Points on the predicted movement trajectory with predicted speeds less than a preset static speed threshold are selected as static points. The time period of the target object at the static point is obtained by combining the predicted speed as the static time period. When generating the optimal pursuit trajectory, the start time of the next stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is determined by combining the velocity and acceleration limit values. If it is not later than the pursuit deadline, a speed priority instruction is generated. If it is later than the pursuit deadline, the end time of the stationary time period is used as the pursuit deadline. The robot's fastest time to catch up with the target object is determined again. If it is not later than the pursuit deadline, a smoothness priority instruction is generated. If it is later than the pursuit deadline, the start time of the next stationary time period or the time when the target object reaches the end of the predicted movement trajectory is used as the pursuit deadline. The time it takes for the robot to move from the starting point to the ending point of the optimal pursuit trajectory is taken as the pursuit time. After determining the pursuit deadline, if a speed priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum pursuit time; if a smoothness priority instruction is obtained, the optimal pursuit trajectory is generated based on the minimum number of changes in linear velocity and angular velocity on the optimal pursuit trajectory, provided that the pursuit time is not later than the pursuit deadline.

10. A mobile robot adaptive following system based on UWB and LiDAR, characterized in that, The mobile robot adaptive following method based on UWB and LiDAR, as described in any one of claims 1-9, includes: The preprocessing module powers on the robot and loads parameters, including UWB timeout threshold, de-jittering time for obstacle appearance and disappearance, switching cooling time, speed and acceleration limit values. The data acquisition and obstacle detection module acquires UWB pose information and LiDAR scanning data; it smooths the UWB pose information and performs anti-jitter processing on the scanning data based on the anti-jitter time of obstacle appearance and disappearance to obtain smooth pose and Boolean values ​​representing whether there is an obstacle in front and the nearest forward distance. The state machine decision module sets up an idle state, a follow state, and an obstacle avoidance state. After the robot is powered on, the current state is initialized to the idle state, and the internal state of the controller is cleared, including the follow state and the obstacle avoidance state. Within each preset control cycle, the module determines whether the UWB pose information is invalid or has timed out based on the validity of the UWB pose information and the UWB timeout threshold. If invalid or timed out, the robot's current state is switched to the idle state. After the robot enters the idle state, its speed is zero, the data in the current control cycle is cleared, and a cooling timer is started according to the switching cooling time. If the pose is valid and the cooling timer ends, the current state is switched to the follow state or the obstacle avoidance state according to the Boolean value. When the controller is enabled, its internal state is cleared to zero instantly. When disabled, it immediately releases zero velocity and enters the disabled state. It acquires smoothed UWB pose information and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution. The obstacle avoidance controller, when enabled, instantly clears its internal state to zero; when disabled, it immediately releases zero speed and enters the disabled state. It acquires Boolean values ​​representing whether there is an obstacle ahead and the nearest forward distance, and calculates linear velocity or angular velocity. Based on velocity and acceleration limit values, it constrains the changes in linear velocity, angular velocity, and linear velocity and angular velocity within the control cycle, and outputs the constrained data to the robot for execution. The state machine decision module enables the controller corresponding to the current state after the switch only when switching states, and disables other controllers.

Images