Robot satellite attitude control method based on mechanical arm dynamics

Abstract

The invention discloses a robot satellite attitude control method based on mechanical arm dynamics. The method comprises the steps that the initial state of a base of a robot satellite is initialized; according to the angular velocity and the velocity of the base at the current moment, constructing a recursion expression of inertia force and inertia moment based on outward recursion and inward recursion; an optimization algorithm is adopted to carry out optimization solving on a target function constructed based on the recursion expressions of the inertia force and the inertia moment of outward recursion and inward recursion, and the angular acceleration of each connecting rod is obtained; according to the angular acceleration of each connecting rod, the driving torque of the corresponding joint of each connecting rod at the current moment is calculated; judging whether the current moment is the end time of the total duration of pose transformation, if so, entering the next step, and otherwise, returning to the next step of the initial state; and outputting the driving torque of the joint corresponding to each connecting rod at each moment within the total pose transformation duration, and inputting the driving torque to a robot satellite joint so as to realize motion curve tracking of a satellite base.

Description

Technical Field

[0001] The invention relates to a satellite attitude tracking technology, and in particular to a robot satellite attitude control method based on mechanical arm dynamics. Background Art

[0002] With the development of space exploration and satellite technology, space robots have gradually become key equipment for on-orbit servicing, maintenance, capturing and cleaning orbital debris. Especially in low-Earth orbit and deep space missions, the use of robotic manipulators to perform complex operations has been widely studied and applied. Space robots are generally composed of a manipulator and a base. The movement of the manipulator will produce a reaction force on the base, causing the base's posture and position to change. This coupling phenomenon between the base and the manipulator 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 may 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 capability, resulting in mission failure. Especially in long-term missions or deep space exploration, fuel replenishment is extremely difficult. 2. The system is complex and heavy: The propulsion system requires complex equipment such as fuel storage devices, control valves, and nozzles, which increases the overall weight and manufacturing cost of the satellite, and is not conducive to reducing launch and operating costs.

[0004] The existing technology "Coordinated Control of Free-Floating Space Robot Capturing Targets" mainly studies how a free-floating space robot can capture a target through the movement of the robotic arm without external forces. The paper proposes to use the coupling relationship between the robotic arm and the base to control the position and posture of the end of the robotic arm using kinematic analysis, while also regulating the posture of the base to a certain extent. This solution uses a decomposed motion control method to achieve coordinated motion control of the robotic arm and the base based on the Jacobian matrix. The following disadvantages exist when using this coordinated control technology:

[0005] Only point-to-point control is achieved: the existing technology can only achieve attitude adjustment from one point to another, without continuous tracking and control of the motion state of the base. This means that during the execution of the mission, the attitude change of the base does not have the ability to be accurately controlled, and cannot meet the mission 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 mission. This defect makes this method unsuitable for mission 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 problem in the system, and does not discuss in depth the impact of the acceleration, inertial force and torque of the manipulator on the base posture. Therefore, in some cases, the posture of the base may not be accurately controlled, especially when complex dynamic interference is involved, and the accuracy and reliability of the existing methods are insufficient.

[0007] The paper "Coordinated attitude control of combined spacecraft based on evaluation of manipulator coupling torque" mainly focuses on the attitude stability problem of the combined spacecraft after the service spacecraft and the target spacecraft dock in the on-orbit service mission. In the paper, the author proposed an attitude coordination control method based on the manipulator coupling torque, and adjusted the attitude of the combined spacecraft through the dynamic coupling of the manipulator and the spacecraft. This method combines the space manipulator and the reaction wheel, using the reaction wheel as an auxiliary. The simulation verifies that the proposed method can achieve attitude stability control of the combined spacecraft without consuming fuel; it lacks directional tracking of the motion state of the base during the implementation process: the existing technology mainly focuses on the attitude stability problem of the combined spacecraft, but does not make in-depth discussions on the directional motion state of the base (such as tracking and adjusting the displacement of the base). This makes the technology unable to cope with tasks that require precise control of the curved motion of the base. Summary of the invention

