A three-axis fast maneuvering attitude trajectory planning and tracking method for low-orbit remote sensing satellites

Abstract

A low-orbit remote sensing satellite three-axis rapid maneuvering attitude trajectory planning and tracking method relates to the technical field of aerospace, in particular to the technical field of low-orbit remote sensing satellite three-axis rapid maneuvering attitude trajectory planning and tracking, which effectively solves the balance problem of maneuverability and stability in the process of attitude transformation and improves the superiority of anti-interference capability. The method comprises the following steps: establishing a satellite kinematics model and a satellite dynamics model; calculating the quaternion of the desired coordinate system relative to the satellite initial coordinate system at the maneuvering starting moment; completing attitude trajectory planning through an improved tracking differentiator; calculating the total control rate; obtaining the real control rate through an actuator; calculating the quaternion of the satellite body coordinate system relative to the inertial coordinate system and the angular velocity of the satellite body coordinate system relative to the inertial coordinate system; obtaining the real body quaternion and the real body angular velocity through a measuring mechanism; and cyclically planning the trajectory and tracking.

Description

Technical Field

[0001] This invention relates to the field of aerospace technology, specifically to a field of three-axis rapid maneuvering attitude trajectory planning and tracking technology for low-orbit remote sensing satellites. Background Technology

[0002] With the continuous development of aerospace technology, low-orbit optical remote sensing satellites have been widely used in resource management, Earth observation, and environmental monitoring. In multi-point imaging missions, satellites can acquire images from different azimuths through multiple maneuvers; the higher the maneuver efficiency within the same time frame, the wider the imaging range. Current attitude maneuver research faces a challenge in balancing maneuverability and stability. Controllers prioritizing rapid response may sacrifice stability to some extent, reducing image quality; conversely, emphasizing stability may slow maneuver convergence, affecting imaging range and efficiency. At the planning level, existing optimization methods suffer from high computational costs, while analytical methods do not consider the stability of the actuators. Some researchers have used tracking differentiators for path planning, improving maneuverability while considering acceleration smoothing, but this only considers single-axis maneuvers and has not been extended to three axes. At the control level, most industry practices use PD control. While simple and feasible, this method has poor anti-interference capabilities and does not consider actuator saturation. Other methods, while considering actuators and environmental interference, face difficulties in parameter tuning or high computational costs, making them impractical for engineering applications. Summary of the Invention

[0003] To address the aforementioned problems, this invention discloses a method for three-axis rapid maneuvering attitude trajectory planning and tracking of low-orbit remote sensing satellites, relating to the field of aerospace technology. It effectively solves the problem of balancing maneuverability and stability during the three-axis attitude transformation process of satellites and improves the superiority of anti-interference capability.

[0004] The method includes the following steps:

[0005] S1. Establish satellite kinematics and satellite dynamics models;

[0006] S2, the quaternion of the expected coordinate system relative to the initial coordinate system of the satellite at the start of computer operation;

[0007] S3. Attitude trajectory planning is completed by improving the tracking differentiator;

[0008] S4. Calculate the overall control rate;

[0009] S5. Obtain the true control rate through the implementing agency;

[0010] S6. Calculate the quaternion of the satellite body coordinate system relative to the inertial coordinate system and the angular velocity of the satellite body coordinate system relative to the inertial coordinate system.

[0011] S7. Obtain the quaternion and angular velocity of the actual body through the measuring mechanism;

[0012] S8, Loop Planning Trajectory and Tracking.

[0013] Furthermore, the kinematic equations of the kinematic model are specifically as follows: The Let q0 represent the derivative of the quaternion of the satellite body coordinate system relative to the inertial coordinate system, where q0 represents the real part of Q, Q represents the quaternion of the satellite body coordinate system relative to the inertial coordinate system, and q represents the imaginary part of Q. Let S(q) represent the derivative of q0, S(q) be the antisymmetric matrix of q, I3 be the 3rd order identity matrix, ω represent the angular velocity of the satellite body coordinate system relative to the inertial coordinate system, and T represent the controller step size; the dynamic equations of the dynamic model are specifically as follows: The The derivative of ω is denoted by ω. The positive definite matrix representing the satellite's moment of inertia, the 3 In a three-dimensional space, S(ω) represents the antisymmetric matrix of ω, and h w ∈ 3 The total angular momentum of the reaction flywheel is given by u′, which represents the true controllability.

