Balancing control method and system for a rotating two-stage inverted pendulum and storage medium

Abstract

The present application relates to the technical field of inverted pendulum control experiment, and particularly relates to a balance control method and system for a rotating two-stage inverted pendulum and a storage medium. Whether to perform swing-up control is determined according to current total energy of the current rotating two-stage inverted pendulum and current rotation directions of two swing rods. Then, the central rotating main rod is controlled to rotate according to a swing-up control model to realize swing-up of the two swing rods. Finally, the two swing rods are controlled according to a stable swing control model to realize stable swing. The present application realizes swing-up and stable swing control of the rotating two-stage inverted pendulum through the swing-up control model and the stable swing control model, and the transition process from swing-up to stable swing can be smoothly switched. The control logic is simple and easy to realize, and the control stability is strong. Meanwhile, the special swing-up condition of the rotating two-stage pendulum is further analyzed, and specific swing-up control methods are given for the two swing rods in different states.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of inverted pendulum control experiment, and particularly relates to a balance control method and system of a rotating two-stage inverted pendulum and a storage medium. BACKGROUND

[0002] With the rapid development of control theory, computer technology and information technology, many new theories and control algorithms are constantly emerging, and inverted pendulum systems are gradually used by people to verify the effectiveness of control methods. Through continuous research and development, inverted pendulum systems have developed from one stage to two stages, three stages, four stages and even more stages, and various inverted pendulums such as planar inverted pendulums, parallel inverted pendulums and ring-shaped inverted pendulums have been produced, and the control methods are also various. Due to the simple structure, low price, easy adjustment of physical structure and parameters and other characteristics, inverted pendulum systems have a great application stage in the control systems of the industry and the aerospace industry, and the research on the theory of inverted pendulum systems has a profound significance. It is of great significance to people's life, study and verification of control methods and theories.

[0003] The research of any theory is to adapt to the needs of reality, and the theory research of inverted pendulum system also has important industrial, military and economic background. Although the first robot was introduced in the United States thirty years ago, the walking control technology of the robot has not been well solved, and its standing and walking are similar to a double inverted pendulum system; at the same time, the control of the attitude of rockets, satellites and other aircraft, the elimination of vibration to improve the quality of reconnaissance satellite skin pictures, and the principle of the fashionable electric balance car widely used in people's life are also similar to inverted pendulum systems. Or the flexible multi-stage rocket born to prevent single-stage rockets from breaking when turning. The control of the flight attitude of the flexible multi-stage rocket can also be researched by using a multi-stage inverted pendulum system. Since the inverted pendulum system has important application in the military, aerospace, robot field and general industrial process, the research on the inverted pendulum has great value.

[0004] At present, the research on inverted pendulum is more towards the direction of complexity of two-stage, three-stage, four-stage and even more stages, and the research on rotating inverted pendulum is less. A utility model patent named "Ring-shaped Inverted Pendulum Experiment Device" is disclosed in CN218384311U, but it only provides a mechanical structure for realizing a rotating inverted pendulum, and does not disclose a specific control method. SUMMARY

[0005] The first object of the present application is to provide a balance control method of a rotating two-stage inverted pendulum, which can control the starting and stable swinging of the rotating two-stage inverted pendulum.

[0006] The first object of the application is achieved by the technical solution that the rotating two-stage inverted pendulum comprises a central rotating main rod and two swing rods located on both sides of the central rotating main rod, the two swing rods can be inverted and kept balanced with the central rotating main rod, and the specific control method is as follows:

[0007] The swing control model and the stable swing control model are established for the rotating two-stage inverted pendulum.

[0008] The central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods.

[0009] After the swing, the two swing rods are controlled according to the stable swing control model to realize the stable swing.

[0010] The design has the advantages that the swing and stable swing control of the rotating two-stage inverted pendulum are realized by the swing control model and the stable swing control model, the transition process of the swing and the stable swing can be smoothly switched, the control logic is simple and easy to realize, and the control stability is strong.

[0011] Further, before the central rotating main rod is controlled to realize the swing of the two swing rods, the following steps are further included:

[0012] The current total energy of the current rotating two-stage inverted pendulum is calculated to determine whether the current total energy exceeds the swing energy threshold.

[0013] If the swing energy threshold is not exceeded, the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods.

[0014] If the swing energy threshold is exceeded, the rotation direction of the two swing rods is determined.

