Control method under rcm constraint, robot, chip and storage medium

Abstract

The application provides a control method under RCM constraint, a robot, a chip and a storage medium, and is suitable for the technical field of robots. A force and torque sensor is arranged at the connection between the end of a mechanical arm and an endoscope, the force and torque applied on the endoscope operating rod are detected through the force and torque sensor, and the force and torque applied on the endoscope operating rod are mapped from a force and torque detection coordinate system to the degrees of freedom corresponding to the rigid section in an RCM coordinate system under the RCM constraint, so that more intuitive interactive operation is provided for people, visual servo and interactive control of the endoscope robot are realized from the motion control level, the realization process is simpler, and the scheme does not need to rely on external sensing equipment, and the endoscope robot does not need to interact and calculate with the external sensing equipment, so that the data calculation amount is reduced.

Description

Technical Field

[0001] This application belongs to the field of robotics technology, and in particular relates to a control method under RCM constraints, a robot, a chip, and a storage medium. Background Technology

[0002] In recent years, with the development of robotics technology and the improvement of medical standards, medical robots have been successfully applied in the field of minimally invasive surgery. For example, endoscopic robots can replace assistant doctors in controlling the endoscope. Research on endoscopic robot systems has revealed that the rigid segment of the endoscopic robot's joystick is constrained by the incision, allowing only translation and rotation along the axis of the rigid segment and rotation around the incision. This constraint is also known as the Remote Center of Motion (RCM) constraint. In other words, endoscopic robots need to control the endoscope while satisfying the RCM constraint.

[0003] Currently, control methods for endoscopic robots can be implemented by using motion planning methods with the aid of environmental sensing devices to achieve interaction between the endoscopic robot and the user. However, this method requires external environmental sensing devices and involves a large amount of data computation. Alternatively, force sensors can be installed on the joints of the endoscopic robot, combined with dynamics methods to achieve interactive control between the endoscopic robot and the user. However, this method involves dynamics, and the implementation process of methods involving dynamics is relatively complex. Summary of the Invention

[0004] This application provides a control method, robot, chip, and storage medium under RCM constraints. From the perspective of motion control, it solves the problem of visual servoing and interactive control under RCM constraints, reduces the amount of data computation, and simplifies the implementation process.

[0005] In a first aspect, embodiments of this application provide a control method under RCM constraints, applied to an endoscope robot. The endoscope robot further includes: a robotic arm, an endoscope, and force and torque sensors. The endoscope includes an endoscope joystick, which includes a rigid section and a flexible section. The force and torque sensors are located at the connection between the robotic arm and the endoscope, and are used to detect the force and torque applied to the endoscope joystick.

[0006] Control methods under RCM constraints include:

[0007] The RCM error is determined and the RCM constraint is established based on the projection of the direction vector of the end position of the rigid segment relative to the RCM point and the orthogonal plane of the endoscope control lever along the lever direction.

[0008] Under RCM constraints, the forces and moments applied to the endoscope control lever in the force and moment detection coordinate system are mapped to the degrees of freedom corresponding to the rigid segment in the RCM coordinate system, and the force components in the degrees of freedom corresponding to the rigid segment are determined.

[0009] The motion velocity of the rigid segment is determined based on the mass damping model and the force components on the degrees of freedom corresponding to the rigid segment.

[0010] The movement of the endoscope and the robotic arm is controlled according to the movement speed of the rigid segment, the movement angular velocity of the flexible segment, and the movement speed of each joint among the multiple joints of the robotic arm.

[0011] The first aspect provides a control method under RCM constraints, which sets force and torque sensors at the connection between the end of the robotic arm and the endoscope. The force and torque sensors detect the force and torque applied to the endoscope control lever. Under RCM constraints, the force and torque applied to the endoscope control lever are mapped from the force and torque detection coordinate system to the degrees of freedom corresponding to the rigid segment in the RCM coordinate system, providing a more intuitive interactive operation. From the motion control level, it realizes visual servoing and interactive control of the endoscope robot under RCM constraints. The implementation process is simpler. Moreover, this solution does not require external sensing devices, and the endoscope robot does not need to interact with or perform calculations with external sensing devices, thereby reducing the amount of data computation.

[0012] In one possible implementation of the first aspect, the method further includes: determining the rate of change of the RCM error under RCM constraints during the movement of the endoscope; and determining the movement speed of each joint among the multiple joints included in the robotic arm based on the movement speed of the rigid segment and the rate of change of the RCM error of the rigid axis. In this implementation, the movement speed of each joint among the multiple joints included in the robotic arm is associated with the degree of freedom corresponding to the rigid segment in the RCM coordinate system, reducing the amount of data computation.