[0014] Furthermore, the formula for calculating the quaternion of the desired coordinate system relative to the initial coordinate system of the satellite at the start of the maneuver is as follows: q OG Let q be the quaternion of the desired coordinate system relative to the orbital coordinate system. iO0 The quaternion of the initial coordinate system relative to the satellite orbit coordinate system at the start of the maneuver, the This represents the tensor product operation.

[0015] Furthermore, the attitude trajectory planning specifically involves: utilizing the rotation axis e iG0 Obtain the quaternion q of the desired satellite coordinate system relative to the initial coordinate system. G0 angular velocity ω G0 and acceleration a G0 The The e iG0 q iG0 The rotation axis, the This indicates the expected perspective of the planning; the stated The Indicates the desired angular velocity; the The This represents the desired angular acceleration during planning.

[0016] Furthermore, the improvement of the tracking differentiator to complete the attitude trajectory planning specifically involves: representing the planner as a second-order discrete system; the second-order discrete system specifically refers to:

[0017] The t represents the index in the discrete time series.

[0018] Furthermore, the total control rate is the sum of the adaptive control rate and the compensation control rate, and its calculation formula is as follows: u = u a +u b The u represents the total control law, the u a U represents the adaptive control law. a =[u a1 ,u a2 ,u a3 The calculation process is as follows: The j = [1,2,3], the δ k Where k is a positive constant, u is the adaptive gain. max To control torque limiting, the s aj For s a The elements in, the s a Let s be the sliding vector. a =ω e +k 2 q e13 The q e13 For q e The imaginary part of q e For the bias quaternion, the The q n The q represents the real ontology quaternion. G Represents the real-time expected quaternion in the inertial frame; the ω e The deviation angular velocity, ω e =ω n -R(q e )ω G The ω n The ω represents the actual angular velocity of the body. G R(q) represents the real-time desired angular velocity in the inertial frame. e ) represents the transformation matrix from the target coordinate system to the body coordinate system; the u b Indicates the compensation control rate; the The The C e =R(q) e ).

[0019] Furthermore, the actuator is a flywheel model, and the total control rate u is input into the flywheel model to obtain the actual control rate u′.

[0020] Furthermore, the angular velocity ω of the satellite body coordinate system relative to the inertial coordinate system is calculated using the satellite dynamics model; the quaternion Q of the satellite body coordinate system relative to the inertial coordinate system is calculated using the satellite kinematics model.

[0021] Furthermore, the measurement mechanism includes a gyroscope model and a star sensor model; specifically, obtaining the true quaternion and true angular velocity of the satellite body through the measurement mechanism involves inputting the angular velocity ω of the satellite body coordinate system relative to the inertial coordinate system into the gyroscope model to obtain the true angular velocity ω. n The quaternion Q of the satellite's body coordinate system relative to the inertial coordinate system is input into the star sensor model to obtain the actual body quaternion q. n .

[0022] Furthermore, the cyclic planning trajectory and tracking specifically involves: using the actual body angular velocity ω n and the real quaternion q n Enter step S4 to complete the planning and tracking of the desired trajectory.

[0023] The beneficial effects of this invention are as follows:

[0024] This invention enables trajectory planning from any satellite attitude to the imaging attitude, planning the shortest path across three axes and performing path tracking while considering external interference and flywheel constraints. For attitude tracking, a tracking controller based on adaptive parameters and compensation estimation is designed. Adaptive parameters are used to improve anti-interference capability, and actuator saturation control is considered to ensure smooth satellite tracking of the planned trajectory to the target attitude. Compared with current advanced control methods such as FAMF and PD, this method comprehensively considers the satellite's maneuverability and stability, improving maneuverability while ensuring attitude stability during imaging. Furthermore, compensation estimation is designed using the attitude angular velocity and angular acceleration obtained from attitude planning to improve tracking capability. This method is simple to tune and easy to implement in engineering. Attached Figure Description

[0025] Figure 1 This is a flowchart of the attitude trajectory planning and tracking method according to an embodiment of the present invention;

