A method for controlling the surface motion of a spherical robot based on adaptive variable forward distance.

Abstract

This invention discloses a method for controlling the surface motion of a spherical robot based on adaptive variable forward distance, comprising the following steps: calibrating the zero position of the heading angle and starting the script to receive IMU data; converting the heading angle to a northeast-northeast coordinate system; reading the pre-set desired path of the spherical robot; obtaining the deviation between the current heading angle and the desired heading angle of the spherical robot using an adaptive variable forward distance algorithm based on the current state measurement value and the desired path; calculating the desired angular velocity based on the deviation between the current heading angle and the desired heading angle; and allocating the rotational speed of the left and right propellers by the thrust distribution unit of the spherical robot according to the desired angular velocity, so that the heading angle and path tracking error converge until the path is completed. This scheme has a simpler design architecture and does not rely on a model, thus exhibiting good robustness to various curve equations and different hulls, and achieving a very smooth approximation effect when approaching the desired path.

Description

Technical Field

[0001] This invention relates to the field of spherical robot control, and in particular to a method for controlling the surface motion of a spherical robot based on adaptive variable forward distance. Background Technology

[0002] Compared to traditional ship structures, the spherical structure of amphibious spherical robots results in greater overshoot during turning in water, increasing the difficulty of controlling the heading angle. Overshoot is also more likely to occur when the desired path has significant bends, leading to larger path tracking errors at these points. Conventional guidance laws are less effective for tracking curved paths. Summary of the Invention

[0003] This invention primarily addresses the technical problem of poor guidance performance for curved path tracking in existing technologies. It provides an adaptive variable forward distance-based water surface motion control method for spherical robots that exhibits good robustness to various curved shapes and allows for a very smooth approximation effect when the spherical robot approaches the desired path.

[0004] The present invention addresses the aforementioned technical problems primarily through the following technical solution: a method for controlling the surface motion of a spherical robot based on adaptive variable forward distance, comprising the following steps:

[0005] S1. Calibrate the zero point of the heading angle and start the script to receive IMU (Inertial Measurement Unit) data. The heading angle based on this method is calculated from due north. According to the northeast coordinate system and the right-hand rule, clockwise is the positive direction of the heading angle. However, the initial zero point of the heading angle measured by the IMU is the orientation of the ball at the moment the IMU data receiving script is started. Therefore, it is necessary to calibrate the zero point of the heading angle before starting the corresponding script.

[0006] S2. Perform coordinate transformation on the heading angle fed back by the IMU to the Northeast Earth coordinate system; the IMU heading angle is based on the Northeast Sky coordinate system, so coordinate transformation is required.

[0007] S3. Read the desired path of the spherical robot that has been set up. The desired path is represented by two pre-given parametric equations x(t) and y(t), and is a curved path.

[0008] S4. Receive GPS data from IMU and after starting RTK (Real Time Kinematic) as state measurement values. Based on the current state measurement values ​​and the desired path, use the adaptive variable forward distance algorithm to obtain the deviation between the current heading angle and the desired heading angle of the spherical robot.

[0009] S5. Calculate the desired angular velocity based on the deviation between the current heading angle and the desired heading angle of the spherical robot;

[0010] S6. The thrust distribution unit of the spherical robot distributes the rotational speed of the left and right propellers according to the desired angular velocity, so that the heading angle and path tracking error converge.

[0011] S7. Determine whether the spherical robot has completed the tracking path. If it has, the heading control ends. If it has not completed the tracking path, repeat steps S2 to S6 until the path is completed.

[0012] Preferably, in step S4, the step of obtaining the deviation between the current heading angle and the desired heading angle of the spherical robot using the adaptive variable forward distance algorithm specifically involves:

[0013] S401. Obtain the minimum distance between the spherical robot and the desired path, the minimum distance point, and t1; the minimum distance refers to the minimum distance between the spherical robot and the desired path, the minimum distance point is the point (x(t1), y(t1)) on the desired path that is closest to the spherical robot, the minimum distance refers to the distance between the minimum distance point and the spherical robot, and t1 refers to the parameter t value corresponding to the minimum distance point when expressed by two parametric equations;

