Mechanical arm system planning control method for simulating satellite tumbling motion

Abstract

The invention relates to a mechanical arm system planning control method, in particular to a mechanical arm system planning control method for simulating satellite rolling motion. The invention aims to solve the problems that the motion state of a failure satellite is difficult to accurately simulate and the simulatable motion range is small in the prior art. The method comprises the following steps: setting pose parameters of a mass center coordinate system of a failed satellite relative to a capture surface coordinate system of the failed satellite; the inertia of the main inertia axis of the failure satellite and the initial angular velocity of the relative centroid coordinate system are set; 3, obtaining a pose matrix of the motion of the failed satellite according to the angular velocity integration of the failed satellite relative to the centroid coordinate system; 4, an expected pose matrix of the mechanical arm is obtained through calculation; and step 5, generating a driving torque required by an expected joint angle qd of the servo mechanical arm by a mechanical arm joint controller, driving the mechanical arm to move, and realizing simulation of tumbling motion of the failed satellite. The invention belongs to the technical field of spaceflight ground simulation experiments.

Description

Technical Field

[0001] The invention relates to a mechanical arm system planning and control method, belonging to the technical field of aerospace ground simulation experiments. Background Art

[0002] With the development of human space science and technology, the scale of space exploration missions has been increasing. At the same time, the number of failed satellites has increased dramatically. These satellites have occupied limited orbital resources and seriously affected the safe operation of normal satellites.

[0003] A failed satellite has no external torque in space, and its angular momentum axis remains constant. However, most satellites have residual angular velocity, forming a tumbling state of spin-coupled nutation superimposed precession, that is, the satellite rotates around the principal axis of inertia, the principal axis of inertia precesses around the momentum axis, and at the same time the nutation angle formed by the principal axis of inertia and the momentum axis will shake slightly.

[0004] This complex motion state of the failed target poses a huge challenge to the on-orbit service mission. Simulating the tumbling motion of a failed satellite can provide a deep understanding of the dynamic characteristics of the target satellite. Its motion information is the basis for the service vehicle to formulate rendezvous, capture and operation strategies, which directly affects the success rate and reliability of the mission. Therefore, designing a ground device and method to simulate the tumbling motion of a failed satellite is of great significance for the reliable implementation of the on-orbit service mission. Summary of the invention

[0005] The present invention aims to solve the problems in the prior art that the motion state of a failed satellite is difficult to simulate accurately and the range of simulated motion is small, and further proposes a robot arm system planning and control method for simulating the tumbling motion of a satellite.

[0006] The technical solution adopted by the present invention to solve the above-mentioned problem is: the steps of the present invention include:

[0007] Step 1: Set the pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system, and calculate the pose matrix between the two, where the X, Y, and Z axes of the failed satellite's centroid coordinate system are the principal inertia axes corresponding to the failed satellite, and the X axis is also the spin axis of the failed satellite;

[0008] Step 2, setting the inertia of the main inertia axis of the failed satellite and the initial angular velocity relative to the center of mass coordinate system, and calculating the angular velocity of the failed satellite relative to the center of mass coordinate system;

[0009] Step 3, the position and posture matrix of the failed satellite motion is obtained by integrating the angular velocity of the failed satellite relative to the mass center coordinate system;

[0010] Step 4: Calculate the desired pose matrix of the robot arm, and solve the desired joint angles of the robot arm by inverse kinematics of the robot arm;

[0011] Step 5: The robot arm joint controller generates the desired joint angle q of the servo robot arm d The required driving torque drives the mechanical arm to move, thus simulating the tumbling motion of the failed satellite.

[0012] Furthermore, step 1 specifically includes:

[0013] Step 101: The position and posture parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite are recorded as [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ], where [x cr ,y cr ,z cr ] is the distance parameter, [α cr ,β cr ,γ cr ] is the attitude parameter, expressed by the ZYX Euler angle of the system;

[0014] Step 102: [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ] Calculate the pose matrix of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system The details are as follows:

