A relative trajectory-based following control method, system, device, and medium

Abstract

The application discloses a relative trajectory-based following control method, system, device and medium, relates to the technical field of following control, and the method comprises the following steps: obtaining current pose data of a following object and relative pose data of a tracking target relative to the following object according to a preset sampling period; performing target positioning based on the current pose data and the relative pose data to obtain position data of the tracking target relative to the following object; forming a relative trajectory of the tracking target relative to the following object based on position data of a plurality of sampling periods; and controlling the following object to run according to the relative trajectory, so that the following object follows the tracking target. The method can improve positioning accuracy and improve target following accuracy.

Description

Technical Field

[0001] This application relates to the field of tracking control technology, and in particular to a tracking control method, system, device and medium based on relative trajectory. Background Technology

[0002] In fields such as wilderness rescue, industrial production, and military logistics, there are frequent scenarios involving the transportation of large quantities of equipment or supplies from assembly points to target sites. These transport routes often traverse complex terrains such as mountains and muddy areas, and are frequently accompanied by extreme weather conditions like fires and heavy rain. Relying on manual labor for transportation not only impacts the physical stamina of the personnel but also reduces operational efficiency. Using unmanned vehicles to transport supplies and employing a vehicle-to-vehicle following method can effectively reduce the burden on personnel and improve transportation efficiency.

[0003] Currently, the industry uses automatic following technology to achieve autonomous vehicle following. Typically, when following outdoors, RTK (Real-Time Kinematic) devices are installed on both the target being followed and the target being followed. By acquiring the longitude, latitude, and heading angle information of the target being followed, as well as the longitude, latitude, and heading angle information of the target being followed, the longitude / latitude / heading angle deviation between the target being followed and the target being followed is calculated. This determines the distance and angular deviation between the target being followed and the target being followed, thereby controlling the forward direction angle and acceleration / deceleration of the target being followed.

[0004] However, in practical applications, using only RTK devices is easily affected by environmental factors such as multipath effects, light, and signal blockage, which increases the probability of inaccurate positioning and results in poor target tracking accuracy. Summary of the Invention

[0005] To overcome the problem that RTK devices are easily affected by environmental factors such as multipath effect, light, and signal blockage in practical applications, which increases the probability of inaccurate positioning and leads to poor target tracking accuracy, this application provides a tracking control method, system, device, and medium based on relative trajectory.

[0006] Firstly, in order to solve the aforementioned technical problems, this application provides a following control method based on relative trajectory, comprising: Acquire the current pose data of the followed object and the relative pose data of the tracking target relative to the followed object according to the preset sampling period; Target localization is performed based on current pose data and relative pose data to obtain the position data of the tracked target relative to the followed object; The relative trajectory of the tracking target relative to the follower object is formed based on position data from multiple sampling periods; Control the object to follow along a relative trajectory to achieve the object's following of the target.

[0007] Secondly, this application also provides a relative trajectory-based following control system, comprising: The acquisition module is used to acquire the current pose data of the followed object and the relative pose data of the tracking target relative to the followed object according to a preset sampling period; The positioning module is used to locate the target based on the current pose data and the relative pose data, and to obtain the position data of the tracked target relative to the followed object; The trajectory forming module is used to form the relative trajectory of the tracking target relative to the follower object based on position data from multiple sampling periods; The follow control module is used to control the follower object to run according to a relative trajectory, so as to realize the follower object's following of the target.

[0008] Thirdly, this application also provides a computing device, including a memory, a processor, and a program stored in the memory and running on the processor, wherein the processor executes the program to implement the steps of the relative trajectory-based following control method described above.

[0009] Fourthly, this application also provides a computer-readable storage medium storing instructions that, when executed on a terminal device, cause the terminal device to perform steps of a relative trajectory-based following control method.

[0010] The beneficial effects of this application are as follows: First, the current pose data of the tracking object and the relative pose data of the tracking target relative to the tracking object are acquired according to a preset sampling period. Based on each current pose data and the corresponding relative pose data, target localization is performed to obtain the position data of the tracking target relative to the tracking object in the corresponding sampling period, thereby achieving relative localization of the tracking target. Second, a relative trajectory of the tracking target relative to the tracking object is formed based on the position data from multiple sampling periods, and the tracking object is controlled to run according to the relative trajectory to achieve tracking of the tracking target. In this way, by using the current pose data of the tracking object to perform relative localization of the tracking target to obtain multiple position data of the tracking target relative to the tracking object, the influence of environmental factors such as multipath effects, light, and signal obstruction caused by direct localization using only RTK equipment on the localization of the tracking target can be reduced. This improves the localization accuracy of the tracking target, thereby improving the accuracy of the relative trajectory of the tracking target formed based on multiple position data, and further improving the accuracy of controlling the tracking object to follow the tracking target according to the relative trajectory. Attached Figure Description

[0011] Figure 1 This is a schematic diagram illustrating the positional relationship between the followed object and the tracked target in this application; Figure 2This is a schematic diagram illustrating the inconsistent movement speeds of the tracking target and the followed object in this application; Figure 3 This is a flowchart illustrating an exemplary embodiment of the following control method based on relative trajectory. Figure 4 This is an example diagram of a mobile robot in an exemplary embodiment of this application; Figure 5 This is an exemplary diagram illustrating the device installation of an RTK device and a UWB device in an exemplary embodiment of this application. Figure 6 This is an exemplary diagram illustrating the device installation of the IMU device and the UWB device in an exemplary embodiment of this application. Figure 7 This is an example diagram of the global coordinate system in an exemplary embodiment of this application; Figure 8 This is a flowchart illustrating the relative trajectory-based following control method provided by the application in an exemplary embodiment of this application; Figure 9 This is a schematic diagram illustrating the structure of a relative trajectory-based following control system, which is an exemplary embodiment of this application. Figure 10 This is a schematic diagram of the structure of a computer system of a computing device, illustrating an exemplary embodiment of this application. Detailed Implementation