[0014] S402. Calculate the angle e between the current heading angle and the tangent vector of the desired path at position t1. psi ;

[0015] S403. When the minimum distance is greater than the distance traveled within 10 seconds at the current speed, the forward distance is set to 0.

[0016] When the minimum distance is less than the distance between 10 times the speed value, the forward distance val is determined by the following formula:

[0017]

[0018] In the formula, A, B, and C are empirical parameters; typical values ​​are A = 7, B = 0.1, and C = 40.

[0019] S404. Convert the forward distance into the change of parameter t, dt. The arc length corresponding to dt is d. Calculate the value of parameter t2 corresponding to arc length d; that is, t2 = t1 + dt. Here, the parameters refer to the parameters of the two parametric equations x(t) and y(t).

[0020] S405. Calculate the angle between the vector connecting the current position of the spherical robot to the forward distance point and the y-axis, which is the desired heading angle. Subtract the vector to obtain the deviation between the current heading angle and the desired heading angle.

[0021] That is, when the spherical robot on the water surface is close to the curve, if epsi A larger value indicates that the spherical robot is more likely to rush towards the curve at an angle tending towards 90 degrees (or even greater). In this case, the forward distance should be increased to allow the spherical robot to gradually approach the desired path at a smoother angle. If e psi A smaller forward distance indicates that the spherical robot's orientation is already quite close to the tangent of the path, at which point the forward distance can be appropriately reduced. Reducing the forward distance has two advantages: firstly, when the spherical robot has not yet officially reached the desired path, it can approach the desired path at a higher approximation speed; secondly, when the spherical robot has already reached the desired path, a smaller forward distance allows it to better handle situations with large curvatures. Therefore, e psi It is roughly proportional to the forward distance.

[0022] Preferably, in step S403, when the minimum distance is less than the distance between twice the speed value, after determining the forward distance, the forward distance is corrected using the S-plane control algorithm:

[0023]

[0024] z is the offset of the forward distance. z is added to the original forward distance to complete the correction. k1 and k2 are parameters, and g represents the normalized value of the deviation between the minimum distance and the expected minimum distance. The normalized value of the rate of change of g is obtained by dividing the deviation by the sampling time interval. Typical values ​​are k1 = 4, k2 = 0.8, and the sampling time interval is 0.01 seconds.

[0025] In certain special cases (such as complex curves with curvature that constantly changes with position, like the Archimedean spiral), after a spherical robot reaches the desired path, it exhibits sinusoidal oscillations around that path. The average lateral deviation is typically no more than 1 meter, but this significantly reduces heading efficiency. This is because the curvature changes rapidly, and the small forward distance of the spherical robot still results in a non-negligible overshoot. Therefore, it is necessary to further optimize the forward distance for cases where the minimum distance min_dist is very small. Thus, an S-surface control algorithm is used to add an extra offset to dt.

[0026] The forward distance is defined as the arc length that increases along the curve parameter t, starting from the point of minimum distance. Selecting the correct forward distance is crucial and challenging. On one hand, if the forward distance is too small, the spherical robot is likely to overshoot when approaching the desired path; on the other hand, if the forward distance is too large, the spherical robot's convergence to the desired path may be too slow. This proposed solution avoids overshoot while also achieving a good convergence speed.

[0027] Preferably, in step S401, the minimum distance is obtained by the following method:

[0028] Within a specified finite interval, first try to find the minimum value using parabolic interpolation. If that fails, then use the golden section method to find the minimum distance.

[0029] In practical applications, the process of finding the minimum distance is encapsulated as a code function, and the finite interval is specified when the code function is called. By calling the code function, the specified curve parameter t interval and the function to be found as the minimum value are input, and the minimum distance, the minimum distance point and the corresponding parameter value t1 are obtained directly.

[0030] The distance function is:

[0031]

[0032] (x1, y1) represents the coordinates of the spherical robot.

[0033] Preferably, in step S5, the desired angular velocity is obtained through PID control.

