CN1985773A - Celebral operating robot system based on optical tracking and closed-loop control and its realizing method - Google Patents

Abstract

A brain surgery robot system based on optical tracking closed-loop control, which is a brain surgery robot that receives medical image information, measures and determines the location of lesions, assists in surgical planning, and performs surgical guidance; it is composed of a computer, a five-degree-of-freedom robot, and an optical tracking device. It forms a closed-loop robot pose measurement and real-time feedback control system with the passive marker. The passive marker is installed at the end of the five-degree-of-freedom robot; the five-degree-of-freedom robot includes a robotic arm and a robotic arm controller; the auxiliary surgery planning and guidance software includes Digital image input and preprocessing module, lesion extraction and three-dimensional reconstruction module, surgical planning module, and surgical implementation module; its advantages are: improve the trajectory tracking and positioning accuracy of the robot system, and solve the absolute accuracy problem that limits the popularization and use of surgical robots , reduce the cost of robot design and manufacturing; simplify the registration and calibration process, reduce the workload of doctors and the probability of error, and reduce the pain of patients.

Description

Closed-loop control brain surgery robot system and implementation method based on optical tracking

technical field

The invention belongs to the technical field of robots, and in particular relates to a closed-loop control brain surgery robot system based on optical tracking and its implementation method, specifically a high-precision robot system and its implementation method that can assist doctors in fine brain surgery minimally invasive operations .

Background technique

Brain surgery minimally invasive surgical robot is a robot system that can assist doctors in performing frameless stereotaxic surgery. It generally includes a computer that can assist in surgical planning and guidance, and a robot that performs stereotaxic assisted surgical operations. Among them, the computer that assists in surgical planning and guidance is installed with supporting auxiliary surgical planning and guidance software. Send motion commands to the robot controller to control the robot movement. The software usually includes functional modules such as digital image input and preprocessing, lesion extraction and three-dimensional reconstruction, surgical planning, and surgical implementation. The function of the digital image input and preprocessing module is to receive the externally input medical image data of the patient's brain and convert it into a format that can be displayed by the computer. On this basis, the image is preprocessed, including image enhancement, noise reduction, histogram Statistics, etc.; the role of the lesion extraction and 3D reconstruction module is to allow doctors to segment the lesion on the image in an interactive manner, and extract the contour of the skull, so as to generate a 3D medical model of the lesion and the head; in the operation planning module, the doctor can Carry out the identification of target points and marker points, and plan the surgical path in the virtual reality environment; in the surgical implementation module, according to the pre-planned surgical path, timely send motion instructions to the robot controller to control the movement of the robot, At the same time, the virtual reality function can provide real-time monitoring for the operation.

The robot used in the system is composed of a robotic arm and a controller. Generally, it has 5 joint degrees of freedom. Each joint has a driving mechanism to drive the joint movement, which can ensure that the surgical instrument can reach any point in the surgical space with any posture. The robot controller can accept the movement instructions sent by the computer, and control the movement of each joint of the robot to the designated position in real time.

The biggest problem with existing brain surgery robots is that the positioning accuracy cannot meet the high precision requirements of fine brain surgery. Surgical robots not only require high repetition accuracy, but also have stricter requirements on the absolute accuracy of instrument movement, because the surgical planning performed in the virtual 3D medical model space is ultimately realized by the movement of the robot. Due to the factors of mechanism and control errors, there is inevitably an error between the desired position command and the position actually achieved by the robot. This error may usually reach several millimeters to one centimeter, which is far beyond the precision requirements of minimally invasive surgery and must be overcome. In industrial applications, the absolute positioning error of the robot can be overcome by various methods of calibration, but as a surgical device, the application of surgical robots varies greatly, and generally there is no strictly fixed placement position. Obviously, it is inappropriate to require strict and cumbersome on-site calibration for every operation, and some non-geometric error factors, such as elastic deformation of the mechanism, joint gap and flexibility, are also difficult to solve by calibration. At present, many works consider the method of improving the precision from the mechanical point of view. It is difficult to achieve the desired effect by relying on high-precision mechanism processing, assembly and high-precision control and compensation to ensure the accuracy of the system, and it greatly increases the design and labor of the robot. manufacturing cost. Existing brain surgery robots are globally open-loop, that is, from the position command given by the computer to the process of guiding the robot's movement and positioning, there is a lack of feedback and correction of the robot's end pose, and various error factors cannot be globally closed-loop corrected , the absolute pose accuracy is difficult to guarantee.

In addition, from published reports (such as Chinese Patent Publication No. CN1243690A), the so-called mapping and positioning method to determine the position of the lesion relative to the robot's operating space is too complicated, requiring the use of stereotaxic frames, mapping pins, or double-layer templates, six Joint digital robotic arm, force control human-computer interaction technology, etc., are time-consuming and labor-intensive, and bring pain and pressure to patients; there are also registration methods using visual measurement technology (such as Chinese Patent Publication No. CN1554315A), but the accuracy and environmental adaptation Performance also needs to be improved, and it is difficult to be practical.

Contents of the invention

The purpose of the present invention is to provide a closed-loop control brain surgery robot system based on optical tracking and its implementation method, which can overcome the shortcomings of the prior art. It combines the optical tracking device with the brain surgery robot, uses the optical tracking device to track the current position and attitude of the robot's terminal equipment in real time, and performs real-time position and posture closed-loop control in the optical measurement space, so that the brain surgery robot can accurately follow the pre-planned The ideal surgical path can be moved and positioned to realize a high-precision brain surgery robot system that can meet the requirements of fine brain surgery. At the same time, with the help of optical tracking technology, the process and method of mapping transformation between the three-dimensional medical model space and the coordinate space of the robot base are greatly simplified. Using the invention can make the brain surgery robot achieve high precision, low cost, time saving, convenience, practicality and easy operation.

