Autonomous trajectory planning method for rope-driven agile space manipulator to quickly capture moving target

Abstract

The invention relates to an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target, which comprises the following steps: carrying out equivalence on a rope-driven joint according to the motion coupling characteristic of the rope-driven agile space manipulator; according to the current tail end pose difference of the target and the mechanical arm, the target speed and the time-varying tail end speed and tail end acceleration limitation as constraints, the expected speed direction of the tail end mechanical arm is calculated; constructing a speed mapping matrix, calculating a motor speed, a motor maximum speed and a mechanical arm tail end maximum speed constraint, and judging and assigning the mechanical arm tail end speed according to the tail end maximum speed constraint; and the maximum acceleration constraint of the tail end of the mechanical arm is calculated according to the relation between the joint motor torque and the joint torque of the rope-driven agile space mechanical arm. By means of the method, it is ensured that the maximum tail end speed and acceleration which can be achieved when the mechanical arm moves within the motor movement capacity range are planned and controlled, and the movement performance of the flexible drive agile space mechanical arm is fully exerted.

Description

Technical Field

[0001] The invention relates to the field of rope-driven flexible robots, and in particular to an autonomous trajectory planning method for a rope-driven agile space robot arm to quickly capture a moving target. Background Art

[0002] The traditional rigid robotic arm has a simple structure, with motors, reducers and other structures arranged at the joints of the robotic arm to achieve joint movement. The weight of the robotic arm itself is evenly distributed on the arm rod of the robotic arm. Compared with the flexible-driven agile space robotic arm, the flexible-driven agile space robotic arm does not have motors at the joints. Instead, the motors, reducers and other structures are arranged at the base end of the robotic arm through rope transmission, which greatly reduces the mass and moment of inertia of the end arm rod, and enables the robotic arm to obtain greater speed and acceleration performance.

[0003] The configuration, motor speed limit and maximum motor torque of the flexible-driven agile space manipulator will affect the maximum acceleration of the manipulator end. The constant maximum acceleration limit used in dealing with time optimization problems often leads to the phenomenon that the joint angular velocity and motor speed cannot match in the process of tracking the end trajectory. Therefore, in order to give full play to the motion performance of the flexible-driven agile space manipulator, it is necessary to use a time-varying maximum end velocity / acceleration constraint to achieve the maximum speed of approaching the target for rapid capture. Summary of the invention

[0004] The present invention provides an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target, aiming to solve at least one of the technical problems existing in the prior art.

[0005] The technical solution of the present invention is an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target, which is applied to a rope-driven agile space manipulator. The autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target comprises the following steps:

[0006] S100, kinematic modeling of the rope-driven agile space manipulator is performed through the DH coordinate system, the current joint angle, joint angular velocity, target velocity, and target posture of the rope-driven agile space manipulator are obtained and initialized, and the rope-driven joints are equivalent according to the motion coupling characteristics of the rope-driven agile space manipulator;

[0007] S200, based on the difference between the target and the current end position of the robotic arm , Target speed and the time-varying terminal velocity and the end acceleration limit as constraints to calculate the desired velocity direction of the end manipulator ;

[0008] S300, construct velocity mapping matrix , calculate the motor speed , Maximum motor speed and the maximum velocity constraint of the end of the robot , according to the terminal maximum speed constraint , for the end speed of the robot arm Make judgment assignment;

[0009] S400, joint motor torque of rope-driven agile space manipulator and joint torque , calculate the maximum acceleration constraint at the end of the robot arm .

[0010] Furthermore, in step S200, the expected speed direction of the planned end manipulator is Updated with the target's position and velocity, and constrained by the time-varying terminal velocity and acceleration limits, the desired velocity direction is the sum of the pose vector of the end of the robotic arm pointing to the target and the target motion velocity vector, that is,

[0011] ,

[0012] in, is the pose vector of the end effector of the robot arm, is the pose vector of the target handle, is the six-dimensional motion speed of the target handle, Used to adjust the direction and magnitude of the desired terminal speed. is the pose difference between the target and the current end position of the robot.

[0013] Further, the step S300 includes:

[0014] S310, according to the mapping relationship between the motors and joint angles of the elbow and wrist, the mapping relationship between the motor speed of the rope-driven joint and the joint speed is obtained by time differentiation, and the speed mapping matrix is ​​constructed. ;

[0015] S320, according to the expected speed direction , calculate the corresponding motor speed ;

[0016] S330, calculate the desired speed direction according to the motor speed limit Maximum motor speed and the maximum velocity constraint of the robot end .

[0017] Furthermore, in step S310, the speed mapping matrix for:

