A tracking method, device, equipment and medium for a self-balancing robot

Abstract

The application relates to the technical field of robot control, and discloses a tracking method, device, equipment and medium for a self-balancing robot, wherein the method comprises the following steps: acquiring a target pose at a low first frequency by a self-balancing robot serving as a slave vehicle, and calculating a motion state containing position, velocity and acceleration according to the target pose. In order to adapt to high-frequency control required by the self-balancing robot, the method performs extrapolation processing on the determined motion state to generate a target trajectory which is continuously updated at a second frequency and has a frequency much higher than the first frequency. Finally, the robot determines a control torque according to the high-frequency target trajectory, and realizes stable following of a moving target serving as a master vehicle on the premise of maintaining dynamic balance of the robot. The application generates a high-frequency trajectory through extrapolation, effectively solves the technical problem that low-frequency pose acquisition and high-frequency balance control requirements do not match, and improves continuity and stability of tracking control.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular to a tracking method, device, equipment and medium for a self-balancing robot. BACKGROUND

[0002] When performing a tracking task, the control system of a self-balancing robot needs high-frequency and continuous instructions to maintain its dynamic balance in real time. However, in the existing tracking scheme, the sensor for perceiving the target pose usually provides discrete data periodically at a relatively low frequency. When such low-frequency and discontinuous pose data is directly used to generate control instructions, it will cause the mutation and discontinuity of the instructions, which is easy to cause the attitude jitter of the self-balancing robot, and even cause instability in the dynamic tracking process due to unsmooth control.

[0003] Therefore, there is an urgent need for a tracking method for a self-balancing robot, which can effectively convert low-frequency and discrete target pose measurements into continuous and smooth target trajectories for high-frequency balancing controllers. SUMMARY

[0004] Therefore, the present application provides a tracking method, device, equipment and medium for a self-balancing robot to solve the problem of discontinuous control instructions and dynamic instability caused by the mismatch between sensing and control frequencies in the prior art.

[0005] In a first aspect, the present application provides a tracking method for a self-balancing robot, which is performed by a self-balancing robot as a slave vehicle, and the method is used to track a moving target as a master vehicle, and the method comprises:

[0006] periodically obtaining pose data of the moving target at a first frequency to obtain time-series pose data of the moving target;

[0007] determining the motion state of the moving target according to the time-series pose data; the motion state includes position, velocity and acceleration;

[0008] extrapolating the motion state to generate a target trajectory of the self-balancing robot at a second frequency; the second frequency is higher than the first frequency;

[0009] determining the control torque of the self-balancing robot based on the target trajectory, so that the self-balancing robot keeps balance when tracking along the target trajectory.

[0010] The tracking method for the self-balancing robot provided in the application determines the complete motion state of the moving target including position, velocity and acceleration according to the time sequence pose data. This provides more abundant and more accurate physical model input for subsequent trajectory prediction and extrapolation, and is the basis for realizing high-quality trajectory generation. The determined motion state is extrapolated to generate a target trajectory at a second frequency which is much higher than the data acquisition frequency (first frequency). The low-frequency and discrete state input points are converted into a high-frequency and continuous trajectory instruction stream. Therefore, even in the gap between two original pose data acquisition, the control system of the self-balancing robot can continuously obtain smooth and coherent target guidance, and fundamentally solves the control instruction delay and jump problem caused by the mismatch of data frequency. Finally, the self-balancing robot determines the control torque based on the high-frequency and smooth target trajectory. Since the reference trajectory serving as the control basis is continuous and changes smoothly, the calculated control torque is also continuous and smooth, avoiding the violent impact on the robot posture caused by the instruction mutation. This enables the robot to accurately track the moving target while effectively maintaining its dynamic balance, thereby significantly improving the smoothness and stability of the entire tracking process, and solving the problem that the self-balancing robot in the prior art is prone to shaking or even instability and falling due to discontinuous control.

[0011] In an optional embodiment, the pose data of the moving target is acquired, including:

[0012] The visual marker installed on the moving target is detected by the camera mounted on the self-balancing robot, the pose of the visual marker relative to the camera is calculated, and the pose data of the moving target is obtained.

[0013] The tracking method for the self-balancing robot provided in the application uses the visual marker as a passive beacon, so that the pose acquisition process is completely independent of external wireless communication (such as Wi-Fi, GPS). Therefore, it has the ability to work stably in indoor, tunnel and other communication limited or signal free environments, reduces the dependence of the system on communication hardware, thereby reducing the cost and power consumption, and fundamentally avoids the risk of tracking failure caused by communication delay or interruption.

[0014] In an optional embodiment, the motion state of the moving target is determined, including:

[0015] The time sequence pose data is used to determine the relative pose of the moving target relative to the self-balancing robot;

[0016] Based on the relative pose and the pose of the self-balancing robot in the world coordinate system, the time sequence position data of the moving target in the world coordinate system is obtained through coordinate system conversion;

[0017] Based on the time-series position data, the speed and acceleration of the moving target are calculated by backward difference calculation to determine the motion state of the moving target.

[0018] The tracking method for the self-balancing robot provided in the application enables the motion planning and control of the robot to be performed in a unified global coordinate system through coordinate conversion; and through differential calculation, the system can grasp the dynamic trend (speed and acceleration) of the target, rather than only its instantaneous position, thereby providing an indispensable data basis for subsequent accurate prediction and control.

[0019] In an optional embodiment, before extrapolating the motion state, the method further comprises:

[0020] The motion state is smoothed by exponential weighted filtering.

[0021] The tracking method for the self-balancing robot provided in the application can effectively suppress the data jitter and burr introduced by sensor measurement noise and differential calculation by introducing exponential weighted filtering. This makes the motion state data input into the subsequent extrapolation model smoother and more stable, and closer to the real physical motion of the target. Therefore, the quality of the final target trajectory generated is significantly improved, and sudden changes in the trajectory caused by noise interference are avoided, thereby laying a solid foundation for achieving more stable robot control.

[0022] In an optional embodiment, the motion state is extrapolated, comprising:

[0023] The motion state is input into a preset constant acceleration prediction model for extrapolation to generate a target trajectory; wherein the target trajectory is composed of a series of target points, and the time sampling frequency of the target points is a second frequency.