[0013] In one possible implementation of the first aspect, the method further includes: determining the motion relationship between the endoscope and the target object's pixels in the image coordinate system; characterizing the motion relationship between the endoscope and the target object's pixels in the image coordinate system and the motion velocity of the rigid segment as a quadratic programming problem; solving the quadratic programming problem to determine the motion angular velocity of the flexible segment. In this implementation, the visual servoing task and interactive control of the endoscope under RCM constraints are characterized as a quadratic programming problem with a general form. Solving the quadratic programming problem determines the motion angular velocity of the flexible segment of the endoscope joystick, reducing the amount of data computation.

[0014] In one possible implementation of the first aspect, the force components on the degree of freedom corresponding to the rigid segment satisfy the following formula:

[0015]

[0016]

[0017]

[0018]

[0019] Among them, F α F β F γ F δ These are the force and torque components applied to the endoscope control lever along the rigid segment in four degrees of freedom relative to the RCM point, respectively. h =n t T (P t -P c ), where P c Indicates the location of the RCM point, P t Indicates the position of the end of the rigid segment, f h and τ h These are the forces and torques applied to the endoscope control lever, detected in the force and torque detection coordinate system, respectively, where f h =(f hx f hy f hz ), τ h =(τ hx , τ hy , τ hz In this implementation, the force components of the corresponding degree of freedom of the rigid segment can be determined more directly, thus improving the efficiency of determining the force components of the corresponding degree of freedom of the rigid segment.

[0020] In one possible implementation of the first aspect, the velocity of the rigid segment satisfies the following formula:

[0021]

[0022] Where F is the force applied to the endoscope control lever, F = [F α F β F γ F δ ] T , where D is the acceleration of the endoscope control lever under the influence of force and torque. f The damping coefficient of the endoscope control lever under the influence of force and torque. This represents the velocity of the rigid segment under the influence of force and torque. This implementation allows for a more direct determination of the velocity of the rigid segment under external force, enabling direct passive movement of the endoscope under force and improving the efficiency of the overall passive operation calculation.

[0023] In one possible implementation of the first aspect, the rate of change of the rigid shaft RCM error satisfies the following formula:

[0024]

[0025] or,

[0026]

[0027] Where, x c Indicates the RCM error value. Indicates the rate of change of RCM error. The acceleration representing the change in RCM error, a and b, satisfy the condition: a = b > 0, where a, b, and c are all integers between 1 and 100. In this implementation, the direction vector of the rigid segment's end position relative to the RCM point satisfies the above formula, ensuring the stability of the RCM point.

[0028] In one possible implementation of the first aspect, the movement speed of each of the plurality of joints of the robotic arm is determined according to the following formula:

[0029]

[0030] Where, q r The motion speed of each joint in the multiple joints of the robotic arm. Let A be the pseudo-inverse of matrix A, and A = B. c T [I 3×3 (P t -P c )^]J t J t Let N be the Jacobian matrix of the robotic arm, where N = [Z T G T ],in, G is the Jacobian matrix of the robotic arm. t Zero space, J is the Jacobian matrix of the robotic arm. t The pseudo-inverse, R t =[a t o t n t ] represents the direction matrix at the end of the rigid segment of the endoscope. Indicates the RCM error value. This refers to the motion speed of the rigid segment under the influence of force and torque. This implementation allows for faster and more accurate determination of the motion speed of each of the multiple joints within the robotic arm, improving the accuracy and efficiency of determining the motion speed of each joint.

[0031] Secondly, embodiments of this application provide an endoscope robot, which includes: a robotic arm, an endoscope, force and torque sensors, and a processor. The endoscope includes an endoscope joystick, which includes a rigid section and a flexible section. The force and torque sensors are located at the connection between the robotic arm and the endoscope. The force and torque sensors are used to detect the force and torque applied to the endoscope joystick. The processor is used to execute the method in the first aspect or any possible implementation of the first aspect.

[0032] Thirdly, embodiments of this application provide a chip, including: a processor, configured to call and run a computer program from a memory, causing the processor on which the chip is installed to execute the method in the first aspect or any possible implementation of the first aspect.

[0033] Fourthly, embodiments of this application provide a computer-readable storage medium, including: a computer program stored in the computer-readable storage medium, which, when executed, performs the method in the first aspect or any possible implementation of the first aspect.

[0034] Fifthly, a computer program product is provided, comprising a computer program that, when executed by a processor, performs the method of the first aspect or any possible implementation thereof.

[0035] It is understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0037] Figure 1 This is a schematic diagram of the structure of an endoscopic robot provided in one embodiment of this application;

[0038] Figure 2 This is a flowchart illustrating a control method under RCM constraints provided in an embodiment of this application;