[0018] ,

[0019] ,

[0020] in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and The number of cable turns set on the elbow pulley and wrist pulley respectively, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

[0021] Furthermore, in step S320, the motor speed for:

[0022] ,

[0023] in, is the velocity mapping matrix, is the manipulator Jacobian matrix The pseudo-inverse matrix of

[0024] Time-varying terminal velocity constraint Based on the multi-space kinematic modeling of flexible-driven agile space manipulator, including the kinematics of motor and joint space and the kinematics of joint and end space, the motor speed is limited in the derivation process. As a constraint, the end velocity of the robot arm can be Convert to joint velocity ; Then the joint speed is converted to Converted to motor speed , get the terminal speed constraint that meets the motor speed limit requirements ; The mapping relationship between the motor and the joint space needs to be considered separately for different joints. The shoulder, elbow and wrist joints have different structures and different mapping relationships. For the forward and inverse kinematic relationship between the joint and the end space, the special motion characteristics of the rope-driven joint and its equivalence in kinematic representation need to be considered.

[0025] Furthermore, in step S330, the maximum speed of the motor for:

[0026] ,

[0027] Maximum speed of the end of the robot for:

[0028] ,

[0029] ,

[0030] in, is the maximum joint velocity, is the motor speed, is the Jacobian matrix of the robot arm, DI is the velocity mapping matrix, is with and the desired speed direction Related variables, is the degree of freedom of the robot arm.

[0031] Furthermore, in step S400, the time-varying terminal maximum jerk constraint Based on the dynamic model of flexible-driven agile space manipulator, the dynamic analysis considering the mass inertia of the arm is carried out to obtain the joint torque generated by the movement of the manipulator arm. ; The joint torque can be analyzed by rope tension analysis of rope-driven joints. Converted into motor torque , get the terminal maximum acceleration constraint that meets the motor rated speed limit ;

[0032] Among them, the rope-driven joint motor torque and joint torque The relationship is:

[0033] ,

[0034] ,

[0035] in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and are the number of cable turns on the elbow pulley and wrist pulley, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

[0036] Further, in step S400, the rope tension analysis of the flexible drive joint is based on the force and torque balance equation, and the force and torque balance equation is established respectively in combination with the different structures of the elbow and wrist joints to obtain the mapping relationship between the joint torque and the motor torque related to the joint angle; the dynamic modeling of the mass inertia of the robot arm adopts the iterative Newton-Euler method to obtain the relationship between the joint torque and the joint angle, the joint angular velocity and the joint angular acceleration;

[0037] By further deriving the differential kinematics of the robot arm joint and the terminal space, the expressions of terminal acceleration and joint angle, joint angular velocity and joint angular acceleration are obtained; the maximum joint angular acceleration under the rated torque of the motor obtained by the dynamic model is converted into the maximum acceleration of the terminal through the Hessian matrix;

[0038] In the direction of the desired speed, The maximum joint acceleration corresponding to the motor torque limit , obtained by the inverse dynamics of the robot arm, satisfies the following constraints:

[0039] ,

[0040] in, is the maximum torque of the motor, SI is the mapping matrix between joint torque and motor torque, inverseDynamics is the inverse dynamics function of the robot arm, is the joint angle, is the joint angular velocity, is the maximum acceleration of the joint angle, is the Jacobian matrix of the robot arm, is the proportional coefficient, accflag indicates whether the corresponding velocity component is in the six-dimensional state quantity of acceleration or deceleration;

[0041] By differentiating the velocity Jacobian matrix, we can obtain the maximum acceleration at the end. for;

[0042] ,

[0043] ,

[0044] in, is the time derivative of the Jacobian matrix, Is a 6 The matrix of is the number of degrees of freedom of the robot arm, is the Hessian matrix, Is a 6 Array of An array is a collection of matrices. Contains 6 planes, of which the i-th plane is denoted as , is a The matrix of .

[0045] Further, the present invention also proposes a rope-driven agile space manipulator, which is used to execute the autonomous trajectory planning method of the rope-driven agile space manipulator to quickly capture a moving target, wherein the rope-driven agile space manipulator comprises a shoulder joint, a drive box, a first arm, a first elbow joint, a second arm, a second elbow joint, a third arm, a wrist joint and an end effector connected in sequence, and a rope winding device is arranged in the drive box;

[0046] Among them, the shoulder joint of the robot arm is a rigid joint composed of a motor and a reducer, the elbow joint and the wrist joint are rope-driven joints, and the driving motors of the elbow joint and the wrist joint are both arranged in the rope winding device.

