Ball float attitude closed-loop control method and system based on global navigation satellite system

Abstract

The application discloses a global navigation satellite system-based floating ball attitude closed-loop control method and system, and particularly relates to the technical field of floating ball attitude control, and is used for solving the problem that the existing linear control method causes attitude out-of-control due to control force reversal and heavy block self-locking when the floating ball swings greatly; the method is achieved by acquiring the floating ball attitude angle and the real-time position of the built-in heavy block, determining the control force reversal interval according to the sign deviation times of the gravity restoring moment and the target correction torque, calculating the included angle between the gravity component vector and the direction of the target correction torque after the determination, constructing a plane with the included angle and the gravity restoring moment, and judging the approaching of the geometric self-locking dead zone according to the trajectory convergence, and generating a correction control instruction to make the built-in heavy block move in the direction of separation first and then execute the attitude correction.

Description

Technical Field

[0001] This invention relates to the field of buoy attitude control technology, and more specifically, to a buoy attitude closed-loop control method and system based on a global navigation satellite system. Background Technology

[0002] Ocean buoy platforms are used to carry observation equipment for long-term operations at sea and need to maintain a defined attitude to meet measurement pointing requirements. Existing buoy attitude control systems often utilize multi-antenna attitude measurement from a global navigation satellite system to obtain real-time attitude angle information, forming a closed-loop feedback. The actuators typically employ built-in weights or eccentric pendulums, using the movement of the weights to generate gravitational eccentric torque to correct the buoy's sway. The controller calculates the target position of the weights based on the attitude deviation and drives the actuators to continuously adjust the roll and pitch.

[0003] This type of closed-loop control method typically assumes that the float operates within a small swing angle range during controller design, approximating the output characteristics of the weight mechanism as a linear relationship. In actual operation, when large waves cause the float to tilt significantly, the gravitational component and geometric configuration of the weight along the guide rail change significantly. The movement command output by the existing linear control law contradicts the actual torque direction that the mechanism can provide at this time, resulting in reversed control force. With further increases in the tilt angle, the weight may fail to reset itself due to the attenuation of the gravitational component, entering a self-locking dead zone at the mechanical travel limit. Although the control system continuously issues correction commands, the attitude remains completely uncontrolled, causing the float to lose stability under extreme swing conditions. This directly limits the reliable operating range of existing gravity-based attitude closed-loop control methods. Summary of the Invention

[0004] To overcome the aforementioned deficiencies of the prior art, the present invention provides a method and system for closed-loop control of a float attitude based on a global navigation satellite system to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: The buoy attitude closed-loop control method based on the Global Navigation Satellite System includes the following steps: S1: Obtain the attitude angle of the buoy obtained by the multi-antenna attitude measurement of the global navigation satellite system, and the real-time position of the built-in weight on the arc-shaped guide rail; S2: Determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and obtain the target correction torque output by the attitude closed-loop controller. When the number of times the signs of the gravity recovery torque and the target correction torque deviate within the set judgment window reaches the frequency threshold, it is determined that the built-in weight is in the control force reversal interval. S3: When it is determined that the control force is in the reverse zone, calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque based on the attitude angle and real-time position. S4: Construct a phase plane by combining the included angle and the gravity recovery torque. Based on the convergence of the trajectory of the current working point of the built-in weight in the phase plane, determine whether the built-in weight approaches the geometric self-locking dead zone. S5: When it is determined that the geometric self-locking dead zone is approaching, a correction control command is generated. The correction control command causes the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then the attitude correction is performed.

[0006] Furthermore, S1 includes: By using multiple global navigation satellite system antennas to receive satellite signals, the attitude angle of the buoy at the current moment in a set coordinate system can be calculated; Meanwhile, the offset of the built-in weight relative to the zero position of the arc-shaped guide rail is obtained by using a displacement sensor linked to the built-in weight. After signal conditioning, the real-time position of the built-in weight on the arc-shaped guide rail is obtained.

[0007] Furthermore, the offset of the built-in weight relative to the zero position of the arc-shaped guide rail is obtained using a displacement sensor linked to the built-in weight. After signal conditioning, the real-time position of the built-in weight on the arc-shaped guide rail is obtained, including: The displacement sensor is a rotary encoder mounted on the end of the arc-shaped guide rail. The rotating shaft of the rotary encoder is coaxially connected to the drive screw of the arc-shaped guide rail via a coupling. Read the pulse count output by the rotary encoder and convert the pulse count into the offset of the built-in weight on the arc-shaped guide rail relative to the zero position of the arc-shaped guide rail; The offset is low-pass filtered to remove high-frequency mechanical vibration noise caused by the swaying of the float, thus obtaining the real-time position of the built-in weight on the arc-shaped guide rail.

[0008] Furthermore, S2 includes: Using the mass of the built-in weight, the radius of curvature of the arc-shaped guide rail, the attitude angle, and the real-time position, the gravitational restoring torque generated by the built-in weight on the float in the current state is calculated through rigid body statics. The target correction torque generated in the current control cycle is directly read from the output of the attitude closed-loop controller. Within the set judgment window, continuously compare the positive and negative signs of the gravity recovery torque and the target correction torque, and accumulate the number of times the signs are different as the number of deviations; When the number of deviations reaches the frequency threshold, the built-in heavy block is determined to be in the control force reversal zone.

[0009] Furthermore, within the defined judgment window, the signs of the gravity restoring torque and the target correction torque are continuously compared, and the number of times the signs differ is recorded as the deviation count, including: Divide the judgment window into multiple sub-windows of equal length; Within each sub-window, a deviation is recorded when the sign of the gravity recovery torque is different from the sign of the target correction torque. The number of divergences in all child windows is summed to obtain the number of divergences within the set judgment window.

[0010] Furthermore, S3 includes: When it is determined that the built-in weight is in the reverse range of the control force, the direction of the gravity force on the built-in weight on the arc-shaped guide rail is determined based on the attitude angle, and the gravity component vector in the tangential direction of the arc-shaped guide rail is determined in combination with the real-time position. Obtain the direction vector of the target correction torque from the attitude closed-loop controller; Calculate the spatial angle between the gravity component vector and the direction vector of the target correction moment, which is used as the angle between the gravity component vector of the built-in weight on the arc-shaped guide rail and the direction of the target correction moment.

[0011] Furthermore, S4 includes: The phase plane is constructed by using the calculated included angle as the first coordinate axis of the phase plane and the determined gravitational restoring torque as the second coordinate axis of the phase plane. The combined value of the angle between the built-in weight and the gravity restoring torque in the current control cycle is mapped to the current operating point in the phase plane; Record the operating points of multiple consecutive control cycles preceding the current operating point in the phase plane to form the operating point trajectory; Analyze the movement trend of the working point trajectory towards the boundary region corresponding to the geometric self-locking dead zone in the phase plane. When the trend meets the preset convergence condition, it is determined that the built-in heavy block approaches the geometric self-locking dead zone.

