A redundancy space manipulator trajectory planning method based on fault-tolerant configuration group

Abstract

The application provides a redundancy space manipulator trajectory planning method based on a fault-tolerant configuration group, comprising the following steps: constructing a redundancy space manipulator degenerate operability and joint limit index; constructing a mapping relationship between the joint speed and the end speed of the redundancy space manipulator, obtaining the speed relationship between joint 1 and other joints, and obtaining the speed interval of joint 1 when all joint speeds are not over limited according to the joint safety speed interval; solving the configuration group corresponding to the end trajectory of the redundancy space manipulator, screening the configurations in the configuration group by using the degenerate operability and joint limit index, and constructing a fault-tolerant configuration group; adding speed constraint information according to the fault-tolerant configuration group and the joint 1 speed interval, constructing a fault-tolerant configuration group containing speed constraint information, planning the joint 1 trajectory on this basis, solving the trajectories of other joints according to the joint 1 trajectory, and completing the trajectory planning.

Description

Technical Field

[0001] This invention relates to a redundancy-based spatial robotic arm trajectory planning method, belonging to the technical field of fault-tolerant trajectory planning for spatial robotic arms. Background Technology

[0002] Many on-orbit missions on the space station rely on space robotic arms with strong operational capabilities, high mobility, and strong environmental adaptability. This places high demands on the reliability and safety of these robotic arms, thus requiring them to possess a certain degree of fault tolerance. In practical aerospace applications, many space robotic arms are designed as redundant to increase their fault tolerance through redundant degrees of freedom; for example, SSRMS, ERA, and the robotic arm of the Chinese space station are all 7-DOF robotic arms. Therefore, improving the trajectory planning methods for redundant space robotic arms to achieve superior motion capabilities is of great significance for further enhancing the on-orbit reliability of space robotic arms.

[0003] Trajectory planning for redundant spatial manipulators relies on their inverse kinematics, and solving the inverse kinematics requires knowledge of the self-motion variables. Therefore, the core of redundant spatial manipulator trajectory planning is planning the trajectory of the self-motion variables. Currently, there are existing trajectory planning methods for redundant spatial manipulators based on fault-tolerant configuration groups. These methods first plan the trajectory of the self-motion variables within the fault-tolerant configuration group, and then solve the trajectories of other joints based on the inverse kinematics. However, existing fault-tolerant configuration groups consist only of points representing the manipulator configuration and do not include velocity constraint information. This can easily lead to joint velocities exceeding limits in redundant spatial manipulators, which is detrimental to their safe operation. Therefore, it is urgent to consider velocity constraint information when constructing fault-tolerant configuration groups to improve the operational safety of spatial manipulators. Summary of the Invention

[0004] In view of this, the present invention provides a redundancy spatial manipulator trajectory planning method based on fault-tolerant configuration groups. The method selects configuration groups corresponding to the end trajectory based on the degradation operability and joint limit index, constructs a fault-tolerant configuration group, solves the velocity range of joint 1 when the velocity of all joints does not exceed the limit, and incorporates it into the fault-tolerant configuration group. Finally, the trajectory is planned based on the fault-tolerant configuration group containing velocity constraint information.

[0005] This invention provides a trajectory planning method for a redundant spatial robotic arm based on a fault-tolerant configuration group, comprising:

[0006] Step S1: Construct the redundancy space robotic arm's degenerate operability and joint limit indicators;

[0007] Step S2: Construct the velocity relationship between joint 1 and other joints, and obtain the velocity range of joint 1 when the velocity of all joints does not exceed the limit, based on the manually set safe velocity range of joints;

[0008] Step S3: Solve for the configuration group corresponding to all end trajectory points of the space manipulator. Based on the redundancy space manipulator degradation operability and joint limit index, filter the configurations in the configuration group and construct the redundancy space manipulator fault-tolerant configuration group.

[0009] Step S4: Based on the redundancy space manipulator fault-tolerant configuration group and the speed range of joint 1 when all joint speeds do not exceed the limit, add speed constraint information to construct a redundancy space manipulator fault-tolerant configuration group containing speed constraint information.

