Dexterous slave robot arm control system and control method

Abstract

A kind of dexterous slave robot arm control system and control method, comprising: parameter module is saved with the structural parameters of multiple different types robot arm;Detection module is used to detect the detection information of robot arm;Control module selects control algorithm, and outputs control instruction to drive module;Drive module receives the control instruction output by control module, and according to control instruction, drive robot arm to carry out motion control, dither control, obstacle avoidance control and arm change control.The present application improves the flexibility of robot arm, improves the control accuracy and safety of robot arm, improves the control accuracy, prevents human operation hand shake and other misoperation, and also avoids the random movement of slave robot arm when the master robot arm control system fails and loses instructions, thereby reducing the damage to the surrounding operating environment.

Description

TECHNICAL FIELD

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

[0002] With the rapid development of robot technology, its application field is increasingly expanding. The execution component of the robot and the working environment have a very close effect, and the dexterous robot as an end effector affects the improvement of the robot intelligence and the operation level. The robot arm system usually uses master-slave control mode: when the operator operates the master hand, the hand movement will drive the master hand to move, and the sensor at the joint of the master hand can measure the movement information, and the movement of the master hand is mapped to the slave hand through master-slave control, and the slave hand moves, and the instrument is driven to realize corresponding movement.

[0003] The existing traditional robot arm uses master-slave hand mode as the main working mode, and the flexibility and degree of freedom of the slave robot are not high, most of which rely on the master hand to drive, and the slave hand is an instrument for entering the operation space and operating. At present, for the master-slave hand mode, the master hand drives the slave hand to operate some actions, and the control system of the multi-joint slave robot is not much researched, and the degree of freedom of the slave hand is not high, and the obstacle avoidance and anti-shake ability and protection mechanism of the slave hand control system are not mature and reliable, which are not conducive to the operation of the slave hand in a small space. SUMMARY

[0004] The technical problem solved by the present application is to overcome the shortcomings of the prior art and provide a dexterous slave robot arm control system and control method, which can freely shuttle in a small space, help the operator to complete the operation task efficiently and safely, increase the information acquisition function and the control method of system positioning, and improve the effective obstacle avoidance of the dexterous robot; the end position posture adjustment and control method reduces the shaking of the end.

[0005] The technical solution of the present application 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 driving module.

[0007] The slave robot arm comprises: an active segment and a plurality of driven segments; the root of the slave robot arm is the active segment, and the active segment is used to drive the driven segments to move; the plurality of driven segments are connected in series, one end of the series is hinged to the active segment, and the other end is connected to an external instrument device as the end of the slave robot arm.

[0008] The parameter module: saves the structure parameters of a plurality of different types of slave robot arms;

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

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

[0011] Drive module: Receives control commands from the control module and drives the slave robot arm to perform motion control, jitter control, or obstacle avoidance control according to the control commands.

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

[0013] The communication module receives the expected position information command sent by the superior and outputs the expected position information command to the control module, which then outputs control commands to perform motion control.

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

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

[0016] Preferably, the hand robot arm is divided into an execution arm and a manipulator arm according to its purpose. The execution arm is used to perform the operation tasks input by the superior, and the manipulator arm is used to illuminate the surrounding environment, collect remote sensing images and transmit them to the operator.

[0017] The actuator arm consists of an active section and a driven section; the control arm consists of an active section and a driven section.

[0018] Preferably, it includes:

[0019] Based on the task instructions sent by the superior and the detection information from the detection module, the drive module drives the slave robot arm to perform motion control.

[0020] During the movement of the slave robot arm, the control module outputs control commands to the drive module to continuously control obstacle avoidance and vibration of the slave robot arm.

[0021] Preferably, the process of the control module continuously performing obstacle avoidance control on the slave robot arm also includes determining whether the slave robot arm has collided, specifically:

[0022] Determine whether the space where the robotic arm is located is unreachable;

[0023] If the robotic arm moves within a limited but accessible space, the collision avoidance force ΔF of the external environment is compared with the force limit value of 0.5N. If the collision avoidance force ΔF of the external environment is greater than the force limit value of 0.5N, it is determined that the robotic arm has collided and obstacle avoidance control is required.

[0024] If the robotic arm moves within a limited space where it is required to avoid contact, then if any of the following conditions are met, it is determined that the robotic arm has collided and obstacle avoidance control is required.

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

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

