Closed-loop Adaptive Control-based Median Dynamic Calibration Method and System for Balanced Ring Machinery

Abstract

The present invention discloses a dynamic calibration method and system for the median of a balance ring mechanism based on closed-loop adaptive control. The mechanical median is corrected in real time based on a dynamic mechanical median self-calibration mechanism. By real-time monitoring of the angular velocity #imgabs0# and the angle θ, the mechanical median θ is automatically updated when the system is in balance. mech , without relying on static preset parameters, breaking through the traditional static calibration mode, and realizing adaptive calibration in a dynamic environment; a non-linear balance algorithm based on sliding mode control, which adopts a sliding mode control law and combines the design of a 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 through a switching term. It has the advantages of dynamic self-calibration, strong anti-interference ability, low cost, high reliability, and fast response.

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 solutions are adopted for balance control of self-balancing scooters and industrial balancing devices, but there are significant defects: (1) Dual-gyro PID control: The dual-gyro 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 changes, 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 cars on a circular track. However, there are problems: Environmentally sensitive, easily affected by magnetic field interference or temperature changes; 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, and it is difficult to maintain high-precision balance in complex environments (such as dynamic loads, 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:

[0006] System modeling to establish a balance dynamic equation;

[0007] Real-time monitoring to obtain the angle and angular velocity;

[0008] Calculating the angle error and angular velocity error according to the real-time angle and angular velocity;

[0009] 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;

[0010] 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;

[0011] Generate a motor torque signal according to the control input value and drive the motor to adjust the angle of the balance ring.

[0012] Furthermore, perform system modeling and establish a balance dynamic equation, specifically including:

[0013] Simplify the dynamic equation of the segway into a first-order rotational system:

[0014]

[0015] where, J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ;

[0016] : current angle, with the unit of rad;

[0017] : angular acceleration, with the unit of rad / s²;

[0018] u: control input value, with the unit of N·m;

[0019] d(t): external disturbance, with the unit of N·m.

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

[0021]

[0022]

[0023] 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 .

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

[0025] Design the sliding mode surface:

[0026]

[0027] where, c: sliding mode gain, which determines the speed of error convergence;

[0028] : angle error, with the unit of rad;

[0029] : Angular velocity error, unit: rad / s;

[0030] When s = 0, the system reaches the sliding surface, and at this time, the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0031] Furthermore, calculate the control input value according to the sliding mode control law, specifically including:

[0032] The control input value u includes a continuous term and a switching term, and the expression is:

[0033]

[0034] where k: continuous control gain, which determines the convergence speed of the system;

[0035] ξ: switching term coefficient, used to suppress the external disturbance d(t);

[0036] : the second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0;

[0037] J: moment of inertia, assumed to be a constant, unit: kg·m 2 ;

[0038] sign(s) is the sign function, defined as follows:

[0039]

[0040] where the continuous term is used to provide linear feedback and accelerate error convergence;

[0041] The switching term is used to resist the external disturbance d(t) and enhance robustness;

[0042] is used to compensate for the influence of the target angular acceleration.

[0043] Furthermore, 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 update the mechanical median according to the current angle. If it does not hold, keep the current mechanical median unchanged, specifically including:

[0044] Judge whether it holds;

[0045] If it holds, update the mechanical median, let ;

[0046] If it does not hold, skip updating the mechanical median value;

[0047] Among them, : calibration threshold, used to judge whether the system is close to the equilibrium state, unit: rad / s;

[0048] : mechanical median value, representing the equilibrium reference point of the system, unit: rad;

[0049] : current angle, unit: rad;

[0050] : angular velocity, unit: rad / s.

[0051] According to the second aspect, in one embodiment, a dynamic calibration system for the mechanical median value of a balance ring based on closed-loop adaptive control is provided. The system includes:

[0052] A system modeling module, used for system modeling and establishing a balance dynamic equation;

[0053] A sensor data acquisition module, used for real-time monitoring and obtaining the angle and angular velocity;

[0054] An error calculation module, used for calculating the angle error and angular velocity error according to the real-time angle and angular velocity;

[0055] A sliding mode surface and control input calculation module, used 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;

[0056] A mechanical median value calibration module, used for 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 equilibrium state, and the mechanical median value is updated according to the current angle. If it does not hold, the current mechanical median value remains unchanged;

[0057] An electric motor drive output module, used for generating an electric motor torque signal according to the control input value and driving the electric motor to adjust the angle of the balance ring.

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

[0059] Design a sliding mode surface:

[0060]

[0061] Among them, c: sliding mode gain, determining the speed of error convergence;

