A Lie group-based control method for multi-degree-of-freedom robotic satellite

Abstract

The present invention provides a multi-degree-of-freedom robot satellite control method based on Lie group. First, a multi-degree-of-freedom robot satellite is designed. Then, the kinematic model of the robot satellite is constructed based on the spinor theory based on Lie group SE (3). The dynamic equation of the robot satellite is established by the Euler-Lagrange method. Finally, a sliding mode variable structure control law with good robustness is designed to perform motion control based on the dynamic model of the robot. The present invention solves the problem of too complicated coordinate system in multi-body system modeling and effectively reduces the amount of calculation. The sliding mode control law is designed based on this model. Compared with the traditional PID control, the sliding mode control law has the advantages of faster response speed, fewer adjustment parameters and better robustness.

Description

Technical Field

[0001] The invention belongs to the field of aerospace technology and relates to a multi-freedom robot satellite control method based on Lie groups. Background Art

[0002] With the increasing exploration and development of human beings in the field of outer space, major space missions such as future space solar power generation, large-capacity high-speed communication, high-resolution space-based imaging, and interstellar exploration urgently need large-scale space structures such as giant solar arrays, large-aperture antennas, large-aperture telescopes, large-aperture cameras, and large-scale optical and electric propulsion sails. Due to the limited rocket carrying capacity and the high complexity of large space structures, it is impossible to achieve the required space structure's orbital entry and operation through a single launch vehicle launch and orbital deployment. Therefore, the on-orbit assembly technology based on robotic satellites has emerged. The multi-degree-of-freedom robotic satellite can effectively assemble modular structural components on-orbit, which is no longer restricted by the above conditions; at the same time, it has the ability to change the configuration of the space structure on-orbit to flexibly apply various mission scenarios; in addition, it can be used to replace faulty modules to improve the reliability of the system. However, in order to enable the multi-degree-of-freedom robotic satellite to reliably complete the on-orbit assembly task, the problem of robot dynamics and control is involved. Given the extremely high degrees of freedom of the multi-degree-of-freedom robotic satellite system and its complex nonlinear dynamic characteristics, it is extremely challenging in modeling and control. In terms of modeling, the traditional DH method is often used at home and abroad to establish the kinematic and dynamic models of robot systems. This method requires the establishment of a local body coordinate system at each joint. For robots with multiple degrees of freedom, various coordinate systems are too complicated and computationally complex. In recent years, the spinor method based on Lie group SE(3) has gradually developed to construct robot models. It only needs to establish an inertial coordinate system and a tool coordinate system, which effectively simplifies the description of complex multi-body systems, greatly improving the modeling efficiency while having a clear physical meaning. Summary of the invention

[0003] Purpose of the invention: In order to solve the task of completing the on-orbit assembly of large space structures under the constraints of rocket carrying capacity, fairing appearance and structural complexity, the present invention provides a multi-degree-of-freedom robot satellite control method based on Lie groups, comprising the following steps:

[0004] Step 1, kinematic modeling of the robotic satellite;

[0005] Step 2, dynamic modeling of the robotic satellite;

[0006] Step 3: Establish the robot satellite motion control method.

[0007] In step 1, the robot satellite includes a satellite base and a single 6-DOF robotic arm installed on the satellite base; adjustable photovoltaic solar panels are arranged on both sides of the satellite base, and the satellite base is internally integrated with oxidizer, chemical fuel propellant, airborne nozzle, momentum wheel, inertial measurement unit, airborne industrial computer, robotic arm servo controller module, wireless communication module, battery, solenoid valve and relay; the satellite base is externally provided with a nozzle and an airborne camera, and the nozzle is connected to the airborne nozzle and momentum wheel; the single 6-DOF robotic arm is installed on the satellite base, and can be equipped with various actuators at the end of the robotic arm, such as end actuator for transportation; end actuator for processing; end actuator for measurement, etc., which are respectively used to realize functions such as clamping, processing, and measurement in tasks.

[0008] The photovoltaic solar panel converts the collected light energy into electrical energy, which is then stored in the battery in the satellite base to power the robot satellite when it is working;