[0027] Preferably, the method for determining the external environmental collision avoidance force ΔF is as follows:

[0028]

[0029] in,

[0030] S mx Given the target position along the x-axis of the hand robot arm coordinate system, S sx This refers to the actual position of the robot arm in the x-axis direction of the hand robot's coordinate system;

[0031] S my Given the target position along the y-axis of the hand robot arm coordinate system, S sy This refers to the actual position of the robot arm in the y-axis direction of the hand robot's coordinate system;

[0032] S mz Given the target position along the z-axis of the hand robot arm coordinate system, S sz This refers to the actual position of the robot arm in the z-axis direction of the hand robot's coordinate system;

[0033] F x K represents the force along the x-axis of the slave robot arm coordinate system detected by the detection module. x This is the coefficient of the force along the x-axis of the robot arm coordinate system;

[0034] F y K represents the force along the y-axis of the slave robot arm coordinate system detected by the detection module. y This is the coefficient of the force along the y-axis of the robot arm's coordinate system;

[0035] F z K represents the force along the z-axis of the slave robot arm coordinate system detected by the detection module. z This is the coefficient of the force along the z-axis of the robot arm's coordinate system;

[0036] K x K y and K z The values ​​of all values ​​are in the range of 0-2;

[0037] Cx C represents the equivalent stiffness of the slave robot arm lever in the x-axis direction of the slave robot arm coordinate system. y C represents the equivalent stiffness of the robotic arm lever in the y-axis direction of the robotic arm coordinate system. z Let be 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 and driven segments is taken as the origin O of the coordinate system. The x-axis is perpendicular to the plane formed by the axes of the driven and driven segments, and the y-axis and z-axis are located in the plane formed by the axes of the driven and driven segments. The z-axis is orthogonal to the x-axis and y-axis, forming the coordinate system of the robot arm.

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

[0040]

[0041] Among them, S x S y S z These are the position coefficients for the x-axis, y-axis, and z-axis, respectively, with values ​​ranging from 0 to 1.

[0042] Preferably, the method by which the control module continuously controls the shaking of the slave robot arm is as follows:

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

[0044] 12) Determine the motion trajectory of the slave robot arm based on the expected position information command in the task command 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 completes the expected position information command and reaches the target position, proceed to step 13);

[0045] 13) The control module performs anti-shake judgment. The control module uses the angular velocity and position of the slave robot arm collected by the detection module to determine whether the slave robot arm meets the shake threshold. 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 shake threshold is met, the slave robot arm moves into position and enters the working state, and executes subsequent task instructions; otherwise, proceed to step 14) to start the anti-shake program control.

[0046] 14) Calculate the difference ΔL between the actual position and the target position, and the angular velocity w; using the difference ΔL, the angular velocity w, and the length of the arm of the hand robot, calculate the rotational position of the arm connection at the root of the hand robot arm. If the rotational position at the root connection of the hand robot arm swings back and forth, change the parameters of the position PID and the speed PID to reduce the jitter, and proceed to step 15); conversely, if the rotational position at the root connection of the hand robot arm remains unchanged and only the end effector jitters, move the arm at the root of the hand robot arm in the opposite direction. Each movement is half the difference ΔL. After each movement of half the difference ΔL, determine again whether the jitter threshold is met, until the jitter threshold is met, and then proceed to step 15).

[0047] 15) Complete the jitter control of the expected location information command and execute subsequent task commands.

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

[0049] 1. This invention improves the dexterity, accuracy, and safety of the slave robot arm, enhances control precision, prevents misoperations such as hand tremors, and avoids erratic movement of the slave robot arm due to the lack of a slave control system when the master robot arm control system malfunctions. An obstacle avoidance function is designed to reduce injury. The invention also improves the computational logic of the control logic system, enhancing the operational flexibility of the robot arm controlled by the control system and improving safety.

[0050] 2. This invention designs a control module that features positioning from the hand-controlled system and end-effector posture adjustment and anti-shake functions. By detecting the status information from the parameter detection module, it automatically detects the robot's status and monitors the robot system for anomalies in real time. This timely detection of anomalies reduces unplanned downtime and misoperation during robot operation, providing a reference for robot system maintenance. Furthermore, the detection method is simple, efficient, and accurate. It solves the technical problems of not being able to adjust the robot's movement path in real time and not being able to manually intervene in the robot's movement path, thereby reducing operational risks and improving safety. This control method can precisely control the robot's end-effector posture, improving accuracy and safety.