[0012] The following embodiments are further explanations and supplements to this application and do not constitute any limitation on this application.

[0013] There are two types of follow control logics in the current unmanned transport vehicle platforms in the industry.

[0014] The first method primarily uses manual remote control. However, manual remote control requires the operator to constantly monitor the vehicle's driving status while also ensuring their own safety, making it difficult to balance both.

[0015] The second approach utilizes automatic following technology to enable autonomous vehicles to follow each other. This method typically employs a single sensor (radar, camera, RTK, etc.) for vehicle localization and controls the vehicle's motion based on their relative positions. For example, RTK devices are installed on both the target vehicle and the vehicle being followed to control the target's heading angle and acceleration / deceleration, thus enabling the target to follow the target. Automatic following technology refers to the ability of a device to identify and continuously follow a specific target without manual intervention.

[0016] However, while RTK devices offer a horizontal accuracy of approximately ±3cm and a heading angle error of 0.1°~0.5°, in practical applications, they are susceptible to environmental factors such as multipath effects, lighting conditions, and signal obstruction. This can exacerbate horizontal accuracy errors to ±10cm and heading angle errors to 5° or even higher, sometimes resulting in complete signal loss. This significantly increases the probability of inaccurate positioning, leading to poor target tracking accuracy. Furthermore, RTK devices are generally expensive, costing between 12,000 and 20,000 yuan. Therefore, using a single sensor for vehicle-to-vehicle following is highly susceptible to environmental factors (lighting, obstructions, etc.), easily causing inaccurate positioning and subsequent tracking control failure, and also presents a high cost issue.

[0017] To address the aforementioned problems, embodiments of this application provide a relative trajectory-based following control method, system, device, and medium, which will be described in detail below.

[0018] The relative trajectory-based following control method provided in this application can be specifically executed by a server. It should be noted that the server can be a standalone server, or a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, content delivery networks (CDNs), and big data and artificial intelligence platforms; no limitation is imposed here.

[0019] In this application, the positional relationship between the followed object and the tracked target at any given time can be as follows: Figure 1 As shown, Figure 1 In this scenario, assuming the target being tracked is A and the follower is B, and simplifying target A to a point mass, and assuming the follower and target move at the same speed, then the current position and heading angle of the target at any given time can be determined from the current position of the follower. , and heading angle The calculated value is L, representing the vehicle's length. The final step is to calculate and record the target's position coordinates relative to the world coordinate system. and .

[0020] However, the movement speeds of the object being followed and the target being tracked are often difficult to keep in sync (e.g.) Figure 2As shown in the image, or due to data congestion or loss, it may be necessary to record the target's trajectory (absolute trajectory or relative trajectory to the target) to ensure the target follows the target's path and maintains a small tracking deviation. This prevents situations such as the target moving too fast at curves or the target falling off a cliff while tracking the target's current position. Therefore, the target needs to record and save (memorize) the target's trajectory in real time so that it can follow the target accordingly.

[0021] Please see Figure 3 , Figure 3 An exemplary embodiment of this application illustrates a relative trajectory-based following control method, such as... Figure 3 As shown, this application provides a tracking control method based on relative trajectory, including: S301, acquire the current pose data of the followed object and the relative pose data of the tracking target relative to the followed object according to the preset sampling period; S302, target localization is performed based on current pose data and relative pose data to obtain the position data of the tracked target relative to the followed object; S303, forming the relative trajectory of the tracking target relative to the following object based on position data from multiple sampling periods; S304 controls the following object to run according to a relative trajectory, so as to achieve the following object following the target.

[0022] The relative trajectory-based tracking control method provided in this application first acquires the current pose data of the target being tracked and the relative pose data of the target relative to the target being tracked according to a preset sampling period. Then, based on each current pose data and the corresponding relative pose data, target localization is performed to obtain the position data of the target relative to the target being tracked in the corresponding sampling period, thereby achieving relative localization of the target being tracked. Secondly, a relative trajectory of the target being tracked relative to the target being tracked is formed based on the position data from multiple sampling periods, and the target being tracked is controlled to run according to the relative trajectory to achieve tracking of the target by the target being tracked. In this way, by obtaining multiple position data of the target being tracked relative to the target through relative localization using the current pose data of the target being tracked, the influence of environmental factors such as multipath effects, light, and signal obstruction caused by direct localization using only RTK devices on the localization of the target being tracked can be reduced. This improves the localization accuracy of the target being tracked, thereby improving the accuracy of the relative trajectory of the target being tracked based on multiple position data, and ultimately improving the accuracy of target tracking by controlling the target being tracked according to the relative trajectory.

[0023] Specifically, the object to be followed can be a vehicle, a mobile robot, etc. Please refer to the example diagram of a mobile robot. Figure 4 .

[0024] Furthermore, when the number of sampling periods is 1, the following object can move directly in the direction of the target's position data to achieve initial following. When the number of sampling periods reaches 2 or more, the position data from multiple sampling periods can form a relative trajectory between the target and the following object. In this case, simply controlling the following object to run according to the relative trajectory will achieve following the target.

[0025] Optionally, an RTK device is installed on the object being followed. When the RTK device is functioning correctly, the current pose data includes the current position and current heading angle detected by the RTK device; the relative pose data includes the Euclidean distance and the following angle deviation angle. Target localization is performed based on current pose data and relative pose data to obtain the position data of the tracked target relative to the followed object, including: Based on the current position, current heading angle, Euclidean distance, and follow angle deviation angle, coordinate calculations are performed to obtain the first target coordinates of the tracked target in the world coordinate system. The formula for calculating the coordinates of the first target is as follows:

[0026]

[0027] in, The x-coordinate of the first target is represented. Represents Euclidean distance. Indicates the current heading angle. Indicates the following angle deviation angle. This represents the x-coordinate of the current position in the world coordinate system. Indicates the length of the following object. The ordinate of the first target is represented by its vertical axis. This represents the ordinate of the current location in the world coordinate system; The first target coordinates are defined as the position data of the tracking target relative to the following object in the world coordinate system.