[0026] Figure 2 This is a schematic diagram of the axis-angle curve in an embodiment of the present invention;

[0027] Figure 3 This is a schematic diagram of the angular velocity curve of the planning shaft in an embodiment of the present invention;

[0028] Figure 4 This is a schematic diagram of the axial acceleration curve according to an embodiment of the present invention;

[0029] Figure 5 This is a schematic diagram of the deviation quaternion curve in an embodiment of the present invention;

[0030] Figure 6 This is a schematic diagram of the deviation angular velocity curve in an embodiment of the present invention;

[0031] Figure 7 This is a schematic diagram of the side swing angle comparison curve of an embodiment of the present invention;

[0032] Figure 8 This is a schematic diagram of the side yaw rate comparison curves according to an embodiment of the present invention;

[0033] Figure 9 This is a schematic diagram of the three-axis deviation angle curves according to an embodiment of the present invention;

[0034] Figure 10 This is a schematic diagram of the triaxial deviation angular velocity curves according to an embodiment of the present invention. Detailed Implementation

[0035] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0036] Example 1

[0037] This embodiment provides a method for three-axis rapid maneuvering attitude trajectory planning and tracking of low-Earth orbit remote sensing satellites. The relevant parameters of this embodiment are shown in Table 1.

[0038] Table 1

[0039]

[0040]

[0041] The flowchart of the method is as follows: Figure 1 The method includes the following steps:

[0042] S1. Establish satellite kinematics and satellite dynamics models.

[0043] The relevant operations in step S1 will be introduced with specific examples:

[0044] The kinematic equations of the kinematic model are as follows: The The derivative of the quaternion of the satellite body coordinate system with respect to the inertial coordinate system is given by Q, where Q = (q0, q1, q2, q3). T =(q0,q T ) Tq0 represents the real part of Q, q represents the imaginary part of Q, and q1, q2, and q3 represent the imaginary unit of Q. Let S(q) represent the derivative of q0, S(q) be the antisymmetric matrix of q, I3 be the 3rd order identity matrix, and ω represent the angular velocity of the satellite body coordinate system relative to the inertial coordinate system, where ω = (ω1, ω2, ω3). T ω1, ω2, and ω3 represent the angular velocities of the three axes of the body coordinate system, T represents the controller step size, and the symbol "·" indicates differentiation; the dynamic equations of the dynamic model are specifically as follows: The The derivative of ω is denoted by ω. The positive definite matrix representing the satellite's moment of inertia, the 3 In a three-dimensional space, S(ω) represents the antisymmetric matrix of ω, and h w ∈ 3 The total angular momentum of the reaction flywheel is given by u′, which represents the true controllability.

[0045] S2, Coordinate Transformation: Quaternion of the desired coordinate system relative to the initial coordinate system of the satellite at the start of computer motion.

[0046] The relevant operations in step S2 will be introduced with specific examples:

[0047] The quaternion of the initial coordinate system relative to the satellite orbit coordinate system at the start of the maneuver; the formula for calculating the quaternion of the desired coordinate system relative to the initial coordinate system of the satellite at the start of the maneuver is: q OG Let q be the quaternion of the desired coordinate system relative to the orbital coordinate system. iO0 The quaternion of the initial coordinate system relative to the satellite orbit coordinate system at the start of the maneuver; q iO0 The calculation formula is: The q i0 The initial quaternion of the satellite body, q O0 The initial orbital quaternion.

[0048] S3. Attitude trajectory planning is completed by improving the tracking differentiator.

[0049] The relevant operations in step S3 will be introduced with specific examples:

[0050] In the satellite multi-point imaging mission mode, different imaging points correspond to different yaw angles. The satellite's initial state is an arbitrary attitude, and the satellite sets the target attitude to yaw angles of 10°, -10°, 20°, and -20° relative to the orbital coordinate system at 0s, 200s, 300s, and 400s, respectively.