[0034] When using PID control to track a robot's actual angular velocity to a desired angular velocity, the controller calculates the difference between the desired and actual angular velocities and adjusts the control input accordingly. A PID controller consists of three parts: proportional (P), integral (I), and derivative (D). The proportional (P) part is a simple multiple of the error. The proportional gain (Kp) determines the strength of the response. If the error is large, the proportional term increases the control input to reduce the error more quickly. However, using only proportional control often leads to overshoot. The integral (I) part is the cumulative sum of the error over time. In angular velocity control, the integral gain (Ki) helps eliminate steady-state errors caused by environmental disturbances. The integral term provides additional control input when the robot's angular velocity approaches the target but fails to reach the desired value due to disturbances (potentially persistent disturbances). The derivative (D) part is the rate of change of the error. The derivative gain (Kd) helps predict the system's behavior and reduces overshoot and oscillations. When the actual angular velocity rapidly approaches its target angular velocity, the derivative term reduces the control input, making it approach the target more smoothly.

[0035] In addition, LQR, sliding membrane control, fuzzy control, S-surface control and other methods can be used to obtain the desired angular velocity.

[0036] When performing calculations, the spherical robot is treated as a point, typically either the center point or the centroid. The heading angle is the angle between the heading vector and true north (the y-axis).

[0037] This invention, for desired path curves that can be represented by parametric equations, such as x(t) and y(t), and assuming that the current position coordinates of the spherical robot on the water surface have been obtained relatively accurately via differential GPS, can calculate the minimum distance of the robot from the curve (i.e., the desired path), the point on the curve corresponding to the minimum distance (referred to as the minimum distance point), and its corresponding parameter t value. When the robot is far from the curve, the robot's heading angle is adjusted to face the minimum distance point, and the robot moves towards that point. When the robot is at a medium distance from the shortest distance to the curve, the robot's desired heading angle is adjusted to face a certain distance forward from the minimum distance point (referred to as the forward distance). The purpose of this is to allow the spherical robot to smoothly and gradually approach the desired path, thereby avoiding inefficient situations such as braking and re-turning midway or overshoot. When the robot's distance from the shortest distance to the curve is very small, an offset of the forward distance is applied to the calculated forward distance. The goal is to ensure that the minimum distance remains at 0 under the traction of this forward distance, thereby achieving the effect of curve path tracking.

[0038] This scheme does not use the desired circle radius method when designing the desired heading angle. Instead, it introduces the concept of forward distance to achieve curved path tracking. This method is simpler in design architecture than existing curved path tracking methods (such as virtual ship guidance and model-based backstepping methods), and because it is model-independent, it exhibits good robustness to various curve equations and ship hulls. By using a nonlinear control algorithm, the S-surface controller, to further optimize the forward distance when the spherical robot approaches the desired path, the robot achieves a very smooth approach. This scheme can be easily applied to mobile robots with non-spherical structures such as ships and aircraft, which is another significant advantage. Attached Figure Description

[0039] Figure 1 This is a schematic diagram of one structure of the present invention. Detailed Implementation

[0040] The technical solution of the present invention will be further described in detail below through embodiments and in conjunction with the accompanying drawings.

[0041] Example: This example illustrates a method for controlling the surface motion of a spherical robot based on adaptive variable forward distance, such as... Figure 1 As shown, it includes the following steps:

[0042] S1. Calibrate the zero point of the heading angle and start the script to receive IMU (Inertial Measurement Unit) data. The heading angle based on this method is calculated from due north. According to the northeast coordinate system and the right-hand rule, clockwise is the positive direction of the heading angle. However, the initial zero point of the heading angle measured by the IMU is the orientation of the ball at the moment the IMU data receiving script is started. Therefore, it is necessary to calibrate the zero point of the heading angle before starting the corresponding script.

[0043] S2. Perform coordinate transformation on the heading angle fed back by the IMU to the Northeast Earth coordinate system; the IMU heading angle is based on the Northeast Sky coordinate system, so coordinate transformation is required.

[0044] S3. Read the desired path of the spherical robot that has been set up. The desired path is represented by two pre-given parametric equations x(t) and y(t), and is a curved path.

