Balance ring mechanical median dynamic calibration method and system based on closed-loop adaptive control

Abstract

The invention discloses a balance ring mechanical mid-value dynamic calibration method and system based on closed-loop adaptive control, a mechanical mid-value is corrected in real time based on a dynamic mechanical mid-value self-calibration mechanism, the mechanical mid-value theta mech is automatically updated when the system is balanced by monitoring the angular velocity # imgabs0 # and the angle theta in real time, the dependence on static preset parameters is not needed, and the mechanical mid-value dynamic calibration method and system based on closed-loop adaptive control are realized. A traditional static calibration mode is broken through, and self-adaptive calibration in a dynamic environment is realized; based on a nonlinear balance algorithm of sliding-mode control, a sliding-mode control law is adopted and sliding-mode surface design is combined, rapid convergence and anti-interference suppression are achieved, and compared with linearization limitation of PID control, sliding-mode control suppresses external interference through a switching item. The system has the advantages of dynamic self-calibration, high interference resistance, low cost, high reliability, quick response and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of balance control, and particularly to a balance ring mechanical median dynamic calibration method and system based on closed-loop adaptive control. Background Art

[0002] In the prior art, the following schemes are adopted for balance control of a self-balancing vehicle and an industrial balancing device, but there are significant defects: (1) Dual gyroscope PID control: The dual gyroscope mechanism is controlled by a fixed-parameter PID algorithm, and the balance is maintained by relying on preset parameters. Its defects are as follows: Dependence on static parameters, unable to adapt to the dynamic environment (such as load change, ground inclination) in real time, resulting in the accumulation of calibration errors; Response lag, the PID integral term is prone to saturation under high-frequency interference, and the balance accuracy decreases. (2) Magnetic repulsion balance ring: The balance is achieved by distributing magnetic force carts on a circular track. However, there are problems: Environmentally sensitive, easily affected by magnetic field interference or temperature change; High cost, the hardware cost is high due to magnetic sensors and precision track processing. (3) Mechanical transmission center of gravity adjustment: Rely on mechanical structures such as gears / linkages to adjust the center of gravity. However, there are problems: Mechanical wear, the accuracy decreases after long-term use; Slow response, the physical transmission delay limits the dynamic performance.

[0003] According to the above content, it can be seen that the prior art basically adopts a static calibration mode, unable to correct the mechanical median in real time, with insufficient adaptability to the dynamic environment, resulting in a decrease in balance stability, and relying on a single sensor or a linear algorithm, the algorithm accuracy is limited, unable to cope with non-linear interference, difficult to maintain high-precision balance in a complex environment (such as dynamic load, uneven ground), and the structure is complex or the cost is too high and it is easy to fail. Summary of the Invention

[0004] The present invention provides a balance ring mechanical median dynamic calibration method and system based on closed-loop adaptive control to solve the problems of insufficient adaptability to the dynamic environment, limited algorithm accuracy, and contradiction between cost and reliability existing in the existing balance control technology.

[0005] According to a first aspect, in one embodiment, a balance ring mechanical median dynamic calibration method based on closed-loop adaptive control is provided, and the method includes: System modeling to establish a balance dynamic equation; Real-time monitoring to obtain the angle and angular velocity; Calculating the angle error and angular velocity error according to the real-time angle and angular velocity; Calculating a sliding mode surface according to the angle error and angular velocity error, and calculating a control input value according to the sliding mode control law; Judging whether the absolute value of the angular velocity is less than a preset calibration threshold. If it holds, it is considered that the system is close to the balanced state, and the mechanical median is updated according to the current angle. If it does not hold, the current mechanical median remains unchanged; Generate a motor torque signal according to the control input value and drive the motor to adjust the angle of the balance ring.

[0006] Furthermore, perform system modeling and establish a balance dynamic equation, specifically including: Simplify the dynamic equation of the self-balancing vehicle into a first-order rotational system:

[0007] where, J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ; : current angle, with the unit of rad; : angular acceleration, with the unit of rad / s²; u: control input value, with the unit of N·m; d(t): external disturbance, with the unit of N·m.

[0008] Furthermore, calculate the angle error and angular velocity error according to the real-time angle and angular velocity, specifically including:

[0009]

[0010] That is, subtract the target angle from the current angle to obtain the angle error , and directly use the angular velocity as the angular velocity error .

[0011] Furthermore, calculate the sliding mode surface according to the angle error and angular velocity error, specifically including: Design the sliding mode surface:

[0012] where, c: sliding mode gain, which determines the speed of error convergence; : angle error, with the unit of rad; : angular velocity error, with the unit of rad / s; When s = 0, the system reaches the sliding mode surface. At this time, the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0013] Furthermore, calculate the control input value according to the sliding mode control law, specifically including: The control input value u includes a continuous term and a switching term, and the expression is:

[0014] where, k: continuous control gain, which determines the convergence speed of the system; ξ: switching term coefficient, which is used to suppress the external disturbance d(t); : the second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0; J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ; sign(s) is the sign function, defined as follows:

[0015] where, the continuous term is used to provide linear feedback to accelerate the error convergence; the switching term is used to resist the external disturbance d(t) and enhance the robustness; is used to compensate for the influence of the target angular acceleration.

[0016] Furthermore, it is judged whether the absolute value of the angular velocity is less than a preset calibration threshold. If it holds, it is considered that the system is close to the equilibrium state, and the mechanical median is updated according to the current angle. If it does not hold, the current mechanical median remains unchanged. Specifically, it includes: Judge whether it holds; If it holds, update the mechanical median, and let ; If it does not hold, skip updating the mechanical median; where, : calibration threshold, which is used to judge whether the system is close to the equilibrium state, with the unit of rad / s; : mechanical median, which represents the equilibrium reference point of the system, with the unit of rad; : current angle, with the unit of rad; : angular velocity, with the unit of rad / s.

[0017] According to the second aspect, in one embodiment, a dynamic calibration system for the mechanical median of a balance ring based on closed-loop adaptive control is provided. The system includes: A system modeling module, which is used for system modeling to establish a balance dynamic equation; A sensor data acquisition module, which is used to monitor and obtain the angle and angular velocity in real time; An error calculation module, which is used to calculate the angle error and angular velocity error according to the real-time angle and angular velocity; Sliding mode surface and control input calculation module, which is used to calculate the sliding mode surface according to the angle error and angular velocity error, and calculate the control input value according to the sliding mode control law; Mechanical median calibration module, which is used to judge whether the absolute value of the angular velocity is less than the preset calibration threshold. If it holds, it is considered that the system is close to the equilibrium state, and the mechanical median is updated according to the current angle. If it does not hold, the current mechanical median remains unchanged; Motor drive output module, which is used to generate a motor torque signal according to the control input value and drive the motor to adjust the angle of the balance ring.

[0018] Furthermore, the sliding mode surface and control input calculation module specifically is used for: Design the sliding mode surface:

[0019] where c is the sliding mode gain, which determines the speed of error convergence; is the angle error, with the unit of rad; is the angular velocity error, with the unit of rad / s; When s = 0, the system reaches the sliding mode surface. At this time, the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0020] Furthermore, the sliding mode surface and control input calculation module specifically is used for: The control input value u includes a continuous term and a switching term, and the expression is:

[0021] where k is the continuous control gain, which determines the convergence speed of the system; ξ is the switching term coefficient, which is used to suppress the external disturbance d(t); is the second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0; J is the moment of inertia, assumed to be a constant, with the unit of kg·m 2 ; sign(s) is the sign function, defined as follows:

[0022] where the continuous term is used to provide linear feedback and accelerate error convergence; The switching term is used to resist the external disturbance d(t) and enhance the robustness; is used to compensate for the influence of the target angular acceleration.

