RU2774990C1 - Method for controlling a robotic surgical complex - Google Patents

Abstract

FIELD: medical technology.

SUBSTANCE: invention relates to medicine, namely to methods for controlling a robotic surgical complex. The complex contains a surgeon’s console equipped with a main manipulator with a control handle made in the form of a planetary lever mechanism, a patient’s stand with a delivery unit equipped with an executive manipulator with surgical instruments and a video camera attached to them, a display and local controllers connected to an automatic control system and servo encoders in the systems of the main and executive manipulators, each of which is characterized by a coordinate system. The method includes continuous determination by means of encoders of coordinates and angular positions of the main manipulator, coordinates and angles of rotation of the knees of the delivery unit, coordinates of executive manipulators with a video camera and with a surgical instrument. The trocar point is selected with its position fixed in the global coordinate system. The angular positions, points and vectors of the delivery unit and the main manipulator are determined in the global coordinate system and in the coordinate system of the camera. The deviations of the points and vectors of the control handle in the coordinate systems of the main manipulator and the display are determined by a mathematical algorithm as a result of the operator’s hand acting on the control handle, causing the movement of the surgical instrument in the operating field by means of deviations of the points, vectors and angular positions of the rod and the end effector of the surgical instrument, and the executive manipulator calculated by a mathematical algorithm. The calculated values are transmitted to the servo motors of the executive manipulator.

EFFECT: dynamic accuracy of positioning of the surgical instrument is achieved.

6 cl, 11 dwg

Description

The invention relates to the field of medical equipment, in particular to a method for controlling a robotic surgical complex.

The traditional type of surgery is laparotomy, which involves making a long incision in the abdomen through which traditional surgical instruments are inserted. However, the implementation of long incisions inevitably leads to high blood loss and long-term recovery of patients. In addition, the risk of infectious complications increases.

The use of minimally invasive surgical interventions (laparoscopy) makes it possible to eliminate these shortcomings. Instead of one long incision, four to five small incisions are made on the patient through which long and thin surgical instruments and endoscopic cameras are inserted. This method reduces blood loss, reduces the time spent in the hospital and reduces the pain of the patient during the recovery period. However, despite the above advantages, laparoscopy requires extremely high skills from the surgeon. The entry incision acts as a pivot point, reducing the freedom to position and orient the instruments within the patient. In addition, work with endoscopic instruments forces surgeons to work in an uncomfortable position, which can be tiring for several hours of work, sensitivity decreases, and the surgeon's hand tremor increases.

The development of laparoscopy has led to the emergence of robotic surgery and, in particular, to the possibility of using robotic surgical systems. Minimally invasive surgical operations using robotic systems are characterized by low trauma, faster patient recovery, reduced risk of complications and high efficiency.

With the help of a computerized robotic interface, these systems allow remote laparoscopy, when the surgeon sits at the console and controls the manipulators, which transmit movements directly to the surgical instruments, using the control handle.

There are various ways to control the movement of a surgical instrument attached to the executive manipulator of a robotic surgical complex.

A known method for controlling a camera in a robotic surgical complex (RF patent No. 2721461, IPC B25J 9/16, B25J 19/04, A61B 34/37, published on May 19, 2020), which is characterized by the following steps:

transmission of motion vector data via three translational and three rotational degrees of freedom of the right and left control controllers to the automatic control system at a constant frequency,

switching the control controllers to the control mode of the camera fixed on the manipulator by pressing the control pedal;

control of the movement of the manipulator with the camera attached to it by simultaneously moving the control controllers and then performing the following steps:

the step of storing the received motion vectors of the control controllers in the automatic control system;

the stage of processing and combining the motion vectors of the right and left control controllers, characterizing the translational motion, to find the total displacement;

a step of scaling the displacement vector obtained in the previous step;

motion compensation stage;

a step of compensating for a deviation of the horizon line from the center line of the frame;

the stage of simultaneous transmission of the data obtained in the previous steps to the actuators of the manipulator for its translational movement and rotation around the longitudinal axis of the camera fixed on it.

This method solves the problem of controlling a camera mounted on one of the manipulators using two master manipulators. The method of transferring the control commands of the coordinate system of the leading manipulators to the commands to the drives of the executive manipulator with the installed camera is described. The method of intuitive control of instruments and executive manipulators from the surgeon's leading manipulators is not presented.

A known method of controlling a medical robotic system (US patent No. US 7785320, IPC A61B-017/00, A61B-017/04, A61B-017/11, A61B-017/28, A61B-019/00, A61B-019/02, A61B-019/08, published 08/31/2010), including: receiving information from the first set of sensors that determine the movement of the input device; commanding the manipulator to move the surgical instrument around the pivot point in response to the received information about the movement of the input device; receiving information from the second set of sensors that determine the movement of the manipulator; determining whether the pivot point is out of the predetermined range based on the obtained information about the movement of the manipulator; as well as sending a command to the manipulator to stop the surgical instrument around the pivot point if the pivot point is outside the specified range of values.

This method describes only the way to control the manipulator and the tool without reference to the camera coordinate system. An actuator with 7 degrees of freedom is considered. The leading manipulator having 5 degrees of freedom is considered. There is no description of the technical solution that provides an intuitive transfer of the control action from the leading manipulator to the executive manipulator, taking into account all axes of the system.

A known method for positioning a manipulating arm (RF patent No. 2719919, IPC A61B 34/20, A61B 34/30, B25J 9/10, published on April 23, 2020) in the coordinate system (X, Y, Z) of a device for robotic surgery, in which coordinates (xz, yz, zz) of the target zone in the patient and at which, for positioning the manipulating hand, the tool block is connected to the interface block of the manipulating hand, and the tool block includes a surgical instrument that has a tool shank with a longitudinal axis, in which, when connecting the instrumental block with a conjugation block, using the control block, an intermediate vector (V) perpendicular to the longitudinal axis is determined between the longitudinal axis and the given coordinates (xz, yz, zz) of the target zone, and at which the first optical and/or acoustic signal is output using the output unit, when the value defined intermediate vector (V) is at or below the first preset value .

In this method, only the method of positioning and location of coordinate systems associated with the surgical field, the patient's body and the manipulator is considered. The control associated with the coordinate system of the video camera is not considered. This method does not solve the problem of intuitive control. The method of transferring the control action from the hands of the surgeon to the manipulator is not described.

