Robot Satellite Attitude Control Method Based on Manipulator Dynamics

Abstract

The present invention discloses a robot satellite attitude control method based on manipulator dynamics. The steps include initializing the initial state of the base of the robot satellite; constructing recursive expressions of inertial forces and inertial torques based on outward recursion and inward recursion according to the angular velocity and velocity of the base at the current moment; using an optimization algorithm to optimize and solve the objective function constructed from the recursive expressions of inertial forces and inertial torques based on outward recursion and inward recursion to obtain the angular acceleration of each link; calculating the driving torque of each joint corresponding to each link at the current moment according to the angular acceleration of each link; determining whether the current moment is the end time of the total pose transformation duration. If so, proceed to the next step; otherwise, return to the step immediately following the initial state; output the driving torque of each joint corresponding to each link at each moment within the total pose transformation duration and input it to the robot satellite joints to achieve the motion curve tracking of the satellite base.

Description

Technical Field

[0001] The present invention relates to satellite attitude tracking technology, and particularly to a method for controlling the attitude of a robotic satellite based on the dynamics of a robotic arm. Background Art

[0002] With the development of space exploration and satellite technology, space robots have gradually become key equipment for tasks such as on-orbit service, repair, capture, and cleaning of orbital debris. Especially in low Earth orbit and deep space missions, the use of robotic arms to perform complex operations has been widely studied and applied. A space robot generally consists of a robotic arm and a base. The movement of the robotic arm will generate a reaction force on the base, resulting in changes in the attitude and position of the base. This coupling phenomenon between the base and the robotic arm poses a great challenge to the control of space robots.

[0003] In traditional satellite attitude control, thrusters are usually used to achieve attitude adjustment. However, the following problems will occur: 1. High fuel consumption: The use of thrusters depends on fuel consumption. Once the fuel is exhausted, the satellite will lose its attitude control ability, leading to mission failure. Especially in long-term missions or deep space exploration, it is extremely difficult to replenish fuel. 2. Complex system and large weight: The propulsion system requires complex equipment such as fuel storage devices, control valves, and nozzles, increasing the overall weight and manufacturing cost of the satellite, which is not conducive to reducing launch and operation costs.

[0004] The prior art "Coordinated Control of Free-Floating Space Robots Capturing Targets" mainly studied how a free-floating space robot captures a target through the movement of the robotic arm without external forces. The paper proposed to use the coupling relationship between the robotic arm and the base, and use kinematic analysis to control the position and attitude of the end of the robotic arm, while performing certain regulation on the attitude of the base. This scheme adopts a decomposed motion control method and realizes the coordinated motion control of the robotic arm and the base based on the Jacobian matrix. The disadvantages of using this coordinated control technology are as follows:

[0005] Only point-to-point control is achieved: The prior art can only achieve attitude adjustment from one point to another, and does not continuously track and control the motion state of the base. This means that during the task execution, the attitude change of the base does not have precise control ability and cannot meet the task requirements that require continuous adjustment and optimization of the base attitude. Therefore, it is impossible to directionally track the motion state of the base during the task execution. This defect makes this method not suitable for task scenarios that require high-precision attitude adjustment, such as deep space exploration missions or long-term on-orbit missions that require continuous and stable adjustment of the base attitude.

[0006] Lack of dynamic analysis: This technology is mainly based on kinematic analysis, ignoring the dynamic coupling problems in the system and not delving deeply into the impacts of the manipulator's acceleration, inertial forces, and torques on the base attitude. Therefore, in some cases, the base attitude may not be precisely controlled, especially when dealing with complex dynamic disturbances, and the accuracy and reliability of existing methods are insufficient.

