Trajectory tracking control method for multi-degree-of-freedom robot arm recycling failed satellite

Abstract

The application discloses a trajectory tracking control method for recycling failed satellites by a multi-degree-of-freedom mechanical arm, and belongs to the technical field of aerospace. The method of the application is as follows: a control equation of the mechanical arm trajectory tracking is established; an automatic group collection algorithm based on Kane equation is used to establish an explicit dynamic equation for a large flexible combination, and a dynamic equation of the mechanical arm is derived to establish the control equation of the mechanical arm trajectory tracking; based on the control equation, an extended state observer is designed, disturbance estimation is realized by observing the system state, a finite time controller is designed, the robustness of the control system to unknown disturbances is improved, and the convergence speed of attitude stabilization is accelerated; a filter is added for dynamic decoupling in view of vibration of a flexible accessory of the system in the recycling process; a double-closed-loop control strategy of "state observer + finite time attitude controller" combination is used for trajectory tracking, the tracking speed and tracking precision of the mechanical arm are improved, and stable tracking control of the recycling process of the mechanical arm is realized.

Description

Technical Field

[0001] This invention relates to a trajectory tracking control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm, and more particularly to a trajectory tracking control method for a multi-degree-of-freedom robotic arm in the process of recovering a satellite with large flexibility failure, belonging to the field of aerospace technology. Background Technology

[0002] With the development of the space industry and the passage of time, large, defunct satellites (such as fuel-depleted communication satellites and remote sensing satellites) generated by various space missions occupy a large amount of orbital space. These satellites are of high intrinsic value and have large flexible attachments. Extending their lifespan or recovering them not only aligns with the concept of sustainable development but also represents a current research hotspot and challenge in space debris cleanup.

[0003] Using robotic arms to capture such failed satellites and perform life extension operations has become a research hotspot in the aerospace field in recent years. It is worth noting that the combined structure formed after capturing a highly flexible failed satellite using a robotic arm consists of multiple rigid and flexible articulated bodies, resulting in a complex configuration, high model dimensionality, and significant parameter uncertainties, which is detrimental to control design. Furthermore, during the recovery process, the flexible solar panels of the failed satellite will vibrate due to dragging; without active control, this could lead to coupled vibrations or even instability of the combined structure, ultimately causing the capture mission to fail. Summary of the Invention

[0004] The main objective of this invention is to provide a trajectory tracking control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm. This method achieves trajectory tracking control of each joint of the multi-degree-of-freedom robotic arm through a dual closed-loop control strategy combining a state observer and finite-time control, thereby enabling stable recovery control of a highly flexible assembly.

[0005] The objective of this invention is achieved through the following technical solution.

[0006] The trajectory tracking and control method disclosed in this invention for recovering a failed satellite using a multi-degree-of-freedom robotic arm includes the following steps:

[0007] Step 1: Establish the control equations for robotic arm trajectory tracking. For the large flexible assembly, an automatic assembly algorithm based on Kane's equations is used to establish explicit dynamic equations. The dynamic equations of the robotic arm are then derived, thus establishing the control equations for robotic arm trajectory tracking, which facilitates the controller design in Step 2.

[0008] Step 1.1: The dynamic equations of the large flexible assembly are established using an automatic assembly algorithm based on the Kane equation.

[0009]

[0010] in: is the quality matrix of the system as a whole; u = [u1 T u2 T … u n T ] T is the column matrix composed of the generalized velocities of the system in turn; F A is the generalized active force of the system, including the force and torque applied by the outside world and the generalized internal force term of the elastic body; is the nonlinear term of the generalized inertial force of the system.

[0011] Step 1.2: According to the established system dynamics equation (1), the dynamics equation of the manipulator in the combination body can be written as

[0012]

[0013] Where, θ a is the column matrix of the joint angles of the manipulator, M a is the nominal mass matrix of the manipulator, but the influence of the elastic displacement and elastic angle of the manipulator is not considered, which is a function of the joint angle θ a of each manipulator, and is a known quantity; ΔM a is the uncertainty term of the mass matrix, mainly caused by the flexibility deformation of the manipulator and the uncertainty of the mass of the target star; d a is the sum of the internal and external disturbances of the system, including the disturbance of the environmental torque, the disturbance of the service star body motion, the disturbance of the target star flexible vibration, the disturbance of the manipulator flexible vibration, and the unknown nonlinear coupling quantity in the system; T a is the control torque vector composed of the control torque of each joint, and is the control input of the system.

[0014] Step 1.3: According to equation (2), the control equation of the manipulator trajectory tracking is further transformed into

[0015]