[0012] Furthermore, the movement trend of the working point trajectory towards the boundary region corresponding to the geometric self-locking dead zone in the phase plane is analyzed. When the trend meets the preset convergence condition, it is determined that the built-in heavy block approaches the geometric self-locking dead zone, including: Calculate the vector direction between adjacent working points in the working point trajectory; When the vector directions between a preset number of consecutive adjacent working points all point to the boundary region corresponding to the geometric self-locking dead zone, and the distance between the adjacent working points and the boundary region corresponding to the geometric self-locking dead zone decreases point by point, it is determined that the preset convergence condition is met, and the built-in heavy block approaches the geometric self-locking dead zone.

[0013] Furthermore, S5 includes: When it is determined that the built-in weight is approaching the geometric self-locking dead zone, the direction of leaving the geometric self-locking dead zone is determined based on the real-time position of the built-in weight on the arc-shaped guide rail and the geometric parameters of the arc-shaped guide rail. Generate the first control command to drive the built-in weight along the arc-shaped guide rail to move towards the safe position range in the direction of breaking away from the geometric self-locking dead zone; After the built-in weight enters the safe position range, a second control command is generated to drive the built-in weight to perform attitude correction.

[0014] On the other hand, the present invention provides a closed-loop control system for the attitude of a float based on a global navigation satellite system, comprising the following modules: The position acquisition module is used to acquire the attitude angle of the buoy obtained by the attitude measurement of the buoy through the multi-antenna of the global navigation satellite system, as well as the real-time position of the built-in weight on the arc-shaped guide rail; The reverse judgment module is used to determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and to obtain the target correction torque output by the attitude closed-loop controller. When the signs of the gravity recovery torque and the target correction torque deviate from each other multiple times within the set judgment window, the built-in weight is determined to be in the control force reverse interval. The angle calculation module is used to calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque when it is determined that the control force is in the reverse zone, based on the attitude angle and real-time position. The approach evaluation module is used to construct a phase plane by combining the included angle and the gravitational restoring torque. Based on the trajectory convergence of the current working point of the built-in weight in the phase plane, it determines whether the built-in weight is approaching the geometric self-locking dead zone. The instruction generation module is used to generate correction control instructions when the geometric self-locking dead zone is approached. The correction control instructions cause the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then perform attitude correction.

[0015] Compared with the prior art, the present invention has the following beneficial effects: 1. By continuously monitoring the sign divergence between the gravity restoring torque and the target correction torque, the reverse interval of the control force is identified. This enables the controller to promptly detect the conflict between the force output direction of the actuator and the correction intention under large swing conditions. This solves the problem of existing linear control laws causing a disconnect between the command and the actual capability of the mechanism due to the assumption of linear force output at small angles. After determining the reverse of the control force, the angle between the gravity component vector and the direction of the target correction torque, together with the gravity restoring torque, is used to construct a phase plane. By analyzing the trajectory convergence of the operating point in the phase plane, it is predicted whether the built-in weight is approaching the geometric self-locking dead zone. This evaluation method does not rely on a fixed tilt angle threshold, but extracts self-locking precursors from the motion trend of the system's operating point itself, making the identification of self-locking risk more proactive and adaptable to operating conditions.

[0016] 2. When the self-locking approach is anticipated, the built-in weight is first driven to move in the direction of escaping the geometric self-locking dead zone to a safe position range before attitude correction is performed. This avoids the problem that the controller continuously outputs invalid commands at the self-locking edge and the built-in weight cannot be reset. It expands the reliable working range of attitude closed-loop control from the normal small swing range to large angle extreme conditions, and enhances the ability of the buoy to maintain a directional attitude continuously in harsh sea conditions. Attached Figure Description

[0017] Figure 1 This is a flowchart of the float attitude closed-loop control method based on a global navigation satellite system according to the present invention; Figure 2 This is a schematic diagram of the structure of the float attitude closed-loop control system based on the global navigation satellite system of the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] Example 1: Figure 1 The present invention provides a closed-loop control method for the attitude of a float based on a global navigation satellite system, which includes the following steps: S1: Obtain the attitude angle of the buoy obtained by the multi-antenna attitude measurement of the global navigation satellite system, and the real-time position of the built-in weight on the arc-shaped guide rail; S2: Determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and obtain the target correction torque output by the attitude closed-loop controller. When the number of times the signs of the gravity recovery torque and the target correction torque deviate within the set judgment window reaches the frequency threshold, it is determined that the built-in weight is in the control force reversal interval. S3: When it is determined that the control force is in the reverse zone, calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque based on the attitude angle and real-time position. S4: Construct a phase plane by combining the included angle and the gravity recovery torque. Based on the convergence of the trajectory of the current working point of the built-in weight in the phase plane, determine whether the built-in weight approaches the geometric self-locking dead zone. S5: When it is determined that the geometric self-locking dead zone is approaching, a correction control command is generated. The correction control command causes the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then the attitude correction is performed.

[0020] S1: Obtain the attitude angle of the buoy obtained by multi-antenna attitude measurement of the global navigation satellite system, and the real-time position of the built-in weight on the arc-shaped guide rail. The implementation process is as follows: Multiple Global Navigation Satellite System (GNSS) antennas are used to receive satellite signals, and the attitude angles of the buoy at the current moment are calculated in a set coordinate system. Multiple GNSS antennas are fixedly mounted on the upper surface of the buoy's shell, and the relative spatial positions of the antennas constitute a known baseline vector. When the GNSS antennas simultaneously receive carrier phase observations from multiple navigation satellites, carrier phase differential techniques are used to solve the single-difference and double-difference carrier phase equations between the antennas, obtaining the direction of the baseline vector in space. This direction is then converted into Euler angles in the set coordinate system. The set coordinate system is the local horizontal coordinate system, with its origin located at the geometric center of the buoy. The X-axis points due east, the Y-axis points due north, and the Z-axis points vertically upward. The buoy's attitude angles at the current moment include roll and pitch angles. The roll angle is the angle of rotation of the buoy around the X-axis of the set coordinate system, and the pitch angle is the angle of rotation of the buoy around the Y-axis of the set coordinate system. The calculation process is completed in real time in the attitude calculation board built into the float. The attitude calculation board outputs a set of updated attitude angle data in each control cycle. The duration of the control cycle is set according to the spectral characteristics of the float's oscillation motion. For example, the control cycle is a fixed value in the range of 20 milliseconds to 100 milliseconds.