A known method of moving the end effector of the manipulator along a predetermined trajectory (US patent No. 8774969, IPC G05B19/18, published 07/08/2014), in which the manipulator has zero space in relation to a predetermined trajectory, with at least two positions of the manipulator associated with the same position of the end actuator, while the manipulator is placed in zero space, and in the processor the process variable of the end effector is automatically changed in accordance with the detected placement. The position of the manipulator is described by the hinge angle q=[q 1, q 2, … q 6]. The end effector, indicated by the instrument's center point TCP, must travel a predefined horizontal path x(s). By default, during operation, the robot moves along a predetermined path x(s) with a constant speed dx/dt=v. The method includes the following steps: registering the placement of the manipulator in the specified null space, wherein the specified placement is direct manipulation of the specified manipulator in the specified null space; and automatically changing at least one process variable corresponding to the registered placement, wherein said at least one process variable is changed without changing the position of the end effector relative to said predetermined path.

This technical solution describes a method for planning the movement of an end effector point along a predetermined trajectory at a constant speed. The method proposed in the claimed technical solution ensures the movement of the end effector along the trajectory specified by the operator's hand.

A known method of controlling a medical system of minimally invasive intervention (RF patent No. 2518806, IPC B25J 13/02, published 06/10/2014), containing a manipulator having an executive body equipped with a 6 degrees of freedom (6-DOF) force/torque sensor, which has a coordinate system (X, Y, Z) of a sensor with three mutually perpendicular axes, and a minimally invasive instrument, having a first end fixed on the executive body, a second end located behind the external center of rotation, which limits the tool in movement, and a tool rod , whose longitudinal axis is collinear with the axis (Z) of the tool of the coordinate system (X, Y, Z) of the sensor,

wherein the method includes determining the position of the instrument relative to the external center of rotation, including determining the vector of the initial initial distance from the initial coordinate system (X, Y, Z) of the sensor to the external center of rotation, which, in turn, includes moving the instrument of minimal invasive intervention along two axes (X, Y) coordinate system (X, Y, Z) of the probe perpendicular to the (Z) axis of the tool, respectively, as long as the reaction force along both (X, Y) axes is below the specified threshold value, and determining the position of the outer center of rotation along the axis ( Z) of the tool, using the law of the lever, in particular by turning the tool until a sufficient contact force is reached, measuring the module of the moment vector and the module of the force vector corresponding to this contact force, and calculating the position of the external center of rotation along the axis (Z) of the tool by dividing the module of the moment vector to the modulus of the force vector.

The described technical solution is aimed at determining the force and moment by 6 degrees of freedom at the outer center of rotation and at the end of the working part of the tool. This solution does not provide a way to transfer control from the leading manipulator of the surgeon to the executive manipulator in the operated area.

The technical task of the claimed invention is to ensure accurate positioning of the surgical instrument and improve the intuitiveness of control.

EFFECT: controlling the movement of a surgical instrument of a robotic surgical complex, which ensures dynamic positioning accuracy of the surgical instrument.

The technical result is achieved by the fact that the method of controlling a robotic surgical complex containing the following units: a surgeon's console equipped with a main manipulator with a control handle made in the form of a planetary lever mechanism, a delivery unit equipped with executive manipulators with surgical instruments and a video camera attached to them, a display and local controllers associated with the automatic control system and servo encoders in the systems of the main and executive manipulators, each of which is characterized by a coordinate system; includes continuous determination by means of encoders of the coordinates and angular positions of the main manipulator, the coordinates and angles of rotation of the knees of the delivery unit, the coordinates of the executive manipulators with a video camera and with a surgical instrument; selection of a trocar point with fixation of its position in the global coordinate system; determining the basic angular positions, points and vectors of the delivery unit and the main manipulator in the global coordinate system and in the camera coordinate system; determination by a mathematical algorithm of deviations of points and vectors of the control arm in the coordinate systems of the main manipulator and the display as a result of the operator’s hand on the control arm, causing the surgical instrument to move in the operating field by means of deviations of points, vectors and angular positions of the rod and end effector of the surgical instrument calculated by the mathematical algorithm instrument and executive manipulator; transfer of the calculated values to the servomotors of the executive manipulator.

Brief description of the drawings

Fig. 1 - Main parameters of the main manipulator;

Fig. 2 - Main parameters of the delivery node;

Fig. 3 - Basic mathematical parameters of the executive manipulator;

Fig. 4 - The coordinate system of the video camera and the coordinate system of the camera matrix;

Fig. 5 - Display coordinate system D (X _D , Y _D , Z _D );

Fig. 6 - Schematic arrangement of the main and auxiliary vectors of the main manipulator;

Fig. 7 - Main parameters of the end effector of the surgical instrument;

Fig. 8 - View of the plane P1;

Fig. 9 - View of the plane P2;

Fig. 10 - Schematic arrangement of vectors of the main manipulator;

Fig. 11 - Flowchart for calculating deltaRot bias.

Description of the invention

The invention relates to a method for controlling a robotic surgical complex for performing a surgical operation.

At the same time, the robotic surgical complex includes a surgeon's console, equipped with a main manipulator with a control handle; a delivery unit equipped with executive manipulators, on which a surgical instrument and a video camera are fixed to view the surgical field; display to which the image from the video camera is transmitted; local controllers associated with servo encoders in the systems of the main and executive manipulators, providing data processing and sending data from the executive manipulator to the main manipulator and control signals from the main manipulator to the executive manipulator; automatic control system, which is connected with control controllers and executive manipulators.

The components of the robotic surgical complex, namely, the display, the main manipulator, the flange of the executive manipulator, the delivery unit, including the executive manipulator, as well as the surgical instrument and the video camera installed on the executive manipulators, are characterized by the following coordinate systems:

1) Movable coordinate system t of the main manipulator (Xt, Yt, Zt) and fixed coordinate system N of the main manipulator (X _N , Y _N , Z _N )

The main manipulator (Fig. 1) is a planetary-lever mechanism and includes levers AB, BC and COj, the lengths of which, respectively, are L1, L2, L3, and a control handle that forms a planetary system made in the form of three arcs of circles with a common center at the point pOj with the possibility of rotation relative to the levers of the main manipulator, the direction and position of which are determined using the vector myV and the point pOj.