[0051] To achieve the shortest path rotation for the satellite's three-axis attitude transformation, a shortest path is designed using the invariant rotation axis constraint during the three-axis attitude transformation. Constraints are also placed on the satellite's three-axis angular acceleration and angular velocity. Attitude trajectory planning is then completed by improving the tracking differentiator. Specifically, the attitude trajectory planning involves using the rotation axis e... iG0 Obtain the quaternion q of the desired satellite coordinate system relative to the initial coordinate system. G0 angular velocity ω G0 and acceleration a G0 The The e iG0 q iG0 The axis of rotation The q iG0 The imaginary part of θ iG0 The θ represents the axis angle. iG0 =2arccos(q) iG0 The This indicates the desired planning angle (planning axis angle); the stated... The This represents the desired angular velocity (planning axis angular velocity); the... The This represents the desired angular acceleration (planning axis angular acceleration), such as... Figure 2 , 3 As shown in Figures 4 and 5, it can be seen that the planned trajectory of the present invention takes into account both the limitations of the actuator and the maneuverability of the satellite.

[0052] The method of completing attitude trajectory planning by improving the tracking differentiator specifically involves representing the planner as a second-order discrete system; the second-order discrete system specifically refers to: The t represents the index in the discrete time series; The calculation formula is: The For saturation functions, s = sign(y), f = sign(f′-fix(f′)) + fix(f′), where sign() represents the sign function. The r l This represents the maximum angular acceleration of the axis angle. The r l This represents the maximum angular acceleration along the three axes. The The maximum angular acceleration along the x-axis, the The maximum angular acceleration along the y-axis is represented by the following. The z-axis represents the maximum angular acceleration, and y represents an intermediate calculation variable. The value h is a smoothing factor, where fix() is a floor function. This method can balance the stability and speed of the planned path by adjusting the size of the smoothing factor h.

[0053] S4. Calculate the overall control rate.

[0054] The relevant operations in step S4 will be introduced with specific examples:

[0055] The overall control law is designed as the sum of the adaptive control law and the compensating control law. The adaptive control law employs an adaptive gain to ensure that attitude and angular velocity errors approach zero and explicitly considers control input saturation; the compensating control law is used to improve dynamic performance and attitude stability during attitude tracking. This method can be proven to possess global stability of the entire system in the presence of bounded perturbations and parameter uncertainties.

[0056] The total control rate is the sum of the adaptive control rate and the compensation control rate, and its calculation formula is as follows: u = u a +u b The u represents the total control law, the u a U represents the adaptive control law. a =[u a1 ,u a2 ,u a3 The calculation process is as follows: The j = [1,2,3], the δ k Let k be a positive constant, where k is the adaptive gain, and the derivative of k is: γ is a constant representing the convergence rate; q e0 q e The real part of ω ei Represents vector ω e The i-th element in q ei Represents vector q e The i-th element, the s i This represents the i-th element of vector s. Represents ω e The transpose, the q e13 The transpose of q e13 q e The imaginary part of u max To control torque limiting, the s aj Let s be the sliding vector a The elements in, the s a =ω e +k 2 q e13 The q e13 For q e The imaginary part of qe =[q e0 ,q e1 ,q e2 ,q e3 ] is the deviation quaternion, and the q e1 q e2 and q e3 For q e The imaginary part unit, the The q n The q represents the real ontology quaternion. G Represents the real-time expected quaternion in an inertial frame; the The q OG0 The quaternion represents the desired coordinate system relative to the orbital coordinate system at the moment of maneuver. This planned trajectory is based on the trajectory at the moment of maneuver and needs to take into account the real-time changing orbital quaternion. The q O Represents the real-time orbital coordinate system; the ω e =[ω e1 ,ω e2 ,ω e3 ] represents the deviation angular velocity, where ω is... e1 ω e2 and ω e3 For ω e The element in the first row, ω e =ω n -R(q e )ω G The ω n The ω represents the actual angular velocity of the body. G R(q) represents the real-time desired angular velocity in the inertial frame. e ) represents the transformation matrix from the target coordinate system to the body coordinate system; the ω G =ω OG +ω G0 The ω OG This represents the angular velocity of the orbital coordinate system relative to the inertial coordinate system in the desired coordinate system. The ω O R(q) represents the real-time orbital angular velocity. OG0 ) is q OG0 The rotation matrix; such as Figure 5 and Figure 6 As shown, the satellite's attitude control accuracy and attitude stability during imaging meet the imaging control requirements; the u b Indicates the compensation control rate; the The The C e =R(q) e ).

[0057] S5. Obtain the true control rate through the implementing agency.