[0021] Simultaneously, a displacement sensor linked to the built-in weight is used to obtain the offset of the built-in weight relative to the zero position of the arc-shaped guide rail. After signal conditioning, the real-time position of the built-in weight on the arc-shaped guide rail is obtained. The displacement sensor is a rotary encoder installed at the end of the arc-shaped guide rail, and the shaft of the rotary encoder is coaxially connected to the drive screw of the arc-shaped guide rail via a coupling. The arc-shaped guide rail is fixed inside the float and extends in an arc shape, with the center of the arc-shaped guide rail coinciding with the geometric center of the float. The built-in weight is fitted onto the arc-shaped guide rail and can slide along it. The drive screw is arranged along the tangent of the arc-shaped guide rail, with one end connected to the output shaft of the drive motor and the other end supported at the end of the arc-shaped guide rail by a bearing seat. The built-in weight is engaged with the drive screw via a screw nut. When the drive screw rotates, the screw nut drives the built-in weight to move along the arc-shaped guide rail. The rotary encoder is installed at the end of the arc-shaped guide rail away from the drive motor. The rotating shaft of the rotary encoder is coaxially connected to the end of the drive screw via a coupling, so that the rotating shaft of the rotary encoder rotates synchronously when the drive screw rotates.

[0022] The pulse count output from the rotary encoder is read and converted into the offset of the built-in weight relative to the zero position of the arc-shaped guide rail. The rotary encoder is an incremental rotary encoder, which outputs a fixed number of pulse signals per revolution. The number of pulse signals is proportional to the rotation angle of the drive screw. The zero position of the arc-shaped guide rail is defined as the position of the built-in weight when it is at the lowest point of the arc-shaped guide rail, corresponding to the default position of the built-in weight under static balance of the float. When the drive screw rotates, the rotary encoder outputs A-phase pulses and B-phase pulses. The phase difference between the A-phase and B-phase pulses indicates the direction of rotation. The pulse counting circuit performs a fourfold frequency count on the rising and falling edges of the A-phase and B-phase pulses to obtain the cumulative pulse count. Multiplying the cumulative pulse count by the pulse equivalent coefficient yields the offset of the built-in weight relative to the zero position of the arc-shaped guide rail. The pulse equivalent coefficient is equal to the drive screw lead divided by four times the number of pulses per revolution of the incremental rotary encoder. The sign of the offset indicates the direction in which the built-in weight deviates from the zero position of the arc-shaped guide rail. For example, a positive offset indicates that the built-in weight deviates from the zero position of the arc-shaped guide rail in a clockwise direction, while a negative offset indicates that the built-in weight deviates from the zero position of the arc-shaped guide rail in a counterclockwise direction.

[0023] The offset is low-pass filtered to remove high-frequency mechanical vibration noise caused by the buoy's swaying motion, thus obtaining the real-time position of the built-in weight on the curved guide rail. The low-pass filtering uses a second-order Butterworth low-pass filter to filter the offset in real time. The cutoff frequency of the second-order Butterworth low-pass filter is set according to the dominant frequency range of the buoy's swaying motion. The dominant frequency of the buoy's swaying motion under wave action is below 1 Hz, while the frequency components of the high-frequency mechanical vibration noise are concentrated above 10 Hz. Therefore, the cutoff frequency of the second-order Butterworth low-pass filter is set to a fixed value within the range of 2 Hz to 5 Hz, for example, a cutoff frequency of 4 Hz. The filtering process is implemented by running a digital filter program on the attitude calculation board or a separate microcontroller. Each control cycle reads the latest calculated offset, and the filtered output value for the current control cycle is calculated using the difference equation of the second-order Butterworth low-pass filter. The filtered output value is the real-time position of the built-in weight on the curved guide rail. The coefficients of the difference equation for the second-order Butterworth low-pass filter are pre-calculated based on the cutoff frequency and the duration of the control cycle and stored in the program memory. The filtering process does not introduce additional phase lag that would reduce the control bandwidth. After the above steps, S1 obtains the current attitude angle of the float and the real-time position of the built-in weight on the arc-shaped guide rail, and then passes the attitude angle and real-time position to subsequent steps.

[0024] S2: Determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and obtain the target correction torque output by the attitude closed-loop controller. When the signs of the gravity recovery torque and the target correction torque deviate from each other within the set judgment window, the built-in weight is determined to be in the control force reversal interval. The implementation process is as follows: Using the mass of the built-in weight, the radius of curvature of the curved guide rail, the attitude angle, and the real-time position, the gravitational restoring torque generated by the built-in weight on the float in the current state is calculated through rigid body statics. The mass of the built-in weight is a pre-calibrated known constant stored in the parameter storage area of ​​the attitude calculation board. The radius of curvature of the curved guide rail is the distance from the center of the curved guide rail to any point on the curved guide rail. The radius of curvature of the curved guide rail is measured and recorded as a fixed structural parameter after the float is assembled, and is also stored in the parameter storage area. The attitude angle is the current attitude angle of the float obtained in S1, and the real-time position is the real-time position of the built-in weight on the curved guide rail obtained after low-pass filtering in S1. When calculating the gravitational restoring torque, the current tilt direction and tilt angle of the float relative to the set coordinate system are first determined based on the attitude angle, and the tangential component of the gravity acting on the built-in weight in the tangential direction of the curved guide rail is calculated. The gravity acting on the built-in weight is equal to its mass multiplied by gravitational acceleration, and its direction is always vertically downwards. The tangential direction of the curved guide rail is determined by its real-time position and radius of curvature. Using the center of the curved guide rail as a reference point, the tangential vector at that point is calculated based on the arc length offset angle corresponding to the real-time position. Projecting the gravity vector acting on the built-in weight onto the tangential vector of the curved guide rail yields the tangential component of the gravity acting on the built-in weight along the tangential direction of the curved guide rail. Multiplying this tangential component by the radius of curvature of the curved guide rail yields the gravitational restoring torque exerted by the built-in weight on the float in its current state. The sign of the gravity restoring torque is defined as follows: when the direction of the gravity restoring torque is opposite to the direction of the float's tilt, that is, when the gravity restoring torque tends to pull the float back to its equilibrium position, the sign of the gravity restoring torque is recorded as positive; when the direction of the gravity restoring torque is the same as the direction of the float's tilt, that is, when the gravity restoring torque tends to exacerbate the float's tilt, the sign of the gravity restoring torque is recorded as negative.

