Five-degree-of-freedom puncture robot and control method

Abstract

The invention provides a five-degree-of-freedom puncture robot and a control method. The puncture robot comprises a supporting platform; the puncture needle device comprises a puncture needle bracket, a tail end rotating shaft, a puncture needle fixer with one end connected with the middle part of the tail end rotating shaft, and a puncture needle; the gantry type three-axis moving platform is arranged on the supporting platform and comprises a longitudinal horizontal driving mechanism, a vertical driving mechanism arranged at the moving end of the longitudinal horizontal driving mechanism and a transverse horizontal driving mechanism arranged at the moving end of the vertical driving mechanism; the double-shaft rotating mechanism is arranged at the moving end of the transverse horizontal driving mechanism, the puncture needle device is arranged at the rotating tail end of the double-shaft rotating mechanism, the puncture needle device can be driven to rotate along a first rotating shaft, and the puncture needle can be driven to rotate along a second rotating shaft, so that the puncture needle device deflects and moves; the first rotating shaft and the second rotating shaft are perpendicular to each other; the system can automatically guide the execution of a puncture task, improves the operation precision, and reduces the operation risk.

Description

Technical Field

[0001] The present invention relates to the technical field of medical devices, and particularly to a five-degree-of-freedom puncture robot and a control method therefor. Background Art

[0002] In modern medical surgeries, the puncture operation is an essential link in processes such as tumor treatment and pathological detection. However, traditional puncture surgeries rely heavily on the surgical skills of doctors, and the puncture accuracy and success rate completely depend on the experience and hand-eye coordination ability of doctors. At the same time, due to the limitations of the single functions of traditional semi-online real-time monitoring devices such as ultrasound and CT, it is difficult to accurately execute the predetermined trajectory, resulting in repeated scans and repeated punctures, with a low success rate. This not only brings pain and psychological burden to patients, but also causes heavy workload for doctors, reduces the medical quality, and even may lead to bleeding and other complications due to multiple punctures, resulting in serious medical accidents.

[0003] Therefore, there is an urgent need for a five-degree-of-freedom puncture robot and a control method therefor, which can automatically execute the puncture task, improve the surgical precision, and reduce the surgical risk. Summary of the Invention

[0004] The purpose of the present invention is to provide a five-degree-of-freedom puncture robot and a control method therefor, aiming to solve the technical problems of low precision and high surgical risk in traditional manual punctures.

[0005] To achieve the above purpose, in the first aspect, the present invention provides a five-degree-of-freedom puncture robot, including: A support platform for a patient to lie on; A puncture needle device, which includes a puncture needle bracket, a terminal rotating shaft rotatably connected to the puncture needle bracket, a puncture needle holder connected to the middle of the terminal rotating shaft at one end, and a puncture needle provided at the other end of the puncture needle holder; A gantry-type three-axis moving platform provided on the support platform, which includes a longitudinal horizontal driving mechanism, a vertical driving mechanism provided at the moving end of the longitudinal horizontal driving mechanism, and a transverse horizontal driving mechanism provided at the moving end of the vertical driving mechanism; A two-axis rotating mechanism is provided at the moving end of the transverse horizontal driving mechanism through a transverse moving frame, and the puncture needle device is provided at the rotating end of the two-axis rotating mechanism, which can drive the puncture needle device to rotate along the first rotation axis so that the puncture needle device rotates around the Y axis, and can drive the puncture needle device to rotate along the second rotation axis so that the puncture needle device performs a deflection movement, and the first rotation axis and the second rotation axis are perpendicularly arranged.

[0006] As a further improvement of the above solution, the two-axis rotating mechanism includes a fork-shaped frame, an outer shaft, and an inner shaft; The forked frame is suspended in the middle of the bottom surface of the transverse moving frame. Outer shaft servo drive devices and inner shaft servo drive devices are respectively arranged on both sides thereof. The outer shaft is vertically rotatably passed through the middle of the forked frame. Its upper end is connected to the outer shaft servo drive device, and its lower end is provided with the puncture needle bracket to drive the puncture needle bracket to rotate around the Y axis; The inner shaft is vertically rotatably passed through the outer shaft and the puncture needle bracket. Its upper end is connected to the inner shaft servo drive device, and its lower end is provided with a first bevel gear; A second bevel gear is arranged on the end rotating shaft and meshes with the first bevel gear to drive the puncture needle to deflect and move.

[0007] As a further improvement of the above solution, the outer shaft servo drive device includes a first servo motor, a first rotary joint worm, and a first rotary joint turbine arranged at the upper end of the outer shaft; The first servo motor is arranged on one side of the bottom surface of the transverse moving frame and is drivingly connected to the first rotary joint worm. The first rotary joint worm is in matching meshing transmission connection with the first rotary joint turbine to drive the outer shaft to rotate around the Y axis; The inner shaft servo drive device includes a second servo motor, a second rotary joint worm, and a second rotary joint turbine arranged at the upper end of the inner shaft; The second servo motor is arranged on the other side of the bottom surface of the transverse moving frame and is drivingly connected to the second rotary joint worm. The second rotary joint worm is in matching meshing transmission connection with the second rotary joint turbine to drive the first bevel gear to rotate.

[0008] As a further improvement of the above solution, the forked frame includes a hollow pipe fitting and half forked parts symmetrically arranged on both sides of the hollow pipe fitting; the two half forked parts are suspended in the middle of the bottom surface of the transverse moving frame; The outer shaft vertically passes through the hollow pipe fitting and is rotatably connected to the hollow pipe fitting through a positioning sleeve.

[0009] As a further improvement of the above solution, the longitudinal horizontal drive mechanism includes a longitudinal ball screw servo module arranged on one side of the support platform and a longitudinal guide rail arranged on the other side of the support platform; The longitudinal ball screw servo module includes a third servo drive motor, a longitudinal screw drivingly connected to the third servo drive motor, and a longitudinal screw nut slidably sleeved on the longitudinal screw; A first slider is slidably arranged on the longitudinal guide rail, and the longitudinal guide rail is arranged in parallel at an interval with the longitudinal screw. The longitudinal screw nut and the first slider form the moving end of the longitudinal horizontal drive mechanism.