[0009] The wireless communication module is used to receive instructions from the ground, so as to operate the robot satellite itself, and can also communicate with other on-orbit equipment to achieve the function of coordinated operation; after receiving the instructions from the ground, the wireless communication module transmits the instruction information to the robot arm servo controller module and the airborne industrial computer, and the robot arm servo controller module and the airborne industrial computer then call the relevant parameter information measured by the inertial measurement unit and the airborne camera, and respectively servo-control the robot arm through relays to achieve the desired required posture of the robot arm tool coordinate system; at the same time, the airborne industrial computer controls the opening and closing degree of the valve for the oxidant and chemical fuel propellant to enter the airborne nozzle through the servo control of the solenoid valve, and the mixed gas burns to generate thrust, which is then ejected from the nozzle, so that the satellite base has the corresponding moving power; the airborne industrial computer will also adjust the attitude direction of the satellite base by controlling the momentum wheel, so that the robot satellite reaches the desired posture.

[0010] Step 1 includes:

[0011] Step 1-1, define the inertial coordinate system and the tool coordinate system;

[0012] Step 1-2, calculate the screw axis of each joint of the robot arm in the initial configuration;

[0013] Steps 1-3, calculate the kinematic equations of a single 6-DOF robotic arm.

[0014] Step 1-1 includes: setting the inertial coordinate system {∑0} at the center of the connection between the robot arm and the satellite base; the tool coordinate system {∑ E}Located at the center of the end effector of the robot arm;

[0015] Solve the homogeneous transformation matrix T(0) of the tool coordinate system relative to the inertial coordinate system when the robot arm is in zero position:

[0016]

[0017] Among them, SE(3) is a three-dimensional special Euclidean group, which is used to represent the translation and rotation of a rigid body in three-dimensional space; the three-dimensional rotation group SO(3) is a special case of the special orthogonal group SO(n) when n=3, R∈SO(3) is a parameter used to describe the rotation of the tool coordinate system in three-dimensional space relative to the inertial coordinate system, and the Lie group refers to a group with continuous and smooth properties. SE(3) and SO(3) are continuous in the real number space, so they are both Lie groups; p belongs to the three-dimensional vector space The element is used to describe the position vector of the origin of the tool coordinate system in the inertial coordinate system.

[0018] Step 1-2 includes: determining the unit rotation axis of each joint at zero position and its position vector relative to the inertial coordinate system, thereby obtaining the unit screw axis of each joint relative to the inertial coordinate system:

[0019]

[0020] in, represents the unit rotation axis of the i-th joint when the robot satellite is at zero position; r i express Position vector relative to the inertial coordinate system; v i is the line distance of the i-th joint, indicating Moment vector about the origin of the inertial coordinate system; S i Represents the unit screw axis of the i-th joint relative to the inertial coordinate system, which belongs to the 6-dimensional vector space The elements in .

[0021] Steps 1-3 include: defining the exponential coordinates of the transformation matrix T(θ)∈SE(3) as Then there is the following mapping relationship:

[0022] The exponential mapping exp: [S]θ∈se(3)→T∈SE(3) from the Lie algebra se(3) to SE(3) corresponding to the special Euclidean group SE(3);

[0023] The logarithmic mapping from SE(3) to se(3) log:T∈SE(3)→[S]θ∈se(3)

[0024] Among them, Lie algebra is a structure corresponding to Lie group. Each Lie group corresponds to a Lie algebra, usually written as lowercase so(3) and se(3). se(3) is the Lie algebra corresponding to Lie group SE(3); exp represents the exponential function with the natural constant e as the base; log represents the logarithmic function; → is a mapping, which represents the mutual correspondence between the elements of two sets. Here, it represents the conversion relationship between the elements in the Lie group and the elements in the Lie algebra;

[0025] Any rigid body motion can be realized by moving around a fixed spiral axis S in space, then:

[0026]

[0027] Where I represents the identity matrix, It means that with e as the base, [S i ]θ i is an exponential function of power; [S]θ is [S i ]θ i The general expression form of , which represents the matrix form of Sθ of the 6-dimensional vector; S is the screw axis i The matrix representation of is the element in the Lie algebra se(3) corresponding to the Lie group SE(3); Represents a three-dimensional vector The antisymmetric matrix of is an element in the Lie algebra so(3) corresponding to the Lie group SO(3); ω x ,ω y ,ω z Represents ω i The bold 0 indicates a zero vector; the unscripted I indicates the identity matrix.

