Motion control method for five-degree-of-freedom robot

Abstract

The invention discloses a five-degree-of-freedom robot motion control method, which is based on a discretized motion space, and specifically comprises the following steps: dividing the motion space of a five-degree-of-freedom robot into a plurality of areas, and discretizing points in each area to generate a discrete point data set; storing the discrete point data set and the corresponding motion parameters in a database to form a pose query table; according to the coordinate of the target point, searching the discrete point with the nearest distance in the discrete point data set, and obtaining a corresponding motion parameter; and the inquired motion parameters are sent to a robot control system, and the robot is driven to complete pose adjustment. According to the five-degree-of-freedom robot motion control method provided by the invention, rapid pose query and control are realized, and the efficiency, precision and safety of motion control are remarkably improved; the method is suitable for robot control in magnetic resonance interventional operation treatment, the calculation speed, precision and safety can be remarkably improved, and the operation risk is reduced.

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 technology, high-definition, high-contrast fast imaging equipment has provided good conditions for magnetic resonance interventional surgery, enabling a large number of clinical trials to be carried out. Magnetic resonance interventional surgery has the advantages of no radiation and high soft tissue contrast, and has become an important development direction of minimally invasive surgery. However, magnetic resonance interventional surgery places extremely high requirements on robot motion control, including high precision, high real-time performance and high reliability.

[0003] Traditional robot motion control methods usually rely on real-time calculations to solve the target position through inverse kinematics. However, this method has the following problems in magnetic resonance 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 lot of computing resources, resulting in a decrease in response speed, which makes it difficult to meet the high real-time requirements of magnetic resonance interventional surgery.

[0005] 2. Low precision: In a complex medical environment, 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 move out of control, such as the robot 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 defects of the prior art, the technical problem to be solved by the present invention is that the existing robot motion control method relies on real-time calculation, which has high computational complexity, slow computational speed, low precision, and computational errors that may cause the robot motion to lose control. The present invention provides a five-degree-of-freedom robot motion control method, which realizes fast posture query and control by pre-calculating and storing discrete point data in the motion space, and significantly improves the efficiency, precision and safety of motion control; the method is suitable for robot control in magnetic resonance interventional surgery, which can significantly improve the computational speed, precision and safety, and reduce surgical risks.

[0008] To achieve the above object, 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 a 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 point position.

[0013] Furthermore, the motion space of the five-degree-of-freedom robot includes two moving directions and three rotation directions; wherein the moving directions include the Z 0 (parameter d1), Z 1 (Parameter d2) axis movement, rotation direction including around Z 2 Axis (parameter θ 3 ), Z 3 Axis (parameter θ 4 ) and Z 4 Axis (parameter θ 5 ) of which one rotation 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, and 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 discrete point data is generated by the forward calculation program and stored in the pose query table; wherein the forward calculation program traverses and calculates the discrete points of all areas according to different accuracies;

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

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

[0019] Furthermore, after the points in each region are discretized to generate a discrete point data set, the accuracy of the discretization processing is adjusted.

[0020] Furthermore, according to the coordinates of the target point, an efficient search algorithm is used in the discrete point data set to find the nearest discrete point, and the corresponding motion parameters are obtained, wherein the motion parameters 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 point 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 combining a hash table and a spatial index.

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

[0024] Furthermore, 3 degrees of freedom are specified, and the target point can be reached without moving all 5 axes, 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] A five-degree-of-freedom robot motion control method provided by the present invention 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 robot arm loss of control or collision, and ensuring patient safety. The method is not only suitable for 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 It is a three-dimensional structural schematic diagram of a five-degree-of-freedom machine of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0029] Figure 2 It is a 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 It is a schematic diagram of a five-degree-of-freedom robot motion space of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0031] Figure 4 It is 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 5It is a schematic diagram of the division of the motion space area 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 6 It is a set diagram 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 It is a 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 It is a schematic diagram of region B of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

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