[0016] Where, U a is the control quantity; a a (t) is the total uncertainty in the equation, including the control input error caused by the uncertainty of the system inertia and the influence of the total disturbance on the satellite body attitude on the joint motion of the manipulator; and has

[0017] U a = B a0 T a

[0018] a a (t) = ΔB a T a +B a d a

[0019] B a = (M a + ΔM a ) -1

[0020]

[0021] ΔB a = B a -B a0

[0022] After the above processing, the control equation (3) becomes a second-order differential equation related to the rotation angle vector θ a , and U a is the control variable to be designed. Trajectory tracking control is to design the control torque vector T a so that

[0023]

[0024] where θ ar and are the desired joint angles and angular velocities of the robot arm, respectively, which are given by the trajectory planning of the robot arm. In addition to the desired rotation angle θ ar and the desired angular velocity , the trajectory planning also gives the desired angular acceleration of each joint. The desired angular acceleration is a known quantity, which is very important for trajectory tracking control.

[0025] Step two: based on the trajectory tracking control equation of the robot arm established in step one, an extended state observer is designed to estimate the disturbance using the observation of the system state, thereby providing a basis for control compensation; then a finite-time controller is designed to improve the robustness of the control system to unknown disturbances and to speed up the convergence speed of the attitude stabilization; in view of the vibration of the flexible accessory of the system during the recovery process, a filter is added for dynamic decoupling. The "state observer + finite-time attitude controller" combined double-loop control strategy is used for trajectory tracking to improve the tracking speed and accuracy of the robot arm, and to realize stable tracking control of the robot arm during the recovery process.

[0026] The "state observer + finite-time attitude controller" combined double-loop control strategy, with the angular velocity of each joint of the robot arm as the input of the inner loop, uses the estimated value of the uncertain quantity output by the second-order extended state observer, and the uncertain quantity is compensated in the output control quantity of the outer loop feedback controller, which can improve the accuracy of trajectory tracking control.

[0027] Step 2.1: The extended state observer is designed to track the states of each joint of the manipulator. A double closed-loop control is adopted, which includes an inner loop compensation and an outer loop feedback, to estimate and compensate the total disturbance in real time.

[0028] Step 1: The control equation (3) is a first-order system without nonlinear terms, and the system measurement is Therefore, a second-order extended state observer is used to estimate the system state and the total uncertainty a a (t) in the equation

[0029]

[0030] where β a1 > 0, β a2 > 0, 0 < α a0 < 1, and δ a0 > 0 are design parameters; e a0 , z a1 , z a2 , and are column vectors with the same dimension as the degrees of freedom of the manipulator system; z a1 and z a2 track the system state and the total uncertainty a a (t) in the equation, respectively; fal(e a0 , α a0 , δ a0 ) is a nonlinear function defined as

[0031]

[0032] where

[0033]

[0034] where sgn(·) denotes the sign function.

[0035] Define the state tracking error as and e a0 = z a1 - a a1 (t), where it is easy to see that e a2 = e a0 ; then for the extended state observer designed for equation (5), the state error equation it tracks is

[0036]

[0037] As long as the parameters β a0 , β a , α a , and δa0 Selecting appropriate and satisfying the stability condition of second order extended state observer

[0038]

[0039] The steady-state error of the extended state observer shown in equation (5) will converge to zero in finite time

[0040]

[0041] Where W a is the bound of , that is and are the maximum estimation errors of the state observer for the system state and the total uncertainty a a (t) in equation, respectively. Analysis shows that as long as β a2 is large enough, and will be small enough, at this time the states z a1 and z a2 of the extended state observer will estimate the system state and the total uncertainty a a (t) in equation in real time, respectively.

[0042] Using the estimated value z a2 , the dynamic feedback compensation law for the robot control equation (3) is as follows

[0043] U a = U a0 -z a2 (11)

[0044] Then the control equation (3) will become the following second-order linear system

[0045]

[0046] When and are small enough, the estimation error of the uncertainty a a (t) can be ignored, so the control equation is further simplified to

[0047]

[0048] This equation (13) provides the basis for the design of step 2.2 sliding mode surface.

[0049] Step 2.2: Based on the designed extended state observer in step 2.1, a fast terminal sliding surface and a finite-time controller are designed by using the finite-time control law to improve the convergence speed of the system attitude stability.

[0050] Firstly, define the joint trajectory tracking error state variable of the manipulator as

[0051]

[0052] The state space form of the joint trajectory tracking error dynamics equation of the manipulator is

[0053]

[0054] For the simplified control equation (13) in step 2.1, the following fast terminal sliding surface is designed

[0055]

[0056] Where, β a = d[β a1 ,β a2 ,β a3 ] > 0; 0 < α a < 1; the derivative of the sliding surface is

[0057]

