A method and device for visual control of a rope-driven agile arm with fusion of measurement and correction

Abstract

The present application relates to the measurement and correction fusion visual control method and device of agile arm of rope drive, wherein the method comprises the following steps: based on target pose data and current joint angle, solving the expected mechanical arm end motion pose, for the visual servo control far away from the target, increasing the intermediate node, ensuring that the mechanical arm approaches the target as straight as possible; calculating the motion mode between the intermediate nodes and the time required for the motion between the nodes; in the process of mechanical arm motion, maintaining the observation of the target by the hand-eye camera of the mechanical arm and the pose calculation, starting the visual servo planning based on the mechanical arm end pose deviation and the preset pose deviation threshold; based on the kinematic transformation relationship between the mechanical arm joint and the motor, converting the planned joint motion data into motor motion data. The intermittent measurement and real-time correction are fused, some intermediate nodes are set on the mechanical arm motion path, and the mechanical arm motion is smooth and the end motion trajectory is relatively straight to approach the target.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, and particularly relates to a vision control method and device for measurement and correction fusion of a rope-driven agile arm. BACKGROUND

[0002] As a new type of rope-driven multi-degree-of-freedom mechanical arm, the rope-driven agile arm has the advantages of flexible movement, low inertia and post-driving, and has great application potential in space operations, complex environment operations and the like. Vision servoing is an important means for online autonomous operation of the mechanical arm. The mechanical arm is equipped with a hand-eye camera at the end, which can observe the surrounding environment and solve the target pose in real time. The mechanical arm plans and controls the movement of the mechanical arm to the desired position according to the target pose information.

[0003] Compared with the traditional rigid mechanical arm, the transmission link between the driving end and the working end of the rope-driven agile arm is longer, including motor driving, driving rope transmission, joint movement and end effector movement. In order to realize multi-degree-of-freedom movement and small size envelope, the structures of the joints of the rope-driven agile arm are quite different, the kinematic transformation relationship between the motor, the rope and the joints is complex and nonlinear transformation, and the kinematic model is complex and multi-layer transmission link, which makes it difficult to adjust the closed-loop motion parameters of the mechanical arm joints and the motor, the response characteristics of each motor are inconsistent, the rope driving will cause joint movement hysteresis, and the joint response lags behind the motor driving end. The current vision servoing method is difficult to adapt to the motion characteristics of the rope-driven agile arm, and therefore a new vision servoing control method needs to be developed.

[0004] The offline planning method such as polynomial interpolation or spline interpolation has the advantages of motion smoothing, low requirement for joint and motor motion characteristics, etc. The target pose can be measured at the beginning of the vision servoing, and then the expected joint angle is solved. The mechanical arm is moved to the position by the offline planning method, but this method requires high absolute positioning accuracy of the mechanical arm, and is not suitable for some occasions with high positioning accuracy requirements. SUMMARY

[0005] The present application provides a vision control method and device for measurement and correction fusion of a rope-driven agile arm, which aims to at least solve one of the technical problems existing in the prior art.

[0006] The technical scheme of the present application is a vision control method for measurement and correction fusion of a rope-driven agile arm, which is applied to a rope-driven agile arm. The rope-driven agile arm includes a shoulder first joint, a shoulder second joint, an elbow wrist joint driving box, a first rope-driven rigid arm, an elbow first joint, a second rope-driven rigid arm, an elbow second joint, a wrist first joint, a wrist second joint, a wrist third joint and an end effector connected in sequence. The vision control method for measurement and correction fusion of the rope-driven agile arm includes:

[0007] S100, based on the target pose data and the current joint angle, solving the desired mechanical arm end motion pose, for the visual servo control far from the target, increasing the intermediate node, ensuring that the mechanical arm approaches the target as much as possible along a straight line;

[0008] S200, calculating the motion mode between the intermediate nodes and the time required for the motion between the nodes;

[0009] S300, during the motion of the mechanical arm, maintaining the observation of the target by the hand-eye camera of the mechanical arm and the pose calculation, based on the deviation of the mechanical arm end pose from the preset pose deviation threshold, starting the visual servo planning;

[0010] S400, based on the kinematic transformation relationship between the joints of the mechanical arm and the motor, converting the planned joint motion data into motor motion data.

[0011] Further, the step S100 comprises:

[0012] S110, based on the current joint angle and the target pose measurement value, calculating the current end pose of the mechanical arm 0 T n , and further solving the desired end pose of the mechanical arm moving to the target 0 T t ;

[0013] S120, based on the preset motion flow and the desired end pose 0 T t , increasing the intermediate nodes of the mechanical arm motion and the motion acceleration section, the motion stability section and the motion deceleration section based on the intermediate nodes;

[0014] S130, calculating the joint angle of the desired end pose 0 T t and the intermediate node.

[0015] Further, in the step S110, the desired end pose 0 T t is:

[0016] 0 T t 0 T n * n T t ,

[0017] wherein, 0 ​T n for the current end pose of the robot arm, n T t for the homogeneous transformation matrix of the end pose increment.

[0018] Further, in the step S120,

[0019] Based on the motion capability and motion limit of the robot arm, a first intermediate node and a second intermediate node are set between the initial position and the target position; the motion between the initial position and the first intermediate node is set as a robot arm motion acceleration section, the motion between the first intermediate node and the second intermediate node is set as a motion stabilization section, and the motion between the second intermediate node and the target position is set as a motion deceleration section.