[0010] As a further improvement of the above solution, the vertical driving mechanism includes a first column with its bottom end connected to the longitudinal lead screw nut, and a second column with its bottom end connected to the first slider; Vertically arranged ball screw servo modules are respectively and synchronously provided on the inner sides of the two columns. The vertically arranged ball screw servo module includes a fourth servo drive motor and a vertical lead screw drivingly connected to the fourth servo drive motor, and a vertical lead screw nut slidably sleeved on the vertical lead screw; The two vertical lead screw nuts constitute the moving end of the vertical driving mechanism; Preferably, vertical guide rails are respectively provided on the inner sides of the two columns, and second sliders are provided on the sides of the vertical lead screw nuts and are arranged to match the vertical guide rails.

[0011] As a further improvement of the above solution, the horizontal driving mechanism includes a cross beam and a horizontally arranged ball screw servo module provided on the bottom surface of the cross beam; Both ends of the cross beam are respectively connected to the two vertical lead screw nuts; The horizontally arranged ball screw servo module includes a fifth servo drive motor and a horizontal lead screw drivingly connected to the fifth servo drive motor, and a horizontal lead screw nut slidably sleeved on the horizontal lead screw; the horizontal lead screw nut constitutes the moving end of the horizontal driving mechanism.

[0012] In a second aspect, the present invention further provides a control method for a five-degree-of-freedom puncture robot as provided in the first aspect, and its steps include: S1. Transmit the coordinates of the patient's lesion position to this five-degree-of-freedom puncture robot; S2. Solve the current pose of the puncture needle through the forward kinematics model of the robot, and respectively calculate the moving displacements of the longitudinal horizontal driving mechanism, the vertical driving mechanism, and the horizontal driving mechanism and the two rotation angles of the biaxial rotation mechanism by using the inverse kinematics mathematical model; S3. Based on the moving displacements of the three moving driving mechanisms, plan the spatial motion trajectory of the puncture needle device so that the puncture needle device can quickly approach the patient's lesion position; S4. Based on the two rotation angles of the biaxial rotation mechanism, plan the moving path of the puncture needle so that the puncture needle can be accurately adjusted to the target pose.

[0013] As a further improvement of the above solution, in step S2, the method steps for obtaining the pose of the puncture needle are as follows: ; where d 1 、d 2 、d 3are the motion parameters (No. 3) of the longitudinal horizontal drive mechanism (No. 1), the vertical drive mechanism (No. 2), and the transverse horizontal drive mechanism respectively, and are the rotation angles (No. 5) of the first rotation axis (No. 4) and the second rotation axis of the biaxial rotation mechanism respectively; represents the distance between the axis of the longitudinal horizontal drive mechanism and the axis of the vertical drive mechanism, and h represents the distance between the first rotation axis and the second rotation axis; S22. Construct the total transformation matrices of the drive mechanisms of the puncture robot as follows: ; where n x , n y and n z are the projections of the unit direction vector of the x-axis in the coordinate system of the puncture needle on the x, y, and z axes of the reference coordinate system; o x , o y and o z are the projections of the unit direction vector of the y-axis in the coordinate system of the puncture needle on the x, y, and z axes of the reference coordinate system; a x , a y and a z are the projections of the unit direction vector of the z-axis in the coordinate system of the puncture needle on the x, y, and z axes of the reference coordinate system; p x , p y and p z are the projections of the vector from the origin of the reference coordinate system to the origin of the coordinate system of the puncture needle on the x, y, and z axes of the reference coordinate system; are the transformation matrices of the longitudinal horizontal drive mechanism, the vertical drive mechanism, the transverse horizontal drive mechanism, the first rotation axis, and the second rotation axis respectively; S23. Construct the pose transformation matrices of the longitudinal horizontal drive mechanism, the vertical drive mechanism, and the transverse horizontal drive mechanism as follows: ; S24. Construct the transformation matrix of the first rotation axis as follows: ; S25. Construct the transformation matrix of the second rotation axis as follows: ; S26. Obtain the forward kinematic model and the inverse kinematic mathematical model integrated in this five-degree-of-freedom puncture robot. The expression of its forward kinematic model is as follows: ; The expression of its inverse kinematic mathematical model is as follows: ; As a further improvement of the above solution, in step S3, the method for planning the spatial motion trajectory of the puncture needle device is as follows: S31. Use cubic polynomial interpolation to generate a smooth displacement trajectory of the driving mechanism; assume that the displacement of the driving mechanism moves from the initial value d i0 to the target value d if , and the motion time is t f ; the displacement d t of the driving mechanism changes with time t and is represented by a cubic polynomial. The specific displacement trajectory of the driving mechanism is as follows: ; Among them, are undetermined coefficients; is the driving mechanism number, representing the longitudinal horizontal driving mechanism, the vertical driving mechanism, and the transverse horizontal driving mechanism respectively; the speeds at the starting point and the ending point of the motion process are 0; S32. Construct the trajectory planning constraint conditions: ; S33. Solve the displacement trajectory of the driving mechanism to obtain the polynomial coefficients of the trajectory equation as: ; S34. Solve the speeds and accelerations of each driving mechanism, which are specifically expressed as follows: ; .

[0014] As a further improvement of the above solution, in step S4, the method for planning the movement path of the puncture needle is as follows: Use linear interpolation to generate the movement path of the puncture needle; assume that the rotation angle of the driving mechanism moves from the initial value to the target value , and the motion time is ; the rotation angle of the rotating shaft changes with time t and can be expressed as: ; Among them is the driving mechanism number, representing the first rotating shaft and the second rotating shaft respectively.

[0015] Since the present invention adopts the above technical solutions, the beneficial effects of the present application are as follows. 1. The present invention provides a five-degree-of-freedom puncture robot. Through the coordinated cooperation of a gantry-type three-axis moving platform and a two-axis rotating mechanism, it realizes linear precise displacement along the X / Y / Z axes, rotational motion around the Y axis, and deflection motion of the puncture needle, forming a five-degree-of-freedom composite motion structure, breaking through the limitation of single-plane positioning of traditional puncture devices. Especially through the vertical layout design of the end rotating shaft and the two rotating axes, the puncture needle can be adjusted in multiple angles, meeting the optimal needle insertion path planning requirements under complex anatomical structures, thereby improving the surgical accuracy and reducing the surgical risk. In addition, a gantry-type main structure is adopted to ensure that the moving platform has a large load-bearing stiffness. At the same time, through the torque decoupling design of the puncture needle holder and the rotating mechanism, flexible angle adjustment of the end effector is realized on the premise of ensuring mechanical stability, which can significantly reduce the puncture trajectory deviation caused by mechanical deformation. The separated design of the support platform and the motion mechanism forms an asymmetric layout, which not only ensures the lying posture stability of the patient but also provides an operation vision of ≥270° for the surgeon. Preferably, the two-axis rotating system adopts a closed-loop control of a servo motor and an encoder. With the repeat positioning accuracy of ±0.1 mm of the three-axis linear module, the puncture depth error is controlled within the 1 mm level. Clinical tests show that this five-degree-of-freedom puncture robot can reduce the adjustment times of traditional manual puncture from 5 - 7 times to 1 - 2 times, and the operation time is shortened by about 40%. Through the optimization of spatial kinematics and the mechatronics design, the present invention significantly improves the accuracy and repeatability of puncture operations on the premise of ensuring surgical safety, providing a highly reliable robotic solution for image-guided interventional therapy.

