CN101414156A - Dynamic servo control method of under drive mechanical device ACROBOT - Google Patents

Abstract

A dynamic servo control method of an under-actuated mechanical device ACROBOT relates to a dynamic servo control method of an under-actuated mechanical system, and aims at solving the problems of single application field and narrow research field of the existing under-actuated systems. The method comprises the following steps: obtaining instantaneous state quantities of pivot angles of a connecting rod and a pendulum rod from the under-actuated mechanical system to compute a pivot angle error; obtaining a state variable of the current pivot angle of the pendulum rod; obtaining a model time-variant parameter in a feedback control law; obtaining mechanical energy of the under-actuated mechanical system and comparing the mechanical energy with a given value to obtain a target error; and substituting the data into a feedback control law formula to obtain a control torque which is required now, and outputting the control torque to the mechanical system by a motor. In the method, the dynamic servo control is achieved by constructing a Lyapunov function, and a large-scale motion control of the under-actuated system is achieved by constructing a periodic orbit similar to a motion track of a single pendulum which takes the peak of the orbit as the target point, and by stabilization control of a balance point and follow-up control of a special track.

Description

The dynamic servo control method of activation lacking mechanical device ACROBOT

Technical field

The present invention relates to the dynamic servo control method of a kind of activation lacking mechanical system, belong to activation lacking mechanical system motion control technology field.

Background technology

Owing drive system is the special nonlinear system of a class, the control of owing drive system is one of research focus of nonlinear Control, its definition is the number that the number of control input is less than the system architecture state variable, for mechanical system, the number that is equivalent to driving is less than the number of degree of freedom in system.Except that having nonlinear feature, it also has some new characteristics, and as not realizing exact linearization method etc., these characteristics make to its control complicated more.The reason that causes system to owe to drive has a lot, be divided into four classes substantially: system this as owing to drive, as crane, flexible link mechanical arm etc.; For being designed to, specific purpose owes to drive, as twin screws satellite etc.; Some sensor or drives fail cause in the full drive unit; Design for control theory research is artificial, as bat system, inverted pendulum etc.

Owe drive system and compare with full drive system, though owing to drive to reduce and to cause control to become complicated, the minimizing of drive unit can mitigation system weight, reduce energy resource consumption, improve system reliability.These characteristics make it be particularly suitable for the field to energy consumption, weight and reliability sensitivity such as space just.Identical with the popularity of nonlinear system, the expression-form of owing drive system also is diversified, there is not unified expression formula, not suitable fully equally control theory, therefore in the working control process, be difficult to realize that the problem of being concerned about mainly is the tracking of the calm or special track of equilibrium point to owing the grand movement control of drive system.

Summary of the invention

The present invention provides the dynamic servo control method of a kind of activation lacking mechanical device ACROBOT for solving the existing problem that drive system can not realize grand movement control of owing.The present invention includes following steps:

Step 1 obtains the connecting rod pivot angle from the activation lacking mechanical system And swinging angle of swinging rod

The immediate status amount, calculate the pivot angle error $e_{q_2}=q_2^k-q_{2d};$

Step 2, pass through formula:

${\stackrel{\·}{q}}_1\≈\frac{{\mathrm{\Δq}}_1}{\mathrm{\Δt}}=\frac{q_1^k-q_1^{k-1}}{\mathrm{\Δt}},$ ${\stackrel{\·}{q}}_2\≈\frac{{\mathrm{\Δq}}_2}{\mathrm{\Δt}}=\frac{q_2^k-q_2^{k-1}}{\mathrm{\Δt}}$

Obtain the current state variable With

Step 3, pass through formula:

d ₁₁＝θ ₁+θ ₂+2θ ₃?cos?q ₂

d ₁₂＝d ₂₁＝θ ₂+θ ₃?cos?q ₂

d ₂₂＝θ ₂

$h_1=-{\mathrm{\θ}}_3{\stackrel{\·}{q}}_2\sin q_2(2{\stackrel{\·}{q}}_1+{\stackrel{\·}{q}}_2)$