The origin of the fixed coordinate system N of the main manipulator (X _N , Y _N , Z _N ) corresponds to point O (coincides with point A), where the lever AB is attached to the pulley of the main manipulator. The coordinate system N of the main manipulator (X _N , Y _N , Z _N ) is fixed and connected to the ground, while the XN axis is directed from left to right, the YN axis is directed vertically down, and the ZN axis is directed perpendicular to the XN and YN axes from the surgeon forward.

The coordinate system (Xt, Yt, Zt) is connected to the knees COj and BC and is mobile relative to the ground. The origin of the moving coordinate system t of the main manipulator (Xt, Yt, Zt) corresponds to the point Oj. The lever COj is rigidly attached to the lever BC so that the levers BC and COj form a right angle.

The axes of the coordinate system (Xt,Yt,Zt) are arranged as follows: the Yt axis is directed along the vector OjС from top to bottom, the Zt axis is aligned with the knee CB (from the surgeon), and the Xt axis is directed horizontally from left to right and perpendicular to the Zt and Yt axes.

2) Delivery node coordinate system (X _G , Y _G , Z _G )

A global coordinate system (X _G , Y _G , Z _G ) is associated with the rack of the delivery node (Fig. 2), in which the Y _G axis is directed vertically down along the rack, the X _G axis is from left to right, and the Z _G axis is inward and perpendicular to the axes Y _G and X _G .

The origin of the coordinate system is located in the plane of the upper section of the rack at the point Osh.

3) Auxiliary coordinate system of the arm flange (iAT, kSym, nSym)

The origin of the auxiliary coordinate system (Fig. 3) of the executive manipulator flange (jAT, kSym, nSym) corresponds to point C1 (corresponds to point pA).

4) Surgical instrument coordinate system (iXM, jYM, kZM)

The origin of the coordinate system of the surgical instrument (iXM, jYM, kZM) mounted on the executive manipulator corresponds to the pD0 point, while the kZM axis is directed along the surgical instrument, the jYM and iXM axes are directed at angles to the kZM axis (Fig. 3).

5) Camera coordinate system Cam (Xcam, Ycam, Zcam)

The origin point of the camera coordinate system (Xcam, Ycam, Zcam) is at the point of the trocar of the video camera pTrC (FIG. 4) corresponding to the point of the trocar pTr indicated in FIG. 3. At the same time, the coordinate system of the camera matrix (Xr, Yr, Zr) is also located on the Zcam axis, the origin of which determines the location of the central pixel of the camera matrix. The distance between the central pixel of the video camera and the point of the trocar of the video camera is set by the parameter LTrD0C. The video camera coordinate axes (Xr, Yr) are collinear to the corresponding axes of the video camera coordinate system (Xcam, Ycam).

6) Display coordinate system D (X _D , Y _D , Z _D )

In general, the surgeon's gaze (head) is in the display coordinate system D (X _D , Y _D , Z _D ) associated with the display, which is rotated by an angle aD around the X _N axis of the N coordinate system of the main manipulator (X _N , Y _N , Z _N ) (Fig. 5). The origin of the display coordinate system D (X _D , Y _D , Z _D ) is located at point A, which coincides with point O - the origin of the coordinate system of the main manipulator (X _N , Y _N , Z _N ).

The display coordinate system D (X _D , Y _D , Z _D ), and the camera coordinate system Cam (Xcam, Ycam, Zcam) are similar coordinate systems located in the central pixel of the video camera.

At each stage of the method for controlling the robotic surgical complex, the angular positions and coordinates of points and vectors of the main and executive manipulator are continuously measured by means of encoders installed on servo drives in the systems of the main and executive manipulators. In this case, to determine the position of the surgical instrument, the above coordinate systems of the components of the robotic surgical complex are used.

The method for controlling the robotic surgical complex includes:

1. Bringing by the operator of the surgical instrument fixed on the executive manipulator to the trocar

The operator, using the positioning system, brings the surgical instrument fixed on the executive manipulator to the trocar. Then the surgical instrument is inserted into the trocar.

2. Fixing the trocar point

After fixing the axes of the delivery node, the position of the trocar point in the global coordinate system (X _G , Y _G , Z _G ) is determined. After the surgical instrument is inserted into the trocar, the axes of the coordinate system associated with the surgical instrument (iXM, jYM, kZM) in the global coordinate system (X _G , Y _G , Z _G ) are determined. This includes the coordinate system associated with the video camera (Xcam, Ycam, Zcam).

Further, after determining all the above coordinate systems of the components of the robotic surgical complex and fixing the trocar point, the coordinates and angles of rotation of the knees of the delivery unit, the coordinates of the executive manipulators with a video camera and the executive manipulators with a surgical instrument installed on the delivery unit, as well as the coordinates of the central pixel of the video camera matrix, are determined. and coordinates of the end of the rod of the surgical instrument, with subsequent coordination of the above coordinate systems with each other.

3. Determination of base points and vectors of the delivery node and coordination of coordinate systems in static mode

3.1. Determining the coordinates and angles of rotation of the knees of the delivery node in the coordinate system of the delivery node (X _G , Y _G , Z _G )

The robotic surgical complex contains 4 delivery nodes, each of which has a manipulator, while the delivery node (Fig. 2) includes the first knee A0A1, the second knee B0B1, the third (inclined) knee C0C1.

Each of the delivery units mounted on the patient stand (Fig. 2) has three vertical axes of rotation with encoders (OiA0, A1B0 and B1C0) registering and fixing the angle of rotation of each knee A0A1, B0B1 and C0C1 of the delivery unit with respect to the previous knee. In addition, each delivery unit has its own motor, which moves the entire delivery unit together with the manipulator in the vertical direction, changing its height or the coordinate yOi of the base point Oi.

For each manipulator, let's designate the axis of the first knee - OiA0, the axis of the second knee - A1B0 and the axis of the third knee - B1C0. In this case, the base point of the delivery node Oi has coordinates {xOi, yOi, zOi}, where xOi, zOi are constants, yOi is a variable.

At the end (the plane passing through the point C1 perpendicular to the axis C0C1) there is another encoder attached to the manipulator itself. This encoder changes its values during a surgical operation.