[0028] Finally, the kinematic equation of a single 6-DOF manipulator is obtained as follows:

[0029]

[0030] Among them, T(0) is the homogeneous transformation matrix of the tool coordinate system relative to the inertial coordinate system; T(θ) is the homogeneous transformation matrix of the tool coordinate system relative to the inertial coordinate system after 6 joint changes.

[0031] Step 2 includes:

[0032] Step 2-1, calculate the connecting rod speed;

[0033] Step 2-2, calculate the system kinetic energy;

[0034] Step 2-3, calculate the potential energy of the system;

[0035] Step 2-4, calculate and obtain the dynamic model of the robot satellite.

[0036] Step 2-1 includes: establishing a body coordinate system {∑ C}, the body coordinate system of the i-th link Transformation matrix relative to the inertial coordinate system {∑0} for:

[0037]

[0038] in, It represents the transformation matrix of the coordinate system corresponding to the left subscript i relative to the coordinate system corresponding to the left superscript j. Represents the body coordinate system of the i-th link Transformation matrix relative to the inertial coordinate system {∑0}; Represents the body coordinate system of the second link The body coordinate system relative to the first link The transformation matrix of

[0039] The body coordinate system of the i-th link The object speed V i for:

[0040]

[0041] in is the derivative of the joint variable with respect to time, representing the joint velocity;

[0042] The Jacobian matrix J of the i-th link i (θ) is:

[0043]

[0044] in, express The inverse of a matrix, for The adjoint operator of It is represented as the adjoint operator of the element T in the Lie group SE(3), which is an endomorphism of the Lie group.

[0045] Step 2-2 includes: calculating the system kinetic energy using the following formula

[0046]

[0047] Among them, the system kinetic energy is about θ and Function of In the coordinate system The generalized inertia matrix of connecting rod i is represented by represents the moment of inertia matrix of the i-th rod, m i represents the mass of connecting rod i, and I represents the identity matrix;

[0048] Step 2-3 includes: setting the system potential energy U=0;

[0049] Steps 2-4 include: Lagrangian function of the robotic satellite system for:

[0050]

[0051] By Lagrange equation The dynamic model of the robot system is obtained as follows:

[0052]

[0053] Among them, τ i is the torque of the ith joint, express Taking the derivative with respect to time t, represents L versus θ i Find the partial derivative, is the mass matrix of the robotic satellite; Coriolis matrices for robotic satellites; is the generalized torque vector on each joint.

[0054] Step 3 includes:

[0055] Step 3-1, design the sliding surface:

[0056]

[0057] Among them, s(t) is the synovial surface function; is the derivative of the sliding surface function with respect to time t; α = diag[α1, α2, …, α6] is the sliding surface constant, diag is a diagonal matrix, a diagonal matrix is ​​a matrix in which all elements outside the main diagonal are 0, often written as diag[α1, α2, …, α6], where α1, α2, …, α6 are the elements on its diagonal; θ d e(t) = θ(t) and θ(t) represent the change function of the desired joint angle and the change function of the real-time joint angle respectively; e(t) = θ d (t)-θ(t) is the tracking error of the joint angle; is the tracking speed, is the time derivative of the desired joint angle, is the derivative of the real-time joint angle with respect to time;

[0058] Step 3-2, design the reaching law:

[0059]

[0060] Among them, ε is the gain coefficient; sgn(s) represents the sign function; ks is the exponential approach term;

[0061] Introducing the saturation function sat(s) instead of the sign function sgn(s), the final reaching law is:

[0062]

[0063] The expression of the saturation function is:

[0064]

[0065] Where ψ is called the boundary layer thickness;

[0066] Step 3-3, construct the control law:

[0067] Robotic Satellite Dynamics Model Substitute into There are:

[0068]

[0069] Combined with the reaching law, we get:

[0070]

[0071] Solving the control law τ(t) yields:

[0072]

[0073] Among them, τ eq represents equivalent control; τ sw Indicates switching control;

[0074] Step 3-4, analyze stability:

[0075] Construct the Lyapunov function V:

[0076]

[0077] Where T represents the matrix transpose;

[0078] Then there is

[0079] prove The control law τ can meet the arrival condition, where is the derivative of the function V with respect to time t.

[0080] In terms of control, the sliding mode variable structure control adopted in the present invention has a fast response speed, is robust to external noise interference and parameter perturbations, and can stably control a system with complex nonlinear dynamics such as a robotic satellite.