[0020] Further, the step S130 further includes calculating the node motion distance based on the robot arm end pose motion speed threshold value, and the target position input data is the pose deviation of the target relative to the end of the agile arm P x , P y , P z , R x , R y , R z respectively calculating the modulus of the position deviation vector of the target relative to the end of the agile arm P || and the modulus of the attitude deviation vector R ||,

[0021]

[0022] wherein, P x is the pose deviation of the end of the agile arm in the X-axis direction, P y is the pose deviation of the end of the agile arm in the Y-axis direction, P z is the pose deviation of the end of the agile arm in the Z-axis direction, R x is the attitude deviation of the end of the agile arm in the X-axis direction, R y is the attitude deviation of the end of the agile arm in the Y-axis direction, R z is the attitude deviation of the end of the agile arm in the Z-axis direction.

[0023] Further, the step S200 includes:

[0024] S210, calculate the motion time between intermediate nodes, in the motion acceleration section, the robot arm slowly accelerates, in the motion deceleration section, the robot arm slowly decelerates or stops motion, limit the motion time between each intermediate node based on the speed limit of the robot arm joint angle;

[0025] S220, plan the joint angle, joint angular velocity and joint angular acceleration of each cycle.

[0026] Further, in the step S210,

[0027] The running speed of the robot arm joint angle in the motion stabilization section is less than the robot arm joint angle speed threshold.

[0028] Further, in the step S300,

[0029] When the robot end pose deviation is greater than a certain range from the preset pose deviation, the robot motion deviates too much from the target, at this time, the visual servo planning is re-performed based on the pose measurement information, the visual servo planning calculates the expected joint angle based on the current joint angle and the target pose, and the joint angle interpolation planning is performed on the robot.

[0030] Further, for the application scene with high requirement on target pose measurement accuracy, the robot is stationary when moving to a position close to the target, and the target pose is measured under the condition that the robot end and the target are relatively stationary, the expected joint angle is calculated and the robot motion is planned.

[0031] Further, the present application also proposes a measurement and correction fusion visual control device of a rope-driven agile arm, which is used to execute the measurement and correction fusion visual control method of the rope-driven agile arm, and the device comprises:

[0032] A shoulder first joint driven by a motor through a reducer;

[0033] A shoulder second joint driven by a motor through a reducer, which is connected with the shoulder first joint;

[0034] An elbow wrist joint driving box connected with the shoulder second joint;

[0035] A first rope-driven rigid arm connected with the elbow wrist joint driving box;

[0036] An elbow first joint provided with a first joint encoder, which is connected with the first rope-driven rigid arm;

[0037] A second rope-driven rigid arm connected with the elbow first joint;

[0038] The elbow second joint is provided with a second joint encoder, and the elbow second joint is connected with the second rope-driven rigid arm;

[0039] The wrist first joint is a universal joint, and the wrist first joint is provided with a third joint encoder, and the wrist first joint is connected with the elbow second joint;

[0040] The wrist second joint is a universal joint, and the wrist second joint is provided with a fourth joint encoder, and the wrist second joint is connected with the wrist first joint;

[0041] The wrist third joint is a roll joint, and the wrist third joint is provided with a fifth joint encoder, and the wrist third joint is connected with the wrist second joint;

[0042] The end effector is connected with the wrist third joint;

[0043] The elbow-wrist joint driving box comprises a first driving motor connected with the elbow first joint through a first driving rope, a second driving motor connected with the elbow second joint through a second driving rope, a third driving motor and a fourth driving motor connected with the wrist first joint and the wrist second joint through a third rope and a fourth driving rope, and a fifth driving motor connected with the wrist third joint through a fifth driving rope,

[0044] Rotary variable transducers for measuring the rotation angle of the motors are arranged on the first driving motor, the second driving motor, the third driving motor, the fourth driving motor and the fifth driving motor.

[0045] The beneficial effects of the present application are:

[0046] The measurement and correction fusion visual control method and device of the rope-driven agile arm combine visual servo pose solving and offline planning, so that the mechanical arm movement has the advantages of visual servo correctable mechanical arm movement trajectory and offline planning movement smoothness, and low requirement for mechanical arm joint and motor movement characteristics. Different visual servo strategies are formulated in the whole visual servo process. When far from the target, the movement speed of the mechanical arm is planned to be large to realize fast approaching of the target. When close to the target, the movement speed of the mechanical arm is planned to be small to prevent the mechanical arm from colliding with the target in advance and other unexpected situations. Intermediate nodes are set to correct the end movement trajectory of the mechanical arm, so that the end movement trajectory of the mechanical arm can keep straight as much as possible to approach the target, and the phenomenon that the mechanical arm deviates from the target during the movement process is avoided. The intermittent measurement and real-time correction are fused. During the movement process of the mechanical arm, the measurement and pose solving of the target are kept. When the end of the mechanical arm deviates from the target greatly, the visual servo planning is introduced to correct the movement of the mechanical arm. When the mechanical arm frequently deviates or is very close to the target, the movement of the mechanical arm can be stopped to keep the relative static state between the end of the mechanical arm and the target to measure the target pose, so as to eliminate the influence of the lag of the target pose measurement information or the large measurement error of the target pose in the movement state. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1 A flow chart of the measurement and correction fusion visual control method of the rope-driven agile arm according to the present application is shown.

[0048] Figure 2 A flow chart of solving the expected end movement pose of the mechanical arm and adding intermediate nodes in the measurement and correction fusion visual control method of the rope-driven agile arm according to the present application is shown.

[0049] Figure 3 A schematic diagram of the measurement and correction fusion visual control method of the rope-driven agile arm according to the present application is shown.

