A mobile robot following system based on historical trajectory tracking and a control method thereof

Abstract

The application relates to a mobile robot following method and system based on historical trajectory tracking. The method is as follows: in the following process, the time sequence of the position of a target object relative to a mobile robot is continuously detected and recorded to form a historical trajectory; a continuous reference driving trajectory is generated based on the historical trajectory; the mobile robot is controlled to track the reference driving trajectory; the historical trajectory is stored in a ring buffer, the ring buffer fixedly stores position points in the past N time points or a preset time window, and is updated according to the first-in first-out principle to form a historical trajectory point set Traj_history={P(t-N+1), P(t-N+2),..., P(t)}. The system of the application comprises a perception module, a trajectory recording module, a trajectory generating module and a trajectory tracking control module. The application can realize smoother, more predictable and high-robustness following effects, and overcomes the defects of response lag, overshoot, easy loss of a target and lack of path predictability in a traditional direct position tracking method.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot autonomous navigation and target tracking technology, specifically relating to a mobile robot control method and system that records and tracks the historical trajectory of a target object during the following process. Background Technology

[0002] Mobile robot following systems are an important research area in robotics, widely used in service robots, warehousing and logistics robots, and autonomous following vehicles. Traditional mobile robot following systems, such as vision- or LiDAR-based following robots, typically rely on the core control logic of real-time detection of the target object's current position and direct robot movement towards that position. This "direct position tracking" method has the following drawbacks in engineering practice: First, there is response lag and overshoot. When the target object moves or turns quickly, the robot always chases a "current point", resulting in a path with severe jagged edges or frequent overshoot, poor following smoothness, and unnatural motion posture.

[0003] Second, it is easy to lose track of the target. If the robot's speed or turning ability is limited, when the target suddenly turns at a large angle, the robot may lose track of the target because it cannot respond in time. This problem is particularly prominent in high-speed following scenarios.

[0004] Third, it lacks path prediction capability. The robot cannot predict the movement trend of the target object and can only passively respond to its current position, resulting in poor performance in complex dynamic environments (such as among crowds and obstacles) and difficulty in adapting to complex real-world application scenarios.

[0005] To address the aforementioned issues, some improvements have been made in existing technologies, such as estimating the target's future position by adding a prediction step or reducing response latency by optimizing control parameters. However, these solutions still directly track the target's instantaneous or predicted position, failing to fundamentally solve the problems of smoothness and robustness in the following path. Summary of the Invention

[0006] To address the problems existing in the prior art, this invention provides a mobile robot following system and its control method based on historical trajectory tracking, which can achieve a smoother, more predictable and more robust following effect, overcoming the defects of traditional direct position tracking methods such as response lag, overshoot, easy loss of target and lack of path predictability.

[0007] This invention adopts the following technical solution: a mobile robot following method based on historical trajectory tracking, characterized by including the following steps: Step S1: During the following process, continuously detect and record the temporal sequence of the target object's position relative to the mobile robot to form a historical trajectory; Step S2: Based on the historical trajectory, generate a continuous reference driving trajectory; Step S3: Control the mobile robot to track the reference driving trajectory; The historical trajectory is stored in a circular buffer, which stores the position points of the past N moments or within a preset time window, and updates them in a rolling manner according to the first-in-first-out principle, forming a historical trajectory point set Traj_history={P(t-N+1), P(t-N+2), ..., P(t)}.

[0008] Preferably, in step S1, the relative position of the target object in the robot coordinate system or world coordinate system is recorded at a fixed sampling frequency f as P(t)=(x(t), y(t), θ(t)), where x(t) and y(t) are the coordinates of the target object on the two-dimensional plane, and θ(t) is the orientation angle of the target object.

[0009] Preferably, in step S2, a set of historical trajectory points is read from a circular buffer, and one or more of Bézier curves, B-spline curves, or polynomial fitting methods are used to fit the historical trajectory points to generate a continuous reference trajectory extending from near the robot to the front of the target object's historical path. This reference trajectory reflects the past movement trend of the target object. Specifically, when the curvature of the historical trajectory point set is greater than a preset curvature threshold, the corresponding trajectory point is identified as a turning key point, and this turning key point is retained when generating the reference driving trajectory. Alternatively, the historical trajectory points on both sides of the turning key point are segmented and fitted to ensure that the lateral deviation of the reference driving trajectory relative to the historical trajectory does not exceed a preset deviation threshold.