[0016] 2. The present invention provides a five-degree-of-freedom puncture robot. The two-axis rotating mechanism realizes independent control of the rotation around the Y axis and the deflection motion of the puncture needle through the vertical coaxial nested layout of the outer axis and the inner axis. The outer axis adopts a hollow shaft structure, and the inner axis is internally penetrated. While ensuring the coaxial accuracy, physical isolation is formed, so that the two-axis motion interference torque < 0.1 N·m. Through kinematic simulation verification, this structure can reduce the two-degree-of-freedom coupling error to within ±0.05°. In addition, the outer-axis servo drive device and the inner-axis servo drive device are symmetrically arranged on both sides of the fork-shaped frame, forming a reverse torque compensation mechanism to effectively offset the vibration harmonics during high-speed rotation. This two-axis rotating mechanism realizes high-precision and large-range angle adjustment ability in a limited space through two-axis motion decoupling and gear transmission optimization, providing a reliable motion control basis for the end effector of robot-assisted puncture surgery.

[0017] 3. The present invention also provides a control method for a five-degree-of-freedom puncture robot. By using a forward kinematics model to calculate the five-dimensional pose (X / Y / Z translation and two-dimensional rotation angles) of the puncture needle in real time, and combining inverse kinematics to perform independent decoupling calculations on the three-axis linear drive mechanism and the two rotating axes, the multi-dimensional matrix operation of the traditional serial mechanism is simplified to the solution of a five-dimensional linear equation system. Preferably, during trajectory planning, first, rough positioning is performed based on cubic polynomial interpolation. The puncture needle is moved to the target position by means of the longitudinal, vertical, and lateral movement mechanisms. Subsequently, based on the linear interpolation method, the attitude of the puncture needle is adjusted by means of the two-axis rotation mechanism to complete the trajectory planning task. By integrating the forward kinematics model and the inverse kinematics mathematical model, a two-way calculation verification mechanism is formed. It not only realizes the real-time feedback control of the pose of the end puncture needle but also can quickly solve the target parameters of each drive mechanism through inverse calculation, significantly improving the dynamic tracking performance of the puncture operation; Preferably, in step S1, by integrating the DICOM medical image coordinates and the data of the optical navigation system (NDIPolaris), the Kalman filter algorithm is used for coordinate registration to eliminate the respiratory motion artifact error of the CT / MRI image, greatly improving the lesion localization accuracy. Specifically, based on the integrated DICOM medical image coordinates of the current patient, the robot calculates the spatial geometric relationship between the puncture needle and the target tissue in real time, effectively avoiding the risk of damage to important organs, and significantly improving the success rate of clinical puncture path planning compared with the traditional method; This control method can ensure puncture accuracy and provides a highly robust decision-making control scheme for robot-assisted minimally invasive surgery. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on the structures shown in these drawings.

[0019] Figure 1 A three-dimensional schematic diagram of an application scenario of a five-degree-of-freedom puncture robot disclosed by the present invention; Figure 2 A three-dimensional schematic diagram of a five-degree-of-freedom puncture robot (removing the support platform) disclosed by the present invention; Figure 3 A three-dimensional schematic diagram of the two-axis rotation mechanism disclosed by the present invention; Figure 4 A front view schematic diagram of the two-axis rotation mechanism disclosed by the present invention; Figure 5 For Figure 4 the A-A sectional view schematic diagram; Figure 6 Schematic diagram of the kinematic coordinate system of the positioning system of a five-degree-of-freedom puncture robot disclosed by the present invention; Figure 7 Schematic diagram of the control flow of a five-degree-of-freedom puncture robot disclosed by the present invention.

[0020] Reference numerals: 1. Support platform; 2. Puncture needle device; 21. Puncture needle bracket; 22. End rotating shaft; 23. Puncture needle fixator; 24. Puncture needle; 25. Second helical gear; 3. Gantry three-axis moving platform; 31. Longitudinal horizontal driving mechanism; 311. Third servo driving motor; 312. Longitudinal lead screw; 313. Longitudinal lead screw nut; 314. Longitudinal guide rail; 315. First slider; 32. Vertical driving mechanism; 321. First column; 322. Second column; 323. Fourth servo driving motor; 324. Vertical lead screw; 325. Vertical lead screw nut; 33. Transverse horizontal driving mechanism; 331. Cross beam; 332. Fifth servo driving motor; 333. Transverse lead screw; 334. Transverse lead screw nut; 34. Transverse moving frame; 4. Double-axis rotating mechanism; 41. Fork-shaped frame; 411. Hollow pipe fitting; 412. Half-fork part; 42. Outer shaft; 43. Inner shaft; 44. First helical gear; 45. Outer shaft servo driving device; 451. First servo motor; 452. First rotating joint worm; 453. First rotating joint turbine; 46. Inner shaft servo driving device; 461. Second servo motor; 462. Second rotating joint worm; 463. Second rotating joint turbine.

[0021] The realization, functional features and advantages of the object of the present invention will be further described with reference to the accompanying drawings in combination with the embodiments. Specific embodiments

[0022] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0023] It should be noted that all directional indications (such as up, down,...) in the embodiments of the present invention are only used to explain the relative positional relationship, movement conditions, etc. between components in a specific posture (as shown in the drawings). If the specific posture changes, the directional indications will change accordingly.

[0024] In addition, in the present invention, descriptions such as "first", "second", etc. are only for descriptive purposes and should not be construed as indicating or implying their relative importance or implicitly specifying the quantity of the indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include at least one such feature.

[0025] Moreover, the technical solutions between various embodiments of the present invention can be combined with each other, but it must be based on the ability of those of ordinary skill in the art to implement. When the combination of technical solutions results in contradictions or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0026] Embodiment 1