[0028] In the embodiment provided in this application, the current position and heading angle of the tracking object detected by the RTK device installed on the tracking object, as well as the Euclidean distance and tracking angle deviation angle of the tracking target relative to the tracking object, are used to calculate the coordinates. This yields the first target coordinates of the tracking target in the world coordinate system, which are then determined as the position data of the tracking target relative to the tracking object in the world coordinate system. By calculating the position data of the tracking target relative to the tracking object in the world coordinate system from the current position of the tracking object, relative positioning of the tracking target can be achieved. This reduces the impact of environmental factors such as multipath effects, light, and signal obstruction caused by direct positioning with only the RTK device on the positioning of the tracking target, thereby improving the positioning accuracy of the tracking target. This facilitates subsequent improvement of the accuracy of target following based on the relative trajectory formed by multiple position data.

[0029] Specifically, RTK equipment refers to devices that employ RTK positioning technology. RTK positioning technology is a high-precision positioning method based on a carrier phase dynamic real-time differential method, achieving centimeter-level high-precision positioning through the collaboration of a base station and a rover. Furthermore, RTK equipment typically incorporates a six-axis IMU (Inertial Measurement Unit, i.e., a three-axis gyroscope + a three-axis accelerometer), which can acquire the motion state of the object being followed in space in real time, including linear acceleration and rotational velocity in three directions (forward, backward, left, right, up, and down), i.e., three-axis linear acceleration and three-axis angular velocity. When the positioning data of the RTK equipment is significantly affected by clouds and surrounding buildings, resulting in abnormal (abnormal) RTK data, positioning and following are achieved by acquiring data through the IMU built into the RTK equipment. For a detailed implementation method, please refer to the next optional embodiment.

[0030] Furthermore, the Euclidean distance and tracking angle deviation angle of the tracking target relative to the followed object are obtained as follows: A UWB signal receiver is installed on the followed object, and a UWB signal transmitter is installed on the tracking target. Through signal transmission between the UWB signal transmitter and receiver, the Euclidean distance and tracking angle deviation angle of the tracking target relative to the followed object are directly detected. The UWB signal transmitter and receiver constitute a UWB device, which refers to a device that uses UWB technology for positioning. UWB positioning technology is a wireless positioning method based on ultra-wideband technology. It achieves centimeter-level high-precision positioning by transmitting nanosecond-level narrow pulse signals and utilizing time difference measurement or time-of-flight algorithms.

[0031] In this embodiment, to reduce positioning interference caused by errors in the original data, the current position, current heading angle, Euclidean distance, and follow angle deviation angle are all preprocessed data values ​​that have undergone Kalman filtering or moving average filtering. These Kalman filtering and moving average filtering methods are general filtering methods and will not be elaborated upon here.

[0032] In one exemplary embodiment provided in this application, only an RTK device and a UWB signal receiver (hereinafter referred to as UWB base station) need to be installed on the tracking object, and a UWB signal transmitter (hereinafter referred to as UWB tag, which can be made into a wristband or any other form, embedded in a helmet, clothing, or worn directly as a card, etc.) needs to be installed on the tracking target.

[0033] Specifically, a UWB signal transmitter and a UWB signal receiver constitute a UWB device. An example of installing RTK and UWB devices can be found as follows: Figure 5 As shown, Figure 5 In this method, the UWB base station on the vehicle is installed facing the target, typically with a detection angle of 120°-180°. The RTK device is mounted on top of the vehicle (as close to the center as possible; the following assumes it is in the center. If it is not in the center, a position adjustment is required). Therefore, compared to installing RTK devices on both the vehicle and the target for positioning and tracking, this embodiment uses an RTK device, a UWB signal receiver, and a UWB signal transmitter for positioning and tracking, resulting in lower hardware costs.

[0034] The real-time data that can be obtained from the sensors corresponding to RTK and UWB devices includes: (1) RTK data, i.e., the current position of the following object in the world coordinate system ( ), and the current heading angle (The angular deviation value is taken with due north as the axis, and clockwise is positive).

[0035] (2) UWB data, namely the Euclidean distance S between the tracking target and the following object and the following angle deviation angle γ with the front of the following object's direction of movement as the axis.

[0036] When RTK data acquisition is normal, the data transmitted from the RTK device at any given time is the current position of the followed object in the world coordinate system. ), and the current heading angle At that moment, the target's position in the world coordinate system is:

[0037]

[0038] The tracking target calculates and stores its position data in the world coordinate system in real time based on RTK and UWB data, forming a relative trajectory. .

[0039] Optionally, an IMU device is installed on the object being followed, and the current pose data includes the triaxial linear acceleration and triaxial angular velocity detected by the IMU device; the relative pose data includes the Euclidean distance and the following angle deviation angle. Target localization is performed based on current pose data and relative pose data to obtain the position data of the tracked target relative to the followed object, including: Based on standard calculations using triaxial linear acceleration and triaxial angular velocity, the linear velocity and angular velocity of the following object relative to the ground are obtained; A global coordinate system is constructed based on the following object; Based on linear velocity, angular velocity, Euclidean distance, and following angle deviation, the position is derived in the global coordinate system to obtain the position data of the tracked target in the global coordinate system.

[0040] In the embodiment provided in this application, firstly, the linear acceleration and angular velocity of the three axes detected by the IMU device installed on the tracked object are calculated using standard methods to obtain the linear velocity and angular velocity of the tracked object relative to the ground. This calculation method is relatively simple and will not be elaborated here. Secondly, a global coordinate system is constructed based on the tracked object, and the position is derived in the global coordinate system based on the linear velocity, angular velocity, Euclidean distance of the tracked target relative to the tracked object, and the tracking angle deviation. This allows for relative positioning of the tracked target in the global coordinate system based on the tracked object, reducing the impact of environmental factors such as multipath effects, light, and signal obstruction caused by direct positioning by RTK devices on the positioning of the tracked target. This improves the positioning accuracy of the tracked target and facilitates subsequent improvement of the accuracy of target tracking based on the relative trajectory formed by multiple position data.

