Control method of main control arm, program product, electronic equipment and storage medium

Abstract

The invention relates to the technical field of surgical robot control, and particularly provides a control method of a main control arm, a program product, electronic equipment and a storage medium. The main control arm comprises a first rotating joint, a second rotating joint and a third rotating joint which are sequentially connected through joint connecting rods. According to the control method of the main control arm, a first motion parameter of a first rotary joint is monitored, and a second motion parameter of a third rotary joint is determined according to the first motion parameter under the condition that the first motion parameter meets a preset deviation condition; and controlling the third rotary joint to move based on the second motion parameter. When the second rotating joint is in the zero position, the rotating axes of the first rotating joint and the third rotating joint are coaxial, and the second motion parameter is determined according to the first motion parameter, so that the possibility of collision between the hand of the user and the joint connecting rod or different joint connecting rods can be reduced, the user operation is smoother, and the user experience is improved. And the user experience in the use process of the main control arm is improved.

Description

Technical Field

[0001] The present application relates to the field of surgical robot control technology, and in particular to a control method, program product, electronic device and storage medium for a main control arm. Background Art

[0002] With the continuous development of medical equipment, computer technology and control technology, minimally invasive surgery has been more and more widely used due to its advantages of small surgical trauma, short recovery time and less pain for patients. Minimally invasive surgical robots can avoid operational limitations, such as hand tremors during filtering operations, with their high dexterity, high control accuracy and intuitive surgical images, and are widely used in surgical areas such as the abdomen, pelvis and chest.

[0003] At present, the largest category of minimally invasive surgical robots is laparoscopic (surgical) robots, which generally include a doctor's console (also called the master end) and a surgical platform (also called the slave end). The surgical platform is equipped with multiple surgical arms. The master control arm of the doctor's console collects the doctor's operation signal and generates a control signal for the surgical arm after processing by the control system. The surgical arm controls the surgical instrument attached to it to perform the surgical operation or the endoscope to perform image acquisition. In order to improve the flexibility of operation and avoid singularities, the master control arm is usually a redundant mechanism (a redundant mechanism refers to a mechanism with a greater number of joints than the number of spatial degrees of freedom). If the user can actively control the redundant mechanism during the use of the master control arm, it will bring a better user experience. Summary of the invention

[0004] In view of this, an object of the embodiments of the present application is to provide a control method, a program product, an electronic device and a storage medium for a master control arm, so as to improve the user experience during the use of the master control arm.

[0005] In a first aspect, an embodiment of the present application provides a control method for a master control arm, the control method being used to control a master control arm; the master control arm comprises a first rotation joint, a second rotation joint and a third rotation joint connected in sequence through a joint link; when the second rotation joint is at a zero position, the rotation axes of the first rotation joint and the third rotation joint are coaxial;

[0006] The control method comprises:

[0007] monitoring a first motion parameter of the first rotational joint;

[0008] When the first motion parameter satisfies a preset deviation condition, determining a second motion parameter of the third rotation joint according to the first motion parameter;

[0009] Based on the second motion parameter, the third rotational joint movement is controlled.

[0010] In the above implementation process, the control method of the master control arm monitors the first motion parameter of the first rotation joint, and determines the second motion parameter of the third rotation joint according to the first motion parameter when the first motion parameter meets the preset deviation condition; and controls the movement of the third rotation joint based on the second motion parameter. Since the rotation axes of the first rotation joint and the third rotation joint are coaxial when the second rotation joint is at the zero position, by determining the second motion parameter of the third rotation joint according to the first motion parameter when the first motion parameter meets the preset deviation condition, the possibility of collision between the user's hand and the joint link or between different joint links can be reduced, making the user operation smoother and improving the user experience during the use of the master control arm.

[0011] Optionally, in an embodiment of the present application, the first motion parameter includes a first rotation angle increment of the first rotation joint relative to the zero position; the second motion parameter includes a second rotation angle increment of the third rotation joint relative to the zero position; determining the second motion parameter of the third rotation joint based on the first motion parameter includes: when the second rotation joint is at the zero position, determining the second rotation angle increment based on the first rotation angle increment; when the second rotation joint is not at the zero position, determining the second rotation angle increment based on the first rotation angle increment and the joint deviation angle of the second rotation joint.

[0012] In the above implementation process, when the second rotation joint is at the zero position, that is, when the rotation axes of the first rotation joint and the third rotation joint are coaxial, the second rotation angle increment is determined according to the first rotation angle increment, so that the third rotation joint follows the rotation of the first rotation joint, thereby reducing the possibility of collision between the user's hand and the joint link or between different joint links. When the second rotation joint is not at the zero position, that is, when the rotation axes of the first rotation joint and the third rotation joint are not coaxial, the second rotation angle increment that enables the third rotation joint to follow the rotation of the first rotation joint can be determined according to the first rotation angle increment and the joint deviation angle of the second rotation joint; the movement of the third rotation joint is controlled based on the second rotation angle increment, so as to reduce the possibility of collision between the user's hand and the joint link or between different joint links.

[0013] Optionally, in an embodiment of the present application, determining the second motion parameter of the third rotation joint based on the first motion parameter specifically includes: when the second rotation joint is at the zero position, determining the first rotation angle increment as the second rotation angle increment; when the second rotation joint is not at the zero position, determining the product of the first rotation angle increment and the cosine value of the joint deviation angle as the second rotation angle increment.

