Surgical robotic field of view follow system

Abstract

The application discloses a kind of surgical robot field follow-up method, comprising: when surgical robot enters master-slave control mode, the initial posture of display is obtained;Real-time acquisition of the posture of display, the posture change amount of endoscope is obtained in combination with its initial posture and the posture conversion matrix between display and endoscope, and then the first position change amount of endoscope and the posture change amount of endoscope field are obtained by mapping;Real-time acquisition of the acceleration information of display, the position increment of display is calculated, and the second position change amount of endoscope is obtained by mapping;According to the first position change amount of endoscope, the second position change amount and the posture change amount of endoscope field of endoscope, the target pose of current endoscope is calculated, and each joint movement of surgical robot is controlled.The application increases the safety and smoothness of surgical operation, and the operation time is shorter.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more particularly to a surgical robot field-following system. Background Technology

[0002] Minimally invasive surgery has largely replaced open surgery as the main direction of development in the field of surgical medicine. Compared with traditional open surgery, minimally invasive surgery has advantages such as less trauma, less pain, and faster recovery. With the development of robotics technology, minimally invasive surgery based on laparoscopic surgical robots has gradually matured and is being widely used.

[0003] Laparoscopic surgical robots are master-slave teleoperated systems, consisting of a master robotic arm and a slave robotic arm. The operator can control the slave robotic arm by manipulating the master robotic arm, thereby using surgical instruments at the end of the slave robotic arm to perform surgery on the patient. The slave robotic arm is typically divided into a tool arm and a scope-holding arm, with surgical instruments and an endoscope respectively attached to their ends.

[0004] During surgery, the instruments at the end of the surgical arm must be kept within the endoscopic field of vision. When these instruments deviate from the optimal viewing range, the operator typically needs to depress the endoscope pedal to switch control from the instrument arm to the endoscope-holding arm. The main robotic arm then controls the endoscope-holding arm, moving the lens at its end along with the main robotic arm until the instrument is within the optimal viewing range or the operator's desired field of vision. After the operator releases the endoscope pedal, the main robotic arm and the instrument at the end of the surgical arm are repositioned, and control switches back from the endoscope-holding arm to the instrument arm, repeating this process. To ensure surgical safety and hand-eye coordination, the operator must constantly adjust the endoscope to maintain the best surgical field of vision. However, this constant adjustment leads to frequent interruptions in the surgical procedure, significantly reducing the continuity of the procedure and the operator's comfort, while also prolonging the surgical time and increasing surgical risks. Summary of the Invention

[0005] Purpose of the invention: To address the above-mentioned shortcomings, the present invention provides a surgical robot field-following system, which increases the safety, smoothness, and comfort of surgical operations, reduces surgical time, and lowers surgical risks.

[0006] Technical solution:

[0007] A surgical robot field-following method includes: When the surgical robot enters master-slave control mode, the initial posture of the display is acquired; The attitude of the monitor is acquired in real time. The attitude change of the endoscope is obtained by combining its initial attitude and the attitude transformation matrix between the monitor and the endoscope. Then, the first position change of the endoscope and the attitude change of the endoscope field of view are mapped to obtain the attitude change of the endoscope. The acceleration information of the display is acquired in real time, the position increment of the display is calculated, and the second position change of the endoscope is mapped to the display. The target pose of the endoscope is calculated based on the first position change, the second position change, and the pose change of the endoscope's field of view, and the movement of each joint of the surgical robot is controlled accordingly.

[0008] The mapping obtains the first position change of the endoscope and the attitude change of the endoscope's field of view, specifically including: The changes in the endoscope's posture are converted into rotation angles of the endoscope around the coordinate axes of the endoscope coordinate system. The displacement of the endoscope lens plane center point on each projection plane is calculated based on the initial pose of the endoscope, and the position change of the endoscope is obtained by combining the displacements. The initial pose of the endoscope is the pose of the endoscope relative to the RCM point when the surgical robot enters the master-slave control mode. Calculate the transformation matrix of the endoscope's rotational motion about a direction perpendicular to the lens plane relative to its initial pose to obtain the attitude change of the endoscope's field of view.

[0009] The specific steps of converting the endoscope's attitude change into rotation angles around each coordinate axis are as follows: convert the endoscope's attitude change into RPY angles, and obtain the rotation angles of the endoscope around each coordinate axis of its own coordinate system according to the calculation formula of the rotation matrix.

[0010] Set a minimum threshold for the rotational motion of the endoscope. If the rotation angle of the endoscope around a certain coordinate axis of its coordinate system is less than the minimum threshold, the rotational motion is ignored.