[0010] Preferably, in step S3, any one of the following algorithms—pure tracking algorithm, Stanley algorithm, or model predictive control algorithm—is used as the trajectory tracking algorithm to calculate the robot's desired linear velocity and angular velocity. The control objective is to minimize the lateral and heading deviations between the robot's actual position and the reference trajectory, enabling the robot to travel along the reference trajectory. Specifically, the aiming distance of the trajectory tracking algorithm and the upper speed limit of the mobile robot are dynamically adjusted based on the curvature of the reference trajectory. When the curvature increases, the aiming distance is reduced and the upper speed limit is lowered, allowing the mobile robot to make large-angle turns along the reference trajectory.

[0011] Preferably, steps S1 to S3 are repeated at each moment, the historical trajectory is updated, the reference trajectory is dynamically adjusted accordingly, and the robot continuously tracks the updated reference trajectory.

[0012] Preferably, when the target object is briefly obscured, resulting in the loss of position data, the robot continues to travel along the last generated valid reference trajectory for a preset time or distance until the target reappears or a preset time threshold is exceeded. If the target object is not detected again after the preset time threshold is exceeded, the robot is controlled to decelerate and stop or enter a target re-search state.

[0013] Preferably, in step S2, before trajectory fitting, the historical trajectory points are first filtered and preprocessed. The filtering and preprocessing uses one or more of Kalman filtering, median filtering, complementary filtering, or extended Kalman filtering to remove measurement noise and outliers.

[0014] Preferably, in step S2, the fitting method is adaptively selected based on the curvature change of the historical trajectory point set: polynomial fitting is used when the curvature change is small, and piecewise linear or piecewise curve fitting is used when the curvature change is large. Step S2 also includes selectively generating short-term predicted trajectories based on the motion trend of the historical trajectory, determining walkable and non-walkable areas based on a pre-established environmental map and / or environmental perception data, and performing traversability verification on the short-term predicted trajectories. The short-term predicted trajectories that pass the traversability verification are used for mobile robot tracking. When the short-term predicted trajectory intersects with a non-walkable area, the deviation from the historical trajectory exceeds a preset threshold, or the prediction confidence is lower than a preset threshold, the use of the short-term predicted trajectory is stopped, and the mobile robot is controlled to track the reference driving trajectory generated by the historical trajectory, rather than driving directly toward the instantaneous current position of the target object.

[0015] Preferably, in step S1, any one of LiDAR, RGB-D camera, UWB positioning system or Bluetooth AOA array is used as the sensing means to obtain the relative position of the target object.

[0016] This invention also provides a mobile robot following system based on historical trajectory tracking, characterized by comprising: a perception module for real-time acquisition of the target object's position information and obtaining the target object's relative position sequence in the robot coordinate system or world coordinate system; a trajectory recording module, including a circular buffer for continuously storing the target object's position coordinates within the past N moments or a preset time window, forming a historical trajectory point set; a trajectory generation module for fitting and generating a continuous reference trajectory curve based on the historical trajectory point set; and a trajectory tracking control module for controlling the robot chassis to travel along the reference trajectory using a trajectory tracking algorithm.

[0017] Compared with the prior art, the present invention has the following beneficial effects: First, the robot no longer reacts violently to transient jitters or slight deviations in the target position, but instead travels along a smooth trajectory, resulting in a more natural motion posture and avoiding the problem of severe jagged edges in the path in traditional methods.

[0018] Second, when the target turns, the robot will follow the target's historical arc instead of rushing directly to the current point, effectively avoiding overshoot and frequent oscillations caused by direct position tracking.

[0019] Third, when the target object is briefly obscured and its position is lost, the robot can continue to travel a distance along the last valid historical trajectory, buying time to recapture the target and significantly improving the robustness of the system.

[0020] Fourth, the target's motion direction and curvature can be naturally obtained through trajectory fitting, enabling the robot to decelerate or adjust its orientation in advance, cope with faster and more agile following scenarios, and perform better in complex dynamic environments. Attached Figure Description