[0014] Optionally, in an embodiment of the present application, the second motion parameter also includes: the real-time rotation speed and real-time rotation acceleration of the third rotation joint; before controlling the movement of the third rotation joint based on the second motion parameter, the control method also includes: when the first rotation angle increment is greater than or equal to the first preset angle and less than the second preset angle, determining the target rotation speed and target rotation acceleration of the third rotation joint according to the spatial angular velocity of the main control arm; when the first rotation angle increment is greater than or equal to the second preset angle, determining the target rotation speed and target rotation acceleration of the third rotation joint according to a preset speed threshold and a preset acceleration threshold, respectively; and calculating the real-time rotation speed and real-time rotation acceleration based on the target rotation speed and the target rotation acceleration.

[0015] In the above implementation process, when the first rotation angle increment is small, the target rotation speed and target rotation acceleration of the third rotation joint are determined according to the spatial angular velocity of the main control arm, and the real-time rotation speed and real-time rotation acceleration are calculated based on the target rotation speed and target rotation acceleration; that is, the movement synchronization between the third rotation joint and the main control arm is maintained as much as possible to reduce the possibility of collision between the user's hand and the joint link or between different joint links, and improve the movement continuity of the third rotation joint. When the first rotation angle increment is large, the target rotation speed and target rotation acceleration of the third rotation joint are determined according to the preset speed threshold and the preset acceleration threshold, and the real-time rotation speed and real-time rotation acceleration are calculated based on the target rotation speed and target rotation acceleration; that is, the deviation compensation of the third rotation joint to the first rotation joint is realized as soon as possible, thereby reducing the possibility of collision between the user's hand and the joint link or between different joint links.

[0016] Optionally, in an embodiment of the present application, when the first motion parameter satisfies a preset deviation condition, determining the second motion parameter of the third rotational joint according to the first motion parameter includes: when the first motion parameter satisfies the preset deviation condition, calculating the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotational joint based on the trajectory planning strategy, the second rotation angle increment, the target rotation speed and target rotation acceleration of the third rotational joint; when the real-time rotation speed is less than or equal to the preset speed threshold, and the real-time rotation acceleration is less than or equal to the preset acceleration threshold, determining the real-time rotation position, real-time rotation speed and real-time rotation acceleration as the second motion parameter; when the real-time rotation speed is greater than the preset speed threshold, or the real-time rotation acceleration is greater than the preset acceleration threshold, replacing the real-time rotation speed based on the preset speed threshold, and replacing the real-time rotation acceleration based on the preset acceleration threshold; and determining the real-time rotation position and the replaced real-time rotation speed and real-time rotation acceleration as the second motion parameter.

[0017] In the above implementation process, when the first motion parameter meets the preset deviation condition, based on the trajectory planning strategy, the second rotation angle increment, the target rotation speed and target rotation acceleration of the third rotation joint, the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotation joint are calculated; according to the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotation joint, the third rotation joint is controlled, so as to improve the motion control accuracy of the third rotation joint and better reduce the possibility of collision between the user's hand and the joint link or between different joint links. In addition, when the real-time rotation speed is greater than the preset speed threshold, or when the real-time rotation acceleration is greater than the preset acceleration threshold, the real-time rotation speed is replaced based on the preset speed threshold, and the real-time rotation acceleration is replaced based on the preset acceleration threshold; it can be ensured that the real-time rotation speed and real-time rotation acceleration of the third rotation joint are both within the preset range.

[0018] Optionally, in an embodiment of the present application, the preset deviation condition includes a preset deviation angle threshold and / or a preset angular velocity threshold of the first rotational joint; when the first motion parameter satisfies the preset deviation condition, determining the second motion parameter of the third rotational joint according to the first motion parameter includes: when the first rotation angle increment is greater than or equal to the preset deviation angle threshold, and / or when the spatial angular velocity of the main control arm is greater than or equal to the preset angular velocity threshold, determining the second motion parameter of the third rotational joint according to the first motion parameter.

[0019] In the above implementation process, when the first rotation angle increment is greater than or equal to the preset deviation angle threshold, that is, when the first rotation angle increment is large, the second motion parameter of the third rotation joint is determined according to the first motion parameter; the motion synchronization between the third rotation joint and the first rotation joint can be maintained as much as possible, thereby reducing the possibility of "collision between joint links connected to the first rotation joint when the first rotation angle increment of the first rotation joint is large". When the spatial angular velocity of the main control arm is greater than or equal to the preset angular velocity threshold, that is, when the operation speed of the main control arm is too fast, the second motion parameter of the third rotation joint is determined according to the first motion parameter; the motion synchronization between the third rotation joint and the first rotation joint can be maintained as much as possible, thereby reducing the possibility of "collision between the user's hand and the joint link or between different joint links when the operation speed of the main control arm is fast".

[0020] Optionally, in the embodiment of the present application, the control method further includes: when the first motion parameter does not satisfy the preset deviation condition, controlling the third rotation joint to maintain a current position.

[0021] In the above implementation process, by controlling the third rotation joint to maintain the current position when the first motion parameter does not meet the preset deviation condition, the possibility of collision between the user's hand and the joint link or between different joint links can be reduced while reducing the movement complexity of the main control arm.

[0022] In a second aspect, an embodiment of the present application provides a computer program / instruction, which, when executed by a processor, implements the control method of the master control arm as described in any one of the first aspects above.