[0011] The pose of the endoscope relative to the RCM point is obtained through robot forward kinematics calculation.

[0012] The calculated position increment of the display is specifically as follows: Based on the real-time acceleration information of the display, the speed of the display at the current moment is calculated by combining the speed of the display at the previous moment, the position increment of the display at the current moment is calculated, and then the position increment of the display from the time of entering the master-slave control mode to the current moment is calculated.

[0013] The position increment of the display is the position increment of the display along the vertical direction.

[0014] The attitude and acceleration information of the display are acquired in real time by an inertial measurement unit placed inside the display.

[0015] The display is a VR display.

[0016] Beneficial effects: The surgical robot of the present invention has a field of vision that follows the movement of the head display in real time and is completely independent of the control of the tool arm. It eliminates the need for doctors to frequently switch control by stepping on the foot pedal, which increases the safety and smoothness of the surgical operation and shortens the operation time. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the display of the present invention; Figure 2 A schematic diagram of the surgical robot's endoscope arm; Figure 3 This is a flowchart of the field-of-view tracking method of the present invention; Figure 4 This is a schematic diagram of the endoscope in its coordinate system's yz plane at the initial stage; Figure 5 This is a schematic diagram showing the change in the position of the endoscope. Detailed Implementation

[0018] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments.

[0019] Laparoscopic surgical robot systems typically include a user console, a patient operating table, and an instrument cart. The user console receives hand movement information from the operator as motion control signal input for the entire system. The user console may include a seat assembly, footrest assembly, main robotic arm, and a display. The display shows a view of the surgical site within the patient's body and is typically an open or immersive display, mounted on a column of the user console, and can be adjusted in height according to operator commands. In this invention, the display is a head-mounted display, specifically a VR display, such as... Figure 1 As shown, it is worn on the operator's head and its posture changes accordingly with the operator's head movements. For example, an inertial measurement unit (IMU) is provided inside the display to acquire the display's attitude and acceleration information.

[0020] Reference Figure 2 During surgery, surgical instruments and endoscopes undergo conical motion with the RCM point as the vertex. For rigid endoscopes, since they have no degrees of freedom other than rotation, it is impossible to achieve a complete mapping between the posture information of the endoscope (or its corresponding endoscope coordinate system) and the posture information of the display (or its corresponding display coordinate system). Based on this, the surgical robot field-of-view tracking method of the present invention can map the posture changes of the display to the position changes of the endoscope tip.

[0021] The surgical robot field-following method of the present invention is as follows: Figure 3 As shown, it includes the following steps: (1) When the surgical robot enters the master-slave control mode, the posture of the display is obtained as its initial posture; at the same time, the initial pose of the endoscope relative to the RCM point is obtained through the robot's forward kinematics calculation; wherein, after the surgical robot completes the positioning in the pre-adjustment stage, it can enter the master-slave control mode of the surgical state.

[0022] Optionally, the attitude can be represented in the form of Euler angles, quaternions, or matrices. In this invention, matrices are used as the way to describe the attitude.

[0023] Let the initial pose matrix of the display be... The initial position vector is .

[0024] Based on the initial pose of the endoscope relative to the RCM point, let the initial pose matrix of the endoscope relative to the RCM point be denoted as . The initial position vector is Therefore, the initial attitude matrix of the endoscope relative to the RCM point is also the attitude transformation matrix between the endoscope coordinate system and the RCM coordinate system at the initial moment.

[0025] Specifically, the RCM coordinate system is established as follows: with the RCM point as the origin, the vertical downward direction as the z-axis, and the direction perpendicular to the z-axis in the normal plane of the front plane of the lens holder (i.e., the plane formed by extending from the axis of the lens holder to the front side of the lens holder) as the y-axis, the x-axis is determined by the right-hand rule.

[0026] Specifically, the endoscope coordinate system is established as follows: the center point of the endoscope lens plane is taken as the origin, the direction perpendicular to the endoscope lens surface is taken as the z-axis, the direction perpendicular to the z-axis in the normal plane of the front plane of the endoscope arm (i.e. the plane formed by extending from the axis of the endoscope arm to the front side of the endoscope arm) is taken as the y-axis, and the x-axis is determined by the right-hand rule.

[0027] (2) The orientation of the display is obtained in real time, and the rotation angle of the endoscope is obtained accordingly.

[0028] Let the attitude matrix of the display at a certain moment be denoted as . Then the change in the display's current pose relative to its initial pose is: Accordingly, the change is converted onto the endoscope to obtain the corresponding attitude change. This allows us to obtain the rotation angle of the endoscope; where, Let be the transformation matrix between the monitor and the endoscope (i.e., the transformation matrix between the coordinate system corresponding to the monitor and the coordinate system of the endoscope), which is a 3*3 constant matrix and is known.