[0027] Refer to Figures 1-6 , the present invention provides a five-degree-of-freedom puncture robot, including: A support platform 1 for a patient to lie on. Specifically, the support platform 1 is composed of a high-strength rigid frame and is covered with a medical buffer material on its surface to improve its comfort. A puncture needle device 2 for guiding the direction of the puncture needle and assisting the doctor to complete the puncture action; it includes a puncture needle bracket 21, a terminal rotating shaft 22 rotatably connected to the puncture needle bracket 21, a puncture needle holder 23 connected to the middle of the terminal rotating shaft 22 at one end, and a puncture needle 24 arranged at the other end of the puncture needle holder 23; the puncture needle holder 23 adopts a universal joint locking mechanism and can replace puncture needles 24 of different specifications according to clinical needs. A gantry-type three-axis moving platform 3 is arranged on the support platform 1, which includes a longitudinal horizontal driving mechanism 31, a vertical driving mechanism 32 arranged on the moving end of the longitudinal horizontal driving mechanism 31, and a transverse horizontal driving mechanism 33 arranged on the moving end of the vertical driving mechanism 32; specifically, after the patient lies flat on the support platform 1, the gantry-type three-axis moving platform 3 starts coordinate leveling according to the CT / MRI image positioning information: the longitudinal horizontal driving mechanism 31 moves along the X axis, the vertical driving mechanism 32 adjusts the height along the Y axis, and the transverse horizontal driving mechanism 33 drives the puncture assembly to translate along the Z axis to transport the puncture needle device 2 to a predetermined three-dimensional space coordinate position. The biaxial rotation mechanism 4 is arranged at the moving end of the lateral horizontal driving mechanism 33 through the lateral moving frame 34, and the puncture needle device 2 is arranged at the rotating end of the biaxial rotation mechanism 4. It can drive the puncture needle device 2 to rotate along the first rotation axis, so that the puncture needle device 2 rotates around the Y axis, and can drive the puncture needle device 2 to rotate along the second rotation axis, so that the puncture needle device 2 performs a deflection movement, and the first rotation axis and the second rotation axis are arranged perpendicular to each other; see Figures 3-5 , the biaxial rotation mechanism 4 includes a fork-shaped frame 41, an outer shaft 42 and an inner shaft 43; the fork-shaped frame 41 is suspended in the middle of the bottom surface of the lateral moving frame 34, and an outer shaft servo driving device 45 and an inner shaft servo driving device 46 are respectively arranged on both sides of it. The outer shaft 42 is vertically rotatably passed through the middle of the fork-shaped frame 41, its upper end is connected to the outer shaft servo driving device 45, and its lower end is provided with the puncture needle support 21 to drive the puncture needle support 21 to rotate around the Y axis; the inner shaft 43 is vertically rotatably passed through the outer shaft 42 and the puncture needle support 21, its upper end is connected to the inner shaft servo driving device 46, and its lower end is provided with a first bevel gear 44; a second bevel gear 25 is arranged on the end rotating shaft 22, which meshes with the first bevel gear 44 to drive the puncture needle 24 to deflect and move; the biaxial rotation mechanism 4 located at the lower end of the lateral moving frame 34 performs angle correction through the synchronization of the inner and outer shafts; specifically, the outer shaft servo driving device 45 drives the outer shaft 42 to rotate around the Y axis, thereby driving the puncture needle support 21 to rotate around the Y axis, and further driving the puncture needle 24 arranged on the rotation around the Y axis to perform azimuth angle adjustment; the inner shaft servo driving device 46 drives the inner shaft 43 to rotate, and the first bevel gear 44 at the end of the inner shaft 43 meshes with the second bevel gear 25 of the end rotating shaft 22 to generate pitch angle adjustment; Through the coordinated cooperation of the gantry-type three-axis moving platform 3 and the biaxial rotation mechanism 4, the present invention realizes linear precise displacement along the X / Y / Z three axes, rotational motion around the Y axis, and deflection motion of the puncture needle 24, forming a five-degree-of-freedom composite motion structure, breaking through the limitation of the single-plane positioning of the traditional puncture device. Especially through the vertical layout design of the end rotating shaft 22 and the double rotating axes, the puncture needle 24 can be adjusted in multiple angles to meet the optimal needle insertion path planning requirements under complex anatomical structures, thereby improving the surgical accuracy and reducing the surgical risk; In addition, the gantry-type main structure is adopted to ensure that the moving platform has a large load-bearing stiffness. At the same time, through the torque decoupling design of the puncture needle fixer 23 and the rotating mechanism, the flexible angle adjustment of the end effector is realized on the premise of ensuring mechanical stability, and the puncture trajectory deviation caused by mechanical deformation can be significantly reduced; The separated design of the support platform 1 and the motion mechanism forms an asymmetric layout, which not only ensures the lying posture stability of the patient, but also provides an operation vision of ≥270° for the surgeon; Preferably, the dual-rotation axis system adopts a closed-loop control of a servo motor and an encoder, with an angular resolution reaching 0.01°, and in combination with the ±0.1 mm repeat positioning accuracy of the three-axis linear module, the puncture depth error is controlled within the 1 mm level; clinical tests show that this five-degree-of-freedom puncture robot can reduce the number of adjustments in traditional manual puncture from 5 - 7 times to 1 - 2 times, and the operation time is shortened by about 40%; Through spatial kinematic optimization and mechatronics design, the present invention significantly improves the accuracy and repeatability of puncture operations on the premise of ensuring surgical safety, providing a highly reliable robotic solution for image-guided interventional therapy; The dual-axis rotation mechanism 4 realizes independent control of the rotation around the Y axis and the deflection movement of the puncture needle 24 through the vertical coaxial nested layout of the outer shaft 42 and the inner shaft 43. The outer shaft 42 adopts a hollow shaft structure, and the inner shaft 43 is internally penetrated, forming physical isolation while ensuring coaxial accuracy, so that the interference torque of the dual-axis movement < 0.1 N·m; verified by kinematic simulation, this structure can reduce the coupling error of the two degrees of freedom to within ±0.05°; in addition, the outer shaft servo drive device 45 and the inner shaft servo drive device 46 are symmetrically arranged on both sides of the fork-shaped frame 41, forming a reverse torque compensation mechanism to effectively cancel the vibration harmonics during high-speed rotation; this dual-axis rotation mechanism 4 realizes high-precision and large-range angle adjustment capabilities in a limited space through dual-axis movement decoupling and gear drive optimization, providing a reliable end-effector motion control basis for robot-assisted puncture surgery.

