Smart slave manipulator robot arm control system and control method

Abstract

The invention discloses a smart slave manipulator robot arm control system and a control method. The control system comprises a parameter module which stores structure parameters of a plurality of different types of robot arms; the detection module is used for detecting detection information of the robot arm; the control module selects a control algorithm and outputs a control instruction to the driving module; and the driving module receives the control instruction output by the control module, and drives the robot arm to perform motion control, jitter control, obstacle avoidance control and arm change control according to the control instruction. The flexibility of the robot arm is improved, the control accuracy and safety of the robot arm are improved, the control accuracy is improved, misoperation such as manual operation hand shaking is prevented, and the situation that when a master manipulator robot arm control system breaks down, after a slave manipulator robot arm loses an instruction, the slave manipulator robot arm moves randomly due to the absence of a slave manipulator control system is avoided. Therefore, the damage influence on the surrounding operation environment is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot, medical technology and control field, and in particular to a control system and a control method of a dexterous slave robot arm. Background Art

[0002] With the rapid development of robot technology, its application field is expanding day by day. The robot's actuator has a very close effect on the working environment. As an end effector, the dexterous robot affects the improvement of robot intelligence and operation level. The robot arm system usually uses a master-slave control mode: when the operator operates the master hand, the hand movement will drive the master hand to move with it. The sensors at the joints of the master hand can measure the movement information. Through the master-slave control, the movement of the master hand is mapped to the slave hand. The joints of the slave hand move, driving the device to achieve corresponding movement.

[0003] The existing traditional robot arms mainly work in the master-slave mode. The flexibility and freedom of the slave robot are not high, and most of it is driven by the master hand. The slave is a device that enters the operating space and performs operations. At present, the master-slave mode is mainly for the master hand to drive the slave to perform some actions. There is not much research on the control system of multi-joint slave robots. At the same time, the slave hand has a low degree of freedom, and the obstacle avoidance and anti-shake capabilities and protection mechanisms of the slave control system are not mature and reliable. These are not conducive to the operation of the slave in a small space. Summary of the invention

[0004] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art, to provide a control system and control method for a dexterous slave robot arm, so that it can shuttle freely in a narrow space, helping operators to complete operating tasks efficiently and safely, adding information collection function and system positioning control method, and improving the effective obstacle avoidance of the dexterous robot; the terminal position adjustment and control method reduces the jitter of the terminal.

[0005] The technical solution of the present invention is:

[0006] A dexterous slave robot arm control system, comprising: a slave robot arm, a parameter module, a detection module, a control module, and a drive module;

[0007] The slave robot arm includes: an active segment and multiple slave segments; the root of the slave robot arm is the active segment, and the active segment is used to drive the slave segment to move; the multiple slave segments are connected in series in sequence, one end of the series connection is hinged to the active segment, and the other end is used as the end of the slave robot arm to connect external instruments and equipment;

[0008] Parameter module: saves the structural parameters of multiple different types of slave robot arms;

[0009] Detection module: used to detect the temperature, voltage, current, angular velocity, torque and position of the slave robot arm as detection information, and output it to the control module;

[0010] Control module: receives the detection information output by the detection module, performs motion control, jitter control, obstacle avoidance control and arm change control, and outputs control instructions to the drive module;

[0011] Drive module: receives the control instructions output by the control module, and drives the slave robot arm to perform motion control, jitter control or obstacle avoidance control according to the control instructions.

[0012] Preferably, it also includes: a communication module;

[0013] The communication module receives the expected position information instruction sent by the superior, and outputs the expected position information instruction to the control module, so that the control module outputs the control instruction to perform motion control.

[0014] Preferably, it also includes: an arm changing module;

[0015] The arm-changing module receives the arm-changing instruction sent by the superior and controls the arm-changing of the slave robot arm.

[0016] Preferably, the slave robot arm is divided into an execution arm and a manipulation arm according to the type of use, the execution arm is used to execute the operation task input by the superior, and the manipulation arm is used to illuminate the surrounding environment, collect remote sensing images and transmit them to the operator;

[0017] The actuator arm is composed of an active section and a driven section; the operating arm is composed of an active section and a driven section.

[0018] Preferably, it includes:

[0019] According to the task instructions sent by the superior and the detection information of the detection module, the driving module drives the slave robot arm to perform motion control;

[0020] During the movement of the slave robot arm, the control module outputs control instructions to the drive module to continuously perform obstacle avoidance control and jitter control on the slave robot arm.

[0021] Preferably, the control module continuously performs obstacle avoidance control on the slave robot arm, further comprising determining whether the slave robot arm collides, specifically:

[0022] Determine whether the space where the slave robot arm is located is inaccessible;

[0023] When the slave robot arm moves in a limited but accessible space, the external environment anti-collision force ΔF is compared with the force limit value 0.5N. If the external environment anti-collision force ΔF is greater than the force limit value 0.5N, it is determined that the slave robot arm has collided and obstacle avoidance control is required;

[0024] The slave robot arm moves in a limited space that cannot be touched. If any of the following conditions is met, it is judged that the slave robot arm has collided and obstacle avoidance control is required;

[0025] Condition 1: The minimum distance between the robot arm and the surrounding space is less than 2 mm;

[0026] Condition 2: The internal environment anti-collision force ΔF1 is greater than 0.1N.

[0027] Preferably, the method for determining the external environment anti-collision force ΔF is specifically as follows:

[0028]

[0029] in,

[0030] S mx is the target position given in the x-axis direction of the robot arm coordinate system, S sx is the actual position of the slave robot arm coordinate system in the x-axis direction;

[0031] S my is the target position given in the y-axis direction of the robot arm coordinate system, S sy is the actual position of the slave robot arm coordinate system in the y-axis direction;

[0032] S mz is the target position given in the z-axis direction of the robot arm coordinate system, S sz is the actual position of the slave robot arm coordinate system in the z-axis direction;

[0033] F x K is the x-axis force of the slave robot arm coordinate system detected by the detection module, x is the coefficient of the x-axis force in the slave robot arm coordinate system;

[0034] F y is the y-axis force of the slave robot arm in the slave robot arm coordinate system detected by the detection module, K y is the coefficient of the y-axis force in the slave robot arm coordinate system;

[0035] F z is the z-axis force of the slave robot arm coordinate system detected by the detection module, K z is the coefficient of the z-axis force in the slave robot arm coordinate system;

[0036] K x , K y and K z The value range of is 0-2;

[0037] Cx is the equivalent stiffness of the arm of the slave robot in the x-axis direction of the slave robot arm coordinate system, C y is the equivalent stiffness of the arm of the slave robot in the y-axis direction of the slave robot arm coordinate system, C z is the equivalent stiffness of the arm of the slave robot in the z-axis direction of the slave robot arm coordinate system;

[0038] The fixed hinge point between the driven segment and the active segment is taken as the coordinate origin O, the x-axis is perpendicular to the plane formed by the axes of the active segment and the driven segment, the y-axis and the z-axis are located in the plane formed by the axes of the active segment and the driven segment; the z-axis is orthogonal to the x-axis and the y-axis, forming the coordinate system of the slave robot arm.

[0039] Preferably, the method for determining the internal environment anti-collision force ΔF1 is specifically as follows:

[0040]

[0041] Among them, S x , S y , S z They are the position coefficients in the x-axis, y-axis and z-axis directions, and their value ranges are 0-1.

[0042] Preferably, the method in which the control module continuously performs shaking control on the slave robot arm is specifically as follows:

[0043] 11) Determine the structural parameters of the dexterous slave robot arm;

[0044] 12) Determine the motion trajectory of the slave robot arm according to the expected position information instruction in the task instruction output by the master control system and the current position of the slave robot arm; the control module controls the drive module to drive the slave robot arm to move, and when the slave robot arm executes the expected position information instruction and reaches the target position, enter step 13);

[0045] 13) The control module performs anti-shake determination. The control module determines whether the slave robot arm meets the jitter threshold through the angular velocity and position of the slave robot arm collected by the detection module. When the difference between the actual position and the target position of the slave robot arm is less than 0.2cm, and the measured angular velocity is less than 2° / s, it is determined that the jitter threshold is met, the slave robot arm moves to the position and enters the working state, and executes the subsequent task instructions; otherwise, enter step 14) to start the anti-shake program control;

[0046] 14) Calculate the difference ΔL between the actual position and the target position, as well as the angular velocity w; calculate the rotation position of the arm rod connection at the root of the slave robot arm through the difference ΔL, the angular velocity w and the rod length of the slave robot arm. When the rotation position of the root connection of the slave robot arm swings back and forth, change the position PID parameters and the speed PID parameters to reduce the jitter, and enter step 15); on the contrary, when the rotation position of the root connection of the slave robot arm remains unchanged and only the end jitters, the arm rod at the root of the slave robot arm is moved in the opposite direction, and the position of each movement is half of the difference ΔL. After each movement of half of the difference ΔL, determine again whether the jitter threshold is met, until the jitter threshold is met, and then enter step 15);