[0010] Step S5: Based on the redundancy spatial manipulator fault-tolerant configuration group containing velocity constraint information, plan the trajectory of joint 1, solve the trajectories of other joints based on the trajectory of joint 1, and complete the trajectory planning.

[0011] Step S1 includes:

[0012] Setting the i-th column of the redundancy space manipulator Jacobian matrix J to zero yields the degenerate redundancy space manipulator Jacobian matrix. i J, according to the degenerate Jacobian matrix i J. Construction redundancy, spatial robotic arm degradation, operability, and joint limit indicators:

[0013]

[0014] In the formula, J is the Jacobian matrix of the redundancy space robot arm. i J is the redundancy space degenerate Jacobian matrix of the robotic arm. i J T Represents the degenerate Jacobian matrix i The transpose of J, det( i J i J T ) represents a matrix i J i J T The determinant, i w1 represents the redundancy space manipulator's degenerate operability, w2 is the joint limit index, and θ i θ is the current joint angle of the i-th joint. imax θ imin These are the upper and lower limits of the joint angle of the i-th joint, respectively, and n is the dimension of the redundancy space of the robotic arm joint space.

[0015] Step S2 includes:

[0016] Establish a mapping relationship between the joint velocities and the end effector velocity of the redundant space robotic arm:

[0017]

[0018] In the formula, v e The end effector velocity of the redundant space robotic arm. For the velocities of all joints of the redundant space robotic arm, It is the velocity of the i-th joint;

[0019] Based on the mapping relationship between the joint velocities and the end effector velocities of the redundant space robotic arm, the velocity relationship between joint 1 and other joints is obtained:

[0020]

[0021] In the formula, For the speed of other joints, J is the velocity of joint 1. col_1 The first column of the Jacobian matrix J for the redundancy space manipulator is... The redundancy space Jacobian matrix J is the matrix formed by removing the first column, where m is the dimension of the operation space.

[0022] Based on the artificially set safe speed range of the joint Each Substituting the velocity relationships between joint 1 and other joints, we obtain the velocity ranges of joint 1 for n-1 groups. in, These represent the minimum and maximum values ​​of the manually set safe speed range for the joint, respectively. These represent the minimum and maximum velocities of joint 1 when the velocity of joint i does not exceed the limit;

[0023] Intersect the velocity ranges of joint 1 in groups n-1 to obtain the velocity range of joint 1 when all joint velocities do not exceed the limit. in, These represent the minimum and maximum values ​​of the velocity of joint 1 when all joint velocities are within their limits.

[0024] Step S3 includes:

[0025] Solve for the configuration group corresponding to all end-effector trajectory points of the space robot:

[0026] W = {s|f(s) = x} ej j = 1, 2, ..., n step}

[0027] In the formula, W represents the configuration group corresponding to all end-point trajectory points, s represents a single robot arm configuration, f represents the redundancy space robot arm forward kinematics mapping relationship, and x ej Represents a single terminal trajectory point. It is the number of steps in the task execution, t fdt is the total execution time of the task, and dt is the time step of the control system.

[0028] Based on the degenerate operability and joint limit index of the redundant spatial manipulator, configurations in the configuration group are selected to construct a fault-tolerant configuration group for the redundant spatial manipulator:

[0029] W i ={s|f(s)=x ej , i w1(s)>[ i w1],w2(s)<[w2],s∈W,

[0030] i∈1,2,…,n,j=1,2,…,n step}

[0031] In the formula, W i It is a group of fault-tolerant configurations for redundant space robotic arms. i [w1] and [w2] are the degradation operability threshold and the joint limit index threshold, respectively.

[0032] Step S4 includes:

[0033] Based on the speed range of joint 1 when all joint speeds do not exceed the limit. The tangential range of the joint trajectory curve is [β1, β2], where β1 and β2 are expressed as follows:

[0034]

[0035] For all configuration points in the redundancy spatial manipulator fault-tolerant configuration group, solve the tangential range [β1, β2] of the joint trajectory curve, add two arrows to all configuration points, and make the angles between the two arrows and the horizontal direction β1 and β2 respectively, to construct a redundancy spatial manipulator fault-tolerant configuration group containing velocity constraint information.

