Controller design method based on unit quaternion and modified Rodrigues parameters

Abstract

The invention discloses a controller design method based on unit quaternion and modified Rodrigues parameters. The method comprises the following steps: S1, constructing a mathematical model of a rigid spacecraft attitude system; s2, constructing a mathematical model of a rigid spacecraft attitude error system based on the corrected Rodrigues parameters; s3, constructing a control target based on the attitude error; s4, constructing a disturbance observer, and constructing a Lyapunov function; s5, constructing an attitude error Lyapunov function according to the angular velocity error of the rigid spacecraft; s6, according to the mathematical model of the rigid spacecraft attitude error system, constructing an attitude system Lyapunov function; and S7, constructing a torque controller according to the Young inequality. The method is used for designing a continuous controller for controlling the spacecraft, asymptotic tracking of the trajectory can be completed, and the unwinding phenomenon can be avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of rigid spacecraft control, and particularly to a controller design method based on unit quaternion and modified Rodriguez parameters. Background Art

[0002] Spacecraft attitude tracking control is a process aimed at precisely adjusting and stabilizing the spacecraft from an initial stable attitude to a desired attitude. This control has shown extremely high practical value in both civilian and military fields and has been regarded as a core attitude control mode during spacecraft operation, attracting extensive attention in the industry. Unit quaternions and modified Rodriguez parameters (MRP) are usually used to describe attitude dynamics. However, in the design of controllers based on quaternions and modified Rodriguez parameters, the unwinding phenomenon is inevitable. The "unwinding phenomenon" refers to the situation where the initial attitude of the spacecraft is close to the desired attitude, but instead of rotating towards the desired attitude, it deviates from the desired attitude, resulting in unnecessary energy consumption of the spacecraft.

[0003] In view of this, the present invention is specifically proposed. Summary of the Invention

[0004] The object of the present invention is to solve the deficiencies existing in the prior art, and to propose a controller design method based on unit quaternion and modified Rodriguez parameters for designing a continuous controller for controlling a spacecraft, which can achieve asymptotic tracking of the trajectory and avoid the occurrence of the unwinding phenomenon.

[0005] To achieve the above object, the present invention also adopts the following technical solutions:

[0006] A controller design method based on unit quaternion and modified Rodriguez parameters includes the following steps:

[0007] S1: Based on the unit quaternion, construct a mathematical model of the rigid spacecraft attitude system, and obtain the equilibrium point of the mathematical model of the rigid spacecraft attitude system;

[0008] S2: For the stable equilibrium point in the mathematical model of the rigid spacecraft attitude system, construct a mathematical model of the rigid spacecraft attitude error system based on the modified Rodriguez parameters;

[0009] S3: Construct a control objective based on the attitude error;

[0010] S4: Construct a disturbance observer and a Lyapunov function;

[0011] S5: According to the angular velocity error of the rigid spacecraft, construct an attitude error Lyapunov function;

[0012] S6: Construct the Lyapunov function of the attitude system according to the mathematical model of the rigid spacecraft attitude error system;

[0013] S7: Construct the torque controller according to the Young's inequality.

[0014] Furthermore, the S1 includes the following steps:

[0015] S11: Based on the unit quaternion, construct the mathematical model of the rigid spacecraft attitude system, and the model is as follows:

[0016]

[0017] In the formula, q is the attitude of the spacecraft, q 0 and q v are the scalar part and the vector part of q respectively, I is the identity matrix, J is the moment of inertia of the spacecraft, ω is the angular velocity of the spacecraft in the body coordinate system, is the pure quaternion, is the multiplication of unit quaternions, τ is the torque of the spacecraft, d is the external disturbance received by the spacecraft, S(q v ) is the skew-symmetric matrix of q v ;

[0018] S12: Obtain the equilibrium points of the rigid spacecraft attitude system constructed based on the unit quaternion, including the unstable equilibrium point q = -1 and the stable equilibrium point q = +1.

[0019] Furthermore, the S2 includes the following steps:

[0020] S21: For the stable equilibrium points in the mathematical model of the rigid spacecraft attitude system, define the attitude error Furthermore, it can be obtained that Taking the derivative of both sides of this formula, we can get:

[0021]

[0022] Denote ω e = ω - ω p , where The above formula can be written as where Construct the error correction Rodriguez vector p e as follows:

[0023]

[0024] In the formula, q e0 and q ev are the scalar part and the vector part of q e respectively, θe is the rotation angle error of the spacecraft, n e is the Euler axis of the spacecraft;

[0025] S22: Differentiate the constructed modified Rodriguez vector p e to obtain the error kinematic equation of the rigid body spacecraft expressed in terms of the modified Rodriguez vector, as follows:

[0026]

[0027] In the formula, ω e = ω - ω p is the angular velocity error of the rigid body spacecraft, ω p is the desired angular velocity ω d expressed in the body coordinate system, p e is the attitude error of the rigid body spacecraft;

[0028] S23: Construct the dynamic equation of the rigid body spacecraft error in the body coordinate system, as follows:

[0029]

[0030] S24: Construct the mathematical model of the rigid body spacecraft attitude error system, the model is as follows:

[0031]

[0032] Furthermore, the said S3 includes the following steps:

[0033] S31: Construct the control objective based on the desired attitude, as follows:

[0034]

[0035] In the formula, q d is the desired attitude of the rigid body spacecraft;

[0036] S32: Convert the control objective based on the desired attitude to the control objective based on the attitude error, as follows:

[0037]

[0038] Furthermore, the said S4 includes the following steps:

[0039] S41: Construct a disturbance observer, as follows:

[0040]

[0041] In the formula, L is the positive gain coefficient of the observer;

[0042] S42: Construct the Lyapunov function V0 , and take its derivative, V 0 has the following form:

[0043]

[0044] In the formula, κ is a constant, L > κ > 0, where,

[0045]

[0046] Furthermore, the said S5 includes the following steps:

[0047] S51: Construct the attitude error Lyapunov function V 1 , and take its derivative, V 1 has the following form:

[0048]

[0049] S52: Construct the angular velocity stabilization function ω α , ω α has the following form:

[0050] ω α =-c 1 p e

[0051] In the formula, c 1 is a constant, c 1 >0, and makes the following inequality hold

[0052]

[0053] where,

[0054]

[0055] ω z =ω e -ω α

[0056] In the formula, S(p e ) is the skew-symmetric matrix of p e , and I is the identity matrix.

[0057] Furthermore, the said S6 includes the following steps:

[0058] S61: Construct the attitude system Lyapunov function V 2 , and take its derivative, V 2 has the following form:

[0059]

[0060] In the formula, γ is a constant, γ > 0, and the following inequality holds

[0061]

[0062] Furthermore, the S7 includes the following steps:

[0063] S71: According to the Young inequality, construct a torque controller in the following form:

[0064]

[0065] In the formula, κ and c 2 are constants, κ, c 2 > 0 and the following inequality holds

[0066]

[0067] where

[0068]

[0069] In the formula, λ max (J) is the maximum eigenvalue of J.

[0070] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0071] Modeling the spacecraft using unit quaternions, compared with the traditional Euler angle method, avoids the singularity problem and can continuously and smoothly represent the change of the spacecraft attitude; for the unwinding phenomenon caused by the controller designed by unit quaternions, the modified Rodriguez parameter vector is used to represent the attitude error equation, where the equilibrium point is located at the origin of the coordinate system; a continuous controller is designed using the modified Rodriguez parameter vector, enabling the spacecraft to track both the attitude and avoid the unwinding phenomenon during the tracking process. Brief Description of the Drawings

[0072] Figure 1 is a flowchart of the controller design method based on unit quaternions and modified Rodriguez parameters provided by the present invention;

[0073] Figure 2 is a coordinate system diagram of a rigid spacecraft provided by the present invention;

[0074] Figure 3 is a schematic structural diagram of a spacecraft attitude dynamics model calculation module provided by the present invention;

[0075] Figure 4 is a structural diagram of a torque control module provided by the present invention;

[0076] Figure 5 is an experimental display module of the control result provided by the present invention;

[0077] Figure 6 The simulation result for Case 1;

[0078] Figure 7 The simulation result for Case 2. Specific implementation manner

[0079] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments.