[0047] 15) Complete the jitter control of the expected position information instructions and execute subsequent task instructions.

[0048] Compared with the prior art, the advantages of the present invention are mainly reflected in the following aspects:

[0049] 1. The present invention improves the flexibility of the slave hand, improves accuracy and safety, improves control accuracy, prevents hand shaking and other misoperations, avoids the slave hand robot arm from moving around due to the lack of a slave hand control system after losing instructions when the master hand robot arm control system fails, and designs an obstacle avoidance function to reduce damage. The operation logic of the control logic operation system is improved, the operation flexibility of the robot arm controlled by the control system is enhanced, and safety is improved.

[0050] 2. The present invention designs a control module, which has the control functions of positioning the control system from the hand, adjusting the posture of the end and preventing shaking. It detects the status information of the module through parameters, and automatically detects the status of the robot according to the status information, automatically monitors whether the robot system is abnormal in real time, and promptly discovers the abnormality of the robot system, reduces unplanned downtime and misoperation during the operation of the robot, and provides a reference for the maintenance of the robot system. In addition, the detection method is simple, and the detection efficiency and detection accuracy are high. It solves the technical problems of not being able to adjust the robot's moving path in real time and not being able to manually intervene in the robot's moving path, thereby reducing the risk in operation and improving safety. This control method can accurately control the posture of the robot's end and improve accuracy and safety.

[0051] 3. The present invention designs a control module and adds an obstacle avoidance control method to determine whether a collision will occur between any two unconnected structures, between any arm or between the arm and the operating space, and to avoid obstacles, thereby improving accuracy and efficiency.

[0052] 4. The present invention designs a parameter detection module, which encapsulates various structural parameters of the dexterous robot, can provide structural parameters and data parameters, provides reliable data for protection and control, and improves reliability. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 Schematic diagram of a control system of a dexterous robot arm in one embodiment of the present invention.

[0054] Figure 2 This is a schematic diagram of the drive module.

[0055] Figure 3 Schematic diagram of the protection mechanism module.

[0056] Figure 4 The control method flow chart of the slave control system.

[0057] Figure 5 This is a flow chart of the arm change control method.

[0058] Figure 6 The figure is a flow chart of the protection mechanism. DETAILED DESCRIPTION

[0059] The following detailed description of the present invention will make the features and advantages of the present invention more clear and explicit.

[0060] The present invention provides a control system, control and protection method for a dexterous robot arm, which mainly receives commands from a master control system through a communication module, calls the structural parameters of the robot arm, selects a control module to output corresponding control instructions to a drive module, drives and controls the dexterous robot through the drive module, and performs protection control through a protection mechanism module, and stores data at the same time. This control system improves the flexibility of the slave hand, improves accuracy and safety, improves control precision, prevents erroneous operations such as hand shaking, and avoids the slave hand robot arm from moving randomly after losing instructions due to the lack of a slave hand control system when the master hand robot arm control system fails.

[0061] like Figure 1 As shown, a dexterous slave robot arm control system includes: a communication module, a robot arm, a parameter module, a detection module, an arm change module, a storage module, a control module, a drive module and a protection mechanism module;

[0062] The master hand control system sends expected position information instructions and arm change instructions to the slave hand control system, and the slave hand control system controls the movement of the dexterous robot arm.

[0063] The robot arm includes: an active segment and multiple driven segments; the root of the robot arm must be the active segment, which is used to drive the driven segments to move; the multiple driven segments are connected in series in sequence, one end of the series connection is hinged to the active segment, and the other end is the end of the robot arm, connected to external equipment;

[0064] The parameters in the parameter module include: structural parameters of multiple different types of robot arms. Parameter module: used to provide the control module with structural parameters of different types of robot arms; the parameter module is mainly used to establish the structural parameters, functional types, construction characteristics, performance characteristics and other data of the dexterous robot arm, and encapsulate the parameters of the arms of different dexterous robots in the parameter module for the slave control system to call, providing reliable structural parameter data for the dexterous robot.

[0065] Detection module: detects the temperature, voltage, current, angular velocity, torque and position of the robot arm as detection information, and outputs it to the control module;

[0066] The detection module includes: temperature detection unit, voltage detection unit, current detection unit, speed detection unit, torque detection unit and position detection unit. It is mainly used to detect and pre-judge the fault of the robot arm, provide fault processing data for the slave main control system, ensure the accurate performance of the slave control system fault processing, and improve the reliability of the system; at the same time, it provides reliable parameter self-adjustment data for the slave intelligent control, ensures the parameter accuracy of each joint of the robot system, reduces the parameter debugging time, and improves the intelligence and reliability of the servo system.