[0029] Specifically, the amount of change in the endoscope's orientation Convert to RPY angles, according to the formula for calculating the rotation matrix: This yields the rotation angle of the endoscope around the endoscope coordinate system, i.e., around the x-axis of the endoscope coordinate system. t y t z t The angles of rotation are respectively , , ,So, , , These are the x-axis and x-axis of the corresponding endoscope coordinate system. t y t z t The amount of rotational change.

[0030] Optionally, to avoid accuracy errors caused by slight head tremors of the operator, a minimum threshold for the rotational movement of the endoscope is set to min. That is, if the rotation angle of a certain coordinate axis is less than this minimum threshold, the rotational movement is ignored. For example, if... That is, ignore the rotational motion; , Similarly.

[0031] (3) Calculate the change in the position of the endoscope and the change in the posture of the endoscope field of view caused by the change in the posture of the monitor based on the rotation angle of the endoscope obtained in step (2).

[0032] In this invention, the change in the position of the endoscope is specifically the change in the position of the center point of the endoscope lens plane. Once the rotation angle of the endoscope is known, the change in the position of the center point of the endoscope lens plane can be calculated. This invention obtains the change in the position of the endoscope by calculating the positional movement of the center point of the endoscope lens plane on each projection plane of the endoscope coordinate system.

[0033] Taking the yz plane of the endoscope coordinate system as an example, such as Figure 4 As shown, Let y0 be the origin of the RCM coordinate system, and z0 be the y-axis and z-axis of the RCM coordinate system, respectively. This is the projection of the center point of the endoscope lens plane before movement onto the yz plane. Let be the target projection point of the center point of the endoscope lens plane in the yz plane. Let be the angle between the line connecting the center point of the endoscope lens plane and the RCM point initially, and the z-axis of the endoscope coordinate system. The angle between the line connecting the center point of the endoscope tip and the RCM point after the endoscope position changes, and the z-axis of the endoscope coordinate system. This represents the initial distance between the endoscope and the RCM point; based on the initial position vector of the endoscope relative to the RCM point. The endoscope's rotation around the coordinate axis x, as calculated abovet rotation angle The change in position of the endoscope in its own coordinate system's yz plane can be calculated. and .

[0034] like Figure 5 As shown, the specific calculation process is as follows: ; ; ; ; .

[0035] Similarly, the change in position of the endoscope in its own coordinate system's zx plane can be calculated. , .

[0036] Therefore, by combining the positional movements on each projection plane, the change in endoscope position caused by the change in monitor orientation can be obtained as follows: .

[0037] Since the angle α of rotation of the endoscope around the z-axis of the endoscope coordinate system changes the endoscope's attitude, it is equivalent to changing the initial pose matrix. Perform a transformation, and its transformation matrix That is, to obtain the amount of change in the endoscopic field of view caused by the change in the posture of the monitor.

[0038] (4) Obtain the position increment of the display corresponding to the master-slave control and map it to the endoscope to obtain the position increment of the endoscope caused by the position change of the display.

[0039] The current cycle is the i-th cycle after entering the master-slave control mode. The acceleration information of the display is collected. The speed of the display at the end of the current cycle can be calculated based on the speed of the display at the end of the previous cycle. Based on this, the position increment of the display in the current cycle can be calculated. Then, through the above, the position increment of the display from the start of entering the master-slave control mode to the current cycle can be calculated. The position increment of the endoscope is obtained by mapping the transformation matrix R between the display and the endoscope, which is the position increment of the endoscope at the current moment relative to the initial moment.

[0040] In this invention, the position increment of the display is calculated by calculating the position increment of the display along each coordinate axis of its coordinate system. In this embodiment, the calculation of the position increment of the display along the z-axis of its coordinate system is taken as an example, as follows: The acceleration of the display along the z-axis of its coordinate system is collected. At the end of the previous moment, the velocity of the display along this direction was Then, at the end of the current period, the velocity along this direction is The position increment along this direction in the current cycle is Then, from the start of entering master-slave control mode to the current cycle, the position increment of the display along its coordinate system's z-axis is... That is, the position increment of the display along the z-axis of its coordinate system at the current moment relative to the initial moment.