[0025] The target correction torque generated in the current control cycle is directly read from the output of the attitude closed-loop controller. The attitude closed-loop controller runs on the attitude calculation board. In each control cycle, it calculates the target correction torque based on the attitude deviation between the attitude angle and the target attitude using a proportional-integral-derivative (PID) control algorithm or other feedback control algorithms. The target correction torque is the theoretical torque value that the attitude closed-loop controller expects to apply to the float to eliminate the attitude deviation. The target correction torque also has a positive and negative sign, defined in the same way as the gravity-restoring torque: a positive sign indicates that the direction of the target correction torque is opposite to the float's tilt direction, and a negative sign indicates that the direction of the target correction torque is the same as the float's tilt direction. The operation of reading the target correction torque is achieved by accessing the memory address of the attitude closed-loop controller's output register in the attitude calculation board. This is done once per control cycle, after the attitude closed-loop controller has completed its calculation for the current control cycle.

[0026] Within the established judgment window, the signs of the gravity restoring torque and the target correction torque are continuously compared, and the number of times the signs differ is recorded as the deviation count. The established judgment window is divided into multiple sub-windows of equal length. The total duration of the established judgment window is determined based on the buoy's swaying period under wave action and the control cycle of the attitude closed-loop controller. The typical swaying period range of the buoy under wave action is estimated based on the buoy's size and the wave statistical characteristics of the operating sea area. For example, when the buoy diameter is between 1 and 2 meters, the typical swaying period is between 2 and 5 seconds. The total duration of the established judgment window is taken as an integer multiple of the buoy's typical swaying period, such as 2 to 3 times the typical swaying period, so that the established judgment window can cover at least one complete swaying period to avoid misjudgments caused by wave periodic changes. The duration of each sub-window is obtained by dividing the total duration of the decision window by the number of sub-windows. The duration of each sub-window is an integer multiple of the control cycle duration of the attitude closed-loop controller. The number of sub-windows is determined based on the total duration and the required time resolution; for example, the number of sub-windows is an integer value within the range of 8 to 16. The decision window is set to advance forward over time in the form of a sliding window. At the end of each control cycle, the decision window is set to move forward by the length of one control cycle.

[0027] Within each sub-window, a deviation is recorded when the sign of the gravity restoring torque differs from that of the target correction torque. Each sub-window records at most one deviation; that is, if at least one instance of a sign difference occurs within a sub-window, it is marked as a deviation window. This design avoids repeated counting due to high-frequency noise within a single sub-window while preserving the temporal distribution characteristics of deviation events. In practice, during each control cycle, the signs of the gravity restoring torque and the target correction torque are compared. If the signs differ, the deviation flag for the sub-window belonging to that control cycle is set. After the sub-window's time interval ends, the deviation flag is read to determine if a deviation has occurred. If a deviation flag exists, the deviation count is incremented, and then the deviation flag is cleared for use by the next sub-window.

[0028] The divergence counts of all child windows are summed to obtain the divergence count within the set judgment window. This summation operation is performed when the set judgment window slides forward one control cycle. When the set judgment window slides forward one control cycle, the oldest child window is removed from the set judgment window, and a new child window is added. The sum of the divergence counts of all child windows within the set judgment window is recalculated and used as the current divergence count within the set judgment window. The divergence count is an integer, ranging from zero to the total number of child windows.

[0029] When the number of deviations reaches the frequency threshold, the built-in weight is determined to be in the control force reversal range. The frequency threshold is set based on the principle that, when the control force direction is normal, the sign of the gravity recovery torque should be consistent with the sign of the target correction torque for the vast majority of the time, with deviations only occurring occasionally due to noise or random disturbances. When the control force direction reverses, the signs of the gravity recovery torque and the target correction torque will continuously deviate at the systemic level, and the number of deviations will increase significantly. The frequency threshold is a preset proportion of the total number of sub-windows, for example, an integer value within the range of 60% to 80% of the total number of sub-windows. The specific value of the frequency threshold is determined comprehensively based on the sea state level of the buoy operation area and the allowable false alarm rate. When the sea state level is high, the frequency threshold is appropriately reduced to improve the detection sensitivity of control force reversal; when the sea state level is low, the frequency threshold is appropriately increased to reduce the probability of false alarms caused by random disturbances. When the number of deviations within the current judgment window is greater than or equal to the frequency threshold, the built-in heavy block is determined to be in the control force reversal interval, and the judgment result is passed to the subsequent step S3.

[0030] S3: When the control force is determined to be in the reverse zone, the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque is calculated based on the attitude angle and real-time position. The implementation process is as follows: When step S2 determines that the built-in weight is in the control force reversal range, the direction of the gravity force on the built-in weight on the arc-shaped guide rail is determined based on the attitude angle, and the gravity component vector in the tangential direction of the arc-shaped guide rail is determined in conjunction with the real-time position. The attitude angle is the attitude angle of the float at the current moment calculated in S1, which includes the roll angle and pitch angle. The attitude angle reflects the tilt direction and tilt angle of the float relative to the set coordinate system. The real-time position is the real-time position of the built-in weight on the arc-shaped guide rail obtained after low-pass filtering in S1. The direction of the gravity force on the built-in weight on the arc-shaped guide rail is always vertically downward, which is expressed as a unit vector along the negative Z-axis of the set coordinate system. The unit vector in the vertically downward direction is rotated according to the attitude angle to obtain the direction vector of the gravity force on the built-in weight on the arc-shaped guide rail in the set coordinate system. Specifically, a negative roll angle is first rotated around the X-axis of the set coordinate system, and then a negative pitch angle is rotated around the Y-axis of the set coordinate system to obtain the direction vector. The real-time position corresponds to a specific point on the curved guide rail. The geometry of the curved guide rail is known. The tangent direction vector at this specific point is calculated based on the real-time position and the radius of curvature of the curved guide rail. The radius of curvature of the curved guide rail is a fixed structural parameter. The center of the curved guide rail is located at the origin of the set coordinate system. The offset angle is obtained by dividing the arc length corresponding to the real-time position by the radius of curvature of the curved guide rail. The offset angle is zero at the lowest point of the curved guide rail. The direction of the tangent direction vector in the set coordinate system is determined by the offset angle. When the tangent direction vector is in the XZ plane of the set coordinate system, it is equal to the cosine of the offset angle multiplied by the unit vector of the X-axis of the set coordinate system, plus the sine of the offset angle multiplied by the unit vector of the Z-axis of the set coordinate system. The same principle applies when the curved guide rail is arranged in the YZ plane, replacing the coordinate axes. Project the direction vector of the gravity acting on the built-in weight on the curved guide rail onto the tangential direction vector. The length of the projection is equal to the dot product of the direction vector and the tangential direction vector. The resulting projected vector is the gravity component vector along the tangential direction of the curved guide rail. The magnitude of the gravity component vector is equal to the magnitude of the gravity acting on the built-in weight multiplied by the cosine of the angle between the direction vector and the tangential direction vector. The direction of the gravity component vector is parallel to the direction of the tangential direction vector, and its direction is determined by the sign of the projection. When the projection is positive, the gravity component vector and the tangential direction vector are in the same direction; when the projection is negative, the gravity component vector and the tangential direction vector are in opposite directions.

