Research on QPSO algorithm for trajectory planning of FFSR general kinematics modeling

. Aiming at the optimal trajectory planning problem faced by free floating space robot ( FFSR ) in capturing non cooperative target ( NCT ) with unknown dynamic parameters, firstly, based on the unique intrinsic characteristics of FFSR in microgravity environment, a general FFSR kinematic model based on generalized velocity variables and an NCT dynamic model based on quaternion method are established. Secondly, the optimal criterion of FFSR trajectory planning and the parametric modeling method of FFSR manipulator joint trajectory based on sinusoidal function are studied; Finally, a QPSO algorithm for FFSR trajectory planning based on the optimal comprehensive index is proposed


Introduction
Free-Floating Space Robot (FFSR) is composed of a tracking satellite body and a mechanical arm mounted on it. It can fly or float freely in the universe and replace astronauts to complete EVA operation. In view of the importance of FFSR in future space operations, the optimal trajectory planning and control problem faced by FFSR capturing Non-Cooperative Target (NCT) has become one of the research focuses of scholars [1][2][3][4][5]. Many scholars put forward the trajectory planning scheme of FFSR capturing NCT from different angles. Most scholars regard NCT as a point target or a rigid body with known state and dynamic parameters, thus ignoring the complex dynamics and autonomous acquisition problems caused by the rolling state of NCT with unknown motion state and dynamic parameters.
Dubowsky et al. proposed to use disturbance diagram to express the influence of manipulator motion on spacecraft attitude [6]. Papadopoulos et al. used coordinate transformation to map nonholonomic constraints into a space that can simplify operation, avoiding the generation of dynamic singular points, and the planned trajectory is smooth and continuous [7]. Inaba et al. proposed a method for on orbit recognition and acquisition of NCT assuming that the shape, size and mass of the target are known [8]. M. D. Lichter et al. studied the recognition of NCT, and used laser imaging to estimate the shape, motion and related parameters of targets [9]. Z. Ma et al. proposed a trajectory iteration algorithm with minimum fuel consumption for the optimal control of the rendezvous between FFSR and spatial rotating targets with known dynamic parameters [10]. F. Aghili et al. first studied the trajectory planning of FFSR capturing NCT without considering the conditional constraints of capture time, and then proposed a similar scheme to increase joint and speed constraints [11]. P. Singla et al. proposed the visual guidance method of FFSR automatic interception and berthing NCT, and proposed the adaptive control method of spacecraft interception and berthing according to the uncertainty of computer vision measurement [12]. Xu W F et al. designed a trajectory planning algorithm in cartesian coordinates by using the nonholonomic motion characteristics of FFSR system, and combined with particle swarm optimization (PSO) to search the optimal solution of the objective function [13]. Liu ZX et al. studied the trajectory optimization of the manipulator of FFSR. In order to make the attitude stability of FFSR system unaffected, a parametric trajectory planning method based on PSO is proposed [14]. Shi Zhong et al. proposed a nonholonomic motion planning method based on polynomial interpolation and PSO for FFSR trajectory planning [15]. Quantum-behaved particle swarm optimization(QPSO) is a very important branch in the field of cluster intelligence. QPSO algorithm has simple concept, less parameters, easy implementation and fast convergence speed. QPSO algorithm mainly adopts the superposition state characteristics and probability expression characteristics in quantum theory. Among them, the superposition state characteristics can make a single particle express more states and potentially increase the diversity of the population. The probability expression characteristic is to express the state of the particle with a certain probability, and the probability of the occurrence state is involved in the operation [16] [17] [18]. Shi Ye et al. transformed the nonholonomic cartesian path planning problem of FFSR system into the optimization problem of nonlinear system, and solved the nonlinear optimization problem by using QPSO algorithm, realizing the goal of nonholonomic path planning [19]. From the current research status, the problem of FFSR capturing NCT faces severe challenges and becomes the bottleneck of research in the research of basic theories and technologies such as QPSO optimal trajectory planning based on machine vision.