[0080] Embodiment 1:

[0081] A controller design method based on unit quaternion and modified Rodriguez parameters, as Figures 1-5 shown, includes the following steps:

[0082] S1: Construct a mathematical model of the rigid body spacecraft attitude system based on unit quaternion, and obtain the equilibrium point of the mathematical model of the rigid body spacecraft attitude system;

[0083] S2: For the stable equilibrium point in the mathematical model of the rigid body spacecraft attitude system, construct a mathematical model of the rigid body spacecraft attitude error system based on the modified Rodriguez parameters;

[0084] S3: Construct a control target based on the attitude error;

[0085] S4: Construct a disturbance observer and construct a Lyapunov function;

[0086] S5: Construct an attitude error Lyapunov function according to the angular velocity error of the rigid body spacecraft;

[0087] S6: Construct an attitude system Lyapunov function according to the mathematical model of the rigid body spacecraft attitude error system;

[0088] S7: Construct a torque controller according to the Young inequality.

[0089] In an alternative embodiment, the S1 includes the following steps:

[0090] S11: Construct a mathematical model of the rigid body spacecraft attitude system based on unit quaternion, and the model is as follows:

[0091]

[0092] In the formula, q is the attitude of the spacecraft, q 0 and q vThey are the scalar part and vector part of q respectively. I is the identity matrix, J is the moment of inertia of the spacecraft, and ω is the angular velocity of the spacecraft in the body coordinate system. is a pure quaternion. is the multiplication of unit quaternions, τ is the torque of the spacecraft, d is the external disturbance acting on the spacecraft, and S(q v ) is the skew-symmetric matrix of q v ;

[0093] Specifically, q v and S(q v ) are as follows:

[0094] q v =(q v1 q v2 q v3 ) T

[0095]

[0096] S12: Obtain the equilibrium points of the rigid spacecraft attitude system constructed based on unit quaternions, including the unstable equilibrium point q = -1 and the stable equilibrium point q = +1.

[0097] In an optional embodiment, the S2 includes the following steps:

[0098] S21: Define the attitude error for the stable equilibrium points in the mathematical model of the rigid spacecraft attitude system Furthermore, it can be obtained that Taking the derivative of both sides of this equation gives:

[0099]

[0100] Denote ω e = ω - ω p , where The above equation can be written as where Construct the error correction Rodriguez vector p e as follows:

[0101]

[0102] In the formula, q e0 and q ev are the scalar part and vector part of q e respectively, θ e is the rotation angle error of the spacecraft, and n e is the Euler axis of the spacecraft;

[0103] S22: Differentiate the constructed modified Rodriguez vector p e to obtain the error kinematic equation of the rigid body spacecraft represented by the modified Rodriguez vector as follows:

[0104]

[0105] In the formula, ω e = ω - ω p is the angular velocity error of the rigid body spacecraft, ω p is the desired angular velocity ω d represented in the body coordinate system, and p e is the attitude error of the rigid body spacecraft;

[0106] S23: Construct the dynamic equation of the error of the rigid body spacecraft in the body coordinate system as follows:

[0107]

[0108] S24: Construct the mathematical model of the rigid body spacecraft attitude error system, and the model is as follows:

[0109]

[0110] In an optional embodiment, the S3 includes the following steps:

[0111] S31: Construct the control objective based on the desired attitude as follows:

[0112]

[0113] In the formula, q d is the desired attitude of the rigid body spacecraft;

[0114] S32: Convert the control objective based on the desired attitude to the control objective based on the attitude error as follows:

[0115]

[0116] In an optional embodiment, the S4 includes the following steps:

[0117] S41: Construct a disturbance observer as follows:

[0118]

[0119] In the formula, L is the positive gain coefficient of the observer;

[0120] S42: Construct the Lyapunov function V 0 , and differentiate it. The form of V 0 is as follows:

[0121]

[0122] In the formula, κ is a constant, where L > κ > 0, and

[0123]

[0124] In an optional embodiment, S5 includes the following steps:

[0125] S51: Construct the attitude error Lyapunov function V 1 , and take the derivative of it. The form of V 1 is as follows:

[0126]