[0021] Figure 1 : Schematic diagram of the system modules of this invention.

[0022] Figure 2 : Schematic diagram of historical trajectory data update within the circular buffer.

[0023] Figure 3 Comparison chart of the effects of traditional direct position tracking and trajectory tracking of the present invention (showing the difference in path when turning).

[0024] Figure 4 : Flowchart of the core control method of this invention. Detailed Implementation

[0025] The present invention will be further described below with reference to specific embodiments.

[0026] like Figure 1 As shown, this invention discloses a mobile robot following system based on historical trajectory tracking, including a perception module for real-time acquisition of the target object's position information and obtaining the target object's relative position sequence in the robot coordinate system or world coordinate system; a trajectory recording module, including a circular buffer, for continuously storing the target object's position coordinates within the past N moments or a preset time window to form a historical trajectory point set; a trajectory generation module for fitting and generating a continuous reference trajectory curve based on the historical trajectory point set; and a trajectory tracking control module for controlling the robot chassis to travel along the reference trajectory using a trajectory tracking algorithm.

[0027] like Figure 2 and Figure 4As shown, based on the disclosed system, the core control method of this invention is as follows: Step S1: During the following process, the position time sequence of the target object relative to the mobile robot is continuously detected and recorded to form a historical trajectory; Step S2: Based on the historical trajectory, a continuous reference driving trajectory is generated; Step S3: The mobile robot is controlled to track the reference driving trajectory; The historical trajectory is stored in a circular buffer, which stores the position points of the past N moments or within a preset time window, and is updated in a rolling manner according to the first-in-first-out principle to form historical trajectory points. Traj_history={P(t-N+1), P(t-N+2), ..., P(t)}. In step S1, the relative position of the target object in the robot coordinate system or world coordinate system is recorded at a fixed sampling frequency f as P(t)=(x(t), y(t), θ(t)), where x(t) and y(t) are the coordinates of the target object in the two-dimensional plane, and θ(t) is the orientation angle of the target object.

[0028] In step S1, any one of the following can be used as a sensing means to obtain the relative position of the target object: LiDAR, RGB-D camera, UWB positioning system or Bluetooth AOA array.

[0029] The following is a detailed explanation of several specific sensing methods: Example 1: A following robot based on lidar and odometry. In this example, the perception module uses lidar to identify leg features and obtain the polar coordinates (d, α) of the target person relative to the robot.

[0030] The trajectory recording module converts the target coordinates of each frame to the global coordinate system (X, Y) at a frequency of 20Hz and stores them in a circular queue that can store 50 points.

[0031] The trajectory generation module retrieves all valid points from the queue every 50ms and fits them with a 3rd-order Bézier curve to obtain a parameterized trajectory B(s), s∈[0,1].

[0032] The trajectory tracking control module uses a pure tracking algorithm to select a lookahead point B (s_lookahead) on the reference trajectory, calculate the steering angle, and control the movement of the robot chassis.

[0033] Example 2: A following robot based on an RGB-D camera In this embodiment, the perception module obtains the three-dimensional position of the target person through a human skeleton recognition algorithm.

[0034] The trajectory recording module records all the location points of the target person in the past 2 seconds.

[0035] When the curvature of the target motion trajectory changes significantly, the trajectory generation module adaptively adopts piecewise linear or quadratic curve fitting to improve the fitting accuracy.

[0036] The trajectory tracking control module uses a model predictive control algorithm, which simultaneously considers trajectory tracking error, robot dynamics constraints, and obstacle avoidance requirements in the prediction time domain, and outputs the optimal speed command.

[0037] Example 3: A UWB (Ultra-Wideband) Based Follower Robot In this embodiment, the sensing module employs a UWB positioning system, with a UWB tag attached to the target object and at least three UWB base stations or one array antenna installed on the robot. Using bidirectional ranging, time difference of arrival, or phase difference of arrival methods, the two-dimensional coordinates (x, y) of the target relative to the robot are calculated in real time, achieving ranging accuracy down to the centimeter level and an update frequency of 50Hz.