[0051] 3. This invention designs a control module and adds an obstacle avoidance control method to determine whether a collision will occur between any two unconnected structures, such as between the various booms or between the boom and the operating space, and performs obstacle avoidance, thereby improving accuracy and efficiency.

[0052] 4. This invention designs a parameter detection module that encapsulates all the structural parameters of the hand-held dexterous robot together, providing structural and data parameters, thus providing reliable data for protection and control and improving reliability. Attached Figure Description

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

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

[0055] Figure 3 This is a schematic diagram of the protection mechanism module.

[0056] Figure 4 This is a flowchart of the control method for a manual control system.

[0057] Figure 5 This is a flowchart of the arm-changing control method.

[0058] Figure 6 Flowchart for the protection mechanism. Detailed Implementation

[0059] The features and advantages of the present invention will become clearer and more apparent from the following detailed description.

[0060] This invention discloses a control system, control method, and protection mechanism for a dexterous robotic arm. The system primarily receives commands from the master control system via a communication module, retrieves the robotic arm's structural parameters, selects the control module to output corresponding control instructions to the drive module, and then uses the drive module to control the dexterous robot. Simultaneously, a protection mechanism module provides protection control, and the data is stored. This control system improves the dexterity, accuracy, and safety of the slave arm, enhances control precision, prevents misoperations such as hand tremors, and avoids erratic movement of the slave arm due to the lack of a slave control system when the master control system malfunctions.

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

[0062] The master control system sends the expected position information command and the arm-changing command to the slave control system, and the slave control system controls the movement of the dexterous robot arm.

[0063] The robotic arm consists of an active segment and multiple driven segments. The root of the robotic arm must be the active segment, which drives the driven segments. The multiple driven segments are connected in series, with one end of the series connected to the active segment and the other end serving as the end of the robotic arm, connecting to external machinery and equipment.

[0064] The parameters in the parameter module include structural parameters for multiple different types of robot arms. The parameter module provides structural parameters for different types of robot arms to the control module. It primarily establishes data on the structural parameters, functional types, construction features, and performance characteristics of dexterous robot arms, encapsulating the parameters of each different dexterous robot arm within the parameter module for use by the slave control system, thus providing reliable structural parameter data for the dexterous robot.

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

[0066] The detection module includes: a temperature detection unit, a voltage detection unit, a current detection unit, a speed detection unit, a torque detection unit, and a position detection unit. It is primarily used to detect and pre-judge robot arm faults, providing fault handling data to the slave control system, ensuring the accuracy of fault handling in the slave control system, and improving system reliability. Simultaneously, it provides reliable parameter self-adjustment data for the slave intelligent control, ensuring the accuracy of parameters at each joint of the robot system, reducing parameter debugging time, and improving the intelligence and reliability of the servo system.

[0067] The control module receives data from the detection module on the robot arm's temperature, voltage, current, angular velocity, torque, and position. It then uses control algorithms for motion control, jitter control, obstacle avoidance control, and arm-switching control, outputting control commands to the drive module. Each robot arm's slave control system has a corresponding control module. The control algorithms in the control module include: slave system motion jitter control method, rapid arm-switching control method, slave obstacle avoidance control method, slave safety protection mechanism control algorithm, and motion control algorithm. It primarily encapsulates the kinematics and dynamics analysis of the multi-arm system of the dexterous robot, along with coordinate transformation of the mirror-holding arm, force feedback calculation, and control drive algorithms. Depending on the different application scenarios, the slave control system performs arm-switching and calls control algorithms, improving reliability in limited operating environments, increasing system intelligence, and enhancing safety and practicality.

[0068] The storage module mainly stores file reading and work logs, various test results and fault modes, which facilitates data retrieval, ensures data traceability, and provides a reference for later troubleshooting and control.

[0069] Drive module: Receives control commands from the control module and drives the robot arm to perform motion control, jitter control, obstacle avoidance control, and arm-switching control according to the control commands. Each robot arm is equipped with a corresponding drive module; in one embodiment of the invention, three sets of drive units are provided, each set of drive units corresponding to a different robot arm. For example... Figure 2The embodiment of the present invention shown mainly includes three sets of drive units. Each set of drive units mainly controls the motor according to the data calculation and motor control thread, so that the dexterous robot moves along the prescribed route, which improves the servo performance of the robot arm, improves the real-time performance and reliability, facilitates the coordinated control of each arm, and improves the obstacle avoidance ability.