[0015] If the rotation directions of the two swing rods are opposite, the two swing rods are controlled by simultaneous energy decay until the decay is within the swing threshold, and then the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods.

[0016] If the rotation directions of the two swing rods are the same, the two swing rods are naturally decayed until the decay is within the swing threshold, and then the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods.

[0017] The design has the advantages that the special swing conditions of the rotating two-stage pendulum are further analyzed, and specific swing control methods are given for the two swing rods in different states. The rotating two-stage pendulum is different from the general inverted two-stage pendulum. If the current state of the two swing rods is ignored during the study of the rotating two-stage pendulum, the swing may not be realized or the swing time may be greatly prolonged. In view of this problem, a number of tests are carried out to classify and deduce the problem, and an effective solution is finally proposed.

[0018] Further, the swing-up control model adopts energy method based on Lyapunov function to realize the control of swing-up of the two swing rods, and the specific control formula is:

[0019]

[0020] In the formula, τ0 is the system input torque, f0 is the connecting rod 0 rotation friction, k is a design parameter, and G is the gravity matrix.

[0021] Further, when the two swing rods satisfy the following conditions, the two swing rods are controlled according to the stable swing control model to realize stable swing:

[0022] |θ1|≤θ 1sw ,|θ2|≤θ 2sw ,|E1|+|E2|≤E sw

[0023] θ1 and θ2 are the inclination angles of the first swing rod and the second swing rod respectively, E1 and E2 are the energies of the first swing rod and the second swing rod respectively, E sw is the energy threshold value, θ 1sw and θ 2sw are the inclination angle threshold values of the first swing rod and the second swing rod respectively.

[0024] Further, the stable swing control model is an optimal controller model, and the control formula is:

[0025]

[0026] In the formula, τ0 is the system input torque, F is the optimal control feedback gain matrix, is the system state vector.

[0027] Further, the stable swing control model is a nonlinear optimal controller model, and the control formula is:

[0028]

[0029] In the formula, τ0 is the system input torque, F s is the nonlinear optimal control feedback gain matrix, and x s is the nonlinear state vector of the system.

[0030] The second object of the present application is to provide a balance control system of a rotary two-stage inverted pendulum.

[0031] The second object of the present application is realized by the technical scheme, which comprises a swing-up control module, a control center rotation main rod rotation, to realize the swing-up of the two swing rods.

[0032] The sway stability judgment module determines whether to perform sway stability control based on the current status of the two pendulums;

[0033] The sway stabilization control module controls the center to rotate the main rod according to the sway stabilization judgment of the sway stabilization judgment module to achieve sway stabilization of the two sway rods.

[0034] Furthermore, the system also includes:

[0035] The swing start judgment module determines whether to perform swing start control based on the current total energy of the currently rotating secondary inverted pendulum and the current rotation direction of the two pendulum rods.

[0036] The third object of the present invention is to provide a storage medium storing a plurality of instructions suitable for loading by a processor using the above method.

[0037] Other advantages, objectives, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following or may be learned from practice of the present invention. The objectives and other advantages of the present invention may be realized and obtained through the following description and claims. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The accompanying drawings of the present invention are described below.

[0039] Figure 1 It is a control flow diagram of the present invention;

[0040] Figure 2 It is a structural diagram of a rotating double inverted pendulum;

[0041] Figure 3 Schematic diagram for defining the double upswing phase of a rotating double inverted pendulum;

[0042] Figure 4 Schematic diagram for defining the double downswing phase of a rotating double inverted pendulum. DETAILED DESCRIPTION

[0043] The present invention will be further described below with reference to the accompanying drawings and examples.

[0044] like Figure 1 、 Figure 2 As shown in FIG, a balance control method for a rotating secondary inverted pendulum, the specific steps are as follows:

[0045] S1. Model the system of the double inverted pendulum, establish the mathematical model of the system using the Lagrangian method, and define the swinging motion phase of the rotating double inverted pendulum;

[0046] The Lagrange method is used to establish a mathematical model. The dynamic model of the rotating two-stage inverted pendulum is as follows:

[0047]

[0048] where θ is defined as follows:

[0049] θ = [θ0 θ1 θ2] T

[0050] The matrix M(θ), and G(θ) are expressed as follows:

[0051]

[0052]

[0053]

[0054] m 11 = J0+ J1sin 2 θ1+ J2sin 2 θ2