[0050] Figure 4 A schematic diagram of the rope-driven agile arm in the measurement and correction fusion visual control method of the rope-driven agile arm according to the present application is shown.

[0051] Figure 5 A schematic diagram of one working scene of the measurement and correction fusion visual control device of the rope-driven agile arm according to the present application is shown.

[0052] REFERENCE NUMERALS:

[0053] 100, first shoulder joint; 110, second shoulder joint; 200, elbow wrist joint driving box; 300, first rope-driven rigid arm; 400, first elbow joint; 500, second rope-driven rigid arm; 600, second elbow joint; 700, first wrist joint; 710, second wrist joint; 720, third wrist joint; 800, end effector. Embodiments

[0054] The concept, specific structure and generated technical effects of the present application will be described clearly and completely in combination with the embodiments and drawings below, so as to fully understand the purpose, scheme and effects of the present application. It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict.

[0055] It should be noted that, unless otherwise specified, when a certain feature is referred to as being "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. In addition, the up, down, left, right, top, bottom and the like used in the present application are only relative to the relative position relationship of the components of the present application in the drawings.

[0056] In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terms used in the specification of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The term "and / or" used herein includes any combination of one or more related listed items.

[0057] It should be understood that although the terms first, second, third, etc. can be used in the present disclosure to describe various elements, 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 can also be referred to as the second element, and similarly, the second element can also be referred to as the first element.

[0058] Reference Figures 1 to 5 In some embodiments, the technical scheme of the present application is a measurement and correction fusion visual control method of a rope-driven agile arm, which is applied to a rope-driven agile arm. The rope-driven agile arm includes a shoulder first joint 100, a shoulder second joint 110, an elbow wrist joint driving box 200, a first rope-driven rigid arm 300, an elbow first joint 400, a second rope-driven rigid arm 500, an elbow second joint 600, a wrist first joint 700, a wrist second joint 710, a wrist third joint 720 and an end effector 800 connected in sequence. Referring to Figure 1 and Figure 4 The measurement and correction fusion visual control method of the rope-driven agile arm includes:

[0059] S100, based on target pose data and current joint angle, solving the expected mechanical arm end motion pose, for visual servo control far from the target, adding intermediate nodes to ensure that the mechanical arm approaches the target as straight as possible;

[0060] S200, calculate the motion mode between intermediate nodes and the time required for motion between nodes;

[0061] S300, during the motion of the mechanical arm, the hand-eye camera of the mechanical arm keeps observing the target and calculating the pose, and based on the deviation of the end pose of the mechanical arm from the preset pose deviation threshold, visual servo planning is started;

[0062] S400, based on the kinematic transformation relationship between the joints of the mechanical arm and the motors, the planned joint motion data is transformed into motor motion data.

[0063] The measurement and correction fusion visual control method of the rope-driven agile arm plans the joint angle motion of the mechanical arm by using the position level visual servo planning method, calculates the expected joint angle according to the measured value of the target pose, plans the motion of the mechanical arm from the current position to the expected joint angle, and monitors the measured value of the target pose during the motion of the mechanical arm. When the measured target pose deviation is greater than a certain range, visual servo planning can be introduced at any time to correct the motion data of the mechanical arm.

[0064] The beneficial effects of the present application are:

[0065] The intermittent measurement and real-time correction fusion visual servo control of the rope-driven agile arm with multiple heterogeneous joints is a set of visual servo control method suitable for the rope-driven agile arm, which solves the problems of complex kinematic model of the rope-driven agile arm, great difficulty in adjusting the kinematic parameters of the joints and motors, inconsistent motor response, joint motion hysteresis and the like.

[0066] The intermittent measurement and real-time correction fusion is to set some intermediate nodes on the motion path of the mechanical arm, so as to ensure the smooth motion of the mechanical arm and the straight approaching of the end motion trajectory to the target, prevent certain deviation during the motion process of the joint angle interpolation planning, and keep the measurement and pose calculation of the target during the motion process. When the mechanical arm deviates from the target, the pose measurement information is introduced to re-plan the visual servo.

[0067] The measurement and correction integrated visual control method and device of the rope-driven agile arm combine visual servo pose calculation with offline planning, so that the mechanical arm movement has the advantages of visual servo correctable mechanical arm movement trajectory and offline planning movement smoothness, and low requirement for mechanical arm joint and motor movement characteristics. Different visual servo strategies are formulated in the whole visual servo process. When far from the target, the movement speed of the mechanical arm is planned to be large to achieve fast approach to the target. When close to the target, the movement speed of the mechanical arm is planned to be small to prevent the mechanical arm from colliding with the target in advance and other accidents. Intermediate nodes are set to correct the movement trajectory of the mechanical arm end, so that it can keep straight-line approach to the target as much as possible, and avoid the phenomenon that the mechanical arm deviates from the target during the movement process planned by joint angle interpolation. The intermittent measurement and real-time correction are integrated. During the movement process of the mechanical arm, the measurement and pose calculation of the target are kept. When the mechanical arm end deviates from the target greatly, the visual servo planning is introduced to correct the movement of the mechanical arm. When the mechanical arm frequently deviates or is very close to the target, the movement of the mechanical arm can be stopped to keep the relative static state between the mechanical arm end and the target to measure the target pose, so as to eliminate the influence of the lag of the target pose measurement information or the large measurement error of the target pose in the movement state.