[0070] Protection Mechanism Module: Receives detection information from the detection module, performs alarm detection, and forwards the information to the control module if normal. Otherwise, the protection mechanism module outputs warnings for overcurrent, overvoltage, and overtemperature of the robotic arm, which the control module then uses to generate drive commands and outputs them to the drive module. For example... 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 primarily utilizes data from the detection module and information from the endoscope (an endoscope is fixedly mounted at the end of the operating arm) to implement obstacle avoidance, anti-shake, and other protection mechanisms. This controls and handles various faults, protecting the entire system and improving the reliability and safety of the slave control system.

[0071] The communication module receives the expected position information command sent by the master control system and outputs it to the control module in the slave control system. The slave control module then outputs control commands for motion control. This primarily includes CAN communication, EtherCAT communication, Ethernet communication, and 5G wireless communication. These communication methods are mainly used for communication between the master and slave control systems, receiving the expected position information command from the master control system. Each data transmission involves multiple communication modes, comparing data commands to execute the operation, thus avoiding operational failures caused by communication issues and improving the system's safety, reliability, real-time performance, and stability.

[0072] The arm-changing module receives arm-changing commands from the upper-level master control system and performs arm-changing control on the robot arm according to actual work requirements.

[0073] Robotic arms are classified into execution arms and manipulator arms according to their uses. Execution arms are used to perform operational tasks input from higher levels, while manipulator arms are used to illuminate the surrounding environment, acquire remote sensing images, and transmit them to the operator, ensuring progress and reliability. In one embodiment of the present invention, the three robotic arms consist of two execution arms and one manipulator arm.

[0074] The actuator arm consists of an active section and a driven section; the control arm consists of an active section and a driven section.

[0075] In both the actuator arm and the manipulator arm, the segment that can actively perform movements is called the driving segment, and the segment that follows the movements of the driving segment is called the driven segment. Furthermore, the root of both the actuator arm and the manipulator arm is always the driving segment.

[0076] From the control methods and processes of the manual control system, such as Figure 4 As shown:

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

[0078] 2) Based on the parameter self-test results, compare them with the preset parameters in the system storage module to see if they are normal. If they are normal, proceed to step 3). If they are not normal, check the cause of the error. The system determines whether it can be repaired based on the storage parameters. If it can be repaired, perform the repair and repeat step 2). If it cannot be repaired, stop the machine and replace the backup equipment.

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

[0080] 4) After receiving the instruction from the master control system from the manual control system, the instruction is checked to determine whether the data obtained from the two communication methods are consistent. If they are consistent, proceed to step 6); if the data obtained from the two communication methods are inconsistent, proceed to step 5.

[0081] 5) Feed back the data inconsistency results to the master control system and record the count by 1. If the number of records 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 and return to step 4). If the data is inconsistent after iterating through all communication methods, stop the machine for inspection.

[0082] 6) Entering the operation state, the manual control system transmits self-test information to the master control system. The master control system uses two stable communication methods to send task commands (task commands include: position information, stop command, etc.).

[0083] 7) Collect detection information of the robot arm from the detection module of the hand control system, store the detection information and send it back to the main hand control system;

[0084] 8) Based on the task instructions and the detection information from the detection module, drive the robot arm to execute the task instructions. During the execution of the task instructions, the control module of the slave control system continuously performs obstacle avoidance control, jitter control, obstacle avoidance control, arm switching control, and safety protection control on the robot arm, and outputs control instructions to the drive module.

[0085] The method for obstacle avoidance control of a robotic arm is as follows:

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

[0087] 2) Determine the structural parameters of the dexterous robot arm. Based on the practical type of the arm, match the parameter types prepared in advance in the parameter detection module to determine the 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 moving in a limited but not contactable space), proceed to step 4). If there is an environment with obstacles (such as the robot arm moving in a limited space that is required to be untouchable), 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 based on structural parameters and expected position information. The control module controls the robot arm to move according to the motion trajectory. Simultaneously, during the movement, it cyclically checks the external environment's anti-collision force ΔF in real time according to the control cycle to determine if a collision has occurred. If a collision is detected, proceed to step 5). If no collision occurs, continue moving along the current trajectory until the task instruction is completed. The method for determining whether a collision has occurred is as follows: compare the external environment's anti-collision force ΔF obtained from formula 1 with the force limit value of 0.5N. If the external environment's anti-collision force ΔF is greater than the force limit value of 0.5N, proceed to step 5).

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