The brain surgery robot system based on optical tracking closed-loop control proposed by the present invention includes:

Five-degree-of-freedom robot, computer (select model PC P42.0GHz/256M), optical tracking device (select model NDI company POLARIS optical tracking device), optical registration tool (select model NDI company POLARIS standard passive tool), passive marker ( Choose model NDI company POLARIS standard spherical passive marker), medical marker (a small spherical marker point made of tantalum metal material used in medicine, highly opaque to radio, compatible with organisms, and can be implanted for a long time Human body), shaping pillow and operating bed and other equipment; the auxiliary operation planning and guidance software part, that is, receiving medical image information, measuring and determining the location of lesions, assisting in operation planning, and performing operation guidance Brain surgery robot; characterized by:

The computer, the five-degree-of-freedom robot, the optical tracking device, and the passive marker constitute a closed-loop robot pose measurement and real-time feedback control system. The passive marker is installed at the end of the five-degree-of-freedom robot and is always tracked by the optical tracking device. ; The five-degree-of-freedom robot includes a five-degree-of-freedom robotic arm and a robotic arm controller; the assisted surgical planning and guidance software includes a digital image input and preprocessing module, a lesion extraction and three-dimensional reconstruction module, and a surgical planning module , and the operation implementation module.

Use the concrete operation step of the present invention to carry out operation stage to be:

1. When preparing for the operation: paste four medical markers on the patient's head, scan the brain with medical images, and input the scanned medical image information into the computer, use the auxiliary operation planning and guidance software to determine the lesion, and focus on The three-dimensional medical model of the structural lesion and the skull is used to identify the surgical target and the image position of the medical landmark, and plan the surgical path.

2. When performing registration and calibration: let the patient lie on the operating bed, and use a shaping pillow to fix the head relative to the operating bed. On the one hand, use an optical registration tool to measure the coordinates of the four medical markers on the patient’s head. The tracking device is given and sent to the computer, and the computer calculates the mapping transformation between the three-dimensional medical model space and the optical measurement space; The marker cooperates with the computer to automatically determine the mapping transformation between the five-degree-of-freedom robot base coordinate space and the optical measurement space.

3. During the implementation of surgery: first, the computer transforms the pre-planned surgical path in the 3D medical model space into the optical measurement space, and calculates the ideal pose that needs to be achieved at the end of the five-degree-of-freedom robot at the current path point; then, The computer performs the coordinate transformation from the optical measurement space to the coordinate space of the five-degree-of-freedom robot base according to the ideal pose of the current path point, and obtains the ideal position of each joint of the five-degree-of-freedom robot by solving the inverse kinematics of the five-degree-of-freedom robot ; Then, the computer inputs the ideal position of each joint to the five-degree-of-freedom robot to control the movement of the five-degree-of-freedom robot; at the same time, the optical tracking device cooperates with the passive marker installed at the end of the five-degree-of-freedom robot to measure the five-degree-of-freedom robot in real time. The end pose of the five-degree-of-freedom robot is sent to the computer, and the end pose of the five-degree-of-freedom robot is controlled in real time by the control method designed in the present invention, so as to realize accurate trajectory tracking and positioning; Surgical operations are performed with the assistance of robots.

The five-degree-of-freedom robotic arm consists of an arm and a wrist, with five joints, adopting a PPRRR configuration of two sliding joints and three rotational joints; the arm part has three joints, namely joints I, II and III, and joint I Sliding joints are used, and the movement direction is perpendicular to the horizontal plane; the second joint is a sliding joint, which is perpendicular to the first joint in a "T" shape; the third joint is a rotating joint, and the axis is parallel to the movement direction of the first joint; the wrist part has two joints , are the IV and V joints respectively, the IV joint adopts a revolving joint, and the axis is parallel to the axis of the III joint; the V joint adopts a revolving joint, and the axis is perpendicular to the axis of the IV joint; The axis is parallel to the axis of joint IV. Each joint has an independent driving mechanism, which is composed of a stepping motor and a reducer, and the driving mechanism of the sliding joint also includes a ball screw.

The robotic arm controller is composed of a programmable logic controller PLC and a stepper motor driver. A combination of three PLCs and five stepper motor drivers is used to control the five joints of the five-degree-of-freedom robotic arm. The robotic arm controller is connected through the RS232C serial port. The communication component communicates with the external computer, accepts the joint position command input by the external computer, and controls the movement of the mechanical arm to reach the specified position.

NDI's POLARIS optical tracking device is a space measurement and positioning instrument widely used in the medical industry. Passive POLARIS equipment includes a position sensor capable of emitting and receiving infrared illumination light, and is provided with several passive markers or passive tools equipped with passive markers; To determine the position and direction of the tool, the 3D mean square error range is usually within 0.35mm; the POLARIS device and the computer communicate through RS-232/RS-422, and the continuous tracking data update frequency reaches 60HZ.

The method of the closed-loop robot pose measurement and real-time feedback control system is: the end pose of the five-degree-of-freedom robot is measured by an optical tracking device, and the computer calculates the control amount based on the measured pose and the desired pose of the five-degree-of-freedom robot , to control the further movement of the five-degree-of-freedom robot to correct the deviation.

The optical tracking device measures the end pose of the five-degree-of-freedom robot by tracking the passive marker installed at the end of the five-degree-of-freedom robot.

Passive markers are installed at the end of the five-degree-of-freedom robot, and the number is at least three.

The geometric position relationship of the passive marker installed at the end of the five-degree-of-freedom robot satisfies: the distance between any two passive markers cannot be less than 50mm, and the minimum space interval between any two line segments formed by the passive marker lines cannot be less than 5mm, any two line segments are not parallel, and the angle between any two line segments cannot be less than 0.5 degrees.

