Five-degree-of-freedom robot motion control method

Abstract

The application discloses a kind of five degrees of freedom robot motion control methods, based on discretization motion space, specifically includes the following steps: the motion space of five degrees of freedom robot is divided into multiple regions, and the point in each region is discretized and handled, generates discrete point dataset;Discrete point dataset and its corresponding motion parameter are stored in database, form pose query table;According to the coordinates of target point, find the nearest discrete point in discrete point dataset, and obtain its corresponding motion parameter;The motion parameter found is sent to robot control system, drives robot to complete pose adjustment.The application provides a kind of five degrees of freedom robot motion control method, realizes quick pose query and control, significantly improves the efficiency, accuracy and safety of motion control;The method is suitable for robot control in magnetic resonance interventional surgical treatment, can significantly improve the calculation speed, accuracy and safety, reduce the risk of operation.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion control, and in particular to a five-degree-of-freedom robot motion control method. Background Art

[0002] With the continuous development of magnetic resonance imaging technology, high-definition, high-contrast, and fast imaging equipment has provided favorable conditions for MRI interventional surgery, enabling the development of a large number of clinical trials. MRI interventional surgery, with its advantages such as no radiation and high soft tissue contrast, has become a key development direction in minimally invasive surgery. However, MRI interventional surgery places extremely high demands on robot motion control, including high precision, high real-time performance, and high reliability.

[0003] Traditional robotic motion control methods typically rely on real-time calculations to solve the target pose through inverse kinematics. However, this approach has the following problems in MRI interventional surgery:

[0004] 1. Slow calculation speed: The motion control of a five-degree-of-freedom robot involves the coordinated calculation of multiple joints. Real-time calculation consumes a large amount of computing resources, resulting in a decrease in response speed, making it difficult to meet the high real-time requirements of magnetic resonance interventional surgery.

[0005] 2. Low precision: In complex medical environments, the accuracy of real-time calculations is difficult to guarantee, which may cause deviations in the robot's posture and affect the surgical effect.

[0006] 3. Risks caused by calculation errors: Errors in real-time calculations may cause the robot to lose control of its movement, such as the robotic arm deviating from the predetermined trajectory or colliding, posing a serious threat to the patient's life safety. Summary of the Invention

[0007] In view of the above-mentioned shortcomings of the prior art, the technical problem to be solved by the present invention is that existing robot motion control methods rely on real-time calculations, which are characterized by high computational complexity, slow computational speed, low precision, and computational errors that may lead to uncontrolled robot motion. The present invention provides a five-degree-of-freedom robot motion control method. By precalculating and storing discrete point data in the motion space, it enables rapid position query and control, significantly improving the efficiency, precision, and safety of motion control. This method is suitable for robot control in magnetic resonance interventional surgery, significantly improving computational speed, precision, and safety, and reducing surgical risks.

[0008] To achieve the above objectives, the present invention provides a five-degree-of-freedom robot motion control method based on a discretized motion space, specifically comprising the following steps:

[0009] The motion space of the five-degree-of-freedom robot is divided into multiple regions, and the points in each region are discretized to generate a discrete point data set;

[0010] The discrete point data set and its corresponding motion parameters are stored in the database to form a pose query table;

[0011] According to the coordinates of the target point, find the nearest discrete point in the pose query table and obtain its corresponding robot motion parameters;

[0012] The queried motion parameters are sent to the robot control system to drive each joint of the robot to complete the posture adjustment and reach the target position.

[0013] Furthermore, the motion space of the five-degree-of-freedom robot includes two moving directions and three rotation directions; among them, the moving direction includes movement along the Z0 (parameter d1) and Z1 (parameter d2) axes, and the rotation direction includes deflection around the Z2 axis (parameter θ3), Z3 axis (parameter θ4) and Z4 axis (parameter θ5), and one of the rotations is used to adjust the position.

[0014] Furthermore, when the motion space is divided into multiple areas, it is divided based on the physical model of the robot and the motion constraints. The division of each area includes the generatrix equation and the constraints.

[0015] Furthermore, the construction of the pose query table includes the following steps:

[0016] The forward calculation program generates discrete point data and stores it in the pose lookup table. The forward calculation program traverses and calculates the discrete points of all areas according to different accuracies.

[0017] Verification of target point motion parameters is achieved through inverse calculation program;

[0018] The pose query table, i.e., the database, is stored using an efficient data structure to improve query speed.