[0068] According to the conventional visual servo method, the joint angle movement increment of the mechanical arm is planned as dQ=[dQ1, dQ2, dQ3, dQ4, dQ5, dQ6, dQ7]. Because of the movement hysteresis of the joint angle and the motor, the joint angle cannot be in place after one period. The movement increments of the joints are different because of different response characteristics, for example, the actual movement increment is dQ'=[0.9dQ1, 0.9dQ2, 0.5dQ3, 0.5dQ4, 0.2dQ5, 0.2dQ6, 0.2dQ7]. In the next period, the new joint angle increment is planned according to the target pose measurement value and the current joint angle. However, the movement amount of part of the joints is still far less than the planned value. The camera measurement period is generally longer than the movement planning period, that is, the current target pose measurement value is actually the target pose measurement value several periods ago. The joint movement hysteresis, movement not in place and movement mismatch will lead to slow movement of the mechanical arm, and the movement of the mechanical arm is inconsistent with the planned value, which leads to that the target is not in the field of view of the hand-eye camera. As for offline planning, the joint angle of each period is planned by using polynomial interpolation or spline interpolation and other methods according to the current joint angle and the expected joint angle. Because of the movement hysteresis and movement not in place of the joints and the motor, the difference between the expected joint angle of the mechanical arm and the current joint angle will gradually increase. For the joint PID closed loop control based on kinematics feedforward, the joint angle output by the control loop is

[0069]

[0070] In the formula, q0 is the current joint angle, q0+dq is the planned joint angle, kPdq is the proportional term of the joint closed-loop control, kIdq is the integral term of the joint closed-loop control, and kDd is the differential term of the joint closed-loop control. The greater the joint angle output by the control loop, the greater the motor angle increment converted to the motor end through the kinematics model, and the greater the increment will improve the motor movement speed and output torque, and speed up its movement to the position. Therefore, as the difference increases, that is, dq increases, the following performance of the joint and the motor will be better and better, and the difference between the expected joint angle and the current joint angle will gradually decrease, and finally the robot arm moves to the expected joint angle position, therefore, the offline planning has the advantages of smooth movement and low requirement on the movement characteristics of the robot arm joint and motor.

[0071] Therefore, the present application plans the movement of the robot arm by monitoring the target pose through vision, calculating the expected joint angle, and using the offline planning mode, and plans the movement speed of the robot arm to be large when far from the target to achieve rapid approach to the target, and plans the movement speed of the robot arm to be small when close to the target to prevent the robot arm from colliding with the target in advance and other unexpected situations, and the intermediate nodes can also correct the movement trajectory of the robot arm end to keep it as straight as possible to approach the target, avoiding the phenomenon that the robot arm deviates from the target during the movement process only through joint angle interpolation planning. During the movement of the robot arm, the measurement and pose calculation of the target are maintained, and when the robot arm end deviates from the target greatly, the vision servo planning is introduced to correct the movement of the robot arm.

[0072] Further, with reference to Figure 2 , the step S100 comprises:

[0073] S110, calculating the current end pose of the robot arm based on the current joint angle and the target pose measurement value 0 T n , and further calculating the expected end pose of the robot arm moving to the target 0 T t ;

[0074] S120, increasing the intermediate nodes of the movement of the robot arm and the movement acceleration section, movement stability section and movement deceleration section based on the preset movement flow and the expected end pose 0 T t ;

[0075] S130, calculating the joint angle of the expected end pose 0 T t and the intermediate nodes.

[0076] Specifically, according to the input target pose measurement data and the current joint angle, the desired mechanical arm end motion pose is calculated, several intermediate motion end pose nodes are determined, and the desired joint angle moving to several nodes is calculated. For visual servo control far from the target, the intermediate nodes can be continuously increased to ensure that the mechanical arm approaches the target as straight as possible. Only joint angle interpolation planning will cause a certain deviation in the motion process. The motion mode between several nodes and the time required for motion between nodes are designed. In the motion starting stage, the mechanical arm slowly accelerates to prevent shaking caused by sudden start of the mechanical arm. When the mechanical arm end pose deviates from the target, the mechanical arm gradually slows down. The mechanical arm hand-eye camera keeps observing the target and calculating the pose during the motion of the mechanical arm. When the deviation between the mechanical arm end pose and the theoretical pose is greater than a certain range, it means that the mechanical arm deviates from the target more. At this time, visual servo planning is introduced again. According to the current joint angle and the target pose, the desired joint angle is calculated and joint angle interpolation planning is performed. For application scenarios with high requirements for target pose measurement accuracy, the mechanical arm can be stationary when moving to a position close to the target. The target pose is measured while keeping the mechanical arm end and the target relatively stationary. The desired joint angle is calculated and the mechanical arm motion is planned. Motor angle control data post-processing, using the kinematic transformation relationship between the agile arm joint and the motor, converts the planned joint motion data into motor motion data.

[0077] Further, with reference to Figure 2 , in the step S110, the desired end pose 0 T t is:

[0078] 0 T t = 0 T n * n T t ,

[0079] wherein, 0 T n is the current end pose of the mechanical arm, n T t is a homogeneous transformation matrix of the end pose increment.

[0080] 0 T t = 0 T n * n T t, the relationship formula of the expected end position, the current end position increment = the expected end position,

[0081]

[0082] Specifically, according to the current joint angle and the target pose measurement value, the current end position of the robot arm is calculated, and then the expected end position of the robot arm moving to the target is calculated. According to the current joint angle and the target pose measurement value, the current end position of the robot arm is calculated 0 T n The end position increment is converted into a homogeneous transformation matrix n T t The end position increment is converted into a homogeneous transformation matrix 0 T t = 0 T n * n T t .