[0047] Furthermore, the present invention also proposes a computer-readable storage medium having program instructions stored thereon, and when the program instructions are executed by a processor, the autonomous trajectory planning method for the rope-driven agile space manipulator to quickly capture a moving target is implemented.

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

[0049] The autonomous trajectory planning method for the rope-driven agile space manipulator to quickly capture a moving target is to ensure that the flexible-driven agile space manipulator obtains the maximum terminal velocity and acceleration that can be achieved by the manipulator within the motor motion capability range when planning and controlling the target tracking motion, so as to give full play to the motion performance of the flexible-driven agile space manipulator. This solution can be applied to the same flexible-driven agile space manipulator products and the independently developed manipulator experimental platform to complete the task of quickly capturing a tumbling target. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a flow chart of an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target.

[0051] Figure 2It is a schematic diagram of the DH coordinate system of the manipulator when the rope-driven agile space manipulator quickly captures a moving target.

[0052] Figure 3 It is a schematic diagram of a space robot capturing a tumbling target in an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target.

[0053] Figure 4 It is a flow chart of the algorithm for rapid capture of a flexible-driven agile space manipulator in the autonomous trajectory planning method for rapid capture of a moving target by a rope-driven agile space manipulator.

[0054] Figure 5 is a schematic diagram of an embodiment of a rope-driven agile space manipulator.

[0055] Figure numerals, 100, shoulder joint; 200, drive box; 300, first arm; 400, first elbow joint; 500, second arm; 600, second elbow joint; 700, third arm; 800, wrist joint; 900, end effector. DETAILED DESCRIPTION

[0056] The following will be combined with the embodiments and drawings to clearly and completely describe the concept, specific structure and technical effects of the present invention, so as to fully understand the purpose, scheme and effect of the present invention. It should be noted that the embodiments and features in the embodiments of this application can be combined with each other without conflict.

[0057] It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly fixed or connected to the other feature, or it may be indirectly fixed or connected to the other feature. In addition, the descriptions of up, down, left, right, top, bottom, etc. used in the present invention are only relative to the relative positional relationship of the components of the present invention in the drawings.

[0058] In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as those generally understood by those skilled in the art. The terms used in this specification are only for describing specific embodiments, not for limiting the present invention. The term "and / or" used herein includes any combination of one or more related listed items.

[0059] It should be understood that, although the terms first, second, third, etc. may be used to describe various elements in the present disclosure, these elements should not be limited to these terms. These terms are only used to distinguish elements of the same type from each other. For example, without departing from the scope of the present disclosure, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element.

[0060] Reference Figures 1 to 5 In some embodiments, the technical solution of the present invention is an autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target, which is applied to the rope-driven agile space manipulator, and the method comprises the following steps:

[0061] S100, kinematic modeling of the rope-driven agile space manipulator is performed through the DH coordinate system, the current joint angle, joint angular velocity, target velocity, and target posture of the rope-driven agile space manipulator are obtained and initialized, and the rope-driven joints are equivalent according to the motion coupling characteristics of the rope-driven agile space manipulator;

[0062] S200, based on the difference between the target and the current end position of the robotic arm , Target speed and the time-varying terminal velocity and the end acceleration limit as constraints to calculate the desired velocity direction of the end manipulator ;

[0063] S300, construct velocity mapping matrix , calculate the motor speed , Maximum motor speed and the maximum velocity constraint of the robot end , according to the terminal maximum speed constraint , for the end speed of the robot arm Make judgment assignment;

[0064] S400, joint motor torque of rope-driven agile space manipulator and joint torque , calculate the maximum acceleration constraint at the end of the robot arm .

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

[0066] The autonomous trajectory planning method for the rope-driven agile space manipulator to quickly capture a moving target is to ensure that the flexible-driven agile space manipulator obtains the maximum terminal velocity and acceleration that can be achieved by the manipulator within the motor motion capability range when planning and controlling the target tracking motion, so as to give full play to the motion performance of the flexible-driven agile space manipulator. This solution can be applied to the same flexible-driven agile space manipulator products and the independently developed manipulator experimental platform to complete the task of quickly capturing a tumbling target.

[0067] In the present invention, the autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target is to give full play to the motion performance of the flexible-driven agile space manipulator, with the time-varying manipulator end velocity and acceleration limits as constraints, and the end constraints are updated in real time according to the current manipulator configuration and the desired end motion direction. The derivation of the time-varying end velocity constraint needs to consider the differential kinematics of the motor and joint space, as well as the differential kinematics of the joint and end space. The derivation of the time-varying end acceleration constraint needs to consider the force analysis of the equivalent joint torque and rope tension, as well as the dynamic modeling of the manipulator arm motion and joint force. The time-varying constraints are limited by the motor speed limit and the motor rated torque, respectively.