[0058] Based on the sliding surface (16), the finite-time controller of the system is designed as

[0059]

[0060] Where, K sa1 = d[K sa1,1 , K sa1,2 , K sa1,3 ] > 0.

[0061] The stability of model (15) under the designed sliding surface (16) and controller (18) is analyzed as follows:

[0062] Introduce the stability criterion of finite-time control system: for the system f(0) = 0, If there exists a positive definite continuous function V(x) defined on the neighborhood U0 of the origin and real numbers a > 0, b > 0, 0 < p < 1, which satisfy Then the system is finite-time stable.

[0063] Firstly, the stability of the sliding surface is analyzed, considering the Lyapunov function

[0064]

[0065] Taking the time derivative and substituting into (15)-(18), we have

[0066]

[0067] where is the parameter matrix K sa1 The minimum element on the diagonal. Then, according to the finite-time stability criterion, the sliding surface (16) is finite-time stable, i.e., the system state can converge to s a = 0 in finite time.

[0068] The stability of the system state after converging to s a = 0 is analyzed below. Consider the Lyapunov function

[0069]

[0070] Taking the time derivative, since when s a = 0, we have Then

[0071]

[0072] where β a，min is the parameter matrix β a The minimum element on the diagonal. According to the finite-time stability criterion, the system state will further converge to {x1= 0, x2= 0} in finite time after reaching s a = 0, i.e., the joint trajectory tracking error of the manipulator will converge to zero in finite time.

[0073] The model (3) will converge to

[0074] Step 2.3: Based on the combined control law designed in steps 2.1 and 2.2, construct a first-order linear filter for dynamic decoupling to avoid unnecessary energy consumption in the attitude control system due to vibration of the flexible appendage.

[0075] The control action excites the modal vibration by filtering. The extended state observer itself already has the function of filtering the measured signal, so only the output of the feedback compensation law (11) needs to be filtered. Design a first-order linear filter as

[0076]

[0077] where β Ta > 0, and select an appropriate value according to the low-order modal frequency.

[0078] The actual mechanical arm joint control torque instruction is

[0079]

[0080] Wherein, as described above, M a is a nominal mass matrix of the mechanical arm, and is a function of the mechanical arm rotation angle θ a Real-time calculation is required according to the measured value of the mechanical arm rotation angle θ a .

[0081] Step 2.4: Based on the extended state observer described in step 2.1, the fast terminal sliding mode surface and the finite time controller described in step 2.2, and the filter described in step 2.3, trajectory tracking is performed by adopting a double closed-loop control strategy of the combination of the "state observer + finite time attitude controller", the tracking speed and accuracy of the mechanical arm are improved, and stable tracking control of the mechanical arm recovery process is realized.

[0082] Based on the trajectory tracking control equation of the mechanical arm established in step one, an extended state observer is designed, disturbance estimation is realized by observing the system state, thereby providing a basis for control compensation; then a finite time controller is designed, the robustness of the control system to unknown disturbances is improved, and the convergence speed of attitude stabilization is accelerated; in view of the vibration of the flexible accessory of the system in the recovery process, a filter is added for dynamic decoupling. By adopting a double closed-loop control strategy of the combination of the "state observer + finite time attitude controller", trajectory tracking is performed, the tracking speed and accuracy of the mechanical arm are improved, and stable tracking control of the mechanical arm recovery process is realized.

[0083] Beneficial effects:

[0084] 1. The trajectory tracking control method for the multi-degree-of-freedom mechanical arm to recover the failed satellite disclosed in the application, by designing an extended state observer to observe the state of each joint of the mechanical arm, real-time disturbance is quickly estimated and compensated, then a fast terminal sliding mode surface and a finite time controller are designed, the robustness of the control system to unknown disturbances is improved, the convergence speed of attitude stabilization is accelerated, and in view of the energy consumption problem caused by the vibration of the flexible accessory of the system, a first-order linear filter is constructed for dynamic decoupling, thereby realizing stable recovery control of the large flexible assembly.

[0085] 2. The trajectory tracking control method for the multi-degree-of-freedom mechanical arm to recover the failed satellite disclosed in the application adopts a double closed-loop control strategy of the combination of the "state observer + finite time attitude controller". The inner loop takes the angular velocity of each joint of the mechanical arm as input, uses a second-order extended state observer to output the estimated value of the uncertain quantity, the uncertain quantity is compensated in the output control quantity of the outer loop feedback controller, the speed and accuracy of the trajectory tracking control are improved, and stable tracking control of the mechanical arm recovery process is realized.