[0083] Further, referring to Figure 2 , in the step S120,

[0084] Based on the motion ability and motion limit of the robot arm, the first intermediate node and the second intermediate node are set between the initial position and the target position; the motion between the initial position and the first intermediate node is set as the acceleration section of the robot arm motion, the motion between the first intermediate node and the second intermediate node is set as the stable motion section, and the motion between the second intermediate node and the target position is set as the deceleration section of the motion.

[0085] Specifically, according to the preset motion flow and the expected end position, the intermediate nodes of the robot arm motion are designed, for example, two nodes are added, the motion between the initial position and the first intermediate node is the acceleration section of the robot arm motion, the motion between the first intermediate node and the second intermediate node is the stable motion section, and the motion between the second intermediate node and the motion end point is the deceleration section of the motion. For visual servo control far from the target, intermediate nodes can be continuously added to ensure that the robot arm approaches the target as straight as possible. Only joint angle interpolation planning will cause a certain deviation in the motion process. The setting of node motion distance can be designed according to the motion ability and motion limit of the robot arm, for example, the node motion distance is designed according to the end position motion speed threshold of the robot arm.

[0086] The setting of node motion distance can be designed according to the motion ability and motion limit of the robot arm, for example, the node motion distance is designed according to the end position motion speed threshold of the robot arm. The target position input data is the pose deviation of the target relative to the end of the agile armP x , P y , P z , R x , R y , R z ], respectively, calculate the modulus of the position deviation vector of the target relative to the end of the agile arm P || and the modulus of the attitude deviation vector R ||. The end position of the end effector 800 and the corresponding joint angles of several intermediate nodes are calculated by using the kinematic model of the end effector to joint angle.

[0087] The mechanical arm end position velocity threshold is P 0, and the attitude velocity threshold is R 0, the average position velocity of the mechanical arm in the acceleration section is about 0.5 P 0, and the average attitude velocity is about 0.5 R 0, while ensuring that the mechanical arm accelerates slowly and the mechanical arm approaches the target slowly, the time of the first and last movement of the mechanical arm is determined according to the acceleration ability of the mechanical arm t 0, the movement distance and the end rotation attitude of the acceleration section and the deceleration section are respectively 0.5 P 0 t 0 and 0.5 R 0 t 0. Then the movement distance and the end rotation attitude of the stable movement section of the mechanical arm are respectively P || P 0 t 0 and R || R 0 t 0. If the mechanical arm is close to the target position and attitude deviation, the movement distance and speed of each section can be appropriately reduced.

[0088] Further, the step S130 further comprises calculating the node movement distance based on the mechanical arm end position motion velocity threshold, and the target position input data is the position deviation of the target relative to the end of the agile arm P x , P y , P z , R x , R y , R z ], respectively, calculate the modulus of the position deviation vector of the target relative to the end of the agile armP ||and the magnitude of the attitude deviation vector|| R ||,

[0089]

[0090] in, P x This represents the pose deviation of the agile arm's end effector in the X-axis direction. P y The pose deviation of the agile arm's end effector in the Y-axis direction is denoted as . P z The pose deviation of the agile arm's end effector in the Z-axis direction is denoted as . R x This represents the attitude deviation of the agile arm's end effector in the X-axis direction. R y The attitude deviation of the agile arm end effector in the Y-axis direction is given. R z This represents the attitude deviation of the agile arm end in the Z-axis direction.

[0091] The kinematic model of the robotic arm's end effector (800° to joint angles) is used to calculate the end-effector pose and the joint angles corresponding to several intermediate nodes. The step lengths of each motion segment calculated in step A2 are converted into the required poses of each segment. P x ', P y ', P z ', R x ', R y ', R z '], transform it into a homogeneous transformation matrix, multiply it with the current end-effector pose of the robotic arm to obtain the expected end-effector pose of each node, and use the kinematic model of the end effector 800 of the robotic arm to the joint angle to solve the expected joint angle of the motion to each node.

[0092] Furthermore, refer to Figure 1 and Figure 4 Step S200 includes:

[0093] S210. Calculate the motion time between intermediate nodes. During the acceleration phase, the robotic arm accelerates slowly. During the deceleration phase, the robotic arm decelerates slowly or stops moving. The motion time between each intermediate node is limited based on the speed of the robotic arm joint angle.

[0094] S220, plan the joint angle, joint angular velocity and joint angular acceleration for each cycle.

[0095] In step S210, motion time between several nodes is designed. In the initial stage, the first motion time is designed to be longer to ensure slow acceleration of the robot arm. In the intermediate nodes, the motion time is designed to be shorter because the joints of the robot arm have higher speed. In the last motion, the last motion time is designed to be longer to ensure safety. The motion time between nodes can be designed according to the speed limit of the joint angle of the robot arm. The running speed of the joint angle in the uniform speed section needs to be less than the joint angle speed threshold.

[0096] In step S220, the joint angle, joint angular velocity and joint angular acceleration of each cycle are planned. The motion between several nodes is planned by using a cubic spline planning method. The cubic spline can ensure that the initial joint angular acceleration, joint angular velocity and the final joint angular acceleration, joint angular velocity are 0, the motion between nodes is smooth, and the joint angular velocity and angular acceleration will not be suddenly changed.

[0097] Further, referring to Figure 1 and Figure 4 , in the step S210,

[0098] The running speed of the joint angle of the robot arm in the motion stable section is less than the joint angle speed threshold of the robot arm.