[0031] The direction vector of the target correction torque is obtained from the attitude closed-loop controller. The attitude closed-loop controller operates within the attitude calculation board. In each control cycle, the attitude closed-loop controller outputs the target correction torque, which is a vector with magnitude and direction. The direction vector of the target correction torque is defined as the direction of the axis of action of the target correction torque, indicating the direction of the rotational axis in which the attitude closed-loop controller intends to apply the torque. The operation of obtaining the direction vector of the target correction torque is achieved by reading the direction vector data stored in the output register of the attitude closed-loop controller. This reading occurs once per control cycle, after step S2 determines the gravity recovery torque and identifies the control force reversal interval. The direction vector of the target corrective torque is expressed in a set coordinate system. For example, when the direction vector of the target corrective torque is represented as a unit vector along the X-axis of the set coordinate system, it means that the attitude closed-loop controller expects the float to rotate around the X-axis of the set coordinate system to eliminate roll deviation. When the direction vector of the target corrective torque is represented as a unit vector along the Y-axis of the set coordinate system, it means that the attitude closed-loop controller expects the float to rotate around the Y-axis of the set coordinate system to eliminate pitch deviation. When the target corrective torque contains components in both the roll and pitch directions, the direction vector of the target corrective torque is the vector synthesis result of the unit vectors in the roll and pitch directions. The synthesized vector is then normalized to obtain the direction vector of the target corrective torque per unit length.

[0032] The spatial angle between the gravity component vector and the direction vector of the target corrective moment is calculated and used as the angle between the gravity component vector of the built-in weight on the curved guide rail and the direction of the target corrective moment. The spatial angle is calculated by taking the dot product of the unit vector of the gravity component vector and the direction vector of the target corrective moment, and then taking the inverse cosine of the dot product to obtain the spatial angle between them. The spatial angle ranges from 0 degrees to 180 degrees. When the gravity component vector and the direction vector of the target corrective moment are in the same direction, the spatial angle is 0 degrees, indicating that the torque generated by the gravity component vector is completely coordinated with the target corrective moment. When the gravity component vector and the direction vector of the target corrective moment are in opposite directions, the spatial angle is 180 degrees, indicating that the torque generated by the gravity component vector is completely opposed to the target corrective moment. The larger the spatial angle, the more severe the deviation between the directions of the gravity component vector and the target corrective moment, and the deeper the degree of reversal of the control force. The calculated spatial angle is used as the angle between the gravity component vector of the built-in weight on the arc-shaped guide rail and the direction of the target correction torque, and is passed to the subsequent step S4 to construct the phase plane.

[0033] S4: Construct a phase plane by combining the included angle and the gravitational restoring torque. Based on the convergence of the trajectory of the current working point of the built-in weight in the phase plane, determine whether the built-in weight is approaching the geometric self-locking dead zone. The implementation process is as follows: The phase plane is constructed using the calculated angle as the first coordinate axis and the determined gravity recovery torque as the second coordinate axis. The calculated angle is the angle between the gravity component vector of the built-in weight on the arc-shaped guide rail (output in step S3) and the direction of the target correction torque. The angle is a degree value. The determined gravity recovery torque is the gravity recovery torque generated by the built-in weight on the float in the current state, calculated in step S2 based on the attitude angle and real-time position using rigid body statics. The gravity recovery torque is a torque value, measured in Newton-meters. The phase plane is a two-dimensional rectangular coordinate plane with the angle as the abscissa and the gravity recovery torque as the ordinate. The abscissa range of the phase plane is 0 to 180 degrees, and the ordinate range is symmetrically distributed between the positive and negative maximum gravity recovery torques. The positive and negative maximum gravity recovery torques are determined by the product of the mass of the built-in weight, the radius of curvature of the arc-shaped guide rail, and the gravitational acceleration. The phase plane is constructed in the memory of the attitude calculation board, storing the coordinate axis scales and range parameters of the phase plane as a two-dimensional floating-point array. The coordinate axis range parameters of the phase plane are updated once in each control cycle to ensure that the display range of the phase plane always covers the possible positions of the current working point. The horizontal axis of the phase plane is uniformly divided into scales in degrees, and the vertical axis is uniformly divided into scales in Newton-meters. The fineness of the scale division is determined based on the accuracy of the angle calculation and the accuracy of the gravity restoring torque calculation.

[0034] The combined value of the angle between the built-in weight and the gravity restoring torque in the current control cycle is mapped to the current operating point in the phase plane. The angle in the current control cycle is calculated in step S3, and the gravity restoring torque in the current control cycle is calculated in step S2. The mapping operation is performed after the angle calculation in step S3 and the gravity restoring torque calculation in step S2. The value of the angle is directly used as the abscissa value of the current operating point in the phase plane, and the value of the gravity restoring torque is directly used as the ordinate value of the current operating point in the phase plane, thus obtaining the coordinate position of the current operating point in the phase plane. The current operating point is stored in the memory of the attitude calculation board in the form of coordinate values, and one coordinate value of the current operating point is stored in each control cycle.

[0035] The working points of multiple consecutive control cycles preceding the current working point are recorded in the phase plane to form the working point trajectory. The number of consecutive control cycles is determined based on the swaying period of the float under wave action and the control cycle duration of the attitude closed-loop controller, ensuring that the time covered by the working point trajectory reflects the dynamic trend of the working point movement within the control force reversal interval. The number of consecutive control cycles is, for example, an integer value within the range of 20 to 50. The recording operation involves sequentially storing the coordinate values ​​of the current working point generated in each control cycle into a circular buffer of length equal to the number of consecutive control cycles. The circular buffer operates in a first-in, first-out (FIFO) manner; when a new current working point coordinate value is stored, the oldest stored current working point coordinate value is removed from the circular buffer. The circular buffer always stores the coordinate values ​​of the working points from the most recent consecutive control cycles. All working points in the circular buffer are arranged in chronological order to form the working point trajectory, which is an ordered sequence of discrete points in the phase plane, with each discrete point corresponding to a working point in one control cycle. The working point trajectory is stored in the memory of the attitude calculation board for subsequent convergence analysis.

