A mechanical arm system planning control method for simulating satellite tumbling motion

Abstract

The application relates to a mechanical arm system planning control method for simulating satellite rolling motion, and relates to a mechanical arm system planning control method.The application is used to solve the problems that the motion state of a failed satellite is difficult to simulate accurately and the motion range is small in the prior art.The steps of the application include the following steps: setting a pose parameter of a center-of-mass coordinate system of a failed satellite relative to a capture surface coordinate system of the failed satellite; setting a main inertia axis of the failed satellite and an initial angular velocity relative to the center-of-mass coordinate system; step 3, obtaining a pose matrix of the motion of the failed satellite by integrating the angular velocity of the center-of-mass coordinate system of the failed satellite; step 4, calculating a desired pose matrix of a mechanical arm; step 5, generating a desired joint angle q of the servo mechanical arm by a joint controller of the mechanical arm d The required driving torque drives the mechanical arm to move, and the simulation of the rolling motion of the failed satellite is realized.The application belongs to the technical field of space ground simulation experiments.

Description

TECHNICAL FIELD

[0001] The application relates to a mechanical arm system planning control method, and belongs to the technical field of spaceflight ground simulation experiments. BACKGROUND

[0002] With the development of human space science and technology, the scale of space exploration tasks is increasing, and the number of failed satellites is also increasing rapidly. These satellites occupy limited orbital resources and seriously affect the safe operation of normal satellites.

[0003] The failed satellite has no external torque in space, and the angular momentum moment axis remains constant. However, the satellite mostly has residual angular velocity, forming a rolling state of spin coupling, precession and nutation superposition, that is, the satellite rotates around the principal axis of inertia, the principal axis of inertia precesses around the momentum moment axis, and the nutation angle formed by the principal axis of inertia and the momentum moment axis slightly shakes.

[0004] The complex motion state of the failed target poses a great challenge to the on-orbit service task. Simulating the rolling motion of the failed satellite can help deeply understand the dynamic characteristics of the target satellite. The motion information is the basis for the service spacecraft to develop a rendezvous capture and operation strategy, and directly affects the success rate and reliability of the task. Therefore, designing a ground device and method for simulating the rolling motion of the failed satellite has important significance for the reliable implementation of the on-orbit service task. SUMMARY

[0005] The application is to solve the problem that the motion state of the failed satellite in the prior art is difficult to accurately simulate and the simulation motion range is small, and further provides a mechanical arm system planning control method for simulating the rolling motion of the satellite.

[0006] The technical scheme adopted by the application to solve the above problems is that the steps of the application include:

[0007] Step 1, setting the pose parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite, and calculating the pose matrix therebetween, wherein the X, Y and Z axes of the center of mass coordinate system of the failed satellite 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 principal 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, integrating the angular velocity of the failed satellite relative to the center of mass coordinate system to obtain the pose matrix of the motion of the failed satellite;

[0010] Step 4, calculating the desired pose matrix of the mechanical arm, and obtaining the desired joint angle of the mechanical arm through inverse kinematics solution;

[0011] Step 5, generating desired joint angles q of the servo robot arm by the robot arm joint controller d The required driving torque to drive the robot arm to move to realize the simulation of the tumbling motion of the failed satellite.

[0012] Further, step 1 specifically comprises:

[0013] Step 101, the pose 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 ], wherein [x cr ,y cr ,z cr ] is the distance parameter, and [α cr ,β cr ,γ cr ] is the attitude parameter, which is represented by the ZYX Euler angle of the system;

[0014] Step 102, calculating the pose matrix of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite from [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ] Specifically as follows:

[0015]

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

[0017] Step 103, calculating 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 from

[0018]

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

[0020] Further, step 2 specifically comprises:

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

[0022] Step 202, differential equations of the rolling motion of the failed satellite are established by [I X ,I Y ,I Z ], wherein is a differential of the angular velocity of the failed satellite relative to the center of mass coordinate system, and [ω x ,ω y ,ω z ] is the angular velocity of the failed satellite relative to the center of mass coordinate system;

[0023]

[0024] Step 203, the angular velocity [ω xi ,ω yi ,ω zi ] of the failed satellite relative to the center of mass coordinate system is obtained by a fourth-order Runge-Kutta numerical integration method from [ω x ,ω y ,ω z ] and the differential equations established in step 202;

[0025]

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

[0027] Further, step 3 specifically comprises:

[0028] Step 301, a unit quaternion differential of the motion of the failed satellite is calculated from [ω x ,ω y ,ω z ];

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

[0030] Step 302, the unit quaternion differential is calculated from The unit quaternion [q0,q1,q2,q3] of the failed satellite motion is obtained by integration;

[0031]

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

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

[0034]

[0035] In formula (9), qustern2mtrx() represents the function of 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 robotic arm;

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

[0041]

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

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

[0044] 1. The present invention can fully 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 inertia and different residual angular velocities. The parameter adjustment range is large, and 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 This is a schematic diagram of the robotic arm system that simulates the tumbling motion of a satellite;