[0068] Specifically, refer to Figure 2 , is the DH coordinate system of the robot arm, and Table 1 shows the DH parameters of the flexible drive agile space robot arm as follows:

[0069] Table 1,

[0070] In Table 1, according to the standard DH coordinate system construction method, refer to Figure 2 , the kinematic modeling of the robot arm is carried out, and the rope-driven joint is equivalent considering the kinematic coupling characteristics of the rope-driven joint. The xyz axes of the established joint coordinate system are as follows Figure 1 As shown. The shoulder is a traditional motor reducer joint with two degrees of freedom. The coordinate system is established with its rotation axis as the z axis. The direction of z0 is along shoulder joint 1, the direction of z1 is along shoulder joint 2, and the xy axis direction meets the standard DH coordinate system construction method. The two elbow joints have similar structures and motion principles, and are equivalent to two pitch joints with equal angle constraints. Their rotation axes are z2, z3, z4, and z5, respectively, and the joint rotation angles have equal angle constraints. , The wrist joint has three degrees of freedom, of which the pitch and yaw degrees of freedom are represented by two revolute joints of equal size and direction, and two revolute joints of equal size and opposite direction. The equivalent joint rotation axes are z6, z7, z8, and z9 respectively, and the equivalent joint rotation angles have constraints , The wrist rotational freedom is independent of it, the joint rotation axis is z10, and the establishment of the xy axis meets the standard DH coordinate system establishment method.

[0071] DH parameters are shown in Table 1, where From x i-1 Axis to x i Axis around z i-1 The angle of rotation of the axis; αi is the angle from z i-1 Axis to z i Axis around x iThe angle of rotation of the axis; ai is the angle from z i-1 Axis to z i Axis along x i The distance measured from the x axis; di is the distance from i-1 Axis to x i The axis is along z i-1 The distance measured by the axis.

[0072] Furthermore, in step S200, the expected speed direction of the planned end manipulator is Updated with the target's position and velocity, and constrained by the time-varying terminal velocity and acceleration limits, the desired velocity direction is the sum of the pose vector of the end of the robotic arm pointing to the target and the target motion velocity vector, that is,

[0073] ,

[0074] in, is the pose vector of the robot end effector 900, is the pose vector of the target handle, is the six-dimensional motion speed of the target handle, Used to adjust the direction and magnitude of the desired terminal speed. is the pose difference between the target and the current end position of the robot.

[0075] Specifically, the rolling target rotates around its center of mass coordinate system At angular velocity The linear velocity of its center of mass is . is an inertial coordinate system. Design the desired terminal velocity, the rolling target around its center of mass C t At angular velocity The linear velocity of its center of mass is To capture fast-moving targets, the robot arm is required to operate at its maximum motor speed. , Maximum motor torque The maximum speed of the robot end that approaches the target motion is achieved within the limit range. Since the inertia of the robot arm is very small, the output torque required by the motor to drive the robot arm is small. The maximum movement speed of the end of the robot arm in this paper is It mainly depends on the robot configuration, motor speed limit and the desired end motion direction, so define the desired end speed .

[0076] Further, the step S300 includes:

[0077] S310, according to the mapping relationship between the motors and joint angles of the elbow and wrist, the mapping relationship between the motor speed of the rope-driven joint and the joint speed is obtained by time differentiation, and the speed mapping matrix is ​​constructed. ;

[0078] S320, according to the expected speed direction , calculate the corresponding motor speed ;

[0079] S330, calculate the desired speed direction according to the motor speed limit Maximum motor speed and the maximum velocity constraint of the robot end .

[0080] Furthermore, in step S310, the speed mapping matrix for:

[0081] ,

[0082] ,

[0083] in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and The number of cable turns set on the elbow pulley and wrist pulley respectively, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

[0084] Furthermore, in step S320, the motor speed for:

[0085] ,

[0086] in, is the velocity mapping matrix, is the manipulator Jacobian matrix The pseudo-inverse matrix of

[0087] Time-varying terminal velocity constraint Based on the multi-space kinematic modeling of flexible-driven agile space manipulator, including the kinematics of motor and joint space and the kinematics of joint and end space, the motor speed is limited in the derivation process. As a constraint, the end velocity of the robot arm can be Convert to joint velocity ; Then the joint speed is converted to Converted to motor speed , get the terminal speed constraint that meets the motor speed limit requirements ; The mapping relationship between the motor and the joint space needs to be considered separately for different joints. The shoulder, elbow and wrist joints have different structures and different mapping relationships. For the forward and inverse kinematic relationship between the joint and the end space, the special motion characteristics of the rope-driven joint and its equivalence in kinematic representation need to be considered.