[0036] Step S5 includes:

[0037] In the redundancy-tolerant configuration group of the robotic arm containing velocity constraint information, starting from the initial configuration point, a continuous curve is planned from left to right, and the curve shape is adjusted so that the tangent of the curve is always between the two arrows to obtain the trajectory of joint 1, and the trajectories of other joints are solved.

[0038] As can be seen from the above technical solutions, the present invention has the following beneficial effects:

[0039] In the technical solution of this invention, the configuration group corresponding to all end-effector trajectory points is solved based on the inverse kinematics of the redundant spatial manipulator, enabling the spatial manipulator to track the end-effector trajectory. Degenerate operability and joint limit indices are constructed, and configurations that meet the index thresholds are selected from the configuration group to form a fault-tolerant configuration group, which enables the spatial manipulator to have fault tolerance and ensures that the joint angles do not exceed limits. The velocity range of joint 1 when all joint velocities do not exceed limits is solved, and this range is added as velocity constraint information to the fault-tolerant configuration group, forming a fault-tolerant configuration group for the redundant spatial manipulator containing velocity constraint information, ensuring that the velocities of all joints of the spatial manipulator do not exceed limits. These beneficial effects improve the safety and reliability of the spatial manipulator's operation, better meeting the needs of space missions. The proposed trajectory planning method for a redundant spatial manipulator based on fault-tolerant configuration groups can provide a basis for the design of system trajectory planning methods. Attached Figure Description

[0040] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort or labor.

[0041] Figure 1 This is a schematic diagram of the trajectory planning method for a redundant spatial robotic arm based on a fault-tolerant configuration group provided in an embodiment of the present invention.

[0042] Figure 2 This is a schematic diagram of a seven-degree-of-freedom spatial robotic arm model provided in an embodiment of the present invention;

[0043] Figure 3 It is a group of fault-tolerant configurations for redundant space robotic arms;

[0044] Figure 4 It is a group of redundancy-space manipulator fault-tolerant configurations that include velocity constraint information;

[0045] Figure 5 These are the trajectories of joint 1 with two unadjusted curve shapes;

[0046] Figure 6 These are the joint trajectories after adjusting the shape of the two curves;

[0047] Figure 7 It is a comparison chart of the speeds of each joint corresponding to the two trajectories before and after the curve shape adjustment. Detailed Implementation

[0048] To better understand the technical solution of the present invention, the present invention will be described in detail below with reference to the accompanying drawings.

[0049] It should be understood that the described embodiments are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0050] This invention provides a trajectory planning method for a redundant spatial robotic arm based on a fault-tolerant configuration group. Please refer to [link / reference]. Figure 1 This is a schematic diagram of the trajectory planning method for a redundant spatial robotic arm based on a fault-tolerant configuration group provided in an embodiment of the present invention. The method includes the following steps:

[0051] Step S1: Construct the redundancy space robotic arm's degenerate operability and joint limit indicators.

[0052] Specifically, setting the i-th column of the redundancy space manipulator Jacobian matrix J to zero yields the redundancy space manipulator degenerate Jacobian matrix. i J, according to the degenerate Jacobian matrix i J. Construction redundancy, spatial robotic arm degradation, operability, and joint limit indicators:

[0053]

[0054] In the formula, J is the Jacobian matrix of the redundancy space robot arm. i J is the redundancy space degenerate Jacobian matrix of the robotic arm. i J T Represents the degenerate Jacobian matrix i The transpose of J, det( i J i J T ) represents a matrix i J i J T The determinant, i w1 represents the redundancy space manipulator's degenerate operability, w2 is the joint limit index, and θ i θ is the current joint angle of the i-th joint. imax θ imin These are the upper and lower limits of the joint angle of the i-th joint, respectively, and n is the dimension of the redundancy space of the robotic arm joint space.

[0055] Step S2: Construct the velocity relationship between joint 1 and other joints. Based on the manually set safe velocity range of the joints, obtain the velocity range of joint 1 when the velocity of all joints does not exceed the limit.

[0056] Specifically, establish the mapping relationship between the joint velocities and the end effector velocities of the redundant space robotic arm:

[0057]

[0058] In the formula, v e The end effector velocity of the redundant space robotic arm. For the velocities of all joints of the redundant space robotic arm, It is the velocity of the i-th joint;

[0059] Based on the mapping relationship between the joint velocities and the end effector velocities of the redundant space robotic arm, the velocity relationship between joint 1 and other joints is obtained:

[0060]

[0061] In the formula, For the speed of other joints, J is the velocity of joint 1. col_1 The first column of the Jacobian matrix J for the redundancy space manipulator is... The redundancy space Jacobian matrix J is the matrix formed by removing the first column, where m is the dimension of the operation space.

[0062] Based on the artificially set safe speed range of the joint Each Substituting the velocity relationships between joint 1 and other joints, we obtain the velocity ranges of joint 1 for n-1 groups. in, These represent the minimum and maximum values ​​of the manually set safe speed range for the joint, respectively. These represent the minimum and maximum velocities of joint 1 when the velocity of joint i does not exceed the limit;

[0063] Intersect the velocity ranges of joint 1 in groups n-1 to obtain the velocity range of joint 1 when all joint velocities do not exceed the limit. in, These represent the minimum and maximum values ​​of the velocity of joint 1 when all joint velocities are within limits. Step S2 converts the original multiple joint velocity constraint information into joint 1 velocity constraint information, which facilitates the addition of velocity constraint information to the fault-tolerant configuration group in subsequent steps.

[0064] Step S3: Solve for the configuration group corresponding to all end trajectory points of the space manipulator. Based on the redundancy space manipulator degradation operability and joint limit index, filter the configurations in the configuration group and construct the redundancy space manipulator fault-tolerant configuration group.

[0065] Specifically, based on the redundancy spatial manipulator's forward kinematics, the configuration group corresponding to all end-effector trajectory points of the spatial manipulator is solved:

[0066] W = {s|f(s) = x} ej j = 1, 2, ..., n step} (4)

[0067] In the formula, W represents the configuration group corresponding to all end-point trajectory points, s represents a single robot arm configuration, f represents the redundancy space robot arm forward kinematics mapping relationship, and x ej Represents a single terminal trajectory point. It is the number of steps in the task execution, t f dt is the total execution time of the task, and dt is the time step of the control system.

[0068] Based on the degenerate operability and joint limit index of the redundant spatial manipulator, configurations in the configuration group are selected to construct a fault-tolerant configuration group for the redundant spatial manipulator:

[0069]

[0070] In the formula, W i It is a group of fault-tolerant configurations for redundant space robotic arms. i [w1] and [w2] are the degradation operability threshold and the joint limit index threshold, respectively.

[0071] Step S4: Based on the redundancy spatial manipulator fault-tolerant configuration group and the speed range of joint 1 when all joint speeds do not exceed the limit, add speed constraint information to construct a redundancy spatial manipulator fault-tolerant configuration group containing speed constraint information.

[0072] Specifically, since the velocity of joint 1 in the fault-tolerant configuration group is represented by the slope of the trajectory curve of joint 1, the velocity of joint 1 can be limited by restricting the tangent of the trajectory curve of joint 1, thereby limiting the velocities of other joints, ultimately ensuring that all joint velocities satisfy the constraints. This is based on the velocity range of joint 1 when all joint velocities do not exceed the limits. The tangential range of the joint trajectory curve is [β1, β2], where β1 and β2 are expressed as follows:

[0073]

[0074] For all configuration points in the redundant spatial manipulator fault-tolerant configuration group, the tangential range [β1, β2] of the joint trajectory curve is solved. Two arrows are added to all configuration points, with the angles between the two arrows and the horizontal direction being β1 and β2, respectively, thus constructing a redundant spatial manipulator fault-tolerant configuration group that includes velocity constraint information. The two added arrows limit the velocity range of joint 1, i.e., they include velocity constraint information. This ensures that the original fault-tolerant configuration group considers not only the manipulator configuration but also the joint velocity constraints, thereby ensuring that the joint velocity does not exceed the limit.

