Improved dynamic primitive method for space manipulator operations

Abstract

The application relates to an improved dynamic primitive method for space manipulator operation and belongs to the technical field of space manipulators. Firstly, aiming at the space environment, primitive trajectory learning is carried out under a cylindrical coordinate system, and obstacle avoidance of the manipulator end and a spacecraft main body carrying the manipulator is realized by introducing an obstacle avoidance term; the cylindrical coordinate system trajectory calculation is converted to a Cartesian coordinate system, and a Cartesian space trajectory is obtained through inverse kinematics calculation based on a Jacobian; aiming at possible non-smoothness and end error problems in the Cartesian space, a quadratic programming method is used, an accurate end configuration is taken as a constraint, trajectory smoothness and similarity to the original trajectory are taken as indexes, and the trajectory is optimized. The quadratic programming is used for trajectory optimization, solving is fast, and real-time optimization of the trajectory can be realized. Finally, the defects of the dynamic primitive method are overcome, and the application of the dynamic primitive method in space manipulator operation is realized.

Description

Technical Field

[0001] This invention belongs to the field of space robotic arm technology, and relates to a method for generating trajectory of a space robotic arm, particularly a method for generating trajectory based on imitation learning of improved dynamic primitives. Background Technology

[0002] Motion planning for space robotic arms requires trajectories with safety, stability, determinism, and high precision. Traditional sampling and optimization-based motion planning methods lack determinism, meaning that planning results may differ under the same conditions, making them difficult to apply to space robotic arm operations. The literature "Ijspeert AJ, Nakanishi J, Hoffmann H, et al. Dynamic movement primitives: Learning attractor models for motorbehaviors. Neural Computation, 2013, 25(2): 328-373" proposes a Dynamic Movement Primitives (DMP) method based on imitation learning. This method incorporates nonlinear terms into a second-order spring-damped dynamic system to achieve trajectory learning, offering advantages such as fast planning and strong generalization ability. However, the trajectories planned by the method described in the literature suffer from end-effector errors, and the trajectory accuracy cannot meet the requirements of space robotic arm operations. Furthermore, the generalized trajectory mapping to the robotic arm joint space may exhibit unevenness, making it unsuitable for direct application in space missions. Summary of the Invention

[0003] The technical problem to be solved by this invention is:

[0004] To achieve rapid trajectory planning for space robotic arms, considering the safety, stability, and high precision requirements of the robotic arm's operational trajectory, and taking into account the characteristics of the space robotic arm's operating environment, this invention provides an improved dynamic primitive method for space robotic arm operations.

[0005] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0006] An improved dynamic primitive method for space robotic arm operations, characterized by comprising:

[0007] The dynamic primitive method formula is improved in cylindrical coordinate system by introducing an obstacle avoidance term in the ρ coordinate direction to achieve obstacle avoidance between the end effector of the robotic arm and the main body of the spacecraft carrying the robotic arm.

[0008] The cylindrical coordinate system trajectory obtained based on the improved dynamic primitive method is transformed into the Cartesian coordinate system, and the Cartesian space trajectory is obtained by inverse kinematics calculation based on Jacobi.

[0009] Secondary planning is performed on the Cartesian space trajectory: the configuration corresponding to the precise pose of the end point is used as the end point constraint, and the similarity index and smoothness index are used as optimization indexes to optimize the Cartesian space trajectory.

[0010] A further technical solution of the present invention: The improved dynamic primitive method formula is as follows:

[0011]

[0012] Where y and z represent the position and the introduced system velocity-related quantity, respectively, g represents the target position, τ is the time constant, and α z and β z It is a positive constant and β z =α z / 4, where f is a nonlinear forcing term. For obstacle avoidance, ρ represents the distance to the spacecraft's central axis, k is a constant coefficient, and the phase variable s allows the obstacle avoidance term to decay as the motion progresses, thereby ensuring the system's target convergence characteristics.

[0013] A further technical solution of the present invention: the transformation of the cylindrical coordinate system trajectory obtained based on the improved dynamic primitive method to the Cartesian coordinate system, and the Cartesian space trajectory calculated based on Jacobi inverse kinematics, specifically involves:

[0014] In the field of robotics, there is a definition for the Jacobian matrix, which has the following properties:

[0015]

[0016] Where, x e Represents the terminal pose, The values ​​represent the end effector velocity and angular velocity, and θ represents the joint configuration of the robotic arm. J represents the angular velocity of the robotic arm joints, and J is the Jacobian matrix;

[0017] Assuming the current pose of the robotic arm is x0, the current configuration is θ0, and the next target pose is x1, and the motion length of each step is a small quantity, i.e., Δx = x1 - x0 is a small quantity; then

[0018] Δθ=pinv(J)Δx

[0019] Using the above formula, the change in the robot's joint angle Δθ when reaching the target pose is calculated through multiple iterations, thus obtaining the target configuration corresponding to the target pose x1:

[0020] θ1=θ0+Δθ

[0021] Obtain the joint space trajectory of the robotic arm Q = [θ0, θ1, ..., θ n ], where n is the step size corresponding to the trajectory.

[0022] A further technical solution of the present invention: the squared difference between the optimized trajectory and the reference trajectory is used as a similarity index, in the following form:

[0023]

[0024] Where, x = [θ 11 ,θ 21 ,...,θ 71 ,...,θ 1n ,θ 2n ,...,θ 7n ] T To optimize variables, Let A1 be the original joint space reference trajectory, and let A1 be a 7n×7n identity matrix. The coefficients of the linear terms are constant vectors, such as h = -2x. r c is only related to the reference trajectory x r Related constant terms.

[0025] A further technical solution of the present invention: the smoothness index is:

[0026]

[0027]

[0028] Among them, 0 6×1 This represents a vector consisting of 6 rows and 1 column of 0 elements.

[0029] A further technical solution of the present invention: the end constraint is:

[0030]

[0031] Where, θ init and θ end These are the initial and final configurations, 0 (n-14)×(n-14) This represents a matrix consisting of (n-14) rows and (n-14) columns of zero elements. (n-14)×1 This represents a vector consisting of (n-14) rows and 1 column of zero elements.

[0032] A computer system is characterized by comprising: one or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement the method described above.

[0033] A computer-readable storage medium is characterized by storing computer-executable instructions, which, when executed, are used to implement the above-described method.

[0034] The beneficial effects of this invention are as follows:

[0035] This invention provides an improved dynamic primitive method for space robotic arm operations. By adding an obstacle avoidance term to the dynamic primitive method, obstacle avoidance between the robotic arm end effector and the spacecraft body is achieved. Through quadratic programming, the end effector error defect of the current dynamic primitive method is overcome.

[0036] Given a teaching trajectory, the proposed method can learn quickly and generalize to similar scenarios. This method can be used to easily achieve obstacle avoidance between the robotic arm's end effector and the spacecraft body, while also eliminating the unsmoothness of the joint space trajectory and the end effector error of the generalized trajectory. Attached Figure Description

[0037] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0038] Figure 1 This is a flowchart of the overall calculation process of the method of the present invention.

[0039] Figure 2 This is a diagram showing the transformation relationship between the Cartesian coordinate system and the cylindrical coordinate system.

[0040] Figure 3 This is a diagram illustrating the effect of the obstacle avoidance function in the method of this invention.

[0041] Figure 4 These are comparison images of joint spatial trajectories before and after trajectory optimization using quadratic programming in the method of this invention: (a) Joint spatial trajectory before optimization; (b) Joint spatial trajectory after optimization.

[0042] Figure 5 The following diagrams illustrate the effect of the method of the present invention on eliminating end-effector errors of dynamic primitives: (a) optimizing the position error of the front end-effector; (b) optimizing the position error of the rear end-effector; (c) optimizing the attitude error of the front end-effector; and (d) optimizing the attitude error of the rear end-effector. Detailed Implementation

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0044] This invention provides an improved dynamic primitive method for space robotic arm operations. First, for the space environment, primitive trajectory learning is performed in a cylindrical coordinate system, and obstacle avoidance is achieved between the robotic arm's end effector and the spacecraft carrying the robotic arm by introducing an obstacle avoidance term. The trajectory calculation in the cylindrical coordinate system is transformed to a Cartesian coordinate system, and the Cartesian space trajectory is obtained through Jacobi-based inverse kinematics calculation. To address potential non-smoothness and end effector errors in Cartesian space, a quadratic programming method is used, with the precise configuration of the end effector as a constraint and trajectory smoothness and similarity to the original trajectory as indicators, to optimize the trajectory. Using quadratic programming for trajectory optimization provides a fast solution and enables real-time trajectory optimization. Ultimately, this overcomes the shortcomings of the dynamic primitive method and enables its application in space robotic arm operations.