FFSR general kinematics modeling
The FFSR studied in this paper consists of  R dimensional vector composed of the angular momentum and linear momentum of connecting link i around its center of mass i C , representing the generalized total momentum (GTM) of connecting link i . The total kinetic energy K of the FFSR system composed of n + 1 connecting rods can be expressed as equation (9).
Where, the physical meanings of ȣ and M represent the generalized velocity vector and generalized mass matrix of FFSR system respectively. Applying the GTM concept of connecting link i to the whole FFSR system, the GTM for any p point of the system can be obtained.
Using the translation theorem and the axis theorem, the generalized total momentum (GTM) of FFSR system can be expressed as equation (11).
p p G C G (11) Where, 6 3 3 u R r cp represents the 3x3 cross product tensor of the centroid position vector cp r of the FFSR system. Replace equation (9) with equation (10) to obtain equation (12). Mȣ ȣ ȣ G T p p w w / (12) According to the kinematic constraint relationship of the connecting rod of FFSR system, the velocity vector can be expressed as equation (13).
Replace (14) with (12) to obtain formula (15). Therefore, the generalized total momentum G of equation (11) can be derived from equations (13) and (15) to equation (16). (16) If the satellite motion is known, taking the satellite as the reference point, the angular velocity E Ȧ and velocity E c r of the end effector can be expressed as: (18) Combining (17) and (18) to describe the generalized velocity vector of the manipulator end effector, the generalized velocity vector of the FFSR system described in equation (13) can be expressed as:

Mȣ G
represents the configuration of FFSR manipulator. 1 represents the joint angular velocity vector of FFSR manipulator. The is the rotational angular displacement of the joint. Scalar i q is the rotational angular velocity of joint i J .  C , it can be seen that its inverse 1 S C exists. * J is the so-called generalized Jacobian matrix(GJM) of FFSR. If the motion of the end effector is known, taking the end effector as the reference point, the generalized velocity vector of the FFSR system described in equation (13) can be expressed as: . E ȣ is determined by equation (22). The generalized total momentum with the end effector as the reference point can be expressed as: The kinematic model (25) with the end effector as the reference point is simplified into: Obviously, the kinematics model with the end effector as the reference point can more effectively deal with the forward kinematics and inverse kinematics control problems of FFSR. For inverse kinematics, the joint angle M Q of the manipulator can be calculated according to the given generalized velocity E ȣ of the end effector.
When n = 6, the matrix * J or J 6 6u R , if the FFSR is of nonsingular configuration, the inverse of * J or J exists, so that M Q can be obtained, and then M Q can be obtained by integration.
When n > 6, FFSR system is redundantly driven, (24) is an underdetermined system with n unknown variables and 6 linear equations, which can not uniquely determine M Q ; In this case, the optimization method can be used to solve the M Q value satisfying the constraint relationship of equation (24), so as to obtain:

NCT dynamic model based on quaternion method
Based on the theory of optimal control and estimation, this paper proposes an FFSR trajectory planning based on machine vision to intercept NCT with unknown dynamic parameters. The QPSO optimal trajectory control diagram of NCT captured by machine vision FFSR is shown in Figure 1. Its main idea is based on the "prediction planning control" method.
The extended Kalman filter (EKF) is used to obtain the reliable estimation of NCT motion state and dynamic parameters according to the noisy NCT measurement data collected by machine vision, so as to make the system converge as soon as possible, so as to reliably predict the NCT motion, and detect the convergence of the system by monitoring the covariance matrix of EKF. In order to avoid the huge impact force generated when FFSR captures NCT, it is necessary to establish an effective collision dynamics model. During the collision between FFSR and NCT, FFSR and NCT can be studied as a complete multi rigid body system. At this time, the collision force between FFSR and NCT can be regarded as internal force. In the microgravity environment, the linear momentum conservation, angular momentum conservation and energy conservation of the whole system meet the collision impulse theorem (collectively referred to as physical constraints). In addition, certain geometric constraints must be met during collision. The key problem is how to establish an effective model of FFSR collision dynamics in microgravity environment under physical and geometric constraints such as collision impulse theorem. Therefore, a dynamic model of Therefore, the dynamic equation of NCT rotation can be described as Euler equation in the form of inertial parameters. According to this equation, based on the robot vision data,

