A space manipulator joint trajectory planning method

Abstract

The application discloses a space mechanical arm joint trajectory planning method, which comprises the following steps: establishing a free-floating system kinematics model, constructing a trajectory optimization problem with a joint as an optimization variable, converting the problem into semi-infinite programming by using control parameterization, constructing a lightweight feedforward neural network as a trajectory generator, designing a differentiable loss function embedded with constraints, and solving by online gradient optimization. The application can generate continuous, smooth trajectories that meet physical and task constraints, does not require pre-training, is not sensitive to initial values, and is suitable for free-floating space mechanical arm systems of any configuration.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm control technology, specifically, it relates to a method for planning the joint trajectory of a spatial robotic arm. Background Technology

[0002] With the increasing number of on-orbit servicing missions, space robotic arms play an irreplaceable role in critical space missions such as satellite maintenance, refueling, and target acquisition. Among these, the free-floating mode, which eliminates the need for active control of the base's position and attitude, significantly saves propellant consumption and extends the spacecraft's on-orbit lifespan, making it a crucial research direction for space robotic arms. In this mode, the total momentum of the entire system (including the base and robotic arm) is conserved, causing the base state to passively change with the robotic arm's movement. This introduces complex characteristics such as strong nonlinear dynamic coupling, nonholonomic constraints, and dynamic singularities, making trajectory planning extremely challenging. Trajectory planning, as the core component of space robotic arm mission execution, aims to generate smooth, safe, and executable joint spatial trajectories while satisfying kinematic, dynamic, and physical constraints. This ensures the end effector accurately reaches the target pose while effectively suppressing base disturbances.

[0003] Currently, the mainstream trajectory planning methods mainly include sampling planning, optimization planning, and reinforcement learning. Sampling methods (such as Fast Random Exploration Tree (RRT) and its variants) are suitable for path search in high-dimensional configuration spaces, but they typically ignore the dynamic characteristics of the system, making it difficult to guarantee the smoothness, time or energy optimality of the generated trajectory, and they cannot strictly handle high-order continuity constraints such as velocity, acceleration, and jerk. Optimization planning models trajectory planning as an optimal control problem, solving it through numerical optimization. Theoretically, it can balance performance indicators and multiple types of constraints, but in practical applications, it often faces problems such as slow convergence speed, sensitivity to initial guesses, and susceptibility to local optima. The difficulty increases significantly, especially when dealing with implicit coupling constraints caused by momentum conservation in free-floating systems. Reinforcement learning methods, while exhibiting good environmental adaptability, rely on a large amount of interactive data for training, resulting in high computational costs. Furthermore, it is difficult to strictly guarantee the satisfaction of physical constraints and mission safety after training, limiting its application in space missions with high reliability requirements. In summary, existing trajectory planning methods still have significant shortcomings in terms of trajectory smoothness, global optimality, constraint handling capabilities, and safety assurance. There is an urgent need to develop a new intelligent planning method that can efficiently handle the complex coupled dynamics and multiple physical constraints of a free-floating space robot arm, and achieve high-precision and high-safety trajectory generation. Summary of the Invention

[0004] The purpose of this invention is to provide a method for planning the joint trajectory of a space robotic arm, which mainly addresses the shortcomings of existing free-floating space robotic arm trajectory planning methods in terms of trajectory smoothness, constraint satisfaction, initial value sensitivity, and safety assurance.

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

[0006] A method for planning the joint trajectory of a space robotic arm includes the following steps:

[0007] S1. Establish the kinematic model of the free-floating space manipulator system consisting of the base and the n-degree-of-freedom manipulator, and its expression is:

[0008] (1)

[0009] in, For a point in time, Let ω be the angular velocity vector of the robotic arm's end effector. This represents the angular velocity vector of the robotic arm joints. Represents the velocity vector of the robotic arm's end effector. , These are the position and angular velocity vectors of the base, respectively. This is the joint angle vector of the robotic arm. , The Jacobian matrices for the spacecraft base motion and the robotic arm motion are as follows:

[0010] (2)

[0011] It is the identity matrix. It is a zero matrix. Let be the unit vector of rotation direction of the i-th robotic arm joint. The vectors representing the terminal position and the positions of the i robotic arm joints. The relative position, Indicates the position of the robotic arm's end effector. represent and cross product;

[0012] S2, based on the changes in the joint angle of the robotic arm, assuming the initial linear momentum and angular momentum are zero, establish the kinematic equations for the base position and angular velocity:

[0013] (3)

[0014] in, The base joint coupling matrix is ​​related to the mass, inertia, joint center of mass position, and rotation vector of the base and joints; substituting equation (3) into equation (1), the kinematic model of the free-floating space manipulator system is updated as follows:

[0015] (4)

[0016] in, , For matrix The row block matrix;

[0017] S3, construct a trajectory optimization problem with joints as optimization variables, establish an objective function, and apply joint physical amplitude constraints, end-effector collision-free constraints, and base attitude constraints. The objective function is:

[0018] (5)

[0019] In the formula, For the end effector of the robotic arm, time is required The target location reached For the end effector of the robotic arm, time is required The target posture reached, Let be the Euler angle vector of the robotic arm's end effector; where, Indicates the yaw angle. Indicates pitch angle, Indicates the roll angle;

[0020] S4, based on the control parameter method, transforms the trajectory optimization problem into a solvable finite-dimensional nonlinear programming problem;

[0021] S5 constructs an L-layer fully connected feedforward neural network to solve the finite-dimensional nonlinear programming problem and outputs the joint trajectory that satisfies all constraints.

[0022] Further, in step S3, the joint physical amplitude limiting constraint is:

[0023] (6)

[0024] In the formula, These represent the minimum and maximum joint angles of the robotic arm, respectively. , , These represent the maximum absolute values ​​of joint angular velocity, joint acceleration, and the derivative of joint acceleration, respectively.

[0025] The terminal collision-free constraint is:

[0026] (7)

[0027] In the formula, It is a zero vector;

[0028] The base attitude constraint is as follows:

[0029] In formula (8), The minimum and maximum values ​​of the base attitude, respectively.

[0030] Further, in step S3, the expression for the trajectory optimization problem is:

[0031] (9)

[0032] In the formula: These are the initial values ​​for the terminal position, terminal attitude, base position, and base attitude, respectively. These are the initial values ​​for joint angle, joint angular velocity, and joint angular acceleration, respectively.

[0033] Furthermore, the conversion process in step S4 is as follows:

[0034] S41, the optimization variables are determined using a control parameterization method. The parameterized approximation is as follows :

[0035] (10)

[0036] In the formula: Optimize the variable for the i-th parameter. The number of segments is a positive integer. For piecewise constant functions:

[0037] (11)

[0038] S42, Definition If we optimize the variables for the parameters, then the optimization problem (9) can be transformed into a finite-dimensional nonlinear programming problem:

[0039] (12).

[0040] Further, in step S5, the feedforward neural network is constructed as follows: the output of the input layer... For the parameter variables that need optimization For the l-th hidden layer, Its output is For the Lth hidden layer, its output is The output of the output layer is ;in, The number of hidden layers in a neural network. , For the first The layer's output, weight matrix, and bias vector. ,symbol Represents the Hadamard product operation. The following are ReLU and sigmoid activation functions:

[0041] (13).

[0042] Further, in step S5, the feedforward neural network is trained, specifically as follows:

[0043] The constraints in the finite-dimensional nonlinear programming problem (11) are transformed into finite-dimensional integral constraints:

[0044] (14)

[0045] Design the loss function:

[0046] (15)

[0047] In the formula, W represents the total weight matrix, b represents the total deviation vector, and w represents the constraint penalty factor vector, with the following expressions:

[0048] (16)

[0049] This represents the output vector of the last layer of the neural network.

[0050] Furthermore, in step S5, the solution process for the finite-dimensional nonlinear programming problem is as follows:

[0051] S51, Initialize the feedforward neural network and randomly generate the initial parameters of the network. Compared with the initial optimal output learning rate Initialize the solver;

[0052] S52, order Repeat the following steps S53 to S55 in a loop;

[0053] S53, based on the current neural network parameters Obtain network output Calculate the loss function value Jump to S54;

[0054] S54, if Then let If the previous step is incorrect, proceed to S54; otherwise, proceed directly to S55.

[0055] S55, Backpropagation to calculate gradient Update using gradient descent .

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

[0057] (1) This invention transforms the infinite-dimensional trajectory optimization problem into a semi-infinite programming problem by controlling parameterization, innovatively constructs a lightweight feedforward neural network as a trajectory generator, and designs a differentiable loss function embedded with multiple types of constraints, using online gradient optimization for solution. This scheme can converge quickly without pre-training, is insensitive to initial values, and effectively solves the problems of slow convergence speed and easy getting trapped in local optima in traditional optimization methods when dealing with coupled constraints of free-floating systems. It significantly improves the efficiency and robustness of trajectory planning and is applicable to free-floating space robotic arm systems of arbitrary configurations.