[0045] S4. Receive GPS data from IMU and after starting RTK (Real Time Kinematic) as state measurement values. Based on the current state measurement values ​​and the desired path, use the adaptive variable forward distance algorithm to obtain the deviation between the current heading angle and the desired heading angle of the spherical robot.

[0046] S5. Calculate the desired angular velocity based on the deviation between the current heading angle and the desired heading angle of the spherical robot;

[0047] S6. The thrust distribution unit of the spherical robot distributes the rotational speed of the left and right propellers according to the desired angular velocity, so that the heading angle and path tracking error converge.

[0048] S7. Determine whether the spherical robot has completed the tracking path. If it has, the heading control ends. If it has not completed the tracking path, repeat steps S2 to S6 until the path is completed.

[0049] In step S4, the specific steps for obtaining the deviation between the current heading angle and the desired heading angle of the spherical robot using the adaptive variable forward distance algorithm are as follows:

[0050] S401. Obtain the minimum distance between the spherical robot and the desired path, the minimum distance point, and t1; the minimum distance refers to the minimum distance between the spherical robot and the desired path, the minimum distance point is the point (x(t1), y(t1)) on the desired path that is closest to the spherical robot, the minimum distance refers to the distance between the minimum distance point and the spherical robot, and t1 refers to the parameter t value corresponding to the minimum distance point when expressed by two parametric equations;

[0051] S402. Calculate the angle e between the current heading angle and the tangent vector of the desired path at position t1. psi ;

[0052] S403. When the minimum distance is greater than the distance traveled within 10 seconds at the current speed, the forward distance is set to 0.

[0053] When the minimum distance is less than the distance between 10 times the speed value, the forward distance val is determined by the following formula:

[0054]

[0055] In the formula, A, B, and C are empirical parameters;

[0056] S404. Convert the forward distance into the change of parameter t, dt. The arc length corresponding to dt is d. Calculate the value of parameter t2 corresponding to arc length d; that is, t2 = t1 + dt. Here, the parameters refer to the parameters of the two parametric equations x(t) and y(t).

[0057] Arc length can be approximated using a numerical method (i.e., integration). When calculating the arc length of a curve, one can divide the curve into many small segments, each approximating a straight line, and then simply add the lengths of these straight line segments together. This method is commonly called numerical integration. This approximation method greatly simplifies the calculation. This patent uses this approximation to calculate the arc length. By incrementally increasing the parameter t and calculating the distance between adjacent points, the arc length on the curve can be estimated. This method is generally effective when the increment of t is small, because within these small intervals, the shape of the curve can be approximated as a straight line.

[0058] S405. Calculate the angle between the vector connecting the current position of the spherical robot to the forward distance point and the y-axis, which is the desired heading angle. Subtract the vector to obtain the deviation between the current heading angle and the desired heading angle.

[0059] Definition of the forward distance point: In the parametric curve equation, the coordinate point obtained by substituting t2 into t is called the forward distance point. t2 corresponds to the coordinate point reached from t1 along the direction of increasing t, traversing the arc length of the forward distance, hence it is called the forward distance point. After obtaining the vector connecting the robot's current position to position t2, the angle between this vector and the y-axis can be obtained using the cosine formula. Then, the angle between this vector and the y-axis is calculated, which is the expected heading angle at the next moment. The method for obtaining this angle is the same as the method for obtaining the angle between the tangent vector of the curve and the y-axis mentioned above.

[0060] That is, when the spherical robot on the water surface is close to the curve, if e psi A larger value indicates that the spherical robot is more likely to rush towards the curve at an angle tending towards 90 degrees (or even greater). In this case, the forward distance should be increased to allow the spherical robot to gradually approach the desired path at a smoother angle. If e psiA smaller forward distance indicates that the spherical robot's orientation is already quite close to the tangent of the path, at which point the forward distance can be appropriately reduced. Reducing the forward distance has two advantages: firstly, when the spherical robot has not yet officially reached the desired path, it can approach the desired path at a higher approximation speed; secondly, when the spherical robot has already reached the desired path, a smaller forward distance allows it to better handle situations with large curvatures. Therefore, e psi It is roughly proportional to the forward distance.

[0061] In step S403, when the minimum distance is less than the distance between twice the speed value, after determining the forward distance, the S-plane control algorithm is used to correct the forward distance:

[0062]

[0063] z is the offset of the forward distance. z is added to the original forward distance to complete the correction. k1 and k2 are parameters, and g represents the normalized value of the deviation between the minimum distance and the expected minimum distance. The normalized value of the rate of change of g is obtained by dividing the deviation by the sampling time interval. Typical values ​​are k1 = 4, k2 = 0.8, and the sampling time interval is 0.01 seconds.

[0064] In certain special cases (such as complex curves with curvature that constantly changes with position, like the Archimedean spiral), after a spherical robot reaches the desired path, it exhibits sinusoidal oscillations around that path. The average lateral deviation is typically no more than 1 meter, but this significantly reduces heading efficiency. This is because the curvature changes rapidly, and the small forward distance of the spherical robot still results in a non-negligible overshoot. Therefore, it is necessary to further optimize the forward distance for cases where the minimum distance min_dist is very small. Thus, an S-surface control algorithm is used to add an extra offset to dt.

[0065] The forward distance is defined as the arc length that increases along the curve parameter t, starting from the point of minimum distance. Selecting the correct forward distance is crucial and challenging. On one hand, if the forward distance is too small, the spherical robot is likely to overshoot when approaching the desired path; on the other hand, if the forward distance is too large, the spherical robot's convergence to the desired path may be too slow. This proposed solution avoids overshoot while also achieving a good convergence speed.

[0066] In step S401, the minimum distance is obtained as follows:

[0067] Within a specified finite interval, first try to find the minimum value using parabolic interpolation. If that fails, then use the golden section method to find the minimum distance.

[0068] In practical applications, the process of finding the minimum distance is encapsulated as a code function, and the finite interval is specified when the code function is called. By calling the code function, the specified curve parameter t interval and the function to be found as the minimum value are input, and the minimum distance, the minimum distance point and the corresponding parameter value t1 are obtained directly.

[0069] The distance function is:

[0070]

[0071] (x1, y1) represents the coordinates of the spherical robot.

[0072] Calculate e psi To calculate the slope of the curve, we first need to find the angle between the tangent direction vector (directing towards the direction of increasing parameter t) at position t1 and the y-axis. The finite difference method is used to determine the tangent slope. This method allows for a relatively quick approximate numerical calculation of the curve's tangent slope. Specifically, the tangent direction vector is defined as a two-dimensional vector (dx / dt, dy / dt) pointing towards the direction of increasing t. The ratio of the y-component (dy / dt) to the x-component (dx / dt) of this vector is the tangent slope. Then, we calculate the angle between this vector and the y-axis. For the y-axis, its direction vector can be considered as (0,1). The angle is calculated using the dot product formula:

[0073]

[0074] Here, A is the tangent direction vector, and B is the y-axis direction vector. Note that the magnitude of vector B is 1, so the formula can be simplified to:

[0075]

[0076] Performing an inverse cosine transform on the cosine value returns the angle in degrees (i.e., the angle between the tangent vector and the y-axis). Subtracting this from the current heading angle of the spherical robot yields the current heading angle psi and the angle e between the tangent vectors of the curve at position t1. psi .

[0077] In step S5, the desired angular velocity is obtained through PID control.

[0078] When using PID control to track a robot's actual angular velocity to a desired angular velocity, the controller calculates the difference between the desired and actual angular velocities and adjusts the control input accordingly. A PID controller consists of three parts: proportional (P), integral (I), and derivative (D). The proportional (P) part is a simple multiple of the error. The proportional gain (Kp) determines the strength of the response. If the error is large, the proportional term increases the control input to reduce the error more quickly. However, using only proportional control often leads to overshoot. The integral (I) part is the cumulative sum of the error over time. In angular velocity control, the integral gain (Ki) helps eliminate steady-state errors caused by environmental disturbances. The integral term provides additional control input when the robot's angular velocity approaches the target but fails to reach the desired value due to disturbances (potentially persistent disturbances). The derivative (D) part is the rate of change of the error. The derivative gain (Kd) helps predict the system's behavior and reduces overshoot and oscillations. When the actual angular velocity rapidly approaches its target angular velocity, the derivative term reduces the control input, making it approach the target more smoothly.