[0041] The position increment of the monitor along its z-axis is mapped to the endoscope's coordinate system using the transformation matrix R between the monitor and the endoscope, resulting in the position increment of the endoscope along its z-axis. ; Among them, eye(3) is a 3*3 matrix with diagonal elements of 1 and other elements of 0; zeros(3,1) is a 3*1 zero matrix, and correspondingly, zeros(1,3) is a 1*3 zero matrix.

[0042] In one implementation, since the movement of the display along the x and y axes of its coordinate system will cause a change in the orientation of the display, as in step (3), the position increment of the display along the x and y axes of its coordinate system can be ignored in this step. By calculating the position increment of the display along the z axis of its coordinate system, the movement of the endoscope in a direction perpendicular to the plane of the endoscope lens can be achieved.

[0043] (5) Combining steps (3) and (4), the pose transformation matrix of the endoscope relative to the robot's base coordinate system at the current moment is obtained, and the movement of each joint of the endoscope-holding arm is controlled to complete the field of view tracking. This invention takes the position change of the display along its coordinate system z-axis as an example for illustration. The pose transformation matrix T of the endoscope relative to the robot's base coordinate system at the current moment is as follows: ; in, The pose transformation matrix between the RCM coordinate system C0 and the robot base coordinate system is calculated based on the robot's forward kinematics.

[0044] After obtaining the pose matrix of the endoscope tip relative to the base coordinate system, this invention allows for the calculation of the target positions of each joint through inverse kinematics of the serial robotic arm. Sending down the target positions enables the endoscope to follow the movement of the robotic arm. Furthermore, during the adjustment of the endoscope's field of view, the pose of the surgical instrument at the end of the tool arm changes relative to the field of view. To avoid master-slave remapping after endoscope adjustment, the end-effector pose of the master robotic arm needs to be adjusted in real time during the field of view adjustment process. This ensures that the pose of the master robotic arm's end-effector in the display is consistent with the pose of the surgical instrument in the endoscope's field of view, guaranteeing real-time consistency of master-slave poses.

[0045] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solutions of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A surgical robot field-following system, characterized in that, include: The inertial measurement unit is used to acquire the attitude and acceleration information of the display in real time, and to obtain the initial attitude of the display when the surgical robot enters the master-slave control mode. The pose change acquisition unit is used to calculate the change in the pose of the display relative to the initial pose of the display based on the real-time pose of the display acquired by the inertial measurement unit and the initial pose of the display. Then, the pose change of the endoscope is obtained by combining the transformation matrix between the display and the endoscope. The pose change acquisition unit converts the endoscope's pose change into the rotation angle of the endoscope around each coordinate axis of the endoscope coordinate system. Based on the endoscope's initial pose, it calculates the displacement of the endoscope lens plane center point on each projection plane of the endoscope coordinate system. The positional displacement on each projection plane is combined to obtain the first position change of the endoscope. Simultaneously, it calculates the transformation matrix of the endoscope's rotational motion about a direction perpendicular to the lens plane relative to its initial pose to obtain the pose change of the endoscope's field of view. The initial pose of the endoscope is the pose of the endoscope relative to the RCM point when the surgical robot enters the master-slave control mode. The position increment acquisition unit is used to calculate the current velocity of the display based on the acceleration of the display acquired in real time by the inertial measurement unit and the velocity of the display at the previous moment. From this, the position increment of the display along the vertical direction is calculated, and then the position increment of the display from the start of entering the master-slave control mode to the current moment is calculated. This is then mapped to the second position change of the endoscope. The field-of-view follower unit is used to calculate the current pose transformation matrix of the endoscope relative to the robot base coordinate system, i.e. the current target pose of the endoscope, based on the initial pose of the endoscope, the first position change, the second position change, and the pose change of the endoscope's field of view, and to control the movement of each joint of the surgical robot.

2. The surgical robot field-following system according to claim 1, characterized in that, The pose change acquisition unit converts the pose change of the endoscope into the rotation angle of the endoscope around each coordinate axis of the endoscope coordinate system. Specifically, the pose change acquisition unit converts the pose change of the endoscope into RPY angles and calculates the rotation angle of the endoscope around each coordinate axis of the endoscope coordinate system according to the calculation formula of the rotation matrix.

3. The surgical robot field-following system according to claim 1, characterized in that, The pose change acquisition unit is set with a minimum threshold for the rotational motion of the endoscope. If the obtained rotation angle of the endoscope around a certain coordinate axis of its coordinate system is less than the minimum threshold, the rotational motion is ignored.

4. The surgical robot field-following system according to claim 1, characterized in that, The pose of the endoscope relative to the RCM point is obtained through robot forward kinematics calculation.

5. The surgical robot field-following system according to claim 1, characterized in that, The display is a VR display.

Images