[0067] Control module: Receives the temperature, voltage, current, angular velocity, torque and position of the robot arm output by the detection module, uses the control algorithm to perform motion control, jitter control, obstacle avoidance control and arm change control, and outputs control instructions to the drive module. Each robot arm's slave control system is provided with a corresponding control module. The control algorithms in the control module include: slave system motion jitter control method, fast arm change control method, slave obstacle avoidance control method, slave safety protection mechanism control algorithm and motion control algorithm; mainly encapsulates the kinematic and dynamic analysis and analysis of the multi-arm of the dexterous robot, the coordinate transformation of the mirror holding arm, the force feedback operation and the control drive algorithm, and according to different application scenarios of the operation, the slave control system performs arm change and control algorithm call, which improves the reliability in limited operating environments, increases the intelligence of the system, and enhances safety and practicality.

[0068] The storage module mainly stores file reading and work logs, various test results and failure modes, which is conducive to data retrieval, ensures data traceability, and serves as a reference for subsequent troubleshooting and control.

[0069] Driving module: receives the control instructions output by the control module, and drives the robot arm to perform motion control, jitter control, obstacle avoidance control and arm change control according to the control instructions. Each robot arm is provided with a corresponding driving module; in one embodiment of the present invention, three groups of driving units are provided, and each group of driving units corresponds to a different robot arm. Figure 2The embodiment of the present invention shown mainly includes three groups of driving units. Each group of driving units mainly controls the motor according to data solution and motor control thread, so that the dexterous robot moves along the specified route, thereby improving the servo performance of the robot arm, improving the real-time performance and reliability, being beneficial to the coordinated control of each arm, and improving the obstacle avoidance capability.

[0070] Protection mechanism module: Receives detection information output by the detection module, and the protection mechanism module performs alarm detection. If normal, it forwards the detection information to the control module; otherwise, the protection mechanism module outputs warning information of over-current, over-voltage and over-temperature of the robot arm, and the control module generates a drive instruction and outputs it to the drive module. Figure 3 As shown, the protection mechanism module includes: overcurrent protection unit, overvoltage protection unit, electromagnetic protection unit, overtemperature protection unit, collision protection unit, overforce protection unit, and position offset protection unit. It mainly uses the data of the detection module and the information of the endoscope (the endoscope is fixedly installed at the end of the operating arm) to perform obstacle avoidance, anti-shake and other protection mechanisms, control and process various faults, protect the entire system, and improve the reliability and safety performance of the hand control system.

[0071] The communication module receives the expected position information instruction sent by the superior master control system, and outputs the expected position information instruction to the control module in the slave control system, which is used for the control module to output the control instruction and perform motion control. It mainly includes CAN communication, EtherCAT communication, Ethernet communication and 5G wireless communication, etc. These communications are mainly used for communication between the master control system and the slave control system, and receive the expected position information instruction of the master control system; each time data is transmitted, multiple communication modes will be used to compare data instructions, so as to execute operations, avoid operational failures caused by communication, and improve the safety, reliability, real-time and stability of the system.

[0072] The arm-changing module receives the arm-changing instructions sent by the superior master arm control system and actual work requirements to control the arm-changing of the robot arm.

[0073] The robot arms are divided into execution arms and operating arms according to their usage. The execution arms are used to execute the operation tasks input by the superiors, and the operating arms are used to illuminate the surrounding environment, collect remote sensing images and transmit them to the operators to ensure progress and reliability. The three robot arms in one embodiment of the present invention are composed of two execution arms and one operating arm.

[0074] The actuator arm is composed of an active section and a driven section; the operating arm is composed of an active section and a driven section.

[0075] The arm that can actively move in the execution arm and the operating arm is the active segment, and the arm that follows the active segment in the execution arm and the operating arm is the driven segment. And the root of the execution arm and the operating arm must be the active segment.

[0076] The control method and process of the manual control system, such as Figure 4 As shown:

[0077] 1) After the system is powered on, the manual control system performs parameter self-check, including but not limited to position parameters, temperature parameters, voltage parameters, current parameters, etc.;

[0078] 2) According to the parameter self-check result, compare it with the preset parameters in the storage module of the system to see if it is normal. If it is normal, go to step 3) If it is not normal, check the cause of the error. The system determines whether it can be repaired according to the storage parameters. If it can be repaired, repair it and repeat step 2). If it cannot be repaired, stop the system and replace the spare equipment;

