CN105912007A - Differential geometry nonlinear control method of spatial mechanical arm anti-interference attitude stabilization - Google Patents

Abstract

The invention discloses a differential geometry nonlinear control method of spatial mechanical arm anti-interference attitude stabilization. A differential geometry theory is used to carry out linearization conversion on a nonlinear attitude dynamics model so as to acquire a kind of linear space form. And then LQR is used to carry out controller design on a linearized attitude stabilization system so as to acquire a non-linear control law. A spatial mechanical arm attitude stabilization controller based on a differential geometry theory and an LQR control method can guarantee robustness stability of a system to various kinds of uncertainties and a non-cooperative interference, a simple linearity control theory is adopted, a control structure is simple, a calculated amount is reduced and resources on a satellite is saved so that the method is especially suitable for a spatial mechanical arm attitude stabilization task which needs real-time calculation and has a limited calculating capability.

Description

The Differential Geometry nonlinear control method of the anti-interference attitude stabilization of space manipulator

[technical field]

The present invention relates to the Differential Geometry nonlinear control method of the anti-interference attitude stabilization of a kind of space manipulator, belong to boat It device control field.

[background technology]

The 26S Proteasome Structure and Function of spacecraft is increasingly sophisticated, and the development of space technology makes the new space tasks such as operation in-orbit become For may, capture in-orbit, keep in repair, assemble, module replacing, fuel adding these operate in-orbit service be space technology develop new Focus.Yet with spatial environments, there is the severe factor such as extreme temperature, vacuum, radiation, weightlessness, mankind's extravehicular activity risk Greatly, thus with the space platform of mechanical arm to implement the effect during spatial operation the most notable.At present, from machinery The development trend of the space platform of arm is it can be seen that operation target is developed to noncooperative target by cooperation, and task is by simple target Capture and develop to target accurate operation.Therefore, the control technology of spacecraft is had higher requirement by these changes, controls System must solve uncertainty, the problem such as strong nonlinearity and real-time simultaneously.This is because space manipulator is being carried out During Control System Design and demonstration, need mathematical model accurate, simple.But in space, space manipulator base The pose of seat is not fixing, and the motion of mechanical arm couples with also existing between the motion of pedestal, the position of pedestal and attitudes vibration Influencing whether the pose of mechanical arm tail end, in turn, the motor-driven of mechanical arm also can produce impact to the pose of pedestal.Further, by Equipment requirements light weight in space, volume are little, it is low to consume energy, and space manipulator often uses light material, elongate rod, subtracts greatly Speed ratio, has relatively strong flexible.The feature of space manipulator determines it and has the strongest non-linear and complicated coupling；With Time, in spatial environments, some parameter is difficult to accurately obtain, and the change of parameter causes system model to there is strong uncertainty. Trace it to its cause, mainly include that the Aerospace Satellite pedestal inertial parameter that fuel consumption causes changes, measures signal noise and arrest Position disturbance moment produced by it etc. after noncooperative target.In addition, the non-coplanar force gradient moment that do not models, solar array The existence of the factors such as optical pressure so that system by extra disturbance torque, the undesirable condition such as the friction in joint, flexibility in addition, These factors all will affect the action effect of control method.Thus, conventional linear system control method will be no longer appropriate for such control System processed design, and be similar to adaptive control algorithm, sliding mode control algorithm general nonlinearity control method also will because of control Device structure processed is complicated and is not suitable for engineering reality.Therefore needing research can overcome non-linear, the close coupling of mechanical arm, suppression is each Kind of interference, uncertain, be easy to the nonlinear robust control algorithm of Project Realization.

Owing to space manipulator is during arresting noncooperative target, to Aerospace Satellite platform stance and each joint angles Control accuracy require the highest, therefore can not that traditional method be utilized to ignore high-order be non-as spacecraft relative orbit controls Linear term carries out linearisation, but accurate linear transfor method must be used to obtain Controlling model, thus ensure spacecraft with Joint of mechanical arm relative Attitude Control for Spacecraft accurate.In recent years in research nonlinear system process, Isidori is successfully by differential Geometric theory is applied to the modeling of nonlinear control system, controls and analyze central.Owing to differential geometric theory can be to non-thread Sexual system carries out accurate linearisation, and therefore it can be greatly reduced local linearization or microvariations assume that this tradition is non-linear The limitation of system processing method.It is to say, differential geometric theory is greatly expanded the application model of linear control theory method Enclose.

To sum up, for ensureing that Space Manipulator System is arresting the assembly posture formed in noncooperative target process Stablize, meet to system rejection to disturbance ability, the computing capability of spaceborne computer and the high request of control accuracy, need research Differential geometric theory is combined with ripe linear control theory, non-to design the space manipulator attitude stabilization of simple in construction Linear robust controller.

[summary of the invention]

When space manipulator arrests noncooperative target formation assembly system, in order to overcome the deficiencies in the prior art, keep away Exempt from tradition nonlinear control method structure the most complicated, be difficult to Project Realization and the bigger problem of amount of calculation, it is considered to have not In the presence of determining factor and various interference, the differential that the present invention proposes the anti-interference attitude stabilization of a kind of space manipulator is several What nonlinear control method, obtains a kind of form Nonlinear control law simple, jamproof.

For reaching above-mentioned purpose, the present invention is achieved by the following technical solutions:

The Differential Geometry nonlinear control method of the anti-interference attitude stabilization of space manipulator, comprises the following steps:

Step one, set up free flight space manipulator affine nonlinear state-space model

By attitude kinematics equations and the system total kinetic energy equation of space manipulator, substitute into Lagrange equation, obtain The attitude dynamics model of space manipulator is:

$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}---\left(1\right)$

In formula:

$C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\stackrel{\·}{M}\stackrel{\·}{\mathrm{\θ}}-\frac{\∂}{\∂\mathrm{\θ}}\[\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}\]---\left(2\right)$

Wherein, θ is space manipulator attitude angle under inertial coodinate system,For space manipulator under inertial coodinate system Attitude angular velocity；M (θ) is the generalized mass matrix of Space Manipulator System, is the matrix function of space manipulator attitude angle；

Choose state variable

$x={\left[\begin{array}{cccccc}{\mathrm{\θ}}_0& {\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\θ}}_1& {\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\θ}}_2& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]}^T$

Input variable and output variable

U=[τ₀ τ₁ τ₂]^T, y=[θ₀ θ₁ θ₂]^T