[0023] In a third aspect, an embodiment of the present application further provides an electronic device; the electronic device includes:

[0024] Memory;

[0025] processor;

[0026] The memory stores a computer program executable by the processor. When the computer program is executed by the processor, the method for controlling the master control arm according to any one of the first aspects is performed.

[0027] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium having computer program instructions stored thereon. When the computer program instructions are executed by a processor, the control method of the master control arm as described in any one of the first aspects is executed.

[0028] The beneficial effects of the present application include at least: the control method of the master control arm provided by the present application monitors the first motion parameter of the first rotary joint, and determines the second motion parameter of the third rotary joint according to the first motion parameter when the first motion parameter meets the preset deviation condition; and controls the motion of the third rotary joint based on the second motion parameter. Since the rotation axes of the first rotary joint and the third rotary joint are coaxial when the second rotary joint is at the zero position, the active control of the redundant joint is achieved by determining the second motion parameter of the third rotary joint according to the first motion parameter when the first motion parameter meets the preset deviation condition, which can reduce the possibility of collision between the user's hand and the joint link or between different joint links, making the user operation smoother and improving the user experience during the use of the master control arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments of the present application will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0030] Figure 1 A schematic flow chart of a control method for a master control arm provided in an embodiment of the present application;

[0031] Figure 2 A schematic diagram of the structure of a master control arm provided in an embodiment of the present application;

[0032] Figure 3 A schematic structural diagram of another master control arm provided in an embodiment of the present application;

[0033] Figure 4 A schematic flow chart of another method for controlling a master control arm provided in an embodiment of the present application;

[0034] Figure 5 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0035] The following embodiments of the technical solution of the present application are described in detail in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present application, and are therefore only used as examples, and cannot be used to limit the scope of protection of the present application.

[0036] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by technicians in the technical field to which this application belongs; the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit this application.

[0037] In the description of the embodiments of the present application, the technical terms "first", "second", etc. are only used to distinguish different objects, and cannot be understood as indicating or implying relative importance or implicitly indicating the number, specific order or primary and secondary relationship of the indicated technical features. In the description of the embodiments of the present application, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0038] The technical solutions of the various embodiments of the present application will be described below in conjunction with the drawings in the embodiments of the present application.

[0039] In this specification, many specific technical details are described in some places so that those skilled in the art can understand the complete technical solution. However, it should be understood that the embodiments of the present application can be implemented without these specific technical details. Such detailed description of technical details should not be regarded as a limitation to the present application, and the protection scope of the present application is limited only by the claims. In other places, known structures, connection / position relationships, circuits and / or other details may not be shown in detail to avoid misunderstandings by the public about the main points of the invention of the present application.

[0040] In this specification, the drawings show schematic diagrams of several embodiments of the present application. However, the drawings are only schematic, and it should be understood that changes in mechanical structure, connection / position relationship, physical composition, electrical and steps can be made without departing from the spirit and scope of the present application. Such changes can be either replacement or combination of elements of several embodiments of the present application, or replacement or combination of known contents.

[0041] The terms used herein below are only used to describe specific embodiments and are not intended to limit the present application. Spatially relative terms, such as "below", "lower", "above", "upper", "middle", "middle", "inside", "outside", "center", "edge", etc., are used to describe the relationship between one component or feature shown in the figure and another component or feature for the convenience of explanation. It should be understood that spatially relative terms can only be used under the conditions of the positioning direction of the device in use or operation (except for the positioning direction specifically defined in the figure), and are not necessarily unique and unchanging. For example, if the device in the figure is flipped 180° up and down along the paper, then the elements described as being "below" other components or features will become "above" other components or features. Therefore, the exemplary term "below" can cover both the above and below directions, depending on how the device is positioned. The device can also be positioned in other directions (for example, rotated 90° or positioned in other directions), and the spatially relative descriptors used herein are interpreted accordingly.

[0042] As used herein, "several", "one", and "the" are intended to include plural forms as well, unless the context indicates otherwise. It should be further understood that the terms "include" and / or "comprise" specify the presence of stated features, steps, operations, elements, and / or parts, but do not exclude the presence of one or more other features, steps, operations, elements, parts, and / or groups thereof.

[0043] The term "object" generally refers to a component or a group of components. Throughout the specification and claims, the terms "object", "component", "portion", "part", "module", "assembly" and "element" are used interchangeably.

[0044] The terms "instrument", "surgical instrument" and "surgical instrument" are used herein to describe a medical device configured to be inserted into a patient and used to perform a surgical or diagnostic procedure, generally including an end effector. An end effector may be a surgical tool associated with one or more surgical procedures, such as forceps, needle holders, scissors, bipolar cauterizers, tissue stabilizers or retractors, clip appliers, stapling devices, imaging devices (e.g., endoscopes or ultrasound probes), and the like. Some instruments used in embodiments of the present application further provide an articulated support (sometimes referred to as a "wrist joint", "joint seat") for the surgical tool, so that the position and / or orientation of the end effector can be flexibly manipulated with one or more mechanical degrees of freedom relative to the instrument axis. Further, many end effectors include functional mechanical degrees of freedom, such as jaws that open or close or blades that translate along a specific path. The instrument may also contain information stored (e.g., on a PCBA board within the instrument) that is permanent or updateable by the surgical system. Accordingly, the system may provide one-way or two-way information communication between the instrument and one or more system components.