[0024] In an optional embodiment, the constant acceleration prediction model extrapolates according to a second-order polynomial, and the expression is:

[0025] ;

[0026] ;

[0027] In the formula, and respectively represent the predicted position and speed of the rear target vehicle at any control time t; 、 、 respectively represent the latest position, speed and acceleration at the time of the last visual detection update; represents the time difference between the current control time t and the time of the last visual update . .

[0028] The tracking method for a self-balancing robot provided by the present application not only considers the position and speed of the target, but also takes the acceleration into the prediction process. Compared with zero-order (keeping position) or first-order (uniform speed) prediction, the motion trend of the target in a short time can be more accurately predicted, especially when the target is accelerating or decelerating. Using this model to generate high-frequency (second frequency) trajectory points ensures that the generated trajectory is not only high in frequency and large in density, but also more reasonable and accurate in physics.

[0029] In an optional embodiment, periodically acquiring the pose data of the moving target at the first frequency further includes:

[0030] When the moving target is continuously successfully identified, and the linear speed of the moving target reaches a preset non-zero threshold, the pose data of the moving target is periodically acquired at the first frequency.

[0031] The tracking method for a self-balancing robot provided by the present application can effectively prevent two typical failure scenarios: one is to start tracking a non-existent or incorrect target due to instantaneous misidentification of the sensor or environmental interference; the other is to try to track a completely stationary target, which may cause unnecessary jitter or oscillation of the robot when fine-tuning in place. Therefore, this feature significantly improves the robustness and safety of the system in the starting stage, ensuring that the tracking task only starts after the target is confirmed to be a stable and effective dynamic object.

[0032] In an optional embodiment, the method further includes:

[0033] When the pose data of the moving target is continuously lost for a preset number of frames, or the error between the predicted target trajectory and the actual control state exceeds a preset threshold, the self-balancing robot switches to a preset standby trajectory control mode.

[0034] The tracking method for a self-balancing robot provided by the present application avoids the robot still blindly executing a predicted trajectory that has been invalidated or has accumulated too much error after losing the target, which is extremely likely to cause the robot to lose control or collide. By switching to a standby mode (such as keeping balance in place or executing a preset safety path), the present application ensures that the robot can still maintain its safety and stability even in the case of visual perception interruption, greatly enhancing the reliability and practicality of the method in complex environments.

[0035] In an optional embodiment, determining the control torque of the self-balancing robot includes:

[0036] Based on the under-actuated dynamics constraint and the ground reaction force constraint of the self-balancing robot, the control torque is generated.

[0037] The tracking method for the self-balancing robot provided in the application directly incorporates the inherent physical constraints (underdrive characteristics, ground friction, etc.) of the robot into the calculation of the control torque, ensuring that each control instruction generated is physically achievable. Invalid instructions (e.g., requiring the robot to instantaneously translate laterally) that exceed the physical limits of the robot and can be issued by traditional simple controllers are avoided. Therefore, the effectiveness and safety of the control instructions can be guaranteed from the root, so that the robot can still stably operate within its physical limits when performing high-speed and large-maneuver tracking tasks, thereby maximizing the performance of the robot while ensuring its balance and stability.

[0038] In summary, the tracking method for the self-balancing robot provided in the application first acquires the relative pose of the target by detecting the visual marker using a camera, and then combines the pose of the robot itself to obtain the complete motion state of the target in the global coordinate system, including position, velocity, and acceleration, through coordinate transformation and differential calculation. A data processing link is constructed from raw visual information to complete physical state without relying on external communication, ensuring that the system can acquire the basic data required for subsequent prediction in any environment. On this basis, in order to improve data quality, an exponential weighted filter is introduced to smooth the motion state, effectively eliminating sensor noise and calculation errors, and providing a more reliable and smooth data source for subsequent prediction. This is a prerequisite for achieving high-quality tracking. Then, using the smoothed motion state, a constant acceleration prediction model is applied for extrapolation to generate a high-density and continuous smooth target trajectory at a second frequency much higher than the data acquisition frequency. This bridges the gap between low-frequency sensing and high-frequency control, fundamentally solving the control instruction jump problem caused by mismatched data frequencies. In addition, to further enhance robustness in practical applications, safety strategies are added at both ends of the process. At the front end, a start judgment mechanism is activated to ensure that the tracking task only starts when the target is confirmed to be a valid dynamic object, avoiding misjudgment and instability in the initial stage. At the back end, a fault-tolerant switching mechanism is used to safely switch to a backup mode in the face of sudden conditions such as target loss, preventing loss of control due to perception interruption and greatly improving reliability. Finally, this target trajectory is input into a controller that fully considers the physical constraints of the robot. This ensures that each motor control torque generated is not only accurate in driving the robot along the trajectory, but also completely within the physical capabilities of the robot.

[0039] In a second aspect, the application provides a tracking device for a self-balancing robot, which is executed by a self-balancing robot as a slave vehicle, and the device is used to track a moving target as a master vehicle, the device comprising:

[0040] acquire, periodically, pose data of the moving target at a first frequency, to obtain time-series pose data of the moving target;

[0041] confirm, according to the time-series pose data, a motion state of the moving target; the motion state comprises position, velocity and acceleration;

[0042] generate, at a second frequency, a target trajectory of the self-balancing robot by extrapolating the motion state; the second frequency is higher than the first frequency;

[0043] determine, based on the target trajectory, a control torque of the self-balancing robot, so that the self-balancing robot keeps balance when tracking the target trajectory.

[0044] In a third aspect, a computer device is provided, comprising a memory and a processor, which are communicatively connected with each other, and the memory stores computer instructions; the processor executes the computer instructions, thereby executing the tracking method for the self-balancing robot according to the first aspect or any possible implementation thereof.

[0045] In a fourth aspect, a computer readable storage medium is provided, which stores computer instructions; the computer instructions are used to make a computer execute the tracking method for the self-balancing robot according to the first aspect or any possible implementation thereof.

[0046] In a fifth aspect, a computer program product is provided, which comprises computer instructions; the computer instructions are used to make a computer execute the tracking method for the self-balancing robot according to the first aspect or any possible implementation thereof. BRIEF DESCRIPTION OF DRAWINGS