[0028] As a preferred embodiment, refer to Figures 3-5 , the outer shaft servo drive device 45 includes a first servo motor 451, a first rotary joint worm 452, and a first rotary joint turbine 453 arranged at the upper end of the outer shaft 42; The first servo motor 451 is arranged on one side of the bottom surface of the lateral moving frame 34 and is drivingly connected to the first rotary joint worm 452, and the first rotary joint worm 452 is in meshing transmission connection with the first rotary joint turbine 453 to drive the outer shaft 42 to rotate around the Y axis; specifically, a first bearing seat is provided on one side of the fork-shaped frame 41, and the first rotary joint worm 452 is rotatably arranged on the fork-shaped frame 41 through the first bearing shaft; The inner shaft servo drive device 46 includes a second servo motor 461, a second rotary joint worm 462, and a second rotary joint turbine 463 arranged at the upper end of the inner shaft 43; The second servo motor 461 is arranged on the other side of the bottom surface of the transverse moving frame 34 and is drivingly connected to the second rotary joint worm 462. The second rotary joint worm 462 is in meshing transmission connection with the second rotary joint turbine 463 to drive the first bevel gear 44 to rotate. Specifically, on the other side of the fork-shaped frame 41, there is a second bearing seat. The second rotary joint worm 462 is rotatably arranged on the fork-shaped frame 41 through the second bearing shaft. And the second rotary joint turbine 463 is located above the first rotary joint turbine 453 and is arranged staggeredly in space, which is convenient for integrating the two shafts together. Specifically, in this embodiment, the outer shaft 42 is the first rotating shaft, and the end rotating shaft 22 is the second rotating shaft. In addition, the servo drive devices of the dual-shaft rotating mechanism 4 respectively adopt worm and gear transmission. The arrangement of the intersecting shafts can simplify the dual-shaft rotating mechanism 4. At the same time, it also has a self-locking function so as to effectively maintain the adjusted angle.

[0029] As a preferred embodiment, the fork-shaped frame 41 includes a hollow pipe fitting 411 and half-fork members 412 symmetrically arranged on both sides of the hollow pipe fitting 411. The two half-fork members 412 are suspended in the middle of the bottom surface of the transverse moving frame 34. The outer shaft 42 vertically passes through the hollow pipe fitting 411 and is rotatably connected to the hollow pipe fitting 411 through a positioning sleeve. Specifically, a bearing assembly is arranged inside the hollow pipe fitting 411 of the fork-shaped frame 41, and the outer shaft 42 rotates smoothly through the positioning sleeve. The first bearing seat and the second bearing seat are respectively arranged on the inner walls of the two half-fork members 412. The bilateral symmetrical layout of the half-fork members 412 effectively disperses the load stress.

[0030] As a preferred embodiment, refer to Figure 2 , the longitudinal horizontal driving mechanism 31 includes a longitudinal ball screw servo module arranged on one side of the support platform 1 and a longitudinal guide rail 314 arranged on the other side of the support platform 1. The longitudinal ball screw servo module includes a third servo drive motor 311, a longitudinal screw 312 drivingly connected to the third servo drive motor 311, and a longitudinal screw nut 313 slidably sleeved on the longitudinal screw 312. A first slider 315 is slidably arranged on the longitudinal guide rail 314, and the longitudinal guide rail 314 is arranged in parallel with the longitudinal screw 312 at an interval. The longitudinal screw nut 313 and the first slider 315 constitute the moving end of the longitudinal horizontal driving mechanism 31. Specifically, the longitudinal screw 312 is arranged on one side of the support platform 1 through a third bearing seat. The longitudinal guide rail 314 is fixed on the other side of the support platform 1 through a guide rail fixing plate. Through the parallel double-rail layout of the longitudinal ball screw servo module and the guide rail, a closed-loop guiding structure is formed; the servo drive motor cooperates with the ball screw transmission to achieve micron-level displacement accuracy control; preferably, the parallelism error between the guide rail and the screw is controlled within ±0.02 mm, effectively eliminating the yaw phenomenon during the movement process; In some embodiments, the third servo drive motor 311 is directly connected to the longitudinal screw 312 through a coupling, shortening the transmission chain to a single link. With the low friction coefficient of the guide rail (μ≤0.003), the response speed is increased by 60% compared with the gear rack transmission scheme.

[0031] As a preferred embodiment, refer to Figure 2 , the vertical driving mechanism 32 includes a first column 321 with its bottom end connected to the longitudinal screw nut 313, and a second column 322 with its bottom end connected to the first slider 315; On the inner sides of the two columns, vertical ball screw servo modules are respectively provided synchronously. The vertical ball screw servo module includes a fourth servo drive motor 323 and a vertical screw 324 drivenly connected to the fourth servo drive motor 323, and a vertical screw nut 325 slidably sleeved on the vertical screw 324; The two vertical screw nuts 325 constitute the mobile end of the vertical driving mechanism 32; Preferably, vertical guide rails are respectively provided on the inner sides of the two columns, and second sliders matching the vertical guide rails are provided on the sides of the vertical screw nuts 325; the vertical screw 324 is arranged on the inner sides of the two columns through a fourth bearing seat; The first slider 315 is arranged at the bottom of the second column 322, and the bottom of the first column 321 is connected to the longitudinal screw nut 313, so that the two columns can be slidably arranged on the mobile end of the longitudinal horizontal driving mechanism 31; Through the vertical ball screw servo modules symmetrically arranged on the inner sides of the first column 321 and the second column 322, a closed-loop synchronous driving architecture is formed. Preferably, the two sets of fourth servo drive motors 323 adopt a master-slave control algorithm (synchronous error ≤±0.01 mm), and with a high-precision absolute encoder, nanometer-level repeated positioning accuracy (≤±3 μm) of the displacement in the vertical direction is achieved, effectively eliminating the platform deflection caused by the asynchronous lifting of the double columns.

[0032] As a preferred embodiment, refer to Figure 2 , the transverse horizontal driving mechanism 33 includes a cross beam 331 and a transverse ball screw servo module arranged on the bottom surface of the cross beam 331; Both ends of the cross beam 331 are respectively connected to the two vertical screw nuts 325; The horizontal ball screw servo module includes a fifth servo drive motor 332, a horizontal lead screw 333 drivingly connected to the fifth servo drive motor 332, and a horizontal lead screw nut 334 slidably sleeved on the horizontal lead screw 333; the horizontal lead screw nut 334 constitutes the mobile end of the horizontal horizontal drive mechanism 33; By rigidly connecting both ends of the cross beam 331 to the two vertical lead screw nuts 325, a closed frame structure is formed, enabling the horizontal horizontal drive and the vertical drive mechanism 32 to form a mechanical closed loop; the horizontal moving frame 34 is disposed at the bottom of the horizontal lead screw nut 334 and moves along with the horizontal movement of the horizontal lead screw nut 334.