[0045] The term "mating" (sometimes referred to as "connecting", "coupling", "mounting", "assembling") can be broadly understood as any situation in which two or more objects are connected in a manner that allows the mating objects to operate in conjunction with each other. It should be noted that mating does not require a direct connection (e.g., a direct physical or electrical connection), but rather many objects or components can be used to mate two or more objects. For example, objects A and B can be mated using object C. In addition, the terms "removably coupled" or "removably mated" can be interpreted to mean a non-permanent connection or mating situation between two or more objects. This means that the removably coupled objects can be uncoupled and separated so that they no longer operate in conjunction.

[0046] Finally, the terms "or" and "and / or" as used herein should be interpreted as inclusive or meaning any one or any combination. Thus, "A, B, or C" or "A, B and / or C" means any one of the following: A; B; C; A and B; A and C; B and C; A, B, and C. An exception to this definition will only occur when a combination of elements, functions, steps, or actions are inherently mutually exclusive in some way.

[0047] Taking the master-slave teleoperated laparoscopic surgical robot as an example, the laparoscopic surgical robot usually includes a doctor control platform, a patient surgical platform and an image platform. The surgeon sits on the doctor control platform and watches the two-dimensional or three-dimensional image of the surgical area transmitted by the laparoscope (sometimes called an "endoscope") placed in the patient's body, and controls the movement of the robotic arm on the patient's surgical platform, as well as the surgical instruments or laparoscopes attached to the robotic arm. The robotic arm is equivalent to simulating a human arm, and the surgical instrument is equivalent to simulating a human hand. Both provide surgeons with a series of movements that simulate the human wrist, while filtering the tremor of the human hand itself. Therefore, it is increasingly widely used in surgery, especially in abdominal, thoracic, and general surgery.

[0048] The patient surgical platform generally includes a chassis, a column, a plurality of mechanical arms connected to the column, and one or more surgical instrument manipulators at the end of the support assembly of each mechanical arm. The surgical instrument and / or laparoscope is detachably connected to the surgical instrument manipulator. Each surgical instrument manipulator supports one or more surgical instruments and / or laparoscopes operated at the surgical site in the patient's body. Each surgical instrument manipulator can be allowed to control the relevant surgical instruments in various forms of one or more mechanical degrees of freedom (e.g., all six Cartesian degrees of freedom, five or less Cartesian degrees of freedom, etc.). Typically, each surgical instrument manipulator is limited by mechanical or software constraints to rotate the relevant surgical instrument around the center of motion on the surgical instrument that remains stationary relative to the patient, and the center of motion is generally located at the position where the surgical instrument enters the body wall, and the center of motion is generally referred to as the "distal center" or "fixed point".

[0049] The imaging platform usually includes a video image capture function (commonly an endoscope) and one or more video displays for displaying surgical instruments in the captured images. In some laparoscopic surgical robots, an optical device is included to transmit images from the patient's body to one or more imaging sensors (e.g., CCD or CMOS sensors) at the distal end of the endoscope, and then transmit the video image to the host of the imaging platform through steps such as photoelectric conversion. Subsequently, through image processing, the processed image is displayed on a video display for observation by other doctors or assistants.

[0050] The doctor control platform usually includes a chassis, a pedal assembly, a stereo monitor, a main control arm, and a manual controller connected to the end of the main control arm. The surgeon controls the manual controller and the pedal assembly to achieve specific movements and / or energy excitation of the surgical instrument. The surgeon control platform can be at a single location in the surgical system composed of a laparoscopic surgical robot or it can be distributed at two or more locations in the system, and the remote control master / slave operation can be completed according to the preset control degree, for example, one location is used as the main control for the main operation, and the other location is used as the auxiliary control for the auxiliary operation. The main control completes the main surgical operation, and the auxiliary control completes the auxiliary operation such as laparoscope movement or tissue traction. In some embodiments, the manual controller can be an input device capable of completing one or more manual operations, such as a joystick, an exoskeleton glove, a power and gravity compensation manipulator, etc. These input devices collect the surgeon's operation signal, and after being processed by the control system, generate control signals of the robotic arm and the surgical instrument manipulator, thereby controlling the remote control motor on the surgical instrument manipulator, and the motor further controls the final movement of the surgical instrument.

[0051] Generally, the force generated by the remote motor is transmitted via a transmission system, which transmits the force from the remote motor to the end effector of the surgical instrument. In some teleoperated surgical embodiments, the input device for controlling the manipulator may be located at a location remote from the patient, in or outside the room where the patient is located, or even in a different city. The input signal from the input device is then transmitted to the control system. Those familiar with teleoperation, telecontrol and telepresentation surgery will understand such systems and their components and will not be described in detail here.

[0052] See also Figure 1 A flow chart of a control method for a master control arm provided by an embodiment of the present application is shown. The control method is used to control a master control arm; the master control arm comprises a first rotation joint, a second rotation joint and a third rotation joint connected in sequence by a joint link; when the second rotation joint is at a zero position, the rotation axes of the first rotation joint and the third rotation joint are coaxial.

[0053] The control method of the master control arm may include the following steps:

[0054] S101, monitoring a first motion parameter of the first rotation joint;

[0055] S102, when the first motion parameter satisfies a preset deviation condition, determining a second motion parameter of the third rotation joint according to the first motion parameter;