The realization method of the brain surgery robot system based on optical tracking closed-loop control is characterized in that the pose measurement is carried out by means of optical tracking technology, including: a simple mapping method for mapping transformation between the three-dimensional medical model space and the optical measurement space, and a five-degree-of-freedom robot An automatic mapping method for the mapping transformation between base coordinate space and optical measurement space and a closed-loop control method for the end pose of a five-degree-of-freedom robot.

The three methods involve five coordinate systems, as shown in Figure 2: establish a three-dimensional medical model coordinate system {V} in the three-dimensional medical model space; establish a patient coordinate system {P} on the real patient's head; Establish an optical measurement coordinate system {M} in the space; establish a robot base coordinate system {R} on the base of the five-degree-of-freedom robot; establish an end-tool coordinate system {T} at the end of the five-degree-of-freedom robot; the entire The system takes the optical measurement coordinate system {M} as the benchmark reference system; the patient coordinate system is described based on the points where the four medical markers pasted on the patient's head are located, and the coordinate system is selected from the four medical markers The point M ₀ where any marker is located is taken as the origin of the coordinate system, and the lines connecting M ₀ with the points M ₁ , M ₂ , and M ₃ where the other three markers are located are used as the three coordinate axes.

Detailed steps and mathematical descriptions of the three methods are given below.

1. Mapping transformation between 3D medical model space and optical measurement space

With the help of optical tracking technology for pose measurement, the simple mapping method for the mapping transformation between the 3D medical model space and the optical measurement space is: the doctor sticks four medical markers on the patient's head, these four markers are not on the same plane, and Any three markers are not on the same straight line; CT or MRI medical image scanning is performed on the patient's head, and the scanned image is input into the computer; the patient is placed on the operating bed, and the head is fixed relatively to the operating bed with a shaping pillow, and the optical The registration tool measures the coordinates of the four medical markers on the patient's head in the optical measurement space, and the measured values are input into the computer by the optical tracking device; the computer calculates the mapping transformation between the three-dimensional medical model space and the optical measurement space.

The mapping transformation consists of two sets of coordinate transformations, namely the transformation from the 3D medical model coordinate system {V} to the patient coordinate system {P}, and the transformation from the patient coordinate system {P} to the optical measurement coordinate system {M}. Group switching was determined based on four medical markers affixed to the patient's skull.

Obviously, according to the definition of the patient coordinate system {P}, the coordinates of the four markers in {P} are: M ₀ (0, 0, 0), M ₁ (1, 0, 0), M ₂ (0 , 1, 0), M ₃ (0, 0, 1).

In addition, since the four medical markers are identifiable in medical scan images, their coordinates in the 3D medical model coordinate system {V} can also be obtained, assuming that it is recorded as: M′ ₀ (x _v0 , y _v0 , z _v0 ), M′ ₁ (x _v1 , y _v1 , z _v1 ), M′ ₂ (x _v2 , y _v2 , z _v2 ), M′ ₃ (x _v3 , y _v3 , z _v3 ).

Since the 3D medical model of the patient's brain is reconstructed from the patient's brain scan data, it can be considered that the mapping between the 3D medical model coordinate system {V} and the patient coordinate system {P} is a rigid transformation (including translation, rotation and pull extension), a homogeneous transformation matrix ^V T _P can be used to complete the position mapping in the two coordinate systems. The homogeneous transformation matrix from {P} to {V} is:

$\mathrm{<span\; class="notranslate">\; T</span>}_{\mathrm{<span\; class="notranslate">\; P</span>}}^{\mathrm{<span\; class="notranslate">\; V</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Similarly, the transformation matrix ^M T _P from the patient coordinate system {P} to the optical measurement coordinate system {M} can be determined. The positions of the four medical markers in the optical measurement coordinate system {M} can be obtained by pointing to the four medical markers with the optical registration tool, set as: M ₀ (x _m0 , y _m0 , z _m0 ), M ₁ (x _m1 , y _m1 , z _m1 ), M ₂ (x _m2 , y _m2 , z _m2 ), M ₃ (x _m3 , y _m3 , z _m3 ). Then the homogeneous transformation matrix from {P} to {M} is:

$\mathrm{<span\; class="notranslate">\; T</span>}_{\mathrm{<span\; class="notranslate">\; P</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\text{<span class="notranslate"> the y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Finally, the homogeneous transformation from the 3D virtual model coordinate system {V} to the optical measurement coordinate system {M} is ^M T _V as:

^M T _V = ^M T _P ^P T _V = ^M T _P ( ^V T _P ) ^-1 (3)

2. Mapping transformation between five-DOF robot base coordinate space and optical measurement space

With the help of optical tracking technology for pose measurement, the automatic mapping method for the mapping transformation between the coordinate space of the five-degree-of-freedom robot base and the optical measurement space is as follows: first, four points in the working space of the five-degree-of-freedom robot are selected. The point should be within the measurement range of the optical tracking device at the same time, and the four points are not coplanar, and any three points are not collinear; the computer sends the joint position to the five-degree-of-freedom robot to the above four points according to the preset program commands to control the end of the five-degree-of-freedom robot to move to the above four points in sequence, and the computer records the coordinates of the above-mentioned four points in the coordinate space of the five-degree-of-freedom robot base; With the cooperation of passive markers, the coordinates of the end of the five-degree-of-freedom robot in the optical measurement space are sequentially measured when it moves to the above four points, and sent to the computer; finally, the computer is based on the above-mentioned four points on the five-degree-of-freedom robot base. The coordinate values of the coordinate space and the optical measurement space are calculated, and the mapping transformation between the coordinate space of the five-degree-of-freedom robot base and the optical measurement space is calculated, so as to complete the automatic mapping.

Suppose the coordinates of the four points in the robot base coordinate system {R} are ^R P _i (r _i , s _i , t _i ), (i=1, 2, 3, 4), and at the same time, the four points The coordinates of a point in the optical measurement coordinate system are ^M P _i (u _i , v _i , w _i ), (i=1, 2, 3, 4), then from the robot base coordinate system {R} to the optical measurement The homogeneous transformation matrix of the coordinate system {M} is:

$\mathrm{<span\; class="notranslate">\; T</span>}_{\mathrm{<span\; class="notranslate">\; R</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ {\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]{\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; the\; s</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ {\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

3. The closed-loop control method of the end pose of the five-degree-of-freedom robot

With the help of optical tracking technology for pose measurement, the closed-loop control method of the end pose of the five-degree-of-freedom robot is: taking the optical measurement space as the reference space, first, the computer transforms the pre-planned surgical path in the 3D medical model space into the optical Measure the space, and calculate the ideal pose that needs to be achieved at the end of the five-degree-of-freedom robot at the current path point; then, the computer performs the transformation from the optical measurement space to the five-degree-of-freedom robot base coordinate space based on the ideal pose of the current path point Coordinate transformation, and by solving the inverse kinematics of the five-degree-of-freedom robot, the ideal position of each joint of the five-degree-of-freedom robot is obtained; then, the computer inputs the ideal position of each joint to the five-degree-of-freedom robot to control the movement of the five-degree-of-freedom robot; at the same time, The optical tracking device and the passive marker installed at the end of the five-degree-of-freedom robot are used to measure the pose of the end of the five-degree-of-freedom robot in real time and send it to the computer; then, the computer combines the real-time measured pose information with the pre-planned current The ideal pose of the waypoint is compared to obtain the pose deviation of the two; finally, the computer calculates the correction control amount according to the predetermined control law according to the pose deviation, corrects the ideal pose of the current waypoint, and starts a new round Motion control, so as to achieve precise trajectory tracking and positioning.

The control law adopted by the computer to calculate the corrected control amount according to the posture deviation is: position control adopts proportional-integral-derivative adjustment; attitude control adopts proportional adjustment.

The end pose of the five-degree-of-freedom robot is closed-loop controlled in the optical measurement space.

As shown in Figure 5, the pose of the end tool coordinate system {T} in the optical measurement coordinate system {M} is represented by X=[P ^T φ ^T ] ^T. Among them, P=[xyz] ^T represents the position of {T}; φ=[αβγ] ^T represents the posture of {T}, which consists of a set of ZYX Euler angles,

Note X _d ＝[P _d ^T φ _d ^T ] ^T represents the ideal pose, P _d ＝[x _d y _d z _d ] ^T , φ _d ＝[α _d β _d γ _d ] ^T ; X _r ＝[P _r ^T φ _r ^T ] ^T represents the actual measured pose, P _r ＝[x _{y r} z _r ] ^T , φ _r ＝[α _r β _r γ _r ] ^T ; ΔX＝[ΔP ^T Δφ ^T ] ^T represents The deviation of X _d and X _r , ΔP=[ΔxΔyΔz] ^T , Δφ=[ΔαΔβΔγ] ^T ,

The positional deviation can be calculated directly, namely:

ΔP=P _d -P _r (5)

The calculation of attitude deviation involves rotation transformation, and R _d represents the ideal rotation transformation matrix of {T} relative to {M}; R _r represents the actual transformation matrix obtained from the measured value; ΔR represents the transformation from R _r to R _d , then :

${\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Z</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Y</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; )</span>$

$<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}\\ {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Z</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Y</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; )</span>$

$<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}& {\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

remember: $\mathrm{\&Delta;R}=R_d{R}_r^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{\mathrm{twenty\; one}}& r_{\mathrm{twenty\; two}}& r_{\mathrm{twenty\; three}}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(8\right)$

And again:

$\mathrm{<span\; class="notranslate">\; \&Delta;R</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Z</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Y</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}<span\; class="notranslate">\; )</span>$

$<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&alpha;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&Delta;\&gamma;</span>}\end{array}\right]$

(9)

Combining equations (6), (7), (8) and (9), the attitude deviation Δφ=[ΔαΔβΔγ] ^T can be solved, where:

a. When sinΔβ≠0,

$\mathrm{<span\; class="notranslate">\; \&Delta;\&beta;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; A</span>}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>\sqrt{{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 31</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 32</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 33</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span>$

Δα=Atan2(r ₂₃ /sinΔβ, r ₁₃ /sinΔβ),

Δγ=Atan2(r ₃₂ /sinΔβ, -r ₃₁ /sinΔβ) (10)

b. When sinΔβ=0, if Δβ=0.0, then:

Δα = 0.0,

Δγ=Atan2(-r ₁₂ , r ₁₁ ) (11)

If Δβ=180.0°, then:

Δα = 0.0,

Δγ=Atan2(r ₁₂ , -r ₁₁ ) (12)

The position correction and attitude correction are discussed below, and the formulas are written in discrete form. The subscript k in parentheses represents the kth step of the control loop.

Proportional-integral-derivative (PID) control is adopted for position correction, and the correction control quantity is recorded as U _P ＝[u _x u _y u _z ] ^T , then:

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PP</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; P.I.</span>}}\mathrm{<span\; class="notranslate">\; T</span>}\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PD</span>}}\frac{\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

Among them, T is the sampling control cycle of the system. For example, according to the highest measurement data update rate set by POLARIS 60HZ, T can be taken as 16.7ms; K _PP is a diagonal matrix composed of three proportional control coefficients; K _PI is a diagonal matrix composed of three integral Diagonal matrix composed of control coefficients; K _PD is a diagonal matrix composed of three differential control coefficients. Rewrite the above formula into incremental form:

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PP</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; T</span>}{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; P.I.</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PD</span>}}}{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span>$

$<span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PP</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PD</span>}}}{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; PD</span>}}}{\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 14</span>}<span\; class="notranslate">\; )</span>$