[0019] Furthermore, after discretizing the points in each region and generating a discrete point data set, the accuracy of the discretization process is adjusted.

[0020] Furthermore, according to the coordinates of the target point, an efficient search algorithm is used to find the nearest discrete point in the discrete point data set, and the corresponding motion parameters are obtained, which include the joint angles and displacements of the robot.

[0021] Furthermore, according to the coordinates of the target point, the distance between it and the discrete points is calculated using the Euclidean distance nearest neighbor search algorithm until the best discrete point is matched.

[0022] Furthermore, the database adopts a storage method that combines a hash table with a spatial index.

[0023] Furthermore, the motion of the five-degree-of-freedom robot is controlled by using a table lookup method.

[0024] Furthermore, by specifying 3 degrees of freedom, it is not necessary to move all 5 axes to reach the target point. That is, 3 parameters are specified for each fixed position in the space. Specifically, any point in the motion space only needs to move 3 degrees of freedom from the original state to reach the target point.

[0025] Technical Effects

[0026] The present invention provides a five-degree-of-freedom robot motion control method, which is based on a discretized motion space. Through the discretized motion space and database query method, the computational complexity is significantly reduced, the real-time performance of motion control is improved, and the posture adjustment requirements during surgery can be quickly responded to. The discrete point data set covers the entire motion space, ensuring the accuracy of posture calculation and meeting the high-precision requirements of magnetic resonance interventional surgery. The error risk of real-time calculation is avoided, and it is particularly suitable for medical robot systems with high-precision requirements, effectively reducing the risk of loss of control or collision of the robotic arm and ensuring patient safety. The method is not only applicable to magnetic resonance interventional surgery, but can also be extended to other fields.

[0027] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 1 is a schematic diagram of the three-dimensional structure of a five-degree-of-freedom robot according to a preferred embodiment of the present invention;

[0029] Figure 2 Schematic diagram of a physical model of a five-degree-of-freedom robot according to a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0030] Figure 3 Schematic diagram of a five-degree-of-freedom robot motion space according to a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0031] Figure 4 A five-degree-of-freedom robot motion space segmentation diagram of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0032] Figure 5 1. It is a schematic diagram of the division of the motion space of a five-degree-of-freedom robot according to a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0033] Figure 6A diagram showing a set of motion space plane generatrix equations of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0034] Figure 7 Schematic diagram of six surfaces of the motion space of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0035] Figure 8 FIG1 is a schematic diagram of area B of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0036] Figure 9 1 is a schematic diagram of region C of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0037] Figure 10 1 is a schematic diagram of region D of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0038] Figure 11 1 is a schematic diagram of region E of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0039] Figure 12 Schematic diagram of regions A1, B1, C1, D1, and E1 of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0040] Figure 13 Schematic diagram of regions A2, B2, C2, D2, and E2 of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0041] Figure 14 It is a schematic diagram of the F1, F2, F3, and F4 areas of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0042] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0043] In the following description, specific details, such as certain internal procedures and techniques, are provided for purposes of illustration and not limitation to facilitate a thorough understanding of the embodiments of the present invention. However, it will be apparent to those skilled in the art that the present invention may be practiced in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present invention with unnecessary detail.

[0044] The present invention provides a five-degree-of-freedom robot motion control method, which is based on a discretized motion space and specifically includes the following steps:

[0045] Step 100, the motion space of the five-degree-of-freedom robot is divided into multiple regions, and the points in each region are discretely processed to generate a discrete point data set; wherein the five-degree-of-freedom robot is composed of five joints, such as Figure 1 As shown, it includes 2 moving directions (parameters d1 and d2) and 3 rotation directions (parameters θ3, θ4, θ5); among them, the moving directions include movement along Z0 and Z1, the moving range along the Z0 axis is 100 mm, and the moving range along the Z1 axis is 100 mm; the rotation directions include a deflection range of 60° around the Z1 axis, a deflection range of 60° around the Z2 axis, and a deflection range of 60° around the Z3 axis.

[0046] When setting the independent motion range of each axis based on the robot's physical structure, ensure that all axes do not interfere with each other. For example, the range of motion along the Z0 axis should not be affected by changes in the length of the robot arm or other joint angles.

[0047] Through physical modeling, the spatial boundaries of the robot's motion are determined, providing a basis for subsequent discretization processing.