The state-space expression obtaining Space Manipulator System is:

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_4\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ g_{21}& g_{22}& g_{23}\\ 0& 0& 0\\ g_{41}& g_{42}& g_{43}\\ 0& 0& 0\\ g_{61}& g_{62}& g_{63}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right\}$

$\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_0\\ {\mathrm{\θ}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\mathrm{\θ}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(3\right)$

In formula:

$\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=-M^{-1}\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]+\frac{1}{2}{M}^{-1}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

$\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]=M^{-1}---\left(4\right)$

Note

$f=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right],g_1=\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right],g_2=\left[\begin{array}{c}0\\ g_{22}\\ 0\\ g_{42}\\ 0\\ g_{62}\end{array}\right],g_3=\left[\begin{array}{c}0\\ g_{23}\\ 0\\ g_{43}\\ 0\\ g_{63}\end{array}\right]---\left(5\right)$

h₁=θ₀,h₂=θ₁,h₃=θ₂

Then obtain the affine nonlinear state space form of Space Manipulator System

$\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}---\left(6\right)$

Step 2, differential geometric theory is utilized to set up the attitude dynamics model of class linear space form

Design following state feedback control law

$\begin{array}{c}u=\mathrm{\α}\left(x\right)+\mathrm{\β}\left(x\right)v\\ =A^{-1}\left(x\right)\[-b\left(x\right)+v\]\end{array}---\left(7\right)$

Wherein

$A^{-1}\left(x\right)={\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]}^{-1}=M\left(x\right)$

$b\left(x\right)=\left[\begin{array}{c}{L}_f^2{h}_1\\ L_f^2{h}_2\\ L_f^2{h}_3\end{array}\right]=\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]---\left(8\right)$

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

Formula (7) (8) is substituted into (6), and the linear space expression-form obtaining space manipulator attitude dynamics model is:

$\begin{array}{c}\stackrel{\·}{x}=Ax+bv\\ y=cx\end{array}---\left(9\right)$

Wherein,Being referred to as Lie derivatives, calculate the factor for one, v is linear system (9) control law designed by, u control law designed by actual nonlinear system (6)；

Step 3, LQR controller design based on linear space system

Linear system (9) is considered as

$\left\{\begin{array}{c}\stackrel{\·}{x}=\tilde{A}x+\tilde{B}v+\tilde{B}d\\ y=\tilde{C}x+\tilde{D}v\end{array}\right.$

Then control law based on Linear-Quadratic Problem optimal regulation problem design lines sexual system is

$v=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)---\left(10\right)$

In formula, R is the weighting matrix in optimum control performance indications to input variable, and P is for meeting following Riccati algebraically Non trivial solution matrix,

PA+A^TP-PBR^-1B^TP+Q=0

Finally give and based on differential geometric spacecraft nonlinear attitude tracking control unit be

U=α (x)+β (x) ν (11)

In formula:

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

$v_{LQR}=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)$

The present invention is further improved by:

In described step one, the attitude dynamic equations of space manipulator is obtained by following steps:

The body series OX of definition space mechanical arm_BY_BZ_BInitial point be space manipulator pedestal barycenter O, OX_B,OY_B,OZ_BThree Axle is fixed on space manipulator base body, and consistent with the principal axis of inertia respectively；

The kinematic relation of doublejointed mechanical arm system is

$\begin{array}{ccc}{\mathrm{\ω}}_0={\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\ω}}_1={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\ω}}_2={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2\end{array}$

$v_{1x}={\stackrel{\·}{r}}_{1x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)$

$v_{1y}={\stackrel{\·}{r}}_{1y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)---\left(12\right)$

$v_{2x}={\stackrel{\·}{r}}_{2x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)-a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)$

$v_{2y}={\stackrel{\·}{r}}_{2y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)+a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)$

Note

$\begin{array}{ccc}{s}_0={\mathrm{sin\θ}}_0& s_{01}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& s_{012}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ c_0={\mathrm{cos\θ}}_0& c_{0a}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& c_{012}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}---\left(13\right)$

Then system total kinetic energy is

$T=\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}=\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\left[\begin{array}{ccc}{M}_{11}& M_{12}& M_{13}\\ M_{21}& M_{22}& M_{23}\\ M_{31}& M_{32}& M_{33}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(14\right)$

Wherein,

M₃₃=S_cc

M₂₃=M₃₂=M₃₃+S_bcc₂

M₁₃=M₃₁=M₂₃+S_acc₁₂

M₂₂=M₂₃+S_bb+S_bcc₂

M₁₂=M₂₁=M₂₂+S_abc₁+S_acc₁₂

M₁₂=M₁₂+S_aa+S_abc₁+S_acc₁₂

$S_{aa}=\frac{1}{M}{m}_0(m_1+m_2)b_0^2+I_0---\left(15\right)$

$S_{bb}=\frac{1}{M}\[m_0(m_1+m_2)a_1^2+m_2(m_0+m_1)b_1^2+2m_0{m}_1{b}_1\]+I_1$

$S_{cc}=\frac{1}{M}{m}_2(m_0+m_2)a_2^2+I_2$

$S_{ab}=\frac{1}{M}{m}_0\[(m_1+m_2)a_1+m_2{b}_1\]b_0$

$S_{ac}=\frac{1}{M}{m}_0{m}_2{b}_0{a}_2$

$S_{bc}=\frac{1}{M}{m}_2\[(m_0+m_1)b_1+m_0{a}_1\]a_2$

(14) formula is substituted into Lagrange equation, obtains the kinetics equation (1) of system；

$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}$

In formula, θ=[θ₀ θ₁ θ₂]^TIt is space manipulator pedestal and the attitude angle in two joints, τ=[τ₀ τ₁ τ₂]^TIt is Space manipulator pedestal and the control moment in two joints.

The Differential Geometry nonlinear Control side of the anti-interference attitude stabilization of space manipulator the most according to claim 1 Method, it is characterised in that in described step 2, differential geometric theory carries out accurate linearization process and comprises the following steps:

Calculate corresponding Lie derivative, obtain matrix

$A\left(x\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f{h}_1& L_{g_2}{L}_f{h}_1& L_{g_3}{L}_f{h}_1\\ L_{g_1}{L}_f{h}_2& L_{g_2}{L}_f{h}_2& L_{g_3}{L}_f{h}_2\\ L_{g_1}{L}_f{h}_3& L_{g_2}{L}_f{h}_3& L_{g_3}{L}_f{h}_3\end{array}\right]=\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]---\left(16\right)$