[0127] S52: Construct the angular velocity stabilization function ω α , and the form of ω α is as follows:

[0128] ω α = -c 1 p e

[0129] In the formula, c 1 is a constant, c 1 > 0, and the following inequality holds

[0130]

[0131] where

[0132]

[0133] ω z = ω e - ω α

[0134] In the formula, S(p e ) is the skew-symmetric matrix of p e , and I is the identity matrix.

[0135] Specifically, p e and S(p e ) are specifically as follows:

[0136] p e = (p e1 p e2 p e3 ) T

[0137]

[0138] In an alternative embodiment, S6 includes the following steps:

[0139] S61: Construct the Lyapunov function V of the attitude system 2 , and take its derivative. The form of V 2 is as follows:

[0140]

[0141] In the formula, γ is a constant, γ > 0, and the following inequality holds

[0142]

[0143] In an alternative embodiment, S7 includes the following steps:

[0144] S71: According to the Young inequality, construct a torque controller in the following form:

[0145]

[0146] In the formula, κ and c 2 are constants, κ, c 2 > 0, and the following inequality holds

[0147]

[0148] where

[0149]

[0150] In the formula, λ max (J) is the maximum eigenvalue of J.

[0151] In this embodiment, a controller design method based on unit quaternions and modified Rodriguez parameters models the spacecraft using unit quaternions. Compared with the traditional Euler angle method, it avoids the singularity problem and can continuously and smoothly represent the change of the spacecraft attitude; for the unwrapping phenomenon caused by the controller designed by unit quaternions, the modified Rodriguez parameter vector is used to represent the attitude error equation, where the equilibrium point is located at the origin of coordinates; the continuous controller is designed using the modified Rodriguez parameter vector, so that the spacecraft can not only track the attitude but also avoid the unwrapping phenomenon during the tracking process.

[0152] To prove the effectiveness of the controller designed by the controller design method based on unit quaternions and modified Rodriguez parameters in this embodiment, simulation verification is carried out on the designed controller.

[0153] In the simulation, the moment of inertia of the rigid body spacecraft is as follows:

[0154]

[0155] The external disturbance is as follows:

[0156] d = (0.01sin(0.05t), 0.005sin(0.05t), -0.01cos(0.05t)) T

[0157] The controller parameters, c 1 = 0.04, c 2 = 0.05, γ = 5, L = 100

[0158] The initial attitude of the spacecraft is as follows:

[0159] Case 1:

[0160] Case 2:

[0161] Case 1 and Case 2 correspond to two unit quaternions of the initial attitude in the three-dimensional physical space.

[0162] The initial angular velocity error of the spacecraft is as follows:

[0163] ω 0 = [0, 0, 0] T

[0164] The desired attitude of the spacecraft is as follows:

[0165]

[0166] To achieve the signal ω d and the following instruction filtering is introduced:

[0167]

[0168] where the superscript c represents the signal processed by the instruction filter.

[0169] From the angular velocity expression, we can obtain:

[0170]

[0171] The following instruction filtering is introduced:

[0172]

[0173] In the controller can be replaced by where k 1 = k 2 = 100.

[0174] Simulations are carried out according to the above simulation conditions. The simulation results of cases 1 and 2 are shown respectively as Figure 6 , 7 shown.

[0175] Figure 6 and Figure 7 The simulation results in the upper left corner of d show the change of the unit quaternion error between the spacecraft attitude and the desired attitude over time. It can be seen that after about the 10th second, q tracks q d . Figure 6 and Figure 7 The simulation results in the upper right corner of e show the change of the modulus of the modified Rodriguez parameter error vector over time. It can be seen that p e ≤1, indicating that the spacecraft avoids the unwrapping phenomenon during the tracking process. Figure 6 and Figure 7 The simulation results in the lower left corner of p show the change of the three components of the angular velocity error over time. It can be seen from the figure that after the 10th second, ω tracks ω p . Figure 6 and Figure 7 The simulation results in the lower right corner of