[0015]

[0016] In formula (1), Tran() represents the translation function, and the output result is the translation [x gc ,y gc ,z gc ] distance position matrix, RotZ() represents the function of rotating around the Z axis, and the output result is a rotation of α around the Z axis cr The pose matrix of RotY() represents the function of rotating around the Y axis, and the output result is the rotation β around the Y axis cr The pose matrix of RotX() represents the function of rotating around the X axis, and the output result is the rotation of γ around the X axis. cr The pose matrix of

[0017] Step 103: Calculate the pose matrix of the capture surface coordinate system of the failed satellite relative to the center of mass coordinate system of the failed satellite

[0018]

[0019] In formula (2), inv() represents a matrix inversion function.

[0020] Furthermore, step 2 specifically includes:

[0021] Step 201: The inertia of the main inertia axis of the failed satellite is recorded as [I X ,I Y ,I Z ], the initial angular velocity of the failed satellite relative to the center of mass coordinate system is recorded as [ω xi ,ω yi ,ω zi ];

[0022] Step 202: [I X ,I Y ,I Z ] Establish the differential equation of the tumbling motion of the failed satellite, where is the angular velocity differential of the failed satellite relative to the mass center coordinate system, [ω x ,ω y ,ω z ] is the angular velocity of the failed satellite relative to the center of mass coordinate system;

[0023]

[0024] Step 203: [ω xi ,ω yi ,ω zi ] and the differential equation established in step 2.2, the angular velocity [ω x ,ω y ,ω z ];

[0025]

[0026] In formula (6), RungeKuttaSolver() represents the fourth-order Runge-Kutta numerical integration function.

[0027] Furthermore, step 3 specifically includes:

[0028] Step 301: [ω x ,ω y ,ω z ] Calculate the unit quaternion differential of the failed satellite motion

[0029] In formula (7), angvelo2quadot() represents the function of converting angular velocity to unit quaternion differential;

[0030] Step 302: The integral obtains the unit quaternion [q0,q1,q2,q3] of the failed satellite motion;

[0031]

[0032] In formula (8), ∫()dt represents the integral over time;

[0033] Step 303: Calculate the position matrix of the failed satellite motion from [q0, q1, q2, q3]

[0034]

[0035] In formula (9), qustern2mtrx() represents a function for converting a unit quaternion to a homogeneous matrix.

[0036] Furthermore, step 4 specifically includes:

[0037] Step 401: Calculate the desired pose matrix of the end of the robotic arm

[0038]

[0039] In formula (10), is the initial pose matrix of the robot arm;

[0040] Step 402: Get the desired joint angle q of the robot arm d ;

[0041]

[0042] In formula (11), inversekinematics() is the inverse kinematics function of the robot arm.

[0043] The beneficial effects of the present invention are:

[0044] 1. The present invention can completely simulate the tumbling state of a failed satellite through the active movement of the robotic arm, without any constrained degrees of freedom;

[0045] 2. The present invention can simulate the rolling state of satellites with different inertias and residual angular velocities, and the parameter adjustment range is large. The use of unit quaternions to solve the posture avoids the posture singularity caused by large nutation angles. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a schematic diagram of the robotic arm system that simulates the tumbling motion of the satellite;

[0047] Figure 2 It is a schematic diagram of the relevant coordinate system for the robotic arm to simulate the tumbling motion of a failed satellite;

[0048] Figure 3 It is a schematic diagram of the angular velocity results of the failed satellite relative to the mass center coordinate system;

[0049] Figure 4 is a schematic diagram of the desired joint angles of the robotic arm;

[0050] Figure 5 It is a schematic diagram of the driving torque generated by the robot arm joint controller;

[0051] Figure 6 This is a schematic diagram of different postures of the capture surface simulator of the failed satellite obtained by the global camera during the movement of the robotic arm. DETAILED DESCRIPTION

[0052] Specific implementation method 1: Figures 1 to 6 As shown, a robot arm system planning and control method for simulating satellite tumbling motion is