[0099] Further, referring to Figure 1 and Figure 4 , in the step S300,

[0100] When the end pose deviation of the robot arm is greater than a certain range from the preset pose deviation, the robot arm moves too much from the target. At this time, the visual servo planning is re-performed based on the pose measurement information. The visual servo planning calculates the expected joint angle based on the current joint angle and the target pose, and performs joint angle interpolation planning on the robot arm.

[0101] Specifically, different visual servo strategies are formulated in the entire visual servo process. When the distance to the target is far, the motion speed of the robot arm is planned to be large to achieve fast approaching to the target. When the distance to the target is close, the motion speed of the robot arm is planned to be small to prevent the robot arm from colliding with the target in advance and other unexpected situations. When the end of the robot arm deviates from the target greatly, the visual servo planning is introduced to correct the motion of the robot arm. When the robot arm frequently deviates or is very close to the target, the motion of the robot arm can be stopped to keep the end of the robot arm and the target relatively static to measure the target pose, so as to eliminate the influence of the lag of the target pose measurement information or the large measurement error of the target pose in the motion state. The setting of the intermediate node can also correct the motion trajectory of the end of the robot arm to keep it as straight as possible to approach the target. Only through joint angle interpolation planning, the end of the robot arm will deviate from the target in the motion process.

[0102] In the process of the mechanical arm movement, the mechanical arm hand-eye camera is kept to observe the target and to calculate the pose, because the actual working scene is generally that the mechanical arm end approaches the target from the far end, that is, the main target pose deviation is in the z-axis direction of the mechanical arm end, therefore, the threshold value for judging the target pose deviation is corrected according to the distance between the target and the z-axis direction of the mechanical arm end, the threshold value can be enlarged when the distance is far, when the distance is close, the deflection of the mechanical arm end has a greater impact on the mechanical arm to the position, and therefore the threshold value needs to be reduced; when the target pose deviation is greater than the threshold value, it is indicated that the mechanical arm movement deviates from the target more, at this time, the visual servo planning is introduced again, the expected joint angle is calculated according to the current joint angle and the target pose, and the joint angle interpolation planning is performed, for the application scene with high requirement on the target pose measurement accuracy, the mechanical arm is kept static when moving to a position close to the target, the target pose is measured under the condition that the mechanical arm end and the target are relatively static, the expected joint angle is calculated, and the mechanical arm movement is planned.

[0103] Further, with reference to Figure 1 and Figure 4 for the application scene with high requirement on the target pose measurement accuracy, the mechanical arm is kept static when moving to a position close to the target, the target pose is measured under the condition that the mechanical arm end and the target are relatively static, the expected joint angle is calculated, and the mechanical arm movement is planned.

[0104] Further, with reference to Figure 3 the application further provides a measurement and correction fusion visual control device of a rope-driven agile arm, which is used for executing the measurement and correction fusion visual control method of the rope-driven agile arm, and the device comprises:

[0105] a shoulder first joint 100, which is driven by a motor through a reducer;

[0106] a shoulder second joint 110, which is driven by a motor through a reducer, and is connected with the shoulder first joint 100;

[0107] an elbow wrist joint driving box 200, which is connected with the shoulder second joint 110;

[0108] a first rope-driven rigid arm 300, which is connected with the elbow wrist joint driving box 200;

[0109] an elbow first joint 400, which is provided with a first joint encoder, and is connected with the first rope-driven rigid arm 300;

[0110] a second rope-driven rigid arm 500, which is connected with the elbow first joint 400;

[0111] The elbow second joint 600 is provided with a second joint encoder, and the elbow second joint 600 is connected with the second rope-driven rigid arm 500;

[0112] The wrist first joint 700 is a universal joint, and the wrist first joint 700 is provided with a third joint encoder, and the wrist first joint 700 is connected with the elbow second joint 600;

[0113] The wrist second joint 710 is a universal joint, and the wrist second joint 710 is provided with a fourth joint encoder, and the wrist second joint 710 is connected with the wrist first joint 700;

[0114] The wrist third joint 720 is a roll joint, and the wrist third joint 720 is provided with a fifth joint encoder, and the wrist third joint 720 is connected with the wrist second joint 710;

[0115] The end effector 800 is connected with the wrist third joint 720;

[0116] The elbow-wrist joint driving box 200 includes a first driving motor connected with the elbow first joint 400 through a first driving rope, a second driving motor connected with the elbow second joint 600 through a second driving rope, a third driving motor and a fourth driving motor connected with the wrist first joint 700 and the wrist second joint 710 through a third rope and a fourth driving rope, and a fifth driving motor connected with the wrist third joint 720 through a fifth driving rope,

[0117] The first driving motor, the second driving motor, the third driving motor, the fourth driving motor and the fifth driving motor are all provided with a rotary variable transducer sensor for measuring the rotation angle of the motor.

[0118] In some specific embodiments, the rope-driven agile arm has seven joints, two joints in the shoulder, two joints directly driven by motors through reducers, the second joint 110 in the shoulder connects the elbow and wrist joint drive box 200, which contains five drive motors for the elbow and wrist joints. The motors are connected to drive the ropes, and the rotation of the motors pulls the drive ropes to drive the joints to rotate. The mechanical arm includes two elbow joints and three wrist joints, of which the first wrist joint 700 and the second wrist joint 710 are universal joints driven by two motors to move the first wrist joint 700 and the second wrist joint 710, and the third wrist joint 720 is a roll joint. Joint encoders are provided at each joint to measure the rotation angle of the joint, and rotary transformers are provided at the motor end to measure the rotation angle of the motor. The closed-loop control mode of the mechanical arm is a combination of joint-level position closed-loop and motor-level position closed-loop control modes.