[0048] Step 101, divide the motion space of the five-degree-of-freedom robot into multiple areas (such as A, B, C, D, E, A1, B1, C1, D1, E1, A2, B2, C2, D2, E2, F1, F2, F3, F4), such as Figure 5 Each area corresponds to different motion characteristics, and each area uses different motion ranges of different axes, which will be explained in detail later. Figure 8 For example, the A-series (e.g., A, A1, A2) is located in the center of the motion space, the B-series is located in the front, and the C-series is located in the back. The 1-series (e.g., A1, B1, C1) is located on the left, and the 2-series (e.g., A2, B2, C2) is located on the right. Similar to the AB region, the D and E regions are located at the bottom, with F1-F4 at the very bottom.

[0049] When the motion space is divided into multiple areas, it is based on the robot's physical model and motion constraints. The division of each area includes the busbar equation and constraints.

[0050] Step 102, then, discretize the points in each area to generate a discrete point data set;

[0051] The discretization process of different regions follows the same principle.

[0052] The first step is to determine the accuracy of discretization;

[0053] The second step is to determine the data size of each region based on the motion characteristics of the region (each region includes three motion parameters and the corresponding motion range);

[0054] The third step is to use the forward kinematics equation to traverse and solve the discrete points in each area and record the parameters of the discrete points. The calculation process is as follows:

[0055] according to Figure 1 To calculate the position of the target point, that is, the position of P (target point coordinate system) relative to G (geodetic coordinate system), the parameters of all joints and distances between P and G are required. The calculation model is the DH method. G T P Represents the coordinate system matrix of point P relative to G, Sθ3 represents Sinθ3, and Cθ3 represents Cosθ3.

[0056] G T P = G ξ0· 0 ξ1· 1 ξ2· 2 ξ3· 3 ξ4· 4 ξ P (1)

[0057]

[0058] The accuracy of the discretization process can be adjusted according to actual needs. For example, if the Z0 and Z1 axes are discretized at intervals of 1 mm, and the rotation angle is discretized at intervals of 1°, the accuracy of the final data will be √3. The higher the required accuracy, the more data will be generated.

[0059] The discretized data set includes the coordinates (x, y, z) of each point in the geodetic coordinate system G and its corresponding motion parameters (d1, d2, θ3, θ4, θ5).

[0060] Step 200: Store the discrete point data set and its corresponding motion parameters in a database to form a pose query table. The pose query table is constructed by the following steps:

[0061] The forward calculation program generates discrete point data and stores it in the pose lookup table. The forward calculation program traverses and calculates the discrete points of all areas according to different accuracies.

[0062] Verification of target point motion parameters is achieved through inverse calculation program;

[0063] The pose query table, i.e., the database, is stored using an efficient data structure to improve query speed.

[0064] The pose query table, i.e. the database, uses a storage method that combines hash tables and spatial indexes. The specific structure is as follows:

[0065] Table field design:

[0066] Discrete point ID (primary key, auto-increment integer)

[0067] Coordinate fields: x (floating point, precision 0.01mm), y (floating point), z (floating point)

[0068] Motion parameter fields: d1 (x-axis displacement), d2 (y-axis displacement), θ3 (joint 1 deflection angle, floating point type, accuracy 0.1°), θ4 (joint 2 deflection angle), θ5 (joint 3 rotation angle)

[0069] Area identifier (character type, such as "A1", "B2")

[0070] Index design includes spatial index and hash index:

[0071] Spatial index: Build an R-Tree index based on x, y, and z coordinates to accelerate area range queries.

[0072] Hash index: Create a hash index for the region identification field to quickly locate the region.

[0073] Data generation example

[0074] Taking the A1 area as an example, the discretized database is as follows:

[0075] area Serial number <![CDATA[d1]]> <![CDATA[d2]]> <![CDATA[θ3]]> <![CDATA[θ4]]> <![CDATA[θ5]]> x y z A1 1 10 10 15 10 3 120.23 23.45 45.34 A1 2 …

[0076] As shown in the table, the first column is the sequence number, the second to sixth columns are the robot's motion parameters, and the seventh to ninth columns represent the generated robot's position.