remember $A=K_{\mathrm{PP}}+{\mathrm{TK}}_{\mathrm{P.I.}}+\frac{K_{\mathrm{PD}}}{T},$ $B=K_{\mathrm{PP}}+\frac{{2K}_{\mathrm{PD}}}{T},$ $C=\frac{K_{\mathrm{PD}}}{T},$ Then the algorithm is further written in the form of computer implementation:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; A\&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; B\&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; C\&Delta;P</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\end{array}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>\right.$

The initial value can be F(k-1)=0, ΔP(k-1)=0, each step of the algorithm must calculate ΔP(k), _UP (k), F(k), where F(k) is used Calculate U _P (k) in the next step.

Attitude correction adopts proportional (P) control, and the correction control amount is recorded as U _φ = [U _α u _β u _γ ] ^T , and the corresponding rotation matrix is but:

U _φ (k)＝K _φP Δφ(k) (16)

where K _φP is a diagonal matrix composed of three proportional control coefficients,

$\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Z</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Y</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span>$

则：Record the corrected pose command as ${\hat{x}}_d=[{\hat{P}}_d^T{\widehat{\mathrm{\&phi;}}}_d^T{]}^T,$ ${\hat{P}}_d=[{\hat{x}}_d{\widehat{\mathrm{the\; y}}}_d{\hat{z}}_d{]}^T,$ ${\widehat{\mathrm{\&phi;}}}_d=[{\widehat{\mathrm{\&alpha;}}}_d{\widehat{\mathrm{\&beta;}}}_d{\widehat{\mathrm{\&gamma;}}}_d{]}^T,$ The corresponding rotation matrix is

but:

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; P</span>}}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; Y</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$

Solve jointly by equations (6), (17) and (19) ${\widehat{\mathrm{\&phi;}}}_d\left(k\right)=[{\widehat{\mathrm{\&alpha;}}}_d\left(k\right){\widehat{\mathrm{\&beta;}}}_d\left(k\right){\widehat{\mathrm{\&gamma;}}}_d\left(k\right){]}^T,$ The process can refer to the joint solution of Δφ=[ΔαΔβΔγ] ^T by formulas (6), (7), (8) and (9) above, and will not be repeated here.

代替X_d作为位姿指令送出，用于引导机器人运动。at last,

Instead of X _d , it is sent as a pose command to guide the robot's movement.

The present invention has the advantage that: by performing real-time closed-loop control on the end pose of the robot, the main error factors that affect the movement and positioning of the robot, such as the mechanism and control errors of the robot body, and various measurement and calculation errors during the operation And other factors can be corrected, thereby effectively ensuring the overall motion and positioning accuracy of the surgical robot system, and solving the problem that the trajectory tracking and positioning accuracy of existing surgical robots cannot meet the requirements of clinical fine surgery applications. On the other hand, after adopting the global pose control, the requirements for the mechanism design, manufacturing and control accuracy of the robot body can be relaxed, thereby reducing the design and manufacturing costs of the robot body, and paying more attention to the robot body. Surgical flexibility and operability, design a surgical robot mechanism system that is more coordinated between man and machine. The method proposed by the invention is also suitable for some industrial robot application fields that have strict requirements on absolute precision.

The present invention can also automatically calibrate the mapping transformation between the robot base coordinate space and the optical measurement space, eliminating the cumbersome manual calibration process of the robot base coordinate system, and by pasting medical markers on the patient's head and using optical registration tools to measure The coordinates of the medical markers can easily obtain the mapping transformation between the three-dimensional medical model space and the optical measurement space.

In a word, the present invention can significantly improve the trajectory tracking and positioning accuracy of the robot system on the one hand, not only solves the critical absolute accuracy problem that limits the popularization and use of surgical robots, but also helps to reduce the design and manufacturing costs of the robot body; on the other hand The registration and calibration process is greatly simplified, which not only reduces the workload, psychological burden and probability of errors of doctors, but also reduces the pain and pressure of patients.

Description of drawings

Fig. 1 is a schematic structural view of the present invention.

Fig. 2 is a block diagram of the working principle of the present invention and a schematic diagram of the definition of the involved coordinate system.

Fig. 3 is a flow chart of the mapping method of the mapping transformation between the three-dimensional medical model space and the optical measurement space of the present invention.

Fig. 4 is a flow chart of the automatic mapping method for the mapping transformation between the five-degree-of-freedom robot base coordinate space and the optical measurement space of the present invention.

Fig. 5 is a flowchart and a functional block diagram of the closed-loop control method for the end pose of a five-degree-of-freedom robot of the present invention.

Fig. 6 is a structural schematic diagram of a five-degree-of-freedom robot arm of the present invention.

Detailed ways

The present invention is described in detail with reference to accompanying drawing:

As shown in the figure, the equipment composition and the meanings of the labels in the figure are that it is composed of a computer 1, a five-degree-of-freedom robot 2, an optical tracking device 3, an optical registration tool 4, a passive marker 5, a medical marker 6, a shaping Pillow 7, operating bed 8 hardware parts, and auxiliary surgery planning and guidance software 9 parts, as well as a five-degree-of-freedom robotic arm 11 and a robotic arm controller 10.

The computer 1, the five-degree-of-freedom robot 2, the optical tracking device 3, and the passive marker 5 constitute a closed-loop robot pose measurement and real-time feedback control system, and the passive marker 5 is installed at the end of the five-degree-of-freedom robot 2 , is always tracked by the optical tracking device 3; said five-degree-of-freedom robot 2 includes a five-degree-of-freedom robot arm 11 and a robot arm controller 10; said assisted surgery planning and guidance software 9 includes a digital image input and preprocessing module , lesion extraction and three-dimensional reconstruction module, operation planning module, and operation implementation module. The number of passive markers 5 is at least three.