[0091] As shown in Formula 1, the value of the external environment collision avoidance force ΔF is calculated using the current location information and the target location:

[0092]

[0093] in,

[0094] S mx S is the target position given in the x-axis direction of the robot arm for each sampling point in the control cycle. sx This represents the actual position of the robot arm along the x-axis; the given target position is determined by the motion trajectory.

[0095] S my S is the target position given in the y-axis direction of the robot arm for each sampling point in the control cycle. sy This represents the actual position of the robot arm along the y-axis.

[0096] S mzS is the target position given in the z-axis direction of the robot arm for each sampling point in the control cycle. sz This represents the actual position of the robot arm along the z-axis.

[0097] F x K represents the x-axis force on the robot arm detected by the detection module. x The coefficient of the force along the x-axis;

[0098] F y K represents the y-axis force on the robot arm detected by the detection module. y The coefficient of the force along the y-axis;

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

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

[0101] C x Let C be the equivalent stiffness of the robot arm lever in the x-axis direction. y S represents the equivalent stiffness of the robot arm lever in the y-axis direction. z Let be the equivalent stiffness of the robot arm lever in the z-axis direction;

[0102] The hinge point at the fixed end of the driven section is taken as the origin O of the coordinate system. The X-axis is perpendicular to the plane formed by the axes of the driving and driven sections, while the Y-axis and Z-axis lie within the plane formed by the axes of the driving and driven sections. The Z-axis is orthogonal to both the X-axis and Y-axis, forming a coordinate system. The driving and driven sections are rigidly connected by a flange.

[0103] 6) If there is an external environment, the dexterous robot arm determines the motion trajectory corresponding to the external environment based on the structural parameters and the expected position information output by the master control system. The control module controls the robot arm to move according to the motion trajectory. At the same time, during the movement, it cycles in real time according to the feedback information from the endoscope and the torque feedback information output by the detection module (i.e., the anti-collision force ΔF1 in the internal environment) to determine whether a collision has occurred. If a collision is determined to have occurred, the obstacle avoidance program in the environment is entered. If no collision has occurred, the robot moves according to the current instruction trajectory.

[0104] The method for determining a collision in step 6) is as follows: if any of the following conditions are met, a collision is determined to have occurred, and the obstacle avoidance procedure in the environment is initiated;

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

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

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

[0108]

[0109] In Formula 2, S x S y S z These are position coefficients along the x, y, and z axes, respectively, ensuring the position remains within the collision limits of the internal environment. In Formula 2, this coefficient S... x S y S z The values ​​range from 0 to 1. ΔF1 is the feedback force of the inner limit edge collision. The driving force from the end of the hand is no greater than ΔF1.

[0110] The method for controlling the jitter of a robotic arm is as follows:

[0111] 11) Determine the structural parameters of the dexterous robot arm. Based on the type of robot arm, match the parameter types prepared in advance in the parameter detection module to determine various parameters of the robot arm.

[0112] 12) Determine the motion trajectory of the dexterous robot arm based on the expected position information command in the task command output by the master 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 completes the expected position information command and reaches the target position, proceed to step 13);

[0113] 13) The control module performs anti-shake judgment. The control module uses the angular velocity and position of the robot arm collected by the detection module to determine whether each robot arm meets the shake threshold. 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 is less than 2° per second, it is determined that the shake threshold is met, the robot arm moves into position and enters the working state, and executes subsequent task instructions; otherwise, proceed to step 14) to start the anti-shake program control (the motion shake control does not cycle with the control cycle, and only one judgment is made after the movement is in place each time it is executed).

[0114] 14) First, calculate the difference ΔL between the actual position and the target position, as well as the angular velocity w. Using the difference ΔL, the angular velocity, and the length of the robot arm, calculate the rotational position of the connection point at the root of the robot arm. If the rotational position at the root connection point swings back and forth, change the parameters of the position PID and the speed PID to reduce the jitter, and proceed to step 15). Conversely, if the rotational position at the root connection point of the robot arm remains unchanged and only the end effector jitters, it indicates that the jitter is caused by the length of the arm extension. In this case, move the arm extension at the root of the robot arm in the opposite direction, moving it to half the difference ΔL each time. After moving it to half the difference ΔL each time, determine whether the jitter threshold is met again. Continue until the jitter threshold is met, and then proceed to step 15).