$h_2={\mathrm{\θ}}_3{\stackrel{\·}{q}}_1^2\sin q_2$

φ ₁＝θ ₄g?cosq ₁+θ ₅g?cos(q ₁+q ₂)

φ ₂＝θ ₅g?cos(q ₁+q ₂)

Obtain the model time-varying parameter d in the FEEDBACK CONTROL rule ₁₁, d ₁₂, d ₂₂, h ₁, h ₂,

With

Step 4, pass through formula:

$E=0.5\stackrel{{\·}^T}{q}M\left(q\right)\stackrel{\·}{q}+{\mathrm{\θ}}_{4}g\sin q_1+{\mathrm{\θ}}_{5}g\sin (q_1+q_2)$

In the formula $M=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right];$ $q=\left[\begin{array}{c}{q}_1\\ q_2\end{array}\right]$

Obtain the mechanical energy E of activation lacking mechanical system, with set-point E _d=θ ₄G sin q ₁+ θ ₅G sin (q ₁+ q ₂) relatively obtain target error e _E=E-E _d

Step 5, bring the data that step 1 to step 4 obtains into feedback control law formula:

$\mathrm{\τ}=\frac{-k_V{\stackrel{\·}{q}}_2-k_P{e}_{q_2}-\frac{K_D}{\mathrm{\Δ}}[d_{21}(h_1+{\mathrm{\φ}}_1)-d_{11}(h_2+{\mathrm{\φ}}_2)]}{k_E{e}_E+\frac{k_D{d}_{11}}{\mathrm{\Δ}}}$

Obtain required this moment control moment, and utilize motor to export control moment to mechanical system.

Beneficial effect: control method of the present invention realizes dynamic servo control by structure Lyapunov function, for owing drive unit, expects that it can reach the configuration space any point, does not require that owing drive unit can be still in the impact point place, only requires accessibility; Construct the periodic orbit of similar simple harmonic motion track, the track peak is an impact point, this track can and drive joint rotation angle by system mechanics to be determined, by to the calm control of equilibrium point and the tracking Control of special track, has realized owing the grand movement control of drive system.

Embodiment

Embodiment one: present embodiment is made up of following steps:

Step 1 obtains the connecting rod pivot angle from the activation lacking mechanical system

And swinging angle of swinging rod

The immediate status amount, calculate the pivot angle error $e_{q_2}=q_2^k-q_{2d};$

Step 2, pass through formula:

${\stackrel{\·}{q}}_1\≈\frac{{\mathrm{\Δq}}_1}{\mathrm{\Δt}}=\frac{q_1^k-q_1^{k-1}}{\mathrm{\Δt}},$

${\stackrel{\·}{q}}_2\≈\frac{{\mathrm{\Δq}}_2}{\mathrm{\Δt}}=\frac{q_2^k-q_2^{k-1}}{\mathrm{\Δt}}$

Obtain the current state variable

With

Step 3, pass through formula:

d ₁₁＝θ ₁+θ ₂+2θ ₃?cos?q ₂

d ₁₂＝d ₂₁＝θ ₂+θ ₃?cos?q ₂

d ₂₂＝θ ₂

$h_1=-{\mathrm{\θ}}_3{\stackrel{\·}{q}}_2\sin q_2(2{\stackrel{\·}{q}}_1+{\stackrel{\·}{q}}_2)$

$h_2={\mathrm{\θ}}_3{\stackrel{\·}{q}}_1^2\sin q_2$

φ ₁＝θ ₄g?cos?q ₁+θ ₅g?cos(q ₁+q ₂)

φ ₂＝θ ₅g?cos(q ₁+q ₂)

Obtain the model time-varying parameter d in the FEEDBACK CONTROL rule ₁₁, d ₁₂, d ₂₂, h ₁, h ₂,

With

Step 4, pass through formula:

$E=0.5\stackrel{{\·}^T}{q}M\left(q\right)\stackrel{\·}{q}+{\mathrm{\θ}}_{4}g\sin q_1+{\mathrm{\θ}}_{5}g\sin (q_1+q_2)$