[0079] In addition, LQR, sliding membrane control, fuzzy control, S-surface control and other methods can be used to obtain the desired angular velocity.

[0080] When performing calculations, the spherical robot is treated as a point, typically either the center point or the centroid. The heading angle is the angle between the heading vector and true north (the y-axis).

[0081] The desired angular velocity can also be obtained through the S-plane heading control algorithm, with the specific formula as follows:

[0082]

[0083] In the formula, z represents the desired angular velocity value, K3 and K4 are control parameters, and h is the normalized value of the deviation between the desired heading angle and the current heading angle. This is the normalized value of the deviation rate between the desired heading angle and the current heading angle.

[0084] When the input consists of bias and bias rate (bivariate), the input-output corresponds to a surface derived from the Sigmoid function, abbreviated as S-surface, where K3 and K4 represent the parameters to be adjusted. Theoretically, the S-surface control algorithm can be used to adjust any degree of freedom of a ship. For underactuated spherical robots, the S-surface control algorithm can be used to adjust the heading angle and velocity. As can be seen, the parameters to be adjusted in the S-surface control algorithm are also two, K3 and K4, just like in PD control. K3 represents the proportional term; taking heading angle adjustment as an example, a larger K3 results in a faster heading angle response, but also a larger actual heading angle overshoot. K4 represents the differential term; a larger K4 results in a smoother response and smaller overshoot, but may amplify noise and clutter in the circuit. K3 and K4 need to be determined through field testing, but qualitatively, they should have the same sign. The essence of adjusting K3 and K4 is to make the gradient direction of the control output on the control surface as close as possible to the direction pointing to the equilibrium point (zero point). The inputs in the S-surface control algorithm need to be normalized.

[0085] The control parameters of the S-surface control algorithm are determined by the following formula:

[0086]

[0087]

[0088] if or or

[0089] In the formula, u1(k) and u2(k) represent the K3 and K4 parameters in the S-face controller at time k, respectively; λ is the weighting factor, λ>0; μ is the penalty factor, μ>0; η is the first step length factor, η∈(0,1]; ρ is the second step length factor, ρ∈(0,1]; ε is a sufficiently small positive number; This is an estimate of φ1(k); φ2(k) is an estimate; φ1(k) and φ2(k) are time-varying scalar parameters. for initial value, for The initial values ​​are both set manually; Δu1(k-1) = u1(k-1) - u1(k-2), which is the increment of parameter K3 ΔK3 from time k-2 to time k-1; Δu2(k-1) = u2(k-1) - u2(k-2), which is the increment of parameter K4 ΔK4 from time k-2 to time k-1; y *(k+1) represents the desired heading angle (facing the next moment, so the variable is k+1), y(k) represents the current heading angle; Δy(k) = y(k) - y(k-1), y(k-1) is the heading angle obtained at the previous sampling moment.

[0090] The weighting factor, penalty factor, first step length factor, and second step length factor are all empirical parameters, preset manually. Once set, these parameters do not need to be changed. Furthermore, these factors can be easily determined through coarse tuning. Although they may not be optimal values, they are insensitive to the new nonlinear system and dynamic external disturbances, meaning they do not have a decisive impact on the final result, but merely affect the overall performance. and The magnitude of its effect depends on the tracking of nonlinear systems and adaptation to time-varying environments. and It was accomplished through this process.

[0091] The result is obtained from formula (2). and Then, it was further optimized through formula (3), that is, formula (3) introduced an "algorithm reset" mechanism to enhance the tracking performance of formula (2) for time-varying scalar parameters.

[0092] The first and second conditions in formula (3) both indicate that a reset should be performed when the changes are minimal. This is because when either K1 or K2 changes minimally, the reason could be either that a steady state has been reached or that the prediction information is inaccurate. If it is the second reason and the original parameters are allowed to continue, the prediction error may gradually increase. To rule out the second reason, resetting to the initial value can help the system readjust to possible changes. As for the third condition, it is because, theoretically, if this control scheme can ensure that the control system is always stable, the premise must be that the system is "quasi-linear." The equivalent statement of this assumption is: for any time k and Δu(k) are not equal to 0, and The sign remains unchanged. Therefore, once... or When a change in sign occurs, that is, when the sign is opposite to the initial sign, it is necessary to... and Reset it so that the control scheme can always meet the necessary conditions for design and solution.