Nonsingular, therefore system Relative order r₁=r₂=r₃=2, i.e. r=r₁+r₂+r₃=6=n, n are system dimension, therefore empty Room machine arm system meets the necessary and sufficient condition of exact linearization method in differential geometric theory；

Owing to the expression formula (6) of system has been typical non linear system state space canonical form, i.e. Brunowsky marks Pseudotype:

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_1^i={\mathrm{\ξ}}_2^i\\ {\stackrel{\·}{\mathrm{\ξ}}}_1^i=b_i\left(\mathrm{\ξ}\right)+\underset{j=1}{\overset{3}{\Σ}}{a}_{ij}\left(\mathrm{\ξ}\right)u_j\\ y_i={\mathrm{\ξ}}_1^i\end{array}---\left(17\right)$

Wherein

$a_{\mathrm{ij}}\left(\mathrm{\ξ}\right)=L_{g_i}{L}_f{h}_i\left({\mathrm{\φ}}^{-1}\left(\mathrm{\ξ}\right)\right),1\≤i\≤\mathrm{2,1}\≤j\≤3$

$b_i\left(\mathrm{\ξ}\right)=L_f^2{h}_i\left({\mathrm{\φ}}^{-1}\right(\mathrm{\ξ}\left)\right),1\≤i\≤2$

The state feedback control law of design formula (7):

U=α (x)+β (x) ν

=A^-1(x)[-b(x)+ν]

State feedback control law is substituted into the space after Space Manipulator System kinetic model (6) obtains exact linearization method Manipulator Dynamic is:

$\begin{array}{c}\stackrel{\·}{x}=A_{L}x+b_{L}v\\ y=c_{L}x\end{array}---\left(18\right)$

Wherein,

$A_L=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right],b_L=\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right],c_L={\left[\begin{array}{c}1\\ 0\\ 1\\ 0\\ 1\\ 0\end{array}\right]}^T.$

Compared with prior art, the method have the advantages that

Space manipulator of the present invention is after arresting noncooperative target formation assembly system, owing to noncooperative target may go out The non-cooperation of existing the unknown is motor-driven, and system has stronger uncertainty and the problem such as non-linear, preferably uses the optimum of strong robustness Control to be controlled rule design, to ensure the monolithic stability of assembly system.Based on differential geometric theory and LQR control method Space manipulator pose stabilization control device not only can ensure that system robust stability in the case of strong jamming, and have employed Relatively simple linear control theory, control structure is easy, reduces amount of calculation, saves resource on star, is therefore particularly suitable for needing Real-time resolving and the limited space manipulator of computing capability is wanted to arrest task.This invention simultaneously also extends to be managed based on Linear Control The application in nonlinear system of the control method of opinion, provides a kind of new thinking for such problem.

[accompanying drawing explanation]

Fig. 1 is doublejointed Space Manipulator System structural representation in the present invention；

Fig. 2 is system symbol and coordinate system schematic diagram in the present invention；

Fig. 3 is the structural representation of system controller in the present invention；

Fig. 4-1 to Fig. 4-5 draws mechanical arm anti-interference attitude stabilization Differential Geometry non-thread in application space in the case of partially for parameter The attitude stabilization result figure of property control method.

After Fig. 5-1 to Fig. 5-5 is for arresting noncooperative target formation assembly system, the anti-interference attitude of application space mechanical arm Stablize the attitude stabilization result figure of Differential Geometry nonlinear control method.

Fig. 6-1 to Fig. 6-3 is that space manipulator anti-interference attitude stabilization Differential Geometry nonlinear control method is to random class The attitude stabilization result figure of type non-cooperation interference.

Fig. 7-1 to Fig. 7-3 is that space manipulator anti-interference attitude stabilization Differential Geometry nonlinear control method is to pulse class The attitude stabilization result figure of type non-cooperation interference.

Fig. 8-1 to Fig. 8-3 is that space manipulator anti-interference attitude stabilization Differential Geometry nonlinear control method is to time-varying class The attitude stabilization result figure of type non-cooperation interference.

Fig. 9-1 to Fig. 9-3 is that space manipulator anti-interference attitude stabilization Differential Geometry nonlinear control method is to constant value class The attitude stabilization result figure of type non-cooperation interference.

[detailed description of the invention]

Below in conjunction with the accompanying drawings the present invention is described in further detail:

See Fig. 1-Fig. 9, the nonlinear control method of the anti-interference attitude stabilization of space manipulator of the present invention, its concrete steps Including:

Step one, set up free flight space manipulator affine nonlinear state-space model

Free flight refers to, while controlling space manipulator motion, only be controlled the attitude of pedestal, and the most right Affect a kind of operational mode that less base position is controlled.In order to keep normal under conditions of not consuming multi fuel Communication and solar array Direct to the sun, must make system operate under free flight pattern.Owing to space equipment is in normal manipulation Under there is weightlessness, space run mechanical arm system all can be equivalent to planar mechanical motion, therefore design controller time, With reference to the plane two connecting rod free flight Space Manipulator System shown in Fig. 1.

Space Manipulator System coordinate system schematic diagram as shown in Figure 2, the body series OX of definition space mechanical arm_BY_BZ_B's Initial point is space manipulator pedestal barycenter O, OX_B,OY_B,OZ_BThree axles are fixed on space manipulator base body, and respectively with used Property main shaft consistent, we obtain the kinematic relation of plane two connecting rod mechanical arm system:

$\begin{array}{ccc}{\mathrm{\ω}}_0={\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\ω}}_1={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\ω}}_2={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2\end{array}$

$v_{1x}={\stackrel{\·}{r}}_{1x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)$

$v_{1y}={\stackrel{\·}{r}}_{1y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)---\left(19\right)$

$v_{2x}={\stackrel{\·}{r}}_{2x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)-a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)$

$v_{2y}={\stackrel{\·}{r}}_{2y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)+a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)$

Note

$\begin{array}{ccc}{s}_0={\mathrm{sin\θ}}_0& s_{01}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& s_{012}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ c_0={\mathrm{cos\θ}}_0& c_{0a}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& c_{012}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}---\left(20\right)$

Then system total kinetic energy is