[0056] S103. Control the movement of the third rotational joint based on the second movement parameter.

[0057] Please refer to Figure 2 , Figure 2This is a schematic diagram of the structure of a master control arm provided in an embodiment of the present application. Figure 3 , Figure 3 A schematic structural diagram of another master control arm provided in an embodiment of the present application. Figure 2 and Figure 3 All of them show the situation that the joint connection between the first rotation joint and the second rotation joint, and the second rotation joint and the third rotation joint is realized based on an "L-shaped" joint connecting rod. The first rotation joint and the second rotation joint, and the second rotation joint and the third rotation joint can also be connected based on a linear joint connecting rod, a broken line joint connecting rod, or a curved joint connecting rod, etc., and this application does not make specific limitations on this. Figure 2 It shows the situation that the second rotary joint is at the zero position and the rotation axes of the first rotary joint and the third rotary joint are coaxial. Figure 3 It shows the situation that the second rotation joint is not in the zero position and the rotation axes of the first rotation joint and the third rotation joint are not coaxial.

[0058] Wherein, in step S101, the first motion parameter may include an angle increment of the first rotational joint relative to the zero position. It can be considered that when the second rotational joint is at the zero position and the rotation axes of the first rotational joint and the third rotational joint are coaxial, the first rotational joint is at the zero position. The first motion parameter may also include a deviation angular velocity of the first rotational joint.

[0059] Wherein, in step S102, the preset deviation condition may include a preset deviation angle threshold of the first rotational joint and / or a deviation angular velocity threshold of the first rotational joint. Accordingly, when the angle increment of the first rotational joint relative to the zero position is greater than or equal to the preset deviation angle threshold, it can be judged that the first motion parameter meets the preset deviation condition; when the angle increment of the first rotational joint relative to the zero position is less than the preset deviation angle threshold, it can be judged that the first motion parameter does not meet the preset deviation condition. It can also be judged that the first motion parameter meets the preset deviation condition when the deviation angular velocity of the first rotational joint is greater than or equal to the deviation angular velocity threshold; when the deviation angular velocity of the first rotational joint is less than the deviation angular velocity threshold, it can be judged that the first motion parameter does not meet the preset deviation condition. The second motion parameter may include an angle increment of the third rotational joint relative to the zero position, and the angle increment of the third rotational joint relative to the zero position may be equal to the angle increment of the first rotational joint relative to the zero position, or may be in direct proportion or other functional relationship with the angle increment of the first rotational joint relative to the zero position.

[0060] Wherein, in step S103, the second motion parameter may include an angle increment of the third rotational joint relative to the zero position, and the angle increment of the third rotational joint relative to the zero position may be determined according to the angle increment of the first rotational joint relative to the zero position. According to the angle increment of the third rotational joint relative to the zero position, the movement of the third rotational joint is controlled.

[0061] based on Figure 2 , Figure 3 It can be seen from the master control arm shown that when the first rotary joint rotates and the angle increment relative to the zero position is large, a collision may occur between the two connecting rods connected to the first rotary joint. By determining the second motion parameter of the third rotary joint according to the first motion parameter of the first rotary joint, and controlling the motion of the third rotary joint based on the second motion parameter, the motion synchronization between the third rotary joint and the first rotary joint can be improved, thereby reducing the possibility of a collision between the two connecting rods connected to the first rotary joint.

[0062] It should be noted that the control method of the master control arm provided in the present application can also be used for a master control arm including a larger number of rotating joints or moving joints. Figure 2 , Figure 3 The seven-degree-of-freedom redundant robotic arm shown simulates the human arm. Figure 2 , Figure 3 The master control arm shown also includes a fourth rotation joint connected to the first rotation joint through a joint link, and the rotation axes of the fourth rotation joint, the first rotation joint and the second rotation joint intersect at a point, which can be controlled by the operator to achieve multi-degree-of-freedom movement of the master control arm in Cartesian space.

[0063] It can be seen that the control method of the master control arm provided in the present application monitors the first motion parameter of the first rotary joint, and determines the second motion parameter of the third rotary joint according to the first motion parameter when the first motion parameter meets the preset deviation condition; and controls the motion of the third rotary joint based on the second motion parameter. Since the rotation axes of the first rotary joint and the third rotary joint are coaxial when the second rotary joint is at the zero position, by determining the second motion parameter of the third rotary joint according to the first motion parameter when the first motion parameter meets the preset deviation condition, the possibility of collision between the user's hand and the joint link or between different joint links can be reduced, making the user operation smoother and improving the user experience during the use of the master control arm.

[0064] In some optional embodiments, the first motion parameter includes a first rotation angle increment of the first rotation joint relative to the zero position; the second motion parameter includes a second rotation angle increment of the third rotation joint relative to the zero position; determining the second motion parameter of the third rotation joint based on the first motion parameter includes: when the second rotation joint is at the zero position, determining the second rotation angle increment based on the first rotation angle increment; when the second rotation joint is not at the zero position, determining the second rotation angle increment based on the first rotation angle increment and the joint deviation angle of the second rotation joint.