[0062] : angle error, unit: rad;

[0063] : Angular velocity error, unit: rad / s;

[0064] When s = 0, the system reaches the sliding surface, and at this time, the angle error will decay exponentially and finally tend to zero, and the system is stable.

[0065] Furthermore, the sliding surface and the control input calculation module are specifically used for:

[0066] The control input value u includes a continuous term and a switching term, and the expression is:

[0067]

[0068] where k: continuous control gain, which determines the convergence speed of the system;

[0069] ξ: switching term coefficient, used to suppress the external disturbance d(t);

[0070] : second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0;

[0071] J: moment of inertia, assumed to be a constant, unit: kg·m 2 ;

[0072] sign(s) is the sign function, defined as follows:

[0073]

[0074] where the continuous term is used to provide linear feedback and accelerate error convergence;

[0075] The switching term is used to resist the external disturbance d(t) and enhance the robustness;

[0076] is used to compensate for the influence of the target angular acceleration.

[0077] Furthermore, the mechanical median calibration module is specifically used for:

[0078] Judge whether it holds;

[0079] If it holds, then update the mechanical median ;

[0080] If it does not hold, then skip updating the mechanical median;

[0081] where : calibration threshold, used to judge whether the system is close to the equilibrium state, unit: rad / s;

[0082] : Mechanical median value, representing the balance reference point of the system, with the unit of rad;

[0083] : Current angle, with the unit of rad;

[0084] : Angular velocity, with the unit of rad / s.

[0085] The present invention provides a dynamic calibration method and system for the mechanical median value of a balance ring based on closed-loop adaptive control, having the following beneficial effects:

[0086] (1) Dynamic self-calibration: Based on the dynamic mechanical median value self-calibration mechanism, the mechanical median value is corrected in real time. By monitoring the angular velocity and the angle θ in real time, the mechanical median value θ mech is automatically updated when the system is balanced, 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°.

[0087] (2) Strong anti-interference: Based on the nonlinear 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 linearization limitation of PID control, the sliding mode control suppresses external interference (such as vibration and impact) through the switching term, and reduces the angular velocity fluctuation by 80%.