[0119] Referring to Figure 5 , an application example of a practical working scene of the rope-driven agile arm measurement and correction integrated visual control device is as follows:

[0120] The agile mechanical arm can be used for target capture. The pose of the target is measured by the hand-eye camera of the mechanical arm, the motion of the mechanical arm to the target position is planned, and the target is grasped by the gripper at the end of the mechanical arm. The target is moved autonomously from a position far from the end of the mechanical arm, the pose of the target is measured, the expected motion pose of the end of the mechanical arm is calculated, several motion nodes are designed between the current end pose and the expected end pose, the first node is close to the current end pose of the mechanical arm, the motion takes a long time, and the acceleration stage of the mechanical arm is designed to prevent the mechanical arm from shaking due to too fast start. The intermediate nodes are designed according to the distance between the target and the mechanical arm, for example, a motion node can be set at an interval of 200 mm to ensure that the mechanical arm approaches the target along an approximate straight line, and a motion node is set at a position close to the target. The distance between the two points is close, and the motion time is long, so that the mechanical arm can gradually slow down to the target position to prevent collision and other situations. The joint angles corresponding to the intermediate node poses are calculated, and the expected joint angles of each period are planned through joint angle quintic polynomial interpolation or cubic spline interpolation, etc. The kinematic transformation relationship between the joints and the motors of the agile arm is used to convert the joint motion data planned in step B or step C into motor motion data. During the motion of the mechanical arm, the target pose is calculated in real time, and when the deviation between the current end pose of the mechanical arm and the theoretical pose is greater than a certain range, such as the position deviation or attitude deviation of a certain axis at the current time is greater than that of the previous several periods, which indicates that the mechanical arm has deviated from the target, the visual servo planning is introduced again, and the above steps are repeated.

[0121] The above merely describes preferred embodiments of the present application, and the present application is not limited to the above-described embodiments. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure. All shall belong to the protection scope of the present application. The technical solutions and / or embodiments within the protection scope of the present application can have various modifications and changes.

Claims

1. A vision control method of measurement and correction fusion of a rope-driven agile arm, the vision control method of measurement and correction fusion of the rope-driven agile arm is applied to a rope-driven agile arm, the rope-driven agile arm comprises a shoulder first joint (100), a shoulder second joint (110), an elbow-wrist joint driving box (200), a first rope-driven rigid arm (300), an elbow first joint (400), a second rope-driven rigid arm (500), an elbow second joint (600), a wrist first joint (700), a wrist second joint (710), a wrist third joint (720) and an end effector (800) connected in sequence, characterized in that, Comprise: S100, based on target pose data and current joint angle, solve the expected mechanical arm end motion pose, for the visual servo control far from the target, increase the intermediate node, ensure that the mechanical arm as far as possible along the straight line approach target; The step S100 comprises: S110, based on the current joint angle and the target pose measurement value, calculate the current end pose of the robot arm 0 T n , and then solve the desired end pose of the robot arm moving to the target 0 T t ; In the step S110, the desired end pose 0 T t is: 0 T t = 0 T n *nT t , wherein, 0 T n is the current end-effector pose of the robot arm, n T t is a homogeneous transformation matrix for the end-effector pose increment; 0 T t = 0 T n *nT t , is a relationship formula of the desired end pose, the current end pose x, the end pose increment = desired end pose, According to the current joint angle and the target pose measurement value, a current end position of the robot arm is calculated, and a desired end position of the robot arm moving to the target is solved 0 T n The end position increment is converted into a homogeneous transformation matrix n T t , and a desired end position of the robot arm moving to the target is solved 0 T t = 0 T n *nT; S120, determining a motion flow based on the preset motion flow and the expected end position 0 T t , increase the intermediate node of the mechanical arm motion and the motion acceleration section, motion stability section and motion deceleration section based on the intermediate node In the step S120, Based on the motion ability and motion limit of the mechanical arm, set the first intermediate node and the second intermediate node between the initial position and the target position; Set the motion between the initial position and the first intermediate node as the mechanical arm motion acceleration section, set the motion between the first intermediate node and the second intermediate node as the motion stable section, set the motion between the second intermediate node and the target position as the motion deceleration section; According to the preset motion flow and the expected end pose, design the intermediate node of the mechanical arm motion, such as adding two nodes, the motion between the initial position and the first intermediate node is the mechanical arm motion acceleration section, the motion between the first intermediate node and the second intermediate node is the motion stable section, and the motion between the second intermediate node and the motion end point is the motion deceleration section, for the visual servo control far from the target, the intermediate node can be continuously increased to ensure that the mechanical arm approaches the target as far as possible along the straight line, and only the joint angle interpolation planning motion process will occur a certain deviation; The setting of node motion distance can be designed according to the motion ability and motion limit of the mechanical arm, such as designing the node motion distance according to the mechanical arm end pose motion speed threshold value; The setting of the node motion distance can be designed according to the motion ability and motion limit of the mechanical arm, such as designing the node motion distance according to the end pose motion speed threshold of the mechanical arm; the target position input data is the pose deviation [P x ,P y ,P z ,R x ,R y ,R z ] of the target relative to the end pose of the agile arm, the modulus ||P|| of the position deviation vector of the target relative to the end pose of the agile arm and the modulus ||R|| of the attitude deviation vector are calculated respectively, and the kinematics model of the end effector (800) to the joint angle is used to solve the end pose and the corresponding joint angle of several intermediate nodes. The mechanical arm end position speed threshold value is P0, the attitude speed threshold value is R0, then the average position speed of the mechanical arm in the acceleration section is about 0.5P0, and the average attitude speed is about 0.5R0, while ensuring that the mechanical arm slowly accelerates and the mechanical arm slowly moves after approaching the target, according to the acceleration ability of the mechanical arm, the time t0 of the first section and the last section of the mechanical arm motion is determined, then the motion distance and the end rotating attitude of the acceleration section and the deceleration section are 0.5P0t0 and 0.5R0t0 respectively; Then the motion distance and the end rotating attitude of the stable motion section of the mechanical arm are ||P||-P0t0 and ||R||-R0t0 respectively; If the mechanical arm is far from the target position and the attitude deviation is small, the motion distance and speed of each section can be appropriately reduced; S130, calculate the expected end pose 0 T t and joint angles of the intermediate nodes; S200, calculate the motion mode between the intermediate nodes and the time required for the motion between the nodes; S300, in the process of the mechanical arm motion, keep the observation of the target by the hand-eye camera of the mechanical arm and the pose calculation, based on the mechanical arm end pose deviation and the preset pose deviation threshold value, start the visual servo planning; S400, based on the kinematic transformation relationship between the joints of the mechanical arm and the motor, convert the planned joint motion data into motor motion data.