According to the design of the delivery node (figure 2), the point pA0 is located above the point Oi (along the negative direction of the Y _G axis in the global coordinate system of the delivery node, therefore, the coordinates of the main points of the delivery node are calculated as follows (Formulas 1-5):

where pA0 is a constant,

LD1 - length of the first leg A0A1 (constant),

LD2 - length of the second leg B0B1 (constant),

alf1 - angle of rotation of the first knee A0A1 relative to the patient's stand,

alf2 - angle of rotation of the second leg B0B1 relative to the first leg A0A1,

dh1 - rise (height) of the first knee OiA0 (constant);

dh2 - rise (height) of the second knee B0B1,

dh3 - rise (height) of the third knee С0С1,

xOi, zOi - constants, yOi - variable.

The angle of rotation of the third knee С0С1 relative to the rack alf4 is determined as the sum of the angles:

where alf3 is the angle of rotation of the third knee С0С1 relative to the second knee B0B1.

Next, find the coordinates of the flange vectors of the executive manipulator (jATG, nSymG and kSymG) with a video camera and with a surgical instrument belonging to the coordinate system of the executive manipulator flange (jAT, kSym, nSym) in the coordinate system of the video camera (Xcam, Ycam, Zcam) and in the global coordinate system delivery node (X _G , Y _G , Z _G ), respectively. In this case, the executive manipulator is fixed on the delivery unit at point C1 (Fig. 2), corresponding to the point pA of the flange of the executive manipulator (Fig. 3).

3.2. Determining the coordinates of the executive arm flange vectors in the global coordinate system of the delivery node (X _G , Y _G , Z _G ) for the executive arm with a surgical instrument and in the camera coordinate system (Xcam, Y _cam , Zcam) for the executive arm with a video camera

In order to distinguish between the names of variables for the executive manipulator with a video camera and for the executive manipulator with a surgical instrument, a corresponding suffix is added to the end of each variable:

• for an executive manipulator with a video camera - the letter C;

• for an executive manipulator with a surgical instrument - the letter M;

Thus, the jATMG, nSymMG, kSymMG and jATСG, nSymСG, kSymСG vectors denote the jATG, nSymG and kSymG vectors (Fig. 3) in the coordinate system of the operating manipulator with a surgical instrument and the executive manipulator with a video camera, respectively.

The coordinates of vectors jATMG, nSymMG and kSymMG of the executive arm with the tool (Fig. 3) are determined by matching the auxiliary coordinate system of the executive arm flange (jAT, kSym, nSym) with the global coordinate system of the delivery node (X _G , Y _G , Z _G ).

The coordinates of the jATMG vector along the virtual axis of the manipulator in the global coordinate system of the delivery node (XG, YG, ZG) are calculated as follows:

where alfHM is the angle of inclination of the third knee С0С1 to the horizon in the coordinate system of the manipulator with the tool,

alf4M - angle of rotation of the third knee С0С1 relative to the rack in the coordinate system of the manipulator with the tool.

In the same way, similarly to Formula (7), the coordinates of the vector nSymMG and kSymMG are calculated in the global coordinate system of the delivery node (X _G , Y _G , Z _G ):

where the nSymMG vector is a unit normal to the В1С0Tr plane of the executive manipulator with a surgical instrument, and the kSymMG vector is a unit vector perpendicular to the virtual axis passing through the points C1 and pTr and lying in the В1С0pTr plane for the executive manipulator with a surgical instrument.

Next, the coordinates of the jATСG, nSymСG and kSymСG vectors of the executive manipulator with the video camera are determined by matching the auxiliary coordinate system of the executive manipulator flange (jAT, kSym, nSym) with the camera coordinate system (X _cam , Y _cam , Z _cam ):

where alfHC is the angle of inclination of the third leg С0С1 to the horizon in the coordinate system of the manipulator with the camera,

alf4C - angle of rotation of the third knee С0С1 relative to the stand in the coordinate system of the manipulator with the camera.

In this camera coordinate system, the nSymMG vector is a unit normal to the В1С0Tr plane of the executive manipulator with a video camera, and the kSymMG vector is a unit vector perpendicular to the virtual axis and lying in the В1С0Tr plane for the executive manipulator with a video camera.

Then the flange center point is calculated for fastening the executive manipulator with the pC1M surgical instrument and the executive manipulator with the pC1C video camera:

where pC1M and pC1C are the coordinates of the point C1 in the coordinate system of the executive manipulator with a surgical instrument and in the coordinate system of the executive manipulator with a video camera, respectively;

pC0M and pC0C - coordinates of point С0 in the coordinate system of the executive manipulator with the surgical instrument and in the coordinate system of the executive manipulator with the video camera, respectively;

LD3 - the length of the third knee С0С1.

Then the coordinates of the trocar points pTr are found for the executive manipulator with a surgical instrument (pTrM) and for the executive manipulator with a video camera (pTrC) in the global coordinate system of the delivery node (X _G , Y _G , Z _G ).

3.3. Determining the coordinates of the pTrM trocar point of the executive manipulator with a surgical instrument and the pTrC trocar point of the executive manipulator with a video camera in the global coordinate system of the delivery node (X _G , Y _G , Z _G )

Four free parameters (angles of rotation of the knees of the delivery unit alf1, alf2 and alf3, as well as the linear movement of the delivery unit along the patient’s stand) for each executive manipulator allow you to select the point of the trocar pTr (Fig. 3) arbitrarily, as well as choose the projection of the virtual axis on the horizontal plane in any desired way.

The pTr trocar point is a structurally fixed point located at the site of the patient's skin puncture, on the pApTr virtual axis.

The angle of inclination of the third knee С0С1 to the horizon has a fixed value alfH for all executive manipulators.

The coordinates of the trocar points of the executive manipulators with the pTrM surgical instrument and the pTrC video camera, respectively, in the global coordinate system of the delivery node (X _G , Y _G , Z _G ) are determined as follows:

where LM is the distance from the point C1 of the flange to the point of the trocar pTr.

pC1M and pC1C - points of the center of the flange for fastening the executive manipulator with a surgical instrument and the executive manipulator with a video camera, respectively, the coordinates of which are determined as follows:

where LD3 is the length of the third leg С0С1 (constant),

pC0M and pC0C - coordinates of the C0 point of the manipulator with the tool and the manipulator with the camera, respectively.

jATMG and jATCG - unit vectors along the virtual axis of the manipulator with the tool and with the camera, respectively, collinear to the С0С1 vector.

Next, for the executive manipulator with a surgical instrument, a trocar point (pTrM) is found in the coordinate system of the video camera (Xcam, Ycam, Zcam) - pTrMCam.

3.4. Determining the coordinates of the pTrM trocar point of the executive manipulator with the instrument in the camera coordinate system (X _cam , Y _cam , Z _cam )

The fixed point of the trocar of the executive manipulator with the video camera pTrC={0,0,0} is taken as the reference point of the video camera coordinate system.

In this case, the trocar point of the manipulator with the instrument in the video camera coordinate system (X _cam , Y _cam , Z _cam ) is calculated by Formula (19):

where matС is the matrix for transition from the video camera coordinate system to the manipulator coordinate system, and is determined by Formula (20):

where kZCG is a vector running along the camera rod and perpendicular to the iXСG and jYСG vectors, which is found by Formula (21):

where phiC is the angle corresponding to the angle of rotation of the main axis of the executive manipulator phiM (Fig. 3), but related to the executive manipulator with a video camera.

alfTC is the angle between the rod and the virtual axis, the angle alfTC lies in the interval (Pi/20, 19 * Pi/20) due to design limitations, determined by the sum of the angles between the links of the executive manipulator with the video camera αFC (similar to the angle αFM) and Δ0C (Fig. 3):

where the value of the angle αFC is set directly by the encoder, and the angle Δ0C is set constructively.

The vector kZCG is supplemented with two mutually perpendicular vectors {iX0CG, jY0CG}, so that this triple of vectors forms a right orthonormal triple of vectors, relative to which the angle of rotation of the video camera around its axis is counted:

Next, a rotating trio of vectors is introduced, which will rotate relative to the vectors {iX0CG, jY0CG}, around the vector kZCG, thereby determining the angle of rotation of the video camera around the optical axis using the values of the vectors determined by Formula (23) and Formula (24), respectively:

in this case, the rotC-angle is read directly from the encoder of the video camera.

Next, the coordinates of the central pixel of the video camera and the coordinates of the end of the rod of the surgical instrument are found in the global coordinate system of the delivery node (X _G , Y _G , Z _G ).

3.5. Finding the coordinates of the central pixel of the video camera and the coordinates of the end of the tool rod in the global coordinate system of the delivery node (X _G , Y _G , Z _G )

The coordinates of the central pixel of the video camera are calculated by Formula (27):

The tool rod end coordinates are determined by Formula (28):

where LTrD0С and LTrD0М are the depth of immersion of the video camera rod and the surgical instrument rod, respectively, into the human body. These values are variables read directly from the linear encoder of the actuator.

kZСG - coordinates of the unit vector along the camera rod in the global coordinate system (X _G , Y _G , Z _G ), calculated in section 3.4.

kZMG - coordinates of the unit vector along the tool stem in the global coordinate system (X _G , Y _G , Z _G )

where, Normalize is the vector normalization operator.

After determining the coordinates of the knees of the delivery unit, the coordinates of the executive manipulators with the video camera and the surgical instrument, as well as the coordinates of the central pixel of the video camera matrix and the coordinates of the end of the surgical instrument rod, the robotic surgical complex is ready for the operating mode, in which the surgical instrument installed on the executive manipulator is controlled using control handle attached to the main manipulator.

4. Working mode

The operator, being at the console of the surgeon, controls the surgical instrument attached to the executive manipulator (Fig. 3).

The operator can move the surgical instrument as follows:

turning the petals of the control handle (angle rotp),

movement of the control handle as a whole by turning around the COj axis (angle a1P),

compression-unclenching of the petals of the control handle,

rotation of the main manipulator, on which the control handle is fixed, as a whole around the vertical axis YN (angle a1J),

moving the levers AB (angle a1L) and BC (angle a2L) up and down relative to the Yn axis (Fig. 1).

In this case, a strictly synchronous transformation of the movement of the control handle mounted on the main manipulator into the movement of a surgical instrument fixed on the executive manipulator is carried out. Due to the use of digital processing of data according to the present method, taken from encoders installed on the servo motors of the manipulators, the transformation is carried out according to a certain mathematical algorithm, which is described below and allows for intuitive control.

4.1. Calculation of main points and vectors of the main manipulator

4.1.1 Measurement of the deviation of the location of the point pO _j in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N )