[0055] m i2 = -m i l 01 r1cosθ1

[0056] m 13 = -m2l 02 r2cosθ2

[0057]

[0058]

[0059]

[0060] g2= -m1gr1sinθ1

[0061] g3= -m2gr2sinθ2

[0062] where J i is the rotational inertia of the rotating joint;

[0063]

[0064]

[0065] where m i is the mass of the rod i, J i is the rotational inertia of the rod i, l 0i is the distance from the connecting rod 0 to the pendulum rod i, and r iis the distance from the center of mass of the swing bar i to the center of rotation, θ is the angle of the bar i. i is the angle of the bar i.

[0066] where, in this example, m0=0.08 kg, m i 1=0.196 kg, m2=0.098 kg, J0=2.92×10 -2 kg·m 2 , J1=4.72×10 -4 kg·m 2 , J2=1.18×10 -4 kg·m 2 , l 01 1=0.3 m, l 02 2=0.3 m, r1=0.148 m, r2=0.074 m.

[0067] where the swing motion phase of the rotating two-stage inverted pendulum is defined as shown in Figure 3 、 Figure 4 when the two swing bars are in a downward state at the same time, the state is defined as a double swing phase, and when the two swing bars are in a vertical upward state at the same time, the state is defined as a double swing phase.

[0068] In this step, the rotating two-stage inverted pendulum dynamics model established by the Lagrange method is a basic model, and the control method under the swing-up and the control method under the stable swing are respectively studied on the basis of the basic model.

[0069] S2, the total energy of the rotating two-stage inverted pendulum system is calculated, and whether the system energy at this moment exceeds the self-swing energy threshold is judged according to the calculated energy. If the self-swing energy threshold is not exceeded, the subsequent steps can be performed on this basis to reduce the swing-up time; if the self-swing energy threshold is exceeded, the swing direction of the rotating two-stage inverted pendulum system is judged, if the swing direction is opposite, the self-swing energy control can be used to attenuate to the swing state, if the swing direction is the same, due to the coupling relationship of the two swing bars, the energy attenuation of the two swing bars cannot be performed at the same time, and the subsequent steps need to be performed after the system energy naturally attenuates to the self-swing energy threshold, to complete the self-swing and stable swing of the rotating two-stage inverted pendulum.

[0070] The motion equation of each swing bar can be described as

[0071]

[0072] where i=1, 2;

[0073] We assume that the control input of the system is the acceleration of the swing arm The energy of each swing bar can be described as

[0074]

[0075] We define the potential energy of the pendulum at the vertical up position as 0. From the above equation, we can get

[0076]

[0077] The condition to determine whether to directly enter the self-swing control is |E1| + |E2| > > 0, and the two pendulum rotation directions are the same.

[0078] Where m i is the mass of the rod i, J i is the moment of inertia of the rod i, l 0i is the distance from the connecting rod 0 to the pendulum i, r i is the distance from the mass center of the pendulum i to the rotation center of gravity, and θ i is the angle of the rod i.

[0079] In this example, according to the experiment, we get θ 1sw = θ 2sw = π / 7, E sw = 0.05.

[0080] The purpose of this step is to provide a method for the two pendulums to swing up in any case, and of course, an external force can be used to make the two pendulums stop at the same time, and then start swinging together.

[0081] S3, in the self-swing control, the first stage, mainly uses the energy method based on Lyapunov function to realize the swing control of the pendulum 1 and the pendulum 2, so that the pendulum swings to the balance point near the vertical up position, and the inclination angles of the two pendulums are less than the inclination angle threshold θ 1sw and θ 2sw of the controller switching, and the energy of the system is less than the energy threshold E sw of the controller switching.

[0082] For swing control, the Lyapunov function candidate used is as follows

[0083]

[0084] In the formula, a and β are normal number design parameters, and Eo1 and Eo2 are the expected energy values of the first and second pendulums near the vertical position. In this invention, we hope that the two pendulums can reach the vertical up position and remain in the prohibited state, so we set Eo1 = Eo2 = 0.

[0085] From equation (4), we can get

[0086]

[0087] Where

[0088]

[0089] We design a control input such that (6) is negative semi-definite, the control input is as follows

[0090]

[0091] where k is a design parameter. Note that the acceleration of the driven arm is considered as the control input.

[0092] Thus, we have