$T=\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}=\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\left[\begin{array}{ccc}{M}_{11}& M_{12}& M_{13}\\ M_{21}& M_{22}& M_{23}\\ M_{31}& M_{32}& M_{33}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(21\right)$

Wherein,

M₃₃=S_cc

M₂₃=M₃₂=M₃₃+S_bcc₂

M₁₃=M₃₁=M₂₃+S_acc₁₂

M₂₂=M₂₃+S_bb+S_bcc₂

M₁₂=M₂₁=M₂₂+S_abc₁+S_acc₁₂

M₁₂=M₁₂+S_aa+S_abc₁+S_acc₁₂

$S_{aa}=\frac{1}{M}{m}_0(m_1+m_2)b_0^2+I_0---\left(22\right)$

$S_{bb}=\frac{1}{M}\[m_0(m_1+m_2)a_1^2+m_2(m_0+m_1)b_1^2+2m_0{m}_1{b}_1\]+I_1$

$S_{cc}=\frac{1}{M}{m}_2(m_0+m_2)a_2^2+I_2$

$S_{ab}=\frac{1}{M}{m}_0\[(m_1+m_2)a_1+m_2{b}_1\]b_0$

$S_{ac}=\frac{1}{M}{m}_0{m}_2{b}_0{a}_2$

$S_{bc}=\frac{1}{M}{m}_2\[(m_0+m_1)b_1+m_0{a}_1\]a_2$

(21) formula is substituted into Lagrange equation, obtains the kinetics equation (23) of system.

$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}---\left(24\right)$

In formula,

$C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\stackrel{\·}{M}\stackrel{\·}{\mathrm{\θ}}-\frac{\∂}{\∂\mathrm{\θ}}\[\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}\]---\left(25\right)$

Wherein, θ=[θ₀ θ₁ θ₂]^TFor space manipulator attitude angle under inertial coodinate system, θ₀, θ₁, θ₂It is empty respectively Room machine arm pedestal and the attitude angle in two joints,For space manipulator attitude angular velocity under inertial coodinate system；M(θ) For the generalized mass matrix of Space Manipulator System, it is the matrix function of space manipulator attitude angle, τ=[τ₀ τ₁ τ₂]^TIt it is space Mechanical arm pedestal and the control moment in two joints.

Choose state variable

$x={\left[\begin{array}{cccccc}{\mathrm{\θ}}_0& {\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\θ}}_1& {\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\θ}}_2& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]}^T---\left(26\right)$

Input variable and output variable

U=[τ₀ τ₁ τ₂]^T, y=[θ₀ θ₁ θ₂]^T(27) state obtaining Space Manipulator System is empty Between expression formula (28) be:

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_4\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ g_{21}& g_{22}& g_{23}\\ 0& 0& 0\\ g_{41}& g_{42}& g_{43}\\ 0& 0& 0\\ g_{61}& g_{62}& g_{63}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right\}$

$\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_0\\ {\mathrm{\θ}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\mathrm{\θ}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(28\right)$

In formula:

$\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=-M^{-1}\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]+\frac{1}{2}{M}^{-1}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

$\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]=M^{-1}---\left(29\right)$

Note

$f=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right],g_1=\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right],g_2=\left[\begin{array}{c}0\\ g_{22}\\ 0\\ g_{42}\\ 0\\ g_{62}\end{array}\right],g_3=\left[\begin{array}{c}0\\ g_{23}\\ 0\\ g_{43}\\ 0\\ g_{63}\end{array}\right]---\left(30\right)$

h₁=θ₀,h₂=θ₁,h₃=θ₂

Then obtain the affine nonlinear state space form of Space Manipulator System

$\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}---\left(31\right)$

Step 2, differential geometric theory is utilized to set up the attitude dynamics model of class linear space form

Meet filling of differential geometric theory exact linearization method firstly the need of clarifying space mechanical arm attitude dynamics model to want Condition.

Theorem 1 is for the nonlinear system shown in (31) formula, if it is at x₀Place has Relative order, namely at x₀At Dian Be in poised state, then the necessary and sufficient condition using feedback of status to carry out exact linearization method is: total Relative order of systemDeng Dimension n in system.

Prove: system dimension n=6, m=3.Take n₁=n₂=n₃=2

Calculate Lie derivative