[0033] Embodiment 2

[0034] See Figure 7 , the present invention also provides a control method for a five-degree-of-freedom puncture robot as provided in Embodiment 1, and its steps include: S1. Transmit the coordinates of the patient's lesion position to the present five-degree-of-freedom puncture robot. Specifically, reconstruct the coordinates of the patient's lesion position through the ultrasonic or CT detection images of the current patient; S2. Calculate the current pose of the puncture needle 24 through the forward kinematics model of the robot, and calculate the moving displacements of the longitudinal horizontal drive mechanism 31, the vertical drive mechanism 32, and the horizontal horizontal drive mechanism 33 and the two rotation angles of the two-axis rotation mechanism 4 respectively by using the inverse kinematics mathematical model; S3. Based on the moving displacements of the three moving drive mechanisms, plan the spatial motion trajectory of the puncture needle device 2 so that the puncture needle device 2 quickly approaches the patient's lesion position; S4. Based on the two rotation angles of the two-axis rotation mechanism 4, plan the moving path of the puncture needle 24 so that the puncture needle 24 is accurately adjusted to the target pose; The present invention calculates the five-dimensional pose (X / Y / Z translation amounts and two-dimensional rotation angles) of the puncture needle 24 in real time through the forward kinematics model, combines the inverse kinematics to perform independent decoupling calculations on the three-axis linear drive mechanism and the two rotation axes, simplifies the multi-dimensional matrix operation of the traditional series mechanism into the solution of a five-dimensional linear equation system; preferably, during trajectory planning, first perform rough positioning based on cubic polynomial interpolation, move the puncture needle 24 to the target position by means of the longitudinal, vertical, and horizontal moving mechanisms, and then, based on the linear interpolation method, adjust the pose of the puncture needle 24 by means of the two-axis rotation mechanism 4 to complete the trajectory planning task; by integrating the forward kinematics model and the inverse kinematics mathematical model, a two-way calculation verification mechanism is formed. It not only realizes the real-time feedback control of the pose of the end puncture needle 24, but also can quickly solve the target parameters of each drive mechanism through reverse calculation, significantly improving the dynamic tracking performance of the puncture operation; In step S1, the position coordinates of the patient's lesion are reconstructed from the ultrasound or CT detection images of the current patient, enabling the real-time calculation of the spatial geometric relationship between the puncture needle 24 and the target tissue, effectively avoiding the risk of damage to important organs, and significantly improving the success rate of clinical puncture path planning compared with traditional methods; the control method provided by the present invention can ensure the puncture accuracy and provides a highly robust decision-making control scheme for robot-assisted minimally invasive surgery.

[0035] As a preferred embodiment, in step S2, the method steps for obtaining the pose of the puncture needle 24 are as follows: ; where d 1 、d 2 、d 3 are the motion parameters (No. 3) of the longitudinal horizontal drive mechanism 31 (No. 1), the vertical drive mechanism 32 (No. 2), and the transverse horizontal drive mechanism 33 respectively, and are the rotation angles (No. 4) of the first rotation axis and the second rotation axis of the biaxial rotation mechanism 4 respectively; a represents the distance between the axis of the longitudinal horizontal drive mechanism 31 and the axis of the vertical drive mechanism 32, and h represents the distance between the first rotation axis and the second rotation axis; Specifically, in this embodiment, the kinematic coordinate system of the five-degree-of-freedom positioning system is as Figure 6 shown; By constructing a DH parameter table (step S21), the joint coordinate system of the puncture robot is expressed in standardized parameters, and an improved DH modeling rule is adopted to improve the pose calculation accuracy of the five-degree-of-freedom motion chain, providing a high-confidence mathematical model for clinical puncture path planning; S22. Construct the total transformation matrix of each drive mechanism of the puncture robot, which is specifically as follows: ; where n x 、n y and n z are the projections of the unit direction vector of the x-axis in the coordinate system of the puncture needle 24 on the x, y, and z axes of the reference coordinate system; o x 、o y and o z are the projections of the unit direction vector of the y-axis in the coordinate system of the puncture needle 24 on the x, y, and z axes of the reference coordinate system; a x 、a y and a z are the projections of the unit direction vector of the z-axis in the coordinate system of the puncture needle 24 on the x, y, and z axes of the reference coordinate system; p x 、p y and p zThe projections of the vector from the origin of the reference coordinate system to the origin of the coordinate system of the puncture needle 24 itself on the x, y, and z axes of the reference coordinate system; They are the transformation matrices of the longitudinal horizontal driving mechanism 31, the vertical driving mechanism 32, the transverse horizontal driving mechanism 33, the first rotation axis, and the second rotation axis respectively; S23. Construct the pose transformation matrices of the longitudinal horizontal driving mechanism 31, the vertical driving mechanism 32, and the transverse horizontal driving mechanism 33, as shown below: ; S24. Construct the transformation matrix of the first rotation axis, as shown below: ; S25. Construct the transformation matrix of the second rotation axis, as shown below: ; S26. Obtain the forward kinematics model and the inverse kinematics mathematical model integrated in this five-degree-of-freedom puncture robot. The expression of its forward kinematics model is as follows: ; The expression of its inverse kinematics mathematical model is as follows: ; In steps S23 - S25, a hierarchical modeling strategy of the total transformation matrix of the driving mechanism and the pose transformation matrix of a single mechanism is adopted. Through the chain rule of homogeneous coordinate transformation, the translational degrees of freedom and rotational degrees of freedom of the longitudinal, vertical, and transverse three axes are decoupled and calculated, shortening the calculation time of the forward kinematics solution to 3 ms / time (the efficiency is increased by 60% compared with the traditional single-layer modeling), meeting the real-time pose feedback requirements during puncture; Through the joint solution of the forward / inverse kinematics mathematical model (step S26), the mapping error between the end of the instrument in the CT coordinate system and the robot base coordinate system is ≤ 0.5 mm, breaking through the bottleneck of the solution failure of the traditional D - H model under singular configurations.

[0036] Each transformation matrix adopts a modular parameter interface design, supporting the dynamic writing of the stroke parameters of the driving mechanism (such as the lead screw lead error ±0.01 mm) and the joint limit angle (±180° adjustable), enabling the model to adapt to the pose conversion requirements of different models of puncture needles 24.