[0086] 3. The trajectory tracking control method for a multi-degree-of-freedom manipulator to recycle a failed satellite according to the present application, on the basis of the designed extended state observer, a finite time controller with stability is designed for each of the seven channels of the manipulator system by using a finite time control law, so as to accelerate the convergence speed of the system attitude stability. Compared with the traditional PD controller, the finite time control law used for each channel has a fast convergence speed, good anti-interference and robust performance.

[0087] 4. The trajectory tracking control method for a multi-degree-of-freedom manipulator to recycle a failed satellite according to the present application, the dynamics equation of the large flexible assembly is established by using an automatic group set algorithm based on the Kane equation, and then the dynamics equation of the manipulator system is obtained, so as to establish the control equation of the manipulator trajectory tracking. The Kane equation can avoid tedious derivative operation, the obtained dynamics equation has a simple form and clear physical meaning, is convenient for the design of the controller, and is suitable for complex spacecraft system modeling. BRIEF DESCRIPTION OF DRAWINGS

[0088] Figure 1 It is a configuration diagram of the large flexible assembly after capture in the embodiment of the present application.

[0089] Wherein: A is a service satellite; B is a failed satellite; 10 and 13 are rigid manipulators; 11 and 12 are flexible manipulators; 21 and 31 are flexible panels of the service satellite; and 41 and 51 are flexible panels of the failed satellite.

[0090] Figure 2 It is a total flow chart of the trajectory tracking control method for a multi-degree-of-freedom manipulator to recycle a failed satellite according to the present application.

[0091] Figure 3 It is a structure block diagram of the trajectory tracking control method for a multi-degree-of-freedom manipulator to recycle a failed satellite according to the present application.

[0092] Figure 4 It is a curve diagram of the joint angle and angular velocity control error of the manipulator under the action of the FTA controller in the embodiment of the present application, wherein Figure 4 (a) is a curve diagram of the joint angle control error, and Figure 4 (b) is a curve diagram of the joint angular velocity control error.

[0093] Figure 5 It is a curve diagram of the joint angle and angular velocity control error of the manipulator under the action of the PDA controller in the embodiment of the present application, wherein Figure 5 (a) is a curve diagram of the joint angle control error, and Figure 5 (b) is a curve diagram of the joint angular velocity control error. DETAILED DESCRIPTION

[0094] The following describes in detail, with reference to embodiments and accompanying drawings, a trajectory tracking and control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm, as disclosed in this invention.

[0095] The simulation model used in this example is Figure 1 The large flexible composite model shown consists of a satellite body with a side length of 2m and two solar panels on it, a 7-DOF robotic arm, and a target satellite body with a side length of 2m and two solar panels on it. Among them, the service satellite, the target satellite body, and the shorter robotic arm are rigid bodies, while the satellite body, the target satellite body, the attached solar panels, and the longer robotic arm are considered flexible bodies.

[0096] The masses and geometric parameters of each individual unit in the combined system are as follows:

[0097] Service quality M b =1000kg, moment of inertia of the main shaft J b =d[2000 / 3,2000 / 3,2000 / 3]kg·m 2 The operator d[x] returns a square matrix with diagonal elements of the components of vector x; the target star mass M bT =1500kg, moment of inertia of the main shaft J bT =d[1000,1000,1000]kg·m 2 Windsurfing mass M p = 37.6546 kg, with the geometric parameter taken as width b p = 1.72m, length is h p = 6.795m, thickness d p =0.002m; When considering the mass parameters of the robotic arm, the influence of the hinges on the system mass parameters needs to be taken into account. Assume that the mass of the three hinges of the robotic arm is M. joint = 1kg. The inner diameter r of the robotic arm. m =0.03m, outer diameter R m =0.05m, material density ρ = 7850kg / m³ 3 The length dimension parameters are l 10 =0.2m, l 11 =1m,l 12 =1.2m, l 13 =0.3m.

[0098] like Figure 2 The flowchart shown in this embodiment illustrates the overall process of trajectory tracking and control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm. The specific implementation steps are as follows:

[0099] Step one: Establish the trajectory tracking control equation of the manipulator. The automatic assembly algorithm based on Kane equation is used to establish the explicit dynamic equation of the large flexible assembly, and the dynamic equation of the manipulator is derived, so as to establish the control equation of the trajectory tracking of the manipulator, which is convenient for the design of the controller in step two.

[0100] Step 1.1: The dynamic equation of the large flexible assembly is established by using the automatic assembly algorithm based on Kane equation as

[0101]

[0102] Among them: is the mass matrix of the system as a whole; u=[u1 T u2 T … u n T ] T , is the column matrix composed of the system generalized speed in turn, the generalized speed and initial value in this embodiment are given in table 1; F A is the generalized active force of the system, including the force and torque applied by the outside world, and the generalized internal force term of the elastic body; is the nonlinear term of the system generalized inertia force.