[0088] Furthermore, in step S330, the maximum speed of the motor for:

[0089] ,

[0090] Maximum speed of the end of the robot for:

[0091] ,

[0092] ,

[0093] in, is the maximum joint velocity, is the motor speed, is the Jacobian matrix of the robot arm, DI is the velocity mapping matrix, is with and the desired speed direction Related variables, is the degree of freedom of the robot arm.

[0094] Furthermore, in step S400, the time-varying terminal maximum jerk constraint Based on the dynamic model of flexible-driven agile space manipulator, the dynamic analysis considering the mass inertia of the arm is carried out to obtain the joint torque generated by the movement of the manipulator arm. ; The joint torque can be analyzed by rope tension analysis of rope-driven joints. Converted into motor torque , get the terminal maximum acceleration constraint that meets the motor rated speed limit ;

[0095] Among them, the rope-driven joint motor torque and joint torque The relationship is:

[0096] ,

[0097] ,

[0098] in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and are the number of cable turns on the elbow pulley and wrist pulley, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

[0099] Further, in step S400, the rope tension analysis of the flexible drive joint is based on the force and torque balance equation, and the force and torque balance equation is established respectively in combination with the different structures of the elbow and wrist joints to obtain the mapping relationship between the joint torque and the motor torque related to the joint angle; the dynamic modeling of the mass inertia of the robot arm adopts the iterative Newton-Euler method to obtain the relationship between the joint torque and the joint angle, the joint angular velocity and the joint angular acceleration;

[0100] By further deriving the differential kinematics of the robot arm joint and the terminal space, the expressions of terminal acceleration and joint angle, joint angular velocity and joint angular acceleration are obtained; the maximum joint angular acceleration under the rated torque of the motor obtained by the dynamic model is converted into the maximum acceleration of the terminal through the Hessian matrix;

[0101] In the direction of the desired speed, The maximum joint acceleration corresponding to the motor torque limit , obtained by the inverse dynamics of the robot arm, satisfies the following constraints:

[0102] ,

[0103] in, is the maximum torque of the motor, SI is the mapping matrix between joint torque and motor torque, inverseDynamics is the inverse dynamics function of the robot arm, is the joint angle, is the joint angular velocity, is the maximum acceleration of the joint angle, is the Jacobian matrix of the robot arm, is the proportional coefficient, accflag indicates whether the corresponding velocity component is in the six-dimensional state quantity of acceleration or deceleration;

[0104] By differentiating the velocity Jacobian matrix, we can obtain the maximum acceleration at the end. for;

[0105] ,

[0106] ,

[0107] in, is the time derivative of the Jacobian matrix, Is a 6 The matrix of is the number of degrees of freedom of the robot arm, is the Hessian matrix, Is a 6 Array of An array is a collection of matrices. Contains 6 planes, of which the i-th plane is denoted as , is a The matrix of .

[0108] In a specific embodiment, referring to Figure 4 The present invention also provides an autonomous trajectory planning method process for a rope-driven agile space manipulator to quickly capture a moving target. First, the current flexible-driven agile space manipulator and the collected target state are input, including the current joint angle, the current joint angular velocity, the target posture and the target speed; the algorithm parameters are initialized; the desired direction of the terminal speed is calculated; the maximum achievable speed limit in this direction is calculated based on the desired direction of the terminal speed with the motor speed limit as a constraint; the maximum achievable acceleration in this direction is calculated based on the motor torque limit as a constraint; and finally, the algorithm is terminated when the target and the manipulator end posture and speed error thresholds are met.

[0109] Specifically, refer to Figure 4 , including the following steps:

[0110] Step 1: Input the current joint angle, joint angular velocity, target velocity, and target posture.

[0111] Step 2: Initialize the algorithm parameters, lim is the maximum number of loops; ε p and ε v are the position and speed error judgment thresholds between the target and the end of the manipulator, respectively; dt is the time interval; is the current robot arm speed.

[0112] Step 3: According to the difference between the target and the current end position of the robot arm and the target speed Design the desired terminal speed .

[0113] Step 4: Calculate the maximum velocity constraint of the robot end The calculation process is as follows: The mapping relationship between the motor and joint angle of the elbow and wrist can be obtained by derivation with respect to time. The mapping relationship between the motor speed of the rope-driven joint and the joint speed can be used to construct the speed mapping matrix DI (θ). Then it can be calculated Corresponding motor speed ,in is the manipulator Jacobian matrix According to the motor speed limit, we can calculate Maximum motor speed in direction . Then we can get the maximum speed of the motor The corresponding maximum speed of the robot end , where the parameters ,in, represents the maximum joint velocity; is with and The relevant variable; n is the number of degrees of freedom of the robot.