[0047] In order to more clearly illustrate the technical solutions of the present application or prior art, the following will briefly introduce the drawings needed in the specific embodiments or prior art description. Obviously, the drawings described below are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative labor.

[0048] Figure 1 is a flowchart of a tracking method for a self-balancing robot according to an embodiment of the present application;

[0049] Figure 2 is a working flowchart of a tracking system for a two-wheeled vehicle according to an embodiment of the present application;

[0050] Figure 3is a structural diagram of a tracking device for a self-balancing robot according to an embodiment of the present application;

[0051] Figure 4 is a hardware structure schematic diagram of a computer device according to an embodiment of the present application. DETAILED DESCRIPTION

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

[0053] In the autonomous tracking application of the self-balancing robot, the prior art mainly relies on two types of schemes: formation control based on communication and visual detection based on deep learning. The communication-based scheme exchanges state information such as pose and speed between vehicles through wireless networks (such as Wi-Fi, Bluetooth), which can achieve accurate formation control, but it is severely dependent on communication quality, completely fails in signal-limited environments (such as indoors, tunnels), and has inherent defects such as communication delay, high cost and safety risks. The visual detection scheme based on deep learning (such as YOLO, SSD) does not require communication, but its computational complexity is extremely high, making it difficult to achieve high-frame-rate real-time operation on resource-limited self-balancing robot embedded systems; at the same time, such algorithms are sensitive to changes in light, target occlusion and appearance changes, and have insufficient detection stability, and their output is usually a target bounding box, which requires complex post-processing to estimate the accurate six-degree-of-freedom pose, all of which can easily cause the self-balancing robot to lose stability and fall down due to target loss or pose calculation error.

[0054] To overcome the above-mentioned defects, a more stable and reliable solution is to use a camera to detect a pre-set visual marker (such as AprilTag) to obtain the target pose. However, even with this seemingly better solution, a series of deep technical problems still exist and need to be solved. First, the sampling frequency of the camera (e.g. 30Hz) is much lower than the frequency required for the balance control of the self-balancing robot (e.g. above 500Hz), and this significant frequency mismatch problem is the root cause of the discontinuity of the control instructions and the jitter of the robot operation. Second, when trying to estimate the speed and acceleration by differentiating the discrete position data at low frequency, the measurement noise of the sensor itself will be amplified sharply, resulting in the estimated motion state being full of burrs and interference, which cannot be directly used for accurate control. Third, as a dynamically unstable system, the self-balancing robot is extremely sensitive to the initial state and abnormality during operation, and the existing technology generally lacks targeted start-up stabilization strategies and fault tolerance mechanisms when the target is lost, which is prone to lose control when the task starts or the perception is interrupted. Finally, the general controller usually does not fully consider the unique physical dynamics constraints such as under-actuation and ground friction of the self-balancing robot, which is easy to output control instructions that cannot be physically realized, thereby limiting the performance of the robot and may cause instability.

[0055] Therefore, the prior art has not yet provided a complete and highly robust autonomous tracking solution that can systematically solve all the above problems.

[0056] In this embodiment, a tracking method for a self-balancing robot is provided, which is executed by a self-balancing robot as a slave vehicle, and the method is used to track a moving target as a master vehicle, Figure 1 The flowchart of the tracking method for a self-balancing robot according to the embodiments of the present application is shown in Figure 1 The flowchart includes the following steps:

[0057] S101, periodically obtain the pose data of the moving target at a first frequency to obtain the time-series pose data of the moving target.

[0058] Specifically, the self-balancing robot is a special robot whose physical structure makes it unable to naturally maintain stability when stationary or in motion, and must be maintained by active and continuous control.

[0059] The master vehicle refers to the moving target being tracked, and the slave vehicle refers to the self-balancing robot executing the method.

[0060] The pose data is a comprehensive information containing the position and attitude of the target. The position refers to the specific coordinates (e.g. x, y, z) of the target in three-dimensional space, and the attitude describes the orientation of the target (e.g. roll (roll) \ pitch (pitch) \ yaw (yaw) rotation angle around the x, y, z axis).

[0061] The first frequency refers to the rate of collecting target pose data from on-board perception systems (e.g. cameras), which is a fixed, low frequency, e.g. 30 times per second (30 Hz).

[0062] The time-series pose data is a series of pose data points with timestamps, since the collection is periodic. These data points are arranged in time order, forming a history of the target's pose at different time instants.

[0063] S102, determine the motion state of the moving target according to the time-series pose data; the motion state includes position, velocity and acceleration.

[0064] Specifically, the motion state is a complete description of how the target moves. It is not enough to have position information alone, the motion trend also needs to be known.

[0065] Determining the motion state is a process of processing and calculating the original time-series pose data to derive more abundant motion information.

[0066] The position is directly obtained from the pose data acquired in the previous step.

[0067] The velocity can be calculated by comparing the change in position between two consecutive time points and the time interval, which can determine the speed and direction of the target movement.

[0068] Similarly, the acceleration can be calculated by comparing the change in velocity between two consecutive time points and the time interval, which can determine the rate of change of the target's velocity.

[0069] S103, extrapolate the motion state to generate the target trajectory of the self-balancing robot at a second frequency; the second frequency is higher than the first frequency.

[0070] Specifically, extrapolation is one of the core steps of the embodiment. Since the first frequency (perception frequency) is low, the motion state obtained is discrete and has intervals. Extrapolation is a prediction technique that uses known motion states (position, velocity, acceleration) to fill in the gaps between two known data points and predict the state in the near future.

[0071] The second frequency is a frequency much higher than the first frequency, e.g. 500 times per second (500 Hz). This is the instruction frequency required by the balance control system of the self-balancing robot.

[0072] The target trajectory is a series of dense and continuous target points generated by the system at a high second frequency through extrapolation. These points together form a smooth path that the robot can follow at a high frequency. The essence of this step is to convert low-frequency, sparse perception data into high-frequency, dense control instructions.

[0073] S104, based on the target trajectory, determine the control torque of the self-balancing robot to keep the self-balancing robot balanced when tracking the target trajectory.