Movement by the operator of the control stick fixed on the main manipulator (Fig. 1) leads to the deviation of the point pOj in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ). The specified deviation is continuously recorded by the encoder, converted into a digital signal and transmitted to the digital processing unit. The coordinates of the point pOj in the N coordinate system of the main manipulator (X _N , Y _N , Z _N ), based on the readings of the encoders, are calculated as follows:

The origin point is set in the N coordinate system - pAN={0,0,0}, then the coordinates of points В, С and Oj in the N coordinate system of the main manipulator (X _N , Y _N , Z _N ) are calculated by Formulas (30-32 ):

where L1 - The length of the lever AB of the main manipulator;

L2 - Length of arm BC of the main manipulator;

L3 - Lever length COj of the main manipulator;

a1L - angle between Y-axis _N and lever AB;

a2L - angle between Y-axis _N and lever CB;.

a1J - angle of rotation of the main manipulator around the vertical axis Y _N .

4.1.2 Definition of the vector MyV in the moving coordinate system of the main manipulator (Xt, Yt, Zt) and in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N )

When the operator moves the control stick fixed on the main manipulator (Fig. 1), in addition to changing the coordinates of the point pOj in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ), the coordinates of the vector myV also change (Fig. 1, Fig. 6), which sets the direction of the axis on the main manipulator, around which the petals of the control handle rotate, controlled by the surgeon's fingers and designed to rotate the stem of the surgical instrument around its axis and open and close the branches (which are clamps, scissors or needle holders) of the end effector of the surgical instrument in desired plane.

The vector myV has the following coordinates: {tan(alfXV), tan(alfYV), 1} relative to the ground (the z-coordinate is normalized to 1 for ease of calculation and brevity), where alfXV is the angle between the vertical plane containing the vector myV and the vertical plane YNZN, and if alfXV>0, then the X-coordinate of the vector myV is positive (Fig. 6);

alfYV is the angle between the horizontal plane containing the vector myV and the horizontal plane XNZN, and if alfYV>0, then the Y-coordinate of the vector myV is positive (Fig. 6).