[0115] 15) Execute subsequent task instructions.

[0116] like Figure 5 As shown, a method for rapid arm switching control of a robot arm is described.

[0117] 21) The control module receives the arm-changing command sent by the master 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) Obtain detection information using the detection module, and calculate the arm's retraction path LS1 based on the detection information and the robot arm's structural parameters, while simultaneously calculating the differential correction parameter S. i ;

[0120] The differential correction parameter S corresponding to the i-th control cycle i As shown in Formula 3, which uses three robotic arms as an example:

[0121]

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

[0123] In Formula 3, 1, 2, and 3 represent the labels of multiple robotic arms that work independently; therefore,

[0124] S 1i This represents the actual position of the first arm during the i-th control cycle.

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

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

[0127] C 1i The retraction angle corresponding to the i-th control cycle of the first arm (measured by the detection module, the angle of the robot arm around the fixed end).

[0128] 24) Use the difference correction parameter S corresponding to the i-th control cycle. i If the difference between the actual position and the target position of the robot arm is greater than 2mm, then proceed to step 25) to 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 retracts to the initial state outside the body, then proceed to step 26).

[0129] 25) Based on the difference correction parameter S 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 retracts to the initial state outside the body, and then enter step 26);

[0130] 26) After retracting to the initial state outside the body according to the retraction path LS1 and completing the arm replacement process, continue to execute subsequent tasks.

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

[0132] 31) During the movement, the motion 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 type of protection to trigger; select the appropriate protection unit from the protection mechanism module. All protection units are as follows: Figure 6 As shown;

[0134] 33) When the current is detected to exceed a certain limit (2A), the overcurrent protection mechanism will be triggered. If overcurrent is detected after three consecutive current readings, the machine will stop for inspection; otherwise, it will continue to operate.

[0135] 34) When overvoltage, overforce, overtemperature, or electromagnetic trigger threshold conditions are detected, handle them in the same way as overcurrent conditions.

[0136] 35) When collision protection is triggered, the obstacle avoidance process described above shall be performed;

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

[0138] While the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the invention. Any person skilled in the art can make possible variations and modifications to the technical solutions of the present invention using the disclosed methods and techniques without departing from the spirit and scope of the invention. Therefore, any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention, without departing from the content of the technical solutions of the present invention, shall fall within the protection scope of the present invention. Where there is no conflict, the embodiments of this application and the technical features thereof can be combined with each other.

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

Claims

1. A method for controlling a dexterous hand-held robotic arm, characterized in that, include: Based on the task instructions sent by the superior and the detection information from the detection module, the drive module drives the slave robot arm to perform motion control. During the movement of the slave robot arm, the control module outputs control commands to the drive module to continuously control obstacle avoidance and vibration of the slave robot arm; The control module's continuous obstacle avoidance control of the slave robot arm also includes determining whether a collision has occurred, specifically: Determine whether the space where the robotic arm is located is unreachable; The robotic arm moves within a limited but accessible space, thus mitigating the impact forces of the external environment. Compared to the limit value of 0.5N, if the external environment's anti-collision force... If the force exceeds the limit value of 0.5N, it is determined that a collision has occurred in the robotic arm and obstacle avoidance control is required. If the robotic arm moves within a limited space where it is required to avoid contact, then if any of the following conditions are met, it is determined that the robotic arm has collided and obstacle avoidance control is required. Condition 1: The minimum distance between the robotic arm and the surrounding space is less than 2mm; Condition 2: Internal environment collision resistance Greater than 0.1N.