[0074] Specifically, the control torque is the physical driving force calculated by the robot controller and applied to the wheel motor. This torque directly determines the speed and direction of the wheel.

[0075] The controller will continuously compare the current state of the robot with the next target point on the high-frequency target trajectory, and calculate the control torque required to accurately move to the target point.

[0076] Keeping balance while tracking is the ultimate goal and constraint. The calculated control torque must meet two conditions: one is to drive the robot to track the target trajectory generated in the previous step; the second is to maintain the dynamic balance of the robot itself to prevent it from falling.

[0077] The tracking method for the self-balancing robot provided by the embodiments of the present application determines the complete motion state of the moving target including position, velocity and acceleration according to the time sequence pose data. This provides more abundant and accurate physical model input for subsequent trajectory prediction and extrapolation, and is the basis for realizing high-quality trajectory generation. The determined motion state is extrapolated to generate a target trajectory at a second frequency much higher than the data acquisition frequency (first frequency). The low-frequency, discrete state input points are converted into high-frequency, continuous trajectory instruction streams. Therefore, even in the gap between two original pose data acquisition, the control system of the self-balancing robot can continuously obtain smooth and coherent target guidance, fundamentally solving the control instruction delay and jump problem caused by the mismatch of data frequency. Finally, the self-balancing robot determines its control torque based on this high-frequency, smooth target trajectory. Since the reference trajectory as the control basis is continuous and changes smoothly, the calculated control torque is also continuous and smooth, avoiding the violent impact on the robot's posture caused by sudden changes in instructions. This enables the robot to accurately track the moving target while effectively maintaining its dynamic balance, thereby significantly improving the smoothness and stability of the entire tracking process, solving the problem of the self-balancing robot in the prior art being prone to shaking and even instability and falling due to discontinuous control.

[0078] In an optional embodiment, the pose data of the moving target is obtained, comprising:

[0079] The camera mounted on the self-balancing robot detects the visual marker installed on the moving target, calculates the pose of the visual marker relative to the camera, and obtains the pose data of the moving target.

[0080] The visual marker is used as a passive beacon, so that the pose acquisition process is completely independent of external wireless communication (such as Wi-Fi, GPS). Therefore, it has the ability to work stably in indoor, tunnel and other communication limited or signal-free environments, reduces the dependence of the system on communication hardware, thereby reducing cost and power consumption, and fundamentally avoids the risk of tracking failure caused by communication delay or interruption.

[0081] In an optional embodiment, determining the motion state of the moving target comprises:

[0082] Determining the relative pose of the moving target with respect to the self-balancing robot from the time-series pose data;

[0083] Based on the relative pose and the self pose of the self-balancing robot in the world coordinate system, the time-series position data of the moving target in the world coordinate system is obtained through coordinate system conversion;

[0084] Based on the time-series position data, the velocity and acceleration of the moving target are calculated through backward difference calculation to determine the motion state of the moving target.

[0085] Through coordinate conversion, the motion planning and control of the robot can be carried out in a unified global coordinate system; and through difference calculation, the system can grasp the dynamic trend (velocity and acceleration) of the target, not just its instantaneous position, providing an essential data foundation for subsequent accurate prediction and control.

[0086] In an optional embodiment, before extrapolating the motion state, it further comprises:

[0087] The motion state is smoothed by exponential weighted filtering.

[0088] By introducing exponential weighted filtering, data jitter and burr introduced by sensor measurement noise and difference calculation can be effectively suppressed. This makes the motion state data input to the subsequent extrapolation model smoother and more stable, closer to the real physical motion of the target. Therefore, the quality of the final target trajectory generated is significantly improved, avoiding sudden changes in the trajectory caused by noise interference, thereby laying a solid foundation for more stable robot control.

[0089] In an optional embodiment, extrapolating the motion state comprises:

[0090] The motion state is input to a preset constant acceleration prediction model for extrapolation to generate a target trajectory; wherein the target trajectory is composed of a series of target points, and the time sampling frequency of the target points is a second frequency.

[0091] In an optional embodiment, the constant acceleration prediction model extrapolates according to a second-order polynomial, and the expression is:

[0092] ;

[0093] ;

[0094] wherein, and respectively represent the predicted target position and velocity of the rear vehicle at any control time t; , , respectively represent the latest position, velocity and acceleration at the last visual detection update time; represents the time difference between the current control time t and the last visual update time . .

[0095] Not only the position and velocity of the target are considered, but also its acceleration is taken into account in the prediction process. Compared with zero-order (keeping position) or first-order (uniform velocity) prediction, the motion trend of the target in a short time can be more accurately predicted, especially when the target is accelerating or decelerating. Using this model to generate high-frequency (second frequency) trajectory points ensures that the generated trajectory is not only high in frequency and large in density, but also more reasonable and accurate in physics.

[0096] In an optional embodiment, periodically acquiring the pose data of the moving target at the first frequency further comprises:

[0097] When the moving target is continuously identified successfully and the linear velocity of the moving target reaches a preset non-zero threshold, periodically acquiring the pose data of the moving target at the first frequency.

[0098] It can effectively prevent two typical failure scenarios: one is to start tracking a non-existent or incorrect target due to instantaneous misidentification of the sensor or environmental interference; the other is to try to track a completely stationary target, which may cause unnecessary jitter or oscillation when the robot fine-tunes in place. Therefore, this feature significantly improves the robustness and safety of the system in the starting stage, ensuring that the tracking task only starts after the target is confirmed to be a stable and effective dynamic object.

[0099] In an optional embodiment, the method further comprises:

[0100] When the pose data of the moving target is continuously lost for a preset number of frames, or the error between the predicted target trajectory and the actual control state exceeds a preset threshold, the self-balancing robot switches to a preset standby trajectory control mode.

[0101] The robot avoids blindly executing a predicted trajectory that has already failed or accumulated too much error after losing the target, which can lead to a loss of control or collision. By switching to a backup mode (such as maintaining balance in place or executing a preset safety path), the present invention ensures that the robot remains safe and stable even in the event of a visual perception interruption, greatly enhancing the reliability and practicality of the method in complex environments.

[0102] In an optional embodiment, determining the control torque of the self-balancing robot comprises:

[0103] Generating the control torque based on the under-actuated dynamics constraint and the ground reaction force constraint of the self-balancing robot.

[0104] Directly incorporating the inherent physical constraints (under-actuated characteristics, ground friction, etc.) of the robot into the calculation of the control torque ensures that each control instruction generated is physically achievable. Avoiding invalid instructions (e.g., requiring the robot to instantaneously translate sideways) that may be issued by traditional simple controllers that exceed the physical limits of the robot. Therefore, the effectiveness and safety of the control instructions can be guaranteed from the root, so that the robot can still operate within its physical limits when performing high-speed, large-maneuver tracking tasks, thereby maximizing the performance of the robot while ensuring its balance and stability.

[0105] In summary, the tracking method for the self-balancing robot provided by the embodiments of the present application first acquires the relative pose of the target by detecting the visual marker using a camera, and combines the pose of the robot itself to obtain the complete motion state of the target in the global coordinate system, including position, velocity and acceleration, through coordinate transformation and differential calculation. A data processing link is constructed from raw visual information to complete physical state, which does not depend on external communication, ensuring that the system can acquire the basic data required for subsequent prediction in any environment. On this basis, in order to improve the data quality, an exponential weighted filter is introduced to smooth the motion state, effectively eliminating sensor noise and calculation errors, and providing a more reliable and smooth data source for subsequent prediction. This is the prerequisite for achieving high-quality tracking. Then, using the smoothed motion state, a constant acceleration prediction model is applied for extrapolation to generate a high-density and continuous smooth target trajectory at a second frequency much higher than the data acquisition frequency. This bridges the gap between low-frequency sensing and high-frequency control, fundamentally solving the problem of control command jumping caused by mismatched data frequencies. In addition, in order to further enhance the robustness in practical application, safety strategies are added at both ends of the process. At the front end, by starting the judgment mechanism, it is ensured that the tracking task is started only after the target is confirmed to be a valid dynamic object, avoiding misjudgment and instability in the initial stage. At the back end, through the fault-tolerant switching mechanism, the system can safely switch to the backup mode when facing sudden conditions such as target loss, preventing loss of control due to sensing interruption and greatly improving reliability. Finally, the target trajectory is input into a controller that fully considers the physical constraints of the robot. This ensures that each motor control torque generated ultimately not only accurately drives the robot to move along the trajectory, but also is completely within the physical capabilities of the robot.

[0106] By way of example, the method of the above embodiments will be specifically described below with a specific example.

[0107] On the basis of the method of the above embodiments, a tracking system for a two-wheeled vehicle is provided, taking a two-wheeled vehicle as an example.

[0108] With the rapid development of robot technology and unmanned driving technology, two-wheeled vehicles have wide application value in fields such as logistics distribution, environmental inspection and entertainment competition due to their simple structure, flexible movement and low energy consumption. In the multi-vehicle coordination scenario, the autonomous tracking technology of two-wheeled vehicles can be used to realize functions such as vehicle formation and target following, for performing monitoring, investigation and transportation tasks. In the prior art, two-wheeled vehicle tracking mainly relies on two methods: formation technology based on communication[1] and visual detection method based on deep learning, but both have significant limitations.

[0109] The platoon-based tracking technology mainly relies on wireless communication to realize information interaction. Through communication, the front vehicle transmits its state information such as pose and speed to the rear vehicle, and the rear vehicle plans a path and controls the motion according to the received data. However, this method has the risk of system instability caused by communication delay and signal interference, and is completely ineffective in closed scenes such as tunnels. In recent years, target detection algorithms such as YOLO and SSD based on deep learning have performed well in visual tracking and can be used to identify and locate the front vehicle. However, this method has the problems of high computational complexity, difficulty in real-time operation of embedded devices, sensitivity to light and object appearance changes, insufficient detection stability, easy loss of target leading to the fall of the double-wheeled vehicle, and additional processing required to obtain accurate pose and motion state, increasing the algorithm burden.

[0110] A more stable solution is to use a camera to detect the marker of the front vehicle, estimate the relative pose of the front vehicle through image processing algorithm, and realize tracking combined with control algorithm. However, due to its structural characteristics, the double-wheeled vehicle needs to maintain its balance while tracking. At the same time, visual detection based tracking cannot meet the real-time requirements of high-frequency control of the double-wheeled vehicle. To address the above problems, the embodiment provides a double-wheeled vehicle autonomous tracking algorithm based on visual detection and interpolation optimization, which can efficiently and stably estimate the motion state of the front vehicle in a communication-free environment and realize smooth and stable tracking.

[0111] The system workflow of the tracking system for the double-wheeled vehicle provided by the embodiment is shown in Figure 2 The state estimation of the front vehicle is realized by combining visual detection and differential estimation, the real-time trajectory information of the front vehicle is used, the interpolation optimization method is used to bridge the frequency gap, and the distributed trajectory tracking controller is used to execute the control command to realize multi-vehicle tracking motion, ensuring the real-time and stability of tracking. The following processes are included:

[0112] Each double-wheeled vehicle is equipped with an IMU sensor (Inertial Measurement Unit), and an AprilTag marker (such as 36h11 series with a side length of 10 cm) is installed at the tail of each double-wheeled vehicle to ensure that the marker can be clearly identified under different light and viewing angles. A Realsense camera is installed on the rear vehicle and fixed in front of the vehicle body with the height aligned with the AprilTag and the lens facing forward.

[0113] The front vehicle is the tracked object in the tracking system, periodically generates its own trajectory and continuously moves, and its position, speed and acceleration are the detection and reference objects of the rear vehicle. To ensure the maneuverability of the double-wheeled vehicle, torque control is used for driving, and the trajectory information includes the position, speed and acceleration of the center of mass of the vehicle:

[0114] ;

[0115] This formula defines the motion state of the front vehicle (the tracked vehicle) at any time t. It is a column vector that contains the complete motion information of the front vehicle's center of mass and is the target of the rear vehicle's tracking. r(t) represents the complete trajectory information or state vector of the front vehicle at time t; q(t) represents the position of the front vehicle's center of mass at time t; v(t) represents the speed of the front vehicle's center of mass at time t; a(t) represents the acceleration of the front vehicle's center of mass at time t.