[0023] Furthermore, the mechanical median calibration module is specifically configured to: Judge Whether it holds; If it holds, update the mechanical median ; If it does not hold, skip updating the mechanical median; Wherein, : Calibration threshold, used to judge whether the system is close to the equilibrium state, unit: rad / s; : Mechanical median, representing the equilibrium reference point of the system, unit: rad; : Current angle, unit: rad; : Angular velocity, unit: rad / s.

[0024] The present invention provides a method and system for dynamically calibrating the mechanical median of a balance ring based on closed-loop adaptive control, having the following beneficial effects: (1) Dynamic self-calibration: Based on the dynamic mechanical median self-calibration mechanism, the mechanical median is corrected in real time. By monitoring the angular velocity and the angle θ, the mechanical median θ mech is automatically updated when the system is in equilibrium, without relying on static preset parameters, breaking through the traditional static calibration mode (such as PID control relying on fixed parameters), realizing adaptive calibration in a dynamic environment (temperature change, component wear, load offset), and reducing the balance error to <0.5°.

[0025] (2) Strong anti-interference: Based on the non-linear balance algorithm of sliding mode control, the sliding mode control law is adopted and combined with the design of the sliding mode surface to achieve fast convergence and anti-interference suppression. Compared with the linear limitation of PID control, the sliding mode control suppresses external interference (such as vibration, impact) through the switching term, and the angular velocity fluctuation is reduced by 80%.

[0026] (3) Low cost and high reliability: The non-contact motor drive replaces the magnetic structure, reducing the cost by 90% and the failure rate <2%.

[0027] (4) Fast response: The control delay <50ms, and the accuracy reaches ±0.1°, meeting the high-precision requirements of industry / medical treatment. Brief Description of the Drawings

[0028] Figure 1 It is a flowchart of a method for dynamically calibrating the mechanical median of a balance ring based on closed-loop adaptive control provided by an embodiment of the present invention; Figure 2Schematic diagram of the logic structure of a balance ring mechanical median dynamic calibration system provided by an embodiment of the present invention. Detailed implementation manners

[0029] The present invention will be further described in detail below in conjunction with the accompanying drawings through specific implementation manners. Similar elements in different implementation manners are labeled with related similar element numbers. In the following implementation manners, many detailed descriptions are provided to enable a better understanding of the present invention. However, those skilled in the art can easily recognize that some of the features can be omitted in different situations, or can be replaced by other elements, materials, or methods. In some cases, some operations related to the present invention are not shown or described in the specification to avoid the core part of the present invention being overwhelmed by excessive descriptions. For those skilled in the art, it is not necessary to describe these related operations in detail, and they can fully understand the related operations based on the descriptions in the specification and general technical knowledge in the field.

[0030] In addition, the features, operations, or characteristics described in the specification can be combined in any appropriate manner to form various implementation manners. At the same time, the steps or actions in the method description can also be reordered or adjusted in an obvious manner by those skilled in the art. Therefore, the various sequences in the specification and drawings are only for clearly describing a certain embodiment and do not mean a necessary sequence unless it is stated that a certain sequence must be followed.

[0031] A balance ring mechanical median dynamic calibration method provided by the first embodiment of the present invention will be described in detail below in conjunction with Figure 1 for detailed description.

[0032] As Figure 1 shown, in step S100, system modeling is performed to establish a balance dynamic equation.

[0033] Specifically, the dynamic equation of the balance car is simplified to a first-order rotation system:

[0034] where J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ; : current angle, with the mechanical median as the reference point, in radians (rad); : angular acceleration, in rad / s²; u: control input value (motor torque), in N·m; d(t): external disturbance, in N·m.

[0035] As shown Figure 1 In step S200, the angle and angular velocity are monitored and obtained in real time.

[0036] Specifically, the gyroscope angle and angular velocity are read. The real-time angle and angular velocity data are obtained through the sensor interface.

[0037] As shown Figure 1 In step S300, the angle error and angular velocity error are calculated based on the real-time angle and angular velocity.

[0038] Specifically,

[0039]

[0040] That is, the current angle is subtracted from the target angle to obtain the angle error , and the angular velocity is directly used as the angular velocity error .