[0045] like Figure 1 As shown, it includes the following steps:

[0046] Step 1: Formula Improvement of the Dynamic Primitive Method

[0047] The traditional position-level DMP (Dynamic Movement Primitives) method is based on the following formula:

[0048]

[0049] Where y and z represent the position and the introduced system velocity-related quantity, respectively, and g represents the target position. τ is the time constant, and α... z and β z It is a positive constant and β z =α z / 4, where the f term is called the nonlinear forcing term. By designing the f term, the system evolution process can be modulated. Due to the interference effect of the f term, the traditional DMP method cannot strictly converge to the target state. The f term is as follows:

[0050]

[0051] Where s is the phase variable, satisfying ψ i (·) represents the i-th basis function, ω i This represents the weight of the i-th basis function.

[0052]

[0053] in, And there is a relationship The conjugate of a quaternion is defined as:

[0054]

[0055] The quaternion-based attitude DMP formula is as follows:

[0056]

[0057] The constant parameters in the formula are the same as those in the position DMP method. η in the formula is a quaternion with a scalar part of 0, i.e. It is a quantity related to the target angular velocity. g o f represents the target pose in quaternion form, and q represents the current pose. o (x) has the following form:

[0058]

[0059] In the formula Given a teaching trajectory, attitude trajectory learning is achieved by calculating the weight coefficient vector in the LWR formula.

[0060] In space scenarios, spacecraft carry out space missions using robotic arms. For example... Figure 2 As shown, a cylindrical coordinate system can be established along the spacecraft's main axis. Compared to the Cartesian coordinate system, the coordinate parameters (x, ρ, φ) of the cylindrical system have a more intuitive physical meaning. x represents the axial position coordinate, ρ represents the distance to the spacecraft's central axis, and φ represents the angle with a given reference axis. Learning the end effector trajectory of a robotic arm in a cylindrical system can achieve a more natural generalization effect. The cylindrical and Cartesian systems have a simple transformation relationship. Given Cartesian space coordinates (x, y, z), its corresponding coordinates (x, ρ, φ) in the cylindrical system are:

[0061]

[0062] Similarly, given the position (x, ρ, φ) in a cylindrical system, the position in a Cartesian system can be calculated:

[0063]

[0064] Where ρ represents the distance from the robotic arm's end effector to the spacecraft's central axis, adding an obstacle avoidance term F to the original DMP formula in the ρ coordinate direction enables the robotic arm's end effector to avoid collisions with the spacecraft's main body. Referring to the form of universal gravitation in nature, the obstacle avoidance term F is designed as follows:

[0065]

[0066] Where k is a constant coefficient, and the phase variable s allows the obstacle avoidance term to decay during motion, thus ensuring the system's target convergence characteristics. Under the action of the obstacle avoidance term, the robotic arm's end effector tends to move away from the spacecraft body, thus ensuring that the end effector does not collide with the spacecraft body. The improved ρ-coordinate direction DMP formula is as follows:

[0067]

[0068] The DMP formulas for the remaining coordinate directions and attitudes remain unchanged.

[0069] Step 2: Obtain the joint space trajectory based on Jacobi iteration

[0070] Based on the improved DMP method, after obtaining the trajectory in the cylindrical system, its Cartesian space trajectory can be obtained. In the field of robotics, there is a definition for the Jacobian matrix, which has the following properties:

[0071]

[0072] Where, x e Represents the terminal pose, This represents the end effector velocity and angular velocity. θ represents the joint configuration of the robotic arm. This represents the angular velocity of the robotic arm joints. A connection is established between the Cartesian space and the joint space through the Jacobian matrix J.

[0073] To meet the requirements of complex space missions, space robotic arms are typically 7-DOF redundant robotic arms. Assume the current pose of the robotic arm is x0, the current configuration is θ0, and the next target pose is x1. The motion in each step is a small quantity, i.e., Δx = x1 - x0 is a small quantity. Then...