[0079] 3) Check the communication channel and determine the communication mode; the communication modes include: CAN communication, EtherCAT communication, Ethernet communication and 5G wireless communication; the master control system sends task instructions (expected position information instructions and arm change instructions) to the slave control system through any two of the communication modes.

[0080] 4) After receiving the command sent by the master hand control system, the slave hand control system checks whether the data obtained by the two communication methods are consistent. If they are consistent, it goes to step 6); if the data obtained by the two communication methods are inconsistent, it goes to step 5);

[0081] 5) Feedback the inconsistent data result to the master control system and record the number of times plus 1. If the number of times is less than or equal to 3, proceed to step 6). If it is greater than 3, prompt the master control system to change the current two communication methods to other two communication methods for communication, and return to step 4). If the data is inconsistent after traversing all communication methods, stop the machine for inspection;

[0082] 6) Entering the operating state, the slave control system transmits self-test information to the master control system, and the master control system uses two stable communications to send task instructions (task instructions include: position information, shutdown instructions, etc.);

[0083] 7) The detection module of the slave hand control system collects the detection information of the robot arm, stores the detection information and transmits it back to the master hand control system;

[0084] 8) According to the task instructions and the detection information of the detection module, the robot arm is driven to execute the task instructions. During the execution of the task instructions by the robot arm, the control module of the hand control system continuously performs obstacle avoidance control, jitter control, obstacle avoidance control, arm change control and safety protection control on the robot arm, and outputs control instructions to the drive module.

[0085] The method for controlling the robot arm to avoid obstacles is as follows:

[0086] 1) Before performing the operation, determine the number of robot arms. Generally, it can be 1, 2, 3 or more;

[0087] 2) Determine the structural parameters of the dexterous robot arm, match the parameter type prepared in advance in the parameter detection module through the practical type of the arm, and determine various parameters of the arm;

[0088] 3) When performing the operation, determine the current position of each arm of the dexterous robot (through video information, position information, etc.), and determine whether there is an external environment. If there is no external environment (such as the robot arm moves in a limited space that is not afraid of contact, etc.), proceed to step 4). If there is an environment with obstacles (such as the robot arm moves in a limited space that is not required to be in contact, etc.), proceed to step 6);

[0089] 4) If there is no external environment, the hand control system determines the motion trajectory of the dexterous robot arm according to the structural parameters and expected position information instructions. The control module controls the robot arm to move according to the motion trajectory. At the same time, during the movement process, the control cycle loop is used to determine whether a collision occurs in real time according to the external environment anti-collision force ΔF. If it is determined that a collision occurs, then go to step 5). If no collision occurs, move according to the current motion trajectory until the task instruction is completed. The method for determining whether a collision occurs is specifically: compare the external environment anti-collision force ΔF obtained by solving formula 1 with the force limit value 0.5N. If the external environment anti-collision force ΔF is greater than the force limit value 0.5N, then go to step 5);

[0090] 5) Adjust the coefficient of each axial force of the robot arm to reduce the collision force and prevent collisions. By adjusting the force, collisions can be prevented.

[0091] As shown in Formula 1, the value of the external environment anti-collision force ΔF is calculated through the current position information and the target position:

[0092]

[0093] in,

[0094] S mx is the target position of the robot arm in the x-axis direction corresponding to each sampling point in the control cycle, S sx is the actual position of the robot arm in the x-axis direction; the given target position is determined by the motion trajectory;

[0095] S my is the target position of the robot arm in the y-axis direction corresponding to each sampling point in the control cycle, S sy is the actual position of the robot arm in the y-axis direction;

[0096] S mzis the target position of the robot arm in the z-axis direction corresponding to each sampling point in the control cycle, S sz is the actual position of the robot arm in the z-axis direction;

[0097] F x is the x-axis force on the robot arm detected by the detection module, K x is the coefficient of the x-axial force;

[0098] F y is the y-axis force on the robot arm detected by the detection module, K y is the coefficient of the y-axis force;

[0099] F z is the z-axis force on the robot arm detected by the detection module, K z is the coefficient of z-axis force;

[0100] K x , K y and K z The value range is 0-2;

[0101] C x is the equivalent stiffness of the robot arm in the x-axis direction, C y is the equivalent stiffness of the robot arm in the y-axis direction, S z is the equivalent stiffness of the robot arm in the z-axis direction;

[0102] The hinge point of the fixed end of the driven section is used as the coordinate origin O, the X axis is perpendicular to the plane formed by the axes of the active section and the driven section, the Y axis and the Z axis are located in the plane formed by the axes of the active section and the driven section; the Z axis is orthogonal to the X axis and the Y axis to form a coordinate system. The active section and the driven section are hard-connected by flanges.