[0036] Calculate the vector direction between adjacent working points in the working point trajectory. The vector direction between adjacent working points is defined as the direction of a directed line segment pointing from the earlier working point in time to the later working point in time. The vector direction is represented by an angle in the phase plane, with the reference zero point of the vector direction being the positive direction of the abscissa axis of the phase plane. The value of the vector direction increases counterclockwise, and the range of the vector direction is from 0 degrees to 360 degrees. The calculation method for the vector direction is as follows: take two temporally adjacent working points in the working point trajectory, calculate the difference in abscissa and ordinate of the two working points in the phase plane, and use the difference in abscissa and ordinate as the two components of the directed line segment. Calculate the arctangent value of the two components to obtain the vector direction of the directed line segment in the phase plane. Perform the above vector direction calculation for each pair of adjacent working points in the working point trajectory to obtain a vector direction sequence corresponding to the working point trajectory. The length of the vector direction sequence is one less than the number of discrete points in the working point trajectory.

[0037] When the vector directions between a predetermined number of consecutive adjacent working points all point to the boundary region corresponding to the geometric self-locking dead zone, and the distance between adjacent working points and the boundary region corresponding to the geometric self-locking dead zone decreases point by point, the predetermined convergence condition is met, and the built-in weight approaches the geometric self-locking dead zone. The boundary region corresponding to the geometric self-locking dead zone is predefined in the phase plane. It is a region in the phase plane where the included angle is close to 180 degrees and the absolute value of the gravity restoring torque is close to 0. The method for determining the boundary region corresponding to the geometric self-locking dead zone is as follows: based on the geometric parameters of the arc-shaped guide rail and the extreme position of the built-in weight on the arc-shaped guide rail, calculate the range of combined values ​​of the included angle and the gravity restoring torque corresponding to the mechanical extreme position of the built-in weight on the arc-shaped guide rail. Map this range of combined values ​​to a closed or semi-open region in the phase plane, which serves as the boundary region corresponding to the geometric self-locking dead zone. Once determined, the boundary region corresponding to the geometric self-locking dead zone is stored as a fixed parameter in the parameter storage area of ​​the attitude calculation board and does not change with the control cycle.

[0038] The number of consecutive preset points is determined based on the total number of discrete points in the working point trajectory and the required judgment sensitivity and anti-interference capability. The number of consecutive preset points is less than the total number of discrete points in the working point trajectory; for example, the number of consecutive preset points is an integer value within the range of 3 to 8. The method for determining the direction pointing to the boundary region corresponding to the geometric self-locking dead zone is to compare the calculated vector direction with the directed direction from the current working point to the center point of the boundary region corresponding to the geometric self-locking dead zone. When the angle between the vector direction and the directed direction is less than the preset direction tolerance angle, it is determined that the vector direction points to the boundary region corresponding to the geometric self-locking dead zone. The preset direction tolerance angle is set based on the noise level of the working point trajectory in the phase plane and the distribution characteristics of the boundary region corresponding to the geometric self-locking dead zone; for example, the preset direction tolerance angle is a fixed value within the range of 10 degrees to 30 degrees.

[0039] The distance between adjacent working points and the boundary region corresponding to the geometric self-locking dead zone is defined as the shortest Euclidean distance from the later working point among the adjacent working points to the boundary region corresponding to the geometric self-locking dead zone. The calculation method for the shortest Euclidean distance of the boundary region corresponding to the geometric self-locking dead zone is as follows: when the boundary region corresponding to the geometric self-locking dead zone is a closed region, the shortest Euclidean distance is recorded as zero if the later working point is located inside the closed region; if the later working point is located outside the closed region, the straight-line distance from the later working point to the nearest point on the boundary of the closed region is calculated. The method for determining point-by-point decreasing distance is as follows: for a sequence consisting of a preset number of consecutive working points plus one consecutive working point, the shortest Euclidean distance from each working point in the sequence to the boundary region corresponding to the geometric self-locking dead zone is calculated one by one. When the shortest Euclidean distance of each working point in the sequence is less than the shortest Euclidean distance of its immediate preceding working point, i.e., the shortest Euclidean distance is strictly monotonically decreasing, the point-by-point decreasing distance condition is determined to be true.

[0040] When both the vector direction condition and the distance decreasing point by point condition are met, the preset convergence condition is established, the built-in heavy block is determined to approach the geometric self-locking dead zone, and the determination result is passed to the subsequent step S5.

[0041] S5: When the geometric self-locking dead zone is approached, a correction control command is generated. The correction control command causes the built-in weight to move along the arc-shaped guide rail in the direction of escaping the geometric self-locking dead zone, and then the attitude correction is performed. The implementation process is as follows: When step S4 determines that the built-in weight is approaching the geometric self-locking dead zone, the direction for leaving the geometric self-locking dead zone is determined based on the real-time position of the built-in weight on the arc-shaped guide rail and the geometric parameters of the arc-shaped guide rail. The real-time position of the built-in weight on the arc-shaped guide rail is the real-time position of the built-in weight on the arc-shaped guide rail obtained after low-pass filtering in S1. The geometric parameters of the arc-shaped guide rail include the radius of curvature of the arc-shaped guide rail, the center position of the arc-shaped guide rail, the arc length range of the arc-shaped guide rail, and the spatial orientation of the arc-shaped guide rail in the set coordinate system. The geometric parameters of the arc-shaped guide rail are measured and recorded as fixed structural parameters after the float assembly is completed, and stored in the parameter storage area of ​​the attitude calculation board. The specific method for determining the direction for leaving the geometric self-locking dead zone is to first compare the real-time position with the arc length range of the arc-shaped guide rail to determine the distance of the current arc length position of the built-in weight on the arc-shaped guide rail relative to the two endpoints of the arc-shaped guide rail. The two endpoints of the curved guide rail are the first mechanical limit end and the second mechanical limit end. The first mechanical limit end corresponds to the minimum value of the arc length coordinate of the curved guide rail, and the second mechanical limit end corresponds to the maximum value of the arc length coordinate. The geometric self-locking dead zone is located near the mechanical limit end of the curved guide rail. When the built-in weight approaches the geometric self-locking dead zone, the built-in weight will inevitably move closer to either the first mechanical limit end or the second mechanical limit end of the curved guide rail. By comparing the arc length coordinates corresponding to the real-time position of the built-in weight with the arc length coordinates of the midpoint of the arc length range of the arc guide, it is determined which mechanical limit end of the arc guide the built-in weight is approaching. If the arc length coordinates corresponding to the real-time position of the built-in weight are greater than the arc length coordinates of the midpoint of the arc length range of the arc guide, then the built-in weight is approaching the second mechanical limit end of the arc guide, and the direction of leaving the geometric self-locking dead zone is towards the first mechanical limit end of the arc guide, i.e., along the direction where the arc length of the arc guide decreases. If the arc length coordinates corresponding to the real-time position of the built-in weight are less than the arc length coordinates of the midpoint of the arc length range of the arc guide, then the built-in weight is approaching the first mechanical limit end of the arc guide, and the direction of leaving the geometric self-locking dead zone is towards the second mechanical limit end of the arc guide, i.e., along the direction where the arc length of the arc guide increases. The direction of leaving the geometric self-locking dead zone is represented by a signed arc length increment; a positive arc length increment indicates a direction where the arc length of the arc guide increases, and a negative arc length increment indicates a direction where the arc length of the arc guide decreases.