The specific operation steps are:

1. When preparing for surgery: paste four medical markers 6 on the patient's head, scan the brain with medical images, input the scanned medical image information into the computer 1, and use the auxiliary operation planning and guidance software 9 to determine Lesions, reconstruction of 3D medical models of lesions and skulls, identification of surgical targets and image positions of medical landmarks, and planning of surgical routes;

2. When registering and demarcating: let the patient lie on the operating bed 8, the head is relatively fixed with the operating bed 8 using the shaping pillow 7, and on the one hand use the optical registration tool 4 to measure the four medical markers 6 on the patient's head The coordinates and measured values are given by the optical tracking device 3, and sent to the computer 1, and the computer 1 calculates the mapping transformation between the three-dimensional medical model space and the optical measurement space; on the other hand, the mapping method designed by the present invention is used by five The degree of freedom robot 2, the optical tracking device 3 and the computer 1 cooperate to automatically determine the mapping transformation between the five-degree-of-freedom robot base coordinate space and the optical measurement space;

3. During the implementation of the operation: first, the computer 1 transforms the pre-planned operation path in the three-dimensional medical model space into the optical measurement space, and calculates the ideal pose that needs to be achieved at the end of the five-degree-of-freedom robot at the current path point; then , the computer 1 performs the coordinate transformation from the optical measurement space to the coordinate space of the five-degree-of-freedom robot base according to the ideal pose of the current path point, and by solving the inverse kinematics of the five-degree-of-freedom robot, each joint of the five-degree-of-freedom robot 2 is obtained The ideal position of each joint; then, the computer 1 inputs the ideal position of each joint to the five-degree-of-freedom robot 2 to control the movement of the five-degree-of-freedom robot 2; at the same time, the optical tracking device 3 and the passive marker 5 installed at the end of the five-degree-of-freedom robot Cooperate, measure the terminal pose of five-degree-of-freedom robot in real time and send into computer 1, carry out real-time control to the terminal pose of five-degree-of-freedom robot with the control method of the present invention, realize accurate trajectory tracking and positioning; Finally, five-freedom The five-degree-of-freedom robot 2 is locked, and the doctor performs the operation with the assistance of the five-degree-of-freedom robot 2.

The five-degree-of-freedom robotic arm consists of an arm and a wrist, with five joints, adopting a PPRRR configuration of two sliding joints and three rotational joints; the arm part has three joints, namely joints I, II and III, and joint I 11-1 adopts a sliding joint, and the movement direction is perpendicular to the horizontal plane; the second joint 11-2 adopts a sliding joint, which is perpendicular to the "T" shape of the first joint; the third joint 11-3 adopts a rotating joint, and the axis moves with the first joint The directions are parallel; the wrist part has two joints, namely IV and V joints, the IV joint 11-4 adopts a revolving joint, and the axis is parallel to the axis of the III joint; the V joint 11-5 adopts a revolving joint, and the axis is parallel to the IV joint The axis is vertical; the end instrument is installed on the V joint of the manipulator, and the installation axis is parallel to the IV joint axis. Each joint has an independent driving mechanism, which is composed of a stepping motor and a reducer, and the driving mechanism of the sliding joint also includes a ball screw.

The robotic arm controller 10 includes a programmable logic controller PLC and a stepper motor driver. A combination of three PLCs and five stepper motor drivers is used to control the five joints of the five-degree-of-freedom robotic arm 11. The robotic arm controller 10 is connected via RS232C The serial port communication component communicates with the computer 1, accepts the joint position command input by the computer 1, and controls the five-degree-of-freedom mechanical arm 11 to move to a designated position.

The method of the closed-loop robot pose measurement and real-time feedback control system is: the end pose of the five-degree-of-freedom robot 2 is measured by the optical tracking device 3 by tracking the passive marker 5 installed at the end of the five-degree-of-freedom robot, and the computer 1 according to The measured pose and the expected pose of the five-degree-of-freedom robot 2 are used to calculate the control amount, and the five-degree-of-freedom robot 2 is controlled to move further to correct the deviation; the optical tracking device 3 communicates with the computer 1 through the RS232/RS422 serial port; five The DOF robot 2 communicates with the computer 1 through the RS232C serial port.

In the brain surgery robot system, the geometric positional relationship of the passive marker 5 installed at the end of the five-degree-of-freedom robot 2 satisfies: the distance between any two passive markers 5 cannot be less than 50 mm, and any two passive markers are connected by 5 passive markers. The minimum space interval between the line segments formed by the lines cannot be less than 5mm, any two line segments are not parallel, and the angle between any two line segments cannot be less than 0.5 degrees.

The implementation method of the brain surgery robot system based on optical tracking closed-loop control, with the help of optical tracking technology for pose measurement, including: a simple mapping method for mapping transformation between the 3D medical model space and the optical measurement space, and the coordinate space of the five-degree-of-freedom robot base An automatic mapping method for optical measurement space mapping transformation, a closed-loop control method for the end pose of a five-degree-of-freedom robot.

The simple mapping method for mapping transformation between 3D medical model space and optical measurement space is:

a. The doctor sticks four medical markers 6 on the patient's head, and it is required that these four markers are not on the same plane, and any three markers are not on the same straight line;

b. Carry out CT or MRI medical image scanning on the patient's head, and input the scanned image into the computer 1;

c. Determine the coordinates of the scanning imaging points of the four medical markers in the three-dimensional medical model space;