[0103] 6) If there is an external environment, the motion trajectory of the dexterous robot arm corresponding to the external environment is determined according to the structural parameters and the expected position information command output by the main hand control system. The control module controls the robot arm to move according to the motion trajectory. At the same time, during the movement process, it is judged in real time according to the feedback information of the endoscope and the torque feedback information output by the detection module (i.e., the anti-collision force ΔF1 under the internal environment) according to the control cycle cycle. If it is judged that a collision occurs, the obstacle avoidance program in the environment is entered. If no collision occurs, the robot arm moves according to the current command trajectory.

[0104] The method for determining the occurrence of a collision in step 6) is: if any one of the following conditions is met, a collision is determined to have occurred, and an obstacle avoidance program is entered in the environment;

[0105] Condition 1: When the feedback information from the endoscope shows that the minimum distance between the robot arm and the surrounding objects is less than 2 mm;

[0106] Condition 2: The internal environment anti-collision force ΔF1 is greater than 0.1N.

[0107] As shown in Formula 2, this is the solution formula for the internal environment collision force ΔF1,

[0108]

[0109] In the formula 2, S x , S y , S z are the position coefficients in the x-axis, y-axis and z-axis directions respectively, ensuring that the position is within the internal environment collision limit. In formula 2, this coefficient S x , S y , S z The value range is 0-1. ΔF1 is the inner limit edge collision force feedback. The driving force from the end of the hand is not greater than ΔF1.

[0110] The method for controlling the vibration of the robot arm is as follows:

[0111] 11) Determine the structural parameters of the dexterous robot arm, match the parameter type prepared in advance in the parameter detection module through the type of the robot arm, and determine various parameters of the robot arm;

[0112] 12) Determine the motion trajectory of the dexterous robot arm according to the expected position information instruction in the task instruction output by the master hand control system and the current position of the robot arm; the control module controls the drive module to drive the robot arm to move, and when the robot arm executes the expected position information instruction and reaches the target position, enter step 13);

[0113] 13) The control module performs anti-shake determination. The control module determines whether each robot arm meets the jitter threshold through the angular velocity and position of the robot arm collected by the detection module. When the difference between the actual position and the target position of each robot arm is less than 0.2cm, and the measured angular velocity value is less than 2° per second, it is determined that the jitter threshold is met, the robot arm moves to the position and enters the working state, and executes the subsequent task instructions; otherwise, enter step 14) to start the anti-shake program control (motion jitter control does not follow the control cycle cycle, and each execution is performed once, after the movement is in place, only one determination is performed);

[0114] 14) First, calculate the difference ΔL between the actual position and the target position, as well as the angular velocity w. The rotation position of the arm rod connection at the root of the robot arm is converted through the difference ΔL, the angular velocity and the rod length of the robot arm. When the rotation position of the root connection of the robot arm swings back and forth, change the position PID parameters and the speed PID parameters to achieve the effect of reducing the jitter, and enter step 15); on the contrary, when the rotation position of the root connection of the robot arm remains unchanged and only the end jitters, it means that the inertial jitter is caused by the arm rod lead length. At this time, the arm rod at the root of the robot arm is moved in the opposite direction, and the position of each movement is half of the difference ΔL. After each movement of half of the difference ΔL, it is determined again whether the jitter threshold is met, until the jitter threshold is met, and then enter step 15);

[0115] 15) Execute subsequent task instructions.

[0116] like Figure 5 As shown, a method for quickly changing the arm of a robot is provided;

[0117] 21) The control module receives the arm change command sent by the master hand control system;

[0118] 22) The control module determines the current position of each arm of the dexterous robot (e.g., through video information, position information, etc.);

[0119] 23) Use the detection module to obtain detection information, calculate the arm's withdrawal path LS1 based on the detection information and the structural parameters of the robot arm, and calculate the difference compensation correction parameter S i ;

[0120] The difference compensation correction parameter S corresponding to the i-th control cycle i As shown in Formula 3, Formula 3 takes three robot arms as an example:

[0121]

[0122] In the formula, S i The distance between the actual position and the target position of each arm in the i-th control cycle of the arm. The unit is mm.

[0123] In formula 3, 1, 2, and 3 represent the numbers of multiple robotic arms that work independently of each other; then,

[0124] S 1i is the actual position of the first arm corresponding to the i-th control cycle;

[0125] β 1i is the position adjustment coefficient of the first arm corresponding to the i-th control cycle, β 1i The value range is 0-2.5;

[0126] is the retraction force corresponding to the i-th control cycle of the first arm (i.e., the driving force output by the driving module);