[0053] The robotic arm system consists of a 7-DOF robotic arm, a capture surface simulator for a failed satellite, an external light source and a global camera.

[0054] in:

[0055] The capture surface simulator of the failed satellite is fixedly connected to the end of the 7-DOF robotic arm, and the active movement of the robotic arm forms the tumbling state of the failed satellite.

[0056] An external light source illuminates the capture surface simulator of the failed satellite to create optical conditions similar to the real capture scene in space;

[0057] The global camera collects dynamic images of the capture surface simulator of the failed satellite during the movement of the robotic arm as reference information for the movement of the failed satellite.

[0058] The specific steps of the method include:

[0059] Step 1: Set the pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system, and calculate the pose matrix between the two, where the X, Y, and Z axes of the failed satellite's centroid coordinate system are the principal inertia axes corresponding to the failed satellite, and the X axis is also the spin axis of the failed satellite; specifically:

[0060] Step 101: The position and posture parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite are recorded as [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ], where [x cr ,y cr ,zcr ] is the distance parameter, [α cr ,β cr ,γ cr ] is the attitude parameter, expressed by the ZYX Euler angle of the system;

[0061] Step 102: [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ] Calculate the pose matrix of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system The details are as follows:

[0062]

[0063] In formula (1), Tran() represents the translation function, and the output result is the translation [x gc ,y gc ,z gc ] distance position matrix, RotZ() represents the function of rotating around the Z axis, and the output result is a rotation of α around the Z axis cr The pose matrix of RotY() represents the function of rotating around the Y axis, and the output result is the rotation β around the Y axis cr The pose matrix of RotX() represents the function of rotating around the X axis, and the output result is the rotation of γ around the X axis. cr The pose matrix of

[0064] Step 103: Calculate the pose matrix of the capture surface coordinate system of the failed satellite relative to the center of mass coordinate system of the failed satellite

[0065]

[0066] In formula (2), inv() represents the matrix inversion function;

[0067] Step 2: setting the inertia of the main inertia axis of the failed satellite and the initial angular velocity relative to the center of mass coordinate system, and calculating the angular velocity of the failed satellite relative to the center of mass coordinate system; specifically including:

[0068] Step 201: The inertia of the main inertia axis of the failed satellite is recorded as [I X ,I Y ,I Z ], the initial angular velocity of the failed satellite relative to the center of mass coordinate system is recorded as [ω xi ,ω yi ,ω zi ];

[0069] Step 202: [IX ,I Y ,I Z ] Establish the differential equation of the tumbling motion of the failed satellite, where is the angular velocity differential of the failed satellite relative to the mass center coordinate system, [ω x ,ω y ,ω z ] is the angular velocity of the failed satellite relative to the center of mass coordinate system;

[0070]

[0071] Step 203: [ω xi ,ω yi ,ω zi ] and the differential equation established in step 2.2, the angular velocity [ω x ,ω y ,ω z ];

[0072]

[0073] In formula (6), RungeKuttaSolver() represents the fourth-order Runge-Kutta numerical integration function;

[0074] Step 3: obtain the position matrix of the failed satellite motion by integrating the angular velocity of the failed satellite relative to the mass center coordinate system; specifically including:

[0075] Step 301: [ω x ,ω y ,ω z ] Calculate the unit quaternion differential of the failed satellite motion

[0076] In formula (7), angvelo2quadot() represents the function of converting angular velocity to unit quaternion differential;

[0077] Step 302: The integral obtains the unit quaternion [q0,q1,q2,q3] of the failed satellite motion;

[0078]

[0079] In formula (8), ∫()dt represents the integral over time;

[0080] Step 303: Calculate the position matrix of the failed satellite motion from [q0, q1, q2, q3]

[0081]

[0082] In formula (9), qustern2mtrx() represents the function of converting the unit quaternion to the homogeneous matrix;

[0083] Step 4: Calculate the desired pose matrix of the robot arm, and solve the desired joint angle of the robot arm by inverse kinematics of the robot arm; specifically include:

[0084] Step 401: Calculate the desired pose matrix of the end of the robotic arm

[0085]

[0086] In formula (10), is the initial pose matrix of the robot arm;

[0087] Step 402: Get the desired joint angle q of the robot arm d ;

[0088]

[0089] In formula (11), inversekinematics() is the inverse kinematics function of the robot arm;

[0090] Step 5: The robot arm joint controller generates the desired joint angle q of the servo robot arm d The required driving torque drives the mechanical arm to move, thus simulating the tumbling motion of the failed satellite.

[0091] Example

[0092] Establish as Figure 1 The robotic arm system shown in the figure has a failed satellite's capture surface simulator installed at the end of the robotic arm, and an external light source is placed on the side to illuminate the failed satellite's capture surface simulator to form a real optical environment. The global camera collects dynamic images of the failed satellite's capture surface simulator during the movement of the robotic arm as reference information for the failed satellite's movement.

[0093] Based on this system, the following steps are taken to implement the robot arm to simulate the tumbling motion of a failed satellite:

[0094] Step 1: Set the pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system, such as Figure 2 As shown, the pose matrix between the two is calculated;

[0095] The pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system are:

[0096] x cr =-0.5m,ycr =0, z cr =0,α cr =0°, β cr =0°,γ cr =180°

[0097] Calculate the pose matrix of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system:

[0098]

[0099] Calculate the pose matrix of the capture surface coordinate system of the failed satellite relative to the center of mass coordinate system of the failed satellite:

[0100]

[0101] Step 2, setting the inertia of the main inertia axis of the failed satellite and the initial angular velocity relative to the center of mass coordinate system, and calculating the angular velocity of the failed satellite relative to the center of mass coordinate system;

[0102] The inertia of the main inertia axis of the failed satellite is:

[0103] I X =1606kg·m 2 , I Y =783kg·m 2 , I z =1309kg·m 2

[0104] The initial angular velocity of the failed satellite relative to the center of mass coordinate system is:

[0105] ω xi =16° / s,ω yi =4° / s,ω zi =4° / s

[0106] The established differential equation is as follows:

[0107]

[0108] According to the initial value [16,4,4] and the above differential equation, the angular velocity of the failed satellite relative to the center of mass system is obtained by the fourth-order Runge-Kutta numerical integration method as follows: Figure 3 As shown;

[0109] Step 3, the position matrix of the failed satellite motion is obtained by integrating the angular velocity of the failed satellite relative to the mass center coordinate system;

[0110] Solve the differential of a unit quaternion:

[0111]

[0112] right The integral gives the unit quaternion of the failed satellite motion:

[0113]

[0114] The pose matrix of the failed satellite motion is calculated by [q0, q1, q2, q3]:

[0115]

[0116] Step 4: Calculate the desired pose matrix of the robot arm, and solve the desired joint angle of the robot arm by the inverse kinematics of the robot arm;

[0117] The initial pose matrix of the robotic arm is:

[0118]

[0119] Calculate the desired pose matrix of the end of the robot arm

[0120]

[0121] Depend on Get the desired joint angles of the robot arm The solution result is as follows Figure 4 As shown;

[0122] Step 5: The robot joint controller generates the desired joint angle q of the servo robot d The required driving torque, the torque result is as follows Figure 5 As shown, the simulation of the tumbling motion of a failed satellite is realized.

[0123] The images of different positions of the capture surface simulator of the failed satellite taken by the global camera during the movement of the robotic arm are as follows: Figure 6 shown.

[0124] The above is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technician familiar with this profession can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent replacement and improvement made to the above embodiments without departing from the content of the technical solution of the present invention, based on the technical essence of the present invention, within the spirit and principles of the present invention, still fall within the protection scope of the technical solution of the present invention.

Claims