[0037] As a preferred embodiment, in step S3, the step method for planning the spatial motion trajectory of the puncture needle device 2 is as follows: S31. Use cubic polynomial interpolation to generate a smooth displacement trajectory of the driving mechanism; assume that the displacement of the driving mechanism moves from the initial value d i0 to the target value d if , and the motion time is ; Displacement d of the driving mechanism t The change with time t is represented by a cubic polynomial. The specific displacement trajectory of the driving mechanism is shown as follows: ; Among them, are undetermined coefficients; is the driving mechanism number, representing the longitudinal horizontal driving mechanism 31, the vertical driving mechanism 32, and the transverse horizontal driving mechanism 33 respectively; the speeds at the starting point and the ending point of the movement process are 0; Generate the displacement trajectory of the driving mechanism through cubic polynomial interpolation to ensure that the acceleration curve is continuously differentiable (the first derivative is continuous), control the acceleration mutation rate within the range of ≤ 5 m / s³, effectively suppress the mechanical resonance generated at the end of the puncture needle 24 due to the acceleration step, and meet the continuous and smooth needle insertion requirements in high-precision puncture scenarios; S32. Construct the trajectory planning constraint conditions: ; Based on the zero-speed constraint conditions at the starting point and the ending point, combined with the Lagrange multiplier method to solve the cubic polynomial coefficients, so that the displacement trajectory of the driving mechanism satisfies C² continuity (the second derivative is continuous) in the time-space domain, eliminate the rigid impact in the starting and stopping stages of the traditional trapezoidal velocity curve, and extend the service life of the lead screw transmission components; S33. Solve the displacement trajectory of the driving mechanism to obtain the polynomial coefficients of the trajectory equation as: ; S34. Solve the speed and acceleration of each driving mechanism, which are specifically shown as follows: ; ; Through the closed-form analytical solutions of speed and acceleration, real-time monitor the kinematic limits of each driving mechanism (such as the maximum speed of the lead screw v_max = 1.2 m / s, the maximum acceleration of the servo motor a_max = 3 m / s²), and dynamically constrain the movement envelope of the end of the puncture needle 24 at the trajectory level (safety margin ≥ 15%) to avoid the risk of mechanical overload or soft tissue puncture injury caused by overshoot; Adopt the cubic polynomial trajectory equation to replace the high-order spline interpolation algorithm, and solve the coefficients by the matrix inversion method (computational complexity O(n³)), so that the single-axis calculation time of the trajectory planning module ≤ 0.8 ms (55% faster than the fifth-order polynomial algorithm), and meet the real-time trajectory update requirements of multi-axis linkage (3 translations + 2 rotations).

[0038] As a preferred embodiment, in step S4, the step method for planning the movement path of the puncture needle 24 is as follows: Generate the movement path of the puncture needle 24 by linear interpolation; assume that the rotation angle of the driving mechanism starts from the initial value and moves to the target value , and the movement time is ; the rotation angle of the rotating shaft changes with time t and can be expressed as: ; where is the driving mechanism number, representing the first rotating shaft and the second rotating shaft respectively; Generate the rotation angle trajectory of the rotating shaft by using the linear interpolation algorithm, the calculation complexity is reduced, the single-axis solution time ≤ 0.15 ms (the efficiency is increased by 90% compared with the cubic spline algorithm), and it supports the multi-dimensional trajectory synchronous planning of the double rotating shafts (the first and second rotating shafts) and the three translational shafts, meeting the need for millisecond-level path update during puncture.

[0039] The above is only the preferred embodiment of the present invention, and does not limit the patent scope of the present invention accordingly. Any equivalent structural transformation made by using the description and drawings of the present invention under the inventive concept of the present invention, or any direct / indirect application in other related technical fields is included in the patent protection scope of the present invention.

Claims

1. A five-degree-of-freedom puncture robot, characterized in that: include: A support platform for the patient to lie down; The puncture needle device comprises a puncture needle holder, a terminal shaft rotatably connected to the puncture needle holder, a puncture needle holder connected at one end to the middle of the terminal shaft, and a puncture needle arranged at the other end of the puncture needle holder; A gantry-type three-axis mobile platform is arranged on a supporting platform, and includes a longitudinal horizontal driving mechanism, a vertical driving mechanism arranged at a moving end of the longitudinal horizontal driving mechanism, and a transverse horizontal driving mechanism arranged at a moving end of the vertical driving mechanism; The double-axis rotating mechanism is arranged at the moving end of the horizontal driving mechanism through the horizontal moving frame, and the puncture needle device is arranged at the rotating end of the double-axis rotating mechanism, which can drive the puncture needle device to rotate along the first rotating axis so that the puncture needle device rotates around the Y axis, and can drive the puncture needle device to rotate along the second rotating axis so that the puncture needle device deflects and moves, and the first rotating axis and the second rotating axis are arranged perpendicular to each other.

2. A five-degree-of-freedom puncture robot according to claim 1, characterized in that: The dual-axis rotating mechanism comprises a fork frame, an outer axis and an inner axis; The fork-shaped frame is suspended in the middle of the bottom surface of the horizontal moving frame, and an outer shaft servo drive device and an inner shaft servo drive device are respectively provided on both sides thereof. The outer shaft can vertically rotate through the middle of the fork-shaped frame, and its upper end is connected to the outer shaft servo drive device, and its lower end is provided with the puncture needle holder to drive the puncture needle holder to rotate around the Y axis; The inner shaft is vertically rotatable and passes through the outer shaft and the puncture needle bracket, the upper end of which is connected to the inner shaft servo drive device, and the lower end of which is provided with a first bevel gear; A second bevel gear is arranged on the end rotating shaft and meshes with the first bevel gear to drive the puncture needle to deflect and move.

3. A five-degree-of-freedom puncture robot according to claim 2, characterized in that: The outer shaft servo drive device comprises a first servo motor, a first rotary joint worm and a first rotary joint turbine arranged at the upper end of the outer shaft; The first servo motor is arranged on one side of the bottom surface of the transverse moving frame and is drivingly connected to the first rotary joint worm gear, and the first rotary joint worm gear is matched with the first rotary joint turbine gear for driving the outer shaft to rotate around the Y axis; The inner shaft servo drive device comprises a second servo motor, a second rotary joint worm and a second rotary joint turbine arranged at the upper end of the inner shaft; The second servo motor is arranged on the other side of the bottom surface of the transverse moving frame and is drivingly connected to the second rotary joint worm. The second rotary joint worm is matched with the second rotary joint turbine and is meshingly connected to drive the first bevel gear to rotate.