[0127] C 1i is the retraction angle corresponding to the ith control cycle of the first arm (measured value of the detection module, the angle of the robot arm around the fixed end).

[0128] 24) Use the corresponding difference correction parameter S of the i-th control cycle i , determine whether the difference between the actual position and the target position of the robot arm is greater than 2mm. If the difference is greater than 2mm, enter step 25) and perform real-time compensation processing on LS1 (LS1 represents the trajectory curve); otherwise, return to step 23) to enter the next control cycle until the robot arm is withdrawn to the initial state outside the body, and then enter step 26);

[0129] 25) Correct the parameter S according to the difference compensation i , move the robot arm to the target position of the current i-th control cycle, return to step 23) to enter the next control cycle, until the robot arm is withdrawn to the initial state outside the body, and then enter step 26);

[0130] 26) Retract to the initial state outside the body according to the withdrawal path LS1, complete the arm change process, and continue to perform subsequent tasks.

[0131] like Figure 6 As shown, the method for controlling the safety protection mechanism of the robot arm is specifically as follows:

[0132] 31) During the movement, the movement information of each arm is detected and fed back to the slave control system;

[0133] 32) Compare the information with the stored theoretical value to determine which protection is triggered; select the appropriate protection unit in the protection mechanism module. All protection units are as follows: Figure 6 As shown;

[0134] 33) When it is detected that the current exceeds a certain limit, the limit is 2A at this time, then the overcurrent protection mechanism will be triggered. If the current is detected three times in a row and overcurrent occurs, the machine will be stopped for inspection, otherwise it will continue to work.

[0135] 34) When over-voltage, over-force, over-temperature or electromagnetic trigger threshold is detected, it is handled in the same way as over-current;

[0136] 35) When collision protection is triggered, the above-mentioned obstacle avoidance process is performed;

[0137] 36) When position deviation occurs, the target position and actual position are compared and detected, and position compensation is performed for the position deviation.

[0138] Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solution of the present invention by using the above disclosed methods and technical contents without departing from the spirit and scope of the present invention. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention shall belong to the protection scope of the technical solution of the present invention. In the absence of conflict, the embodiments of the present application and the technical features in the embodiments can be combined with each other.

[0139] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A dexterous slave robot arm control system, characterized in that: include: From hand robot arm, parameter module, detection module, control module, drive module; The slave robot arm includes: an active segment and multiple slave segments; the root of the slave robot arm is the active segment, and the active segment is used to drive the slave segment to move; the multiple slave segments are connected in series in sequence, one end of the series connection is hinged to the active segment, and the other end is used as the end of the slave robot arm to connect external instruments and equipment; Parameter module: saves the structural parameters of multiple different types of slave robot arms; Detection module: used to detect the temperature, voltage, current, angular velocity, torque and position of the slave robot arm as detection information, and output it to the control module; Control module: receives the detection information output by the detection module, performs motion control, jitter control, obstacle avoidance control and arm change control, and outputs corresponding control instructions to the drive module; Drive module: receives the control instructions output by the control module, and drives the slave robot arm to perform motion control, jitter control or obstacle avoidance control according to the control instructions.

2. The dexterous slave-hand robot arm control system according to claim 1 is characterized in that: Also includes: Communication module; The communication module receives the expected position information instruction sent by the superior, and outputs the expected position information instruction to the control module, so that the control module outputs the control instruction to perform motion control.

3. The dexterous slave-hand robot arm control system according to claim 2 is characterized in that: Also includes: Arm change module; The arm-changing module receives the arm-changing instruction sent by the superior and controls the arm-changing of the slave robot arm.

4. A dexterous slave-hand robot arm control system according to any one of claims 1 to 3, characterized in that: The robot arm is divided into execution arm and operation arm according to the use type. The execution arm is used to execute the operation task input by the superior, and the operation arm is used to illuminate the surrounding environment, collect remote sensing images and transmit them to the operator. The actuator arm is composed of an active section and a driven section; the operating arm is composed of an active section and a driven section.

5. A dexterous slave-hand robot arm control method implemented by a dexterous slave-hand robot arm control system as claimed in claim 4, characterized in that: include: According to the task instructions sent by the superior and the detection information of the detection module, the driving module drives the slave robot arm to perform motion control; During the movement of the slave robot arm, the control module outputs control instructions to the drive module to continuously perform obstacle avoidance control and jitter control on the slave robot arm.