[0058] The relevant operations in step S5 will be introduced with specific examples:

[0059] The actuator is a flywheel model. In the simulation, the total control law u is input into the flywheel model to obtain the actual control law u′ with noise.

[0060] S6. Calculate the quaternion of the satellite body coordinate system relative to the inertial coordinate system and the angular velocity of the satellite body coordinate system relative to the inertial coordinate system.

[0061] The relevant operations in step S6 will be described using specific examples:

[0062] The angular velocity ω of the satellite body coordinate system relative to the inertial coordinate system is calculated using the satellite dynamics model; the quaternion Q of the satellite body coordinate system relative to the inertial coordinate system is calculated using the satellite kinematics model.

[0063] S7. Obtain the true quaternion and true angular velocity of the body through the measuring mechanism.

[0064] The relevant operations in step S7 will be introduced with specific examples:

[0065] The measuring mechanism includes a gyroscope model and a star sensor model; in the simulation, the angular velocity ω of the satellite's body coordinate system relative to the inertial coordinate system is input into the gyroscope model to obtain the real body angular velocity ω with noise. n In the simulation, the quaternion Q of the satellite body coordinate system relative to the inertial coordinate system is input into the star sensor model to obtain the noisy, real body quaternion q. n .

[0066] S8, Loop Planning Trajectory and Tracking.

[0067] The cyclic planning trajectory and tracking specifically involves: calculating the actual body angular velocity ω... n and the real quaternion q n Enter step S4 to complete the planning and tracking of the desired trajectory.

[0068] Example 2

[0069] This embodiment provides a comparative experiment on a three-axis rapid maneuvering attitude trajectory planning and tracking method for low-Earth orbit remote sensing satellites; the process is as follows:

[0070] First, for multi-point imaging tasks, the maneuverability of this invention is compared with that of FAMF and PD methods, such as... Figure 7 and 8As shown in the diagram, the side swing angle comparison curve and the side swing angular velocity comparison curve show that the present invention can achieve faster convergence in terms of side swing angle and side swing angular velocity compared with the other two methods.

[0071] A stable maneuver is defined as a state where the satellite's orbital angular velocity within a 30-second time window is less than or equal to 0.001° / s (3 times the standard deviation, 99.7% confidence level). The maneuver stabilization time is defined as the time from the start of the stable state to the start of the maneuver. The attitude maneuver stabilization times for the three methods are shown in Tables 2 and 3. To quantify maneuver efficiency, the attitude maneuver time of this invention is considered a 100% benchmark. As can be seen from Tables 2 and 3, the maneuver efficiency of this invention is higher than the other two methods. This proves that the path planned by this invention is the shortest path across the three axes, and the control method of this invention has superior tracking performance, thus improving the satellite's maneuverability.

[0072] Table 2

[0073] Target side angle This invention FAMF PD 10° 73.875s(100％) 86.625s(117％) 84.750s(115％) -10° 35.375s(100％) 49.125s(139％) 52.625s(149％) 20° 41.375s(100％) 54.875s(133％) 57.250s(138％) -20° 46.250s(100％) 59.500s(129％) 61.875s(134％)

[0074] Table 3

[0075]

[0076] This invention also conducts comparative experiments on attitude stability and control accuracy in the Earth-pushing imaging mode. In Earth-pushing imaging mode, satellites require sufficiently low attitude stability and control accuracy to ensure image quality. The attitude planning and control process sets the satellite's initial state to an arbitrary attitude and the target state to a 30-degree side-slip angle relative to the orbital coordinate system. The experimental results show the deviation angle and angular velocity values ​​obtained by this invention as follows: Figure 9 , 10 As shown.

[0077] The attitude stability and control deviation of this invention are considered as a 100% benchmark. The comparison of attitude stability and attitude control deviation after satellite stabilization is shown in Table 3. Experiments show that the attitude stability and control deviation of this invention are superior to the other two methods when the attitude is stable. This demonstrates the superior anti-interference capability of the controller used in this invention. This method achieves progressive tracking of the planned trajectory and suppression of external noise interference.

Claims