$\begin{array}{c}{\mathrm{ad}}_f{g}_1\left(x\right)=\frac{\∂g_1}{\∂x}f-\frac{\∂f}{\∂x}{g}_1\\ =\left(\frac{\∂}{\∂x}\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right]\right)\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right]+\left(\frac{\∂}{\∂x}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right]\right)\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right]\\ =\left[\begin{array}{c}0\\ {\left(\frac{\∂g_{21}}{\∂x}\right)}^{T}f\\ 0\\ {\left(\frac{\∂g_{41}}{\∂x}\right)}^{T}f\\ 0\\ {\left(\frac{\∂g_{61}}{\∂x}\right)}^{T}f\end{array}\right]-\left[\begin{array}{c}{g}_{21}\\ {\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_1\\ g_{41}\\ {\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_1\\ g_{61}\\ {\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_1\end{array}\right]=\left[\begin{array}{c}-g_{21}\\ {\left(\frac{\∂g_{21}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_1\\ -g_{41}\\ {\left(\frac{\∂g_{41}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_1\\ -g_{61}\\ {\left(\frac{\∂g_{61}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_1\end{array}\right]\end{array}$

${\mathrm{ad}}_f{g}_2\left(x\right)=\frac{\∂g_2}{\∂x}f-\frac{\∂f}{\∂x}{g}_2=\left[\begin{array}{c}-g_{22}\\ {\left(\frac{\∂g_{22}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_2\\ -g_{42}\\ {\left(\frac{\∂g_{42}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_2\\ -g_{62}\\ {\left(\frac{\∂g_{62}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_2\end{array}\right]$

${\mathrm{ad}}_f{g}_3\left(x\right)=\frac{\∂g_3}{\∂x}f-\frac{\∂f}{\∂x}{g}_3=\left[\begin{array}{c}-g_{23}\\ {\left(\frac{\∂g_{23}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_3\\ -g_{43}\\ {\left(\frac{\∂g_{43}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_3\\ -g_{63}\\ {\left(\frac{\∂g_{63}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_3\end{array}\right]$

Then matrix

$\begin{array}{c}\left[\begin{array}{cccccc}{g}_1& g_2& g_3& {\mathrm{ad}}_f{g}_1& {\mathrm{ad}}_f{g}_2& {\mathrm{ad}}_f{g}_3\end{array}\right]\\ =\left[\begin{array}{cccccc}0& 0& 0& -g_{21}& -g_{22}& -g_{23}\\ g_{21}& g_{22}& g_{23}& {\left(\frac{\∂g_{21}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{22}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{23}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_3\\ 0& 0& 0& -g_{41}& -g_{42}& -g_{43}\\ g_{41}& g_{42}& g_{43}& {\left(\frac{\∂g_{41}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{42}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{43}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_3\\ 0& 0& 0& -g_{61}& -g_{62}& -g_{63}\\ g_{61}& g_{62}& g_{63}& {\left(\frac{\∂g_{61}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{62}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{63}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_3\end{array}\right]\end{array}$

Due toNonsingular, i.e..Therefore

$\begin{array}{c}rank\left[\begin{array}{cccccc}{g}_1& g_2& g_3& {\mathrm{ad}}_f{g}_1& {\mathrm{ad}}_f{g}_2& {\mathrm{ad}}_f{g}_3\end{array}\right]\\ =rank\left[\begin{array}{cccccc}{g}_{21}& g_{22}& g_{23}& {\left(\frac{\∂g_{21}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{22}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{23}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_2}{\∂x}\right)}^T{g}_3\\ g_{41}& g_{42}& g_{43}& {\left(\frac{\∂g_{41}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{42}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_4}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{43}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_3\\ g_{61}& g_{62}& g_{63}& {\left(\frac{\∂g_{61}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_1& {\left(\frac{\∂g_{62}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_2& {\left(\frac{\∂g_{63}}{\∂x}\right)}^{T}f-{\left(\frac{\∂f_6}{\∂x}\right)}^T{g}_3\\ 0& 0& 0& -g_{21}& -g_{22}& -g_{23}\\ 0& 0& 0& -g_{41}& -g_{42}& -g_{43}\\ 0& 0& 0& -g_{61}& -g_{62}& -g_{63}\end{array}\right]\\ =3+3=6=n\end{array}$

This nonlinear system i.e. meets the necessary and sufficient condition of theorem 1, and card is finished.

Therefore can calculate corresponding Lie derivative, obtain matrix

$A\left(x\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f{h}_1& L_{g_2}{L}_f{h}_1& L_{g_3}{L}_f{h}_1\\ L_{g_1}{L}_f{h}_2& L_{g_2}{L}_f{h}_2& L_{g_3}{L}_f{h}_2\\ L_{g_1}{L}_f{h}_3& L_{g_2}{L}_f{h}_3& L_{g_3}{L}_f{h}_3\end{array}\right]=\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]---\left(32\right)$

Nonsingular, therefore system Relative order r₁=r₂=r₃=2, i.e. r=r₁+r₂+r₃=6=n.

Owing to the expression formula (31) of system has been that (Brunowsky marks typical non linear system state space canonical form Pseudotype):

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_1^i={\mathrm{\ξ}}_2^i\\ {\stackrel{\·}{\mathrm{\ξ}}}_1^i=b_i\left(\mathrm{\ξ}\right)+\underset{j=1}{\overset{3}{\Σ}}{a}_{ij}\left(\mathrm{\ξ}\right)u_j\\ y_i={\mathrm{\ξ}}_1^i\end{array}---\left(33\right)$

Wherein

$a_{\mathrm{ij}}\left(\mathrm{\ξ}\right)=L_{g_i}{L}_f{h}_i\left({\mathrm{\φ}}^{-1}\left(\mathrm{\ξ}\right)\right),1\≤i\≤\mathrm{2,1}\≤j\≤3$

$b_i\left(\mathrm{\ξ}\right)=L_f^2{h}_i\left({\mathrm{\φ}}^{-1}\right(\mathrm{\ξ}\left)\right),1\≤i\≤2$

It is referred to as Lie derivatives, calculates the factor for one.

The most here it is made without differomorphism to map, the state feedback control law of design formula (34)

$\begin{array}{c}u=\mathrm{\α}\left(x\right)+\mathrm{\β}\left(x\right)v\\ =A^{-1}\left(x\right)\[-b\left(x\right)+v\]\end{array}---\left(34\right)$

Wherein

$A^{-1}\left(x\right)={\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]}^{-1}=M\left(x\right)$

$b\left(x\right)=\left[\begin{array}{c}{L}_f^2{h}_1\\ L_f^2{h}_2\\ L_f^2{h}_3\end{array}\right]=\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]---\left(35\right)$

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

So, apply the nonlinear system of feedback of status, be achieved that exact linearization method.State feedback control law is substituted into Space manipulator kinetic model after Space Manipulator System kinetic model (31) obtains exact linearization method is

$\begin{array}{c}\stackrel{\·}{x}=A_{L}x+b_{L}v\\ y=c_{L}x\end{array}---\left(36\right)$

Wherein,

$A_L=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right],b_L=\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right],c_L={\left[\begin{array}{c}1\\ 0\\ 1\\ 0\\ 1\\ 0\end{array}\right]}^T$

V is the control law designed by linear system (36), u control designed by actual nonlinear system (31) Rule.

Step 3, LQR controller design based on linear space system

Linear system (36) is considered as

$\left\{\begin{array}{c}\stackrel{\·}{x}=\tilde{A}x+\tilde{B}v+\tilde{B}d\\ y=\tilde{C}x+\tilde{D}v\end{array}\right.$

Then control law based on Linear-Quadratic Problem optimal regulation problem design lines sexual system is

$v=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)---\left(37\right)$

In formula, R is the weighting matrix in optimum control performance indications to input variable, and P is for meeting following Riccati algebraically Non trivial solution matrix,

PA+A^TP-PBR^-1B^TP+Q=0

Finally give and based on differential geometric spacecraft nonlinear attitude tracking control unit be

U=α (x)+β (x) ν (38)

In formula:

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

$v_{LQR}=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)$

The structural representation of system controller is as shown in Figure 3.

The case verification of the inventive method:

Assuming doublejointed Space Manipulator System, its structure chart is as it is shown in figure 1, nominal parameters such as table 1 shows

Table 1 plane two connecting rod Space Manipulator System nominal parameters

Parameter	Numerical value	Parameter	Numerical value
				m0	600kg	a1	0.5m
m1	10kg	a2	0.5m
				m2	20kg	b0	0.5m
I0	50kgm2	b1	0.5m
				I1	0.83kgm2	b2	0.5m
I2	1.67kgm2

Designing controller according to this nominal model, choosing LQR parameter matrix is

$Q=\left[\begin{array}{ccc}{Q}_1& 0& 0\\ 0& Q_2& 0\\ 0& 0& Q_3\end{array}\right]Q_1=\left[\begin{array}{cc}50& 0\\ 0& 10\end{array}\right]Q_2=\left[\begin{array}{cc}100& 0\\ 0& 50\end{array}\right]Q_3=\left[\begin{array}{cc}100& 0\\ 0& 50\end{array}\right]R=\left[\begin{array}{ccc}5& 0& 0\\ 0& 10& 0\\ 0& 0& 10\end{array}\right]$

Expression formula according to spacecraft attitude tracking control unit u is

U=α (x)+β (x) ν

In formula:

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

$v_{LQR}=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)$

Verify that 1 parameter draws inclined experimental verification

The most inevitably consume fuel due to Space Manipulator System, thus cause the quality of carrier base And rotary inertia changes.For the checking this patent control method robustness to parameter uncertainty, it is considered in the nominal of table 1 On the basis of parameter, produce the inertial parameter change shown in table 2.If the desired trajectory of each joint angle is

θ_d(t)=10sin (0.1t) °

The Parameter Perturbation that table 2 fuel consumption causes

According to the non-linear appearance of space manipulator based on differential geometric theory Yu LQR control method obtained by nominal system State stability controller, is applied to parameter by this control law and draws inclined system so that it is three attitude angle are according to expectation instruction, and it controls effect Fruit is shown in Table 3.In Matlab software, simulation time is 80s, integration step 0.01s.

Table 3 space manipulator Attitude tracking control effect

Fig. 4-1 to 4-5 is each joint angles of space manipulator, the simulation result curve of angular velocity and control moment, from Experimental result is it can be seen that draw system to the rear for parameter, and non-linear differential geometry LQR controls to realize joint to expectation letter Number tracing control, angle steady-state error is: 2.325 × 10^-2°, velocity error is 2.333 × 10^-3°/s, response time is 3.8 seconds, control moment was less than 15N m.Therefore, based on differential geometric non-linear LQR control method design space mechanical arm Control system, draws in the case of partially there is parameter, and during following the tracks of desired trajectory, angle and angular velocity curve smoothing, during regulation Between short, meet the dynamic performance requirements of system, system has stronger stability and dynamic property, illustrates that this controller is to parameter Uncertain problem has the control moment needed for stronger robust stability and robust performance, and this patent control method relatively Little, it is possible to solve fuel consumption problem well.

Checking 2 is arrested noncooperative target assembly and is stablized experimental verification

The completing of Space Manipulator System majority task all needs to be captured by end to realize, it is achieved during these functions, very Difficulty accurately obtains load parameter, thus causes the perturbation of system model.Reality is tested consideration Space Manipulator System end and is arrested one The cubic objects of length of side 400mm, and noncooperative target connects firmly with space mechanism shoulder joint 2, this load will convert bar 2 On, thus there is Parameters variation shown in table 4.Remaining parameter is still for nominal value (being shown in Table 1).

The Parameter Perturbation that table 4 causes after arresting noncooperative target

Also according to the space manipulator non-thread based on differential geometric theory Yu LQR control method obtained by nominal system Sexual stance stability controller, is applied to assembly system by this control law so that it is three attitude angle are according to expectation instruction, and it controls Effect is shown in Table 5.

Table 5 space manipulator Attitude tracking control effect

Fig. 5-1 to 5-5 is each joint angles of space manipulator, the simulation result curve of angular velocity and control moment, from Experimental result is it can be seen that for the system causing Parameters variation after capture target, non-linear differential geometry LQR controls to realize The joint tracing control to desired signal, angle steady-state error is: 2.581 × 10^-2°, velocity error is 2.589 × 10^-3°/s, Response time is 3.6 seconds, and control moment is less than 40N m.Therefore, design based on differential geometric non-linear LQR control method Space manipulator control system, after arresting noncooperative target, in the case of there is parameter large change, follows the tracks of desired trajectory mistake Cheng Zhong, angle and angular velocity curve smoothing, regulating time is short, meets the dynamic performance requirements of system, and system still has stronger Stability and dynamic property, further relate to this controller and have stronger robust stability and robust performance.Although on the other hand After forming assembly, the quality of end bar is greatly increased with rotary inertia, but each joint driven torque is the most significantly Increasing, maximum is still less than 40N m, it was demonstrated that the method can solve fuel consumption problem well.

Checking 3 is arrested noncooperative target assembly and is stablized experimental verification

Aerospace Satellite platform complete noncooperative target arrest formation assembly after, often do not consuming multi fuel Under the conditions of keep normal communication and solar array Direct to the sun.But owing to there is coupling between the motion of mechanical arm and the motion of pedestal Closing, the position of pedestal and attitudes vibration influence whether the pose of mechanical arm tail end, and in turn, the motor-driven of mechanical arm also can be to pedestal Pose produce impact.On the other hand carry out spacecrafts rendezvous formation assembly system with noncooperative target after, space manipulator System can effectively implement the transfer to noncooperative target and mobile operation, simultaneously because the uncertain factor of noncooperative target with And unknown motor-driven problem may be produced, it is therefore necessary to solve robust stable bounds and the attitude anti-interference problem of assembly.

When assuming to capture target, passive space vehicle is zero and for remaining static relative to the angle of inertial coodinate system, Each joint angle initial value parameter is θ=[0 0 0]^TRad,Aerospace Satellite platform is made to remain attitude Static is zero, and two connecting rods are followed the tracks of joint angles and instructed:

θ_0d=0 °

θ_1d=θ_2d=10sin (0.1t) °

Also according to the space manipulator non-thread based on differential geometric theory Yu LQR control method obtained by nominal system Sexual stance stability controller, is applied to assembly system by this control law, it is considered to the unknown motor-driven feelings of different noncooperative targets in table 6 Pedestal stability contorting under condition, each disturbance torque acts at noncooperative target barycenter, and it controls effect and is shown in Table 7.

Table 6 noncooperative target motor type

Random disturbances	Scope is in [-0.1,0.1] N m equally distributed random disturbances moment
		Impulse disturbances	Maximum is the Gaussian pulse disturbance torque of 0.5N m
Time-varying is disturbed	Amplitude is the sinusoidal time-varying disturbance torque of 0.5N m
		Constant value is disturbed	Moment values is the constant value disturbance torque of 0.5N m

System rejection to disturbance capability evaluation under the different non-cooperation motor type of table 7

Fig. 6-1 to 9-3 is under various disturbance torque effects, each joint angles of space manipulator, angular velocity and control The simulation result curve of moment, from experimental result it can be seen that based on designed by differential geometric non-linear LQR control method Space manipulator pose stabilization control system, it is possible to for including random disturbances type, impulse disturbances type, time-varying interference type And the non-cooperation of constant value interference type these four is motor-driven, it is ensured that the attitude stabilization of assembly system, the attitude angle of satellite platform with The steady-state error of attitude angular velocity, response time and control moment value are satisfied by controller performance index request, further relate to After control system designed by the present invention is for arresting noncooperative target, the common non-cooperation of noncooperative target is motor-driven has order The capacity of resisting disturbance that people is satisfied.

Above content is only the technological thought that the present invention is described, it is impossible to limit protection scope of the present invention with this, every presses The technological thought proposed according to the present invention, any change done on the basis of technical scheme, each fall within claims of the present invention Protection domain within.

Claims (3)

1. the Differential Geometry nonlinear control method of the anti-interference attitude stabilization of space manipulator, it is characterised in that include following step Rapid:

Step one, set up free flight space manipulator affine nonlinear state-space model

By attitude kinematics equations and the system total kinetic energy equation of space manipulator, substitute into Lagrange equation, obtain space The attitude dynamics model of mechanical arm is:

$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}---\left(1\right)$

In formula:

$C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\stackrel{\·}{M}\stackrel{\·}{\mathrm{\θ}}-\frac{\∂}{\∂\mathrm{\θ}}\[\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}\]---\left(2\right)$

Wherein, θ is space manipulator attitude angle under inertial coodinate system,For space manipulator appearance under inertial coodinate system State angular velocity；M (θ) is the generalized mass matrix of Space Manipulator System, is the matrix function of space manipulator attitude angle；

Choose state variable

$x={\left[\begin{array}{cccccc}{\mathrm{\θ}}_0& {\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\θ}}_1& {\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\θ}}_2& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]}^T$

Input variable and output variable

U=[τ₀ τ₁ τ₂]^T, y=[θ₀ θ₁ θ₂]^T

The state-space expression obtaining Space Manipulator System is:

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_2\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_4\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ g_{21}& g_{22}& g_{23}\\ 0& 0& 0\\ g_{41}& g_{42}& g_{43}\\ 0& 0& 0\\ g_{61}& g_{62}& g_{63}\end{array}\right]\left[\begin{array}{c}{\mathrm{\τ}}_0\\ {\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right]$

$\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_0\\ {\mathrm{\θ}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\mathrm{\θ}}_2\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(3\right)$

In formula:

$\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=-M^{-1}\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]+\frac{1}{2}{M}^{-1}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

$\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]=M^{-1}---\left(4\right)$

Note

$f=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ f_2\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ f_4\\ {\stackrel{\·}{\mathrm{\θ}}}_2\\ f_6\end{array}\right],g_1=\left[\begin{array}{c}0\\ g_{21}\\ 0\\ g_{41}\\ 0\\ g_{61}\end{array}\right],g_2=\left[\begin{array}{c}0\\ g_{22}\\ 0\\ g_{42}\\ 0\\ g_{62}\end{array}\right],g_3=\left[\begin{array}{c}0\\ g_{23}\\ 0\\ g_{43}\\ 0\\ g_{63}\end{array}\right]---\left(5\right)$

h₁=θ₀,h₂=θ₁,h₃=θ₂

Then obtain the affine nonlinear state space form of Space Manipulator System

$\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}---\left(6\right)$

Step 2, differential geometric theory is utilized to set up the attitude dynamics model of class linear space form

Design following state feedback control law

$\begin{array}{c}u=\mathrm{\α}\left(x\right)+\mathrm{\β}\left(x\right)v\\ =A^{-1}\left(x\right)\[-b\left(x\right)+v\]\end{array}---\left(7\right)$

Wherein

$A^{-1}\left(x\right)={\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]}^{-1}=M\left(x\right)$

$b\left(x\right)=\left[\begin{array}{c}{L}_f^2{h}_1\\ L_f^2{h}_2\\ L_f^2{h}_3\end{array}\right]=\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]---\left(8\right)$

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

Formula (7) (8) is substituted into (6), and the linear space expression-form obtaining space manipulator attitude dynamics model is:

$\begin{array}{c}\stackrel{\·}{x}=Ax+bv\\ y=cx\end{array}---\left(9\right)$

Wherein,Being referred to as Lie derivatives, calculate the factor for one, v is that linear system (9) is set The control law of meter, u control law designed by actual nonlinear system (6)；

Step 3, LQR controller design based on linear space system

Linear system (9) is considered as

$\left\{\begin{array}{c}\stackrel{\·}{x}=\tilde{A}x+\tilde{B}v+\tilde{B}d\\ y=\tilde{C}x+\tilde{D}v\end{array}\right.$

Then control law based on Linear-Quadratic Problem optimal regulation problem design lines sexual system is

$v=-R^{-1}{\tilde{B}}^{T}Px\left(t\right)---\left(10\right)$

In formula, R is the weighting matrix in optimum control performance indications to input variable, and P is for meeting following Riccati algebraic equation Dematrix,

PA+A^TP-PBR^-1B^TP+Q=0

Finally give and based on differential geometric spacecraft nonlinear attitude tracking control unit be

U=α (x)+β (x) ν (11)

In formula:

$\mathrm{\α}\left(x\right)=-A^{-1}\left(x\right)b\left(x\right)=-M\left(x\right)\left[\begin{array}{c}{f}_2\\ f_4\\ f_6\end{array}\right]=\stackrel{\·}{M}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]-\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\frac{\∂M}{\∂\mathrm{\θ}}\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]$

β (x)=A^-1(x)=M (x)

$v_{LQR}=-R^{-1}{\tilde{B}}^{T}Px\left(t\right).$

The Differential Geometry nonlinear control method of the anti-interference attitude stabilization of space manipulator the most according to claim 1, its Being characterised by, in described step one, the attitude dynamic equations of space manipulator is obtained by following steps:

The body series OX of definition space mechanical arm_BY_BZ_BInitial point be space manipulator pedestal barycenter O, OX_B,OY_B,OZ_BThree axles are solid It is connected on space manipulator base body, and consistent with the principal axis of inertia respectively；

The kinematic relation of doublejointed mechanical arm system is

$\begin{array}{c}\begin{array}{ccc}{\mathrm{\ω}}_0={\stackrel{\·}{\mathrm{\θ}}}_0& {\mathrm{\ω}}_1={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1& {\mathrm{\ω}}_2={\stackrel{\·}{\mathrm{\θ}}}_0+{\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\\ v_{1x}={\stackrel{\·}{r}}_{1x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)\\ v_{1y}={\stackrel{\·}{r}}_{1y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+a_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)\\ v_{2x}={\stackrel{\·}{r}}_{2x}=v_{0x}-b_0{\mathrm{\ω}}_0{\mathrm{sin\θ}}_0-l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)-a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)sin({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ v_{2y}={\stackrel{\·}{r}}_{2y}=v_{0y}+b_0{\mathrm{\ω}}_0{\mathrm{cos\θ}}_0+l_1({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)+a_2({\mathrm{\ω}}_0+{\mathrm{\ω}}_1+{\mathrm{\ω}}_2)cos({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}---\left(12\right)$

Note

$\begin{array}{ccc}{s}_0={\mathrm{sin\θ}}_0& s_{01}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& s_{012}=\sin ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ c_0={\mathrm{cos\θ}}_0& c_{0a}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1)& c_{012}=\cos ({\mathrm{\θ}}_0+{\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}---\left(13\right)$

Then system total kinetic energy is

$T=\frac{1}{2}{\stackrel{\·}{\mathrm{\θ}}}^{T}M\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}=\frac{1}{2}\left[\begin{array}{ccc}{\stackrel{\·}{\mathrm{\θ}}}_0& {\stackrel{\·}{\mathrm{\θ}}}_1& {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]\left[\begin{array}{ccc}{M}_{11}& M_{12}& M_{13}\\ M_{21}& M_{22}& M_{23}\\ M_{31}& M_{32}& M_{33}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_0\\ {\stackrel{\·}{\mathrm{\θ}}}_1\\ {\stackrel{\·}{\mathrm{\θ}}}_2\end{array}\right]---\left(14\right)$

Wherein,

M₃₃=S_cc

M₂₃=M₃₂=M₃₃+S_bcc₂

M₁₃=M₃₁=M₂₃+S_acc₁₂

M₂₂=M₂₃+S_bb+S_bcc₂

M₁₂=M₂₁=M₂₂+S_abc₁+S_acc₁₂

M₁₂=M₁₂+S_aa+S_abc₁+S_acc₁₂

$S_{aa}=\frac{1}{M}{m}_0(m_1+m_2)b_0^2+I_0---\left(15\right)$

$S_{bb}=\frac{1}{M}\[m_0(m_1+m_2)a_1^2+m_2(m_0+m_1)b_1^2+2m_0{m}_1{b}_1\]+I_1$

$S_{cc}=\frac{1}{M}{m}_2(m_0+m_2)a_2^2+I_2$

$S_{ab}=\frac{1}{M}{m}_0\[(m_1+m_2)a_1+m_2{b}_1\]b_0$

$S_{ac}=\frac{1}{M}{m}_0{m}_2{b}_0{a}_2$

$S_{bc}=\frac{1}{M}{m}_2\[(m_0+m_1)b_1+m_0{a}_1\]a_2$

(14) formula is substituted into Lagrange equation, obtains the kinetics equation (1) of system；

$M\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+C(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})\stackrel{\·}{\mathrm{\θ}}=\mathrm{\τ}$

In formula, θ=[θ₀ θ₁ θ₂]^TIt is space manipulator pedestal and the attitude angle in two joints, τ=[τ₀ τ₁ τ₂]^TIt it is space Mechanical arm pedestal and the control moment in two joints.

The Differential Geometry nonlinear control method of the anti-interference attitude stabilization of space manipulator the most according to claim 1, its Being characterised by, in described step 2, differential geometric theory carries out accurate linearization process and comprises the following steps:

Calculate corresponding Lie derivative, obtain matrix

$A\left(x\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f{h}_1& L_{g_2}{L}_f{h}_1& L_{g_3}{L}_f{h}_1\\ L_{g_1}{L}_f{h}_2& L_{g_2}{L}_f{h}_2& L_{g_3}{L}_f{h}_2\\ L_{g_1}{L}_f{h}_3& L_{g_2}{L}_f{h}_3& L_{g_3}{L}_f{h}_3\end{array}\right]=\left[\begin{array}{ccc}{g}_{21}& g_{22}& g_{23}\\ g_{41}& g_{42}& g_{43}\\ g_{61}& g_{62}& g_{63}\end{array}\right]---\left(16\right)$

Nonsingular, therefore system Relative order r₁=r₂=r₃=2, i.e. r=r₁+r₂+r₃=6=n, n are system dimension, therefore space machine Mechanical arm system meets the necessary and sufficient condition of exact linearization method in differential geometric theory；

Owing to the expression formula (6) of system has been typical non linear system state space canonical form, i.e. Brunowsky standard Type:

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_1^i={\mathrm{\ξ}}_2^i\\ {\stackrel{\·}{\mathrm{\ξ}}}_1^i=b_i\left(\mathrm{\ξ}\right)+\underset{j=1}{\overset{3}{\Σ}}{a}_{ij}\left(\mathrm{\ξ}\right)u_j\\ y_i={\mathrm{\ξ}}_1^i\end{array}---\left(17\right)$

Wherein $a_{ij}\left(\mathrm{\ξ}\right)=L_{g_i}{L}_f{h}_i\left({\mathrm{\φ}}^{-1}\right(\mathrm{\ξ}\left)\right),1\≤i\≤2,1\≤j\≤3$ $b_i\left(\mathrm{\ξ}\right)=L_f^2{h}_i\left({\mathrm{\φ}}^{-1}\right(\mathrm{\ξ}\left)\right),1\≤i\≤2$

The state feedback control law of design formula (7):

U=α (x)+β (x) ν

=A^-1(x)[-b(x)+ν]

State feedback control law is substituted into the space mechanism after Space Manipulator System kinetic model (6) obtains exact linearization method Arm kinetic model is:

$\begin{array}{c}\stackrel{\·}{x}=A_{L}x+b_{L}v\\ y=c_{L}x\end{array}---\left(18\right)$

Wherein,

$A_L=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right],b_L=\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right],c_L={\left[\begin{array}{c}1\\ 0\\ 1\\ 0\\ 1\\ 0\end{array}\right]}^T.$