[0039] Figure 3 This is a schematic diagram of establishing a coordinate system on an endoscope joystick according to an embodiment of this application;

[0040] Figure 4 (a) is a schematic diagram of the degrees of freedom of the rigid segment of an endoscope control lever in the RCM coordinate system according to an embodiment of this application;

[0041] Figure 4 (b) is a schematic diagram of the degrees of freedom of a flexible segment of an endoscope control lever in the RCM coordinate system according to an embodiment of this application;

[0042] Figure 5 This is a schematic diagram of the force and torque detected by a force and torque sensor and the force components of a rigid segment in the RCM coordinate system provided in an embodiment of this application;

[0043] Figure 6 This is a schematic diagram of another endoscopic robot provided in an embodiment of this application;

[0044] Figure 7 This is a schematic block diagram of a chip system provided in an embodiment of this application. Detailed Implementation

[0045] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0046] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0047] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0048] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0049] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0050] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0051] In traditional minimally invasive surgery, assistant surgeons manually operate a rigid endoscope to obtain visual images of the surgical area. However, surgeries can last for several hours, and prolonged operation of a rigid endoscope can cause muscle fatigue in the assistant surgeon's hands, leading to hand tremors and unstable visual images of the surgical area. Assistant surgeons have also found that traditional rigid endoscopes have drawbacks such as large size, simple structure, and fixed observation angle. With advancements in medical technology, flexible endoscopes with an added flexible section have been developed, which can expand the surgical field of view, but this also increases the difficulty of operation.

[0052] In recent years, with the development of robotics technology and the improvement of medical standards, medical robots have been successfully applied in the field of minimally invasive surgery. For example, medical robots include endoscopic robots, which can replace assistant doctors in controlling the endoscope. Research on endoscopic robots has revealed that the rigid segment of the joystick is constrained by the incision, allowing only translation and rotation along the axis of the rigid segment and rotation around the incision. This constraint is also known as the Remote Center of Motion (RCM) constraint. In other words, endoscopic robots need to satisfy the RCM constraint to control the endoscope.

[0053] To ensure endoscopic robots meet the RCM (Restricted Coordinate Mechanism) constraint requirements, special mechanical mechanisms, such as parallelogram structures, can be designed. However, these special mechanisms can fix the RCM point's position relative to the robotic arm. Alternatively, algorithms can be adjusted to allow RCM movement (e.g., the body's movements due to breathing can cause changes in the RCM point). However, during surgery, when the robotic arm is being dragged, the endoscope may fail to meet the RCM constraint. Currently, control methods for endoscopic robots include using motion planning with environmental sensing devices to achieve interaction between the robot and the user. However, this method requires external sensing equipment and involves significant data computation. Another approach is to install force sensors on the robot's joints and combine them with dynamics methods to achieve interactive control between the robot and the user. However, this method involves dynamics, and its implementation is complex.

[0054] In view of this, this application provides a control method under RCM constraints, applied to an endoscope robot. Under RCM constraints, by setting force and torque sensors at the connection between the end of the robotic arm and the endoscope, the forces and torques applied to the endoscope joystick are mapped from the force and torque detection coordinate system to the degrees of freedom corresponding to the rigid segments in the RCM coordinate system, providing a more intuitive interactive operation. From the motion control level, this method realizes visual servoing and interactive control of the endoscope robot under RCM constraints, making the implementation process simpler. Furthermore, this solution does not require external sensing devices, and the endoscope robot does not need to interact with or perform calculations with external sensing devices, thereby reducing the amount of data computation.

[0055] The control method under RCM constraints provided in this application will be described in detail below.

[0056] Figure 1 The diagram shown is a structural schematic of an endoscopic robot provided in an embodiment of this application. Figure 1 As shown, the endoscopic robot includes a robotic arm, an endoscope, and force and torque sensors.

[0057] The robotic arm includes a base, several levers (e.g., lever 1, lever 2, lever 3, lever 4), and several joints (e.g., joint 1, joint 2, joint 3, joint 4, joint 5). The base supports and secures the other components of the robotic arm, while the joints connect different numbers of levers, which can be 4, 5, 6, 7, or other numbers, without limitation herein. It is understood that the robotic arm can simulate the human arm, providing doctors with a series of simulated human arm and wrist movements. Within the confined space of the human body, it can complete many complex surgical tasks without trembling even during prolonged operation, thus assisting doctors in performing minimally invasive surgeries more stably, saving surgical time, improving surgical precision, and ultimately achieving better treatment outcomes.

[0058] like Figure 1 As shown, the endoscope provided in this application embodiment includes an endoscope control lever. The endoscope control lever includes a rigid section and may also include a flexible section. An endoscope camera is connected to one end of the flexible section.