[0065] in, Figure 2 The figure shows the situation that the second rotary joint is at the zero position. At this time, the rotation axes of the first rotary joint and the third rotary joint are coaxial. Figure 3 What is shown is the situation that the second rotary joint is not in the zero position, at this moment, the rotation axis of the first rotary joint and the third rotary joint are not coaxial. When the second rotary joint is in the zero position, the second rotation angle increment can be made equal to the first rotation angle increment; or, when the first rotation angle increment is less than the rotation angle threshold value (can be 10 °, can also be 20 °, 45 ° or other reasonable values), the second rotation angle increment can be made equal to zero; and when the first rotation angle increment is greater than or equal to the rotation angle threshold value, the difference between the second rotation angle increment and the first rotation angle increment can be made equal to the above-mentioned rotation angle threshold value. Through the above-mentioned processing, it can be ensured that the rotation angle difference between the first rotary joint and the third rotary joint is always kept within the rotation angle threshold value, thereby reducing the possibility of collision between the two connecting rods connected to the first rotary joint. Therefore, by the second rotary joint being in the zero position, that is, when the rotation axis of the first rotary joint and the third rotary joint are coaxial, the second rotation angle increment is determined according to the first rotation angle increment, so that the third rotary joint follows the rotation of the first rotary joint, and the possibility of collision between the joint connecting rods can be reduced.

[0066] Wherein, when the second rotational joint is not at the zero position, the above correspondence between the first rotational angle increment and the second rotational angle increment can be corrected according to the joint deviation angle of the second rotational joint, so as to determine the second rotational angle increment that enables the third rotational joint to rotate along with the first rotational joint, thereby reducing the possibility of collision between the two connecting rods connected to the first rotational joint. When the second rotational joint is not at the zero position, the second rotational angle increment that enables the third rotational joint to rotate along with the first rotational joint can be determined according to the first rotational angle increment and the joint deviation angle of the second rotational joint; the movement of the third rotational joint is controlled based on the second rotational angle increment, thereby reducing the possibility of collision between the joint connecting rods.

[0067] In some optional embodiments, determining the second motion parameter of the third rotation joint based on the first motion parameter specifically includes: when the second rotation joint is at the zero position, determining the first rotation angle increment as the second rotation angle increment; when the second rotation joint is not at the zero position, determining the product of the first rotation angle increment and the cosine value of the joint deviation angle as the second rotation angle increment.

[0068] Among them, by determining the first rotation angle increment as the second rotation angle increment when the second rotation joint is at the zero position, the rotation synchronization between the third rotation joint and the first rotation joint can be guaranteed to the greatest extent, so as to minimize the possibility of collision between the joint links. When the second rotation joint is not at the zero position, as the joint deviation angle of the second rotation joint becomes larger and larger, the rotation axis between the third rotation joint and the first rotation joint also gradually deviates. As the joint deviation angle increases, that is, the degree of deviation between the rotation axis of the third rotation joint and the rotation axis of the first rotation joint becomes larger and larger, resulting in the third rotation joint The deviation compensation effect of the first rotation joint is getting worse and worse; until the joint deviation angle is equal to 90°, the third rotation joint does not have a deviation compensation effect on the first rotation joint. The product of the first rotation angle increment and the cosine value of the joint deviation angle can characterize the effective component of the first rotation angle increment in the direction of the rotation axis of the third rotation joint; by determining the product of the first rotation angle increment and the cosine value of the joint deviation angle as the second rotation angle increment, the synchronization of the rotation of the third rotation joint and the "rotation of the first rotation joint in the direction of the rotation axis of the third rotation joint" can be ensured, thereby reducing the possibility of collision between the joint links.

[0069] Please refer to Figure 4 , Figure 4 A schematic flow chart of another method for controlling a master control arm provided in an embodiment of the present application.

[0070] In some optional embodiments, the second motion parameter also includes: the real-time rotation speed and real-time rotation acceleration of the third rotation joint; before S103, based on the second motion parameter, controlling the movement of the third rotation joint, the control method also includes: S104, when the first rotation angle increment is greater than or equal to the first preset angle and less than the second preset angle, determining the target rotation speed and target rotation acceleration of the third rotation joint according to the spatial angular velocity of the main control arm; S105, when the first rotation angle increment is greater than or equal to the second preset angle, determining the target rotation speed and target rotation acceleration of the third rotation joint according to a preset speed threshold and a preset acceleration threshold, respectively; S106, calculating the real-time rotation speed and real-time rotation acceleration based on the target rotation speed and the target rotation acceleration.

[0071] The first preset angle may be 20°, 30° or other reasonable values. The second preset angle is greater than the first preset angle, and the second preset angle may be 40°, 45°, 60° or other reasonable values. The spatial angular velocity of the master control arm refers to the angle that the end of the master control arm rotates in a unit time when the rotation angle of the rotary joint changes. The target rotation speed and target rotation acceleration of the third rotary joint may be determined according to the spatial angular velocity of the master control arm or the angular velocity component of the spatial angular velocity of the master control arm in the direction of the rotation axis of the first rotary joint. Specifically, the target rotation speed and target rotation acceleration of the third rotary joint may be positively correlated with the "spatial angular velocity of the master control arm" or the "angular velocity component of the spatial angular velocity of the master control arm in the direction of the rotation axis of the first rotary joint". The preset speed threshold may be the maximum speed of the third rotary joint, and the preset acceleration threshold may be the maximum acceleration of the third rotary joint; when the first rotation angle increment is greater than or equal to the second preset angle, the real-time rotation speed and real-time rotation acceleration of the third rotary joint may be set to the maximum value to achieve deviation compensation of the first rotary joint as soon as possible. By determining the target rotation speed and target rotation acceleration of the third rotation joint according to the spatial angular velocity of the main control arm when the first rotation angle increment is small, and calculating the real-time rotation speed and real-time rotation acceleration based on the target rotation speed and the target rotation acceleration; that is, the movement synchronization between the third rotation joint and the main control arm is maintained as much as possible, so as to improve the movement continuity of the third rotation joint while reducing the possibility of collision between the joint links.