[0038] The trajectory recording module establishes a circular buffer with a capacity of 100 location points, continuously recording the target coordinates at a frequency of 50Hz. Simultaneously, it records the robot's own pose (obtained from the odometry or IMU) for each timestamp, and converts all coordinates to a global coordinate system for storage.

[0039] The trajectory generation module extracts the historical point set from the buffer within the most recent 2 seconds every 20ms. Since UWB data may contain outliers due to multipath effects, Kalman filtering or median filtering is first used to smooth the historical points before fitting a 5th-order B-spline curve to generate a C-line. 2 Continuous reference trajectory.

[0040] The trajectory tracking control module employs an improved Stanley method to calculate the lateral error and heading angle error between the robot's front axle center and the nearest point on the reference trajectory, outputting the steering angle control value. Simultaneously, it dynamically adjusts the robot's speed limit based on the curvature of the reference trajectory (automatically decelerating when curvature is large) to ensure smooth cornering.

[0041] As a useful supplement to this embodiment, when the UWB signal is briefly lost (such as when the target enters a metal-covered area), the robot continues to travel for up to 3 seconds using the last valid historical trajectory, during which the speed gradually decreases until the signal is restored or a re-search is triggered.

[0042] Example 4: A Bluetooth-based AOA (Angle of Arrival) Follower Robot In this embodiment, the sensing module employs Bluetooth AOA positioning technology: the target object carries a Bluetooth beacon (such as a smartphone or a dedicated tag), and a Bluetooth antenna array (such as a 4×4 array) is installed on the robot. Using Bluetooth 5.1 or later AOA positioning technology, the arrival phase difference of the radio frequency signal is measured, and the target's orientation angle θ and distance d relative to the robot's coordinate system are calculated (this needs to be combined with RSSI or multi-antenna phase difference ranging). The typical update frequency is 10-30Hz, and the angular accuracy can reach 3-5 degrees.

[0043] The trajectory recording module takes into account the potential impact of measurement noise, environmental reflections, and multipath propagation on Bluetooth AOA angle measurements, resulting in error characteristics that differ from UWB ranging data. Therefore, a dual-ring buffer is established: one stores the original measurement value (d, θ), and the other stores the smoothed position (x, y) after complementary filtering or extended Kalman filtering. The smoothed position points within the most recent 1.5 seconds are recorded.

[0044] Since the Bluetooth AOA update frequency is relatively low and may include jumps, the trajectory generation module first performs cubic spline interpolation on the historical trajectory points to increase the point density to be consistent with the robot control cycle (e.g., 100Hz). Then, it uses quadratic polynomial local weighted regression fitting to obtain a smooth reference trajectory with good real-time performance.

[0045] The trajectory tracking control module uses a model predictive control algorithm with a prediction time domain of 1 second and a control time domain of 0.2 seconds. The objective function includes three terms: the deviation between the robot's predicted path and the reference trajectory, the rate of change of the control variable (to prevent chattering), and the approach term towards the end point of the reference trajectory. Simultaneously, raw angle information from Bluetooth AOA is used as an aid; when the trajectory fitting confidence is low (e.g., the target is stationary), it reverts to direct angle tracking mode.

[0046] This embodiment offers the advantage of low power consumption: the Bluetooth AOA tag can be powered by a regular button battery for more than a year, and some terminals that support the corresponding Bluetooth direction finding function can be used as target beacons, which helps reduce the additional hardware requirements of the target end. The trajectory tracking mode of this invention significantly reduces the reliance on high-precision, high-frequency angle measurements. Even with a 10% packet loss rate in Bluetooth data, the robot can still maintain smooth following by using historical trajectories.

[0047] Furthermore, the trajectory generation module can also be used as a trajectory fitting and prediction module. This module generates a smooth reference trajectory based on the historical trajectory point set in the circular buffer, and can selectively predict a short-term predicted trajectory along the reference trajectory based on the target object's motion direction, speed, and trajectory curvature, so that the mobile robot can decelerate or adjust its orientation in advance.

[0048] This invention differs from direct position tracking in its control objective. Direct position tracking uses the difference between the target object's current position and the robot's current position as the control error, i.e., E_current = P_target(t) - P_robot(t). This invention uses the lateral and heading deviations of the robot's actual path relative to the historical reference trajectory as the trajectory tracking error. Therefore, the robot tracks the target's newly formed, real motion trajectory, rather than continuously chasing a fluctuating instantaneous position.