[0041] Specifically, the three-axis linear acceleration includes forward acceleration, lateral acceleration, and vertical acceleration, while the three-axis angular velocities include yaw rate (also known as yaw speed, rotating around the Z-axis, i.e., turning left and right), pitch rate (rotating around the Y-axis, i.e., turning up or down), and roll rate (rotating around the X-axis, turning on the side or side).

[0042] In this embodiment, due to the ranging error of ±5cm to ±10cm in UWB and the angular velocity drift problem in IMU, the original linear velocity... angular velocity Euclidean distance and following angle deviation angle Measurement noise also exists. Therefore, in order to reduce positioning interference caused by errors in the raw data, the linear velocity used in the calculation in this embodiment... angular velocity Euclidean distance and following angle deviation angle All data values ​​have undergone preprocessing such as Kalman filtering or moving average filtering. These Kalman filtering and moving average filtering methods are general filtering methods and will not be elaborated upon here.

[0043] Specifically, the IMU device can be integrated with the object being followed, built into the RTK device (used when the RTK device malfunctions), or installed separately on the object being followed. The IMU device collects three-axis linear acceleration and three-axis angular velocity, and the relative trajectory memory calculation method of this embodiment is used to obtain the trajectories of the tracking target and the object being followed. The movement of the object being followed is controlled based on the relative trajectory to achieve target following of the tracking target.

[0044] In addition, the dynamic motion of the tracking target A and the following object B can be transformed into storable and traceable trajectory data. Its core relies on the accumulation of B's ​​motion state (velocity, angular velocity) and the transformation of A's position (distance, angle) relative to B. This calculation method is simple, low-cost, and meets the needs of practical applications.

[0045] In one exemplary embodiment provided in this application, only an IMU device and a UWB signal receiver (hereinafter referred to as a UWB base station) need to be installed on the object being followed, and a UWB signal transmitter (hereinafter referred to as a UWB tag, which can be made into a wristband or any other form, embedded in a helmet, clothing, or worn directly as a card, etc.) needs to be installed on the target being tracked. The solution of this embodiment can simultaneously cover abnormal situations such as the RTK device having no signal in the previous optional embodiment, so as to ensure that the object being followed accurately tracks the target.