1. A method for three-axis fast maneuvering attitude trajectory planning and tracking of a low earth orbit remote sensing satellite, characterized in that, The method comprises the following steps: S1, establishing a satellite kinematics model and a satellite dynamics model; S2, calculating a quaternion of a desired coordinate system relative to a satellite initial coordinate system at a computer motion start time; S3, completing attitude trajectory planning through an improved tracking differentiator; The trajectory planning of the posture is completed by improving the tracking differentiator, specifically: representing the planner as a second-order discrete system; the second-order discrete system is specifically: , the represents an index in a discrete time series; S4, calculating a total control law; total control law for an adaptive control law and a compensation control law the sum of S5, obtaining a real control law through an actuator; The actuator is a flywheel model, and the total control law Input the flywheel model to obtain the real control law ; S6, calculating a quaternion of a satellite body coordinate system relative to an inertial coordinate system and an angular velocity of the satellite body coordinate system relative to the inertial coordinate system; S7, obtaining a real body quaternion and a real body angular velocity through a measuring mechanism; The measurement mechanism includes a gyroscope model and a star sensor model; obtaining the true quaternion and true angular velocity of the satellite body through the measurement mechanism specifically involves: calculating the angular velocity of the satellite body coordinate system relative to the inertial coordinate system. Input the gyroscope model to obtain the actual body angular velocity. The quaternion of the satellite body coordinate system relative to the inertial coordinate system. Input the star sensor model to obtain the true ontology quaternion. ; S8, cyclically planning a trajectory and tracking. 2.The three-axis fast maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 1, characterized in that, The kinematic equation of the kinematic model is specifically: , wherein represents the derivative of the quaternion of the satellite body coordinate system relative to the inertial coordinate system, and represents the real part of , and represents the quaternion of the satellite body coordinate system relative to the inertial coordinate system, and represents the imaginary part of , and represents the derivative of , and represents the skew-symmetric matrix of , and represents a 3-order unit matrix, and represents the angular velocity of the satellite body coordinate system relative to the inertial coordinate system; the dynamic equation of the dynamic model is specifically: , wherein represents the derivative of , and represents the positive definite matrix of the satellite moment of inertia, and represents the skew-symmetric matrix of , and is the total angular momentum of the reaction flywheel, and represents the real control law.

3. The three-axis fast maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 2, wherein a calculation formula of a quaternion of the desired coordinate system relative to an initial coordinate system of the satellite at the maneuver starting moment is: , is a quaternion of the desired coordinate system relative to a satellite orbit coordinate system, is a quaternion of the initial coordinate system relative to the satellite orbit coordinate system at the maneuver starting moment, and .

4. The three-axis rapid maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 3, wherein the attitude trajectory planning is specifically: utilizing a rotation axis to obtain a quaternion of a satellite attitude coordinate system relative to an initial coordinate system , an angular velocity and an angular acceleration ; , wherein the represents a rotation axis of the , and the represents a planned expected angle; , wherein the represents a planned expected angular velocity; , wherein the represents a planned expected angular acceleration.

5. The three-axis fast maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 4, wherein the total control law is a sum of an adaptive control law and a compensation control law, and the calculation formula is: wherein the total control law is represented by, the adaptive control law is represented by, and the compensation control law is represented by. .​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​ 6. The three-axis fast maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 5, wherein the angular velocity of the satellite body coordinate system relative to the inertial coordinate system is calculated by a satellite dynamics model; and the quaternion of the satellite body coordinate system relative to the inertial coordinate system is calculated by a satellite kinematics model. calculated by a satellite dynamics model; and the quaternion of the satellite body coordinate system relative to the inertial coordinate system is calculated by a satellite kinematics model. calculated by a satellite dynamics model; and the quaternion of the satellite body coordinate system relative to the inertial coordinate system is calculated by a satellite kinematics model.

7. The three-axis fast maneuvering attitude trajectory planning and tracking method for a low-orbit remote sensing satellite according to claim 6, wherein the cyclic planning trajectory and tracking is: inputting the real body angular velocity ωr and the real body quaternion qrbt into the model for calculating the total control law in step S4 to complete the planning and tracking of the expected trajectory. and real body quaternion The model for calculating the total control law in step S4 is inputted with the real body angular velocity ωr and the real body quaternion qrbt to complete the planning and tracking of the expected trajectory.

Images