[0059] like Figure 1 As shown in the embodiment of this application, the force and torque sensor is located at the connection point between the end of the robotic arm and the endoscope. This force and torque sensor is used to detect the force and torque applied to the endoscope. Since the force and torque applied to the endoscope are equivalent to the force and torque applied to the endoscope control lever, it can also be considered that the force and torque sensor is used to detect the force and torque applied to the endoscope control lever. The force and torque sensor can be a six-dimensional force sensor or other measuring devices capable of measuring force and torque.

[0060] It should be understood that the above Figure 1 The schematic diagram of the endoscopic robot shown is only intended to help those skilled in the art better understand the embodiments of this application, and is not intended to limit the scope of the embodiments of this application. Those skilled in the art can add or delete certain structures based on the above examples, or make equivalent modifications or changes to certain structures described above, and such modifications and changes are also within the scope of the embodiments of this application.

[0061] Figure 2 The diagram shown is a schematic flowchart of a control method under RCM constraints provided in an embodiment of this application. Figure 2 The method shown can be applied to control Figure 1 The endoscopic robot shown.

[0062] like Figure 2 As shown, an embodiment of this application provides a control method under RCM constraints, including steps S201 to S209, wherein:

[0063] S201, the endoscopic robot determines the RCM error and establishes RCM constraints based on the projection of the direction vector of the end position of the rigid segment relative to the RCM point and the orthogonal plane of the endoscopic control stick along the rod direction.