[0058] (2) This invention uses joints as the core optimization variable to comprehensively construct a multi-dimensional constraint system that includes joint physical limits, terminal collision-free operation, and base posture. The constraint depth is embedded into the optimization process through the differentiable loss function of the neural network. Compared with the sampling planning method, which ignores dynamic characteristics and is difficult to guarantee high-order continuity, this method can generate continuous and smooth joint trajectories that strictly satisfy all physical and task constraints, effectively avoiding trajectory abrupt changes and collision risks, and ensuring the safety and accuracy of the robotic arm's movement.

[0059] (3) This invention does not rely on a large amount of interactive data for pre-training. It directly generates feasible trajectories through online gradient optimization and verifies and satisfies constraints in real time during the solution process, without the need for additional constraint verification steps. Compared with the limitations of reinforcement learning methods, such as high computational cost and difficulty in ensuring physical constraint satisfaction, this scheme significantly reduces the consumption of computational resources while strictly ensuring the physical feasibility of the trajectory and mission safety. It can be directly applied to aerospace mission scenarios with high reliability requirements. Attached Figure Description

[0060] Figure 1 This is a schematic diagram comparing iterative convergence under different optimization methods in the embodiments of the present invention.

[0061] Figure 2 This is a schematic diagram illustrating the change in terminal position in an embodiment of the present invention.

[0062] Figure 3 This is a schematic diagram of terminal posture changes in an embodiment of the present invention.

[0063] Figure 4 This is a schematic diagram of the joint angle trajectory in an embodiment of the present invention.

[0064] Figure 5 This is a schematic diagram of the base attitude angle change in an embodiment of the present invention. Detailed Implementation

[0065] The present invention will be further described below with reference to the accompanying drawings and embodiments. The embodiments of the present invention include, but are not limited to, the following embodiments.

[0066] This invention discloses a method for joint trajectory planning of a space robotic arm. The free-floating space robotic arm system consists of a base and an n-degree-of-freedom robotic arm. Its kinematic model can be described as follows:

[0067] (1)

[0068] in, For a point in time, Let ω be the angular velocity vector of the robotic arm's end effector. This represents the angular velocity vector of the robotic arm joints. Represents the velocity vector of the robotic arm's end effector. , These are the position and angular velocity vectors of the base, respectively. This is the joint angle vector of the robotic arm. , The Jacobian matrices for the spacecraft base motion and the robotic arm motion are as follows:

[0069] (2)

[0070] It is the identity matrix. It is a zero matrix. Let be the unit vector of rotation direction of the i-th robotic arm joint. The vectors representing the terminal position and the positions of the i robotic arm joints. The relative position, Indicates the position of the robotic arm's end effector. represent and The cross product.

[0071] In free-floating mode, the state of the base will be combined with the motion of the joints and will change with the joint angles. Assuming the initial linear momentum and angular momentum are zero, the kinematic equations for the base position and angular velocity are:

[0072] (3)

[0073] in, The base joint coupling matrix is ​​related to the mass, inertia, joint center of mass position, and rotation vector of the base and joints; substituting equation (3) into equation (1), the kinematic model of the free-floating space manipulator system is updated as follows:

[0074] (4)

[0075] in, , For matrix A row-block matrix.

[0076] Assume the end effector of the robotic arm needs to be in time Reach the target location and target posture The objective function for optimizing the joint trajectory of a space robotic arm is defined as:

[0077] (5)

[0078] In the formula: Let Euler angles be the attitude vectors of the robotic arm's end effector. Indicates the yaw angle. Indicates pitch angle, This represents the roll angle; it is related to the angular velocity. The relationship is:

[0079]

[0080]

[0081] The space robotic arm has physical constraints, considering the following constraints on joint angles, velocities, accelerations, and jerk (the derivative of acceleration):

[0082] (6)

[0083] In the formula, The minimum and maximum joint angles of the robotic arm are respectively. , , These represent the maximum absolute values ​​of joint angular velocity, acceleration, and jerk, respectively.

[0084] To ensure that the robotic arm's end effector reaches a designated pose without collision forces, the following constraints are applied to its velocity and acceleration at any given moment:

[0085] (7)

[0086] In the formula, It is a zero vector. Furthermore, since the state of the base is affected by the motion of the robotic arm while in free-floating state, in order to maintain the base's orientation... Without significant changes, maintaining basic attitude maneuvering tasks, consider the following base attitude constraints:

[0087] (8)

[0088] In the formula, These represent the minimum and maximum values ​​of the base attitude, respectively. Similarly, the relationship between the base attitude and its angular velocity is:

[0089]

[0090]

[0091] Considering the need for trajectory smoothing, the Jerk value of the joint angle is chosen as the optimization variable for the joint trajectory. Considering the kinematic equations of the space robot and the various imposed constraints, the following optimal joint trajectory control problem is established:

[0092] (9)

[0093] In the formula: These are the initial values ​​for the terminal position, terminal attitude, base position, and base attitude, respectively. These are the initial values ​​for joint angle, joint angular velocity, and joint angular acceleration, respectively.

[0094] The optimal control problem for joint trajectory planning of a space robotic arm (9) is an infinite-dimensional optimization problem. In order to transform it into a solvable finite-dimensional nonlinear programming problem, the control parameterization method is first used to optimize the variables. The parameterized approximation is as follows :

[0095] (10)

[0096] In the formula: Optimize the variable for the i-th parameter. The number of segments is a positive integer. For piecewise constant functions:

[0097] (11)

[0098] definition If we optimize the variables for the parameters, then the optimization problem (9) can be transformed into the following semi-infinite programming problem:

[0099] (12);

[0100] For the semi-infinite programming problem (12), conventional methods struggle to efficiently handle constraints and are sensitive to initial values. We then propose a neural network-based optimization method. A feedforward neural network is constructed to optimize the variables... As network input, network output is defined as The neural network is constructed as follows: Input layer output For the parameter variables that need optimization For the l-th hidden layer, Its output is For the Lth hidden layer, its output is The output of the output layer is ;in, The number of hidden layers in a neural network. , For the first The layer's output, weight matrix, and bias vector. ,symbol Represents the Hadamard product operation. The following are ReLU and sigmoid activation functions:

[0101] (13)

[0102] Based on the above network design, during the neural network training process, the loss function is related to the objective function and constraint values, and backpropagation optimization is performed. In the process, feasible values ​​that are close to the optimal value will gradually emerge. of To handle the infinite-dimensional constraints in the semi-infinite programming problem (12), we first transform them into finite-dimensional integral constraints:

[0103] (14)

[0104] The loss function is designed as follows:

[0105] (15)

[0106] In the formula, the total weight matrix Total deviation vector and constraint penalty factor vector They are respectively:

[0107] (16)

[0108] And satisfy The proposed neural network solution algorithm has the following flowchart, where... The maximum number of training steps, For the learning rate, the back gradient solver can employ mature optimizers such as Adam. The specific process is as follows:

[0109] S51, Initialize the feedforward neural network and randomly generate the initial parameters of the network. Compared with the initial optimal output learning rate Initialize the solver;

[0110] S52, order Repeat the following steps S53 to S55 in a loop;

[0111] S53, based on the current neural network parameters Obtain network output Calculate the loss function value Jump to S54;

[0112] S54, if Then let If the previous step is incorrect, proceed to S54; otherwise, proceed directly to S55.

[0113] S55, Backpropagation to calculate gradient Update using gradient descent .

[0114] In the simulation case, consider a spatial robotic arm with a six-degree-of-freedom manipulator as follows:

[0115]

[0116] The physical constraint parameters for the joint and base orientations are as follows:

[0117]

[0118] The initial states of the base and the robotic arm, and the desired end effector position of the robotic arm are set as follows:

[0119]

[0120] The parameters of the proposed neural network-based joint trajectory planning algorithm are: There are 4 neurons in each layer.

[0121] Depend on Figure 1 As can be seen, the proposed neural network-based optimization algorithm has better global optimality than the classic heuristic optimization algorithm, particle swarm optimization, and the active set algorithm of the Matlab software-Fmincon nonlinear programming solver. Figure 2 and Figure 3 The position and attitude vector changes of the robotic arm's end effector precisely achieved the desired pose. Figure 4 As shown in the joint angle trajectory variation, the joint angle trajectory is smooth, satisfying the set joint angle physical constraints, and the curve at the terminal moment approaches the x-axis, meaning that the velocity and acceleration are 0, satisfying the applied terminal collision-free force constraint. Figure 5 As shown, the change in the base attitude angle satisfies the set constraint range, realizing that in the free-floating mode, while the robotic arm terminal reaches the specified pose, the affected base attitude can also be controlled within a certain range.

[0122] The above embodiments are merely one of the preferred embodiments of the present invention and should not be used to limit the scope of protection of the present invention. Any modifications or refinements made to the main design concept and spirit of the present invention that are not of substantial significance, but solve the same technical problem as the present invention, should be included within the scope of protection of the present invention.

Claims