[0075] Step S5: Based on the redundancy spatial manipulator fault-tolerant configuration group containing velocity constraint information, plan the trajectory of joint 1, solve the trajectories of other joints based on the trajectory of joint 1, and complete the trajectory planning.

[0076] Specifically, in the redundancy-tolerant configuration group of the robotic arm containing velocity constraint information, starting from the initial configuration point, a continuous curve is planned from left to right, and the shape of the curve is adjusted so that the tangent of the curve is always between the two arrows, thus obtaining the trajectory of joint 1.

[0077] By solving the trajectories of other joints based on the inverse kinematics of the redundant space manipulator and the trajectory of joint 1, the trajectories of other joints can be obtained, thus completing the trajectory planning of the redundant space manipulator. This ensures that the redundant space manipulator has fault tolerance during task execution and that the speed of all joints does not exceed the limit.

[0078] Based on the method provided in the embodiments of the present invention, a simulation experiment was conducted on the trajectory planning method of a redundant spatial robotic arm based on a fault-tolerant configuration group.

[0079] Please refer to Figure 2 This is a schematic diagram of a seven-degree-of-freedom spatial robotic arm model provided in an embodiment of the present invention, and its kinematic and dynamic parameters are shown in Table 1.

[0080] Table 1. Kinematic and dynamic parameters of a 7-DOF spatial robotic arm

[0081]

[0082] Assume the initial configuration of the space robotic arm is [30,40,30,-40,-40,-40,-30]°, the target position is [5,-3,0]m, the target attitude is [0,0,0]rad, the total mission time is 20s, the time step is 0.05s, the end effector trajectory is a straight line, and the degradation operability threshold is […]. 2 w1, 4 [w1] = [0.15, 0.1], the joint limit index threshold is [w2] = 14.7, and the safe speed range for each joint is set. Based on step S3, a group of fault-tolerant configurations for redundant spatial robotic arms can be constructed. Please refer to [link / reference]. Figure 3 The blank areas in the graph represent regions with no solution or that do not meet the threshold. From Figure 3 It can be seen that a continuous curve can be planned from left to right in the fault-tolerant configuration group, with a larger area on the left and a smaller area on the right. Based on steps S2 and S4, a fault-tolerant configuration group for a redundant spatial robotic arm containing velocity constraint information can be constructed. Please refer to [reference needed]. Figure 4 To demonstrate the universality of this invention, this embodiment takes two joint trajectories as examples, respectively passing through... Figure 4The upper left and lower left parts then converge on the right side. Please refer to [the image / reference]. Figure 5 This consists of the trajectories of two joints with unadjusted curve shapes. From Figure 5 It can be seen that the tangents of both curves are not located between the two arrows at the nearest configuration point in some sections. Adjust the curve shape according to step S5; please refer to [reference needed]. Figure 6 This is the trajectory of joint 1 after adjusting the shape of two curves. From... Figure 6 As can be seen, after adjusting the curve shape, the tangents of all parts of both curves are now between the two arrows of the nearest configuration point. By solving for the trajectories of the other joints according to step S5, the trajectory planning can be completed. Please refer to... Figure 7 This is a comparison chart of the speeds of each joint corresponding to the two trajectories before and after the curve shape adjustment. From Figure 7 As can be seen from (a) and (b), before the curve shape was adjusted, the velocities of each joint in both trajectories exceeded the limits. Figure 7 As can be seen from (c) and (d), after the curve shape is adjusted, the speeds of each joint corresponding to the two trajectories do not exceed the limits, indicating that using the above-mentioned method provided by the embodiment of the present invention for trajectory planning can enable the space robotic arm to have fault tolerance during the execution of tasks and ensure that all joint angles and speeds do not exceed the limits.