[0049] When target location data is temporarily interrupted due to occlusion or loss of wireless signal, the robot continues to travel along the last valid reference trajectory for a preset time or distance, gradually reducing its speed during this period. If the target is regained within a threshold time, historical trajectory updates are resumed; otherwise, the robot enters a deceleration and parking state or a target re-search state. The above short-term prediction is only an optional enhancement to historical trajectory tracking and does not change the control mechanism that uses historical trajectory as the primary tracking object.

[0050] Figure 3 Supplementary explanation of the turning scenario shown like Figure 3 As shown, when a target object makes a large instantaneous turn of nearly 90° in a narrow passage, corner, shelf corner, or other restricted area, the traditional direct position tracking method generates an approach direction based on the line connecting the current position of the mobile robot and the current position of the target object. This line may pass through walls, shelves, equipment, or other non-walkable areas on the inside of the turn, causing the mobile robot to travel at an angle and increasing the risk of collision.

[0051] This invention continuously records the time sequence of the actual locations traversed by a target object. The effective historical trajectory points of the target object represent the locations the target object has actually passed through. Instead of directly moving towards the target object's instantaneous current position, the mobile robot turns along a reference trajectory generated based on the effective historical trajectory points, thereby reducing the risk of the mobile robot entering untravelable areas when making sharp turns.

[0052] To avoid creating a chamfer on the inside of the turn during curve smoothing, the trajectory generation module identifies key turning points based on the curvature changes of the historical trajectory point set. When the curvature exceeds a preset curvature threshold, the key turning points are retained during curve fitting, or the historical trajectory points on both sides of the key turning points are segmented and fitted separately, limiting the maximum lateral deviation of the generated reference driving trajectory relative to the original historical trajectory. When the fitted reference driving trajectory deviates from the historical trajectory by more than a preset deviation threshold, the fitting smoothness is reduced, the fitting weight of the key turning points is increased, or the trajectory reverts to the segmented historical trajectory as the reference driving trajectory.

[0053] The trajectory tracking control module dynamically adjusts the aiming distance and speed limit based on the curvature of the reference trajectory. When the curvature of the reference trajectory increases, the aiming distance of the pure tracking algorithm is reduced and the upper limit of the linear velocity of the mobile robot is lowered to reduce the angular deviation caused by premature turning due to excessive aiming distance or excessive speed.

[0054] As an optional implementation, the system determines walkable and non-walkable areas based on a pre-established environmental map and / or environmental perception data. It then expands the non-walkable areas according to the mobile robot's outline and a preset safety distance, and verifies the traversability of the generated reference trajectory or short-term predicted trajectory. When the trajectory intersects with the expanded non-walkable area, the system regenerates the reference trajectory, stops using the short-term predicted trajectory, reduces the mobile robot's speed, or controls the mobile robot to stop.

[0055] Short-term predicted trajectories are only used when they pass drivability checks, the deviation from historical trajectories does not exceed a preset threshold, and the prediction confidence is not lower than a preset threshold; otherwise, the mobile robot tracks a reference trajectory generated from the target object's valid historical trajectory. Therefore, this invention not only improves the smoothness and robustness of following motion but also reduces the risk of the direct position tracking path entering an unwalkable area in scenarios where the target object suddenly makes a large-angle turn.

[0056] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A mobile robot following method based on historical trajectory tracking, characterized in that... Includes the following steps: Step S1: During the following process, continuously detect and record the temporal sequence of the target object's position relative to the mobile robot to form a historical trajectory; Step S2: Based on the historical trajectory, generate a continuous reference driving trajectory; Step S3: Control the mobile robot to track the reference driving trajectory; The historical trajectory is stored in a circular buffer, which stores the position points of the past N moments or within a preset time window, and updates them in a rolling manner according to the first-in-first-out principle, forming a historical trajectory point set Traj_history={P(t-N+1), P(t-N+2), ..., P(t)}.

2. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that... In step S1, the relative position of the target object in the robot coordinate system or world coordinate system is recorded at a fixed sampling frequency f as P(t)=(x(t), y(t), θ(t)), where x(t) and y(t) are the coordinates of the target object on the two-dimensional plane, and θ(t) is the orientation angle of the target object.

3. The mobile robot following method based on historical trajectory tracking according to claim 2, characterized in that... In step S2, the historical trajectory point set is read from the circular buffer, and one or more of the following methods—Bézier curve, B-spline curve, or polynomial fitting—are used to fit the historical trajectory points to generate a continuous reference trajectory extending from near the robot to the front of the target object's historical path. The reference trajectory reflects the past motion trend of the target object. When the curvature of the historical trajectory point set is greater than a preset curvature threshold, the corresponding trajectory point is identified as a turning key point, and the turning key point is retained when generating the reference driving trajectory. Alternatively, the historical trajectory points on both sides of the turning key point are segmented and fitted to ensure that the lateral deviation of the reference driving trajectory relative to the historical trajectory does not exceed a preset deviation threshold.

4. The mobile robot following method based on historical trajectory tracking according to claim 3, characterized in that... In step S3, any one of the following algorithms—pure tracking algorithm, Stanley algorithm, or model predictive control algorithm—is used as the trajectory tracking algorithm to calculate the robot's desired linear velocity and angular velocity. The control objective is to minimize the lateral and heading deviations between the robot's actual position and the reference trajectory, so that the robot travels along the reference trajectory. The aiming distance of the trajectory tracking algorithm and the upper speed limit of the mobile robot are dynamically adjusted according to the curvature of the reference trajectory. When the curvature increases, the aiming distance is reduced and the upper speed limit is lowered, so that the mobile robot can make large-angle turns along the reference trajectory.

5. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that... Steps S1 to S3 are repeated at each moment, the historical trajectory is updated, the reference trajectory is dynamically adjusted accordingly, and the robot continuously tracks the updated reference trajectory.

6. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that... When the target object is briefly obscured, resulting in the loss of location data, the robot continues to travel along the last generated valid reference trajectory for a preset time or distance until the target reappears or exceeds a preset time threshold. If the target object is not detected again after the preset time threshold is exceeded, the robot is controlled to slow down and stop or enter a target re-search state.

7. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that, In step S2, before trajectory fitting, the historical trajectory points are first filtered and preprocessed. The filtering and preprocessing uses one or more of Kalman filtering, median filtering, complementary filtering, or extended Kalman filtering to remove measurement noise and outliers.

8. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that... In step S2, the fitting method is adaptively selected based on the curvature change of the historical trajectory point set: polynomial fitting is used when the curvature change is small, and piecewise linear or piecewise curve fitting is used when the curvature change is large. Step S2 also includes selectively generating short-term predicted trajectories based on the motion trend of the historical trajectory, determining walkable and non-walkable areas based on a pre-established environmental map and / or environmental perception data, and performing traversability verification on the short-term predicted trajectories. The short-term predicted trajectories that pass the traversability verification are used for mobile robot tracking. When the short-term predicted trajectory intersects with a non-walkable area, the deviation from the historical trajectory exceeds a preset threshold, or the prediction confidence is lower than a preset threshold, the use of the short-term predicted trajectory is stopped, and the mobile robot is controlled to track the reference driving trajectory generated by the historical trajectory, instead of directly driving towards the instantaneous current position of the target object.

9. The mobile robot following method based on historical trajectory tracking according to claim 1, characterized in that... In step S1, any one of the following is used as a sensing means to obtain the relative position of the target object: LiDAR, RGB-D camera, UWB positioning system or Bluetooth AOA array.

10. A mobile robot following system based on historical trajectory tracking that implements the method of any one of claims 1-9, characterized in that... include: The perception module is used to collect the position information of the target object in real time and obtain the relative position sequence of the target object in the robot coordinate system or the world coordinate system. The trajectory recording module includes a circular buffer, which is used to continuously store the position coordinates of the target object in the past N moments or within a preset time window, forming a historical trajectory point set. The trajectory generation module is used to fit and generate a continuous reference trajectory curve based on the historical trajectory point set; The trajectory tracking control module is used to control the robot chassis to travel along the reference trajectory using a trajectory tracking algorithm.

Images