[0088] (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%.

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

[0090] Figure 1 is a flowchart of a dynamic calibration method for the mechanical median value of a balance ring based on closed-loop adaptive control provided by an embodiment of the present invention;

[0091] Figure 2 is a schematic diagram of the logical structure of a dynamic calibration system for the mechanical median value of a balance ring based on closed-loop adaptive control provided by an embodiment of the present invention. Detailed Embodiment

[0092] The present invention will be further described in detail below in conjunction with the accompanying drawings through specific embodiments. Similar elements in different embodiments are denoted by related similar element numbers. In the following embodiments, 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 overwhelming the core part of the present invention with 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 the general technical knowledge in the art.

[0093] In addition, the features, operations, or characteristics described in the specification can be combined in any suitable manner to form various embodiments. 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 the drawings are only for clearly describing a certain embodiment and do not mean that they are the necessary sequences, unless it is stated that a certain sequence must be followed.

[0094] A method for dynamic calibration of the mechanical median of a balance ring based on closed-loop adaptive control provided by the first embodiment of the present invention will be described in detail below in conjunction with Figure 1 this.

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

[0096] Specifically, the dynamic equation of the balance scooter is simplified to a first-order rotational system:

[0097]

[0098] where J: moment of inertia, assumed to be a constant, with the unit of kg·m 2 ;

[0099] : current angle, with the mechanical median as the reference point, in radians (rad);

[0100] : angular acceleration, in rad / s²;

[0101] u: control input value (motor torque), in N·m;

[0102] d(t): external disturbance, in N·m.

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

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

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

[0106] Specifically,

[0107]

[0108]

[0109] 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 .

[0110] As Figure 1 shown, 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.

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

[0112]

[0113] where c > 0: is the sliding mode gain, which determines the speed of error convergence;

[0114] : angle error, in rad;

[0115] : angular velocity error, in rad / s;

[0116] When s = 0, the system reaches the sliding mode surface. At this time, the angle error will exponentially decay and finally tend to zero, and the system is stable.

[0117] The sliding mode control law is designed as:

[0118] The control input value u includes a continuous term and a switching term, and the expression is:

[0119]

[0120] where k > 0: is the continuous control gain, which determines the convergence speed of the system;

[0121] ξ > 0: The switching term coefficient is used to suppress the external disturbance d(t);

[0122] : The second derivative of the target angle, i.e., the target angular acceleration. If the target angle is stationary, then = 0;

[0123] J: The moment of inertia, assumed to be constant, with the unit of kg·m 2 ;

[0124] sign(s) is the sign function, defined as follows:

[0125]

[0126] Among them, the continuous term is used to provide linear feedback to accelerate error convergence;

[0127] The switching term is used to resist the external disturbance d(t) and enhance robustness;

[0128] is used to compensate for the influence of the target angular acceleration.

[0129] This embodiment is based on a nonlinear balancing algorithm of sliding mode control, using 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 .

[0130] 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.

[0131] Specifically:

[0132] Judge whether holds;

[0133] If it holds, update the mechanical median ;

[0134] If it does not hold, skip updating the mechanical median;

[0135] Among them, : The calibration threshold is used to judge whether the system is close to the equilibrium state, with the unit of rad / s;

[0136] : Mechanical median, representing the balance reference point of the system, with the unit of rad;

[0137] : Current angle, with the unit of rad;

[0138] : Angular velocity, with the unit of rad / s.

[0139] In this embodiment, the calibration threshold is set as follows:

[0140] Set according to the system noise level, including:

[0141] If there is noise in the sensor, it should be greater than the noise amplitude to avoid misjudgment;

[0142] Assume that the standard deviation of the sensor noise is , and according to the three - standard - deviation principle, usually take ;

[0143] Set according to the system dynamic characteristics, including:

[0144] If the system response is fast, increase it appropriately to avoid frequent updating of the mechanical median;

[0145] If the system response is slow, decrease it appropriately to ensure timely capture of the balanced state;

[0146] Set according to empirical values, including:

[0147] 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.

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

[0149] 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.

[0150] 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.

[0151] Corresponding to the above-disclosed method for dynamically calibrating the mechanical median of a balance ring based on closed-loop adaptive control, an embodiment of the present invention also discloses a system for dynamically calibrating the mechanical median of a balance ring based on closed-loop adaptive control, as Figure 2 shown, which specifically includes:

[0152] A system modeling module for system modeling to establish a balance dynamic equation;

[0153] A sensor data acquisition module for real-time monitoring and obtaining the angle and angular velocity;

[0154] An error calculation module for calculating the angle error and angular velocity error according to the real-time angle and angular velocity;

[0155] 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;

[0156] A mechanical median calibration module for determining 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;

[0157] A motor drive output module for generating a motor torque signal according to the control input value and driving the motor to adjust the angle of the balance ring.

[0158] Further, the sliding mode surface and control input calculation module is specifically used for:

[0159] Design the sliding mode surface:

[0160]

[0161] where c: sliding mode gain, which determines the speed of error convergence;

[0162] : angle error, in rad;

[0163] : angular velocity error, in rad / s;

[0164] 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.

[0165] Further, the sliding mode surface and control input calculation module is specifically used for:

[0166] The control input value u includes a continuous term and a switching term, and the expression is:

[0167]

[0168] where k is the continuous control gain, which determines the convergence speed of the system;

[0169] ξ is the switching term coefficient, which is used to suppress the external disturbance d(t);

[0170] is the second derivative of the target angle, that is, the target angular acceleration. If the target angle is stationary, then = 0;

[0171] J is the moment of inertia, assumed to be a constant, with the unit of kg·m 2 ;

[0172] sign(s) is the sign function, defined as follows:

[0173]

[0174] where the continuous term is used to provide linear feedback and accelerate error convergence;

[0175] The switching term is used to resist the external disturbance d(t) and enhance robustness;

[0176] is used to compensate for the influence of the target angular acceleration.

[0177] Furthermore, the mechanical median calibration module is specifically used for:

[0178] Judge whether it holds;

[0179] If it holds, then update the mechanical median ;

[0180] If it does not hold, then skip updating the mechanical median;

[0181] where is the calibration threshold, which is used to judge whether the system is close to the equilibrium state, with the unit of rad / s;

[0182] is the mechanical median, which represents the equilibrium reference point of the system, with the unit of rad;

[0183] is the current angle, with the unit of rad;

[0184] is the angular velocity, with the unit of rad / s.

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

[0186] 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 pertains, 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. Generate a motor torque signal according to the control input value and drive the motor to adjust the angle of the gimbal; 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 interference, in N·m; 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 ; 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 The exponential decay will eventually approach zero, and the system will be stable; 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 impact of target angular acceleration; 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.

2. 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; 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 interference, in N·m; 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 ; 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 The exponential decay will eventually approach zero, and the system will be stable; 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 impact of target angular acceleration; 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