[0007] The paper "Attitude Coordination Control of Combined Spacecraft Based on Evaluation of Manipulator Coupling Torque" mainly focuses on the attitude stabilization problem of the combined spacecraft after the service spacecraft and the target spacecraft are docked during on-orbit service missions. In the paper, the authors proposed an attitude coordination control method based on manipulator coupling torque, adjusting the attitude of the combined spacecraft through the dynamic coupling between the manipulator and the spacecraft. This method combines a space manipulator and reaction wheels, using the reaction wheels as an auxiliary, and verified through simulation that the proposed method can achieve attitude stabilization control of the combined spacecraft without consuming fuel; during its implementation, there is a lack of directional tracking of the base motion state: Existing technologies mainly focus on the attitude stabilization problem of the combined spacecraft and do not delve deeply into the directional motion state of the base (such as tracking and adjustment of base displacement). This makes this technology unable to handle tasks that require precise control of the base's curvilinear motion. Summary of the Invention

[0008] Aiming at the above deficiencies in the existing technology, the robot satellite attitude control method based on manipulator dynamics provided by the present invention solves the problem that the existing satellite attitude control relies on fuel-consuming thrusters, which limits the service life of the satellite.

[0009] To achieve the above invention objective, the technical solution adopted by the present invention is as follows:

[0010] Provide a robot satellite attitude control method based on manipulator dynamics, which includes the steps:

[0011] S1. Receive the base displacement curve, pose transformation curve, and total pose transformation duration of the robot satellite, perform digital differentiation on the two curves to obtain the velocity and acceleration curves and the angular velocity and angular acceleration curves;

[0012] S2. According to the angular velocity and velocity of the base at the current moment, perform outward recursion of physical quantities using the Newton-Euler method to obtain the recursive expressions of the inertial forces and inertial torques of each link;

[0013] S3. According to the recursive expressions of the inertial forces and inertial torques of each link, perform inward recursion of physical quantities using the Newton-Euler method to obtain the recursive expressions of the inertial forces and inertial torques of the end link;

[0014] S4. Use an optimization algorithm to optimize and solve the objective function constructed from the recurrence expressions of the inertial forces and inertial torques based on outward recurrence and inward recurrence, and obtain the angular acceleration of each link.

[0015] S5. Calculate the driving torque of each joint corresponding to each link at the current moment according to the angular acceleration of each link and the recurrence expressions of the inertial forces and inertial torques based on outward recurrence and inward recurrence.

[0016] S6. Determine whether the current moment is the end time of the total pose transformation duration. If so, go to step S7; otherwise, return to step S2.

[0017] S7. Output the driving torque of each joint corresponding to each link at each moment within the total pose transformation duration, and input it to the robot satellite joint to achieve the motion curve tracking of the satellite base.

[0018] Further, the robotic arm of the robot satellite is a six-degree-of-freedom robotic arm; the expression of the objective function is:

[0019] min(|F6 - F′6| 2 +|N6 - N′6| 2 )

[0020] where F6 and N6 are respectively the inertial force and inertial torque of the 6th link in outward recurrence; F′6 and N′6 are respectively the inertial force and inertial torque of the 6th link in inward recurrence; |·| is to take the absolute value.

[0021] The constraint conditions of the objective function are:

[0022]

[0023] where θ k , and are respectively the angle, angular velocity and angular acceleration of the kth joint, 1 ≤ k ≤ 6; θ min and θ max are respectively the minimum and maximum values of the joint angle; and are respectively the minimum and maximum values of the angular velocity; is the angular acceleration of the (k - 1)th joint; ∈ is a set threshold; means it is applicable to any joint k.

[0024] Further, step S2 further includes:

[0025] S21. According to the angular velocity recurrence model, calculate the angular velocity i ω k, the recurrence formula of the angular velocity vector is obtained:

[0026]

[0027] Among them, i A k is the rotation matrix from the k-th link coordinate system to the inertial coordinate system ∑i; z k is the unit vector of the joint rotation axis in the k-th link coordinate system; is the angular velocity of the k-th joint, where i represents being in the inertial coordinate system ∑i; i ω k-1 is the angular velocity of the (k - 1)-th link; i ω0 is the angular velocity of the base at the current moment;

[0028] S22. According to the linear velocity recurrence model, using the angular velocity and the centroid position, calculate the linear velocity of the k-th link i v k , the recurrence formula of the linear velocity vector is obtained:

[0029] i v k = i v k-1 + i ω k-1 ×b k-1 + i ω k ×a k + i ω k-1 ×( i A k z k )

[0030] Among them, b k is the vector from the k-th link of the robotic arm to the (k + 1)-th joint in the k-th link coordinate system; b k-1 is the vector from the (k - 1)-th link of the robotic arm to the k-th joint in the (k - 1)-th link coordinate system; i v k-1 is the linear velocity of the (k - 1)-th link; a k is the centroid position of the k-th link in the k-th link coordinate system; i v0 is the velocity of the base at the previous moment;

[0031] S23. According to the angular acceleration recurrence model, using the joint angular acceleration and the rotation matrix, calculate the angular acceleration of the k-th link The recurrence formula of the angular acceleration is obtained:

[0032]

[0033] Among them, is the angular acceleration of the k-th joint; is the linear velocity of the k-th joint;

[0034] S24. According to the linear acceleration recurrence model, use the angular acceleration, angular velocity, and centroid position to calculate the linear acceleration of the k-th link Obtain the recurrence formula for linear acceleration:

[0035]

[0036] where is the linear acceleration of the (k - 1)-th link;

[0037] S25. Use the linear velocity of the link and the mass m k , to calculate the inertial force F k of the k-th link, and obtain the recurrence expression for the inertial force:

[0038]

[0039] where m k is the mass of the k-th link;

[0040] S26. According to the inertial moment recurrence model, use the angular acceleration, angular velocity, and inertia tensor of the link to calculate the inertial moment N k of the k-th link, and obtain the recurrence expression for the inertial moment:

[0041]

[0042] where i I k is the inertia tensor of the k-th link.

[0043] Furthermore, step S3 further includes:

[0044] S31. Calculate the disturbing force F0 and moment N0 that the base expects to receive from the robotic arm:

[0045] F0 = f1, N0 = c 01 × f1 + n1

[0046] where c 01 is the vector from the centroid of the base to the first joint; f1 is the external force of the first link;

[0047] S32. According to the external force f k-1 of the (k - 1)-th link and the inertial force F k-1 , calculate the external force f k of the k-th link:

[0048] f k = f k-1-F k-1

[0049] wherein, F k-1 is the inertial force of the (k - 1)-th link;

[0050] S33. Calculate the external moment n k-1 of the k-th link according to the external moment F k-1 and the inertial moment N k of the (k - 1)-th link:

[0051] n k = n k-1 - N k-1 - (l k-1 × F k-1 ) - (a k × f k )

[0052] wherein, l k-1 is the vector connecting joint k - 1 to joint k in the inertial coordinate system; n k-1 is the external moment of the (k - 1)-th link;

[0053] S34. Calculate the recurrence expressions of the inertial force F'6 and the inertial moment N'6 of the 6th link in the inward recurrence according to the external force f6 and the external moment n6 of the 6th link:

[0054] F'6 = f6, N'6 = n6 - a6 × F6

[0055] wherein, a6 is the position of the center of mass of the 6th link in the coordinate system of the 6th link.

[0056] The beneficial effects of the above technical solution are as follows: By establishing a recursive dynamic model through steps S21 - S26 and steps S31 - S34, and combining the state changes at the upper and lower moments, this solution can effectively correlate the moments and joint angles at the upper and lower moments at each moment, ensuring the smooth transition and stability of the base state of the robot satellite; this time-correlated recursive algorithm can greatly improve the control accuracy.

[0057] Based on the time correlation of the recurrence equation, this solution can calculate the expected moment and joint angle at each moment through the recursive calculation of force and moment; in this way, this solution ensures the precise control of joint angles and moments under complex dynamic conditions.

[0058] Further, step S5 further includes:

[0059] S51. Calculate the external moment n k of the k-th link according to the angular acceleration of each link, using steps S21 - S26 and steps S31 - S33;

[0060] S52. Calculate the driving torque of each joint corresponding to the link at the current moment according to the external torque of the k-th link:

[0061]

[0062] where τ k is the driving torque of the joint corresponding to the k-th link; T is the transpose; z k is the unit vector in the direction of the axis of the k-th joint of the robotic arm in the inertial coordinate system; the k-th joint is the component connecting the k-th link and the (k + 1)-th link.

[0063] Furthermore, the optimization algorithm is a genetic algorithm or a particle swarm optimization algorithm.

[0064] Furthermore, the mass of the base of the robotic satellite and the mass of the robotic arm are of the same order of magnitude.

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

[0066] 1. Reduce fuel consumption and extend the mission life of the robotic satellite: The traditional thruster system relies on fuel consumption for attitude adjustment. This solution realizes the attitude adjustment of the base through the motion perturbation of the robotic arm, significantly reducing the dependence on fuel. In typical missions, the fuel consumption of general space robots generally accounts for about 50% to 60% of the overall weight. However, this solution can use solar power to adjust the satellite attitude, which will greatly extend the duration of small satellite missions. Especially in long-term orbit maintenance or deep space exploration missions, the low energy consumption characteristics of this solution will greatly improve the mission endurance ability, reduce the system complexity and total weight. Without a large number of fuel storage devices and thruster systems, the launch cost and maintenance requirements of the satellite are significantly reduced, which is suitable for the mission scenarios of small robotic satellites, especially for low-cost missions with limited resources.

[0067] 2. Achieve directional tracking of the base: The traditional method usually can only achieve point-to-point attitude adjustment. This solution can track the motion state of the base at each moment through recursive solution based on Newton-Euler dynamics equations, ensuring that the attitude and displacement are always close to the expected target. By introducing the motion constraint conditions of the base, this method can achieve continuous and smooth adjustment of the attitude and displacement, ensuring that the base maintains the best direction throughout the mission execution process.

[0068] 3. Curve tracking control of the base: This solution can control the motion of the base by controlling the motion of the robotic arm, and thus achieve the curve tracking of the base. This curve tracking not only includes precise control of the position, but also covers continuous adjustment of the base attitude, enabling the entire system to move precisely along the set path, ensuring that the robotic satellite can move smoothly and precisely along the set path. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 Flowchart of the robotic satellite attitude control method based on manipulator dynamics.

[0070] Figure 2 Schematic diagram of the structure of the robotic satellite. DETAILED DESCRIPTION

[0071] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0072] To facilitate the understanding of this solution, the following components and terms related to attitude control of the robotic satellite are introduced:

[0073] 1. Base: Robot system (such as Figure 2 As shown, the square structure is the fixed or movable part in the base, which is usually used to support the robotic arm or other actuators. In the present invention, the base refers to the main part of the robot satellite, which is coupled with the robotic arm and has the same mass as the robotic arm.

[0074] 2. Robotic arm: a multi-degree-of-freedom motion structure composed of multiple links and joints, see Figure 2 The structure is fixed on the base and extends along the upper right side, which is used to perform specific operation tasks. The movement ability and degree of freedom of the robot arm depend on the number and type of its joints, as well as the structure of the connecting rod. The connecting rod is a component of the robot arm, which can be understood as a "skeleton" or "arm segment", usually used to connect two joints. The joint is a component that connects two connecting rods and is responsible for providing the movement ability of the robot arm. In the present invention, the robot arm controls the posture and position of the base by adjusting the joint angle and torque.

[0075] 3. Robotic satellite: refers to a space robot system in which the mass of the base is similar to that of the robotic arm. The robotic arm controls the posture and position of the base by adjusting its own movement to achieve specific mission objectives.

[0076] 4. Degrees of freedom: Indicates the number of independent movements a robot can make in space. Six degrees of freedom represent translation in three directions and rotation around three axes, and are often used to describe the movement capabilities of a robot or base.

[0077] 5. Torque: The force that causes an object to rotate about an axis, and its magnitude is related to the distance from the point of application to the axis of rotation. Torque is used in this solution to control the rotation of the robotic arm joints and the attitude adjustment of the base.

[0078] 6. Joint angle: The relative rotational position of each joint of the robotic arm, usually expressed in degrees or radians. The joint angle is an important parameter for the motion control of the robotic arm.

[0079] 7. Recursive dynamics: A dynamic method that calculates forces and torques in stages. Recursive dynamics is divided into outward recursion and inward recursion. Outward recursion: Starting from the base, gradually calculate the forces and torques of each joint until the end effector. Inward recursion: Starting from the end effector, push the forces and torques back to the base to determine the reaction forces and reaction torques of each joint.

[0080] 8. Curve tracking: It means that the base moves along a set trajectory or curve, including precise control of position and attitude. In this solution, the curve tracking of the base is achieved through the adjustment of the robotic arm.

[0081] 9. Desired torque: It is the driving torque τ of the joints in this solution k , which is the torque that needs to be applied to each joint to achieve a specific motion target. The desired torque is calculated through recursive equations and dynamic models, and is used to ensure that the motion of the base or the robotic arm conforms to the predetermined trajectory.

[0082] 10. Attitude control: By adjusting the joint torques and angles, the robotic arm and the base maintain or adjust a specific attitude in space. Attitude control is used in this invention to ensure the stable motion of the base along the curve tracking path.

[0083] Reference Figure 1 , Figure 1 shows a flowchart of a method for controlling the attitude of a robotic satellite based on the dynamics of a robotic arm; as Figure 1 shown, the method S includes steps S1 to S7.

[0084] In step S1, receive the base displacement curve, pose transformation curve, and total pose transformation duration of the robotic satellite, perform digital differentiation on the two curves to obtain the velocity and acceleration curves and the angular velocity and angular acceleration curves; in this solution, it is preferred that the robotic arm of the robotic satellite is a six-degree-of-freedom robotic arm, and the mass of the base of the robotic satellite and the mass of the robotic arm are of the same order of magnitude.

[0085] In step S2, according to the angular velocity and velocity of the base at the current moment, physical quantities are recursively extrapolated using the Newton-Euler method to obtain the recursive expressions for the inertial force and inertial moment of each link; specifically: In this solution, the velocities and accelerations of each link are recursively calculated from link 0 (the spacecraft base) to link n (the end); then, the inertial forces and inertial moments of each link are calculated using the Newton-Euler formula (the relationship between torque and angular acceleration).

[0086] In an embodiment of the present invention, step S2 further includes:

[0087] Input the motion state of the base at this moment: angular velocity i ω0 and velocity i v0, and these two parameters can be obtained from the base velocity and acceleration curves and the angular velocity and angular acceleration curves; based on the input angular velocity i ω0 and velocity i v0, after obtaining the angular acceleration variable, the fourth-order Runge-Kutta integration formula is used for integration to respectively obtain the and Specific iterative calculations are as follows:

[0088] S21. According to the angular velocity recursion model, using the angular velocity of the previous link and the rotation matrix, calculate the angular velocity i ω k of the kth link to obtain the recursion formula for the angular velocity vector:

[0089]

[0090] where, i A k is the rotation matrix from the kth link coordinate system to the inertial coordinate system ∑i; z k is the unit vector of the joint rotation axis in the kth link coordinate system; is the angular velocity of the kth joint, and i represents being in the inertial coordinate system ∑i; i ω k-1 is the angular velocity of the (k - 1)th link; i ω0 is the angular velocity of the base at the current moment;

[0091] S22. According to the linear velocity recursion model, using the angular velocity and the centroid position, calculate the linear velocity i v k of the kth link to obtain the recursion formula for the linear velocity vector:

[0092] i v k = i v k-1 + i ω k-1 ×bk-1 + i ω k ×a k + i ω k-1 ×( i A k z k )

[0093] where b k is the vector from the k-th link of the robotic arm to the k+1-th joint in the k-th link coordinate system; b k-1 is the vector from the (k-1)-th link of the robotic arm to the k-th joint in the (k-1)-th link coordinate system; i v k-1 is the linear velocity of the (k-1)-th link; a k is the centroid position of the k-th link in the k-th link coordinate system; i v0 is the velocity of the base at the previous moment;

[0094] S23. According to the angular acceleration recurrence model, use the joint angular acceleration and rotation matrix to calculate the angular acceleration of the k-th link Obtain the recurrence formula for angular acceleration:

[0095]

[0096] where is the angular acceleration of the k-th joint; is the linear velocity of the k-th joint;

[0097] S24. According to the linear acceleration recurrence model, use the angular acceleration, angular velocity and centroid position to calculate the linear acceleration of the k-th link Obtain the recurrence formula for linear acceleration:

[0098]

[0099] where is the linear acceleration of the (k-1)-th link;

[0100] S25. Use the linear velocity of the link and the mass m k to calculate the inertial force F k of the k-th link, and obtain the recurrence expression for the inertial force:

[0101]

[0102] where m k is the mass of the k-th link;

[0103] S26. Calculate the inertial moment N of the k-th link using the angular acceleration, angular velocity, and inertia tensor of the link according to the inertial moment recurrence model. k , and obtain the recurrence expression for the inertial moment:

[0104]

[0105] where i I k is the inertia tensor of the k-th link.

[0106] In step S3, according to the recurrence expressions for the inertial force and inertial moment of each link, perform an inward recurrence of the physical quantities of the Newton-Euler method to obtain the recurrence expressions for the inertial force and inertial moment of the end link;

[0107] In an embodiment of the present invention, step S3 further includes:

[0108] S31. Calculate the disturbing force F0 and moment N0 that the base expects to receive from the manipulator:

[0109] F0 = f1, N0 = c 01 × f1 + n1

[0110] where c 01 is the vector from the centroid of the base to the first joint; f1 is the external force of the first link;

[0111] S32. Calculate the external force f k-1 and inertial force F k-1 of the (k - 1)-th link, and calculate the external force f k of the k-th link:

[0112] f k = f k-1 - F k-1

[0113] where F k-1 is the inertial force of the (k - 1)-th link;

[0114] S33. Calculate the external moment F k-1 and inertial moment N k-1 of the (k - 1)-th link, and calculate the external moment n k of the k-th link:

[0115] n k = n k-1 - N k-1 -(l k-1 × F k-1 )-(a k × f k )

[0116] where lk-1 is the vector connecting joint k-1 to joint k in the inertial coordinate system; n k-1 is the external moment of the (k-1)-th link;

[0117] S34. According to the external force f6 and external moment n6 of the 6th link, calculate the recurrence expressions of the inertial force F′6 and inertial moment N′6 of the 6th link in the inward recurrence:

[0118] F′6 = f6, N′6 = n6 - a6 × F6

[0119] where a6 is the position of the centroid of the 6th link in the coordinate system of the 6th link.

[0120] In step S4, an optimization algorithm is used to optimize and solve the objective function constructed based on the recurrence expressions of the inertial force and inertial moment in the outward recurrence and inward recurrence, and the angular acceleration of each link is obtained;

[0121] In implementation, the expression of the objective function in this solution is preferably:

[0122] min(|F6 - F′6| 2 +|N6 - N′6| 2 )

[0123] where F6 and N6 are the inertial force and inertial moment of the 6th link in the outward recurrence respectively; F′6 and N′6 are the inertial force and inertial moment of the 6th link in the inward recurrence respectively; |·| is to take the absolute value;

[0124] The constraint conditions of the objective function are:

[0125]

[0126] where θ k , and are the angle, angular velocity and angular acceleration of the k-th joint respectively, 1 ≤ k ≤ 6; θ min and θ max are the minimum and maximum values of the joint angle respectively; and are the minimum and maximum values of the angular velocity respectively; is the angular acceleration of the (k - 1)-th joint; ∩ is the set threshold; means it is applicable to any joint k.

[0127] In this solution, among the above three constraint conditions, the first constraint condition can ensure that the joint does not exceed the physical angle range. The second constraint condition can avoid too fast or too slow joint movement. The third constraint condition means that the angular acceleration of each joint cannot have too large a mutation and must be within a small range of ∈ from the acceleration value of the previous solution to ensure the smooth movement of the joint.

[0128] In step S5, according to the angular acceleration of each link and the recurrence expressions of the inertial force and inertial moment for outward and inward recurrence, the driving torque of the joint corresponding to each link at the current moment is calculated.

[0129] When implemented, this solution preferably further includes in step S5:

[0130] S51. According to the angular acceleration of each link, using steps S21 - S26 and steps S31 - S33, calculate the external torque n of the k-th link k ;

[0131] S52. According to the external torque of the k-th link, calculate the driving torque of the joint corresponding to each link at the current moment:

[0132]

[0133] where τ k is the driving torque of the joint corresponding to the k-th link; T is the transpose; z k is the unit vector in the direction of the axis of the k-th joint of the robotic arm in the inertial coordinate system; the k-th joint is the component connecting the k-th link and the k + 1-th link.

[0134] In step S6, it is judged whether the current moment is the end time of the total pose transformation duration. If so, enter step S7; otherwise, return to step S2.

[0135] In step S7, output the driving torque of the joint corresponding to each link at each moment within the total pose transformation duration and input it to the robotic satellite joint to achieve the motion curve tracking of the satellite base.

[0136] In this solution, the optimization algorithm for solving the objective function is the genetic algorithm or the particle swarm optimization algorithm.

[0137] To sum up, this solution realizes the precise adjustment of the attitude and displacement of the small-base robotic satellite through the dynamic control of the robotic arm, overcoming problems such as high fuel consumption and complex system in the prior art; through the motion control of the robotic arm, the attitude adjustment and curve tracking of the base can be realized, thereby ensuring that the robotic satellite can move smoothly and precisely along the set path.

Claims

1. A robot satellite attitude control method based on mechanical arm dynamics, characterized in that: Includes steps: S1, receiving the base displacement curve, posture change curve and total posture change time of the robot satellite, digitally differentiating the two curves to obtain the velocity and acceleration curve and the angular velocity and angular acceleration curve; S2. According to the angular velocity and speed of the base at the current moment, the physical quantity of the Newton-Euler method is recursively extrapolated to obtain the recursive expression of the inertia force and inertia moment of each connecting rod; S3. According to the recursive expressions of the inertia force and inertia moment of each connecting rod, the physical quantity of the Newton-Euler method is recursively deduced inward to obtain the recursive expressions of the inertia force and inertia moment of the end connecting rod; S4. Using an optimization algorithm, an objective function constructed based on the recursive expressions of the inertial force and the inertial moment of force by outward recursion and inward recursion is optimized and solved to obtain the angular acceleration of each connecting rod; S5. Calculate the driving torque of the joint corresponding to each connecting rod at the current moment according to the angular acceleration of each connecting rod and the recursive expressions of the inertial force and inertial moment recursively deduced outward and inward; S6, determine whether the current time is the end time of the total duration of posture transformation, if so, proceed to step S7, otherwise return to step S2; S7, output the driving torque of each link corresponding to the joint at each moment within the total duration of the posture transformation, and input it to the robot satellite joint to achieve motion curve tracking of the satellite base; Step S3 further comprises: S31. Calculate the interference force F0 and torque N0 that the base expects to receive from the manipulator: Among them, c 01 is the vector from the center of mass of the base to the first joint; f1 is the external force of the first link; S32, according to the external force f of the k-1th connecting rod k-1 and inertial force F k-1 , calculate the external force f of the kth connecting rod k : Among them, F k-1 is the inertia force of the k-1th connecting rod; S33, according to the external moment F of the k-1th connecting rod k-1 and moment of inertia N k-1 , calculate the external moment n of the kth connecting rod k : Among them, l k-1 is the vector connecting joint k-1 to joint k in the inertial coordinate system; n k-1 is the external torque of the k-1th connecting rod; S34. Calculate the inertial force of the sixth connecting rod inward recursion according to the external force f6 and external moment n6 of the sixth connecting rod. and moment of inertia The recursive expression of is: Wherein, a6 is the center of mass position of the sixth link in the sixth link coordinate system; Step S5 further comprises: S51, according to the angular acceleration of each connecting rod, using steps S21-S26 and steps S31-S33, calculate the external moment n of the kth connecting rod k ; S52. According to the external torque of the kth connecting rod, the driving torque of the joint corresponding to each connecting rod at the current moment is calculated: Among them, τ k is the driving torque of the joint corresponding to the kth link; T is the transpose; z k is the unit vector in the direction of the axis of the kth joint of the robotic arm in the inertial coordinate system; the kth joint is the component connecting the kth link and the k+1th link.

2. The robot satellite attitude control method based on mechanical arm dynamics according to claim 1 is characterized in that: The robotic satellite's robotic arm is a six-degree-of-freedom robotic arm; The expression of the objective function is: Among them, F6 and N6 are the inertia force and inertia moment of the sixth connecting rod respectively; and They are the inertia force and inertia moment of the sixth connecting rod pushed inwards respectively; To take the absolute value; The constraints of the objective function are: in, , and are the angle, angular velocity and angular acceleration of the kth joint, 1≤k≤6; θ min and θ max are the minimum and maximum values ​​of the joint angles, respectively; and are the minimum and maximum values ​​of the angular velocity respectively; is the angular acceleration of the k-1th joint; To set the threshold; It means that it is applicable to any joint k.

3. The robot satellite attitude control method based on mechanical arm dynamics according to claim 1 is characterized in that: Step S2 further comprises: S21. According to the angular velocity recursion model, the angular velocity of the kth connecting rod is calculated using the angular velocity of the previous connecting rod and the rotation matrix. , we get the recursive formula of the angular velocity vector: in, is the rotation matrix from the kth rod coordinate system to the inertial coordinate system ∑i; k is the unit vector of the joint rotation axis in the kth link coordinate system; is the angular velocity of the kth joint, i means it is located in the inertial coordinate system ∑i; is the angular velocity of the k-1th connecting rod; is the angular velocity of the base at the current moment; S22. According to the linear velocity recursion model, the linear velocity of the kth connecting rod is calculated using the angular velocity and the center of mass position. , we get the recursive formula of the linear velocity vector: Among them, b k is the vector from the kth link to the k+1th joint of the robot in the kth link coordinate system; b k-1 is the vector from the k-1th link of the robot to the kth joint in the k-1th link coordinate system; is the linear velocity of the k-1th connecting rod; a k is the center of mass position of the kth link in the kth link coordinate system; is the speed of the base at the previous moment; S23. According to the angular acceleration recursion model, using the joint angular acceleration and the rotation matrix, calculate the angular acceleration of the kth link , we get the recursive formula for angular acceleration: in, is the angular acceleration of the kth joint; is the linear velocity of the kth joint; S24. According to the linear acceleration recursion model, using the angular acceleration, angular velocity and center of mass position, calculate the linear acceleration of the kth connecting rod , we get the recursive formula of linear acceleration: in, is the linear acceleration of the k-1th connecting rod; S25, using the connecting rod linear velocity and mass m k , calculate the inertia force F of the kth connecting rod k , we get the recursive expression of inertia force: Among them, m k is the mass of the kth connecting rod; S26. According to the inertia moment recursive model, using the angular acceleration, angular velocity and inertia tensor of the connecting rod, calculate the inertia moment N of the kth connecting rod k , we get the recursive expression of the moment of inertia: in, is the inertia tensor of the kth connecting rod.

4. The robot satellite attitude control method based on mechanical arm dynamics according to any one of claims 1 to 3, characterized in that: The optimization algorithm is a genetic algorithm or a particle swarm optimization algorithm.

5. The robot satellite attitude control method based on mechanical arm dynamics according to any one of claims 1 to 3, characterized in that: The mass of the base of the robotic satellite and the mass of the robotic arm are of the same order of magnitude.

Images