[0042] A first control command is generated, driving the built-in weight along the arc-shaped guide rail towards a safe position range in the direction of escaping the geometric self-locking dead zone. The first control command is a voltage or current command sent to the drive motor, and its content includes the direction and speed of movement. The direction of movement is determined by the direction of escaping the geometric self-locking dead zone, directly taking the sign of the arc length increment corresponding to that direction. A positive sign indicates forward rotation of the drive motor, and a negative sign indicates reverse rotation. The speed of movement is dynamically determined based on the distance between the real-time position of the built-in weight and the boundary region corresponding to the geometric self-locking dead zone. The closer the real-time position of the built-in weight is to the geometric self-locking dead zone, the greater the speed; the closer the real-time position of the built-in weight is to the safe position range, the greater the speed. The specific method for setting the movement speed is as follows: the closest distance between the real-time position of the built-in weight and the boundary area corresponding to the geometric self-locking dead zone is divided into multiple distance intervals. Each distance interval corresponds to a preset movement speed level. For example, the distance intervals are divided into three levels: emergency release interval, accelerated release interval, and slow release interval. The emergency release interval is defined as the range where the closest distance between the built-in weight and the boundary area corresponding to the geometric self-locking dead zone is within 10% of the total arc length of the curved guide rail. The accelerated release interval is defined as the range from 10% to 30%, and the slow release interval is defined as the range above 30%. The emergency release interval corresponds to a higher movement speed, the accelerated release interval corresponds to a medium movement speed, and the slow release interval corresponds to a lower movement speed. The safe position interval is predefined on the curved guide rail. The safe position interval is a continuous arc segment on the curved guide rail away from the geometric self-locking dead zone. The center position of the safe position interval is the midpoint of the arc length range of the curved guide rail. The arc length span of the safe position interval is determined according to the total arc length of the curved guide rail. For example, the arc length span of the safe position interval is a proportion within the range of 20% to 40% of the total arc length of the curved guide rail. The arc length and position of the safe position range are stored as fixed parameters in the parameter storage area of ​​the attitude calculation board. The first control command is continuously updated throughout the motion process. Each control cycle recalculates the motion speed level based on the latest real-time position of the built-in weight on the arc-shaped guide rail until the real-time position of the built-in weight enters the safe position range, at which point the drive stops and the generation of the first control command ends.

[0043] After the built-in weight enters the safe position range, a second control command is generated to drive the built-in weight to perform attitude correction. The conditions for determining if the built-in weight has entered the safe position range are: the arc length coordinate corresponding to the real-time position of the built-in weight is greater than or equal to the lower limit of the arc length of the safe position range and less than or equal to the upper limit of the arc length of the safe position range; the lower limit of the arc length of the safe position range is equal to the midpoint coordinate of the arc length range of the arc guide rail minus half the arc length span of the safe position range; the upper limit of the arc length of the safe position range is equal to the midpoint coordinate of the arc length range of the arc guide rail plus half the arc length span of the safe position range. When the real-time position of the built-in weight meets the above range conditions, it is confirmed that the built-in weight has entered the safe position range, and a second control command is generated. The second control command is a voltage or current command sent to the drive motor. The content of the second control command includes the target position and the movement speed. The target position of the second control command is calculated by the attitude closed-loop controller based on the attitude deviation between the current attitude angle and the target attitude. The movement speed of the second control command is determined by the attitude closed-loop controller according to the magnitude of the attitude deviation using a conventional control law. When the second control command is generated, the control authority of the attitude closed-loop controller is restored to the normal state. The attitude closed-loop controller regains control of the built-in weight and, according to a preset proportional-integral-derivative control algorithm or other feedback control algorithm, calculates the target position of the built-in weight in real time based on the attitude deviation. It then controls the built-in weight to move on the arc-shaped guide rail to generate a corrective torque and execute attitude correction. The generation frequency of the second control command is synchronized with the control cycle. The target position and movement speed of the second control command are updated once in each control cycle, and attitude correction continues to be executed until the next control force reversal interval or geometric self-locking dead zone approach event is triggered again.

[0044] Example 2: Figure 2 A schematic diagram of the buoy attitude closed-loop control system based on the Global Navigation Satellite System (GNSS) of this invention is provided. The buoy attitude closed-loop control system based on the GNSS includes the following modules: The position acquisition module is used to acquire the attitude angle of the buoy obtained by the multi-antenna attitude measurement of the global navigation satellite system, as well as the real-time position of the built-in weight on the arc-shaped guide rail; The reverse judgment module is used to determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and to obtain the target correction torque output by the attitude closed-loop controller. When the signs of the gravity recovery torque and the target correction torque deviate from each other multiple times within the set judgment window, the built-in weight is determined to be in the control force reverse interval. The angle calculation module is used to calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque when it is determined that the control force is in the reverse zone, based on the attitude angle and real-time position. The approach evaluation module is used to construct a phase plane by combining the included angle and the gravitational restoring torque. Based on the trajectory convergence of the current working point of the built-in weight in the phase plane, it determines whether the built-in weight is approaching the geometric self-locking dead zone. The instruction generation module is used to generate correction control instructions when the geometric self-locking dead zone is approached. The correction control instructions cause the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then perform attitude correction.

[0045] All calculations involved in the embodiments are dimensionless numerical calculations, and the preset parameters and thresholds in the calculations are set by those skilled in the art according to the actual situation.

[0046] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0047] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and inventive constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0048] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0049] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or modules may be electrical, mechanical, or other forms.

[0050] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0051] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A closed-loop control method for the attitude of a float based on a global navigation satellite system, characterized in that, Includes the following steps: S1: Obtain the attitude angle of the buoy obtained by the multi-antenna attitude measurement of the global navigation satellite system, and the real-time position of the built-in weight on the arc-shaped guide rail; S2: Determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and obtain the target correction torque output by the attitude closed-loop controller. When the number of times the signs of the gravity recovery torque and the target correction torque deviate within the set judgment window reaches the frequency threshold, it is determined that the built-in weight is in the control force reversal interval. S3: When it is determined that the control force is in the reverse zone, calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque based on the attitude angle and real-time position. S4: Construct a phase plane by combining the included angle and the gravity recovery torque. Based on the convergence of the trajectory of the current working point of the built-in weight in the phase plane, determine whether the built-in weight approaches the geometric self-locking dead zone. S5: When it is determined that the geometric self-locking dead zone is approaching, a correction control command is generated. The correction control command causes the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then the attitude correction is performed.