[0047] Figure 2 This is a schematic diagram of the coordinate system used by the robotic arm to simulate the tumbling motion of a failed satellite;

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

[0049] Figure 4 is a schematic diagram of the desired joint angle of the mechanical arm;

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

[0051] Figure 6 is a schematic diagram of different poses of the failed satellite's capture surface simulator captured by the global camera during the movement of the mechanical arm. DETAILED DESCRIPTION

[0052] Specific implementation one: as shown in the figure, a mechanical arm system planning control method for simulating satellite tumbling motion, Figures 1 to 6

[0053] The mechanical arm system is composed of a 7-DOF mechanical arm, a failed satellite's capture surface simulator, an external light source, and a global camera.

[0054] Wherein:

[0055] The failed satellite's capture surface simulator is fixed to the end of the 7-DOF mechanical arm, and the tumbling state of the failed satellite is formed by the active movement of the mechanical arm;

[0056] The external light source irradiates the failed satellite's capture surface simulator, forming optical conditions similar to the real capture scene in space;

[0057] The global camera collects dynamic pictures of the failed satellite's capture surface simulator during the movement of the mechanical 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 center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite, and calculate the pose matrix between them, wherein the X, Y, Z axes of the center of mass coordinate system of the failed satellite are the principal axes of inertia corresponding to the failed satellite, and the X axis is also the spin axis of the failed satellite; Specifically includes:

[0060] Step 101, the pose parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite are denoted as [x cr ,y cr ,z cr ,α cr ,β cr ,γ cr ], wherein [x cr ,y cr ,z​cr ] are distance parameters, [a cr , b cr , g cr ] are attitude parameters, and are represented by ZYX Euler angles of the system;

[0061] Step 102, calculating the pose matrix of the centroid coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite from [x cr , y cr , z cr , a cr , b cr , g cr ] Specifically as follows:

[0062]

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

[0064] Step 103, calculating the pose matrix of the capture surface coordinate system of the failed satellite relative to the centroid coordinate system of the failed satellite from

[0065]

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

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

[0068] Step 201, the inertia of the principal inertia axis of the failed satellite is denoted as [I X , I Y , I Z ], and the initial angular velocity of the failed satellite relative to the centroid coordinate system is denoted as [w xi , w yi , w zi ];

[0069] Step 202, calculating the angular velocity of the failed satellite relative to the centroid coordinate system from [I​X ,I Y ,I Z ]establishes the differential equation of the tumbling motion of the failed satellite, wherein is the differential of the angular velocity of the failed satellite relative to the center of mass 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, the angular velocity [ω xi ,ω yi ,ω zi ] of the failed satellite relative to the center of mass coordinate system is obtained by the fourth-order Runge-Kutta numerical integration method from [ω x ,ω y ,ω z ] and the differential equation established in step 202;

[0072]

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

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

[0075] Step 301, the unit quaternion differential of the failed satellite motion is calculated from [ω x ,ω y ,ω z ];

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

[0077] Step 302, the unit quaternion [q0, q1, q2, q3] of the failed satellite motion is obtained by integrating ;

[0078]

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

[0080] Step 303, the pose matrix of the failed satellite motion is calculated from [q0, q1, q2, q3]

[0081]

[0082] In formula (9), quatern2mtrx() represents a function of converting a unit quaternion into a homogeneous matrix;

[0083] Step 4, the expected pose matrix of the robot arm is calculated, and the expected joint angle of the robot arm is calculated by inverse kinematics of the robot arm; specifically including:

[0084] Step 401, the expected pose matrix of the end of the robot arm is calculated

[0085]

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

[0087] Step 402, the expected joint angle q of the robot arm is obtained from d ;

[0088]

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

[0090] Step 5, the expected joint angle q of the servo robot arm is generated by the joint controller of the robot arm d The driving torque required to drive the robot arm to move is simulated to simulate the tumbling motion of the failed satellite.

[0091] Embodiment

[0092] A robot arm system as shown in Figure 1 is established, the end of the robot arm is installed with a capture surface simulator of the failed satellite, an external light source is placed beside to irradiate the capture surface simulator of the failed satellite, forming a real optical environment, and a global camera collects dynamic pictures of the capture surface simulator of the failed satellite in the process of robot arm movement as reference information of the motion of the failed satellite.

[0093] Based on the system, the robot arm simulates the tumbling motion of the failed satellite according to the following steps:

[0094] Step one, set the pose parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite, as shown in Figure 2 , and calculate the pose matrix therebetween;

[0095] The pose parameters of the center of mass coordinate system of the failed satellite relative to the capture surface coordinate system of the failed satellite are:

[0096] x cr ​= -0.5m, y cr = 0, z cr = 0, a cr = 0°, b cr = 0°, g cr = 180°

[0097] The pose matrix of the capture plane coordinate system of the failed satellite relative to the center-of-mass coordinate system of the failed satellite is calculated:

[0098]

[0099] The pose matrix of the capture plane coordinate system of the failed satellite relative to the center-of-mass coordinate system of the failed satellite is calculated:

[0100]

[0101] Step two, set the inertia of the principal inertia axis of the failed satellite and the initial angular velocity relative to the center-of-mass coordinate system, and calculate the angular velocity of the failed satellite relative to the center-of-mass coordinate system;

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

[0103] I X = 1606 kg·m 2 , I Y = 783 kg·m 2 , I z = 1309 kg·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 differential equation established 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 shown in Figure 3 ;

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

[0110] The differential of the unit quaternion is solved:

[0111]

[0112] To The integral unit quaternion of the failed satellite motion is:

[0113]

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

[0115]

[0116] Step four, the expected pose matrix of the manipulator is calculated, and the expected joint angle of the manipulator is calculated by inverse kinematics;

[0117] The initial pose matrix of the manipulator is:

[0118]

[0119] The expected pose matrix of the end of the manipulator is calculated

[0120]

[0121] The expected joint angle of the manipulator is obtained by The solving result is shown in Figure 4 .

[0122] Step five, the expected joint angle q of the servo manipulator is generated by the manipulator joint controller d The required driving torque, and the torque result is shown in Figure 5 , to realize the simulation of the failed satellite rolling motion.

[0123] The pictures of different poses of the failed satellite capture surface simulator taken by the global camera during the movement of the manipulator are shown in Figure 6 .

[0124] The above is only the preferred embodiment of the present application, and does not limit the present application in any form. Although the present application has been disclosed as above, it is not intended to limit the present application. Any skilled person in the art can make some changes or modifications to the above disclosed technical content without departing from the scope of the present application, and the equivalent embodiments with equivalent changes are equivalent. Any simple modification, equivalent replacement and improvement of the above embodiments within the scope of the present application, according to the technical essence of the present application, within the spirit and principle of the present application, are still within the protection scope of the present application.​

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 center of mass coordinate system relative to the failed satellite's capture surface coordinate system, and calculate the pose matrix between the two. The X, Y, and Z axes of the failed satellite's center of mass 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: Step 101: The pose parameters of the failed satellite's centroid coordinate system relative to the failed satellite's capture surface coordinate system are recorded as ,in, is the distance parameter, is the attitude parameter, expressed in ZYX Euler angles of the system; Step 102: Calculate the pose matrix of the failed satellite's center of mass coordinate system relative to the failed satellite's capture surface coordinate system , as follows: （1）， In formula (1), Represents a translation function, the output is a translation The distance position matrix, Represents a function of rotation around the Z axis, and the output is rotation around the Z axis The pose matrix of Represents a function of rotation around the Y axis, and the output result is rotation around the Y axis The pose matrix of Represents a function of rotation around the X axis, and the output is rotation around the X axis 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 , （2）， In formula (2), represents the matrix inversion function; Step 2: Set 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 calculate the angular velocity of the failed satellite relative to the center of mass coordinate system; Step 3: Obtain the pose matrix of the failed satellite by integrating the angular velocity of the failed satellite relative to the mass center coordinate system; specifically, the following steps are performed: Step 301: Calculate the unit quaternion differential of the failed satellite motion ; （7）， In formula (7), The function representing the differential of angular velocity to unit quaternion, is the angular velocity of the failed satellite relative to the center of mass coordinate system; Step 302: Integrate to obtain the unit quaternion of the failed satellite motion ; （8）， In formula (8), represents the integral over time; Step 303: Calculate the pose matrix of the failed satellite ; （9）， In formula (9), Function that represents the conversion of unit quaternion to homogeneous matrix; Step 4: Calculate the desired pose matrix of the manipulator and obtain the desired joint angles of the manipulator by solving the inverse kinematics of the manipulator. Specifically, Step 401: Calculate the desired pose matrix of the end of the robotic arm ; （10）， In formula (10), is the initial pose matrix of the robotic arm; Step 402: Get the desired joint angles of the robotic arm ; （11）， In formula (11), is the inverse kinematics function of the robotic arm; Step 5: The robot arm joint controller generates the desired joint angle of the servo robot arm The required driving torque drives the robotic arm to move and simulate 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, wherein: Step 2 specifically includes: Step 201: The inertia of the main inertia axis of the failed satellite is recorded as , the initial angular velocity of the failed satellite relative to the center of mass coordinate system is recorded as ; Step 202: The differential equation of the tumbling motion of the failed satellite is established, where is the angular velocity differential of the failed satellite relative to the center of mass coordinate system, is the angular velocity of the failed satellite relative to the center of mass coordinate system; （3）， （4）， （5）； Step 203: , , ] and the differential equation established in step 202, the angular velocity of the failed satellite relative to the center of mass coordinate system is obtained by the fourth-order Runge-Kutta numerical integration method ; （6）， In formula (6), represents the fourth-order Runge–Kutta numerical integration function.

Images