1. A method for planning the joint trajectory of a space robotic arm, characterized in that, Includes the following steps: S1. Establish the kinematic model of the free-floating space manipulator system consisting of the base and the n-degree-of-freedom manipulator, and its expression is: （1） in, For a point in time, Let ω be the angular velocity vector of the robotic arm's end effector. This represents the angular velocity vector of the robotic arm joints. Represents the velocity vector of the robotic arm's end effector. , These are the position and angular velocity vectors of the base, respectively. This is the joint angle vector of the robotic arm. , The Jacobian matrices for the spacecraft base motion and the robotic arm motion are as follows: （2） It is the identity matrix. It is a zero matrix. Let be the unit vector of rotation direction of the i-th robotic arm joint. The vectors representing the terminal position and the positions of the i robotic arm joints. The relative position, Indicates the position of the robotic arm's end effector. represent and cross product; S2, based on the changes in the joint angle of the robotic arm, assuming the initial linear momentum and angular momentum are zero, establish the kinematic equations for the base position and angular velocity: （3） in, The base joint coupling matrix is ​​related to the mass, inertia, joint center of mass position, and rotation vector of the base and joints; substituting equation (3) into equation (1), the kinematic model of the free-floating space manipulator system is updated as follows: （4） in, , For matrix The row block matrix; S3, construct a trajectory optimization problem with joints as optimization variables, establish an objective function, and apply joint physical amplitude constraints, end-effector collision-free constraints, and base attitude constraints. The objective function is: （5） In the formula, For the end effector of the robotic arm, time is required The target location reached For the end effector of the robotic arm, time is required The target posture reached, Let be the Euler angle vector of the robotic arm's end effector; where, Indicates the yaw angle. Indicates pitch angle, Indicates the roll angle; S4, based on the control parameter method, transforms the trajectory optimization problem into a solvable finite-dimensional nonlinear programming problem; S5 constructs an L-layer fully connected feedforward neural network to solve the finite-dimensional nonlinear programming problem and outputs the joint trajectory that satisfies all constraints.

2. The method for planning the joint trajectory of a space robotic arm according to claim 1, characterized in that, In step S3, the joint physical amplitude limiting constraint is: （6） In the formula, These represent the minimum and maximum joint angles of the robotic arm, respectively. , , These represent the maximum absolute values ​​of joint angular velocity, joint acceleration, and the derivative of joint acceleration, respectively. The terminal collision-free constraint is: （7） In the formula, It is a zero vector; The base attitude constraint is as follows: （8） In the formula, The minimum and maximum values ​​of the base attitude, respectively.

3. The method for planning the joint trajectory of a space robotic arm according to claim 2, characterized in that, In step S3, the expression for the trajectory optimization problem is: （9） In the formula: These are the initial values ​​for the terminal position, terminal attitude, base position, and base attitude, respectively. These are the initial values ​​for joint angle, joint angular velocity, and joint angular acceleration, respectively.

4. The method for planning the joint trajectory of a space robotic arm according to claim 3, characterized in that, The conversion process in step S4 is as follows: S41, the optimization variables are determined using a control parameterization method. The parameterized approximation is as follows : （10） In the formula: Optimize the variable for the i-th parameter. The number of segments is a positive integer. For piecewise constant functions: （11） S42, Definition If we optimize the variables for the parameters, then the optimization problem (9) can be transformed into a finite-dimensional nonlinear programming problem: （12）。 5. The method for planning the joint trajectory of a space robotic arm according to claim 4, characterized in that, In step S5, the feedforward neural network is constructed as follows: the output of the input layer... For the parameter variables that need optimization ; For the l-th hidden layer Its output is ; For the Lth hidden layer, its output is The output of the output layer is ;in, The number of hidden layers in a neural network. , For the first The layer's output, weight matrix, and bias vector. ,symbol Represents the Hadamard product operation. The following are ReLU and sigmoid activation functions: （13）。 6. The method for planning the joint trajectory of a space robotic arm according to claim 5, characterized in that, In step S5, the feedforward neural network is trained, and the specific process is as follows: The constraints in the finite-dimensional nonlinear programming problem (11) are transformed into finite-dimensional integral constraints: （14） Design the loss function: （15） In the formula, W represents the total weight matrix, b represents the total deviation vector, and w represents the constraint penalty factor vector, with the following expressions: （16） This represents the output vector of the last layer of the neural network.

7. The method for planning the joint trajectory of a space robotic arm according to claim 6, characterized in that, In step S5, the solution process for the finite-dimensional nonlinear programming problem is as follows: S51, Initialize the feedforward neural network and randomly generate the initial parameters of the network. Compared with the initial optimal output learning rate Initialize the solver; S52, order Repeat the following steps S53 to S55 in a loop; S53, based on the current neural network parameters Get network output Calculate the loss function value Jump to S54; S54, if Then let If the previous step is incorrect, proceed to S54; otherwise, proceed directly to S55. S55, Backpropagation to calculate gradient Update using gradient descent .

Images