[0046] Specifically, a UWB signal transmitter and a UWB signal receiver constitute a UWB device. An example of device installation for an IMU device and a UWB device can be shown below. Figure 6 As shown, Figure 6 In this embodiment, the UWB base station on the vehicle is installed facing the target, typically with a detection angle of 120°-180°. The IMU device is mounted on top of the vehicle (as close to the center as possible; assuming it's in the center below. If not in the center, a position change is required). Therefore, compared to installing RTK devices on both the vehicle and the target for positioning and tracking, and the positioning and tracking method in the previous optional embodiment, this embodiment uses an IMU device, a UWB signal receiver, and a UWB signal transmitter to achieve positioning and tracking, resulting in lower hardware costs.

[0047] The real-time data that can be obtained from the sensors corresponding to the IMU and UWB devices includes: (1) IMU data, that is, the real-time linear velocity v and angular velocity b of the following object relative to the ground are calculated based on the three-axis linear acceleration and three-axis angular velocity in the three directions of front, back, left, right, up and down. The calculation method adopts the standard calculation method, which will not be elaborated here.

[0048] (2) UWB data, namely the Euclidean distance S between the tracking target and the following object and the following angle deviation angle γ with the front of the following object's direction of movement as the axis.

[0049] Using the sensor data above, the position data of the tracking target in the world coordinate system is calculated for continuous sampling periods, forming the relative trajectory of the tracking target relative to the following object in the world coordinate system. Based on this relative trajectory, the movement of the following object is precisely controlled, thereby realizing a stable and reliable following function between the tracking target and the following object, such as car following car or car following person.

[0050] Optionally, a global coordinate system is constructed based on the following object, including: Obtain the initial pose data of the followed object, which includes the initial position and initial direction of travel. With the initial position as the origin and the initial direction of movement as the x-axis, a two-dimensional Cartesian coordinate system parallel to the horizontal plane is constructed. The two-dimensional Cartesian coordinate system is defined as the global coordinate system.

[0051] In the embodiment provided in this application, the initial pose data of the following object is obtained to understand the initial state information of the following object. A two-dimensional Cartesian coordinate system parallel to the horizontal plane is constructed with the initial position as the origin and the initial forward direction as the x-axis as the global coordinate system. This facilitates the subsequent determination of the position data of the tracking target in the global coordinate system, thereby realizing the relative positioning of the tracking target based on the following object in the global coordinate system. This improves the accuracy of the following object in tracking the target based on the relative trajectory formed by multiple position data.

[0052] Please see Figure 7 , Figure 7 This is an example diagram of the global coordinate system in an exemplary embodiment of this application, such as... Figure 7 As shown, in the global coordinate system (O-XY) parallel to the horizontal plane, the origin O represents the initial position of the object being followed (assuming the position of the object at t=0); the X-axis represents the initial direction of movement of the object at k=0 (defined as the "global direction of movement"); and the Y-axis is perpendicular to the X-axis and pointing upwards (right-handed coordinate system, counterclockwise is the positive angle direction). Therefore, at any given time, relative to the origin O, the global coordinates of the tracked target (i.e., the subsequent coordinates of the second target) are expressed as: The global coordinates of the following object (i.e., the coordinates of subsequent objects) are represented as .

[0053] Optionally, based on linear velocity, angular velocity, Euclidean distance, and tracking angle deviation, position derivation is performed in the global coordinate system to obtain the target's position data in the global coordinate system, including: The current global heading angle of the following object in the global coordinate system is obtained by calculating based on angular velocity. The formula for calculating the current global heading angle is:

[0054] In this context, the start time of each sampling period is considered a sampling time. Indicates the first The current global heading angle at the sampling moment, that is, the heading angle of the following object at the th sampling moment. The angle between the forward direction at each sampling moment and the X-axis of the global coordinate system, with counterclockwise being positive; Indicates the first The angular velocity at each sampling moment is positive for counterclockwise and negative for clockwise. Indicates the first -1 sampling time to the 1st sampling time The time interval between each sampling moment also represents the duration of the sampling period; Based on the global heading angle, linear velocity, Euclidean distance, and follow angle deviation angle, the coordinates of the second target in the global coordinate system are derived to obtain the tracking target's coordinates. The second target coordinates are defined as the position data of the tracking target relative to the following object in the global coordinate system.

[0055] In the embodiment provided in this application, the current global heading angle of the following object in the global coordinate system is obtained by calculation based on angular velocity. Based on the global heading angle, linear velocity, Euclidean distance and following angle deviation angle, coordinate derivation is performed to obtain the second target coordinates of the tracking target in the global coordinate system. These coordinates are determined as the position data of the tracking target relative to the following object in the global coordinate system. This enables the relative positioning of the tracking target based on the following object in the global coordinate system, thereby improving the accuracy of the following object in tracking the target based on the relative trajectory formed by multiple position data.

[0056] In this embodiment, due to the current global heading angle of the following object B... This data accumulates over time, and to avoid numerical overflow, it needs to be normalized periodically. For example, the current global heading angle. After rotating counterclockwise 3 times =6π, which needs to be normalized to [0,2π] or [-π,π].

[0057] Optionally, coordinate derivation is performed based on the global heading angle, linear velocity, Euclidean distance, and following angle deviation angle to obtain the second target coordinates of the tracked target in the global coordinate system, including: Based on the current global heading angle and linear velocity corresponding to each historical sampling period, the displacement is accumulated to obtain the object coordinates of the following object in the global coordinate system under the current sampling period; The formula for calculating the object's coordinates is as follows:

[0058]

[0059] In this context, the start time of each sampling period is considered a sampling time. Indicates the first The x-coordinate of the object's coordinates at each sampling time; Indicates the first The linear velocity at the sampling time n, its direction being the same as the linear velocity of the object at the nth sampling time n. The current direction of travel at each sampling moment; Indicates the first -1 sampling time to the 1st sampling time The time interval between each sampling moment also represents the duration of the sampling period; Indicates the first -1 current global heading angle at sampling time. Indicates the first The ordinate of the object's coordinates at each sampling time; , () represents the coordinates of the origin (0,0) in the global coordinate system; The relative azimuth angle of the tracked target relative to the followed object in the global coordinate system is calculated based on the current global heading angle and the follow angle deviation angle. The formula for calculating the relative azimuth is as follows:

[0060] in, Indicates the first The relative azimuth angle at each sampling time. Indicates the first The current global heading angle at each sampling moment; Indicates the first The following angle deviation angle at the sampling time, that is, the first sampling time. The angular deviation of the tracking target relative to the current direction of travel of the following object at each sampling time is based on the heading angle of the following object, with counterclockwise deviation being negative. The second target coordinates of the tracked target in the global coordinate system are calculated based on the object coordinates, relative azimuth angle, and Euclidean distance. The formula for calculating the coordinates of the second target is as follows:

[0061]

[0062] in, Indicates the first The x-coordinate of the second target coordinate at each sampling time. Indicates the first The x-coordinate of the object's coordinates at each sampling time; Indicates the first The Euclidean distance at the nth sampling time, that is, the nth sampling time. The tracking target's straight-line distance relative to the following object is measured at each sampling time. Indicates the first The relative azimuth angle at each sampling time. Indicates the first The ordinate of the second target coordinate at each sampling time. Indicates the first The ordinate of the object's coordinates at each sampling time.

[0063] In the embodiment provided in this application, firstly, displacement accumulation is performed based on the current global heading angle and linear velocity corresponding to each historical sampling period to obtain the object coordinates of the tracking object in the global coordinate system under the current sampling period. Then, based on the current global heading angle and the following angle deviation angle, the relative azimuth angle of the tracking target relative to the tracking object in the global coordinate system is calculated. Secondly, based on the object coordinates, relative azimuth angle, and Euclidean distance, the second target coordinates of the tracking target in the global coordinate system are calculated and determined as the position data of the tracking target relative to the tracking object in the global coordinate system. This enables the relative positioning of the tracking target based on the tracking object in the global coordinate system, thereby improving the accuracy of the tracking object in following the target based on the relative trajectory formed by multiple position data.

[0064] In addition, to simplify calculations, discrete time step modeling can be used (the sampling period T can be set, usually 10~20ms; the sampling period is generally less than the sensor's data update period). Furthermore, all angle calculations are unified in radians (rad) to avoid confusion with angles (°). For example, if the sensor output is an angle (°), then the angle ° is first converted to radians: rad = ° * π / 180.

[0065] Optionally, a preset PID control method can be used to control the following object to run according to a relative trajectory.

[0066] In the embodiment provided in this application, a preset PID control method is used to control the following object to run according to the relative trajectory, which can achieve precise following control of the following object. For example, the speed and angular velocity of the following object B can be dynamically adjusted in real time, thereby improving the accuracy of the following object in following the target.

[0067] In an exemplary embodiment provided in this application, when applying the provided relative trajectory-based following control method, positioning and following can be specifically achieved using data collected by IMU and UWB devices. The overall approach is: "Establish a global coordinate system → Sample real-time states in discrete time → Accumulate and calculate the global coordinates of A / B → Store the trajectory in a time sequence." The core is to transform the dynamic motion of the tracking target A and the following object B into a global coordinate sequence in discrete time. The specific application steps of the relative trajectory-based following control method can be as follows: Figure 8 As shown, the application steps include: 1) Establish a two-dimensional Cartesian coordinate system with the initial position of the following object as the origin O of the global coordinate system and the initial forward direction of the following object as the positive X-axis (considering only planar motion; if three-dimensional motion is required, the Z-axis can be expanded). Define the two-dimensional Cartesian coordinate system as the global coordinate system. 2) Collect real-time data at fixed time intervals (sampling period T): Euclidean distance s of the tracking target relative to the following object, tracking angle deviation angle r; linear velocity v and angular velocity b of the following object relative to the ground; 3) At each sampling time, first calculate the corresponding coordinates of the following object relative to the global origin (based on the cumulative displacement of the following object's velocity and angular velocity); 4) Based on the positional relationship between the tracking target and the following object (relative pose data: Euclidean distance s, following angle deviation angle r), derive the second target coordinates of the tracking target relative to the global origin; 5) Sort the global coordinates of the tracking target / following object at all sampling times by time to form the historical trajectory storage of the two, and obtain the relative trajectory of the tracking target relative to the following object, as well as the driving trajectory of the following object. This not only makes it easier for the following object to run according to the relative trajectory and realize the target following of the tracking object, but also makes it easier to perform trajectory verification based on the two trajectories to determine the accuracy of positioning and following.

[0068] In this way, by acquiring the Euclidean distance s between the tracking target and the following object, the tracking angle deviation angle r, and the linear velocity v and angular velocity b of the following object's own motion, the trajectory of the tracking target and the following object relative to the initial position of the following object can be calculated in real time. Based on the relative trajectory of the tracking target and the following object, the motion of the following object can be precisely controlled, thereby achieving low-cost automatic robot following.

[0069] The specific steps of this implementation can be exemplified as follows: Step 1: Initialization (at k=0, t=0).

[0070] 1) The initial position of the following object B is the origin of the global coordinate system: .