d. Let the patient lie on the operating bed 8, use the shaping pillow 7 to fix the head relative to the operating bed 8, use the optical registration tool 4 to measure the coordinates of the four medical markers 6 on the patient's head in the optical measurement space, and measure the values input into computer 1 by optical tracking device 3;

e. The computer 1 calculates the mapping transformation between the three-dimensional medical model space and the optical measurement space.

The automatic mapping method for the mapping transformation between the coordinate space of the five-degree-of-freedom robot base and the optical measurement space is:

a. Pre-select four points in the working space of the five-degree-of-freedom robot 2, requiring that these four points should be within the measurement range of the optical tracking device 3, and the four points are not coplanar, and any three points are not collinear;

b. The computer 1 sends joint position commands to the five-degree-of-freedom robot 2 to reach the above-mentioned four points, and controls the five-degree-of-freedom robot 2 to move to the above-mentioned four points in sequence;

c. The computer 1 records the coordinates of the above four points in the coordinate space of the five-degree-of-freedom robot base; at the same time, the optical tracking device 3 cooperates with the passive marker 5 installed at the end of the five-degree-of-freedom robot to measure the five-freedom in turn When the robot 2 moves to the above four points, the coordinates of its end in the optical measurement space are sent to the computer 1;

d. According to the coordinate values of the above four points in the five-degree-of-freedom robot base coordinate space and the optical measurement space, the computer 1 calculates the mapping transformation between the five-degree-of-freedom robot 2 base coordinate space and the optical measurement space, thereby completing the automatic marking Measurement.

The closed-loop control method of the end pose of the five-degree-of-freedom robot is:

a. The computer 1 transforms the surgical path pre-planned in the three-dimensional medical model space into the optical measurement space, and calculates 1 the ideal pose that needs to be achieved at the end of the five-degree-of-freedom robot at the current path point;

b. According to the ideal pose of the current path point, the computer 1 performs coordinate transformation from the optical measurement space to the coordinate space of the five-degree-of-freedom robot base, and obtains each joint of the five-degree-of-freedom robot by solving the inverse kinematics of the five-degree-of-freedom robot the ideal location;

c. Input the ideal position of each joint from the computer 1 to the five-degree-of-freedom robot 2, and control the movement of the five-degree-of-freedom robot 2;

d. Cooperate with the optical tracking device 3 and the passive marker 5 installed at the end of the five-degree-of-freedom robot to measure the pose of the end of the five-degree-of-freedom robot in real time, and send it to the computer 1;

e. Comparing the pose information measured in real time with the ideal pose of the pre-planned current path point by the computer 1 to obtain the pose deviation between the two;

f. Computer 1 uses proportional-integral-derivative adjustment for position control according to the pre-determined control law according to the position deviation; attitude control adopts proportional adjustment, calculates the corrected control amount, corrects the ideal position of the current waypoint, and starts a new one The motion control of the wheel enables precise trajectory tracking and positioning.

Claims (10)

1, a kind of based on optical tracking closed loop control celebral operating robot system, comprise the hardware components that computer (1), robot with five degrees of freedom (2), optictracking device (3), optics registration tool (4), passive marker (5), medical science marker (6), moulding pillow (7) and operation table equipment such as (8) are formed, with assisted surgery planning and guiding software (9) part, be the position that receives medical image information, mensuration and definite focus, the auxiliary celebral operating robot that undergos surgery and plan and carry out surgical guide; It is characterized in that, constitute the robot pose measurement and real-time feedback control system of a closed loop by said computer (1), robot with five degrees of freedom (2), optictracking device (3) and passive marker (5), passive marker (5) is installed in the end of robot with five degrees of freedom (2), is followed the tracks of by optictracking device (3) all the time; Said robot with five degrees of freedom (2) comprises five degree-of-freedom manipulator (11) and mechanical arm controller (10); Said assisted surgery planning comprises digitized video input and pretreatment module with guiding software (9), focus extracts and module is implemented in three-dimensionalreconstruction module, surgery planning module and operation;

Use the undergo surgery concrete operations step in stage of the present invention to be:

One. when before undergoing surgery, preparing: on patient's head, paste four medical science markers (6), brain is carried out medical image scanning, and will scan gained medical image information input computer (1), the planning of utilization assisted surgery is determined focus with guiding software (9), the 3 D medical model of reconstruct focus and head, the sign of target spot and medical science sign image position and planning operation pathway undergo surgery;

Two. registering timing signal: allow patient lie on the operation table (8), head uses moulding pillow (7) and operation table (8) relative fixed, measure the coordinate of four the medical science markers (6) on patient's head on the one hand with optics registration tool (4), measured value is provided by optictracking device (3), and send into computer (1), by the mapping transformation between computer (1) the calculating 3 D medical model space and the optical measurement space; With the assignment test method of the present invention's design, cooperate on the other hand, measure the mapping transformation between robot with five degrees of freedom pedestal coordinate space and the optical measurement space automatically by robot with five degrees of freedom (2), optictracking device (3) and computer (1);

Three. when undergoing surgery enforcement: at first, will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance, and calculate the desirable pose that need reach at current path point robot with five degrees of freedom end by computer (1); Then, by the desirable pose of computer (1) according to current path point, carry out by of the coordinate transform of optical measurement space, and, obtain the ideal position in each joint of robot with five degrees of freedom (2) by finding the solution the robot with five degrees of freedom inverse kinematics to robot with five degrees of freedom pedestal coordinate space; Then, import the ideal position in each joint to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) motion by computer (1); Simultaneously, cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the terminal pose of The real time measure robot with five degrees of freedom is also sent into computer (1), control method with the present invention's design is controlled in real time to the terminal pose of robot with five degrees of freedom, realizes accurate track following and location; At last, robot with five degrees of freedom (2) locking, doctor's operation that under robot with five degrees of freedom (2) auxiliary, undergos surgery.