[0103] Table 1 initial value of system generalized speed

[0104]

[0105]

[0106] Step 1.2: According to the established system dynamic equation (25), the dynamic equation of the manipulator in the assembly can be written as

[0107]

[0108] Among them, θ a is the column matrix of the rotation angle of each joint of the manipulator, here M a is the nominal mass matrix of the manipulator, but the influence of the elastic displacement and elastic rotation angle of the manipulator is not considered, which is the function of the rotation angle θ a of each manipulator, which is a known quantity; ΔM a is the uncertainty term of the mass matrix, which is mainly caused by the flexible deformation of the manipulator, the mass uncertainty of the target star and other factors; d a is the sum of the internal and external disturbances of the system, including the disturbance of the environmental torque, the disturbance of the target star body motion, the disturbance of the target star flexible vibration, the disturbance of the manipulator flexible vibration and the unknown nonlinear coupling quantity in the system; T a is the control torque vector composed of the control torque of each joint, which is the control input of the system.

[0109] Step 1.3: According to the dynamic equation (26), the control equation of the manipulator trajectory tracking can be further written as

[0110]

[0111] where, U a is the control variable; a a (t) is the total uncertainty in the equation, including the control input error caused by the uncertainty of the system inertia and the influence of the total disturbance on the satellite attitude and the joint movement of the manipulator; and there is

[0112] U a = B a0 T a

[0113] a a (t) = ΔB a T a + B a d a

[0114] B a = (M a + ΔM a ) -1

[0115]

[0116] ΔB a = B a - B a0

[0117] Step 2: Based on the manipulator trajectory tracking control equation established in step 1, an extended state observer is designed to estimate the disturbance by observing the state of the system, thereby providing a basis for control compensation. Then, a finite-time controller is designed to improve the robustness of the control system to unknown disturbances and speed up the convergence speed of attitude stabilization. In view of the vibration of the flexible accessory in the system during the recovery process, a filter is added for dynamic decoupling. The double closed-loop control strategy of "state observer + finite-time attitude controller" combination is adopted for trajectory tracking to improve the tracking speed and accuracy of the manipulator, and to realize stable tracking control of the manipulator recovery process.

[0118] Step 2.1: An extended state observer is designed to track the state of each joint of the manipulator, and a double closed-loop control of inner loop compensation and outer loop feedback is adopted to quickly estimate and compensate the total disturbance observed in real time.

[0119] The control equation (27) established in step 1 is a first-order system without nonlinear terms, and the system measurement is Therefore, a second-order extended state observer is used to analyze the system state variables and the total uncertainty 'a' in the equations. a (t) is estimated.

[0120]

[0121] Where, β a1 >0, β a2 >0, 0<α a0 <1,δ a0 >0 represents parameters to be designed; e a0 , z a1 , z a2 and All are column vectors, and their dimensions are equal to the degrees of freedom of the robotic arm system; z a1 and z a2 Track system state variables separately The total uncertainty a in the equation a (t).

[0122] Define the state tracking error as and e a2 =z a2 -a a (t), as long as parameter β a1 β a2 α a0 and δ a0 satisfy The steady-state error of the extended state observer shown in equation (28) will converge to the mean in finite time.

[0123]

[0124] Among them, W a for The boundary, that is and The state observers react to the system state variables respectively. The total uncertainty a in the equation a The maximum estimation error of (t). It can be seen that as long as β a2 Big enough and The value will be small enough that the state z of the extended state observer will be small enough. a1 and z a2 The system state variables will be estimated in real time. The total uncertainty a in the equation a (t).

[0125] Using the estimated value z a2 The following dynamic feedback compensation law is applied to the control equation (27) of the robotic arm:

[0126] U a = U a0 - z a2 (30)

[0127] Then the control equation will become a second order linear system as follows,

[0128]

[0129] When and are small enough, the estimation error of the uncertain quantity a a (t) has negligible effect on the dynamics of the manipulator, thus the control equation can be further simplified as

[0130]

[0131] Equation (32) provides the basis for the design of the step 2.2 sliding surface.

[0132] Step 2.2: Based on the extended state observer designed in step 2.1, a fast terminal sliding surface and a finite time controller are designed through a finite time control law to improve the convergence speed of the system attitude stability.

[0133] Firstly, define the manipulator joint trajectory tracking error state variable as

[0134]

[0135] Then the state space form of the manipulator joint trajectory tracking error dynamics equation is

[0136]

[0137] For the simplified control equation (32) of step 2.1, the following fast terminal sliding surface is designed

[0138]

[0139] Where, β a = d[β a1 , β a2 , β a3 ] > 0; 0 < a a < 1; then the derivative of the sliding surface is