[0071] 2) Following the initial heading angle of object B: The initial direction of travel is the X-axis of the global coordinate system, therefore... .

[0072] 3) Initial global coordinates of target A: First determine the initial relative pose data. and Therefore, the initial relative azimuth angle between the tracking target A and the following object B is: Therefore, the initial global coordinates of the tracking target A are: ; .

[0073] 4) Trajectory storage initialization: Create two arrays (or linked lists) to store the trajectory of the tracked target A and the trajectory of the followed object B into the arrays respectively: .

[0074] Step 2: The kth sampling time (k≥1, t_k=k) Calculation of T).

[0075] The calculation at each sampling moment follows the logic of "first calculating the global state of the following object B, then calculating the global state of the tracking target A". The core is to use the motion of the following object B to accumulate and calculate its object coordinates, and derive the second target coordinates of the tracking target A through the relative pose data.

[0076] Step 2.1: Calculate the global heading angle of the following object B in the global coordinate system at time t_k. The change in heading angle of object B is caused by the accumulation of angular velocity: the kth... From time 1 to time k (time interval T), the angular velocity of B is (Assuming the angular velocity is constant within the sampling period, and approximated by discretization), therefore the heading angle increment of the following object B is: Therefore, the global heading angle following object B is the accumulation of historical increments: Among them, if the angular velocity A positive value (counterclockwise rotation) indicates an increase in the heading angle; a negative value (clockwise rotation) indicates a decrease in the heading angle.

[0077] Sub-step 2.2: Calculate the object coordinates of the following object B in the global coordinate system at time t_k. Follow the linear velocity of object B Along its Heading angle at time Direction (assuming the linear velocity is constant within the sampling period), linear velocity along the heading angle or Directional projection * time.

[0078] Therefore, the displacement component following object B during this time period is: 1) Displacement in the X direction: ; 2) Displacement in the Y direction: .

[0079] Therefore, the object coordinates of object B in the global coordinate system are the cumulative historical displacements (accumulated from the origin): ; .

[0080] Sub-step 2.3: Calculate the coordinates of the second target A in the global coordinate system at time t_k. The global coordinates of the tracking target A = the global coordinates of the following object B + the local coordinates of A relative to B, which need to be converted to components in the global coordinate system.

[0081] 1) Local azimuth angle of tracking target A relative to following object B: The angular deviation of tracking target A relative to following object B is... That is, the azimuth angle of A in B's "local coordinate system" (with B as the origin and B's heading angle as the X' axis of the local coordinate system) is: Wherein, the angle between the local coordinate system X' axis and the global coordinate system X axis of B is... The deviation angle of A relative to B It is an offset based on the X' axis of the local coordinate system, therefore the relative azimuth angle in the local coordinate system is the sum of the two.

[0082] 2) Global displacement components of tracking target A relative to following object B: The distance between tracking target A and following object B is... Therefore, the global component of the relative displacement of A with respect to B is: ; .

[0083] 3) Track the coordinates of the second target of target A at time t_k in the global coordinate system: ; .

[0084] Sub-step 2.4: Trajectory storage and update.

[0085] Add the current coordinates of the tracking target A relative to the following object B, the second target coordinates, and the object coordinates of the following object B to the corresponding trajectory arrays: .

[0086] Step 3: Iterate through the loop (k=1→k=2→...→k=N): Repeat step 2 (from sub-step 2.1 to sub-step 2.4) until the object B stops following or sampling ends, and finally obtain the result. and This refers to the relative trajectory and driving trajectory of the tracking target A and the following object B relative to the starting point (initial position, i.e., the global origin O) of the following object B.

[0087] Step 4: By calculating the relative trajectory of the tracking target A and the driving trajectory of the following object B, and predicting the next position of the following object B based on the relative trajectory of the tracking target A, the following object B is precisely controlled by a preset PID control method. For example, the speed and angular velocity of the following object B are dynamically adjusted in real time (using conventional PID control method, which will not be elaborated here).

[0088] In an exemplary embodiment provided in this application, based on the above-described relative trajectory-based following control scheme, the trajectory of the tracking target A relative to the following object B is plotted in real time through testing on a real vehicle. and Furthermore, by installing an RTK (RTK is used only as a third-party measurement tool) on a real vehicle (following object B), the rationality and accuracy of tracking based on relative trajectory were verified. The trajectory of the tracked target A and the trajectory of the followed object B were found to be consistent, and the Euclidean distance was [data missing]. It matches the actual situation, with an error of less than 10cm.