1. A method for planning and controlling a robotic arm system for simulating satellite tumbling motion, characterized in that: The specific steps include: Step 1: Set the pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system, and calculate the pose matrix between the two, where the X, Y, and Z axes of the failed satellite's centroid coordinate system are the principal inertia axes corresponding to the failed satellite, and the X axis is also the spin axis of the failed satellite; Step 2, setting the inertia of the main inertia axis of the failed satellite and the initial angular velocity relative to the center of mass coordinate system, and calculating the angular velocity of the failed satellite relative to the center of mass coordinate system; Step 3, the position and posture matrix of the failed satellite motion is obtained by integrating the angular velocity of the failed satellite relative to the mass center coordinate system; Step 4: Calculate the desired pose matrix of the robot arm, and solve the desired joint angle of the robot arm by the inverse kinematics of the robot arm; Step 5: The robot arm joint controller generates the desired joint angle q of the servo robot arm d The required driving torque drives the mechanical arm to move, thus simulating the tumbling motion of the failed satellite.

2. The method for planning and controlling a robotic arm system for simulating satellite tumbling motion according to claim 1, characterized in that: Step 1 specifically includes: Step 101: The position and posture parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite are recorded as [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ], where [x cr ,y cr ,z cr ] is the distance parameter, [α cr ,β cr ,γ cr ] is the attitude parameter, expressed by the ZYX Euler angle of the system; Step 102: [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ] Calculate the pose matrix of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system The details are as follows: In formula (1), Tran() represents the translation function, and the output result is the translation [x gc ,y gc ,z gc ] distance position matrix, RotZ() represents the function of rotating around the Z axis, and the output result is a rotation of α around the Z axis cr The pose matrix of RotY() represents the function of rotating around the Y axis, and the output result is the rotation β around the Y axis cr The pose matrix of RotX() represents the function of rotating around the X axis, and the output result is the rotation of γ around the X axis. cr The pose matrix of Step 103: Calculate the pose matrix of the capture surface coordinate system of the failed satellite relative to the center of mass coordinate system of the failed satellite In formula (2), inv() represents a matrix inversion function.

3. The method for planning and controlling a robotic arm system for simulating satellite tumbling motion according to claim 1, characterized in that: Step 2 specifically includes: Step 201: The inertia of the main inertia axis of the failed satellite is recorded as [I X ,I Y ,I Z ], the initial angular velocity of the failed satellite relative to the center of mass coordinate system is recorded as [ω xi ,ω yi ,ω zi ]; Step 202: [I X ,I Y ,I Z ] Establish the differential equation of the tumbling motion of the failed satellite, where is the angular velocity differential of the failed satellite relative to the mass center coordinate system, [ω x ,ω y ,ω z ] is the angular velocity of the failed satellite relative to the center of mass coordinate system; Step 203: [ω xi ,ω yi ,ω zi ] and the differential equation established in step 2.2, the angular velocity [ω x ,ω y ,ω z ]; In formula (6), RungeKuttaSolver() represents the fourth-order Runge-Kutta numerical integration function.

4. The method for planning and controlling a robotic arm system for simulating satellite tumbling motion according to claim 1, characterized in that: Step 3 specifically includes: Step 301: [ω x ,ω y ,ω z ] Calculate the unit quaternion differential of the failed satellite motion In formula (7), angvelo2quadot() represents the function of converting angular velocity to unit quaternion differential; Step 302: The integral obtains the unit quaternion [q0,q1,q2,q3] of the failed satellite motion; In formula (8), ∫()dt represents the integral over time; Step 303: Calculate the position matrix of the failed satellite motion from [q0, q1, q2, q3] In formula (9), qustern2mtrx() represents a function for converting a unit quaternion to a homogeneous matrix.

5. The method for planning and controlling a robotic arm system for simulating satellite tumbling motion according to claim 1, characterized in that: Step 4 specifically includes: Step 401: Calculate the desired pose matrix of the end of the robotic arm In formula (10), is the initial pose matrix of the robot arm; Step 402: Get the desired joint angle q of the robot arm d ; In formula (11), inversekinematics() is the inverse kinematics function of the robot arm.

Images