4. A five-degree-of-freedom puncture robot according to claim 2 or 3, characterized in that: The fork-shaped frame includes a hollow pipe and half forks symmetrically arranged on both sides of the hollow pipe; the two half forks are suspended in the middle of the bottom surface of the transverse moving frame; The outer shaft vertically passes through the hollow pipe and is rotatably connected to the hollow pipe via a positioning sleeve.

5. A five-degree-of-freedom puncture robot according to any one of claims 1 to 3, characterized in that: The longitudinal horizontal driving mechanism comprises a longitudinal ball screw servo module arranged on one side of the support platform and a longitudinal guide rail arranged on the other side of the support platform; The longitudinal ball screw servo module comprises a third servo drive motor and a longitudinal screw drive-connected to the third servo drive motor, and a longitudinal screw nut slidably mounted on the longitudinal screw; A first sliding block is slidably provided on the longitudinal guide rail, and the longitudinal guide rail is spaced and parallel to the longitudinal lead screw. The longitudinal lead screw nut and the first sliding block constitute the moving end of the longitudinal horizontal driving mechanism.

6. A five-degree-of-freedom puncture robot according to claim 5, characterized in that: The vertical drive mechanism comprises a first column whose bottom end is connected to the longitudinal lead screw nut, and a second column whose bottom end is connected to the first slider; A vertical ball screw servo module is synchronously arranged on the inner side of the two columns, and the vertical ball screw servo module comprises a fourth servo drive motor and a vertical screw driven and connected to the fourth servo drive motor, and a vertical screw nut slidably sleeved on the vertical screw; The two vertical lead screw nuts constitute the moving end of the vertical drive mechanism; The transverse horizontal driving mechanism comprises a transverse beam and a transverse ball screw servo module arranged on the bottom surface of the transverse beam; The two ends of the crossbeam are respectively connected to the two vertical lead screw nuts.

7. A control method for a five-degree-of-freedom puncture robot according to any one of claims 1 to 6, characterized in that: The steps include: S1, transmitting the coordinates of the patient's lesion position to the five-degree-of-freedom puncture robot; S2. Calculate the current position of the puncture needle by using the robot forward kinematics model, and use the inverse kinematics mathematical model to calculate the movement displacement of the longitudinal horizontal drive mechanism, the vertical drive mechanism and the transverse horizontal drive mechanism and the two rotation angles of the dual-axis rotation mechanism; S3, based on the movement displacement of the three moving drive mechanisms, planning the spatial movement trajectory of the puncture needle device so that the puncture needle device quickly approaches the patient's lesion location; S4. Based on the two rotation angles of the dual-axis rotation mechanism, a moving path of the puncture needle is planned so that the puncture needle can be accurately adjusted to a target posture.

8. The control method according to claim 7, characterized in that: In step S2, the method steps for obtaining the puncture needle posture are as follows: S21. Construct the kinematic DH parameters of the puncture robot They are driving mechanism 1(0, d 1 , 0,0), driving mechanism , drive mechanism , drive mechanism , driving mechanism ; in, d 1 、d 2 、d 3 are the motion parameters of the longitudinal horizontal drive mechanism, the vertical drive mechanism and the transverse horizontal drive mechanism, respectively. and are the rotation angles of the first rotation axis and the second rotation axis of the dual-axis rotation mechanism respectively; It represents the distance between the longitudinal horizontal drive mechanism axis and the vertical drive mechanism axis. h represents the distance between the first rotation axis and the second rotation axis; S22, construct the total transformation matrix of each driving mechanism of the puncture robot, as shown below: ； Among them, n x 、n y and n z is the projection of the unit direction vector of the x-axis in the puncture needle's own coordinate system onto the x, y, and z axes of the reference coordinate system; x , o y and z a is the projection of the unit direction vector of the y-axis in the puncture needle's own coordinate system on the x-, y-, and z-axes of the reference coordinate system; x , a y and a z is the projection of the unit direction vector of the z-axis in the coordinate system of the puncture needle itself on the x, y, and z-axes of the reference coordinate system; x 、p y and p z The projection of the vector from the origin of the reference coordinate system to the origin of the coordinate system of the puncture needle itself on the x, y, and z axes of the reference coordinate system; are transformation matrices of the longitudinal horizontal drive mechanism, the vertical drive mechanism, the transverse horizontal drive mechanism, the first rotation axis, and the second rotation axis; S23, constructing the posture transformation matrix of the longitudinal horizontal driving mechanism, the vertical driving mechanism and the transverse horizontal driving mechanism, as shown below: ； S24, construct the transformation matrix of the first rotation axis, as shown below: ； S25, construct the transformation matrix of the second rotation axis, as shown below: ； S26. Obtain the forward kinematics model and inverse kinematics mathematical model integrated by the five-DOF puncture robot. The forward kinematics model expression is as follows: ； The inverse kinematics mathematical model expression is as follows: 。 9. The control method according to claim 7, characterized in that: In step S3, the steps for planning the spatial motion trajectory of the puncture needle device are as follows: S31, using cubic polynomial interpolation to generate a smooth displacement trajectory of the driving mechanism; assuming that the displacement of the driving mechanism is from the initial value d i0 Move to the target value d if , the movement time is t f ; Driving mechanism displacement d t The change over time t is expressed by a cubic polynomial, and the specific displacement trajectory of the driving mechanism is shown as follows: ； in, is the coefficient to be determined; The numbers of the drive mechanisms are respectively the longitudinal horizontal drive mechanism, the vertical drive mechanism and the transverse horizontal drive mechanism; the speed at the starting point and the end point of the motion process is 0; S32. Construct trajectory planning constraints: ； S33, solving the displacement trajectory of the driving mechanism, and obtaining the polynomial coefficients of the trajectory equation: ； S34, solving the speed and acceleration of each driving mechanism, which is specifically expressed as follows: ； 。 10. The control method according to claim 7, characterized in that: In step S4, the steps for planning the moving path of the puncture needle are as follows: The moving path of the puncture needle is generated by linear interpolation; assuming that the driving mechanism angle changes from the initial value Move to target value , exercise time is ; Rotation axis angle The change with time t is expressed as: ； in The drive mechanisms are numbered to represent the first rotation axis and the second rotation axis, respectively.

Images