[0077] Step 300: According to the coordinates of the target point, the nearest discrete point is searched in the discrete point data set, and its corresponding motion parameters are obtained; according to the coordinates of the target point, an efficient search algorithm is used to search for the nearest discrete point in the discrete point data set to ensure that the query speed meets the real-time requirements; and the corresponding motion parameters are obtained, and the motion parameters include the joint angle and displacement of the robot; specifically, according to the coordinates of the target point, the Euclidean distance nearest neighbor search algorithm is used to calculate the distance between the target point and the discrete point, and the robot parameters and area corresponding to the target point are found.

[0078] For example, if the robot's target point is P(120.22, 23.44, 45.36), then the above algorithm uses the X, Y, and Z data in the pose lookup table to calculate the target point closest to P as follows:

[0079] Use the Euclidean distance nearest neighbor search algorithm:

[0080] For the target point P(120.22,23.44,45.36), calculate its difference from the discrete point Q(X i ,Y i ,Z i ) distance:

[0081] D=(X P -X i ) 2 +(Y P -Y i ) 2 +(Z P -Z i ) 2

[0082] When D is the smallest, the data in area A is matched (120.23, 23.45, 45.34), and the parameters corresponding to this point are sent to the robot for execution.

[0083] Step 400: Send the queried motion parameters to the robot control system to drive the robot to complete the posture adjustment. Specifically, the motion parameters are sent to the robot's servo motor controller to drive the robot to move to the target posture.

[0084] The following is a specific example to illustrate a specific method for dividing the motion space of a five-degree-of-freedom robot of the present invention.

[0085] In the embodiment of the present invention, a five-axis manipulator is used, and the focus coordinates are controlled by five parameters (movement in two directions + rotation in three directions), such as Figure 1 The following is a structural diagram of the actual calculation involved. By simplifying the model and extracting its actual action line, the actual model can be obtained as follows: Figure 2 As shown:

[0086] In this embodiment, the following parameters are set: the moving range of parameter d1 is 100mm, the moving range of parameter d2 is 100mm, the deflection of θ3 is 60°, the deflection of θ4 is 60°, and the deflection of θ5 is 60°. It can be obtained that the range that the focus can cover is as follows: Figure 3 and Figure 4 As shown:

[0087] The motion space of the five-degree-of-freedom robot is divided into multiple areas, including A, B, C, D, E, A1, B1, C1, D1, E1, A2, B2, C2, D2, E2, F1, F2, F3, and F4, and then each area is discretized. The principle of dividing each area is as follows: In order to find the relationship between the motion space and the five parameters of the robot more quickly and accurately during actual use, the present invention abandons the inverse kinematics solution method and directly specifies three parameters for each fixed position in the space, that is: any point in the motion space can be reached by moving only three degrees of freedom from the original state, thereby greatly saving time and eliminating a lot of time wasted in the inverse process, thereby improving efficiency. The following are the points in each area, the names and ranges of the three joints that need to move.

[0088] Area A: three joint movements: d1, d2, and θ4;

[0089] Region A1: d1, θ4, θ3 (range [-30, 0]);

[0090] A2 region: d1, θ4, θ3 (range [0,30]).

[0091] Region B: three joint movements of d2, θ4, and θ5 (range [-30, 0]);

[0092] Region B1: θ4, θ3 (range [-30, 0]), θ5 (-30, 0]);

[0093] B2 area: θ4, θ3 (range [0,30]), θ5 (-30,0]).

[0094] Region C: three joint movements of θ4, θ3, and θ5 (range [0, 30]);

[0095] C1 region: three joint movements: θ4, θ3 (range [-30, 0]), θ5 (0, 30]);

[0096] C2 area: three joint movements: θ4, θ3 (range [0, 30]), θ5 (0, 30]).

[0097] D region: three joint movements: d1, d2, θ5 (range [-30, 0]);

[0098] D1 area: three joint movements: d1, θ3 (range [-30,0]), θ5 (-30,0]);

[0099] D2 area: three joint movements of d1, θ3 (range [0,30]), θ5 (-30,0]).

[0100] Region E: three joint movements: d1, d2, θ5 (range [-30, 30]);

[0101] E1 region: three joint movements: d1, θ3 (range [-30,0]), θ5 (-30,30]);

[0102] E2 area: three joint movements: θ4, θ3 (range [-30,0]), θ5 (-30,30]).

[0103] F1 area: d1(x,50), d2(-25,0), θ3 range [-30,0], θ4=-30, θ5=-30 three joint movements;

[0104] F4 area: d1(x,50), d2(0,25), θ3 range [-30,0], θ4=-30, θ5=-30 three joint movements;