In the formula $M=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right];$ $q=\left[\begin{array}{c}{q}_1\\ q_2\end{array}\right]$

Obtain the mechanical energy E of activation lacking mechanical system, with set-point E _d=θ ₄G sinq ₁+ θ ₅G sin (q ₁+ q ₂) relatively obtain target error e _E=E-E _d

Step 5, bring the data that step 1 to step 4 obtains into feedback control law formula:

$\mathrm{\τ}=\frac{-k_V{\stackrel{\·}{q}}_2-k_P{\stackrel{\·}{e}}_{q_2}-\frac{K_D}{\mathrm{\Δ}}[d_{21}(h_1+{\mathrm{\φ}}_1)-d_{11}(h_2+{\mathrm{\φ}}_2)]}{k_E{e}_E+\frac{k_D{d}_{11}}{\mathrm{\Δ}}}$

Obtain required this moment control moment, and utilize motor to export control moment to mechanical system.Repeat the described step 1 of present embodiment to step 5 and carry out cycle calculations, can realize dynamic servo control, make the output mechanical energy E of system level off to set-point E _d, and the pivot angle error

Level off to zero.

In traditional control practice, the target that is realized is divided into two kinds: the calm control of equilibrium point and the tracking Control of special track, this has greatly limited the application of owing drive system, and present embodiment combines both, realization is to the special tracking Control of given trace, and with this special tracking Control called after dynamic servo control.

The mathematical definition of dynamic servo track following is: for system: $\stackrel{\·}{x}=f(x,u),$ X (t ₀)=x ₀, under control action, for any given trace γ (t) and permissible error δ:

If

Make t〉T ₀In time, have | x (t)-γ (t) | and＜δ sets up, and then is called servo track and follows the tracks of;

If And t _iT ₀, i=1,2,3,,, make | x (t _i)-γ (t _i) |＜δ, then be called the dynamic servo track following.

With discrete point γ (t on it _i) be constraint, so one of the target of dynamic servo control is that system can at a time reach near the set point in the tolerance interval, emphasizes constantly but not is because to be not that the arbitrfary point can be realized calm fully for owing drive system constantly; In addition owing to have limit cycle in the phase plane, the drive system of owing for not being in equilibrium state exists periodic motion in the system, so another target of dynamic servo is that system can periodically arrive given position, the former is called accessibility, and the latter is called repeatability.Provide dynamic servo control definition below:

The mathematical definition of dynamic servo control is: for system $\stackrel{\&CenterDot;}{x}=f(x,u),$ X (t ₀)=x ₀, under the effect of control input u (t), for any set the goal xd and target error δ of giving, t ₂, t ₃..., make $|x\left(t_i\right)-x_d|<\mathrm{\&delta;},i=\mathrm{1,2,3},\&CenterDot;\&CenterDot;\&CenterDot;,$ Claim this dynamic servo control that is controlled to be.

For ACROBOT, its kinetic model is:

$d_{11}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1{+d}_{12}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+h_1+{\mathrm{\&phi;}}_1=0$

$d_{21}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1{+d}_{22}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+h_2+{\mathrm{\&phi;}}_2=\mathrm{\&tau;}$

Wherein:

d ₁₁＝θ ₁+θ ₂+2θ ₃?cos?q ₂

d ₁₂＝d ₂₁＝θ ₂+θ ₃?cos?q ₂ ${\mathrm{\&theta;}}_1=m_1{l}_{c1}^2+m_2{l}_1^2+I_1$

d ₂₂＝θ _2? ${\mathrm{\&theta;}}_2=m_2{l}_{c2}^2+I_2$

$h_1=-{\mathrm{\&theta;}}_3{\stackrel{\&CenterDot;}{q}}_2\sin q_2(2{\stackrel{\&CenterDot;}{q}}_1+{\stackrel{\&CenterDot;}{q}}_2)$ θ ₃＝m ₂l ₁lc ₂

$h_2={\mathrm{\&theta;}}_3\stackrel{\&CenterDot;}{q_1^2}\sin q_2$ θ ₄＝m ₁l _c1+m ₂l ₁