[0074] Δθ=pinv(J)Δx

[0075] Using the above formula, the change in the robot's joint angle Δθ when reaching the target pose is calculated through multiple iterations, thus obtaining the target configuration corresponding to the target pose x1:

[0076] θ1=θ0+Δθ

[0077] Following the above process, the spatial trajectory Q of the robotic arm joints can be obtained, which can be represented as a set of joint configurations, i.e., Q = [θ0, θ1, ..., θ]. n ], where n is the step size corresponding to the trajectory, and θ i =[θ i1 ,θ i2 ,...,θ i7 ].

[0078] A smooth trajectory in Cartesian space may not necessarily have a smooth trajectory in its corresponding joint space.

[0079] Step 3: Secondary planning of joint space trajectory

[0080] To address the issues of trajectory end-point error and the unsmoothness of joint space trajectories, a quadratic programming method is used to optimize the planning results. The optimization variable is x = [θ]. 11 ,θ 21 ,...,θ 71,...,θ 1n ,θ 2n ,...,θ 7n ] T The original joint space reference trajectory is denoted as The optimization metrics include similarity to the original trajectory and trajectory smoothness. Simultaneously, the configuration corresponding to the precise pose of the end effector is used as an end effector constraint to eliminate end effector errors.

[0081] The squared difference between the optimized trajectory and the reference trajectory is used as the similarity index, in the following form:

[0082]

[0083] Rearranging it into a quadratic form, it is as follows:

[0084]

[0085] Where A1 is a 7n×7n identity matrix, and the coefficients of the linear terms are constant vectors h = -2x r c is only related to the reference trajectory x r The relevant constant terms do not affect the solution of the performance index, and therefore can be discarded in the optimization problem. This index ensures the similarity between the optimization result and the reference trajectory, so that the optimized trajectory retains the characteristics of the original trajectory.

[0086] The smoothness of a trajectory essentially depends on the stability of its motion speed. If the trajectory speed changes continuously and the change is small per unit time, then the trajectory at the corresponding position will naturally be smooth. The values ​​of three adjacent trajectory points at a joint of the robotic arm are denoted as y. i y i+1 and y i+2 Velocity is represented by the positional difference per unit step length, and the velocity difference between two adjacent step lengths can be denoted as y. i+2 -2y i+1 +y i By optimizing the velocity difference between any three adjacent points along the entire trajectory, a smooth and stable trajectory can be obtained. For a 7-DOF (DoF) spatial robotic arm, optimizing the velocity difference of the 7 joint trajectories using the above principle can yield a smooth joint spatial trajectory. Therefore, the smoothness index is designed as follows:

[0087]

[0088] In the formula, matrix A2 has the following form:

[0089]

[0090] To ensure the accuracy of the initial and final poses in the optimization results, the exact solutions of the initial and final configurations are used as constraints in the optimization problem. Let the initial and final configurations be θ. init and θend Design the following equality constraints:

[0091]

[0092] The coefficient matrix on the left side of the above equation is denoted as A3, and the matrix on the right side is denoted as b. The equality constraint is simplified to A3x = b.

[0093] The above analysis yields the following optimization problem:

[0094]

[0095] subject to A3x=b

[0096] Here, α1 and α2 are two positive constants, representing the weighting coefficients of the similarity and smoothness performance indicators.

[0097] Adopting such Figure 2 The space station scenario shown is used as a simulation scenario, and motion planning is performed using the method of this invention. The robotic arm is fixed to the spacecraft, and the base posture is... The robotic arm starts from the initial configuration θ0 = [1.5708, 0.7854, -2.3562, 0.5236, 1.5708, -0.7854, -1.5708] and moves to the target pose. The relevant parameters in the formulas involved in the algorithm are set as follows: τ=7.5, α z =25, β z =6.25, α s =2, N=15, k=1000, α1=1, α2=1000.