With vertical movement of the control handle and, accordingly, the end of the vector myV (that is, when the angle alfYV changes, and the angle alfXV remains unchanged), the first knee pD0pBra of the end effector of the surgical instrument (Fig. 7) moves in some fixed plane P1 (Fig. 8) passing through the rod of the surgical instrument and the point pD0 on the end effector of the surgical instrument, and the plane P1 is set by the angle of rotation of the petals of the control handle mounted on the main manipulator around the vector myV (in other words, the rotation of the petals of the control handle around the axis running along the vector myV corresponds to the rotation of the plane P1 around the stem of the surgical instrument).

If the control handle and, accordingly, the vector myV moves in the horizontal plane (that is, the angle alfXV changes, and the angle alfYV remains unchanged), then the jaws of the second knee of the end effector move in the plane P2, perpendicular to P1, rotating around the axis of the second knee of the jaws, perpendicular to the plane P2 and the first knee of the end effector (Fig. 9).

When moving the control stick, the coordinates of the vector myV are determined in the moving coordinate system of the main manipulator (Xt, Yt, Zt), in which the vector myV is designated as the vector myVT with coordinates {xtv,ytv,ztv}:

where a1P is the angle of rotation of the planetary system around the axis COj, and a1P=0 means that the vector myV lies in the plane ABCOj;

a2P - angle of rotation of the vector myV in the YTZT plane;

The minus sign indicates the following convention: if the end of the vector myV is located above the plane Xt Zt, then the angle a2p>0, that is, the rotation was counterclockwise, when viewed from the end of the Xt axis.

Next, the coordinates of the unit vector myV are determined in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) associated with the ground, in which the vector myV is denoted as the vector myVN:

where the sign "∙" denotes the scalar product of the unit vector myVt along the main axis of the moving coordinate system of the main manipulator (Xt, Yt, Zt), perpendicular to the vectors myV and the lever COj, and MT is the transition matrix from the moving coordinate system of the main manipulator (Xt, Yt, Zt) with the origin at the point Oj into the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) determined by Formula (37):

where the coordinate orts itN of the X _{T axis,} jtN of the Y _T axis and ktN of the Z _T axis in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) are found as follows:

while a1J is the angle of rotation of the control handle around the vertical axis Y _N (when a1J=0, the control handle lies in a plane perpendicular to the display);

a1L - the angle between the Y axis _N and the lever AB;

a2L is the angle between the YN axis and the lever CB;

Thus, the vectors myVN and myVT are the coordinates of the same vector myV in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) and the moving coordinate system of the main manipulator (Xt, Yt, Zt), respectively.

4.1.3. Measuring the deviation of the position of the point pO _j and the vector MyV in the display coordinate system (X _D , Y _D , Z _D )

Then the coordinates of the point pO_jand the vectors myV are defined in the fixed coordinate system D (X_D, Y_D, Z_D) associated with the surgeon's display, in which the vector myV is designated as the vector myVD, while

where MD is the transformation matrix of the transition from the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) to the display coordinate system (X _D , Y _D , Z _D ) in which the surgeon's gaze (head) is located, defined as follows:

where aD is the angle of rotation of the display coordinate system (X _D , Y _D , Z _D ) around the X _N axis of the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) (Fig. 5).

The coordinates of the point pOj in the display coordinate system (X _D , Y _D , Z _D ) are determined by multiplying the coordinates of the point pOjN found by Formula (32) by the transformation matrices MD:

4.1.4 Determining the coordinates of the normal vector WN to the opening plane of the jaws of the surgical instrument in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N )

The vector W (Fig. 10) of the main manipulator is a vector perpendicular to the vector myV and the plane of opening of the petals of the control handle of the main manipulator. The opening plane of the blades of the control handle corresponds to the opening plane of the jaws of the surgical instrument, which is called the cutting plane. The perpendicular (normal) to the cutting plane on the jaws of the surgical instrument corresponds to the vector W of the control handle of the main manipulator (Fig. 8)/

Thus, the vector WN is the vector W in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ).

The operator, using the control handle, controls the branches of the end effector of the surgical instrument. In response to a change in the position of the control handle (and, accordingly, the point pOj and the vector myV), the normal vector WN moves to the opening plane of the jaws of the surgical instrument.

The coordinates of the normal vector WN to the opening plane of the jaws of the surgical instrument in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) are determined by Formula (44):

where the coordinates of the vector myVN are the coordinates of the unit vector myV in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ), and are determined by Formula (36), and the coordinates of the iRotVN vector (Fig. 6, 10), along which the the petals of the control handle of the main manipulator are found as follows:

where myIHorN is the vector myIHor of the normal to the opening plane of the jaws of the surgical instrument, perpendicular to the vectors myV and COj (Fig. 6, 10), which in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) has the following coordinates:

vector myJN - vector myJ, orthogonal to the vectors myVN, whose coordinates are determined by Formula (36), and myIHorN, in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ), while:

myRot is the angle between the vectors myIHorN and iRotVN (characterizing the rotation of the petals of the control handle of the main manipulator relative to the horizontal plane), which is set by the surgeon's fingers intuitively and its exact value is the sum of the values of the angle rotP read by the encoder and the calculated offset deltaRot according to Formula (48):

where the value of rotP (the angle of rotation of the petals of the control handle) is directly read by the encoder, which monitors the rotation of the petals around the vector myV relative to the planetary system (Fig. 1).

deltaRot − is a complex function of five variable angles a1L, a2L, a1J, a1P and a2P (Fig. 1), whose values are read by the corresponding encoders. Thus, myRot is the angle of rotation of the control stick petals around the vector myV, and the reference is from the horizon line.

The offset deltaRot is calculated based on the values of the iPT, myJt and myIt vectors described below. On FIG. 11 is a flowchart for calculating the deltaRot bias.

The unit vector iPT, perpendicular to the vectors myV and COj in the moving coordinate system of the main manipulator (Xt, Yt, Zt), is determined by Formula (49):

and the vector myJt is the vector myJ in the moving coordinate system of the main manipulator (X _t , Y _t , Z _t ):

the vector myIt is the vector myIHor in the moving coordinate system (Xt, Yt, Zt), which is defined as follows:

When moving the control handle fixed on the main manipulator, the coordinates of the normal vector WN to the opening plane of the jaws of the surgical instrument change, as well as the coordinates of the point pBra located on the end effector of the surgical instrument, the angle of rotation of the rod of the surgical instrument rotStM and the angles of rotation of the links of the end effector of the surgical instrument γ and ββ.

4.2. Calculation of points and angles of rotation of the rod and links of a surgical instrument mounted on an executive manipulator

4.2.1 Finding the coordinates of the point pBra on the end effector of the surgical instrument in the coordinate system of the video camera (X _cam , Y _cam , Z _cam ) and in the global coordinate system of the delivery site (X _G , Y _G , Z _G )

The point pBra (Fig. 7) on the end effector of the surgical instrument in the video camera coordinate system (Xcam, Ycam, Zcam), corresponding to the point pOj on the main manipulator in the display coordinate system (X _D , Y _D , Z _D ), is calculated by Formula (52 ):

where, Δ pOjD is the deviation of the point pOj in the display coordinate system (X _D , Y _D , Z _D ), while

At the moment when the robotic surgical complex is ready for operation or when the Clutch pedal is depressed (released) on the surgeon's console, the program remembers the absolute coordinate of the point pOj of the main manipulator and saves this value as a variable pOjStartD. After depressing the pedal, the point pOjStartD is fixed in space and does not move, the point pOj continues to move along with the planetary system of the handle.

When moving the main manipulator, the coordinate of the point pOj changes and the program calculates the difference ΔpOjD between the saved variable pOjStartD and the new coordinate of the point pOj in the display coordinate system ((X _D , Y _D , Z _D ). After that, the difference is multiplied by the scaling factor kShift and added to the initial measured coordinate pBraMStartCam of point pBra of the video camera.

When you press the Clutch pedal on the surgeon's console, pBraMStartCam=pBraMCam is assigned. When the pedal is pressed, the points pOj and pOjStartD move simultaneously.

When the Clutch pedal is released on the surgeon's console, pOjStartD=pOjD is assigned.

The point pBra on the end effector of the surgical instrument in the global coordinate system of the delivery node (X _G , Y _G , Z _G ) is calculated by the following formula:

where matC is a matrix for transition from the video camera coordinate system (Xcam, Ycam, Zcam) to the global coordinate system of the delivery node (X _G , Y _G , Z _G ), while matС has the following coordinates:

pTrCG - Coordinates of the trocar points of the manipulator with the pTrC camera in the global coordinate system of the delivery node (X _G , Y _G , Z _G ), calculated by Formula (16).

4.2.2 Calculation of the angle of rotation of the surgical instrument rod rotStM (rotStock) and the angles of rotation of the end effector links of the surgical instrument γ and ββ in the global coordinate system of the delivery unit (X _G , Y _G , Z _G )

A change in the direction of the myV vector leads to a change in the rotation angles of the end effector links γ (the angle between the stem pTrPD0 and the first knee of the surgical instrument jaw PD0pBra) and ββ (the angle between the first and second knee of the end effector of the surgical instrument), as well as to a change in the angle of rotation of the instrument stem rotStM (rotStock).

The angle γ to which the first knee of the end effector of the surgical instrument must be rotated is determined by Formula (55):

To determine the angle γ, a moving orthonormal triple of vectors (iXMG, jYMG, kZMG) associated with the tool rod is found:

the coordinates of the unit vector kZMG are found by Formula (29),

where rotStM is the angle of rotation of the tool rod, while

;

;

where iX0MG, jY0MG are orthonormal vectors associated with the stem of a surgical instrument, fixed relative to the trio of vectors jATMG, nSymMG, kSymMG:

Vector WG is the vector of the normal to the cut plane in the global coordinate system (X _G , Y _G , Z _G ), which is determined by multiplying the matС matrix by the WCam vector:

the WCam vector, which defines the normal to the cut plane in the camera coordinate system, is equal to the WD vector: WCam=WD.

The main manipulator has a display coordinate system (X _D , Y _D , Z _D ) (Fig. 5), which is associated with the display in which the surgeon observes the operation process. Therefore, the coordinates of the main points and vectors of the fixed coordinate system N of the main manipulator must be recalculated in the display coordinate system (X _D , Y _D , Z _D ). And this means that it is necessary to calculate the coordinates of the vectors myV and W in the display coordinate system (X _D , Y _D , Z _D ), i.e.

where MD is the transformation matrix of the transition from the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ) to the display coordinate system (X _D , Y _D , Z _D ), in which the surgeon's gaze (head) is located, determined by the Formula ( 42), and the vector WN is the normal vector to the opening plane of the jaws of the surgical instrument in the fixed coordinate system of the main manipulator (X _N , Y _N , Z _N ), determined by Formula (44).

evBraMG - vector going from point pD0 to point pBra of the surgical instrument in the global coordinate system (X _G , Y _G , Z _G ):

where Normalize is the vector normalization operator,

tv is an auxiliary vector directed from the trocar point pTr to the point pBra and is calculated as the difference between the coordinates of the point pBra on the end effector of the surgical instrument in the global coordinate system of the delivery node (X _G , Y _G , Z _G ) calculated by Formula (54), and coordinates of the trocar point of the executive manipulator with the pTrM surgical instrument in the global coordinate system of the delivery node (X _G , Y _G , Z _G ) determined by Formula (15):

Next, the angle ββ is calculated, which determines the angle between the first and second knee of the end effector.

where vBra2MCam is a vector that determines the direction of the jaws of the surgical instrument, corresponding to the product of the vector myVD (found by Formula (41)) and the length of the jaws LBra (Fig. 7):

evBraMCam is a vector that defines the directions of the jaws of a surgical instrument, in the video camera coordinate system (Xcam, Ycam, Zcam), corresponding to the myVD vector of the main manipulator in the display coordinate system (X _D , Y _D , Z _D ).

jYMCam is a vector that, together with the iXMCam and kZMCam vectors, forms an orthonormal trinity of vectors of the moving coordinate system associated with the surgical instrument. In this case, this trio of vectors is considered in the coordinate system of the video camera. In the global coordinate system of the delivery node (X _G , Y _G , Z _G ) these vectors are denoted as jYMG, iXMG, kZMG and are calculated by formulas (57), (56), (29), respectively.

4.3. Calculation of the angle of rotation of the main axis of the executive manipulator phiM and the length of the stem of the surgical instrument LTDM, fixed on the executive manipulator in the global coordinate system of the delivery unit (X _G , Y _G , Z _G )