[0093]

[0094] However, in a usual controller design, the available control input is not the acceleration but the torque. Thus, we calculate the torque control input using the Lyapunov function (5) and the equation of motion (1). Using (1), the relationship between the acceleration θ and the torque To is described as

[0095]

[0096] From (9), we have

[0097]

[0098] Here, we consider the candidate of the Lyapunov function, substituting (10) into (6) gives

[0099]

[0100] From (11), the control input satisfying the Lyapunov condition is given by (12)

[0101]

[0102] where τ0is the input torque, f0is the link 0 rotational friction, k is a design parameter, G is the gravity torque matrix, and g0is derived from (9).

[0103] S4, judging whether the two swing rod inclination angles of the rotating two-stage inverted pendulum and the energy of the system satisfy the controller switching threshold condition.

[0104] where the defined threshold condition satisfying the control switching is

[0105] |θ1|≤θ 1sw , |θ2|≤θ 2sw , |E1|+|E2|≤E sw

[0106] where θ1and θ2are the inclination angles of the first swing rod and the second swing rod, respectively, E1and E2are the energies of the first swing rod and the second swing rod, respectively, E sw is the energy threshold, and θ1sw and θ 2sw are the inclination threshold values of the first and second pendulums, respectively, in this example, θ 1sw = θ 2sw = π / 7, E sw = 0.05.

[0107] S5, when the inclination of the two pendulums is less than the inclination threshold value of the controller switching, and the system energy is less than the energy threshold value of the controller switching, the controller is switched to a nonlinear optimal controller to realize simultaneous stable swing control of the pendulum 1 and the pendulum 2.

[0108] In order to realize the stable swing of the rotating two-stage inverted pendulum, an optimal controller and a nonlinear optimal controller are designed.

[0109] Optimal controller design

[0110] The dynamic equation of the rotating two-stage inverted pendulum system is linearized in the state of the two pendulums being vertically upward, and the linear state space equation is as follows

[0111]

[0112] wherein

[0113]

[0114]

[0115] The state variable is selected as

[0116]

[0117] The state space expression is

[0118]

[0119] wherein

[0120]

[0121]

[0122] A quadratic form criterion function is considered

[0123]

[0124] wherein Q and R are weighting matrices. The control input minimizing equation (16) is

[0125] τ0= -Fx = -r -1 B T Px (17)

[0126] where F is controlled by the solution P of the Riccati equation with the feedback gain vector designed by optimization

[0127] PA + A T P-r -1 PBB T P + Q = 0 (18)

[0128] Next, consider the case that the pendulum can rotate several times until it stabilizes to the upright position. To handle this case, we use a new state vector

[0129]

[0130] pendulum angle θ i (i = 0, 1, 2) is always θ i ∈ (-π, π). From the state vector equation (19), the control input is

[0131]

[0132] Nonlinear optimal controller design

[0133] The dynamics equation of the system is written as

[0134]

[0135] where

[0136]

[0137]

[0138]

[0139]

[0140]

[0141]

[0142]

[0143] The state vector is chosen as

[0144]

[0145] The state linear expression is obtained as

[0146]

[0147] where

[0148]

[0149]

[0150] A 12 = diag(cos θ0, cos θ1, cos θ2).

[0151] Consider a quadratic form as a criterion function

[0152]

[0153] where Q s and R s are weighting matrices. The control input that minimizes (24) is

[0154]

[0155] where

[0156]

[0157] where τ0is the system input torque, F s is a nonlinear optimal control feedback gain matrix, and x s is the system nonlinear state vector.

[0158] Those skilled in the art will appreciate that embodiments of the present application can be readily used as a method, a system or a computer program product. Accordingly, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application can take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) embodying computer readable program code.

[0159] The computer program instructions can also be loaded onto a computer, other programmable data processing apparatus, or other devices to cause a series of operational steps to be performed on the computer, other programmable apparatus or other devices to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide processes for implementing the functions specified in the flowchart block or blocks. Figure 1 The flowchart and / or block diagram in the Figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to the present application. In this regard, each block in the flowchart and / or block diagrams can represent a module, segment, or portion of code, which comprises one or more executable Figure 1 The means for functionally implementing each block or steps in the flowchart and / or block diagrams can be implemented by hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. While the

[0160] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the flow Figure 1 one or more flows and / or blocks Figure 1 one or more blocks or multiple blocks.