[0105] F2 area: d1(0,x), d2(-25,0), θ3 range [-30,30], θ4=-30, θ5=-30 three joint motions;

[0106] F3 area: d1(0,x), d2(0,25), θ3 range [-30,30], θ4=-30, θ5=-30 three joint movements.

[0107] The following will explain how each area is divided and its boundaries are defined for each area.

[0108] like Figure 6 As shown, each region has a corresponding mathematical equation and database (pose lookup table). The mathematical equation for each region can be obtained from the generatrix equation. The attached figure shows a plane diagram, and each region is extended from this plane as the generatrix. For example, if the generatrix is ​​the equation of a circle, then the region is a cylinder. Simply define the height range of the cylinder. Therefore, the constraints are used to define the boundaries of the region.

[0109] The boundaries of 19 areas, including Area A, are based on Figure 6 The busbars in the , that is, the busbars are the boundaries of different regions. The busbar equation set is shown in the following table:

[0110] L1:x 2 +z 2 =r 2 , r=92.75, z=[-d1,d1], d1=46.38, x<0;

[0111] L2:(x+d2) 2 +z 2 =r 2,z=[-d1,d1],d2=100,x<0;

[0112] L3:(x+d3+d2) 2 +(z+d4) 2 =r 2 ,d3=46.5,d4=12.46,z=[-(d1+d4),d5],x<0;

[0113] L4:(x-d3) 2 +(z+d4) 2 =r 2 ,z=[-(d1+d4),d5],x<0;

[0114] L5:(x+d7+d2) 2 +(z+d1) 2 =r 2 ,d7=46.5,z=[d1-d 4, d1],x=[-(d2+d3+d7),-(d2+d7)];

[0115] L6:(x-d7) 2 +(z+d1) 2 =r 2 ,x=[-d7,-(d7-d3)]; z=[d1-d 4, d1];

[0116] L7:(x+d7+d2) 2 +(z+3d1) 2 =r 2 ,z=[-(d1+d4) ,- d1],x=[-(d2+d3+d7),-(d2+d7)];

[0117] L8:(x-d7) 2 +(z+3d1) 2 =r 2 ,x=[-d7,-(d7-d3)]; z=[-(d1+d4) , -d1];

[0118] L9:(x-d7) 2 +(z+3d1) 2 =r 2 ,x=[-(d7+d3),-d7]; z=[-(d1+d4) , -d1];

[0119] Each area involves six surfaces of space: up, down, front, back, left, and right. Figure 7 The specific equations and constraints are as follows:

[0120] The equation for region A is:

[0121] Front: x1 2 +z1 2 =r 2 , r=92.75, z1=[-d1,d1], d1=46.38, x<0; y∈[0, 100]

[0122] Back: (x2+d2) 2 +z2 2 =r 2 ,z2=[-d1,d1],d2=100,x<0;y∈[0,100]

[0123] Constraints: x∈[x2,x1]; y∈[0,100]; z∈[-47,47]

[0124] like Figure 8 As shown, the equation for region B is as follows:

[0125] Front: (x1+d2) 2 +z1 2 =r 2 ,z1=[-d1,d1],d2=100,x<0; y∈[0,100];

[0126] Back: (x2+d3+d2) 2 +(z2+d4) 2 =r 2 , d3=46.5, d4=12.46, z2=[-(d1+d4),d5], x<0; y∈[0,100]

[0127] Above: (x3+d7+d2) 2 +(z3+d1) 2 =

[0128] r 2 ,d7=46.5,z3=[d1-d 4, d1],x3=[-(d2+d3+d7),-(d2+d7)];

[0129] Below: (x4+d7+d2) 2 +(z4+3d1) 2 =

[0130] r 2 ,z4=[-(d1+d4) ,- d1],x4=[-(d2+d3+d7),-(d2+d7)];

[0131] Constraints: x∈[x2,x1]; y∈[0,100]; z∈[z4,z3]

[0132] like Figure 9 As shown, the C region equation is as follows:

[0133] Front: (x1-d3) 2 +(z1+d4) 2 =r 2 ,z1=[-(d1+d4),d5],x1<0;y∈[0,100];

[0134] Back: x2 2 +z2 2 =r 2 , r=92.75, z2=[-d1,d1], d1=46.38, x2<0; y∈[0, 100];