[0098] like Figure 3 As shown, the trajectory after adding the repulsive term can effectively move away from the origin, which in a spatial scenario manifests as the robotic arm's end effector tending to move away from the module's axis during movement. For example... Figure 4 As shown, the trajectory optimized by the quadratic programming method is highly similar in shape to the original trajectory, retaining the characteristics of the taught trajectory imitated by the DMP method. Simultaneously, the non-smooth positions in the original trajectory are eliminated, and the transitions in their vicinity are natural. Figure 5 As shown, the trajectory obtained by the original DMP method has significant end-position errors, with an error of approximately 3 cm in this example. The optimized trajectory has an end-position error of less than 10 cm. -8 The error is on the order of m, representing the computational error of the robot's kinematics. This demonstrates that our method eliminates the end-effector error of the DMP method. In this example, the end-effector posture error of the original trajectory is relatively small, but the optimized posture change process is noticeably smoother and more natural. Furthermore, based on an improved DMP formula and quadratic programming, our method can obtain a uniquely determined trajectory for a given scenario, thus meeting the deterministic requirements of spatial robotic arm motion.

[0099] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the scope of the technology disclosed in the present invention, and such modifications or substitutions should all be covered within the scope of protection of the present invention.

Claims

1. An improved dynamic primitive method for space robotic arm operations, characterized in that, include: The formula for the dynamic element method is improved in cylindrical coordinates. An obstacle avoidance term is introduced in the coordinate direction to achieve obstacle avoidance between the robotic arm's end effector and the spacecraft body carrying the robotic arm; the improved dynamic primitive method formula is: in, y and z These represent the position quantity and the introduced system velocity-related quantity, respectively. g Indicates the target location. It is a time constant. and It is a positive constant and , It is a nonlinear forcing term. For obstacle avoidance, Indicates the distance to the spacecraft's central axis. Constant coefficients, phase variables This allows the obstacle avoidance term to decay during the motion process, thereby ensuring the target convergence characteristics of the system; The cylindrical coordinate system trajectory obtained based on the improved dynamic primitive method is transformed into the Cartesian coordinate system, and the Cartesian space trajectory is obtained by inverse kinematics calculation based on Jacobi. Secondary planning is performed on the Cartesian space trajectory: the configuration corresponding to the precise pose of the end point is used as the end point constraint, and the similarity index and smoothness index are used as optimization indexes to optimize the Cartesian space trajectory.

2. The improved dynamic primitive method for space robotic arm operations according to claim 1, characterized in that, The process of transforming the cylindrical coordinate system trajectory obtained based on the improved dynamic primitive method to the Cartesian coordinate system, and then calculating the Cartesian space trajectory based on the inverse kinematics of Jacobi, specifically involves: In the field of robotics, there is a definition for the Jacobian matrix, which has the following properties: in, Represents the terminal pose, Indicates terminal velocity and angular velocity. This indicates the joint configuration of the robotic arm. Indicates the angular velocity of the robotic arm joints. It is a Jacobian matrix; Assuming the current pose of the robotic arm is The current configuration is The next target pose is Each step of motion is a small quantity, that is... It is a small quantity; then Using the above formula, the changes in the robot's joint angles when reaching the target pose can be calculated through multiple iterations. This leads to the corresponding target pose. Target configuration: Obtain the spatial trajectory of the robotic arm joints , This represents the step size corresponding to the trajectory.

3. The improved dynamic primitive method for space robotic arm operations according to claim 1, characterized in that, The squared difference between the optimized trajectory and the reference trajectory is used as the similarity index, in the following form: in, To optimize variables, Record the original joint space reference trajectory. yes The identity matrix, where the coefficients of the linear terms are constant vectors. , It is only with reference trajectory Related constant terms.

4. The improved dynamic primitive method for space robotic arm operations according to claim 1, characterized in that, The smoothness index is: in, This represents a vector consisting of 6 rows and 1 column of 0 elements.

5. The improved dynamic primitive method for space robotic arm operations according to claim 1, characterized in that, The end constraint is: in, and These are the initial and final configurations, express OK A matrix consisting of zero elements in each column express A vector consisting of 0 elements in row 1 and column 1.

6. A computer system, characterized in that... include: One or more processors, a computer-readable storage medium for storing one or more programs, wherein, when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to perform the method of any one of claims 1-5.

7. A computer-readable storage medium, characterized in that... The device stores computer-executable instructions, which, when executed, are used to implement the method described in any one of claims 1-5.

Images