The length of the stem of a surgical instrument in the human body is found by the following formula:

where "⋅" is the scalar product of the difference between pD0MG and pTrMG vectors; wherein

pTrMG - coordinates of the trocar point of the executive manipulator with the pTrM surgical instrument in the global coordinate system of the delivery node (X _G , Y _G , Z _G ), determined by Formula (15).

pD0MG - end point of the instrument rod in the global coordinate system of the delivery node (X _G , Y _G , Z _G ), defined as the difference between the point pBraMG on the end effector of the surgical instrument, found by Formula (54) and the product of the vector evBraMG, calculated by Formula (64 ), on LBra1:

where LBra1 is a constructively specified parameter that determines the length of the working part of the jaws of the end effector of the surgical instrument.

The angle of rotation of the main axis of the executive manipulator phiM, which is the angle of rotation of the plane of the parallelogram C2C3C4TrM and the vector kZM around the virtual axis jATMG, is determined as follows:

where the coordinates of the unit vector kZMG are found by Formula (29), and the coordinates of the vectors nSymMG and kSymMG are determined by Formulas (8) and (9), respectively.

Next, the angle alTM is determined - the angle between the vectors jATMG and kZMG, while

Determining the angle alTM, the angle αFM is found, which is fed directly to the manipulator motor to rotate the arm of the C2C3 manipulator around the point C2 (Fig. 3),

where Δ0 is the constructive deviation of the tool axis from the edge of the parallelogram of the manipulator

5. Transfer of the calculated angles of the axes of the executive manipulator (phiM, LTDM, αFM) and the calculated angles of the axes of the surgical instrument (ββ, γ, rotStM) to the servomotors of the executive manipulator.

The values of the angle of rotation of the main axis of the executive manipulator phiM determined in response to the movement of the control handle; the length of the stem of the LTDM surgical instrument fixed on the executive manipulator; the angles of rotation of the end effector links γ and ββ, as well as the angle of rotation of the tool rod rotStM, are transmitted to the servomotors of the executive manipulator, which provide a change in the position of the executive manipulator, as well as a surgical instrument and a video camera mounted on the executive manipulators, to manipulate the surgical instrument and perform the operation.

The inventive method for processing the position data of points and vectors of the main and executive manipulators on the basis of a robotic surgical complex with a planetary-lever mechanism makes it possible to achieve intuitive control of a surgical instrument.

The proposed technical solution has the following advantages:

- the main manipulator and the executive manipulator with a surgical instrument have 7 degrees of freedom. The delivery node has 3 degrees of freedom. This solution allows you to provide maximum freedom in management.

- combination of the coordinate system of the surgeon's display with the main manipulator and the coordinate system of the video camera of the working area with surgical instruments provide the necessary intuitiveness in control.

- the error of moving the pBra point of the end effector mounted on the executive manipulator is 0.1 mm.

- the response of the surgical instrument in response to the movement of the control handle is 10 ms.

- the location of the video camera in space does not affect the intuitiveness of control.

Claims (11)

1. A method for controlling a robotic surgical complex containing the following units: a surgeon's console equipped with a main manipulator with a control handle made in the form of a planetary lever mechanism, a patient stand with a delivery unit equipped with an executive manipulator with surgical instruments and a video camera attached to them, a display and local controllers associated with the automatic control system and servo encoders in the systems of the main and executive manipulators, each of which is characterized by a coordinate system; including continuous determination by encoders of the coordinates and angular positions of the main manipulator, the coordinates and angles of rotation of the knees of the delivery unit, the coordinates of the executive manipulators with a video camera and with a surgical instrument; selection of a trocar point with fixation of its position in the global coordinate system; determining the angular positions, points and vectors of the delivery unit and the main manipulator in the global coordinate system and in the camera coordinate system; determination by a mathematical algorithm of deviations of points and vectors of the control arm in the coordinate systems of the main manipulator and the display as a result of the operator’s hand on the control arm, causing the surgical instrument to move in the operating field by means of deviations of points, vectors and angular positions of the rod and end effector of the surgical instrument calculated by the mathematical algorithm instrument and executive manipulator; transfer of the calculated values to the servomotors of the executive manipulator. 2. The method of controlling the robotic surgical complex according to claim 1, characterized in that the main manipulator is characterized by movable and fixed coordinate systems. 3. A method for controlling a robotic surgical complex according to claim 1, characterized in that the main manipulator contains a base point pOj in the planetary system of the control handle, which, when the pedal is pressed on the surgeon's console, moves under the influence of the control handle without transferring the control action to the executive manipulator, and when the pedal is depressed save the absolute coordinates of the current position of the point pOj in the variable point pOjStart. 4. The method for controlling the robotic surgical complex according to claim 3, characterized in that the deviation of the position of the control arm as a result of the operator’s hand is determined by determining the deviation of the position of the base point pOj, which is the center of the arcs of circles forming the control arm, from the position of the point pOjStart, the coordinates of which saved after depressing the pedal and deflecting the vector myV, which sets the direction of the axis on the main manipulator, around which the petals of the control handle rotate. 5. The method of controlling the robotic surgical complex according to claim 1, characterized in that the coordinates of the angular positions of the knees of the delivery unit, the coordinates of the vectors of the flange of the executive manipulator, the coordinates of the trocar point of the executive manipulator with the surgical instrument and the video camera, the coordinates of the central pixel of the matrix of the video camera and the coordinates of the end of the surgical rod are determined tool. 6. The method of controlling the robotic surgical complex according to claim 1, characterized in that in order to move the rod and the end effector of the surgical instrument and the executive arm in response to the movement of the main manipulator, the following is carried out: calculating the deviation of the normal vector WN to the plane of opening of the branches, perpendicular to the vector that sets the direction axes on the main manipulator; determination of the angle myRot, which characterizes the rotation of the petals of the handle of the main manipulator relative to the horizontal plane; calculating the change in the position of the base point pBra lying on the axis of intersection of the first and second links of the end effector; determination of the angle of rotation of the rod of the surgical instrument and the angle of rotation of the links of the end effector; determining the angle of rotation of the axis of the executive manipulator phiM and the angle of rotation αFM of the executive manipulator relative to its axis; calculation of the length of the surgical instrument stem in the patient's body.

Images