6. The method for controlling a dexterous slave-hand robot arm according to claim 5, characterized in that: The control module continuously performs obstacle avoidance control on the slave robot arm, and further includes determining whether the slave robot arm collides, specifically: Determine whether the space where the slave robot arm is located is inaccessible; When the slave robot arm moves in a limited but accessible space, the external environment anti-collision force ΔF is compared with the force limit value 0.5N. If the external environment anti-collision force ΔF is greater than the force limit value 0.5N, it is determined that the slave robot arm has collided and obstacle avoidance control is required; The slave robot arm moves in a limited space that cannot be touched. If any of the following conditions is met, it is judged that the slave robot arm has collided and obstacle avoidance control is required; Condition 1: The minimum distance between the robot arm and the surrounding space is less than 2 mm; Condition 2: The internal environment anti-collision force ΔF1 is greater than 0.1N.

7. The method for controlling a dexterous slave-hand robot arm according to claim 6, characterized in that: The method for determining the external environment anti-collision force ΔF is specifically as follows: in, S mx is the target position given in the x-axis direction of the robot arm coordinate system, S sx is the actual position of the slave robot arm coordinate system in the x-axis direction; S my is the target position given in the y-axis direction of the robot arm coordinate system, S sy is the actual position of the slave robot arm coordinate system in the y-axis direction; S mz is the target position given in the z-axis direction of the robot arm coordinate system, S sz is the actual position of the slave robot arm coordinate system in the z-axis direction; F x K is the x-axis force of the slave robot arm coordinate system detected by the detection module, x is the coefficient of the x-axis force in the slave robot arm coordinate system; F y is the y-axis force of the slave robot arm coordinate system detected by the detection module, K y is the coefficient of the y-axis force in the slave robot arm coordinate system; F z is the z-axis force of the slave robot arm coordinate system detected by the detection module, K z is the coefficient of the z-axis force in the slave robot arm coordinate system; K x , K y and K z The value range of is 0-2; C x is the equivalent stiffness of the arm of the slave robot in the x-axis direction of the slave robot arm coordinate system, C y is the equivalent stiffness of the arm of the slave robot in the y-axis direction of the slave robot arm coordinate system, C z is the equivalent stiffness of the arm of the slave robot in the z-axis direction of the slave robot arm coordinate system; The fixed hinge point between the driven segment and the active segment is taken as the coordinate origin O, the x-axis is perpendicular to the plane formed by the axes of the active segment and the driven segment, the y-axis and the z-axis are located in the plane formed by the axes of the active segment and the driven segment; the z-axis is orthogonal to the x-axis and the y-axis, forming the coordinate system of the slave robot arm.

8. The method for controlling a dexterous slave-hand robot arm according to claim 7, characterized in that: The method for determining the internal environment anti-collision force ΔF1 is specifically as follows: Among them, S x , S y , S z They are the position coefficients in the x-axis, y-axis and z-axis directions, and their value ranges are 0-1.

9. The method for controlling a dexterous slave-hand robot arm according to claim 5, characterized in that: The method for the control module to continuously control the vibration of the slave robot arm is as follows: 11) Determine the structural parameters of the dexterous slave robot arm; 12) Determine the motion trajectory of the slave robot arm according to the expected position information instruction in the task instruction output by the master control system and the current position of the slave robot arm; the control module controls the drive module to drive the slave robot arm to move, and when the slave robot arm executes the expected position information instruction and reaches the target position, enter step 13); 13) The control module performs anti-shake determination. The control module determines whether the slave robot arm meets the jitter threshold through the angular velocity and position of the slave robot arm collected by the detection module. When the difference between the actual position and the target position of the slave robot arm is less than 0.2cm, and the measured angular velocity is less than 2° / s, it is determined that the jitter threshold is met, the slave robot arm moves to the position and enters the working state, and executes the subsequent task instructions; otherwise, enter step 14) to start the anti-shake program control; 14) Calculate the difference ΔL between the actual position and the target position, as well as the angular velocity w; calculate the rotation position of the arm rod connection at the root of the slave robot arm through the difference ΔL, the angular velocity w and the rod length of the slave robot arm. When the rotation position of the root connection of the slave robot arm swings back and forth, change the position PID parameters and the speed PID parameters to reduce the jitter, and enter step 15); on the contrary, when the rotation position of the root connection of the slave robot arm remains unchanged and only the end jitters, the arm rod at the root of the slave robot arm is moved in the opposite direction, and the position of each movement is half of the difference ΔL. After each movement of half of the difference ΔL, determine again whether the jitter threshold is met, until the jitter threshold is met, and then enter step 15); 15) Complete the jitter control of the expected position information instructions and execute subsequent task instructions.

Images