[0081] Beneficial effects: The present invention uses the Lie group method to perform kinematic and dynamic modeling of a multi-degree-of-freedom robot satellite system, describes the multi-body system more intuitively in geometry, solves the problem of too complex coordinate systems in multi-body system modeling, and effectively reduces the amount of calculation; a sliding mode control law is designed based on this model, which has the advantages of faster response speed, fewer adjustment parameters and better robustness compared to traditional PID control; the sliding mode control with a saturation function is introduced to effectively suppress the high-frequency jitter of the controlled quantity, making the system more stable and reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments, and the above and / or other advantages of the present invention will become more clear.

[0083] Figure 1 This is a schematic diagram of the overall appearance of the robotic satellite.

[0084] Figure 2 It is a schematic diagram of the internal structure and components of the robot satellite base.

[0085] Figure 3 It is a schematic diagram of the change curve of each joint angle.

[0086] Figure 4 It is a schematic diagram of the change curve of each joint angle error.

[0087] Figure 5 It is a schematic diagram of the change curve of the torque of each joint.

[0088] Figure 6 It is a diagram of the robot satellite architecture of the present invention. DETAILED DESCRIPTION

[0089] The present invention provides a Lie group-based multi-degree-of-freedom robot satellite control method, comprising:

[0090] 1. Robot structure design: Figure 1 , Figure 6 As shown, the present invention designs a multi-DOF robotic satellite, which is composed of a satellite base (spacecraft) with 3 DOF and a single 6-DOF robotic arm 1 installed thereon.

[0091] (1) Satellite base: In order to enable the robotic satellite to complete various on-orbit tasks, the internal structure and components of the robotic satellite base are as follows: Figure 2As shown, the robot satellite system is composed of multiple modules and components, including adjustable photovoltaic solar panels 2 placed on both sides of the base to ensure that the robot satellite system obtains electricity from sunlight; it is loaded with chemical fuel propellant 14 and oxidizer 5, and the combination of the onboard nozzle 8 and the nozzle 4 and the momentum wheel 9 provide the satellite with posture driving force for movement and rotation respectively; the onboard camera 3 is used as a relative measurement sensor; the inertial measurement unit 11 is used as an attitude measurement sensor; at the same time, the onboard industrial computer and robot arm servo controller module 6, wireless communication module 13, battery 12, solenoid valve 7 and relay 10 and other components are configured on the base to ensure the normal operation of the robot satellite.

[0092] (2) Robotic arm: This robot arm is a 6-DOF robot arm with 6 rotational joints. Each joint is equipped with a servo motor, angular displacement, angular velocity and torque sensor. The end can be equipped with actuators such as a multi-finger gripper to quickly grasp and transport structural modules.

[0093] 2. Kinematic modeling:

[0094] (1) Define the inertial coordinate system and the end effector (tool) coordinate system

[0095] For the convenience of calculation, the inertial coordinate system {∑0} is set at the center of the connection between the robot arm and the air-floating base; the tool coordinate system {∑ E} is located at the center of the end effector. At the same time, the homogeneous transformation matrix of the tool coordinate system relative to the inertial coordinate system is solved when the robot arm is in zero position:

[0096]

[0097] Among them, SE(3) is also called the three-dimensional special Euclidean group, which is used to represent the translation and rotation of a rigid body in three-dimensional space; the three-dimensional rotation group SO(3) is a special case of the special orthogonal group SO(n) when n=3, R∈SO(3) is a parameter used to describe the rotation of the tool coordinate system in three-dimensional space relative to the inertial coordinate system, and the Lie group refers to a group with continuous and smooth properties. SE(3) and SO(3) are continuous in the real number space, so they are both Lie groups; p belongs to the three-dimensional vector space The element is used to describe the position vector of the origin of the tool coordinate system in the inertial coordinate system.

[0098] (2) Calculate the screw axis of each joint at the initial position (zero position)

[0099] Determine the unit rotation axis of each joint at zero position and its position vector r relative to the inertial coordinate system i , thus obtaining the unit screw axis of each joint relative to the inertial coordinate system:

[0100]

[0101] (3) Use the exponential product formula to recursively derive the kinematic equations of the robot arm