[0041] As shown Figure 1 In step S400, the sliding mode surface is calculated based on the angle error and angular velocity error, and the control input value is calculated according to the sliding mode control law.

[0042] Specifically, the sliding mode surface is designed as:

[0043] where c > 0: is the sliding mode gain, which determines the speed of error convergence; : angle error, unit is rad; : angular velocity error, unit is rad / s; When s = 0, the system reaches the sliding mode surface, at this time the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0044] The sliding mode control law is designed as: The control input value u includes a continuous term and a switching term, and the expression is:

[0045] where k > 0: is the continuous control gain, which determines the convergence speed of the system; ξ > 0: is the switching term coefficient, which is used to suppress the external disturbance d(t); : The second derivative of the target angle, i.e., the target angular acceleration. If the target angle is stationary, then = 0; J: The moment of inertia, assumed to be constant, with the unit of kg·m 2 ; sign(s) is the sign function, defined as follows:

[0046] where the continuous term is used to provide linear feedback to accelerate error convergence; The switching term is used to resist the external disturbance d(t) and enhance robustness; is used to compensate for the influence of the target angular acceleration.

[0047] This embodiment is based on a nonlinear balancing algorithm of sliding mode control, adopting a sliding mode control law , combined with the design of the sliding mode surface , to achieve fast convergence and anti-interference suppression. Compared with the linearization limitation of PID control, sliding mode control suppresses external disturbances (such as road surface vibrations) through the switching term .

[0048] As Figure 1 shown, in step S500, it is judged whether the absolute value of the angular velocity is less than a preset calibration threshold. If it holds, it is considered that the system is close to the equilibrium state, and the mechanical median is updated according to the current angle. If it does not hold, the current mechanical median remains unchanged.

[0049] Specifically: Judge whether holds; If it holds, update the mechanical median ; If it does not hold, skip updating the mechanical median; where : The calibration threshold, used to judge whether the system is close to the equilibrium state, with the unit of rad / s; : The mechanical median, representing the equilibrium reference point of the system, with the unit of rad; : The current angle, with the unit of rad; : The angular velocity, with the unit of rad / s.

[0050] In this embodiment, the setting method of the calibration threshold includes: Set according to the system noise level, including: If there is noise in the sensor, It should be greater than the noise amplitude to avoid misjudgment; Assume that the standard deviation of the sensor noise is , and according to the three - standard - deviation principle, usually take ; Set according to the system dynamic characteristics, including: If the system response is fast, Appropriately increase it to avoid frequent updating of the mechanical median; If the system response is slow, Appropriately decrease it to ensure timely capture of the equilibrium state; Set according to empirical values, including: In practical applications, determine the reasonable value of through experimental debugging. For example, initially set = 0.01 rad / s, and then adjust according to the system performance.

[0051] This embodiment is based on a dynamic mechanical median self - calibration mechanism to correct the mechanical median in real - time. By real - time monitoring of the angular velocity and the angle θ, the mechanical median θ mech is automatically updated when the system is in equilibrium, without relying on static preset parameters, breaking through the traditional static calibration mode (such as PID control relying on fixed parameters), and realizing adaptive calibration in a dynamic environment (temperature change, component wear, load offset).

[0052] As Figure 1 shown, in step S600, a motor torque signal is generated according to the control input value and the motor is driven to adjust the angle of the balance ring.

[0053] Specifically, the control input u is converted into motor torque to drive the balance ring to adjust the angle. A corresponding motor torque signal is generated according to the calculated control input u to drive the motor to adjust the angle of the balance ring.