[0093] The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which this invention pertains may make various modifications or additions to the described specific embodiments or use similar methods to substitute them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

[0094] Although this paper uses terms such as desired heading angle, desired angular velocity, and S-plane control algorithm frequently, the possibility of using other terms is not excluded. These terms are used merely for the convenience of describing and explaining the essence of this invention; interpreting them as any additional limitation would contradict the spirit of this invention.

Claims

1. A method for controlling the surface motion of a spherical robot based on adaptive variable forward distance, characterized in that, Includes the following steps: S1. Calibrate the heading angle to zero and start the script to receive IMU data; S2. Perform coordinate transformation on the heading angle fed back by the IMU, converting it to the northeast coordinate system; S3. Read the desired path of the spherical robot that has been set up; the desired path is represented by two parametric equations x(t) and y(t); S4. Receive GPS data from IMU and after RTK is started as state measurement values. Based on the current state measurement values ​​and the desired path, use the adaptive variable forward distance algorithm to obtain the deviation between the current heading angle and the desired heading angle of the spherical robot. S5. Calculate the desired angular velocity based on the deviation between the current heading angle and the desired heading angle of the spherical robot; S6. The thrust distribution unit of the spherical robot distributes the rotational speed of the left and right propellers according to the desired angular velocity, so that the heading angle and path tracking error converge. S7. Determine whether the spherical robot has completed the tracking path. If it has, end the heading control. If the tracing path has not been completed, repeat steps S2 to S6 until the path is completed. In step S4, the specific steps for obtaining the deviation between the current heading angle and the desired heading angle of the spherical robot using the adaptive variable forward distance algorithm are as follows: S401. Obtain the minimum distance between the spherical robot and the desired path, the minimum distance point, and t1; the minimum distance refers to the minimum distance between the spherical robot and the desired path, the minimum distance point is the point (x(t1), y(t1)) on the desired path that is closest to the spherical robot, the minimum distance refers to the distance between the minimum distance point and the spherical robot, and t1 refers to the parameter t value corresponding to the minimum distance point when expressed by two parametric equations; S402. Calculate the angle e between the current heading angle and the tangent vector of the desired path at position t1. psi ; S403. When the minimum distance is greater than the distance traveled within 10 seconds at the current speed, the forward distance is set to 0. When the minimum distance is less than the distance between 10 times the speed value, the forward distance val is determined by the following formula: ； In the formula, A, B, and C are empirical parameters; S404. Convert the forward distance into the change in parameter t, dt. The arc length corresponding to dt is d. Calculate the value of parameter t2 corresponding to arc length d. S405. Calculate the angle between the vector connecting the current position of the spherical robot to the forward distance point and the y-axis, which is the desired heading angle. Subtract the vector to obtain the deviation between the current heading angle and the desired heading angle.

2. The method for controlling the surface motion of a spherical robot based on adaptive variable forward distance according to claim 1, characterized in that, In step S403, when the minimum distance is less than the distance between twice the speed value, after determining the forward distance, the S-plane control algorithm is used to correct the forward distance: ； In the formula, z is the offset of the forward distance, which is superimposed on the original forward distance to complete the correction; k1 and k2 are parameters; and g represents the normalized value of the deviation between the minimum distance and the expected minimum distance. The normalized value of the rate of change of g is obtained by quotienting the deviation over the sampling time interval.

3. The method for controlling the surface motion of a spherical robot based on adaptive variable forward distance according to claim 1, characterized in that, In step S401, the minimum distance is obtained as follows: Within a specified finite interval, first try to find the minimum value using parabolic interpolation. If that fails, then use the golden section method to find the minimum distance.

4. The method for controlling the surface motion of a spherical robot based on adaptive variable forward distance according to claim 2, characterized in that, In step S5, the desired angular velocity is obtained through PID control.

Images