[0114] Step 5: According to the maximum speed constraint of the terminal , for the end speed of the robot arm Make judgment assignment.

[0115] Step 6: Calculate the maximum acceleration constraint at the end of the robot The calculation process is as follows: The maximum joint acceleration corresponding to the motor torque limit in the direction , which can be obtained by the inverse dynamics of the robot arm , maximum joint acceleration , where accflag is a 6-dimensional state quantity indicating whether the corresponding velocity component is accelerating or decelerating. The maximum acceleration at the end can be obtained by differentiating the velocity Jacobian matrix , and calculate the current terminal acceleration a e ;

[0116] Step 7: According to the maximum acceleration constraint of the robot end , the terminal velocity v of the next cycle e (k+1) makes judgment correction.

[0117] Step 8: Map the terminal velocity to the joint velocity according to the pseudo-inverse of the Jacobian matrix, and further calculate the joint angle value of the next cycle.

[0118] Step 9: Determine whether the position error and speed error between the target and the end of the robot arm exceed the threshold. If they are less than the threshold, the algorithm ends, otherwise proceed to the next step.

[0119] Step 10: Determine whether the maximum number of cycle periods exceeds the threshold. If not, return to step 3 to perform the next cycle calculation.

[0120] Furthermore, the present invention also proposes a rope-driven agile space manipulator, which is used to execute the autonomous trajectory planning method of the rope-driven agile space manipulator for quickly capturing a moving target. The rope-driven agile space manipulator comprises a shoulder joint 100, a drive box 200, a first arm 300, a first elbow joint 400, a second arm 500, a second elbow joint 600, a third arm 700, a wrist joint 800 and an end effector 900 connected in sequence, and a rope winding device is arranged in the drive box 200;

[0121] Among them, the shoulder joint of the robot arm is a rigid joint composed of a motor and a reducer, the elbow joint and the wrist joint are rope-driven joints, and the driving motors of the elbow joint and the wrist joint are both arranged in the rope winding device.

[0122] Specifically, the design and modeling of the rope-driven agile space manipulator system includes shoulder joint design, elbow joint design and wrist joint design.

[0123] In a specific embodiment, referring to Figure 5The shoulder joint of the rope-driven agile space manipulator has two degrees of freedom, namely yaw and pitch. Both joints are driven by motor + reducer, using cross roller bearings as rotation support. The drive mechanism adopts a built-in design, which makes the overall size of the base compact and can be arranged in a narrow space to avoid interference with other parts on the device. The internal structure of the shoulder yaw joint and pitch joint is the same, consisting of a housing, a motor, a reducer, a speed encoder and a position encoder. The shoulder drive motor is a frameless servo motor. The outer ring stator is directly mounted on the housing through a flange, and the inner ring rotor is connected to the input end of the harmonic reducer. The outer ring of the speed resolver is also mounted on the housing, and the inner ring rotor is connected to the output end of the motor (or the input end of the harmonic reducer). The outer ring of the position resolver is mounted on the housing, and the inner ring rotor is connected to the output shaft of the harmonic reducer to directly measure the joint angle. In order to reduce weight, multiple weight-reducing holes are symmetrically opened on the joint housing. The drive box 200 is arranged on the third joint of the shoulder. The rotation of the motor is converted into the stretching and relaxation of the ropes on both sides through the wire storage wheel and the grooved bearing, which can ensure that the main weight of the robot arm is located at the shoulder joint 100 close to the base, reducing the overall mass inertia of the robot arm.

[0124] In the design of the elbow joint, decoupling is achieved by placing a pulley at the elbow. The ropes include the wrist drive rope and the elbow drive rope. The wrist drive rope passes through the elbow joint and is decoupled through the decoupling groove bearing. The elbow drive rope is placed on the upper and lower sides of the elbow. A wiring groove is embedded in the elbow frame to ensure the accuracy of the elbow joint movement.