2. The method for controlling a dexterous slave robotic arm according to claim 1, characterized in that, External environment collision protection The method for determining this is as follows: * in, Given the target position along the x-axis of the hand robot arm coordinate system, This refers to the actual position of the robot arm in the x-axis direction of the hand robot's coordinate system; Given the target position along the y-axis of the hand robot arm coordinate system, This refers to the actual position of the robot arm in the y-axis direction of the hand robot's coordinate system; Given the target position along the z-axis of the hand robot arm coordinate system. This refers to the actual position of the robot arm in the z-axis direction of the hand robot's coordinate system; The force along the x-axis of the slave robot arm coordinate system, as detected by the detection module. This is the coefficient of the force along the x-axis of the robot arm coordinate system; The force along the y-axis of the slave robot arm's coordinate system, as detected by the detection module. This is the coefficient of the force along the y-axis of the robot arm's coordinate system; The force along the z-axis of the slave robot arm coordinate system, as detected by the detection module. This is the coefficient of the force along the z-axis of the robot arm's coordinate system; , and The values ​​of all values ​​are in the range of 0-2; Let be the equivalent stiffness of the arm of the slave robot in the x-axis direction of the slave robot arm coordinate system. Let be the equivalent stiffness of the lever arm of the slave robot in the y-axis direction of the slave robot arm coordinate system. Let be 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 and driven segments is taken as the origin O of the coordinate system. The x-axis is perpendicular to the plane formed by the axes of the driven and driven segments, and the y-axis and z-axis are located in the plane formed by the axes of the driven and driven segments. The z-axis is orthogonal to the x-axis and y-axis, forming the coordinate system of the robot arm.

3. The method for controlling a dexterous slave robotic arm according to claim 2, characterized in that, Internal environment collision protection The method for determining this is as follows: in, These are the position coefficients for the x-axis, y-axis, and z-axis, respectively, with values ​​ranging from 0 to 1.

4. The method for controlling a dexterous slave robotic arm according to claim 3, characterized in that, The method by which the control module continuously controls the jitter of the slave robot arm is as follows: 11) Determine the structural parameters of the dexterous follower robot arm; 12) Based on the expected position information command in the task command output by the master control system and the current position of the slave robot arm, determine the motion trajectory 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 completes the expected position information command and reaches the target position, proceed to step 13). 13) The control module performs anti-shake judgment. The control module uses the angular velocity and position of the slave robot arm collected by the detection module to determine whether the slave robot arm meets the shake threshold. 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 shake threshold is met, the slave robot arm moves into position and enters the working state, and executes subsequent task instructions; otherwise, proceed to step 14) to start the anti-shake program control. 14) Calculate the difference between the actual position and the target position. And angular velocity w; through the difference The angular velocity ω and the length of the arm's extension are used to calculate the rotational position at the connection point of the arm's root. If the rotational position at the connection point shows back-and-forth swaying, the position PID parameters and velocity PID parameters are changed to reduce the jitter, and the process proceeds to step 15). Conversely, if the rotational position at the connection point remains unchanged and only the end effector jitters, the arm's root extension is moved in the opposite direction, with each movement being the difference in position. Half of L, the difference for each movement After L is half, it is checked again to see if the jitter threshold is met. This process continues until the jitter threshold is met, and then proceeds to step 15. 15) Complete the jitter control of the expected location information command and execute subsequent task commands.

5. A dexterous slave robot arm control system for implementing the dexterous slave robot arm control method as described in claim 4, characterized in that, include: The robot arm consists of a parameter module, a detection module, a control module, and a drive module. The slave robot arm consists of an active segment and multiple driven segments. The root of the slave robot arm is the active segment, which drives the driven segments to move. The multiple driven segments are connected in series, with one end of the series connected to the active segment and the other end serving as the end of the slave robot arm, connecting to external instruments and equipment. Parameter module: Stores structural parameters for multiple different types of hand-held robot arms; Detection module: Used to detect the temperature, voltage, current, angular velocity, torque and position of the robotic arm as detection information and output it to the control module; Control module: Receives detection information output by the detection module, performs motion control, jitter control, obstacle avoidance control, and arm switching control, and outputs corresponding control commands to the drive module; Drive module: Receives control commands from the control module and drives the slave robot arm to perform motion control, jitter control, or obstacle avoidance control according to the control commands.

6. The dexterous slave robot arm control system according to claim 5, characterized in that, Also includes: Communication module; The communication module receives the expected position information command sent by the superior and outputs the expected position information command to the control module, which then outputs control commands to perform motion control.

7. A dexterous slave robot arm control system according to claim 6, characterized in that, Also includes: Arm-changing module; The arm-changing module receives the arm-changing command sent by the superior and controls the arm-changing of the slave robot arm.

8. A dexterous slave robot arm control system according to any one of claims 5 to 7, characterized in that, According to their purpose, robotic arms can be divided into execution arms and manipulator arms. Execution arms are used to perform operational tasks input by the superior, while manipulator arms are used to illuminate the surrounding environment, collect remote sensing images, and transmit them to the operator. The actuator arm consists of an active section and a driven section; the control arm consists of an active section and a driven section.

Images