[0161] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions executed on the computer or other programmable apparatus provide steps for implementing the flow Figure 1 one or more flows and / or blocks Figure 1 one or more blocks or multiple blocks.

[0162] Finally, it should be noted that the above-mentioned embodiments are merely used to illustrate the technical solutions of the present application, rather than limiting the same. Even though the present application has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that the specific embodiments of the present application can be modified or replaced equivalently, and any modification or replacement without departing from the spirit and scope of the present application should be covered within the protection scope of the claims of the present application.

Claims

1. A method for balance control of a rotating two-stage inverted pendulum, the rotating two-stage inverted pendulum comprising a central rotating main rod and two pendulum rods respectively located on both sides of the central rotating main rod, the two pendulum rods being able to follow the rotation of the central rotating main rod to be inverted and to keep balance and stable swing, characterized in that, The specific control method is as follows: A swing control model and a stable swing control model are established for the rotating two-stage inverted pendulum; The central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods; After the swing, the two swing rods are controlled according to the stable swing control model to realize the stable swing; The swing control model adopts an energy method based on a Lyapunov function to realize the control of the swing of the two swing rods, and the specific control formula is: In the formula, τ0 is the system input torque, wherein τ0 is the input torque, f0 is the rotating friction of the connecting rod 0, k is a design parameter, and G is a gravity matrix.

2. The balance control method of a rotating two-stage inverted pendulum according to claim 1, characterized by, Before the central rotating main rod is controlled to realize the swing of the two swing rods, the following steps are further included: The current total energy of the current rotating two-stage inverted pendulum is calculated to determine whether the current total energy exceeds a swing energy threshold; If the swing energy threshold is not exceeded, the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods; If the swing energy threshold is exceeded, the rotational directions of the two swing rods are determined: If the rotational directions of the two swing rods are opposite, the two swing rods are simultaneously controlled to decay in energy until the decay is within the swing threshold, and then the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods; If the rotational directions of the two swing rods are the same, the two swing rods are naturally decayed in energy until the decay is within the swing threshold, and then the central rotating main rod is controlled according to the swing control model to realize the swing of the two swing rods.

3. The balance control method of a rotating two-stage inverted pendulum according to claim 1, characterized by, When the two swing rods meet the following conditions, the two swing rods are controlled according to the stable swing control model to realize the stable swing: |θ1|≤θ 1sw ,|θ2|≤θ 2sw ,|E1|+|E2|≤E sw θ1 and θ2 are the inclination angles of the first and second swing rods, respectively, E1 and E2 are the energies of the first and second swing rods, respectively, E sw is an energy threshold, θ 1sw and θ 2sw are the inclination angle thresholds of the first and second swing rods, respectively.

4. The balance control method of a rotating two-stage inverted pendulum according to claim 1, characterized by, The stable swing control model is an optimal controller model, and the control formula is: where τ0is the system input torque, F is the optimal control feedback gain matrix, is the system state vector.

5. The balance control method of a rotating two-stage inverted pendulum according to claim 1, characterized by, The stable swing control model is a nonlinear optimal controller model, and the control formula is: where τ0is the system input torque, F s is the nonlinear optimal control feedback gain matrix, x s is the system nonlinear state vector.

6. A balance control system for a rotating two-stage inverted pendulum, characterized by comprising: The system includes: A swing control module that controls the central rotating main rod to realize the swing of the two swing rods; A stable swing judgment module that judges whether to perform stable swing control according to the current state of the two swing rods; A stable swing control module that controls the central rotating main rod to realize the stable swing of the two swing rods according to the stable swing judgment of the stable swing judgment module; The swing control module adopts an energy method based on a Lyapunov function to realize the control of the swing of the two swing rods, and the specific control formula is: In the formula, τ0 is the system input torque, wherein τ0 is the input torque, f0 is the rotating friction of the connecting rod 0, k is a design parameter, and G is a gravity matrix.

7. The balance control system of a rotating two-stage inverted pendulum according to claim 6, characterized by, The system further includes: A swing judgment module that judges whether to perform swing control according to the current total energy of the current rotating two-stage inverted pendulum and the current rotational directions of the two swing rods.

8. A storage medium, characterized by The storage medium stores a plurality of instructions, which are suitable for being loaded by the processor to execute the method of any one of claims 1 to 5.

Images