2. celebral operating robot system according to claim 1 is characterized in that, five degree-of-freedom manipulator (11) is made up of arm and wrist, has five joints, takes the PPRRR configuration of two Gliding joints and three cradle heads; Arm segment has three joints, is respectively I, II and III joint, and I joint 11-1 adopts Gliding joint, and the direction of motion is perpendicular to horizontal plane; II joint 11-2 adopts Gliding joint, and is "T"-shaped vertical with the I joint; III joint 11-3 adopts cradle head, and axis is parallel with I joint motions direction; Wrist partly has two joints, is respectively IV and V joint, and IV joint 11-4 adopts cradle head, and axis is parallel with the III joints axes; V joint 11-5 adopts cradle head, and axis is vertical with the IV joints axes; Terminal apparatus is installed in V joint at mechanical arm, and it is parallel with the IV joints axes that axis is installed; All there is independent driving mechanisms in each joint, is made up of motor, decelerator, and arthrodial driving mechanism also includes ball screw.

3. celebral operating robot system according to claim 1, it is characterized in that, mechanical arm controller (10) comprises programmed logic controller (PLC) and stepper motor driver, adopt three PLC and five stepper motor driver combinations, five joints of corresponding control five degree-of-freedom manipulator (11), mechanical arm controller (10) communicates by RS232C Serial Communication Component and computer (1), accept the joint position order of computer (1) input, control five degree-of-freedom manipulator (11) motion arrives assigned address.

4. celebral operating robot system according to claim 1, it is characterized in that, the robot pose measurement of closed loop with the method for real-time feedback control system is: be installed in the terminal pose that the wooden passive marker of holding (5) of robot with five degrees of freedom is measured robot with five degrees of freedom (2) by optictracking device (3) by tracking, calculate controlled quentity controlled variable by computer (1) according to the pose that pose that measures and expectation robot with five degrees of freedom (2) reach, control robot with five degrees of freedom (2) further moves to revise deviation; Communicate by the RS232/RS422 serial ports between optictracking device (3) and the computer (1); Communicate by the RS232C serial ports between robot with five degrees of freedom (2) and the computer (1).

5. celebral operating robot system according to claim 1 is characterized in that, the quantity of passive marker (5) is at least 3.

6. celebral operating robot system according to claim 1, it is characterized in that, passive marker (5) satisfies at the terminal geometry site of installing of robot with five degrees of freedom (2): the distance between any two passive markers (5) can not be less than 50mm, minimal spatial separation between any two line segments that are made of passive marker (5) line can not be less than 5mm, any two line segments are not parallel, and the angle between any two line segments can not be lower than 0.5 degree.

7. implementation method based on the celebral operating robot system of optical tracking closed loop control, it is characterized in that, carry out pose measurement by means of the optical tracking technology, comprising: the closed loop control method of the automatic assignment test method of the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings, robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings, the terminal pose of robot with five degrees of freedom.

8. according to the said implementation method of claim 7, it is characterized in that the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings is:

A. paste four medical science markers (6) by the doctor in patient head, require these four markers not on same plane, and any three markers are not on same straight line;

B. patient head is carried out CT or the scanning of MRI medical image, gained scan-image input computer (1);

That c. determines four medical science markers is scanned into the coordinate of picture point in the 3 D medical model space;

D. allow patient lie on the operation table (8), head uses moulding pillow (7) and operation table (8) relative fixed, four medical science markers (6) of measuring patient head with optics registration tool (4) are at the spatial coordinate of optical measurement, and measured value is by optictracking device (3) input computer (1);

E. calculate the 3 D medical model space and the spatial mapping transformation of optical measurement by computer (1).

9. according to the said method of claim 7, it is characterized in that the automatic assignment test method of robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings is:

A. four points in chosen in advance robot with five degrees of freedom (2) work space require these four points should be in the measuring range of optictracking device (3), and four somes coplane not, and any three points are conllinear not;

B. send the joint position order that arrives above-mentioned four points by computer (1) to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) moves to above-mentioned four points successively;

C. note the coordinate of above-mentioned four points in robot with five degrees of freedom pedestal coordinate space by computer (1); Simultaneously, cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the coordinate of its end in optical measurement space when sequentially determining robot with five degrees of freedom (2) moves to above-mentioned four points, and send into computer (1);

D. by computer (1) according to above-mentioned four points at robot with five degrees of freedom pedestal coordinate space and the spatial coordinate figure of optical measurement, calculate robot with five degrees of freedom (2) pedestal coordinate space and the spatial mapping transformation of optical measurement, thereby finish automatic mapping.

10. according to the said implementation method of claim 7, it is characterized in that the closed loop control method of robot with five degrees of freedom art end pose is:

A. will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance by computer (1), and calculating (1) goes out the desirable pose that need reach at current path point robot with five degrees of freedom end;

B. by the desirable pose of computer (1) according to current path point, carry out by of the coordinate transform of optical measurement space to robot with five degrees of freedom pedestal coordinate space, and, obtain the ideal position in each joint of robot with five degrees of freedom by finding the solution the robot with five degrees of freedom inverse kinematics;

C. import the ideal position in each joint by computer (1) to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) motion:

D. cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the pose of The real time measure robot with five degrees of freedom end, and send into computer (1);

E. by computer (1) the desirable pose of the posture information of The real time measure with the current path point of planning in advance compared, obtain both pose deviations;

F. by computer (1) according to the pose deviation according to predetermined control law, Position Control adopts proportional-integral-differential to regulate; Attitude control employing ratio is regulated, and the calculation correction controlled quentity controlled variable is revised the desirable pose of current path point, and begins the motor control of a new round, thereby realizes accurate track following and location.

Images