[0102] The exponential coordinates of the transformation matrix T(θ)∈SE(3) are defined as Then there is the following mapping relationship:

[0103] Exponential mapping from se(3) to SE(3) exp:[S]θ∈se(3)→T∈SE(3);

[0104] The logarithmic mapping from SE(3) to se(3) log:T∈SE(3)→[S]θ∈se(3).

[0105] Where se(3) is the Lie algebra corresponding to the Lie group SE(3).

[0106] Any rigid body motion can be realized by moving around a fixed screw axis S in space, then:

[0107]

[0108] in, S is the screw axis i The matrix representation of is the element in the Lie algebra se(3) corresponding to the Lie group SE(3); Represents a three-dimensional vector The antisymmetric matrix of is the element of the Lie algebra so(3) corresponding to the Lie group SO(3); the bold 0 indicates a zero vector.

[0109] Finally, the kinematic equation of the 6R robot arm of the present invention is:

[0110]

[0111] 3. Dynamic modeling:

[0112] (1) Calculate the connecting rod speed (motion rotation)

[0113] In order to calculate the speed of each link of the robot arm, it is necessary to establish a body coordinate system {∑ C}.

[0114] The body coordinate system of the i-th link The transformation matrix relative to {∑0} is:

[0115]

[0116] The body coordinate system of the i-th link The object speed is:

[0117]

[0118] Where: Jacobian matrix

[0119] in, is the adjoint operator of the Lie group SE(3) and is an endomorphism of the Lie group.

[0120] (2) Calculate the system kinetic energy:

[0121]

[0122] In the formula,

[0123] in, In the coordinate system The generalized inertia matrix of connecting rod i represented by represents the moment of inertia matrix of the i-th rod, m i represents the mass of connecting rod i, and I without a subscript represents the identity matrix.

[0124] (3) Calculate the system potential energy:

[0125] When the robotic satellite is in a microgravity environment, the potential energy caused by gravity can be ignored, so the system potential energy U=0.

[0126] (4) Construct the Lagrangian equation and obtain the dynamic model of the robotic satellite system

[0127] The Lagrangian function of the robot system is:

[0128]

[0129] By Lagrange equation The dynamic model of the system is obtained:

[0130]

[0131] Where: is the mass matrix of the robot; is the Coriolis matrix of the robot; is the generalized torque vector on each joint.

[0132] 4. Motion control method:

[0133] The present invention adopts a sliding mode variable structure control method based on reaching law and simultaneously adopts a method of replacing a sign function with a saturation function, thereby improving the response speed and eliminating the high-frequency chattering of the system.

[0134] (1) Design of sliding surface

[0135]

[0136]

[0137] Among them, α = diag[α1, α2, …, α6] is the sliding surface constant; θ d (t) and θ(t) represent the change functions of the desired joint angle and the real-time joint angle respectively; e(t) = θ d (t)-θ(t) is the tracking error of the joint angle; is the tracking speed.

[0138] (2) Design convergence law

[0139] The present invention adopts the exponential approach law:

[0140] In addition, in order to eliminate the high-frequency chattering of the system control variables as much as possible and reduce energy consumption, the present invention introduces the saturation function sat(s) instead of the sign function sgn(s), and the final convergence law is obtained as follows:

[0141]

[0142] The expression of the saturation function is:

[0143]

[0144] Among them, ψ is called the boundary layer thickness and is a very small positive number.

[0145] (3) Construction of control law

[0146] The robot dynamics model Substitute into There are:

[0147]

[0148] Combined with the designed reaching law, we have:

[0149]

[0150] Solving the above equation, we can get the control law:

[0151]

[0152] (4) Stability analysis

[0153] Construct the Lyapunov function:

[0154]

[0155] Then there is

[0156] prove It holds true, so the control law τ can satisfy the arrival condition, so the control system is stable, which proves that the control method designed by the present invention is effective.

[0157] Simulation verification:

[0158] The present invention verifies the effectiveness of the proposed control method through the following examples.