2. The float attitude closed-loop control method based on a global navigation satellite system according to claim 1, characterized in that, S1 includes: By using multiple global navigation satellite system antennas to receive satellite signals, the attitude angle of the buoy at the current moment in a set coordinate system can be calculated; Meanwhile, the offset of the built-in weight relative to the zero position of the arc-shaped guide rail is obtained by using a displacement sensor linked to the built-in weight. After signal conditioning, the real-time position of the built-in weight on the arc-shaped guide rail is obtained.

3. The float attitude closed-loop control method based on a global navigation satellite system according to claim 2, characterized in that, The offset of the built-in weight relative to the zero position of the arc-shaped guide rail is obtained using a displacement sensor linked to the built-in weight. After signal conditioning, the real-time position of the built-in weight on the arc-shaped guide rail is obtained, including: The displacement sensor is a rotary encoder mounted on the end of the arc-shaped guide rail. The rotating shaft of the rotary encoder is coaxially connected to the drive screw of the arc-shaped guide rail via a coupling. Read the pulse count output by the rotary encoder and convert the pulse count into the offset of the built-in weight on the arc-shaped guide rail relative to the zero position of the arc-shaped guide rail; The offset is low-pass filtered to remove high-frequency mechanical vibration noise caused by the swaying of the float, thus obtaining the real-time position of the built-in weight on the arc-shaped guide rail.

4. The float attitude closed-loop control method based on a global navigation satellite system according to claim 1, characterized in that, S2 include: Using the mass of the built-in weight, the radius of curvature of the arc-shaped guide rail, the attitude angle, and the real-time position, the gravitational restoring torque generated by the built-in weight on the float in the current state is calculated through rigid body statics. The target correction torque generated in the current control cycle is directly read from the output of the attitude closed-loop controller. Within the set judgment window, continuously compare the positive and negative signs of the gravity recovery torque and the target correction torque, and accumulate the number of times the signs are different as the number of deviations; When the number of deviations reaches the frequency threshold, the built-in heavy block is determined to be in the control force reversal zone.

5. The float attitude closed-loop control method based on a global navigation satellite system according to claim 4, characterized in that, Within the defined judgment window, the signs of the gravity restoring torque and the target correction torque are continuously compared. The number of times the signs differ is recorded as the deviation count, including: Divide the judgment window into multiple sub-windows of equal length; Within each sub-window, a deviation is recorded when the sign of the gravity recovery torque is different from the sign of the target correction torque. The number of divergences in all child windows is summed to obtain the number of divergences within the set judgment window.

6. The float attitude closed-loop control method based on a global navigation satellite system according to claim 1, characterized in that, S3 include: When it is determined that the built-in weight is in the reverse range of the control force, the direction of the gravity force on the built-in weight on the arc-shaped guide rail is determined based on the attitude angle, and the gravity component vector in the tangential direction of the arc-shaped guide rail is determined in combination with the real-time position. Obtain the direction vector of the target correction torque from the attitude closed-loop controller; Calculate the spatial angle between the gravity component vector and the direction vector of the target correction moment, which is used as the angle between the gravity component vector of the built-in weight on the arc-shaped guide rail and the direction of the target correction moment.

7. The buoy attitude closed-loop control method based on a global navigation satellite system according to claim 1, characterized in that, S4 includes: The phase plane is constructed by using the calculated included angle as the first coordinate axis of the phase plane and the determined gravitational restoring torque as the second coordinate axis of the phase plane. The combined value of the angle between the built-in weight and the gravity restoring torque in the current control cycle is mapped to the current operating point in the phase plane; Record the operating points of multiple consecutive control cycles preceding the current operating point in the phase plane to form the operating point trajectory; Analyze the movement trend of the working point trajectory towards the boundary region corresponding to the geometric self-locking dead zone in the phase plane. When the trend meets the preset convergence condition, it is determined that the built-in heavy block approaches the geometric self-locking dead zone.

8. The float attitude closed-loop control method based on a global navigation satellite system according to claim 7, characterized in that, Analyze the movement trend of the working point trajectory towards the boundary region corresponding to the geometric self-locking dead zone in the phase plane. When the trend meets the preset convergence condition, determine that the built-in heavy block is approaching the geometric self-locking dead zone, including: Calculate the vector direction between adjacent working points in the working point trajectory; When the vector directions between a preset number of consecutive adjacent working points all point to the boundary region corresponding to the geometric self-locking dead zone, and the distance between the adjacent working points and the boundary region corresponding to the geometric self-locking dead zone decreases point by point, it is determined that the preset convergence condition is met, and the built-in heavy block approaches the geometric self-locking dead zone.

9. The float attitude closed-loop control method based on a global navigation satellite system according to claim 1, characterized in that, S5 include: When it is determined that the built-in weight is approaching the geometric self-locking dead zone, the direction of leaving the geometric self-locking dead zone is determined based on the real-time position of the built-in weight on the arc-shaped guide rail and the geometric parameters of the arc-shaped guide rail. Generate the first control command to drive the built-in weight along the arc-shaped guide rail to move towards the safe position range in the direction of breaking away from the geometric self-locking dead zone; After the built-in weight enters the safe position range, a second control command is generated to drive the built-in weight to perform attitude correction.

10. A float attitude closed-loop control system based on a global navigation satellite system, used to implement the float attitude closed-loop control method based on a global navigation satellite system as described in any one of claims 1-9, characterized in that, Includes the following modules: The position acquisition module is used to acquire the attitude angle of the buoy obtained by the multi-antenna attitude measurement of the global navigation satellite system, as well as the real-time position of the built-in weight on the arc-shaped guide rail; The reverse judgment module is used to determine the gravity recovery torque currently generated by the built-in weight based on the attitude angle and real-time position, and to obtain the target correction torque output by the attitude closed-loop controller. When the signs of the gravity recovery torque and the target correction torque deviate from each other multiple times within the set judgment window, the built-in weight is determined to be in the control force reverse interval. The angle calculation module is used to calculate the angle between the gravity component vector of the built-in weight on the arc guide rail and the direction of the target correction torque when it is determined that the control force is in the reverse zone, based on the attitude angle and real-time position. The approach evaluation module is used to construct a phase plane by combining the included angle and the gravitational restoring torque. Based on the trajectory convergence of the current working point of the built-in weight in the phase plane, it determines whether the built-in weight is approaching the geometric self-locking dead zone. The instruction generation module is used to generate correction control instructions when the geometric self-locking dead zone is approached. The correction control instructions cause the built-in weight to move along the arc-shaped guide rail in the direction of getting out of the geometric self-locking dead zone first, and then perform attitude correction.

Images