[0064] For example, such as Figure 3 As shown, with the RCM point (i.e. Figure 3 P in c A volume coordinate system established with point (x, y, z) as the origin is also called the RCM coordinate system. The RCM coordinate system includes the x-axis, y-axis, and z-axis. The RCM error of an endoscope is x. c It satisfies the following formula (1):

[0065] x c =B c T (P t -P c (1)

[0066] In formula (1), P c P represents the location of the RCM point. t This can indicate the position of the end of the rigid segment of the endoscope, denoted by P. t A volume coordinate system a is established with the origin as the coordinate origin. t o t n t Coordinate system, a t o t n t The coordinate system includes o t Axis, a t axis, n t axis, where n t The axis is along the direction of the endoscope control lever, such as... Figure 3 As shown, the RCM coordinate system and a t o t n t A coordinate system is a volume coordinate system with parallel coordinate axes established at different origins, where the x-axis is parallel to o. t The y-axis is parallel to a. t axis, z-axis and n t The axes are coaxial. t -P c B is the direction vector of the end position of the rigid segment of the endoscope relative to the RCM point. c =(a t o t ), a t Axis and o t The plane formed by the axes and n t Axis perpendicular, RCM error x c It is P t -P c With n tProjection onto an orthogonal plane.

[0067] It should be noted that in x c When the modulus approaches 0, i.e., |x c |→0, endoscopic RCM constraint is established. It should be noted that P t -P c With n t Projection x of the orthogonal plane c A value close to zero indicates that the smaller the error of the endoscope joystick constraining the RCM point, the better the stability of the endoscope joystick constraining the RCM point.

[0068] It should be noted that this scheme can still satisfy the RCM constraint during the movement of the robotic arm controlled by the endoscope robot, thus making the RCM constraint more stable. Moreover, implementing the RCM constraint from an algorithmic perspective can reduce the complexity of the mechanical structure, making the endoscope more flexible in satisfying the RCM constraint.

[0069] S202, Under RCM constraints, the endoscopic robot determines the rate of change of RCM error during the movement of the endoscope.

[0070] In one possible implementation, the RCM error during the rigid segment movement of the endoscope satisfies a stable formula as shown in formula (2) or formula (3):

[0071]

[0072] In formula (2), Indicates the rate of change of RCM error. The acceleration representing the change in RCM error, a and b satisfy the condition: a = b > 0, and a and b can be integers from 1 to 100. Preferably, a and b can be 50.

[0073]

[0074] In formula (3), The value c represents the rate of change of the RCM error. c can be an integer from 1 to 100. Preferably, c can be 50.

[0075] It is understandable that by solving the above formula (2) or formula (3), the rate of change of RCM error during the movement of the endoscope can be determined.

[0076] It should be noted that satisfying the above formula (2) or formula (3) can ensure the stability of the RCM point.

[0077] S203, the endoscopic robot maps the forces and torques applied to the endoscope joystick in the force and torque detection coordinate system to the degrees of freedom corresponding to the rigid segment of the endoscope joystick in the RCM coordinate system, and determines the force components in the degrees of freedom corresponding to the rigid segment of the endoscope joystick.

[0078] In one possible implementation, the force and torque applied to the endoscope lever are detected by a force and torque sensor in a force and torque detection coordinate system, which is a volume coordinate system established with the force and torque sensor as the origin.

[0079] Specifically, if the force and torque sensor is a six-dimensional force sensor, then the force and torque applied to the endoscope control lever are detected by the six-dimensional force sensor in a six-dimensional force coordinate system, which is a volume coordinate system established with the six-dimensional force sensor as the coordinate origin.

[0080] It should be noted that, in the embodiments provided in this application, the rigid segment of the endoscope control lever has four degrees of freedom relative to the RCM point. These four degrees of freedom include: rotation along three axes of the RCM coordinate system, and movement along the endoscope axis (corresponding to one axis of the RCM coordinate system), such as... Figure 4 As shown in (a), these represent rotation along the x, y, and z axes, and movement along the z-axis, respectively. The rotation angles along the x, y, and z axes are denoted as α, β, and γ, respectively, and the movement distance along the z-axis is denoted as δ. Additionally, as... Figure 4 As shown in (b), the flexible segment of the endoscope control lever includes two degrees of freedom: the central angle θ of the curved arc and the rotation angle ψ of the plane containing the arc, and as shown in (b). Figure 4 As shown in (b) of the diagram, the flexible segment of the endoscope control lever is flexible. It is understood that, as... Figure 4 As shown in (b), the endoscope camera connected to the flexible end of the endoscope control lever can also be bent.

[0081] In one possible implementation, the forces and torques applied to the endoscope control lever in the force and torque detection coordinate system are mapped to the degrees of freedom corresponding to the rigid segment of the endoscope control lever in the RCM coordinate system. That is, the forces and torques applied to the endoscope control lever in the force and torque detection coordinate system are decomposed along different degrees of freedom of the endoscope control lever. The forces decomposed in the degrees of freedom corresponding to the rigid segment of the endoscope satisfy the following formulas (4) to (7):

[0082]

[0083]

[0084]

[0085]

[0086] In formulas (4) to (7), F α F β F γ F δ These are the components of the force and torque applied to the endoscope control lever along the rigid segment of the endoscope control lever in four degrees of freedom relative to the RCM point, respectively. h =n t T (P t -P c ), f h τ h These are the forces and torques applied to the endoscope control lever, detected in the force and torque detection coordinate system, respectively, where f h =(f hx f hy f hz ), τ h =(τ hx , τ hy ,τ hz ).

[0087] S204, based on the mass damping model and the force components on the degrees of freedom corresponding to the rigid segment of the endoscope control lever, the endoscope robot determines the motion velocity of the rigid segment of the endoscope control lever.

[0088] In one possible implementation, the movement speed of the rigid segment of the endoscope lever satisfies the following formula (8):

[0089]

[0090] In formula (8), D represents the acceleration of the rigid segment of the endoscope control lever under the influence of force and torque. f The damping coefficient of the rigid section of the endoscope control lever under the influence of force and torque. Let F be the velocity of the rigid segment of the endoscope control lever under the influence of force and torque, and F be the force applied to the endoscope control lever. F = [F...] α F β F γ F δ ] T It should be noted that F α F β F γ F δ These are the components of the force F along the rigid segment of the endoscope control lever in the four degrees of freedom relative to the RCM point.

[0091] S205, based on the movement speed of the rigid segment of the endoscope joystick and the rate of change of the RCM error, the endoscope robot determines the movement speed of each joint among the multiple joints of the robotic arm.

[0092] In one possible implementation, the joint velocity of each of the multiple joints of the robotic arm is determined according to formula (9):

[0093]

[0094] Where, q r For the joint speed of the robotic arm, Let A be the pseudo-inverse of matrix A, and A = B. c T [I 3×3 (P t -P c )^]J t J t Let N be the Jacobian matrix of the robotic arm, and N = [Z]. T G T ],in G is the Jacobian matrix of the robotic arm. t The zero space; J is the Jacobian matrix of the robotic arm. t The pseudo-inverse, R t =[a t o t n t ] represents the direction matrix at the end of the rigid segment of the endoscope.

[0095] It should be noted that the matrix S in formula (9) varies depending on the number of joints included in the robotic arm.

[0096] S206, the kinematic relationship of visual servoing of the endoscope under RCM constraint is derived for the endoscope robot, and the motion relationship between the endoscope and the target object in the image coordinate system is obtained.

[0097] For example, in order to achieve the visual servoing task, the motion state of the target object's pixels in the image coordinate system is obtained based on visual information. Considering the constraint of the endoscope joystick on the RCM point, the kinematic model of the endoscope is established to obtain the motion relationship between the endoscope and the target object's pixels in the image coordinate system.