[0159] Assume that the robot is initially in position zero, that is, θ0 = [0,0,0,0,0,0] T , the desired joint rotation angle is Sliding surface constant α=diag[3,3,3,3,3,3] T , the reaching law constant ε=1, k=5, and the boundary layer thickness ψ=0.01. According to the above design parameters, the motion controller of the robot based on the dynamic model is obtained by using the control method proposed in the present invention. Figure 3 , Figure 4 , Figure 5 The figure shows the changing curves of several key parameters in the process of the robot moving from zero position to the desired joint angle. It can be seen from the figure that under the designed sliding mode controller, the robot can move quickly and smoothly to the desired joint angle, and the high-frequency jitter of each joint torque is also significantly eliminated.

[0160] In a specific implementation, the present application provides a computer storage medium and a corresponding data processing unit, wherein the computer storage medium can store a computer program, and when the computer program is executed by the data processing unit, the invention content of a multi-degree-of-freedom robot satellite control method based on Lie groups provided by the present invention and some or all of the steps in each embodiment can be run. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0161] Those skilled in the art can clearly understand that the technical solutions in the embodiments of the present invention can be implemented by means of computer programs and their corresponding general hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention can be essentially or partly contributed to the prior art in the form of a computer program, i.e., a software product, which can be stored in a storage medium and includes several instructions for enabling a device including a data processing unit (which can be a personal computer, a server, a single-chip microcomputer, a MUU or a network device, etc.) to execute the methods described in various embodiments of the present invention or certain parts of the embodiments.

[0162] The present invention provides a multi-degree-of-freedom robot satellite control method based on Lie groups. There are many methods and ways to implement the technical solution. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the protection scope of the present invention. All components not specified in this embodiment can be implemented by existing technologies.

Claims