[0083] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0084] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A trajectory planning method for a redundant spatial robotic arm based on a fault-tolerant configuration group, characterized in that, The method includes: Step S1: Construct the redundancy space robotic arm's degenerate operability and joint limit indicators; Step S2: Construct the velocity relationship between joint 1 and other joints. Based on the manually set safe velocity range for joints, obtain the velocity range of joint 1 when the velocities of all joints do not exceed the limits. Its characteristics include: The Jacobian matrix of the redundancy space robotic arm Setting the i-th column to zero yields the redundancy space robotic arm degradation Jacobian matrix. According to the degenerate Jacobian matrix Constructing a redundant spatial robotic arm with degenerate operability and joint limit indicators: In the formula, The Jacobian matrix for the redundancy space of the robotic arm. The degenerate Jacobian matrix for the redundancy space robotic arm. Represents the degenerate Jacobian matrix The transpose of the matrix, Representation matrix The determinant, To reduce the maneuverability of the redundant space robotic arm, It is the joint limit index. It is the current joint angle of the i-th joint. , These are the upper and lower limits of the joint angle of the i-th joint, respectively, and n is the dimension of the redundancy space of the robotic arm joint space. Step S3: Solve for the configuration group corresponding to all end trajectory points of the space manipulator. Based on the redundancy space manipulator degradation operability and joint limit index, filter the configurations in the configuration group and construct the redundancy space manipulator fault-tolerant configuration group. Step S4: Based on the redundant spatial manipulator fault-tolerant configuration group and the speed range of joint 1 when all joint speeds do not exceed the limit, add speed constraint information to construct a redundant spatial manipulator fault-tolerant configuration group containing speed constraint information, characterized by: Based on the speed range of joint 1 when all joint speeds do not exceed the limit. The tangential range of the joint trajectory curve is obtained as follows: ,in , The expression is: Solve the tangential range of the joint trajectory curves for all configuration points in the redundancy-space manipulator fault-tolerant configuration group. Add two arrows to all configuration points, such that the angles between the two arrows and the horizontal direction are respectively... , A group of redundancy-space manipulator fault-tolerant configurations containing velocity constraint information is constructed. Step S5: Based on the redundancy spatial manipulator fault-tolerant configuration group containing velocity constraint information, start from the initial configuration point and plan a continuous curve from left to right, and adjust the curve shape so that the tangent of the curve is always between the two arrows to obtain the trajectory of joint 1. Based on the trajectory of joint 1, solve the trajectories of other joints to complete the trajectory planning.

2. The method according to claim 1, characterized in that, Step S2 includes: Establish a mapping relationship between the joint velocities and the end effector velocities of the redundant space robotic arm: In the formula, The end effector velocity of the redundant space robotic arm. For the velocities of all joints of the redundant space robotic arm, It is the velocity of the i-th joint; Based on the mapping relationship between the joint velocities and the end effector velocities of the redundant space robotic arm, the velocity relationship between joint 1 and other joints is obtained: In the formula, For the speed of other joints, The velocity of joint 1, Jacobian matrix for redundancy space robotic arm Column 1 Jacobian matrix for redundancy space robotic arm The matrix formed after removing the first column, where m is the dimension of the operation space; Based on the artificially set safe speed range of the joint , respectively , Substituting these values ​​into the velocity relationships between joint 1 and other joints, we obtain the velocity ranges of joint 1 for n-1 groups. ,in, , These represent the minimum and maximum values ​​of the manually set safe speed range for the joint, respectively. , These represent the minimum and maximum velocities of joint 1 when the velocity of joint i does not exceed the limit; Intersect the velocity ranges of joint 1 in groups n-1 to obtain the velocity range of joint 1 when all joint velocities do not exceed the limit. ,in, , These represent the minimum and maximum values ​​of the velocity of joint 1 when all joint velocities are within their limits.

3. The method according to claim 1, characterized in that, Step S3 includes: Solve for the configuration group corresponding to all end-effector trajectory points of the space robot: In the formula, W represents the configuration group corresponding to all end-point trajectory points, s represents a single robot arm configuration, and f represents the redundancy space robot arm forward kinematics mapping relationship. Represents a single terminal trajectory point. It is the number of steps in the task execution. It is the total execution time of the task. It is the time step of the control system; Based on the degenerate operability and joint limit index of the redundant spatial manipulator, configurations in the configuration group are selected to construct a fault-tolerant configuration group for the redundant spatial manipulator: In the formula, It is a group of fault-tolerant configurations for redundant space robotic arms. and These are the degradation operability threshold and the joint limit index threshold, respectively.

Images