[0125] In the wrist joint design, the traditional 7-DOF manipulator design usually connects the three wrist joints in series. Although this joint configuration is simple and easy to control, it also causes problems such as excessive volume and mass at the distal end, lack of workspace, and overly complex settings of the rope-driven decoupling module. As a common structure of the rope-driven manipulator, the parallel platform can well realize the pitch and yaw motion of the wrist joint 800. Figure 5 shows a double hemispherical rolling surface, whose motion mode is transformed from the elbow joint motion principle. The common constraint of the 3-(RR)(RR) structure is only rotation along the z-axis, so the structure can realize the pitch and yaw motion of the wrist joint 800. At the same time, according to the design idea of ​​the structure, a set of 4R structures can still be placed in the center of the structure. When the wrist joint performs pitch and yaw motion, the structure can move along the spherical center of the two sets of spherical mechanisms. The length between the spherical centers remains unchanged, and the end joint passes through the wrist spherical structure to realize the decoupling of the final rotary platform. The three degrees of freedom of the wrist are composed of two sets of parallel platforms and a set of revolving joints. This structure not only reduces the overall mass of the wrist, but also increases the overall flexibility of the robotic arm.

[0126] The rope-driven agile space manipulator consists of a shoulder joint 100 (including a yaw joint and a pitch joint), a first arm 300, a second arm 500, a third arm 700, a first elbow joint 400, a second elbow joint 600, a wrist joint 800, and an end effector 900. The shoulder joint of the manipulator is a motor-accelerator rigid joint, while the elbow joint and the wrist joint are rope-driven joints, and their drive motors are concentrated in the rope winding device at the shoulder joint 100. This design reduces the mass at the elbow-wrist joint 800, reduces inertia, and improves the agility and end load capacity of the manipulator. At the same time, the concentration of the drive motors is conducive to centralized thermal protection of them, which is beneficial to providing the overall reliability of the system.

[0127] Furthermore, the present invention also proposes a computer-readable storage medium having program instructions stored thereon, and when the program instructions are executed by a processor, the autonomous trajectory planning method for the rope-driven agile space manipulator to quickly capture a moving target is implemented.

[0128] The above is only a preferred embodiment of the present invention. The present invention is not limited to the above implementation. As long as the technical effect of the present invention is achieved by the same means, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure shall be included in the scope of protection of the present disclosure. All should belong to the protection scope of the present invention. Within the protection scope of the present invention, its technical scheme and / or implementation method can have various modifications and changes.

Claims

1. An autonomous trajectory planning method for a rope-driven agile space manipulator to quickly capture a moving target, applied to a rope-driven agile space manipulator, characterized in that: The autonomous trajectory planning method for the rope-driven agile space manipulator to quickly capture a moving target comprises the following steps: S100, kinematic modeling of the rope-driven agile space manipulator is performed through the DH coordinate system, the current joint angle, joint angular velocity, target velocity, and target posture of the rope-driven agile space manipulator are obtained and initialized, and the rope-driven joints are equivalent according to the motion coupling characteristics of the rope-driven agile space manipulator; S200, based on the difference between the target and the current end position of the robotic arm , Target speed and the time-varying terminal velocity and the end acceleration limit as constraints to calculate the desired velocity direction of the end manipulator ; S300, construct velocity mapping matrix , calculate the motor speed , Maximum motor speed and the maximum velocity constraint of the robot end , according to the terminal maximum speed constraint , for the end speed of the robot arm Make judgment assignment; S400, joint motor torque of rope-driven agile space manipulator and joint torque , calculate the maximum acceleration constraint at the end of the robot arm .

2. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 1 is characterized in that: In step S200, the planned end robot arm expected speed direction Updated with the target's position and velocity, and constrained by the time-varying terminal velocity and acceleration limits, the desired velocity direction is the sum of the pose vector of the end of the robotic arm pointing to the target and the target motion velocity vector, that is, ， in, is the pose vector of the end effector of the robot arm (900), is the pose vector of the target handle, is the six-dimensional motion speed of the target handle, Used to adjust the direction and magnitude of the desired terminal speed. is the pose difference between the target and the current end position of the robot.

3. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 1 is characterized in that: The step S300 includes: S310, according to the mapping relationship between the motors and joint angles of the elbow and wrist, the mapping relationship between the motor speed of the rope-driven joint and the joint speed is obtained by time differentiation, and the speed mapping matrix is ​​constructed. ; S320, according to the expected speed direction , calculate the corresponding motor speed ; S330, calculate the desired speed direction according to the motor speed limit Maximum motor speed and the maximum velocity constraint of the robot end .

4. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 3 is characterized in that: In step S310, the speed mapping matrix for: ， ， in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and The number of cable turns set on the elbow pulley and wrist pulley respectively, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

5. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 4 is characterized in that: In step S320, the motor speed for: ， in, is the velocity mapping matrix, is the manipulator Jacobian matrix The pseudo-inverse matrix of Time-varying terminal velocity constraint Based on the multi-space kinematic modeling of flexible-driven agile space manipulator, including the kinematics of motor and joint space and the kinematics of joint and end space, the motor speed is limited in the derivation process. As a constraint, the end velocity of the robot arm can be Convert to joint velocity ; Then the joint speed is converted to Converted to motor speed , get the terminal speed constraint that meets the motor speed limit requirements ; The mapping relationship between the motor and the joint space needs to be considered separately for different joints. The shoulder, elbow and wrist joints have different structures and different mapping relationships. For the forward and inverse kinematic relationship between the joint and the end space, the special motion characteristics of the rope-driven joint and its equivalence in kinematic representation need to be considered.

6. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 3 is characterized in that: In step S330, the maximum speed of the motor for: ， Maximum speed of the end of the robot for: ， ， in, is the maximum joint velocity, is the motor speed, is the Jacobian matrix of the robot arm, DI is the velocity mapping matrix, is with and the desired speed direction Related variables, is the degree of freedom of the robot arm.

7. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 1 is characterized in that: In step S400, the time-varying terminal maximum jerk constraint Based on the dynamic model of flexible-driven agile space manipulator, the dynamic analysis considering the mass inertia of the arm is carried out to obtain the joint torque generated by the movement of the manipulator arm. ; The rope tension analysis of the rope-driven joint can be used to analyze the joint torque. Converted into motor torque , get the terminal maximum acceleration constraint that meets the motor rated speed limit ; Among them, the rope-driven joint motor torque and joint torque The relationship is: ， ， in, is the first joint angle of the shoulder, is the second joint angle of the shoulder, is the elbow joint angle, is the elbow joint angle, is the angle of the wrist joint, is the angle of the wrist joint. is the angle of the three wrist joints, and are the number of cable turns on the elbow pulley and wrist pulley, and are the diameters of the elbow pulley and wrist motor output shaft, is the elbow pulley inclination angle, is the distance between the elbow pulley and the wrist pulley, The circumferential diameter of the four wrist cables, Arrange the offset angle for the wrist cable.

8. The autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator according to claim 7 is characterized in that: In the step S400, the rope tension analysis of the flexible drive joint is based on the force and torque balance equation, and the force and torque balance equation is established respectively in combination with the different structures of the elbow and wrist joints to obtain the mapping relationship between the joint torque and the motor torque related to the joint angle; The dynamic modeling of the mass inertia of the robot arm adopts the iterative Newton-Euler method to obtain the relationship between the joint torque and the joint angle, joint angular velocity and joint angular acceleration; By further deriving the differential kinematics of the robot arm joint and the terminal space, the expressions of terminal acceleration and joint angle, joint angular velocity and joint angular acceleration are obtained; the maximum joint angular acceleration under the rated torque of the motor obtained by the dynamic model is converted into the maximum acceleration of the terminal through the Hessian matrix; In the direction of the desired speed, The maximum joint acceleration corresponding to the motor torque limit , obtained by the inverse dynamics of the robot arm, satisfies the following constraints: ， in, is the maximum torque of the motor, SI is the mapping matrix between joint torque and motor torque, inverseDynamics is the inverse dynamics function of the robot arm, is the joint angle, is the joint angular velocity, is the maximum acceleration of the joint angle, is the Jacobian matrix of the robot arm, is the proportional coefficient, accflag indicates whether the corresponding velocity component is in the six-dimensional state quantity of acceleration or deceleration; By differentiating the velocity Jacobian matrix, we can obtain the maximum acceleration at the end. for; ， ， in, is the time derivative of the Jacobian matrix, Is a 6 The matrix of is the number of degrees of freedom of the robot arm, is the Hessian matrix, Is a 6 Array of An array is a collection of matrices. Contains 6 planes, of which the i-th plane is denoted as , is a The matrix of .

9. A rope-driven agile space manipulator, used to execute the autonomous trajectory planning method for rapidly capturing a moving target by a rope-driven agile space manipulator as claimed in any one of claims 1 to 8, characterized in that: The rope-driven agile space robot comprises a shoulder joint (100), a drive box (200), a first arm (300), a first elbow joint (400), a second arm (500), a second elbow joint (600), a third arm (700), a wrist joint (800) and an end effector (900) which are connected in sequence, and a rope winding device is arranged in the drive box (200); Among them, the shoulder joint of the robot arm is a rigid joint composed of a motor and a reducer, the elbow joint and the wrist joint are rope-driven joints, and the driving motors of the elbow joint and the wrist joint are both arranged in the rope winding device. 10 . A computer-readable storage medium having program instructions stored thereon, wherein the program instructions are executed by a processor to implement the method according to claim 1 .

Images