[0116] The setting of the vehicle trajectory needs to fully consider the structural characteristics of the two-wheeled vehicle itself to avoid phenomena such as running over, losing control, etc. Therefore, cubic spline interpolation is used to generate the front vehicle's center of mass trajectory in segments to ensure continuity and differentiability in terms of position and speed, as well as continuity in terms of acceleration.

[0117] The camera on the rear vehicle is calibrated in advance to obtain the camera intrinsic parameters and distortion parameters. Zhang Zhengyou's calibration method is used for calibration. The rear camera captures images of the front field at a constant frequency, and then calls the optimized AprilTag detection algorithm to quickly locate the four corner points of the Tag in each grayscale image and solve their edge IDs. Then, using the pre-calibrated camera intrinsic parameters (focal length, principal point, distortion coefficient), the PnP (Perspective-n-Point) algorithm is used to obtain the homogeneous transformation matrix of the Tag coordinate system with respect to the camera coordinate system :

[0118] ;

[0119] This formula represents the homogeneous transformation matrix of the AprilTag coordinate system (T) with respect to the rear camera coordinate system (C). This matrix is obtained by the PnP algorithm combined with RANSAC iterative optimization, which completely describes the three-dimensional spatial position and attitude of the AprilTag in the camera's field of view.

[0120] represents the 4x4 homogeneous transformation matrix from the Tag coordinate system (T) to the camera coordinate system (C).

[0121] is a 3x3 rotation matrix that describes the rotational attitude of the Tag coordinate system with respect to the camera coordinate system.

[0122] is a 3x1 translation vector that describes the three-dimensional position of the Tag coordinate system origin with respect to the camera coordinate system origin.

[0123] Each time the solution is completed, a system timestamp is added And enter the cache queue, if the continuous multi-frame detection fails, trigger the detection failure, by the subsequent logic to determine whether to return to the standby track, to ensure the stability of control.

[0124] The real-time pose of the rear vehicle in the world coordinate system is obtained by fusion of IMU and odometry The static transformation of the camera in the rear vehicle coordinate system is obtained by mechanical installation measurement And the static transformation of the AprilTag relative to the front vehicle Combined with the dynamic transformation of the camera to the AprilTag obtained by the AprilTag detection algorithm Timestamp synchronization is adopted to ensure the consistency of received messages and avoid pose conversion errors. Further, the pose of the front vehicle in the world coordinate system is calculated by constructing a coordinate conversion chain :

[0125] ;

[0126] Subsequently, according to the continuous sampling points of the front vehicle at time , The velocity of the front vehicle is estimated by position difference:

[0127] ;

[0128] The acceleration of the front vehicle is estimated by velocity difference:

[0129] ;

[0130] The velocity and acceleration of the front vehicle are estimated by backward difference method. Since visual detection can only obtain the position information of the front vehicle, it cannot directly obtain the velocity and acceleration information. Therefore, by using the position and velocity change of the continuous two sampling points, the two quantities are approximately calculated.

[0131] is the velocity of the front vehicle estimated at time . is the acceleration of the front vehicle estimated at time . and are the three-dimensional positions of the front vehicle at the current time and the last sampling time . This position information is extracted from calculated above. is the velocity of the front vehicle estimated at the last sampling time . and are the system timestamps of the current and last sampling points.

[0132] After obtaining the 3D pose, velocity, and acceleration of the leading vehicle based on AprilTag visual detection and differential estimation, the following vehicle does not immediately execute the follow-up control mode like other agents, but first executes the tracking according to the preset and enters the visual observation waiting state. When the following conditions are met at the same time, the tracking is started: AprilTag continuous recognition is successful, and the target pose solution is stable; the line speed of the leading vehicle reaches the non-zero threshold, avoiding false locking or misidentifying the target. When the above conditions are met and the follow-up control mode is entered, the target offset of the following vehicle in the leading vehicle coordinate system is set:

[0133] ;

[0134] The formula defines the target expected offset of the following vehicle relative to the leading vehicle, describing the ideal follow-up position of the following vehicle. Among them, is the expected relative position offset, is the expected longitudinal (forward direction) offset, and the last two values represent the target offset of the following vehicle in the lateral and vertical directions of the leading vehicle coordinate system. For example, [-2, 0, 0] represents that the following vehicle hopes to maintain a position 2 meters behind the leading vehicle, with an expected offset of 0 in the lateral and vertical directions, meaning that the target of the following vehicle is to maintain the same trajectory curve as the leading vehicle and only track longitudinally.

[0135] Combined with the target offset of the following vehicle in the leading vehicle coordinate system, the expected tracking trajectory of the following vehicle can be calculated:

[0136] ;

[0137] The formula calculates the final expected tracking trajectory of the following vehicle. Combined with the real-time state of the leading vehicle and the expected relative offset, it provides a clear motion target that needs to be executed for the controller of the following vehicle.

[0138] Among them, represents the expected tracking trajectory of the following vehicle at time t. , , are the position, velocity, and acceleration that the following vehicle expects to achieve, respectively. , v , a are the real-time position, velocity, and acceleration of the leading vehicle, which are obtained by the preceding visual detection and differential estimation. R represents the pose rotation matrix of the leading vehicle, which is extracted from the above calculated , used to convert the relative offset from the leading vehicle coordinate system to the world coordinate system.

[0139] The vehicle motion is expected to be smooth and the trajectory is expected to be smoothly transitioned during the motion process. Considering the discrete state samples of the visual detection output and the frequency difference between the actual detection and the tracking control system, directly using the above generated expected trajectory may lead to difficulty in real-time tracking. To support the high-frequency control requirement, compensation of the detection frequency and the tracking trajectory is needed. First, the down-sampling rate parameter and the detection thread number of the Apriltag detection algorithm are modified to make the detection frequency of the algorithm reach the maximum detection frequency of the camera. Further, interpolation scheduling is used to achieve the smoothness of the trajectory tracking. According to the detected data, the speed and acceleration estimates are obtained by backward difference. For any time, exponential weighted low-pass filtering is used to smooth the fluctuations:

[0140]

[0141]

[0142] The two formulas are used to smooth the differentially estimated speed and acceleration to eliminate fluctuations caused by measurement noise and discrete sampling.