[0135] Above: (x3-d7) 2 +(z3+d1) 2 =r 2 ,x3=[-d7,-(d7-d3)]; z3=[d1-d 4, d1];

[0136] Below: (x4-d7) 2 +(z4+3d1) 2 =r 2 ,x4=[-d7,-(d7-d3)]; z4=[-(d1+d4) , -d1];

[0137] Constraints: x∈[x2,x1]; y∈[0,100]; z∈[z4,z3]

[0138] like Figure 10 As shown, the equation for region D is as follows:

[0139] Front: (x-d7) 2 +(z+3d1) 2 =r 2 ,x=[-(d7+d3),-d7]; z=[-(d1+d4) , -d1];y∈[0,100];

[0140] Back: (x+d7+d2) 2 +(z+3d1) 2 =

[0141] r 2 ,z=[-(d1+d4) ,-d1],x=[-(d2+d3+d7),-(d2+d7)]; y∈[0, 100];

[0142] Constraints: x∈[x2,x1]; y∈[0,100]; z∈[-(d1+d4),-d1]

[0143] like Figure 11 As shown, the equation of area E is as follows:

[0144] Above: (x-d7) 2 +(z+3d1) 2 =r 2 ,x=[-(d7+d3),-(d7-d3)]; z=[-(d1+d4) , -d1];

[0145] Constraints: x∈[-(d7+d3),-(d7-d3)]; y∈[0, 100]; z∈[-(d1+d4), z]

[0146] like Figure 12 As shown, the equations for the areas A1, B1, C1, D1, and E1 are as follows:

[0147] Front: x1 2 +z1 2 =r1 2 ; r1=94; z1∈[-47,47]; y1∈[-50,50]

[0148] Rear: x2 2 +z2 2 =r2 2 ; r2=194; z2∈[-47,47]; y2∈[-50,50]

[0149] Left: x3 2 +z3 2 =r3 2 ; r3=94; z3∈[-47,47]; y3∈[-50,50]

[0150] Right: x4 2 +z4 2 =r4 2 ; r4=94; z4∈[-47,47]; y4∈[-50,50]

[0151] Top: x5∈[-181.4,-81.4]; y5∈[-50,50]; z5=47

[0152] Bottom: x6∈[-181.4,-81.4]; y6∈[-50,50]; z6=-47

[0153] like Figure 13 As shown, the equations for the areas A2, B2, C2, D2, and E2 are as follows:

[0154] Front: x1 2 +z1 2 =r1 2 ; r1=94; z1∈[-47,47]; y1∈[-50,50]

[0155] Rear: x2 2 +z2 2 =r2 2 ; r2=194; z2∈[-47,47]; y2∈[-50,50]

[0156] Left: x3 2 +z3 2 =r3 2 ; r3=94; z3∈[-47,47]; y3∈[-50,50]

[0157] Right: x4 2 +z4 2 =r4 2 ; r4=94; z4∈[-47,47]; y4∈[-50,50]

[0158] Top: x5∈[-181.4,-81.4]; y5∈[-50,50]; z5=47

[0159] Bottom: x6∈[-181.4,-81.4]; y6∈[-50,50]; z6=-47

[0160] like Figure 14 As shown, the equations for the F1, F2, F3, and F4 regions are as follows:

[0161] Front: x1 2 +z1 2 =r1 2 ; r1=94; z1∈[-47,47]; y1∈[-50,50]

[0162] Rear: x2 2 +z2 2 =r2 2 ; r2=194; z2∈[-47,47]; y2∈[-50,50]

[0163] Left: x3 2 +z3 2 =r3 2 ; r3=94; z3∈[-47,47]; y3∈[-50,50]

[0164] Right: x4 2 +z4 2 =r4 2 ; r4=94; z4∈[-47,47]; y4∈[-50,50]

[0165] Top: x5∈[-181.4,-81.4]; y5∈[-50,50]; z5=47

[0166] Bottom: x6∈[-181.4,-81.4]; y6∈[-50,50]; z6=-47

[0167] The feasibility of the invented method will be verified below by taking the example of moving the end of the robotic arm to the target point P (52.33, 33.71, 21.54) during magnetic resonance imaging intervention surgery.

[0168] (1) Target point matching

[0169] Use Euclidean distance nearest neighbor search algorithm

[0170] For the target point P, calculate its distance to the discrete point Q. When the distance is minimum, obtain the robot parameters corresponding to the data.

[0171] (2) Motion parameter acquisition