1. A multi-degree-of-freedom robot satellite control method based on Lie groups, characterized in that: The following steps are involved: Step 1, kinematic modeling of the robotic satellite; Step 2, dynamic modeling of the robotic satellite; Step 3, establishing a robot satellite motion control method; In step 1, the robotic satellite includes a satellite base and a single 6-DOF robotic arm mounted on the satellite base; Step 1 includes: Step 1-1, define the inertial coordinate system and the tool coordinate system; Step 1-2, calculate the screw axis of each joint of the robot arm in the initial configuration; Steps 1-3, calculate the kinematic equations of a single 6-DOF manipulator; Step 1-1 includes: setting the inertial coordinate system {∑0} at the center of the connection between the robot arm and the satellite base; the tool coordinate system {∑ E }Located at the center of the end effector of the robot arm; Solve the homogeneous transformation matrix T(0) of the tool coordinate system relative to the inertial coordinate system when the robot arm is in zero position: Among them, SE(3) is a three-dimensional special Euclidean group used to represent the translation and rotation of a rigid body in three-dimensional space; the three-dimensional rotation group SO(3) is a special case of the special orthogonal group SO(n) when n=3, R∈SO(3) is a parameter used to describe the rotation of the tool coordinate system in three-dimensional space relative to the inertial coordinate system, and the Lie group refers to a group with continuous and smooth properties; SE(3) and SO(3) are continuous in the real number space, so they are both Lie groups; p belongs to the three-dimensional vector space The element is used to describe the position vector of the origin of the tool coordinate system in the inertial coordinate system; Step 1-2 includes: determining the unit rotation axis of each joint at zero position and its position vector relative to the inertial coordinate system, thereby obtaining the unit screw axis of each joint relative to the inertial coordinate system: in, represents the unit rotation axis of the i-th joint when the robot satellite is at zero position; r i express Position vector relative to the inertial coordinate system; v i is the line distance of the i-th joint, indicating Moment vector about the origin of the inertial coordinate system; S i Represents the unit screw axis of the i-th joint relative to the inertial coordinate system, which belongs to the 6-dimensional vector space Elements in Steps 1-3 include: defining the exponential coordinates of the transformation matrix T(θ)∈SE(3) as Then there is the following mapping relationship: The exponential mapping exp: [S]θ∈se(3)→T∈SE(3) from the Lie algebra se(3) to SE(3) corresponding to the special Euclidean group SE(3); The logarithmic mapping from SE(3) to se(3) log:T∈SE(3)→[S]θ∈se(3) Among them, Lie algebra is a structure corresponding to Lie group. Each Lie group corresponds to a Lie algebra. se(3) is the Lie algebra corresponding to Lie group SE(3); exp represents the exponential function with the natural constant e as the base; log represents the logarithmic function; → is a mapping, which indicates the correspondence between the elements of two sets. Here, it represents the conversion relationship between the elements in the Lie group and the elements in the Lie algebra. Any rigid body motion can be realized by moving around a fixed screw axis S in space, then: Where I represents the identity matrix; It means that with e as the base, [S i ]θ i is an exponential function of power; [S]θ is [S i ]θ i The general expression form of , which represents the matrix form of Sθ of the 6-dimensional vector; S is the screw axis i The matrix representation of is the element in the Lie algebra se(3) corresponding to the Lie group SE(3); Represents a three-dimensional vector The antisymmetric matrix of is an element in the Lie algebra so(3) corresponding to the Lie group SO(3); ω x ,ω y ,ω z Represents ω i The three elements in ; the bold 0 indicates a zero vector; the unscripted I indicates the identity matrix; Finally, the kinematic equation of a single 6-DOF manipulator is obtained as follows: Among them, T(0) is the homogeneous transformation matrix of the tool coordinate system relative to the inertial coordinate system; T(θ) is the homogeneous transformation matrix of the tool coordinate system relative to the inertial coordinate system after the six joint changes; Step 2 includes: Step 2-1, calculate the connecting rod speed; Step 2-2, calculate the system kinetic energy; Step 2-3, calculate the system potential energy; Step 2-4, calculate and obtain the dynamic model of the robot satellite; Step 2-1 includes: establishing a body coordinate system {∑ C }, the body coordinate system of the i-th link Transformation matrix relative to the inertial coordinate system {∑0} for: in, It represents the transformation matrix of the coordinate system corresponding to the left subscript i relative to the coordinate system corresponding to the left superscript j. Represents the body coordinate system of the i-th link Transformation matrix relative to the inertial coordinate system {∑0}; Represents the body coordinate system of the second link The body coordinate system relative to the first link The transformation matrix of The body coordinate system of the i-th link The object speed V i for: in is the derivative of the joint variable with respect to time, representing the joint velocity; The Jacobian matrix J of the i-th link i (θ) is: in, express The inverse of a matrix, for The adjoint operator of ; It is represented as the adjoint operator of the element T in the Lie group SE(3), which is an endomorphism of the Lie group; Step 2-2 includes: calculating the system kinetic energy using the following formula Among them, the system kinetic energy is about θ and Function of In the coordinate system The generalized inertia matrix of connecting rod i is represented by represents the moment of inertia matrix of the i-th rod, m i represents the mass of connecting rod i, and I represents the identity matrix; Step 2-3 includes: setting the system potential energy U=0; Steps 2-4 include: Lagrangian function of the robotic satellite system for: By Lagrange equation The dynamic model of the robot system is obtained as follows: Among them, τ i is the torque of the ith joint, express Taking the derivative with respect to time t, represents L versus θ i Find the partial derivative, is the mass matrix of the robotic satellite; Coriolis matrices for robotic satellites; is the generalized torque vector on each joint; Step 3 includes: Step 3-1, design the sliding surface: Among them, s(t) is the synovial surface function; is the derivative of the sliding surface function with respect to time t; α = diag[α1, α2, …, α6] is the sliding surface constant, diag is a diagonal matrix, a diagonal matrix is ​​a matrix in which all elements outside the main diagonal are 0, written as diag[α1, α2, …, α6], where α1, α2, …, α6 are the elements on its diagonal; θ d e(t) = θ(t) and θ(t) represent the change function of the desired joint angle and the change function of the real-time joint angle respectively; e(t) = θ d (t)-θ(t) is the tracking error of the joint angle; is the tracking speed, is the time derivative of the desired joint angle, is the derivative of the real-time joint angle with respect to time; Step 3-2, design the reaching law: Among them, ε is the gain coefficient; sgn(s) represents the sign function; ks is the exponential approach term; Introducing the saturation function sat(s) instead of the sign function sgn(s), the final reaching law is: The expression of the saturation function is: Where ψ is called the boundary layer thickness; Step 3-3, construct the control law: Robotic Satellite Dynamics Model Substitute into There are: Combined with the reaching law, we get: Solving the control law τ(t) yields: Among them, τ eq represents equivalent control; τ sw Indicates switching control; Step 3-4, analyze stability: Construct the Lyapunov function V: Where T represents the matrix transpose; Then there is prove The control law τ can meet the arrival condition, where is the derivative of the function V with respect to time t.

Images