[0098] In one possible implementation, the motion relationship between the endoscope and the target object's pixels in the image coordinate system satisfies the following formula (10):

[0099]

[0100] In formula (10), J is the Jacobian matrix of the endoscope. Let ζ be the angular velocity or speed of the endoscope, where ζ = [α, β, γ, δ, ψ, θ], and α, β, γ are the rotation angles of the rigid segment of the endoscope control lever, δ is the distance the rigid segment of the endoscope control lever moves along the lever direction, ψ, θ are the motion angles of the flexible segment of the endoscope control lever, and b is the displacement of the target object in the image coordinate system. This represents the velocity of the target object in the image coordinate system.

[0101] S207, the endoscopic robot characterizes the motion relationship between the endoscope and the target object in the image coordinate system and the motion speed of the rigid segment of the endoscope joystick as a quadratic programming problem.

[0102] In one feasible implementation, the motion relationship between the endoscope and the target object's pixels in the image coordinate system, and the motion speed of the rigid segment of the endoscope's control lever, are characterized as a quadratic programming problem satisfying a minimization performance index, equality constraints, and bilateral constraints, wherein:

[0103] The performance metric to be minimized is expressed as formula (11):

[0104]

[0105] In formula (11), W represents the weighting matrix.

[0106] The equality constraint is expressed as formula (12):

[0107]

[0108] In formula (12), λ = diag[1; 1; 1; 1; 0; 0], The target speed of movement for the endoscope control lever.

[0109] The bilateral constraints satisfy formula (13):

[0110]

[0111] In formula (13), x - and x + The x represents the angular and / or positional limits of the robotic arm joints. The superscripts + and - in x represent the upper and lower limits of the robotic arm joint limits.

[0112] Where J is the Jacobian matrix of the endoscope. Let ζ be the angular velocity or speed of the endoscope, ζ = [α, β, γ, δ, ψ, θ], where α, β, and γ are the rotation angles of the rigid segment of the endoscope control lever, δ is the translational distance of the rigid segment of the endoscope control lever along the lever direction, and ψ and θ are the motion angles of the flexible segment of the endoscope. To track the velocity of the target point moving in the image space.

[0113] It is understandable that the motion relationship between the endoscope and the target object in the image coordinate system and the motion speed of the rigid segment of the endoscope joystick can be represented as a quadratic programming problem. It can also be understood that the visual servoing task and interactive control of the endoscope under RCM constraints can be represented as a quadratic programming problem.

[0114] S208, the endoscopic robot solves a quadratic programming problem to determine the angular velocity of the flexible segment of the endoscopic control lever.

[0115] In one feasible implementation, a quadratic programming problem is solved using a quadratic programming solver to determine the motion angles ψ and θ of the flexible segment of the endoscope control lever.

[0116] S209, based on the movement speed of the rigid segment of the endoscope joystick, the movement angular velocity of the flexible segment, and the movement speed of each joint among the multiple joints of the robotic arm, the endoscope robot controls the movement of the endoscope and the robotic arm respectively.

[0117] For example, the endoscopic robot controls the movement of the endoscope based on the movement speed of the rigid segment and the movement angular velocity of the flexible segment of the endoscope joystick, and controls the movement of the robotic arm based on the movement speed of each of the multiple joints included in the robotic arm.

[0118] Under RCM constraints, by setting force and torque sensors at the connection between the end of the robotic arm and the endoscope, the forces and torques applied to the endoscope joystick are mapped from the force and torque detection coordinate system to the degrees of freedom corresponding to the rigid segments in the RCM coordinate system. This provides a more intuitive interactive operation. From a motion control perspective, it enables visual servoing and interactive control of the endoscope robot under RCM constraints. The implementation process is simpler, and this solution does not require external sensing devices. The endoscope robot does not need to interact with or perform calculations with external sensing devices, thereby reducing the amount of data computation.

[0119] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0120] For example, refer to Figure 6The endoscopic robot includes: a robotic arm 610, an endoscope 630, a force and torque sensor 620, and a processor 640. The endoscope 630 includes an endoscope joystick, which comprises a rigid section and a flexible section. The force and torque sensor 620 is located at the connection point between the robotic arm and the endoscope. The force and torque sensor 620 is used to detect the forces and torques applied to the endoscope joystick. The processor 640 is used for:

[0121] The magnitude of the RCM error is determined and the RCM constraint is established based on the projection of the direction vector of the end position of the rigid segment relative to the RCM point and the orthogonal plane of the endoscope control lever along the lever direction.