2. The measurement and correction fused visual control method of a rope-driven agile arm according to claim 1, wherein, The step S130 further includes calculating the node motion distance based on the mechanical arm end position motion speed threshold value, and the target position input data is a pose deviation [P x ,P y ,P z ,R x ,R y ,R z ] of the target relative to the agile arm end, respectively calculating the modulus ||P|| of the position deviation vector and the modulus ||R|| of the attitude deviation vector of the target relative to the agile arm end, wherein P x is a pose deviation of the end of the agile arm in the X-axis direction, P y is a pose deviation of the end of the agile arm in the Y-axis direction, P z is a pose deviation of the end of the agile arm in the Z-axis direction, R x is a pose deviation of the end of the agile arm in the X-axis direction, R y is a pose deviation of the end of the agile arm in the Y-axis direction, R z is a pose deviation of the end of the agile arm in the Z-axis direction.

3. The measurement and correction fused vision control method of a rope- driven agile arm according to claim 1, wherein, The step S200 comprises: S210, calculate the motion time between the intermediate nodes, the mechanical arm slowly accelerates in the motion acceleration section, and slowly decelerates or stops moving in the motion deceleration section, based on the speed limit of the joint angle of the mechanical arm, the motion time between each intermediate node is calculated; S220, plan the joint angle, joint angular velocity and joint angular acceleration of each cycle.

4. The measurement and correction fused visual control method of a rope- driven agile arm according to claim 3, wherein, In the step S210, The running speed of the joint angle of the mechanical arm in the motion stable section is less than the joint angular velocity threshold value of the mechanical arm.

5. The measurement and correction fused vision control method of a rope- driven agile arm according to claim 1, wherein, In the step S300, When the mechanical arm end position deviation and the preset position deviation are greater than a certain range, the mechanical arm moves too much from the target, at this time, the visual servo planning is re-performed based on the position measurement information, the visual servo planning calculates the expected joint angle based on the current joint angle and the target position, and the joint angle interpolation planning is performed on the mechanical arm.

6. The measurement and correction fusion visual control method of the rope-driven agile arm according to claim 5, characterized in that, For the application scene with high requirement on target position measurement accuracy, the mechanical arm is static when moving to a position close to the target, the target position is measured under the condition that the mechanical arm end and the target are relatively static, the expected joint angle is calculated and the mechanical arm movement is planned.

7. A vision control device for a rope driven dexterous arm with fused measurement and correction, characterized by, The device for performing the measurement and correction fusion visual control method of the rope-driven agile arm according to any one of claims 1 to 6 comprises: A shoulder first joint (100) driven by a motor through a reducer; A shoulder second joint (110) driven by a motor through a reducer, connected with the shoulder first joint (100); An elbow wrist joint driving box (200) connected with the shoulder second joint (110); A first rope-driven rigid arm (300) connected with the elbow wrist joint driving box (200); An elbow first joint (400) provided with a first joint encoder, connected with the first rope-driven rigid arm (300); A second rope-driven rigid arm (500) connected with the elbow first joint (400); An elbow second joint (600) provided with a second joint encoder, connected with the second rope-driven rigid arm (500); A wrist first joint (700) which is a universal joint, provided with a third joint encoder, connected with the elbow second joint (600); A wrist second joint (710) which is a universal joint, provided with a fourth joint encoder, connected with the wrist first joint (700); A wrist third joint (720) which is a roll joint, provided with a fifth joint encoder, connected with the wrist second joint (710); An end effector (800) connected with the wrist third joint (720). The elbow-wrist joint driving box (200) comprises a first driving motor connected with the elbow first joint (400) through a first driving rope, a second driving motor connected with the elbow second joint (600) through a second driving rope, a third driving motor and a fourth driving motor connected with the wrist first joint (700) and the wrist second joint (710) through a third rope and a fourth driving rope, a fifth driving motor connected with the wrist third joint (720) through a fifth driving rope, A rotary variable transducer sensor is arranged on each of the first driving motor, the second driving motor, the third driving motor, the fourth driving motor and the fifth driving motor to measure the rotation angle of the motor.

Images