φ ₁＝θ ₄g?cos?q ₁+θ ₅g?cos(q ₁+q ₂) θ ₅＝m ₂l _c2

φ ₂＝θ ₅g?cos(q ₁+q ₂)

τ is the fork input torque; I ₁, I ₂For connecting rod and fork around the barycenter moment of inertia.

For ACROBOT, it has two degree of freedom, and the track of system can be described as: γ (t)=[q ₁(t), q ₂And then be converted into the series of discrete point (t)]:

γ(i)＝[q ₁(t ₁)，q ₂(t ₁)]，[q ₁(t ₂)，q ₂(t ₂)]，[q ₁(t ₃)，q ₂(t ₃)]，...，[q ₁(t _i)，q ₂(t _i)]，...

Therefore, the core of dynamic servo control is reached at the control that realizes any given impact point in the configuration space.

For ACROBOT, owing to the driving character of owing of system, it is impossible stable for the point except that equilibrium point, therefore, and for any set point x _d, system can only " instantaneous static " at x _d, according to definition, system can constantly reach these points when stablizing.Here, suppose t _I+1-t _i=T, i=1,2,3 ..., promptly system will move to target location x every the T time _dNear.Be easy to associate the motion of single pendulum from this characteristics of motion: at certain altitude it is discharged, single pendulum can be swung, if do not consider energy loss, then it can periodically swing to the initial release height.

Similarly, can find in ACROBOT, to work as q ₂=0 o'clock, system capacity and two bar cramp angles no longer changed, and saw that from center of gravity (also can be regarded as end) its behavior is identical with the single pole pendulum, and system can constantly swing along a circular periodic orbit, and orbit radius is by q ₂Decision, the track peak is taken as peak with impact point: q by zero energy E decision _d=[q _1d, q _2d].

The dynamic servo control method that is based on energy that is adopted in the present embodiment.By control system ENERGY E and swinging angle of swinging rod q ₂Realize, therefore need control that as an activation lacking mechanical system, system has only a control input τ, can not realize the complete exact linearization method of input and output and state, perhaps input and output decoupling zero, thereby control more complicated to these two amounts.Solve the few problem of control input and can adopt Lyapunov function of this two variable structures, by single control input this Lyapunov function is controlled, and then realized their coordination control, be called single close-loop control scheme.

Realize the dynamic servo controlled target of ACROBOT system, even system can move along aforementioned periodic orbit, and this periodic orbit is by E and swinging angle of swinging rod q ₂Two variable decisions because system has only a control input, can't directly be controlled these two variablees, are constructed as follows the Lyapunov function of form here:

$V=\frac{1}{2}{k}_E{e}_E^2+\frac{1}{2}{k}_D\stackrel{\&CenterDot;}{q_2^2}+\frac{1}{2}{k}_P{e}_{q^2}^2$

In the formula: e _E=E-E _d, $e_{q_2}=q_2-q_{2d},$ k _E, k _D, k _PBe positive constant.

This Lyapunov function is by system capacity E and swinging angle of swinging rod q ₂And the decision of impact point set-point, and be a positive definite function.As can be seen, V → 0 is equivalent to E → E _dAnd q ₂→ q _2dTherefore, only need realize that control gets final product to this Lyapunov function.

Note Δ=d ₁₁d ₂₂-d ₂₁d ₁₂〉=0, ρ=d ₁₁/ Δ〉0