[0122] Under RCM constraints, the forces and moments applied to the endoscope control lever in the force and moment detection coordinate system are mapped to the degrees of freedom corresponding to the rigid segment in the RCM coordinate system, and the force components in the degrees of freedom corresponding to the rigid segment are determined.

[0123] The motion velocity of the rigid segment is determined based on the mass damping model and the force components on the degrees of freedom corresponding to the rigid segment.

[0124] The movement of the endoscope and the robotic arm is controlled according to the movement speed of the rigid segment, the movement angular velocity of the flexible segment, and the movement speed of each joint among the multiple joints of the robotic arm.

[0125] Optionally, in one possible implementation, the processor 640 is further configured to: determine the rate of change of the RCM error under RCM constraints during the movement of the endoscope; and determine the movement speed of each joint among the multiple joints of the robotic arm based on the movement speed of the rigid segment and the rate of change of the RCM error of the rigid axis.

[0126] Optionally, in one possible implementation, the processor 640 is further configured to: determine the motion relationship between the endoscope and the target object in the image coordinate system;

[0127] The motion relationship between the endoscope and the target object in the image coordinate system and the motion velocity of the rigid segment are characterized as a quadratic programming problem.

[0128] Solve the quadratic programming problem to determine the angular velocity of the flexible segment.

[0129] Optionally, in one possible implementation, the processor 640 is further configured to: determine the force components on the degree of freedom corresponding to the rigid segment according to the following formula:

[0130]

[0131]

[0132]

[0133]

[0134] Among them, F α F β F γ F δ These are the force and torque components applied to the endoscope control lever along the rigid segment in four degrees of freedom relative to the RCM point, respectively. h =n t T (P t -P c ), where P c P represents the position of the RCM point. t Indicates the position of the end of the rigid segment, f h and τ h These are the forces and torques applied to the endoscope control lever, detected in the force and torque detection coordinate system, respectively, where f h =(f hx f hy f hz ), τ h =(τ hx , τ hy ,τ hz ).

[0135] Optionally, in one possible implementation, the processor 640 is further configured to: determine the motion velocity of the rigid segment according to the following formula:

[0136]

[0137] Where F is the force applied to the endoscope control lever, F = [F α F β F γ F δ ] T , where D is the acceleration of the endoscope control lever under the influence of force and torque. f The damping coefficient of the endoscope control lever under the influence of force and torque. The velocity of the rigid segment under the influence of force and torque.

[0138] Optionally, in one possible implementation, the processor 640 is further configured to: determine the rate of change of the rigid shaft RCM error according to the following formula:

[0139]

[0140] or,

[0141]

[0142] in, This represents the acceleration of the end of the rigid segment relative to the RCM point. x represents the rate of change of the RCM error of the rigid shaft. c Let a represent the RCM error, which is the projection of the direction vector of the end position of the rigid segment relative to the RCM point onto the orthogonal plane of the endoscope control lever along the lever direction. a and b satisfy the condition: a = b > 0, and a, b, and c are all integers from 1 to 100.

[0143] Optionally, in one possible implementation, the processor 640 is further configured to: determine the motion speed of each of the plurality of joints comprising the robotic arm according to the following formula:

[0144]

[0145] Where, q r The motion speed of each joint in the multiple joints of the robotic arm. Let A be the pseudo-inverse of matrix A, and A = B. c T [I 3×3 (P t -P c )^]J t J t Let N be the Jacobian matrix of the robotic arm, and N = [Z]. T G T ],in, G is the null space of the Jacobian matrix Jt of the robotic arm. J is the Jacobian matrix of the robotic arm. t The pseudo-inverse, R t =[a t o t n t ] represents the direction matrix at the end of the rigid segment of the endoscope. This indicates the rate of change of the RCM error of the rigid shaft. The velocity of the rigid segment under the influence of force and torque.

[0146] It should be noted that the information interaction and execution process between the above-mentioned devices / units are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.

[0147] It is understood that the structure illustrated in this application does not constitute a specific limitation on the endoscopic robot 600. In other implementations of this application, the endoscopic robot 600 may include more or fewer components than illustrated, or combine some components, or separate some components, or have different component arrangements.

[0148] This application also provides a chip system, such as Figure 7 As shown, the chip system includes at least one processor 710 and at least one interface circuit 720. The processor 710 and the interface circuit 720 are interconnected via lines. For example, the interface circuit 720 can be used to send signals to other devices (e.g., the processor 710). Exemplarily, the interface circuit 720 can read instructions stored in memory and send those instructions to the processor 710. When the instructions are executed by the processor 710, the chip system can perform the various steps of the endoscopic robot described in the above example. Of course, the chip system may also include other discrete components, which are not specifically limited in this application.

[0149] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0150] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps described in the various method embodiments above.