[0037] Fig.10 It is a D-area schematic diagram of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

[0038] Fig.11 It 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] Fig.12 It is a schematic diagram of the A1, B1, C1, D1, and E1 regions of a five-degree-of-freedom robot motion control method according to a preferred embodiment of the present invention;

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

[0041] Fig.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 in conjunction with 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 specific internal procedures and techniques are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[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, divide the motion space of the five-degree-of-freedom robot into multiple regions, and perform discrete processing on the points in each region 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 (parameter d 1 d 2 ) and 3 rotation directions (parameter θ 3 ,θ 4 ,θ 5 ), wherein the moving direction includes along Z 0 , Z 1 Move along Z 0 The moving range of the axis is 100mm along the Z 1 The axis movement range is 100mm; the rotation direction includes around Z 1 The deflection range of the axis is 60° around the Z 2 Axis deflection 60° around Z 3 The deflection range is 60°.

[0046] According to the physical structure of the robot, when setting the independent motion range of each axis, ensure that all axes do not interfere with each other. 0 The axis's range of motion is not 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 8For example, the A series (such as A, A1, A2) area is located in the central area of ​​the motion space, the B series area is located in the front area of ​​the motion space, and the C series area is located in the rear area of ​​the motion space. The 1 series area (such as A1, B1, C1) is located on the left side of the motion space, and the 2 series area (such as A2, B2, C2) is located on the right side of the motion space. The D\E area is similar to the AB area, located at the bottom of the AB area, and F1-F4 is located at the bottom of the motion space.

[0049] 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 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 volume of the area according to the motion characteristics of different areas (each area 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 (earth 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 Indicates that Cθ 3 Cosθ 3 .

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

[0057]

[0058] The precision of the discretization process can be adjusted according to actual needs. 0 , Z 1 The axes are discretized at intervals of 1 mm, and the rotation angles are discretized at intervals of 1°. The accuracy of the final data is √3. The higher the required accuracy, the more data is 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 (d 1 , d 2 ,θ 3 ,θ 4 ,θ 5 ).

[0060] Step 200, the discrete point data set and its corresponding motion parameters are stored in a database to form a pose query table. The construction of the pose query table includes the following steps:

[0061] The discrete point data is generated by the forward calculation program and stored in the pose query table; wherein the forward calculation program traverses and calculates the discrete points of all areas according to different accuracies;

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

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

[0064] The pose query table, i.e. the database, uses a storage method that combines a hash table with a spatial index. 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 type, precision 0.01mm), y (floating point type), z (floating point type)

[0068] Motion parameter field: d 1 (x-axis displacement), d 2 (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 speed up 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[d 1 ]]> <![CDATA[d 2 ]]> <![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 serial number, the second to sixth columns are the motion parameters of the robot, and the seventh to ninth columns represent the position of the generated robot.

[0077] Step 300, according to the coordinates of the target point, find the nearest discrete point in the discrete point data set and obtain its corresponding motion parameters; according to the coordinates of the target point, use an efficient search algorithm to find the nearest discrete point in the discrete point data set to ensure that the query speed meets the real-time requirements; and obtain the corresponding motion parameters, which include the joint angles and displacements of the robot; specifically, according to the coordinates of the target point, use the Euclidean distance nearest neighbor search algorithm to calculate the distance between it and the discrete point, and find the robot parameters and area corresponding to the target point.

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

[0079] Using 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, the queried motion parameters are sent to the robot control system to drive the robot to complete the posture adjustment. Specifically, the motion parameters are sent to the servo motor controller of the robot 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 removing its actual action line, the actual model can be obtained as follows: Figure 2 As shown:

[0086] In this embodiment, the parameter d is set: 1 The moving range is 100mm, d 2 Moving range 100mm, θ 3 Deflection 60°, θ 4 Deflection 60°, θ 5 Deflected 60°, the focus can cover the following range: 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 in 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 names and ranges of the three joints that need to move at the points in each area.

[0088] A area: d 1 , d 2 ,θ 4 Three joint movements;

[0089] A1 area: d 1 ,θ 4 ,θ3 (range [-30,0]);

[0090] A2 area: d 1 ,θ 4 ,θ 3 (range [0,30]).

[0091] Area B: d 2 ,θ 4 ,θ 5 (Range [-30,0]) Three joint movements;

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

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

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

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

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

[0097] D area: d 1 , d 2 ,θ 5 (Range [-30,0]) Three joint movements;

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

[0099] D2 area: d 1 ,θ 3 (range [0,30]),θ 5 (-30,0]) Three joint movements.

[0100] E area: d 1 , d 2 ,θ 5 (range [-30,30]) three joint movements;

[0101] E1 area: d 1 ,θ 3 (range [-30,0]),θ 5 (-30,30]) three joint movements;

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

[0103] F1 area: d 1 (x,50),d 2 (-25,0), θ 3 Range [-30, 0], θ 4 =-30,θ 5 =-30 three joint movements;

[0104] F4 area: d 1 (x,50),d 2 (0,25),θ 3 Range [-30, 0], θ 4 =-30,θ 5 =-30 three joint movements;

[0105] F2 area: d 1 (0,x), d 2 (-25,0), θ 3 Range [-30,30],θ 4 =-30,θ 5 =-30 three joint movements;

[0106] F3 area: d 1 (0,x), d 2 (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 6As shown in the figure, each region has a corresponding mathematical equation and database (pose query table). The mathematical equation of each region can be obtained through the generatrix equation. The attached figure shows a plane diagram, and each region is extended with this plane as the generatrix. For example, if the generatrix is ​​the equation of a circle, then the region is a cylinder. It only needs to define the height range of the cylinder. Therefore, the constraint condition is to define the boundary of the region.

[0109] The boundaries of 19 areas, including Area A, are based on Figure 6 The busbar in is generated, that is, the busbar is the boundary 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=[-d 1 ,d 1 ],d 1 =46.38,x<0;

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

[0112] L3:(x+d 3 +d 2 ) 2 +(z+d 4 ) 2 =r 2 ,d 3 =46.5,d 4 =12.46,z=[-(d 1 +d 4 ),d 5 ],x<0;

[0113] L4:(xd 3 ) 2 +(z+d 4 ) 2 =r 2 ,z=[-(d 1 +d 4 ),d 5 ],x<0;

[0114] L5:(x+d 7 +d 2 ) 2 +(z+d 1 )2 =r 2 ,d 7 =46.5,z=[d 1 -d 4, d 1 ],x=[-(d 2 +d 3 +d 7 ),-(d 2 +d 7 )];

[0115] L6:(x-d 7 ) 2 +(z+d 1 ) 2 =r 2 ,x=[-d 7 ,-(d 7 -d 3 )];z=[d 1 -d 4, d 1 ];

[0116] L7:(x+d 7 +d 2 ) 2 +(z+3d 1 ) 2 =r 2 ,z=[-(d 1 +d 4 ) ,- d 1 ],x=[-(d 2 +d 3 +d 7 ),-(d 2 +d 7 )];

[0117] L8:(x-d 7 ) 2 +(z+3d 1 ) 2 =r 2 ,x=[-d 7 ,-(d 7 -d 3 )];z=[-(d 1 +d 4 ) , -d 1 ];

[0118] L9:(x-d 7 ) 2 +(z+3d 1 ) 2 =r 2,x=[-(d 7 +d 3 ),-d 7 ]; z = [-(d 1 +d 4 ) , -d 1 ];

[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: x 1 2 +z 1 2 =r 2 ,r=92.75,z 1 =[-d 1 ,d 1 ],d 1 =46.38,x<0;y∈[0,100]

[0122] Behind: (x 2 +d 2 ) 2 +z 2 2 =r 2 ,z 2 =[-d 1 ,d 1 ],d 2 =100,x<0;y∈[0,100]

[0123] Constraints: x∈[x 2 , x 1 ]; y∈[0,100]; z∈[-47,47]

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

[0125] Front: (x 1 +d 2 ) 2 +z 1 2 =r 2 ,z 1 =[-d 1 ,d 1 ],d 2 =100,x<0;y∈[0,100];

[0126] Behind: (x 2 +d3 +d 2 ) 2 +(z 2 +d 4 ) 2 =r 2 ,d 3 =46.5,d 4 =12.46,z 2 =[-(d 1 +d 4 ),d 5 ],x<0;y∈[0,100]

[0127] Above: (x 3 +d 7 +d 2 ) 2 +(z 3 +d 1 ) 2 =

[0128] r 2 ,d 7 =46.5,z 3 =[d 1 -d 4, d 1 ],x 3 =[-(d 2 +d 3 +d 7 ),-(d 2 +d 7 )];

[0129] Below: (x 4 +d 7 +d 2 ) 2 +(z 4 +3d 1 ) 2 =

[0130] r 2 ,z 4 =[-(d 1 +d 4 ) ,- d 1 ],x 4 =[-(d 2 +d 3 +d 7 ),-(d 2 +d 7 )];

[0131] Constraints: x∈[x 2 , x 1]; y∈[0,100]; z∈[z 4 , z 3 ]

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

[0133] Front: (x 1 -d 3 ) 2 +(z 1 +d 4 ) 2 =r 2 ,z 1 =[-(d 1 +d 4 ),d 5 ],x 1 < 0; y∈[0,100];

[0134] Back: x 2 2 +z 2 2 =r 2 ,r=92.75,z 2 =[-d 1 ,d 1 ],d 1 =46.38,x 2 < 0; y∈[0,100];

[0135] Above: (x 3 -d 7 ) 2 +(z 3 +d 1 ) 2 =r 2 ,x 3 =[-d 7 ,-(d 7 -d 3 )]; z 3 =[d 1 -d 4, d 1 ];

[0136] Below: (x 4 -d 7 ) 2 +(z 4 +3d 1 ) 2 =r 2 ,x 4 =[-d 7 ,-(d 7 -d 3 )]; z 4=[-(d 1 +d 4 ) , -d 1 ];

[0137] Constraints: x∈[x 2 , x 1 ]; y∈[0,100]; z∈[z 4 , z 3 ]

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

[0139] Front: (xd 7 ) 2 +(z+3d 1 ) 2 =r 2 ,x=[-(d 7 +d 3 ),-d 7 ]; z = [-(d 1 +d 4 ) , -d 1 ]; y∈[0,100];

[0140] Back: (x+d 7 +d 2 ) 2 +(z+3d 1 ) 2 =

[0141] r 2 ,z=[-(d 1 +d 4 ) ,- d 1 ],x=[-(d 2 +d 3 +d 7 ),-(d 2 +d 7 )]; y∈[0,100];

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

[0143] like Fig.11 As shown, the equation of the E region is as follows:

[0144] Above: (xd 7 ) 2 +(z+3d 1 )2 =r 2 ,x=[-(d 7 +d 3 ),-(d 7 -d 3 )]; z = [-(d 1 +d 4 ) , -d 1 ];

[0145] Constraints: x∈[-(d7+d3),-(d7-d3)]; y∈[0,100]; z∈[-(d 1 +d 4 ), z]

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

[0147] Front: x 1 2 +z 1 2 =r 1 2 ; r 1 =94; z 1 ∈[-47,47]; y 1 ∈[-50,50]

[0148] After: x 2 2 +z 2 2 =r 2 2 ; r 2 =194; z 2 ∈[-47,47]; y 2 ∈[-50,50]

[0149] Left: x 3 2 +z 3 2 =r 3 2 ; r 3 =94; z 3 ∈[-47,47]; y 3 ∈[-50,50]

[0150] Right: x 4 2 +z 4 2 =r 4 2 ; r 4 =94; z 4∈[-47,47]; y 4 ∈[-50,50]

[0151] Upper: x 5 ∈[-181.4,-81.4]; y 5 ∈[-50,50]; z 5 =47

[0152] Next: x 6 ∈[-181.4,-81.4]; y 6 ∈[-50,50]; z 6 =-47

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

[0154] Front: x 1 2 +z 1 2 =r 1 2 ; r 1 =94; z 1 ∈[-47,47]; y 1 ∈[-50,50]

[0155] After: x 2 2 +z 2 2 =r 2 2 ; r 2 =194; z 2 ∈[-47,47]; y 2 ∈[-50,50]

[0156] Left: x 3 2 +z 3 2 =r 3 2 ; r 3 =94; z 3 ∈[-47,47]; y 3 ∈[-50,50]

[0157] Right: x 4 2 +z 4 2 =r 4 2 ; r 4 =94; z 4 ∈[-47,47]; y 4∈[-50,50]

[0158] Upper: x 5 ∈[-181.4,-81.4]; y 5 ∈[-50,50]; z 5 =47

[0159] Next: x 6 ∈[-181.4,-81.4]; y 6 ∈[-50,50]; z 6 =-47

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

[0161] Front: x 1 2 +z 1 2 =r 1 2 ; r 1 =94; z 1 ∈[-47,47]; y 1 ∈[-50,50]

[0162] After: x 2 2 +z 2 2 =r 2 2 ; r 2 =194; z 2 ∈[-47,47]; y 2 ∈[-50,50]

[0163] Left: x 3 2 +z 3 2 =r 3 2 ; r 3 =94; z 3 ∈[-47,47]; y 3 ∈[-50,50]

[0164] Right: x 4 2 +z 4 2 =r 4 2 ; r 4 =94; z 4 ∈[-47,47]; y 4 ∈[-50,50]

[0165] Upper: x 5 ∈[-181.4,-81.4]; y 5 ∈[-50,50]; z 5 =47

[0166] Next: x 6 ∈[-181.4,-81.4]; y 6 ∈[-50,50]; z 6 =-47

[0167] The feasibility of the invented method will be verified 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 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 the smallest, obtain the robot parameters corresponding to the data.

[0171] (2) Motion parameter acquisition

[0172] Scenario: During magnetic resonance imaging 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: Through KD tree search, the discrete point Q (52.31, 33.83, 21.44) is matched, and the distance D = 0.12mm. Obtain motion parameters: θ3 = 15.5°, θ4 = 0.2°, d1 = 24.5mm.

[0174] (3) Motion control

[0175] The command J1=15.5, J2=0.2, D1=24.5 was sent to the control system. The robot arm completed the posture adjustment within 120ms, and the measured position error was 0.08mm, which met the surgical requirements.

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

[0177] 1. By discretizing the motion space, the traditional solution method that relies on the robot's inverse operation 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 different application purposes, and 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, it can be migrated to other equipment such as industrial robots to reduce the amount of calculation, save time and improve efficiency.

[0181] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined 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 discrete point data set and its corresponding motion parameters are stored in a 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 point position.

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

3. A five-degree-of-freedom robot motion control method as claimed in claim 1, characterized in that: 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 constraints.

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

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

6. A five-degree-of-freedom robot motion control method as claimed in 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, which include the joint angles and displacements of the robot.

7. A five-degree-of-freedom robot motion control method as claimed in claim 6, 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 point until the best discrete point is matched.

8. 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.

9. A five-degree-of-freedom robot motion control method as claimed in claim 1, characterized in that: The motion of the five-degree-of-freedom robot is controlled by table lookup.

10. A five-degree-of-freedom robot motion control method as claimed in claim 1, characterized in that: 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, for any point in the motion space, it only needs to move 3 degrees of freedom from the original state to reach the target point.

Images