[0176] show the change of the modulus of the torque control over time. As described above, only the preferred specific embodiments of the present invention are shown, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. A controller design method based on unit quaternion and modified Rodriguez parameter, characterized in that: The steps include: S1: Construct a mathematical model of the rigid spacecraft attitude system based on unit quaternion, and obtain the equilibrium point of the mathematical model of the rigid spacecraft attitude system; S2: Aiming at the stable equilibrium point in the mathematical model of the rigid spacecraft attitude system, a mathematical model of the rigid spacecraft attitude error system is constructed based on the modified Rodriguez parameters; S3: Construct control targets based on attitude errors; S4: Construct a disturbance observer and construct a Lyapunov function; S5: Construct the attitude error Lyapunov function based on the angular velocity error of the rigid spacecraft; S6: Based on the mathematical model of the attitude error system of the rigid spacecraft, the attitude system Lyapunov function is constructed; S7: Based on Young’s inequality, construct a torque controller.

2. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 1, characterized in that: The S1 comprises the following steps: S11: Construct a mathematical model of the rigid spacecraft attitude system based on unit quaternions. The model is as follows: In the formula, q is the attitude of the spacecraft, q0 and q v are the scalar and vector parts of q, respectively. I is the unit matrix. J is the moment of inertia of the spacecraft. ω is the angular velocity of the spacecraft in the body coordinate system. is a pure quaternion, is the multiplication of unit quaternions, τ is the torque of the spacecraft, d is the external disturbance of the spacecraft, S(q v ) is q v A skew-symmetric matrix; S12: Acquire the balance points of the rigid spacecraft attitude system constructed based on the unit quaternion, including an unstable balance point q=-1 and a stable balance point q=+1.

3. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 2, characterized in that: The S2 comprises the following steps: S21: Define attitude error for stable equilibrium points in the mathematical model of a rigid spacecraft attitude system Further available Taking the derivative of both sides of the equation, we can get: remember ω e =ω-ω p ,in The above formula can be written as in Construct the error-corrected Rodriguez vector p e as follows: In the formula, q e0 and q ev They are q e The scalar part and the vector part of θ e is the rotation angle error of the spacecraft, n e are the Euler axes of the spacecraft.

4. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 3, characterized in that: The S2 further comprises the following steps: S22: Modified Rodriguez vector p for construction e By taking the derivative, we obtain the error kinematic equation of the rigid body spacecraft expressed by the modified Rodriguez vector, as follows: In the formula, ω e =ω-ω p is the angular velocity error of the rigid spacecraft, ω p is the desired angular velocity ω d In body coordinate system, p e is the attitude error of the rigid spacecraft; S23: Construct the dynamic equation of the rigid body spacecraft error in the body coordinate system as follows: S24: Construct a mathematical model of the rigid spacecraft attitude error system. The model is as follows:

5. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 4, characterized in that: The S3 comprises the following steps: S31: Construct the control target based on the desired posture as follows: In the formula, q d is the desired attitude of the rigid spacecraft; S32: Convert the control target based on the desired posture to the control target based on the posture error, as follows:

6. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 5, characterized in that: The S4 comprises the following steps: S41: Construct the disturbance observer as follows: In the formula, L is the positive gain coefficient of the observer; S42: Construct the Lyapunov function V0 and take its derivative. The form of V0 is as follows: In the formula, κ is a constant, L>κ>0, where, 7. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 6, characterized in that: The S5 comprises the following steps: S51: Construct the attitude error Lyapunov function V1 and take its derivative. The form of V1 is as follows: S52: Construct angular velocity stabilization function ω α ,ω α The form is as follows: ω α =-c1p e In the formula, c1 is a constant, c1>0, and the following inequality holds true in, oh z =ω e -oh α In the formula, S(p e ) is p e is a skew-symmetric matrix, and I is the identity matrix.

8. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 6, characterized in that: The S6 comprises the following steps: S61: Construct the attitude system Lyapunov function V2 and take its derivative. The form of V2 is as follows: In the formula, γ is a constant, γ>0, and the following inequality holds true 9. The controller design method based on unit quaternion and modified Rodriguez parameter according to claim 8, characterized in that: The S7 comprises the following steps: S71: According to Young's inequality, the torque controller is constructed in the following form: In the formula, κ and c2 are constants, κ, c2>0, and the following inequality holds true in, In the formula, λ max (J) is the maximum eigenvalue of J.

Images