ρ ^*＝min $\mathrm{\&rho;}=\min \frac{{\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2+2{\mathrm{\&theta;}}_3\cos q_2}{{\mathrm{\&theta;}}_1{\mathrm{\&theta;}}_2-{\left({\mathrm{\&theta;}}_3\cos q_2\right)}^2}>0$

Note E _TopSystem is had potential energy by vertically upward the time.

According to the Lyapunov stability theory, design as follows:

When satisfying condition: k _D2k _EE _Top/ ρ ^*The time, restrain in FEEDBACK CONTROL:

$\mathrm{\&tau;}=\frac{-k_V{\stackrel{\&CenterDot;}{q}}_2-k_P{e}_{q_2}-\frac{K_D}{\mathrm{\&Delta;}}[d_{21}(h_1+{\mathrm{\&phi;}}_1)-d_{11}(h_2+{\mathrm{\&phi;}}_2)]}{k_E{e}_E+\frac{k_D{d}_{11}}{\mathrm{\&Delta;}}}$

Effect under, the Lyapunov function convergence, corresponding states converges to:

e _E→0； ${e_q}_2\&RightArrow;0;$ ${\stackrel{\&CenterDot;}{q}}_2\&RightArrow;0.$

Claims (2)

1, the dynamic servo control method of activation lacking mechanical device ACROBOT is characterized in that it may further comprise the steps:

Step 1 obtains the connecting rod pivot angle from the activation lacking mechanical system

And swinging angle of swinging rod

The immediate status amount, calculate the pivot angle error $e_{q_2}=q_2^k-q_{2d};$

Step 2, pass through formula:

${\stackrel{\&CenterDot;}{q}}_1\&ap;\frac{{\mathrm{\&Delta;q}}_1}{\mathrm{\&Delta;t}}=\frac{q_1^k-q_1^{k-1}}{\mathrm{\&Delta;t}},{\stackrel{\&CenterDot;}{q}}_2\&ap;\frac{{\mathrm{\&Delta;q}}_2}{\mathrm{\&Delta;t}}=\frac{q_2^k-q_2^{k-1}}{\mathrm{\&Delta;t}}$

Obtain the current state variable

With

Step 3, pass through formula:

d ₁₁＝θ ₁+θ ₂+2θ ₃?cos?q ₂

d ₁₂＝d ₂₁＝θ ₂+θ ₃?cos?q ₂

d ₂₂＝θ ₂

$h_1=-{\mathrm{\&theta;}}_3{\stackrel{\&CenterDot;}{q}}_2\sin q_2(2{\stackrel{\&CenterDot;}{q}}_1+{\stackrel{\&CenterDot;}{q}}_2)$

$h_2={\mathrm{\&theta;}}_3{\stackrel{\&CenterDot;}{q}}_1^2\sin q_2$

φ ₁＝θ ₄g?cos?q ₁+θ ₅g?cos(q ₁+q ₂)

φ ₂＝θ ₅g?cos(q ₁+q ₂)

Obtain the model time-varying parameter d in the FEEDBACK CONTROL rule ₁₁, d ₁₂, d ₂₂, h ₁, h ₂,

With

Step 4, pass through formula:

$E=0.5{\stackrel{\&CenterDot;}{q}}^{T}M\left(q\right)\stackrel{\&CenterDot;}{q}+{\mathrm{\&theta;}}_4\stackrel{\&CenterDot;}{g}\sin q_1+{\mathrm{\&theta;}}_{5}g\sin (q_1+q_2)$

In the formula $M=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right];q=\left[\begin{array}{c}{q}_1\\ q_2\end{array}\right]$

Obtain the mechanical energy E of activation lacking mechanical system, with set-point E _d=θ ₄G sin q ₁+ θ ₅G sin (q ₁+ q ₂) relatively obtain target error e _E=E-E _d

Step 5, bring the data that step 1 to step 4 obtains into feedback control law formula:

$\mathrm{\&tau;}=\frac{-k_V{\stackrel{\&CenterDot;}{q}}_2-k_P{e}_{q_2}-\frac{k_D}{\mathrm{\&Delta;}}[d_{21}(h_1+{\mathrm{\&phi;}}_1)-d_{11}(h_2+{\mathrm{\&phi;}}_2)]}{k_E{e}_E+\frac{k_D{d}_{11}}{\mathrm{\&Delta;}}}$

Obtain required this moment control moment, and utilize motor to export control moment to mechanical system.

According to the dynamic servo control method of power 1 described activation lacking mechanical device ACROBOT, it is characterized in that 2, circulation execution in step one to step 5 realizes dynamic servo control, make the output mechanical energy E of system level off to set-point E _d, and the pivot angle error

Level off to zero.