[0072] In some optional embodiments, S102, when the first motion parameter satisfies a preset deviation condition, determining the second motion parameter of the third rotational joint according to the first motion parameter, including: when the first motion parameter satisfies the preset deviation condition, calculating the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotational joint based on the trajectory planning strategy, the second rotation angle increment, the target rotation speed and target rotation acceleration of the third rotational joint; when the real-time rotation speed is less than or equal to the preset speed threshold, and the real-time rotation acceleration is less than or equal to the preset acceleration threshold, determining the real-time rotation position, real-time rotation speed and real-time rotation acceleration as the second motion parameter; when the real-time rotation speed is greater than the preset speed threshold, or the real-time rotation acceleration is greater than the preset acceleration threshold, replacing the real-time rotation speed based on the preset speed threshold, and replacing the real-time rotation acceleration based on the preset acceleration threshold; and determining the real-time rotation position and the replaced real-time rotation speed and real-time rotation acceleration as the second motion parameter.

[0073] Among them, the trajectory planning strategy refers to determining the motion trajectory of the main control arm in space by mathematical methods, including parameters such as position, speed and acceleration, to meet specific constraints or optimization goals. The trajectory planning strategy can be implemented by a model-based or sensor-based path planning method, and can also be implemented by a trajectory filter, which is not specifically limited in this application. By calculating the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotation joint based on the trajectory planning strategy, the second rotation angle increment, the target rotation speed and target rotation acceleration of the third rotation joint when the first motion parameter meets the preset deviation condition; according to the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotation joint, the third rotation joint is controlled, which can improve the motion control accuracy of the third rotation joint and better reduce the possibility of collision between the user's hand and the joint link or between different joint links. In addition, by replacing the real-time rotation speed based on the preset speed threshold and replacing the real-time rotation acceleration based on the preset acceleration threshold when the real-time rotation speed is greater than the preset speed threshold, or when the real-time rotation acceleration is greater than the preset acceleration threshold; it can ensure that the real-time rotation speed and real-time rotation acceleration of the third rotation joint are both within the preset range.

[0074] In some optional embodiments, the preset deviation condition includes a preset deviation angle threshold and / or a preset angular velocity threshold of the first rotational joint; and when the first motion parameter satisfies the preset deviation condition, determining the second motion parameter of the third rotational joint according to the first motion parameter comprises: when the first rotation angle increment is greater than or equal to the preset deviation angle threshold, and / or when the spatial angular velocity of the main control arm is greater than or equal to the preset angular velocity threshold, determining the second motion parameter of the third rotational joint according to the first motion parameter.

[0075] Wherein, the preset deviation angle threshold may be 10°, or 20°, 45°, or other reasonable values. By determining the second motion parameter of the third rotation joint according to the first motion parameter of the first rotation joint when the first rotation angle increment is greater than or equal to the preset deviation angle threshold, that is, when the first rotation angle increment is large, the motion synchronization between the third rotation joint and the first rotation joint can be maintained as much as possible, thereby reducing the possibility of "collision between the joint links connected to the first rotation joint when the first rotation angle increment of the first rotation joint is large". Wherein, the preset angular velocity threshold may be 50° per second, or 60° per second, 90° per second, or other reasonable values. When the operation speed of the main control arm is too fast, the user's hand collides with the joint link or between different joint links. By determining the second motion parameter of the third rotation joint according to the first motion parameter when the spatial angular velocity of the main control arm is greater than or equal to the preset angular velocity threshold, that is, when the operation speed of the main control arm is too fast, the motion synchronization between the third rotation joint and the first rotation joint can be maintained as much as possible, thereby reducing the possibility of "collision between the user's hand and the joint link or between different joint links when the operation speed of the main control arm is fast".

[0076] In some optional embodiments, the control method of the master control arm further includes: when the first motion parameter does not satisfy the preset deviation condition, controlling the third rotation joint to maintain a current position.

[0077] The preset deviation condition may include a preset deviation angle threshold of the first rotational joint. When the first rotation angle increment of the first rotational joint is less than the preset deviation angle threshold, it can be determined that the first motion parameter does not meet the preset deviation condition. By controlling the third rotational joint to maintain the current position when the first motion parameter does not meet the preset deviation condition, the possibility of collision between the joint links can be reduced while reducing the motion complexity of the master control arm.

[0078] Please refer to Figure 5 , Figure 5The electronic device 200 is a schematic diagram of a structure of an electronic device 200 provided in an embodiment of the present application. The electronic device 200 comprises: a memory 202 and a processor 201; the memory 202 stores a computer program executable by the processor 201, and when the computer program is executed by the processor 201, the control method of the master control arm described in any one of the first aspects is executed.

[0079] The memory 202 and the processor 201 may be interconnected and communicate with each other via a communication bus 203 and / or other forms of connection mechanisms (not shown). The memory 202 stores a computer program executable by the processor 201. When the computer program is executed by the processor 201, the control method of the master control arm described in the first aspect above is executed.