Trajectory planning QPSO algorithm
This paper presents a QPSO algorithm for FFSR trajectory planning based on the optimal comprehensive index.

Determine the optimal criterion of FFSR trajectory planning
The commonly used optimization criteria in FFSR trajectory planning are: time optimal trajectory planning, energy optimal trajectory planning, impact optimal trajectory planning and comprehensive optimal trajectory planning. In this paper, a comprehensive index optimization criterion based on FFSR is proposed to minimize the attitude disturbance of satellite base and meet the constraints of satellite base attitude variation range, joint angular velocity and angular acceleration.

Minimum objective function of satellite base attitude disturbance based on FFSR
In order to minimize the base attitude disturbance of FFSR manipulator, the goal of FFSR joint trajectory planning is to plan the movement of each joint angle of FFSR manipulator to meet the constraints of the following equations (34) ~ (36), and minimize the change of base attitude of FFSR satellite after the movement.
Where, ‫ݍ‬ and ‫ݍ‬ ௗ are the initial angle and expected angle values of the i joint angle respectively. ‫ݍ‬ ெ , ‫ݍ‬ ெ௫ , ‫ݍ‬ሶ and ‫ݍ‬ሷ are the ranges of minimum joint angle, maximum joint angle, angular velocity and angular acceleration of the manipulator respectively. When the joint angular velocity and angular acceleration of FFSR manipulator are not limited, the goal of joint trajectory planning can be expressed as: Where, ષ ௗ is the expected joint angle of FFSR manipulator, and ௦ is the attitude angle of FFSR satellite base. According to equation (37), the objective function of manipulator trajectory planning problem with minimum attitude disturbance of FFSR satellite base can be obtained, as shown in equation (38): Where, ԡߜ ௦ ԡ = ඥߜ ௦ ் ߜ ௦ represents the norm of the difference between the attitude angle at the termination time ‫ݐ‬ of the satellite base and the attitude angle at the initial time of the satellite base. ݇ is the weight coefficient, which is determined according to the accuracy requirements of trajectory planning. Different accuracy requirements only need to adjust ݇ , and the objective function ‫ܬ‬ ଵ 1 means that the objective function has met the accuracy requirements.

Objective function considering joint angular velocity and acceleration constraints
Considering the ability of the executive device of the actual FFSR manipulator system, in order to ensure the stability and reliability of the operation of the FFSR manipulator, the angular velocity and angular acceleration of each joint angle of the FFSR manipulator must be limited within a certain range. It is necessary to study the description of the objective function when the joint angular velocity and angular acceleration are limited [20]. Here, the joint angular velocity and angular acceleration cost functions ‫ܬ‬ ષ ሶ and ‫ܬ‬ ષ ሷ are introduced, as shown in equation (39).
Where, ݇ ષ ሶ and ݇ ષ ሷ are the weight coefficients of acceleration constraint and angular acceleration constraint.

Objective function considering the attitude range constraint of FFSR satellite base
Considering that in practice, in order to ensure the normal operation of FFSR satellite, the attitude change of FFSR satellite base is required to be limited within a certain range. Therefore, it is necessary to define the following objective function to restrict the change range of satellite base attitude during the movement of FFSR manipulator: Where, ݇ ఋ ೞ is the attitude angle constraint weight coefficient.
Where,ɁȰ ୱ is the maximum rotation angle of the base around the z axis in time ൣt ‫ݐ‬ ൧. ߜȰ ୱ is the threshold value of the rotation angle of the base around the z axis. When ɁȰ ୱஒ ߜȰ ୱஒ , ‫ܬ‬ ఋ ೞഁ = 0 . When ɁȰ ୱஒ > ߜȰ ୱஒ , ‫ܬ‬ ఋ ೞഁ = ൫ɁȰ ୱஒ െ ߜȰ ୱஒ ൯ ߜȰ ୱஒ ൗ . Where, ɁȰ ୱஒ is the maximum value of the rotation angle of the base around the x axis in time ൣt ‫ݐ‬ ൧. ߜȰ ୱஒ is the threshold value of the rotation angle of the base around the x axis.