[0172] Scenario: During MRI interventional surgery, the end of the robotic arm needs to be moved to the target point P (52.33, 33.71, 21.54).

[0173] Target point matching: Using a KD tree search, the discrete point Q (52.31, 33.83, 21.44) is matched, with a distance D = 0.12 mm. The motion parameters are obtained: θ3 = 15.5°, θ4 = 0.2°, and d1 = 24.5 mm.

[0174] (3) Motion control

[0175] The control system was sent commands J1 = 15.5, J2 = 0.2, and D1 = 24.5. The robotic arm completed posture adjustment within 120 ms, with a measured position error of 0.08 mm, meeting surgical requirements.

[0176] The present invention provides a five-degree-of-freedom robot motion control method with the following advantages:

[0177] 1. By discretizing the motion space, the traditional solution method that relies on robot inverse calculation is simplified to a table lookup method, which greatly saves computing resources, saves time, and improves efficiency;

[0178] 2. Three degrees of freedom are specified for each robot space point, so it is not necessary to move all five axes to reach the target point, saving movement time and efficiency;

[0179] 3. The discrete precision can be flexibly adjusted according to the application purpose. The size of the query table can be flexibly configured according to the precision, which improves the scope and executability of practical applications.

[0180] 4. In addition to medical robots, this technology can be applied to other equipment such as industrial robots to reduce computing power, save time, and improve efficiency.

[0181] The above describes in detail the preferred embodiments of the present invention. It should be understood that those skilled in the art can make numerous modifications and variations based on the concepts of the present invention without inventive effort. Therefore, any technical solutions that can be derived by those skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A five-degree-of-freedom robot motion control method, characterized in that: Based on the discretized motion space, the following steps are specifically included: The motion space of the five-degree-of-freedom robot is divided into multiple regions, and the points in each region are discretized to generate a discrete point data set. The division of the motion space into multiple regions is based on the robot's physical model and motion constraints. The division of each region includes the generatrix equation and constraints. The discrete point data set and its corresponding motion parameters are stored in the database to form a pose query table; According to the coordinates of the target point, find the nearest discrete point in the pose query table and obtain its corresponding robot motion parameters; The queried motion parameters are sent to the robot control system to drive each joint of the robot to complete the posture adjustment and reach the target position; Among them, the five-degree-of-freedom robot uses five parameters to control the focus coordinates. The focus covers the specified range of motion. The five parameters include movement in two directions and rotation in three directions. Three preset motion parameters are specified for each area divided by the motion space. By looking up the table, the robot's end effector can reach the target point, that is, any point in the motion space. It only needs to move three degrees of freedom from the original state to reach the target point, and there is no need to move five axes to reach the target point.

2. A five-degree-of-freedom robot motion control method according to claim 1, characterized in that: The five degrees of freedom of the five-degree-of-freedom robot include two movement directions and three rotation directions; wherein the movement direction includes movement along the Z0 and Z1 axes, and the rotation direction includes deflection around the Z2, Z3 and Z4 axes.

3. A five-degree-of-freedom robot motion control method according to claim 1, characterized in that: The construction of the pose query table comprises the following steps: Generate discrete point data through the forward calculation program and store it in the pose lookup table; Verification of target point motion parameters is achieved through inverse calculation program; The pose query table, i.e., the database, is stored using an efficient data structure to improve query speed.

4. A five-degree-of-freedom robot motion control method according to claim 1, characterized in that: After discretizing the points in each area and generating a discrete point data set, the accuracy of the discretization process is also adjusted.

5. A five-degree-of-freedom robot motion control method according to claim 1, characterized in that: According to the coordinates of the target point, an efficient search algorithm is used to find the nearest discrete point in the discrete point data set, and the corresponding motion parameters of the robot are obtained. The motion parameters include the joint angle and displacement of the robot.

6. A five-degree-of-freedom robot motion control method as claimed in claim 5, characterized in that: According to the coordinates of the target point, the Euclidean distance nearest neighbor search algorithm is used to calculate the distance between it and the discrete points until the best discrete point is matched.

7. A five-degree-of-freedom robot motion control method as claimed in claim 1, characterized in that: The database adopts a storage method combining a hash table and a spatial index.

8. A five-degree-of-freedom robot motion control method according to claim 1, characterized in that: The motion of the five-degree-of-freedom robot is controlled by using a lookup table.

Images