[0089] Please see Figure 9 , Figure 9 An exemplary embodiment of this application illustrates a relative trajectory-based following control system, such as... Figure 9 As shown, this application provides a relative trajectory-based following control system 900, including: The acquisition module 901 is used to acquire the current pose data of the following object and the relative pose data of the tracking target relative to the following object according to a preset sampling period; The positioning module 902 is used to locate the target based on the current pose data and the relative pose data, and to obtain the position data of the tracked target relative to the followed object; The trajectory forming module 903 is used to form a relative trajectory of the tracking target relative to the follower based on position data from multiple sampling periods; The follow control module 904 is used to control the follower object to run according to the relative trajectory, so as to realize the follower object following the target.

[0090] The relative trajectory-based following control system 900 provided in this application first acquires the current pose data of the target being tracked and the relative pose data of the target relative to the target being tracked by the acquisition module 901 according to a preset sampling period. Then, the positioning module 902 performs target positioning based on each current pose data and the corresponding relative pose data to obtain the position data of the target relative to the target being tracked in the corresponding sampling period, thereby achieving relative positioning of the target being tracked. Next, the trajectory forming module 903 forms the relative trajectory of the target being tracked relative to the target being tracked by the position data from multiple sampling periods. The following control module 904 controls the target being tracked to run according to the relative trajectory to achieve tracking of the target by the target being tracked. In this way, by using the current pose data of the target being tracked to perform relative positioning to obtain multiple position data of the target being tracked relative to the target, the influence of environmental factors such as multipath effects, light, and signal obstruction caused by direct positioning by RTK devices on the positioning of the target being tracked can be reduced. This improves the positioning accuracy of the target being tracked, thereby improving the accuracy of the relative trajectory of the target being tracked based on multiple position data, and further improving the accuracy of target tracking by controlling the target being tracked according to the relative trajectory.

[0091] Optionally, an RTK device is installed on the object being followed. When the RTK device is functioning correctly, the current pose data includes the current position and current heading angle detected by the RTK device; the relative pose data includes the Euclidean distance and the following angle deviation angle. Positioning module 902 is specifically used for: Based on the current position, current heading angle, Euclidean distance, and follow angle deviation angle, coordinate calculations are performed to obtain the first target coordinates of the tracked target in the world coordinate system. The first target coordinates are defined as the position data of the tracking target relative to the following object in the world coordinate system.

[0092] Optionally, an IMU device is installed on the object being followed, and the current pose data includes the triaxial linear acceleration and triaxial angular velocity detected by the IMU device; the relative pose data includes the Euclidean distance and the following angle deviation angle. Positioning module 902 is specifically used for: Based on standard calculations using triaxial linear acceleration and triaxial angular velocity, the linear velocity and angular velocity of the following object relative to the ground are obtained; A global coordinate system is constructed based on the following object; Based on linear velocity, angular velocity, Euclidean distance, and following angle deviation, the position is derived in the global coordinate system to obtain the position data of the tracked target in the global coordinate system.

[0093] Optionally, the positioning module 902 is specifically used for: Obtain the initial pose data of the followed object, which includes the initial position and initial direction of travel. With the initial position as the origin and the initial direction of movement as the x-axis, a two-dimensional Cartesian coordinate system parallel to the horizontal plane is constructed. The two-dimensional Cartesian coordinate system is defined as the global coordinate system.

[0094] Optionally, the positioning module 902 is specifically used for: The current global heading angle of the following object in the global coordinate system is obtained by calculating based on angular velocity. Based on the global heading angle, linear velocity, Euclidean distance, and follow angle deviation angle, the coordinates of the second target in the global coordinate system are derived to obtain the tracking target's coordinates. The second target coordinates are defined as the position data of the tracking target relative to the following object in the global coordinate system.

[0095] Optionally, the positioning module 902 is specifically used for: Based on the current global heading angle and linear velocity corresponding to each historical sampling period, the displacement is accumulated to obtain the object coordinates of the following object in the global coordinate system under the current sampling period; The relative azimuth angle of the tracked target relative to the followed object in the global coordinate system is calculated based on the current global heading angle and the follow angle deviation angle. The second target coordinates in the global coordinate system are obtained by calculating the object coordinates, relative azimuth angle, and Euclidean distance.

[0096] Optionally, the follow control module 904 is specifically used for: The preset PID control method is used to control the following object to run according to the relative trajectory.

[0097] It should be noted that the relative trajectory-based following control system provided in the above embodiments and the relative trajectory-based following control method provided in the above embodiments belong to the same concept. The specific ways in which each module and unit performs operations have been described in detail in the method embodiments, and will not be repeated here. In practical applications, the relative trajectory-based following control system provided in the above embodiments can be assigned to different functional modules as needed, that is, the internal structure of the system can be divided into different functional modules to complete all or part of the functions described above, and this is not a limitation here.

[0098] A computing device according to an embodiment of this application includes a memory, a processor, and a program stored in the memory and running on the processor. When the processor executes the program, it implements some or all of the steps of the above-described relative trajectory-based following control method.

[0099] The computing device can be a computer, and the corresponding program is computer software. The parameters and steps of the computing device described above can be referred to the parameters and steps of the embodiment of the relative trajectory-based following control method described above, and will not be repeated here.

[0100] Figure 10 A schematic diagram of a computer system suitable for implementing the embodiments of this application is shown. It should be noted that... Figure 10 The computer system 1000 shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0101] like Figure 10 As shown, the computer system 1000 includes a Central Processing Unit (CPU) 1001, which can perform various appropriate actions and processes, such as executing the methods described in the above embodiments, based on programs stored in Read-Only Memory (ROM) 1002 or programs loaded from Storage Unit 1008 into Random Access Memory (RAM) 1003. The RAM 1003 also stores various programs and data required for system operation. The CPU 1001, ROM 1002, and RAM 1003 are interconnected via a bus 1004. An Input / Output (I / O) interface 1005 is also connected to the bus 1004.