[0143] wherein, and represent the smoothed speed and acceleration values after filtering, respectively. and represent the original speed and acceleration values estimated by difference at the current time k, respectively. and represent the original speed and acceleration values estimated by difference at the previous time k-1, respectively.

[0144] α represents a smoothing factor, and the value of α is between 0 and 1. The greater α is, the greater the weight of the historical value at the previous time is, and the smoother the filtering result is, but the response will be slower; on the contrary, the smaller α is, the greater the weight of the current measurement value is, and the faster the response is but the smoothing effect is poor.

[0145] For any control time, if the visual measurement is not updated or consecutive frames are lost, the constant acceleration model is used for second-order polynomial extrapolation:

[0146]

[0147]

[0148] The two formulas are the key to bridge the gap between low-frequency detection and high-frequency control in the embodiments of the application. When the high-frequency controller needs instructions, but the new visual detection result has not yet arrived, the second-order polynomial based on the constant acceleration kinematics model is used for extrapolation prediction. Wherein, and represent the predicted target position and speed of the rear vehicle at any control time t, respectively.​​​​ 、 、 represent the latest position, velocity and acceleration at the last visual detection update. represent the time difference between the current control time t and the last visual update time , i.e. The target position, velocity and acceleration of the follower are generated in real time for the controller to track smoothly and efficiently, ensuring the system trajectory input to be continuous, derivable and the control response to be smooth.

[0149] Based on the QP, a multi-task control framework for the two-wheeled vehicle is established, and the target function is set to include the spatial trajectory tracking in the task space, complete constraint tracking and non-complete constraint tracking. The constraints include the under-actuated dynamics equation (dynamic constraint, obtained according to the first Lagrange equation), ground reaction force constraint (single-sided normal constraint, tangential friction cone constraint) and joint torque / acceleration upper and lower limit constraint. The control algorithm is highly versatile, ensuring the balance of the vehicle body under various desired trajectories.

[0150] In summary, the tracking system for the two-wheeled vehicle provided in the embodiment realizes real-time relative positioning under the condition of no communication by deploying an Apriltag label at the rear of the two-wheeled vehicle and combining real-time detection with coordinate transformation chain calculation by the rear vehicle camera, so that the follower can accurately obtain the pose of the leader without relying on a wireless link. The difference estimation based on discrete detection data and the polynomial interpolation prediction bridge the time delay gap between low-frequency detection and high-frequency control, thereby avoiding tracking instability caused by low-frequency detection. In the system startup phase and tracking process, preset path driving and detection loss fault tolerance switching strategies are introduced respectively to ensure the balance and stability of the two-wheeled vehicle in the initial and abnormal states. Finally, combined with high-speed WBC closed-loop control, joint torque is calculated and output to implement driving, realizing accurate following of the position, velocity and acceleration of the leader by the follower, which not only reduces the computing burden of the host vehicle, but also has high robustness, high real-time performance and low communication overhead.

[0151] In the embodiment, a tracking method device for a self-balancing robot is also provided, which is used to implement the above-mentioned embodiments and preferred embodiments, and will not be described again. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware, or a combination of software and hardware is also possible and contemplated.

[0152] The embodiment provides a tracking method device for a self-balancing robot, as shown in Figure 3 The device is executed by a self-balancing robot as a follower, and the device is used to track a moving target as a host vehicle. The device comprises:

[0153] The acquisition module 301 is used to periodically acquire the pose data of the moving target at a first frequency to obtain the temporal pose data of the moving target.

[0154] The status confirmation module 302 is used to determine the motion state of the moving target based on the temporal pose data; the motion state includes position, velocity and acceleration;

[0155] The trajectory generation module 303 is used to extrapolate the motion state and generate the target trajectory of the self-balancing robot at a second frequency; the second frequency is higher than the first frequency.

[0156] The control module 304 is used to determine the control torque of the self-balancing robot based on the target trajectory, so that the self-balancing robot can maintain balance when tracking the target trajectory.

[0157] Further functional descriptions of the above modules and units are the same as those in the corresponding embodiments described above, and will not be repeated here.

[0158] The tracking method apparatus for a self-balancing robot in this embodiment is presented in the form of a functional unit. Here, a unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that execute one or more software or fixed programs, and / or other devices that can provide the above functions.

[0159] This application also provides a computer device having the above-described features. Figure 3 The illustrated tracking method apparatus for a self-balancing robot.

[0160] Please see Figure 4 , Figure 4 This is a schematic diagram of the structure of a computer device provided in an optional embodiment of this application, such as... Figure 4 As shown, the computer device includes one or more processors 10, memory 20, and interfaces for connecting the components, including high-speed interfaces and low-speed interfaces. The components communicate with each other via different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the computer device, including instructions stored in or on memory to display graphical information of a GUI on external input / output devices (such as display devices coupled to the interfaces). In some alternative implementations, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple computer devices can be connected, each providing some of the necessary operations (e.g., as a server array, a group of blade servers, or a multiprocessor system). Figure 4 Take a processor 10 as an example.

[0161] The processor 10 can be a central processing unit, a network processing unit, or a combination thereof. The processor 10 can further include a hardware chip. The hardware chip can be an application specific integrated circuit, a programmable logic device, or a combination thereof. The programmable logic device can be a complex programmable logic device, a field programmable logic device, a general array logic, or any combination thereof.

[0162] The memory 20 stores instructions executable by the at least one processor 10 to cause the at least one processor 10 to perform the methods illustrated in the above embodiments.

[0163] The memory 20 can include a program storage area and a data storage area. The program storage area can store an operating system, application programs required by at least one function, and the like. The data storage area can store data created according to the use of the computer device, and the like. In addition, the memory 20 can include a high-speed random access memory, and can further include a non-transitory memory such as at least one of a magnetic disk storage device, a flash memory device, or other non-transitory solid state memory device. In some alternative embodiments, the memory 20 can optionally include a memory disposed remotely from the processor 10, and these remote memories can be connected to the computer device through a network. Examples of the network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0164] The memory 20 can include a volatile memory such as a random access memory, and can also include a non-volatile memory such as a flash memory, a hard disk, or a solid state disk. The memory 20 can further include a combination of the above-mentioned types of memories.