[0008] In view of the above-mentioned deficiencies in the prior art, 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 thrusters that consume fuel, which limits the service life of the satellite.

[0009] In order to achieve the above-mentioned object of the invention, the technical solution adopted by the present invention is:

[0010] A method for controlling a robot satellite attitude based on mechanical arm dynamics is provided, comprising the steps of:

[0011] 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;

[0012] 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;

[0013] 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;

[0014] 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;

[0015] 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;

[0016] 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;

[0017] 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.

[0018] Furthermore, the robotic arm of the robotic 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] Among them, F6 and N6 are the inertia force and inertia moment of the sixth connecting rod pushed outward respectively; F′6 and N′6 are the inertia force and inertia moment of the sixth connecting rod pushed inward respectively; |·| is the absolute value;

[0021] The constraints of the objective function are:

[0022]

[0023] Among them, θ k , 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 angular velocity respectively; is the angular acceleration of the k-1th joint; ∈ is the set threshold; It means that it is applicable to any joint k.

[0024] Furthermore, step S2 further comprises:

[0025] 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. i ω k, we get the recursive formula of the angular velocity vector:

[0026]

[0027] in, i A k 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; i ω k-1 is the angular velocity of the k-1th connecting rod; i ω0 is the angular velocity of the base at the current moment;

[0028] 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. i v k , we get the recursive formula of the linear velocity vector:

[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 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; i v k-1 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; i v0 is the velocity of the base at the previous moment;

[0031] 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 The recursive formula for angular acceleration is obtained:

[0032]

[0033] in, is the angular acceleration of the kth joint; is the linear velocity of the kth joint;

[0034] 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 The recursive formula for linear acceleration is obtained:

[0035]

[0036] in, is the linear acceleration of the k-1th connecting rod;

[0037] 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:

[0038]

[0039] Among them, m k is the mass of the kth connecting rod;

[0040] 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:

[0041]

[0042] in, i I k is the inertia tensor of the kth connecting rod.

[0043] Furthermore, step S3 further includes:

[0044] S31. Calculate the interference force F0 and torque N0 that the base expects to receive from the manipulator:

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

[0046] 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;

[0047] 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 :

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

[0049] Among them, F k-1 is the inertia force of the k-1th connecting rod;

[0050] 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 :

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

[0052] 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;

[0053] S34. According to the external force f6 and external moment n6 of the sixth connecting rod, calculate the recursive expression of the inertial force F′6 and the inertial moment N′6 of the sixth connecting rod:

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

[0055] Wherein, a6 is the center of mass position of the sixth link in the sixth link coordinate system.

[0056] The beneficial effects of the above technical scheme are as follows: this scheme establishes a recursive dynamic model through steps S21 to S26 and steps S31 to S34, and combines the state changes at the upper and lower moments to effectively associate the torque and joint angle at each moment, thereby ensuring the smooth transition and stability of the base state of the robot satellite; this time-related recursive algorithm can greatly improve the control accuracy.

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

[0058] Furthermore, step S5 further includes:

[0059] S51, according to the angular acceleration of each connecting rod, using steps S21 to S26 and steps S31 to S33, calculate the external moment n of the kth connecting rod k ;

[0060] 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:

[0061]

[0062] 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.

[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 at the same order of magnitude.

[0065] Compared with the prior art, the present invention has the following beneficial effects:

[0066] 1. Reduce fuel consumption and extend the mission life of robotic satellites: Traditional thruster systems rely on fuel consumption in attitude adjustment. This solution achieves base attitude adjustment through motion disturbance of the robotic arm, significantly reducing dependence on fuel. In typical missions, the fuel consumption of general space robots needs to account for about 50% to 60% of the total body weight, while 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 orbital maintenance or deep space exploration missions. The low energy consumption characteristics of this solution will greatly enhance the mission's sustainability and reduce system complexity and total weight. Without a large amount of fuel storage devices and thruster systems, the launch cost and maintenance requirements of satellites have been significantly reduced, which is suitable for mission scenarios of small robotic satellites, especially low-cost missions with limited resources.

[0067] 2. Achieve directional tracking of the base: Traditional methods can usually only achieve point-to-point attitude adjustment. This solution can track the motion state of the base at every 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 constraints of the base, this method can achieve continuous and smooth adjustment of attitude and displacement, ensuring that the base maintains the optimal direction during the entire task execution process.

[0068] 3. Curve tracking control of the base: This solution can control the movement of the base by controlling the movement of the robotic arm, thereby achieving curve tracking of the base. This curve tracking not only includes precise control of the position, but also covers the continuous adjustment of the base's attitude, so that the entire system can move accurately along the set path, ensuring that the robot satellite can move smoothly and accurately 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 robot arm or other actuators. In the present invention, the base refers to the main part of the robot satellite, which is coupled with the robot arm and has the same mass as the robot 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 around an axis. Its magnitude is related to the distance from the point of action to the axis of rotation. In this solution, torque is used to control the rotation of the robot arm joints and the posture adjustment of the base.

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

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

[0080] 8. Curve tracking: refers to the movement of the base along a set trajectory or curve, including precise control of position and posture. In this solution, the curve tracking of the base is achieved by adjusting the robotic arm.

[0081] 9. Expected torque: This is the driving torque τ of the joint in this solution k , which is the torque that needs to be applied to each joint to achieve a specific motion goal. The expected torque is calculated through recursive equations and dynamic models to ensure that the motion of the base or robot arm meets the predetermined trajectory.

[0082] 10. Posture control: By adjusting the joint torque and angle, the robot arm and the base can maintain or adjust a specific posture in space. Posture control is used in the present invention to ensure that the base moves stably along the curve tracking path.

[0083] refer to Figure 1 , Figure 1 A flow chart of a robot satellite attitude control method based on mechanical arm dynamics is shown; Figure 1 As shown, the method S includes steps S1 to S7.

[0084] In step S1, the base displacement curve, posture change curve and total posture change time of the robot satellite are received, and the two curves are digitally differentiated to obtain the velocity and acceleration curve and the angular velocity and angular acceleration curve; in this scheme, the robot satellite's robotic arm is preferably a six-degree-of-freedom robotic arm, and the base mass of the robot satellite and the mass of the robotic arm are at the same order of magnitude.

[0085] In step S2, the physical quantities of the Newton-Euler method are extrapolated outward according to the angular velocity and speed of the base at the current moment to obtain the recursive expression of the inertial force and inertial moment of each connecting rod; specifically: this scheme first recursively calculates the velocity and acceleration of each connecting rod from connecting rod 0 (spacecraft base) to connecting rod n (end); and then calculates the inertial force and inertial moment of each connecting rod by the Newton-Euler formula (the relationship between torque and angular acceleration).

[0086] In one embodiment of the present invention, step S2 further comprises:

[0087] Enter the base motion state at this moment: angular velocity i ω0 and speed i v0, these two parameters can be obtained through the base velocity and acceleration curve and the angular velocity and angular acceleration curve; based on the input angular velocity i ω0 and speed i After obtaining the angular acceleration variable v0, the fourth-order Runge-Kutta integral formula is used to integrate and obtain the current moment and The specific iterative calculation is as follows:

[0088] 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. i ω k , we get the recursive formula of the angular velocity vector:

[0089]

[0090] in, i A k 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; i ω k-1 is the angular velocity of the k-1th connecting rod; i ω0 is the angular velocity of the base at the current moment;

[0091] 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. i v k , we get the recursive formula of 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] 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; i v k-1 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; i v0 is the velocity of the base at the previous moment;

[0094] 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 The recursive formula for angular acceleration is obtained:

[0095]

[0096] in, is the angular acceleration of the kth joint; is the linear velocity of the kth joint;

[0097] 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 The recursive formula for linear acceleration is obtained:

[0098]

[0099] in, is the linear acceleration of the k-1th connecting rod;

[0100] 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:

[0101]

[0102] Among them, m k is the mass of the kth connecting rod;

[0103] 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:

[0104]

[0105] in, i I k is the inertia tensor of the kth connecting rod.

[0106] In step 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 terminal connecting rod;

[0107] In one embodiment of the present invention, step S3 further comprises:

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

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

[0110] 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;

[0111] 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 :

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

[0113] Among them, F k-1 is the inertia force of the k-1th connecting rod;

[0114] 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 :

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

[0116] Among them, lk-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;

[0117] S34. According to the external force f6 and external moment n6 of the sixth connecting rod, calculate the recursive expression of the inertial force F′6 and the inertial moment N′6 of the sixth connecting rod:

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

[0119] Wherein, a6 is the center of mass position of the sixth link in the sixth link coordinate system.

[0120] In step S4, an optimization algorithm is used to optimize and solve the objective function constructed based on the recursive expressions of the inertial force and the inertial moment of force by outward recursion and inward recursion to obtain the angular acceleration of each connecting rod;

[0121] During implementation, the expression of the objective function is preferably:

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

[0123] Among them, F6 and N6 are the inertia force and inertia moment of the sixth connecting rod pushed outward respectively; F′6 and N′6 are the inertia force and inertia moment of the sixth connecting rod pushed inward respectively; |·| is the absolute value;

[0124] The constraints of the objective function are:

[0125]

[0126] Among them, θ k , 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 angular velocity respectively; is the angular acceleration of the k-1th joint; ∩ is the set threshold; It means that it is applicable to any joint k.

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

[0128] In step S5, the driving torque of the joint corresponding to each connecting rod at the current moment is calculated 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;

[0129] During implementation, the preferred step S5 of this solution further includes:

[0130] S51, according to the angular acceleration of each connecting rod, using steps S21 to S26 and steps S31 to S33, calculate the external moment n of the kth connecting rod k ;

[0131] 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:

[0132]

[0133] 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.

[0134] In step S6, it is determined whether the current time is the end time of the total duration of the posture transformation. If so, the process proceeds to step S7, otherwise it returns to step S2;

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

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

[0137] In summary, this solution achieves precise adjustment of the attitude and displacement of a small base robot satellite through the dynamic control of the robotic arm, overcoming the problems of high fuel consumption and complex system in the prior art; through the motion control of the robotic arm, the base attitude adjustment and curve tracking can be achieved, thereby ensuring that the robot 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.

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: min(|F6-F′6| 2 +|N6-N′6| 2 ) Among them, F6 and N6 are the inertia force and inertia moment of the sixth connecting rod pushed outward, respectively; F′6 and N′6 are the inertia force and inertia moment of the sixth connecting rod pushed inward, respectively; |·| is the absolute value; The constraints of the objective function are: Among them, θ k , 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; ∈ is the set 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. i ω k , we get the recursive formula of the angular velocity vector: in, i A k 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; i ω k-1 is the angular velocity of the k-1th connecting rod; l ω0 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. i v k , we get the recursive formula of the linear velocity vector: 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 ) 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 i v k-1 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 i v0 is the velocity 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 The recursive formula for angular acceleration is obtained: 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 The recursive formula for linear acceleration is obtained: 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, i I k is the inertia tensor of the kth connecting rod.

4. The robot satellite attitude control method based on mechanical arm dynamics according to claim 2 is characterized in that: Step S3 further comprises: S31. Calculate the interference force F0 and torque N0 that the base expects to receive from the manipulator: F0=f1, N0=c 01 ×f1+n1 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 : f k =f k-1 -F k-1 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 : n k =n k-1 -N k-1 -(l k-1 ×F k-1 )-(a k ×f 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. According to the external force f6 and external moment n6 of the sixth connecting rod, calculate the recursive expression of the inward recursive inertia force F′6 and inertia moment N′6 of the sixth connecting rod: F′6=f6,N′6=n6-a6×F6 Wherein, a6 is the center of mass position of the sixth link in the sixth link coordinate system.

5. The robot satellite attitude control method based on mechanical arm dynamics according to claim 4 is characterized in that: Step S5 further comprises: S51, according to the angular acceleration of each connecting rod, using steps S21 to S26 and steps S31 to 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.

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

7. The robot satellite attitude control method based on mechanical arm dynamics according to any one of claims 1 to 5, 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