[0102] The following components are connected to I / O interface 1005: an input section 1006 including a keyboard, mouse, etc.; an output section 1007 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 1008 including a hard disk, etc.; and a communication section 1009 including a network interface card such as a LAN (Local Area Network) card, modem, etc. The communication section 1009 performs communication processing via a network such as the Internet. A drive 1010 is also connected to I / O interface 1005 as needed. Removable media 1011, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 1010 as needed so that computer programs read from them can be installed into storage section 1008 as needed.

[0103] Specifically, according to embodiments of this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program including a computer program for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 1009, and / or installed from removable medium 1011. When the computer program is executed by central processing unit (CPU) 1001, it performs various functions defined in the system of this application.

[0104] This application embodiment provides a computer-readable storage medium storing instructions that, when executed, perform the steps of the aforementioned relative trajectory-based following control method. The computer-readable storage medium can be either transient or non-transient.

[0105] The technical solutions of this disclosure can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes one or more instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of this disclosure. The aforementioned computer-readable storage medium can be a non-transitory computer-readable storage medium, including: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks, and other media capable of storing program code; it can also be a transient computer-readable storage medium.

[0106] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0107] Those skilled in the art will recognize that this application can be implemented as a system, method, or computer program product. Therefore, this disclosure can be implemented in the following forms: it can be entirely hardware, entirely software (including firmware, resident software, microcode, etc.), or a combination of hardware and software, generally referred to herein as a "module" or "system." Furthermore, in some embodiments, this application can also be implemented as a computer program product contained in one or more computer-readable media, which contains computer-readable program code. Computer-readable storage media can be, for example, but not limited to—electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof.

[0108] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0109] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. A following control method based on relative trajectory, characterized in that, include: The current pose data of the object being followed, and the relative pose data of the target being tracked relative to the object being followed, are obtained according to a preset sampling period. Target localization is performed based on the current pose data and the relative pose data to obtain the position data of the tracked target relative to the followed object; The relative trajectory of the tracking target with respect to the following object is formed based on position data from multiple sampling periods; The following object is controlled to run according to the relative trajectory so as to achieve the following object following the target.

2. The method according to claim 1, characterized in that, The object being followed is equipped with an RTK device. When the RTK device is functioning normally, the current pose data includes the current position and current heading angle detected by the RTK device; the relative pose data includes Euclidean distance and following angle deviation angle. The step of performing target localization based on the current pose data and the relative pose data to obtain the position data of the tracked target relative to the followed object includes: Based on the current position, the current heading angle, the Euclidean distance, and the following angle deviation angle, coordinate calculations are performed to obtain the first target coordinates of the tracked target in the world coordinate system; The first target coordinates are determined as the position data of the tracking target relative to the following object in the world coordinate system.

3. The method according to claim 1, characterized in that, The object being followed is equipped with an IMU device, and the current pose data includes the triaxial linear acceleration and triaxial angular velocity detected by the IMU device; the relative pose data includes Euclidean distance and the following angle deviation angle. The step of performing target localization based on the current pose data and the relative pose data to obtain the position data of the tracked target relative to the followed object includes: Based on the triaxial linear acceleration and triaxial angular velocity, a standard calculation is performed to obtain the linear velocity and angular velocity of the following object relative to the ground; A global coordinate system is constructed based on the following object; Based on the linear velocity, the angular velocity, the Euclidean distance, and the following angle deviation, the position is derived in the global coordinate system to obtain the position data of the tracked target in the global coordinate system.

4. The method according to claim 3, characterized in that, The process of constructing a global coordinate system based on the following object includes: Obtain the initial pose data of the followed object, the initial pose data including the initial position and the initial direction of travel; Using the initial position as the origin and the initial forward direction as the x-axis, a two-dimensional Cartesian coordinate system parallel to the horizontal plane is constructed. The two-dimensional Cartesian coordinate system is defined as the global coordinate system.

5. The method according to claim 3, characterized in that, The step of deriving the position data of the tracked target in the global coordinate system based on the linear velocity, the angular velocity, the Euclidean distance, and the following angle deviation includes: Based on the angular velocity, the current global heading angle of the following object in the global coordinate system is calculated. Based on the global heading angle, the linear velocity, the Euclidean distance, and the following angle deviation angle, coordinate derivation is performed to obtain the second target coordinates of the tracked target in the global coordinate system; The second target coordinates are determined as the position data of the tracking target relative to the following object in the global coordinate system.

6. The method according to claim 5, characterized in that, The process of deriving the second target coordinates of the tracked target in the global coordinate system based on the global heading angle, the linear velocity, the Euclidean distance, and the following angle deviation angle includes: Based on the current global heading angle and linear velocity corresponding to each historical sampling period, the displacement is accumulated to obtain the object coordinates of the following object in the global coordinate system under the current sampling period; The relative azimuth angle of the tracked target relative to the tracked object in the global coordinate system is calculated based on the current global heading angle and the following angle deviation angle. The second target coordinates of the tracked target in the global coordinate system are calculated based on the object coordinates, the relative azimuth angle, and the Euclidean distance.

7. The method according to any one of claims 1 to 6, characterized in that, The following object is controlled to run according to the relative trajectory using a preset PID control method.

8. A tracking control system based on relative trajectory, characterized in that, include: The acquisition module is used to acquire the current pose data of the following object and the relative pose data of the tracking target relative to the following object according to a preset sampling period; The positioning module is used to perform target positioning based on the current pose data and the relative pose data to obtain the position data of the tracked target relative to the followed object; A trajectory forming module is used to form a relative trajectory of the tracking target relative to the following object based on position data from multiple sampling periods; The follow control module is used to control the follow object to run according to the relative trajectory, so as to realize the follow object following the tracking target.

9. A computing device, comprising a memory, a processor, and a program stored in the memory and running on the processor, characterized in that, When the processor executes the program, it implements the steps of a relative trajectory-based following control method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores instructions that, when executed on a terminal device, cause the terminal device to perform the steps of a relative trajectory-based following control method as described in any one of claims 1 to 7.

Images