Optimal objective function of comprehensive index
According to the above analysis, considering the minimum attitude disturbance of satellite base and the minimum position error of manipulator end based on FFSR, and meeting the constraints of satellite base attitude variation range, joint angular velocity and angular acceleration, the optimal objective function of the comprehensive index is:

Parametric modeling of joint trajectory
In order to effectively and directly constrain the range of joint angles of FFSR manipulator and ensure the good smoothness of joint motion, this paper uses sinusoidal function to parameterize the joint function. In order to ensure the smoothness of the motion of FFSR manipulator at the initial time and termination time, the initial and termination states of the joint are usually required to meet the following constraint equations: In addition, the range of motion of the joint angle of the FFSR manipulator is limited by the mechanical structure and meets the following conditions: Where, ߠ and ߠ ௫ are the minimum and maximum values of joint i, respectively.
Where, ܽ ܽ is a pending parameter. ο ଵ and ο ଶ are determined according to the range of joint angle.
Find the first and second derivatives of equation (46), then the joint angular velocity and angular acceleration of ߠ ‫א‬ (ߠ , ߠ ௫ ) are guaranteed to be: The constraint conditions to make the joint angle and base attitude of FFSR manipulator reach the desired value at the same time are: Substituting equations (44) and (50) into equations (46) to (49) can obtain: Where, ߠ ௗ is the desired angle of the FFSR manipulator joint i. From equations (51) ~ (54), it can be seen that after the joint angle function of FFSR manipulator is parameterized, each joint function contains only two unknown parameters ܽ and ܽ . Therefore, for the FFSR manipulator with n joints, if the unknown parameter matrix ‫܉‬Ԗ ଶ× is determined, the joint trajectory of the FFSR manipulator can be uniquely determined, where the matrix ‫܉‬ is: Therefore, the optimal trajectory planning problem of FFSR manipulator studied in this paper can be transformed into: by solving the unknown parameter matrix ‫܉‬ defined in equation (55), the optimal objective function ‫ܬ‬ of the comprehensive index described in equation (43) can be minimized.

Establish the mathematical model of FFSR optimal trajectory planning
The design idea of optimal trajectory planning of FFSR manipulator based on QPSO is described as follows. For FFSR manipulator with n joints, it is necessary to plan n joint angle trajectories at the same time. Each joint angle trajectory is uniquely determined by a set of ( id D id E ) through parameterized sinusoidal function. The essence of optimization of joint space trajectory planning of FFSR manipulator is to find the optimal ( D E ) combination.Therefore, the position j x of the particle can be represented by the , and its search flow chart is shown in Fig. 2.
Let the number of particles be n, the number of iterations be K, and the maximum number of iterations be N ୫ୟ୶ , the main steps of QPSO algorithm are described as follows.
Step 9 If the optimization end conditions are not met, go to step 2 to continue the optimization.

Conclusion
Firstly, based on the unique intrinsic characteristics of FFSR in microgravity environment, a general FFSR kinematics model is established by introducing the concepts of generalized velocity, generalized force, generalized mass, generalized kinetic energy and generalized total momentum; Then, the NCT dynamic model based on quaternion method is established. Secondly, the joint trajectory parameterization model of FFSR manipulator based on sinusoidal function and the objective function of FFSR trajectory planning are established. Finally, considering the optimal objective function of the comprehensive index based on the minimum attitude disturbance of the satellite base and the minimum position error of the manipulator end, and meeting the constraints of the satellite base attitude variation range, joint angular velocity and angular acceleration, a QPSO algorithm for FFSR trajectory planning based on the optimal comprehensive index is proposed