[0165] The computer device further includes a communication interface 30 for communication of the computer device with other devices or communication networks.

[0166] The embodiments of the present application further provide a computer readable storage medium, and the method according to the embodiments of the present application can be implemented in hardware, firmware, or recorded in a storage medium, or be implemented as computer code originally stored in a remote storage medium or a non-transitory machine readable storage medium and downloaded through a network and stored in a local storage medium, so that the method described herein can be processed by such software on a storage medium using a general purpose computer, a special purpose processor, or programmable or special hardware. The storage medium can be a magnetic disk, an optical disk, a read-only memory, a random access memory, a flash memory, a hard disk, or a solid state disk, etc. Further, the storage medium can also include a combination of the above-mentioned types of memories. It can be understood that the computer, the processor, the microprocessor controller, or the programmable hardware includes a storage component that can store or receive software or computer code, when the software or computer code is accessed and executed by the computer, the processor, or the hardware, the method shown in the above embodiments is implemented.

[0167] Part of the present application can be applied as a computer program product, for example, computer program instructions, when executed by a computer, through the operation of the computer, the method and / or technical solutions according to the present application can be invoked or provided. Those skilled in the art should understand that the form of computer program instructions in a computer readable medium includes but is not limited to source files, executable files, installation package files, etc. Correspondingly, the way of executing computer program instructions by a computer includes but is not limited to: the computer directly executes the instructions, or the computer executes the corresponding compiled program after compiling the instructions, or the computer reads and executes the instructions, or the computer reads and installs the instructions and then executes the corresponding installed program. Here, the computer readable medium can be any available computer readable storage medium or communication medium accessible to the computer.

[0168] Although the embodiments of the present application are described in conjunction with the accompanying drawings, various modifications and changes can be made by those skilled in the art without departing from the spirit and scope of the present application, and such modifications and changes fall within the scope defined by the appended claims.

Claims

1. A tracking method for a self-balancing robot, characterized in that, The method is performed by a self-balancing robot acting as a slave vehicle, the method being used to track a moving target acting as a master vehicle, the method comprising: The pose data of the moving target is acquired periodically at a first frequency to obtain the temporal pose data of the moving target; The motion state of the moving target is determined based on the temporal pose data; the motion state includes position, velocity, and acceleration. The motion state is extrapolated to generate the target trajectory of the self-balancing robot at a second frequency; the second frequency is higher than the first frequency. Based on the target trajectory, the control torque of the self-balancing robot is determined so that the self-balancing robot maintains balance when tracking the target trajectory; Extrapolating the motion state includes: The motion state is input into a preset constant acceleration prediction model for extrapolation to generate a target trajectory; wherein the target trajectory consists of a series of target points, and the time sampling frequency of the target points is the second frequency; The constant acceleration prediction model is extrapolated using a second-order polynomial, and its expression is: ； ； In the formula, and Let represent the predicted position and velocity of the following vehicle at any control time t; , , These represent the latest position, velocity, and acceleration at the time of the last visual detection update; Represents the current control time t and the time of the last visual update. The time difference between them, i.e. .

2. The method according to claim 1, characterized in that, Acquire the pose data of the moving target, including: The camera mounted on the self-balancing robot detects visual markers installed on the moving target, calculates the pose of the visual markers relative to the camera, and obtains the pose data of the moving target.

3. The method according to claim 2, characterized in that, Determining the motion state of the moving target includes: The temporal pose data is used to determine the relative pose of the moving target with respect to the self-balancing robot. Based on the relative pose and the self-balancing robot's pose in the world coordinate system, the temporal position data of the moving target in the world coordinate system is obtained through coordinate system transformation. Based on the time-series location data, the velocity and acceleration of the moving target are calculated by backward difference to determine the motion state of the moving target.

4. The method according to claim 3, characterized in that, Before extrapolating the motion state, the method further includes: The motion state is smoothed by exponential weighted filtering.

5. The method according to any one of claims 1 to 4, characterized in that, The step of periodically acquiring the pose data of the moving target at a first frequency further includes: When the moving target is successfully identified continuously and the linear velocity of the moving target reaches a preset non-zero threshold, the pose data of the moving target is acquired periodically at a first frequency.

6. The method according to claim 5, characterized in that, The method further includes: When the pose data of the moving target is lost continuously for a preset number of frames, or when the error between the predicted target trajectory and the actual control state exceeds a preset threshold, the self-balancing robot switches to a preset backup trajectory control mode.

7. The method according to claim 6, characterized in that, The determination of the control torque for the self-balancing robot includes: Based on the underactuated dynamic constraints and ground reaction force constraints of the self-balancing robot, a control torque is generated.

8. A tracking device for a self-balancing robot, characterized in that, The device is executed by a self-balancing robot acting as a slave vehicle, and is used to track a moving target acting as a master vehicle. The device includes: The acquisition module is used to periodically acquire the pose data of the moving target at a first frequency to obtain the temporal pose data of the moving target. The status confirmation module is used to determine the motion state of the moving target based on the temporal pose data; the motion state includes position, velocity, and acceleration. A trajectory generation module is used to extrapolate the motion state and generate a target trajectory for the self-balancing robot at a second frequency; the second frequency is higher than the first frequency. The control module is used to determine the control torque of the self-balancing robot based on the target trajectory, so that the self-balancing robot maintains balance when tracking the target trajectory; The trajectory generation module is used for: The motion state is input into a preset constant acceleration prediction model for extrapolation to generate a target trajectory; wherein the target trajectory consists of a series of target points, and the time sampling frequency of the target points is the second frequency; The constant acceleration prediction model is extrapolated using a second-order polynomial, and its expression is: ； ； In the formula, and Let represent the predicted position and velocity of the following vehicle at any control time t; , , These represent the latest position, velocity, and acceleration at the time of the last visual detection update; Represents the current control time t and the time of the last visual update. The time difference between them, i.e. .

9. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor, configured to execute the computer program to implement the steps of the tracking method for a self-balancing robot 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 a computer program, wherein the computer program, when executed by a processor, implements the steps of the tracking method for a self-balancing robot as described in any one of claims 1 to 7.

Images