[0080] The embodiment of the present application further provides a computer-readable storage medium, on which computer program instructions are stored. When the computer program instructions are executed by the processor 201, the control method of the master control arm described in the first aspect above is executed.

[0081] Among them, the storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable red-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk or optical disk.

[0082] In several embodiments provided by the embodiments of the present application, it should be understood that the disclosed device / system and method can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to the multiple embodiments of the embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of the code, and a part of the module, program segment or code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0083] In addition, the functional modules in each embodiment of the present application can be integrated together to form an independent part, or each module can exist separately, or two or more modules can be integrated to form an independent part.

[0084] The above description is only an optional implementation manner of the embodiments of the present application, but the protection scope of the embodiments of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or replacements within the technical scope disclosed in the embodiments of the present application, which should be covered within the protection scope of the embodiments of the present application.

Claims

1. A control method for a master control arm, characterized in that: The control method is used to control a main control arm; the main control arm comprises a first rotation joint, a second rotation joint and a third rotation joint connected in sequence by a joint link; When the second rotary joint is at a zero position, the rotation axes of the first rotary joint and the third rotary joint are coaxial; The control method comprises: monitoring a first motion parameter of the first rotational joint; When the first motion parameter satisfies a preset deviation condition, determining a second motion parameter of the third rotation joint according to the first motion parameter; Based on the second motion parameter, the third rotational joint movement is controlled.

2. The control method according to claim 1, characterized in that: in, The first motion parameter includes a first rotation angle increment of the first rotation joint relative to the zero position; the second motion parameter includes a second rotation angle increment of the third rotation joint relative to the zero position; The determining the second motion parameter of the third rotation joint according to the first motion parameter comprises: When the second rotation joint is at the zero position, determining the second rotation angle increment according to the first rotation angle increment; When the second rotation joint is not at the zero position, the second rotation angle increment is determined according to the first rotation angle increment and the joint deviation angle of the second rotation joint.

3. The control method according to claim 2, characterized in that: The determining the second motion parameter of the third rotation joint according to the first motion parameter specifically includes: When the second rotation joint is at the zero position, determining the first rotation angle increment as the second rotation angle increment; When the second rotation joint is not at the zero position, the product of the first rotation angle increment and the cosine value of the joint deviation angle is determined as the second rotation angle increment.

4. The control method according to claim 2, characterized in that: in, The second motion parameter further includes: a real-time rotation speed and a real-time rotation acceleration of the third rotation joint; before controlling the motion of the third rotation joint based on the second motion parameter, the control method further includes: When the first rotation angle increment is greater than or equal to a first preset angle and less than a second preset angle, determining a target rotation speed and a target rotation acceleration of the third rotation joint according to the spatial angular velocity of the master control arm; When the first rotation angle increment is greater than or equal to the second preset angle, determining a target rotation speed and a target rotation acceleration of the third rotation joint according to a preset speed threshold and a preset acceleration threshold; Based on the target rotational speed and the target rotational acceleration, the real-time rotational speed and the real-time rotational acceleration are calculated.

5. The control method according to claim 4, characterized in that: The step of determining the second motion parameter of the third rotation joint according to the first motion parameter when the first motion parameter satisfies a preset deviation condition comprises: When the first motion parameter satisfies a preset deviation condition, calculating the real-time rotation position, real-time rotation speed and real-time rotation acceleration of the third rotation joint based on the trajectory planning strategy, the second rotation angle increment, the target rotation speed and target rotation acceleration of the third rotation joint; When the real-time rotation speed is less than or equal to the preset speed threshold, and the real-time rotation acceleration is less than or equal to the preset acceleration threshold, determining the real-time rotation position, the real-time rotation speed and the real-time rotation acceleration as the second motion parameter; When the real-time rotation speed is greater than the preset speed threshold, or the real-time rotation acceleration is greater than the preset acceleration threshold, the real-time rotation speed is replaced based on the preset speed threshold, and the real-time rotation acceleration is replaced based on the preset acceleration threshold; and the real-time rotation position and the replaced real-time rotation speed and real-time rotation acceleration are determined as the second motion parameter.

6. The control method according to any one of claims 2 to 5, characterized in that: in, The preset deviation condition includes a preset deviation angle threshold and / or a preset angular velocity threshold of the first rotational joint; when the first motion parameter satisfies the preset deviation condition, determining the second motion parameter of the third rotational joint according to the first motion parameter includes: When the first rotation angle increment is greater than or equal to the preset deviation angle threshold, and / or when the spatial angular velocity of the master control arm is greater than or equal to the preset angular velocity threshold, the second motion parameter of the third rotation joint is determined according to the first motion parameter.

7. The control method according to claim 1, characterized in that: The control method further comprises: When the first motion parameter does not satisfy the preset deviation condition, the third rotation joint is controlled to maintain a current position.

8. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instruction is executed by a processor, the control method of the master control arm according to any one of claims 1 to 7 is implemented.

9. An electronic device, characterized in that: The electronic device comprises: Memory; processor; The memory stores a computer program executable by the processor. When the computer program is executed by the processor, the control method of the master control arm according to any one of claims 1 to 7 is performed.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer program instructions, and when the computer program instructions are executed by a processor, the method for controlling the master control arm according to any one of claims 1 to 7 is executed.

Images