[0151] This application provides a computer program product that, when run on an endoscopic robot, enables the endoscopic robot to perform the steps described in the above-described method embodiments.

[0152] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include at least: any entity or device capable of carrying computer program code to a photographing device / terminal device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks. In some jurisdictions, according to legislation and patent practice, computer-readable media cannot be electrical carrier signals or telecommunication signals.

[0153] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0154] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0155] In the embodiments provided in this application, it should be understood that the disclosed endoscopic robot and method can be implemented in other ways. For example, the endoscopic robot embodiments described above are merely illustrative. For instance, the division of the endoscopic robot structure is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interfaces, devices, or units, and may be electrical, mechanical, or other forms.

[0156] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0157] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A control method under RCM constraints, characterized in that, An application is made in an endoscopic robot, the endoscopic robot further comprising: a robotic arm, an endoscope, and force and torque sensors. The endoscope includes an endoscope control lever, the endoscope control lever comprising a rigid section and a flexible section. The force and torque sensors are located at the connection point between the robotic arm and the endoscope, and the force and torque sensors are used to detect the forces and torques applied to the endoscope control lever. The method includes: The magnitude of the RCM error is determined and the RCM constraint is established based on the projection of the direction vector of the end position of the rigid segment relative to the RCM point and the orthogonal plane of the endoscope control lever along the lever direction. During the movement of the endoscope, the rate of change of the RCM error under the RCM constraint is determined; Based on the movement speed of the rigid segment and the rate of change of the RCM error, the movement speed of each joint among the multiple joints of the robotic arm is determined; Determine the motion relationship between the endoscope and the target object in the image coordinate system; The motion relationship between the endoscope and the target object in the image coordinate system and the motion velocity of the rigid segment are characterized as a quadratic programming problem. Solve the quadratic programming problem to determine the angular velocity of the flexible segment. Under the RCM constraint, the forces and torques applied to the endoscope control lever in the force and torque detection coordinate system are mapped to the degrees of freedom corresponding to the rigid segment in the RCM coordinate system, and the force components in the degrees of freedom corresponding to the rigid segment are determined. The motion velocity of the rigid segment is determined based on the mass damping model and the force components on the corresponding degrees of freedom of the rigid segment. The movement of the endoscope and the robotic arm are controlled according to the movement speed of the rigid segment, the movement angular velocity of the flexible segment, and the movement speed of each joint among the multiple joints of the robotic arm.

2. The method as described in claim 1, characterized in that, The force components on the degree of freedom corresponding to the rigid segment satisfy the following formula: in, , , , These are the force and torque applied to the endoscope control lever, respectively, along the rigid segment in four degrees of freedom relative to the RCM point. , wherein Indicates the position of the RCM point. Indicates the position of the end of the rigid segment, the and stated These are the force and torque applied to the endoscope control lever, detected in the force and torque detection coordinate system, respectively. = ( ), = ( ).

3. The method as described in claim 2, characterized in that, The velocity of the rigid segment satisfies the following formula: Where F is the force applied to the endoscope control lever. , The acceleration of the endoscope control lever under the influence of force and torque. The damping coefficient of the endoscope control lever under the influence of the forces and torques described above. The velocity of the rigid segment under the influence of the force and torque.

4. The method as described in claim 1, characterized in that, The rate of change of the RCM error satisfies the following formula: or, in, Indicates the RCM error, the This indicates the rate of change of the RCM error. The acceleration representing the change in the RCM error, wherein a and b satisfy the condition: The All values ​​are integers between 1 and 100.

5. The method as described in claim 1, characterized in that, The movement speed of each joint in the robotic arm is determined according to the following formula: in, The motion speed of each of the multiple joints included in the robotic arm. The For matrix The false rebellion, The Let be the Jacobian matrix of the robotic arm. ,in, The The Jacobian matrix of the robotic arm The zero space, the The Jacobian matrix of the robotic arm The false rebellion, , represents the orientation matrix at the end of the rigid segment of the endoscope, the This indicates the rate of change of the RCM error. The velocity of the rigid segment under the influence of the force and torque.

6. An endoscopic robot, characterized in that, The endoscopic robot includes: a robotic arm, an endoscope, force and torque sensors, and a processor. The endoscope includes an endoscope joystick, which includes a rigid section and a flexible section. The force and torque sensors are located at the connection between the robotic arm and the endoscope. The force and torque sensors are used to detect the forces and torques applied to the endoscope joystick. The processor is used to perform the method as described in any one of claims 1 to 5.

7. A chip, characterized in that, include: A processor for retrieving and running a computer program from memory, such that when a processor with the chip mounted executes the computer program, it implements the method as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 5.

Images