[0140]

[0141] Based on the sliding surface (35), the finite time controller of the system can be designed as

[0142]

[0143] Where, Ksa1 =d[K sa1,1 ,K sa1,2 ,K sa1,3 >0.

[0144] The stability of model (34) under the action of the designed sliding surface (35) and controller (37) is analyzed below. The stability analysis is as follows:

[0145] First, a stability analysis of the sliding surface is performed, considering the Lyapunov function.

[0146]

[0147] Taking its time derivative and substituting it into equations (34)-(37), we get...

[0148]

[0149] in, For parameter matrix K sa1 The smallest element on the diagonal. Therefore, according to the finite-time stability criterion, the sliding surface (35) is finite-time stable, meaning the system state converges to s in finite time. a =0.

[0150] The following analysis shows that the system state converges to s. a Stability after = 0. Consider the Lyapunov function.

[0151]

[0152] Taking its time derivative, since when s a =0 when but

[0153]

[0154] Where, β a，min For the parameter matrix β a The smallest element on the diagonal. According to the finite-time stability criterion, the system state reaches state s. a After x1 = 0, the trajectory tracking error of the robotic arm joint will converge to zero within a finite time.

[0155] Thus, it can be seen that model (27) will converge to the desired state in a finite time under the combined action of the state observer (28), dynamic feedback compensation law (30), sliding surface (35), and controller (37).

[0156] Step 2.3: Based on the combination control law designed in step 2.1 and 2.2, a first-order linear filter is designed to dynamically decouple the system to avoid unnecessary energy consumption in the attitude control system due to the vibration of the flexible appendage.

[0157] A practical way to solve the problem of the control action exciting the modal vibration is to filter. The extended state observer itself has the function of filtering the measured signal, so it is only necessary to filter the output of the feedback compensation law. A first-order linear filter is designed as

[0158]

[0159] where β Ta > 0, which can be selected according to the low-order modal frequency.

[0160] The actual control torque command of each joint of the mechanical arm is

[0161]

[0162] where, as described above, M a is the nominal mass matrix of the mechanical arm, which is a function of the mechanical arm rotation angle θ a , and needs to be calculated in real time according to the measured value of the mechanical arm rotation angle θ a .

[0163] The structure block diagram of the trajectory tracking control method for the multi-degree-of-freedom mechanical arm recycling the failed satellite in combination with Figure 3 illustrates the implementation of the embodiment, and the control method and implementation method are the same as the content of the invention.

[0164] In order to compare the control effect, the designed "state observer + finite time trajectory tracking controller" double closed-loop control strategy (hereinafter referred to as FTA controller) is compared with the "state observer + PD attitude controller" double closed-loop control strategy (hereinafter referred to as PDA controller). The PD attitude controller is selected as

[0165]

[0166] where, K ap = d[K ap,1 , K ap,2 , K ap,3 ] > 0; K ad = d[K ad,1 , K ad,2 , K ad,3 ] > 0.

[0167] The nominal total inertia matrix of the system and the nominal mass matrix of the robotic arm are taken as 80% of the actual total inertia matrix of the system and the actual mass matrix of the robotic arm, respectively. Initially, the three-axis attitude angles of the serving star are [3; -3; 2]°, and the attitude angular velocity is [0; 0; 0]° / s. The control objective is to achieve attitude stabilization control.

[0168] The selection of control parameters for robotic arm trajectory tracking is shown in Table 2, where the operator d[x] returns a square matrix with the components of vector x as diagonal elements.

[0169] Table 2 Trajectory Tracking Control Parameters

[0170]

[0171]

[0172] After 15 seconds, the results show that, in this embodiment, both dual-closed-loop control strategies based on the combination of "state observer + trajectory tracking controller" can achieve stable recovery control of the large flexible assembly. Among them, Figure 4 The results show that the FTA controller achieves a joint rotation angle tracking accuracy of better than 0.03 deg and a joint angular velocity tracking accuracy of better than 0.3 deg / s for the robotic arm. Figure 5 This indicates that the PDA controller achieves a joint rotation angle tracking accuracy of only 0.07 degrees and a joint angular velocity tracking accuracy of approximately 0.03 degrees per second for the robotic arm. (Comparison) Figure 4 and Figure 5 It can be seen that the FTA controller has higher joint angle tracking accuracy than the PDA controller. Comparative analysis of the overall error curves shows that the FTA controller has better trajectory tracking accuracy and response speed, and also clearly exhibits the characteristics of a finite-time control method. The comparative results indicate that the trajectory tracking control method disclosed in this invention for recovering failed satellites using a multi-degree-of-freedom robotic arm is more capable of achieving stable recovery control of highly flexible assemblies.

[0173] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description is merely a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A trajectory tracking control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm, characterized by: Includes the following steps, Step 1: Establish the control equations for robotic arm trajectory tracking; For the large flexible assembly, an explicit dynamic equation is established using an automatic assembly algorithm based on the Kane equation. The dynamic equation of the robotic arm is then derived, thereby establishing the control equation for the robotic arm trajectory tracking, which facilitates the design of the controller in step two. The implementation method for step one is as follows: Step 1.1: The dynamic equations of the large flexible assembly are established using an automatic assembly algorithm based on the Kane equation. in: It is the overall quality matrix of the system; u = [u1 T u2 T …u n T ] T F is an array composed of the system's generalized rates arranged sequentially; A It is the generalized active force of the system, including the forces and torques applied from the outside, as well as the generalized internal force terms of the elastic body; It is the nonlinear term of the system's generalized inertial force; Step 1.2: Based on the established system dynamics equation (1), the dynamics equation of the robotic arm in the assembly can be written as follows: Where, θ a The joints of the robotic arm are arranged in an array. M a Let be the nominal mass matrix of the robotic arm, but without considering the effects of elastic displacement and elastic rotation angle of the robotic arm; θ represents the rotation angle of each robotic arm. a A function of , where is a known quantity; ΔM a The uncertainty term in the mass matrix is ​​mainly caused by the flexible deformation of the robotic arm and the mass uncertainty of the target star; d a This is the sum of internal and external disturbances to the system, including disturbances from environmental torques, disturbances from the motion of the serving star, disturbances from the flexural vibrations of the target star, disturbances from the flexural vibrations of the robotic arm, and unknown nonlinear couplings within the system; T a The control torque vector, which is composed of the control torques of each joint, is the control input of the system. Step 1.3: According to equation (2), the control equation for the robotic arm trajectory tracking is further transformed into: Among them, U a For control quantity; a a (t) represents the total uncertainty in the equation, including the control input error caused by the uncertainty of the system inertia and the influence of the total disturbance to the satellite's attitude on the joint motion of the robotic arm; and has U a =B a0 T a a a (t)=ΔB a T a +B a d a B a =(M a +ΔM a ) -1 ΔB a =B a -B a0 After the above processing, the governing equation (3) becomes the equation relating to the rotation vector θ. a The second-order differential equation, and U a The variable to be controlled is the control torque vector T; trajectory tracking control is to design the control torque vector T. a , making Where, θ ar and These are the desired joint angles and angular velocities of the robotic arm, respectively, provided by the robotic arm trajectory planning; the trajectory planning also provides the desired rotation angle θ of each joint. ar and expected angular velocity In addition, the expected angular acceleration of each joint is given. Step 2: Based on the robotic arm trajectory tracking control equations established in Step 1, an extended state observer is designed to estimate disturbances by observing the system state, thus providing a basis for control compensation; a finite-time controller is designed to improve the robustness of the control system to unknown disturbances and accelerate the convergence speed of attitude stabilization; a filter is added for dynamic decoupling to address the vibrations generated by the system's flexible attachments during the recovery process; a dual closed-loop control strategy combining the "state observer + finite-time attitude controller" is adopted for trajectory tracking to improve the robotic arm's tracking speed and accuracy, achieving stable tracking control during the robotic arm recovery process; The second step is implemented as follows: Step 2.1: Design an extended state observer to track the state of each joint of the robotic arm. Use a dual closed-loop control with inner loop compensation and outer loop feedback to quickly estimate and compensate for the total disturbance obtained from real-time observation. Step 2.2: Based on the extended state observer designed in Step 2.1, a fast terminal sliding surface and a finite-time controller are designed using a finite-time control law to improve the convergence speed of the system's attitude stability. Step 2.3: Based on the combined control law designed in Steps 2.1 and 2.2, a first-order linear filter is constructed for dynamic decoupling to avoid unnecessary energy consumption in the attitude control system due to the vibration of the flexible attachment; Step 2.4: Based on the extended state observer described in Step 2.1, the fast terminal sliding surface and finite-time controller described in Step 2.2, and the filter described in Step 2.3, trajectory tracking is performed by adopting a dual closed-loop control strategy combining "state observer + finite-time attitude controller" to improve the tracking speed and accuracy of the robotic arm and achieve stable tracking control during the robotic arm's recovery process. The dual closed-loop control strategy combining the "state observer + finite-time attitude controller" uses the angular velocity of each joint of the robotic arm as input in the inner loop and the estimated value of the uncertainty is output by the second-order extended state observer. The uncertainty is compensated in the control quantity output by the outer loop feedback controller, which can improve the accuracy of trajectory tracking control.

2. The trajectory tracking and control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm as described in claim 1, characterized in that: Step 2.1 is implemented as follows: The governing equation (3) established in step one is a first-order system that does not contain nonlinear terms, and the system quantities are as follows: Therefore, a second-order extended state observer is used to analyze the system state variables and the total uncertainty 'a' in the equations. a (t) is estimated Where, β a1 >0, β a2 >0, 0<α a0 <1,δ a0 >0 represents parameters to be designed; e a0 , z a1 , z a2 and All are column vectors, and their dimensions are equal to the degrees of freedom of the robotic arm system; z a1 and z a2 Track system state variables separately The total uncertainty a in the equation a (t); fal(e) a0 ,α a0 ,δ a0 ) is a nonlinear function, defined as in, Where sgn(·) represents the sign function; Define the state tracking error as and e a2 =z a2 -a a (t), where it is easy to see e a0 =e a1 For the extended state observer designed according to equation (5), its tracking state error equation is: As long as parameter β a1 β a2 α a0 and δ a0 Choose an appropriate one that satisfies the stability conditions of the second-order extended state observer. The steady-state error of the extended state observer shown in equation (5) will converge to a finite time. Among them, W a for The boundary, that is and The state observers react to the system state variables respectively. The total uncertainty a in the equation a The maximum estimation error of (t) is shown in the analysis, as long as β a2 Big enough and The value will be small enough that the state z of the extended state observer will be small enough. a1 and z a2 The system state variables will be estimated in real time. The total uncertainty a in the equation a (t); Using the estimated value z a2 The following dynamic feedback compensation law is applied to the control equation (3) of the robotic arm. U a =U a0 -z a2 (11) Then the control equation (3) will become the following second-order linear system. when and When the value is sufficiently small, the uncertainty a a The estimation error of (t) has a negligible impact on the dynamics of the robotic arm, therefore the governing equations are further simplified to Equation (13) provides the basis for the design of the sliding surface in step 2.