[0054] Corresponding to the above - disclosed method for dynamic calibration of the mechanical median of a balance ring based on closed - loop adaptive control, an embodiment of the present invention also discloses a system for dynamic calibration of the mechanical median of a balance ring based on closed - loop adaptive control, as Figure 2 shown, which specifically includes: A system modeling module for system modeling and establishing a balance dynamic equation; A sensor data acquisition module for real - time monitoring and obtaining the angle and angular velocity; An error calculation module for calculating the angle error and angular velocity error according to the real - time angle and angular velocity; A sliding mode surface and control input calculation module for calculating the sliding mode surface according to the angle error and angular velocity error and calculating the control input value according to the sliding mode control law; A mechanical median calibration module is used to determine whether the absolute value of the angular velocity is less than a preset calibration threshold. If it holds, it is considered that the system is close to the equilibrium state, and the mechanical median is updated according to the current angle. If it does not hold, the current mechanical median remains unchanged; A motor drive output module is used to generate a motor torque signal according to a control input value and drive the motor to adjust the angle of the balance ring.

[0055] Furthermore, the sliding mode surface and control input calculation module specifically are used for: Design a sliding mode surface:

[0056] where c: sliding mode gain, which determines the speed of error convergence; : Angle error, with the unit of rad; : Angular velocity error, with the unit of rad / s; When s = 0, the system reaches the sliding mode surface. At this time, the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0057] Furthermore, the sliding mode surface and control input calculation module specifically are used for: The control input value u includes a continuous term and a switching term, and the expression is:

[0058] where k: continuous control gain, which determines the convergence speed of the system; ξ: switching term coefficient, which is used to suppress the external disturbance d(t); : Second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0; J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ; sign(s) is a sign function, defined as follows:

[0059] where the continuous term is used to provide linear feedback and accelerate error convergence; The switching term is used to resist the external disturbance d(t) and enhance the robustness; is used to compensate for the influence of the target angular acceleration.

[0060] Furthermore, the mechanical median calibration module specifically is used for: Judge Whether it holds; If it holds, update the mechanical median value ; If it does not hold, skip updating the mechanical median value; Among them, : Calibration threshold, used to judge whether the system is close to the equilibrium state, with the unit of rad / s; : Mechanical median value, representing the equilibrium reference point of the system, with the unit of rad; : Current angle, with the unit of rad; : Angular velocity, with the unit of rad / s.

[0061] It should be noted that for the detailed description of a balance ring mechanical median value dynamic calibration system based on closed-loop adaptive control provided in the embodiments of the present invention, reference can be made to the relevant description of a balance ring mechanical median value dynamic calibration method based on closed-loop adaptive control provided in the embodiments of the present invention, which will not be elaborated here.

[0062] The above uses specific examples to elaborate on the present invention, which is only used to help understand the present invention and is not intended to limit the present invention. For those skilled in the technical field to which the present invention belongs, based on the idea of the present invention, several simple deductions, deformations or substitutions can also be made.

Claims

1. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control, characterized in that: The method comprises: System modeling and establishing equilibrium dynamic equations; Real-time monitoring to obtain angle and angular velocity; Calculate the angle error and angular velocity error according to the real-time angle and angular velocity; The sliding surface is calculated according to the angle error and the angular velocity error, and the control input value is calculated according to the sliding mode control law; Determine whether the absolute value of the angular velocity is less than the preset calibration threshold. If so, the system is considered to be close to equilibrium and the mechanical median is updated according to the current angle. If not, the current mechanical median is kept unchanged. The motor torque signal is generated according to the control input value and the motor is driven to adjust the angle of the gimbal.

2. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control as claimed in claim 1, characterized in that: System modeling and establishment of equilibrium dynamic equations, including: Simplify the dynamic equation of the balancing car into a first-order rotation system: Where, J: moment of inertia, assumed to be a constant, unit is kg·m 2 ; : Current angle, in rad; : angular acceleration, in rad / s²; u: control input value, unit is N·m; d(t): external disturbance, in N·m.

3. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control as claimed in claim 1, characterized in that: Calculate the angle error and angular velocity error based on the real-time angle and angular velocity, including: That is, use the current angle Subtract target angle Get the angle error , and directly convert the angular velocity As the angular velocity error .

4. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control as claimed in claim 1, characterized in that: The sliding surface is calculated based on the angle error and angular velocity error, including: Design sliding surface: Where, c: sliding mode gain, which determines the speed of error convergence; : Angle error, in rad; : angular velocity error, in rad / s; When s=0, the system reaches the sliding surface, and the angle error It will decay exponentially and eventually approach zero, and the system becomes stable.

5. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control as claimed in claim 4, characterized in that: The control input value is calculated according to the sliding mode control law, including: The control input value u includes continuous terms and switching terms, and the expression is: Where, k: continuous control gain, which determines the convergence speed of the system; ξ: switching term coefficient, used to suppress external interference d(t); : The second-order derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then =0; J: moment of inertia, assumed to be a constant, unit: kg·m 2 ; sign(s) is the sign function, defined as follows: Among them, the continuous term Used to provide linear feedback and accelerate error convergence; Toggle Item Used to resist external interference d(t) and enhance robustness; Used to compensate for the effect of target angular acceleration.

6. A gimbal mechanical median dynamic calibration method based on closed-loop adaptive control as claimed in claim 1, characterized in that: Determine whether the absolute value of the angular velocity is less than the preset calibration threshold. If so, the system is considered to be close to equilibrium and the mechanical median is updated according to the current angle. If not, the current mechanical median is kept unchanged. Specifically: judge whether it is established; If it holds, update the mechanical median, let ; If not, skip updating the mechanical median; in, : Calibration threshold, used to determine whether the system is close to equilibrium, in rad / s; : Mechanical median, indicating the equilibrium reference point of the system, in rad; : Current angle, in rad; : angular velocity, in rad / s.

7. A gimbal mechanical median dynamic calibration system based on closed-loop adaptive control, characterized in that: The system comprises: System modeling module, used for system modeling and establishing equilibrium dynamic equations; Sensor data acquisition module, used for real-time monitoring and acquisition of angle and angular velocity; An error calculation module, used for calculating an angle error and an angular velocity error according to a real-time angle and an angular velocity; A sliding surface and control input calculation module is used to calculate the sliding surface according to the angle error and the angular velocity error, and calculate the control input value according to the sliding mode control law; The mechanical median calibration module is used to determine whether the absolute value of the angular velocity is less than the preset calibration threshold. If so, the system is considered to be close to a balanced state and the mechanical median is updated according to the current angle. If not, the current mechanical median is kept unchanged. The motor drive output module is used to generate a motor torque signal according to the control input value and drive the motor to adjust the angle of the gimbal.

8. The gimbal mechanical median dynamic calibration system based on closed-loop adaptive control according to claim 7, characterized in that: The sliding surface and control input calculation module is specifically used for: Design sliding surface: Where, c: sliding mode gain, which determines the speed of error convergence; : Angle error, in rad; : angular velocity error, in rad / s; When s=0, the system reaches the sliding surface, and the angle error It will decay exponentially and eventually approach zero, and the system becomes stable.

9. The gimbal mechanical median dynamic calibration system based on closed-loop adaptive control according to claim 8, characterized in that: The sliding surface and control input calculation module is specifically used for: The control input value u includes continuous terms and switching terms, and the expression is: Where, k: continuous control gain, which determines the convergence speed of the system; ξ: switching term coefficient, used to suppress external interference d(t); : The second-order derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then =0; J: moment of inertia, assumed to be a constant, unit: kg·m 2 ; sign(s) is the sign function, defined as follows: Among them, the continuous term Used to provide linear feedback and accelerate error convergence; Toggle Item Used to resist external interference d(t) and enhance robustness; Used to compensate for the effect of target angular acceleration.

10. The gimbal mechanical median dynamic calibration system based on closed-loop adaptive control according to claim 7, characterized in that: The mechanical median calibration module is specifically used for: judge whether it is established; If true, update the mechanical median ; If not, skip updating the mechanical median; in, : Calibration threshold, used to determine whether the system is close to equilibrium, in rad / s; : Mechanical median, indicating the equilibrium reference point of the system, in rad; : Current angle, in rad; : angular velocity, in rad / s.

Images