2.

3. The trajectory tracking and control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm as described in claim 2, characterized in that: Step 2.2 is implemented as follows: First, define the state variable for the robotic arm joint trajectory tracking error as... The state-space form of the dynamic equation for the tracking error of the robotic arm joint trajectory is: For the simplified control equation (13) in step 2.1, the following fast terminal sliding surface is designed. Where, β a =d[β a1 ,β a2 ,β a3 ]>0; 0<α a <1;; then the derivative of the sliding surface has Based on the sliding surface (16), the finite-time controller of the system is designed as follows: Among them, K sa1 =d[K sa1,1 ,K sa1,2 ,K sa1,3 >0; The stability of model (15) under the action of the designed sliding surface (16) and controller (18) is analyzed below. The stability analysis process is as follows: Introducing a stability criterion for finite-time control systems: For the system f(0) = 0, If there exists a positive definite continuous function V(x) defined on the neighborhood U0 of the origin, and real numbers a > 0, b > 0, 0 < p < 1, satisfying Therefore, the system is stable in a finite time. First, a stability analysis of the sliding surface is performed, considering the Lyapunov function. Taking its time derivative and substituting it into equations (15)-(18), we obtain... in, For parameter matrix K sa1 The smallest element on the diagonal; then according to the finite-time stability criterion, the sliding surface (16) is finite-time stable, that is, the system state can converge to s in finite time. a =0; The following analysis shows that the system state converges to s. a Stability after = 0; consider the Lyapunov function. Taking its time derivative, since when s a =0 when but Where, β a，min For the parameter matrix β a The smallest element on the diagonal; according to the finite-time stability criterion, the system state reaches s. a After x1 = 0, it will further converge to {x1 = 0, x2 = 0} in a finite time, that is, the tracking error of the robotic arm joint trajectory will converge to zero in a finite time. Model (3) will converge to the state in a finite time under the combined action of the state observer (5), dynamic feedback compensation law (11), sliding surface (16), and controller (18).

4. The trajectory tracking and control method for recovering a failed satellite using a multi-degree-of-freedom robotic arm as described in claim 3, characterized in that: Step 2.3 is implemented as follows: The control action excitation modal vibration is solved by filtering; the extended state observer itself has the function of filtering the measurement signal, so only the output of the feedback compensation law (11) needs to be filtered; a first-order linear filter is designed as follows: Where β Ta >0, select an appropriate value based on the low-order modal frequency; The actual control torque commands for each joint of the robotic arm are as follows: Among them, as mentioned above, M a The nominal mass matrix of the robotic arms is the rotation angle θ of each robotic arm. a The function, based on the rotation angle θ of each robotic arm. a The measured values ​​are calculated in real time.

Images