Mechanical arm collision detection method
Through the main component analysis of the robotic arm visual model construction and the three-dimensional structured optical camera, the problem of robotic arm motion limitations and traditional collision detection resources is solved, and effective collision detection and motion path optimization of various parts of the robotic arm is achieved, improving the The efficiency and accuracy of the surgery.
Patent Information
- Application Number
- CN202411164079.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-22
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-08-22
AI Technical Summary
In the prior art, the pure autonomous movement of the robot arm is limited to a small space range, the control process is cumbersome, which increases the operating burden, and may affect the efficiency and accuracy of the surgical efficiency and accuracy. The traditional collision detection method consumes a lot of computing resources and the accuracy depends on the sensor manufacturing accuracy, which ignores the The collision risk of each axle of the robotic arm body.
Through the robotic arm visual model construction, equipment registration, object detection and collision detection steps, a three-dimensional structured optical camera and principal component analysis are used to construct a minimum enclosure box for collision detection, and the inverse kinematic analysis is used to determine whether the robotic arm's motion path has occurred. collision.
Effectively detect collisions between various parts of the robotic arm, reduce the possibility of damage to patients by unexpected movements, optimize the movement path, improve surgical efficiency and accuracy, and reduce operating burden.
Smart Images

Figure CN118952202A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of path control, and in particular to a robot arm collision detection method. Background Art
[0002] In modern medical surgery and industrial control, the image navigation technology of multi-degree-of-freedom robotic arms has been widely used. After the doctor completes the surgical planning on the patient's image, the robotic arm carrying the surgical tools moves accurately to the corresponding position of the real patient and assists the doctor in completing the subsequent surgical operations. To ensure safety, any collision with the patient or other surgical instruments must be avoided during the movement of the robotic arm. Through sophisticated control algorithms and real-time monitoring systems, the robotic arm can intelligently adjust its path to ensure the smooth progress of the operation and the safety of the patient.
[0003] Disadvantages of the existing technology: To avoid collisions when the robot arm moves over a large range, when the current position of the robot arm's execution end is far from the target position, the user often needs to manually drag the robot arm to the vicinity of the target position, and then the robot arm will autonomously fine-tune its posture to make the execution end reach the target position accurately. This means that the purely autonomous movement of the robot arm is limited to a very small space, and the entire motion control process is relatively cumbersome, which increases the doctor's operating burden and may also affect the efficiency and accuracy of the operation.
[0004] CN112179602A uses octree segmentation technology to segment the space, uses ball bounding boxes and directional bounding boxes to hierarchically wrap the obtained octree space to obtain a mixed bounding box tree. Use depth-first search algorithm and bidirectional breadth-first search algorithm to perform collision detection of a single configuration of the robot. Disadvantages: Hierarchical wrapping of the octree space with bounding boxes is computationally intensive, especially in large spaces or complex configurations, which increases the complexity of the algorithm; when dealing with certain special cases, depth-first search may fall into a long recursive call, while breadth-first search may need to maintain a very large search queue, which may result in significant consumption of computing resources and time delays.
[0005] The traditional robot arm collision detection determines whether a collision occurs when the robot moves to or approaches the current position, and it is generally a sensor-based collision detection method. For example, CN117323017A constructs a mathematical model of the end contact force signal, and extracts the collision change point features in the contact processing signal through the contact force matrix to realize the collision detection method for force signal processing. Disadvantages: The traditional robot arm collision detection method will increase the probability of direct collision between the patient and the robot arm. For the robot arm collision detection method based on sensor detection, the installation accuracy of the sensor is heavily dependent on the manufacturing and assembly accuracy of the robot and sensor, which not only affects the robot's collision detection accuracy, but also the cost of the sensor is very expensive.
[0006] CN116473678A introduces a virtual wall to guide the end effector to move, the purpose is to prevent the end of the robot arm from moving toward the wall, so as to achieve the effect of end effector collision detection. Disadvantages: Each axis of the multi-degree-of-freedom robot arm will move during the movement, and each axis has a collision risk, but the conventional collision detection focuses on the end of the robot arm and the surgical instruments it carries, ignoring the detection of the various axis parts of the robot arm body. Summary of the invention
[0007] The present invention proposes a robot arm collision detection method, which solves the problem that the purely autonomous movement of the robot arm in the prior art is limited to a very small space, the whole set of motion control process is relatively cumbersome, which increases the user's operating burden and may also affect the efficiency and accuracy of the robot arm control. The technical solution of the present invention is implemented as follows: A robot arm collision detection method comprises the following steps:
[0008] Step S1: Building a visual model of the robotic arm;
[0009] Obtain the three-dimensional model of each sub-part of the robot arm and establish a local coordinate system on each model;
[0010] Step S2: device registration;
[0011] A 3D structured light camera is installed at the end of the robot arm to capture the precise relative position relationship between the working area and other equipment and the robot arm, and then complete the registration of other equipment, the hand-eye calibration of the robot arm, and the image registration of the image in sequence; specifically, it includes:
[0012] The intrinsic base coordinate system of the robot is denoted as F base , the intrinsic TCP coordinate system at the end of the robot is denoted as F flange , the reference array coordinate system is denoted as F object , the three-dimensional structured light camera coordinate system is recorded as F camera .
[0013] F object With F base Keep relatively still during the entire calibration process;
[0014] The intrinsic TCP coordinate system at the end of the robot arm is rigidly connected to the camera coordinate system, F flange To F camera The transformation relationship is the unknown constant to be solved;
[0015] Get F from the robot system base To F flange The transformation relationship T BaseToFlange ;
[0016] Get F from the camera system camera To F objectThe transformation relationship T CameraToObject ;
[0017] Step S3: object detection;
[0018] Based on the surgical registration in step S2, the hand-eye calibration of the robot arm is completed, and the coordinate system F flange To the three-dimensional structured light camera coordinate system F camera The transformation relationship T FlangeToCamera , through the results of hand-eye calibration, combined with the data sent back by the robot itself T BaseToFlange With T FlangeToCamera Two transformation relationships are used to map the object data scanned by the 3D structured light camera to the robot base coordinate system F base Down;
[0019] In the process of building the visual model of the robotic arm, the intrinsic base coordinate system F base and the image coordinate system F of the virtual space image Overlap, mapping the three-dimensional data scanned by the structured light camera into the image coordinate system, and then completing the mapping of the instruments and surgical area in the surgical space in the image coordinate system;
[0020] Step S4: collision detection;
[0021] Plan the target position of the end device of the robot arm, and calculate the angle change of each joint axis during the movement of the robot arm to the target position through inverse kinematics analysis. When the robot arm is in a certain axis angle combination in the motion path, the following method is used to determine whether the robot arm will collide, so as to decide whether to adopt the motion path: 1) Using the method in step S1, update the posture of the robot model at the specified axis angle combination, and merge the sub-part models of the robot at this time into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as robot; 2) Use a 3D structured light camera to scan the patient's surgical surface and other surgical instruments, and record the surface data model of the patient's surgical site. , each line represents a point, which is merged into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as obstacle; 3) Decentralize surface data: , where P is the surface data model of the patient's surgical site, μ is the mean vector of the surface data; 4) Calculate the covariance matrix , where N is the number of surface data model points of the patient’s surgical site; 5) Use principal component analysis to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues and corresponding eigenvalue vectors: , Λ is a diagonal matrix, the elements on its diagonal are eigenvalues, V is an eigenvector matrix, and its columns are eigenvectors; 6) According to the results of PCA calculation, determine The minimum bounding box Oriented BoundingBox is denoted as OBB, and the center of OBB is the mean vector μ of the surface data; 7) Calculate the projection of each point in the main direction , i is the number of the point, j is the number of the eigenvector; calculate the minimum and maximum values in each main direction
[0022] Where n is the number of points; 8) Construct the eight vertices of OBB, recorded as: ,
[0023] 9) Through image registration, the conversion relationship between the camera coordinate system and the image coordinate system can be obtained , the OBB generated by the above PCA method is transformed into , get the coordinates of the eight vertices in the image coordinate system; let the OBB formed by these eight vertices be , let its voxel size be a; calculate Dimensions in the three coordinate axes:
[0024] ,
[0025] Among them, ceil means rounding up; The value of each voxel point is set to 0, and a copy is recorded as ; 10) For For each voxel point of , if the coordinate value of the voxel point falls on , set its value to 1, otherwise keep its value 0; 11) For For each voxel point, if the coordinate value of the voxel point falls inside the robot, its value is set to 1, otherwise it is kept as 0; 12) Yes and , perform multiplication, that is, for For each voxel in The grayscale value of the voxel at the corresponding position in the multiplication operation is recorded as ; 13) Traversal If there is a voxel with a grayscale value of 1, it means that the robot has collided in this posture. If all voxel grayscale values are 0, it means that the robot has not collided in this posture.
[0026] As a preferred technical solution, the specific steps of step 10) are as follows: a) Use obstacle to initialize a VTK stencil tool vtkPolyDataToImageStencil, named obstacleStencil; b) Use the vtkImageStencil filter, use it as the filter input, use obstacleStencil as the template tool used by the filter, use the ReverseStencilOn() method to set the filter mode to reverse template extraction, and use the SetBackgroundValue() method to set the background voxel value to 1; c) Assign the output of the filter to .
[0027] As a preferred technical solution, the specific steps of step 11) are as follows: d) Use robot to initialize a VTK template tool vtkPolyDataToImageStencil, named robotStencil; e) Use the vtkImageStencil filter to As the input of the filter, robotStencil is used as the template tool used by the filter. The filter mode is set to reverse template extraction using the ReverseStencilOn() method, and the background voxel value is set to 1 using the SetBackgroundValue() method; f) Assign the output of the filter to .
[0028] As a preferred technical solution, step S11 specifically includes the following steps: Step 1): Obtain the three-dimensional model of each sub-part of the robot arm, establish a local coordinate system on each model, ensure that the origin of each local coordinate system is located at the center of the rotation axis plane, and the z-axis direction is the same as the rotation axis direction; Step 2) According to the hardware parameters of each sub-component of the robot arm, the DH parameter table can be written; Step 3) For a robot arm posture with known axis angles, calculate the relative position relationship between each sub-part of the robot arm and the base, and move the model accordingly to achieve visual simulation: Combined with the DH parameter table, the relative transformation matrix of each sub-part of the robot arm is obtained.
[0029] As a preferred technical solution, the hand-eye calibration step of the robot arm eye on the hand includes: Step 1) By controlling the end of the robotic arm to move to {P1, P2, P3, P4} positions, the three-dimensional structured light camera coordinate system F is obtained when the robotic arm is in different positions. camera To reference array coordinate system F object The transformation matrix T CameraToObject , and the robot base coordinate system F base To the end coordinate system F of the robot arm flange The transformation matrix T BaseToFlange , recorded as T BaseToFlange1 、T CameraToObject1 , T BaseToFlange2 、T CameraToObject2 , T BaseToFlange3 、T CameraToObject3 and T BaseToFlange4 、T CameraToObject4 ; Step 2) Construct multiple sets of AX=XB equations based on the coordinate system transformation of the robot arm, 3D structured light camera, and reference array. A and B are both known matrices that can be solved, so the whole problem is simplified to solving A. i X=XB i The equation of .
[0030] Step 3) Solve the robot end coordinate system F flange To the 3D structured light camera coordinate system F camera The transformation matrix T FlangeToCamera .
[0031] Compared with the prior art, this solution has the following beneficial effects: 1) Effectively detect whether the various parts of the robotic arm collide with each other, and reduce the possibility of injury to the patient caused by unexpected movement of the robotic arm as a whole; 2) Combined with the visual simulation of the robot arm's movement, it helps the operator to understand the robot arm's movement path in advance and optimize the placement of related equipment on this basis. BRIEF DESCRIPTION OF THE DRAWINGS
[0032] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0033] Figure 1 It is a structural schematic diagram of the mechanical arm of the present invention; Figure 2 is a three-dimensional schematic diagram of the robot arm of the present invention; Figure 3 Schematic diagram of each local coordinate system of the robot arm of the present invention; Figure 4 A visualization model of the robot arm of the present invention; Figure 5 It is a schematic diagram of hand-eye calibration of the robot arm of the present invention; Figure 6 The mapping of the instruments and the surgical area in the surgical space of the present invention in the image coordinate system; Figure 7 It is the surface data model of the surgical site of the patient according to the present invention. DETAILED DESCRIPTION
[0034] The technical solution of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0035] This embodiment discloses a robot arm collision detection method, which mainly includes the steps of building a robot arm visualization model, device registration, object detection and collision detection. Specifically, the application of this method in the field of surgery is used as an example to explain in detail. Specifically, the following steps are included:
[0036] I. Construction of a visual model of a robotic arm (taking a 7-axis SRS robotic arm as an example)
[0037] The names of the various parts of the robot arm are as follows Figure 1 As shown in the figure, the position and direction of each motion axis of the robot arm are Figure 1 Arrow logo in .
[0038] 1. Obtain the 3D models of each sub-part of the robot arm, establish a local coordinate system on each model, ensure that the origin of each local coordinate system is located at the center of the rotation axis plane, and the z-axis direction is the same as the rotation axis direction. The names of each local coordinate system are as follows: Figure 2 and 3 shown.
[0039] 2. According to the hardware parameters of each sub-component of the robot arm, the following DH parameter table can be written:
[0041] in, .
[0042] 3. For a robot arm with known angles of each axis, the angle of the nth axis is , calculate the relative position relationship between each sub-part of the robot and the base through the following steps, and move the model accordingly to achieve visual simulation: 3.1 By combining the DH parameter table with the following formula, we can get the relative transformation matrix of each sub-part of the robot arm:
[0043]
[0044] 3.2 Calculate the transformation matrix of each sub-part 1-8, and record the transformation matrix of sub-part n as , then:
[0045] Transformation matrix from subpart 1 to 2;
[0046]
[0047] Transformation matrix from subsection 2 to 3
[0048]
[0049] Transformation matrix from subsection 3 to 4
[0050]
[0051] Transformation matrix from subsection 4 to 5
[0052]
[0053] Transformation matrix from subsection 5 to 6
[0054]
[0055] Transformation matrix for subsections 6 to 7
[0056]
[0057] Transformation matrix from subsection 7 to 8
[0058]
[0059] 3.3 The transformation matrices of subcomponents 1 to 8 can be obtained by multiplying the transformation matrices of each subcomponent
[0060] Transformation matrix for subsections 1 to 8:
[0061]
[0062] For example, when = 65°, = 60°, = 70°, = 60°, = -80°, = -120°, = -20°, perform the above operation and you can get Figure 4 Visualization model of the robotic arm shown.
[0063] II. Surgical Registration
[0064] Build a system as shown in the figure below, and install a 3D structured light camera at the end of the robotic arm. Complete the instrument registration, robotic arm hand-eye calibration, and image registration in sequence. The structured light camera will be used to capture the precise relative position relationship between the surgical area of the patient in the operating room and other surgical instruments and the robotic arm.
[0065] As shown in the figure below, the intrinsic base coordinate system of the robot is denoted as F base , the intrinsic TCP coordinate system (end flange coordinate system) of the robot end is denoted as F flange , the reference array coordinate system is denoted as F object , the three-dimensional structured light camera coordinate system is recorded as F camera .
[0066] F object With F base Keep relatively still during the calibration process
[0067] The intrinsic TCP coordinate system at the end of the robot arm is rigidly connected to the camera coordinate system, F flange To F camera The transformation relationship is the unknown constant to be solved.
[0068] F base To F flange The transformation relationship is T BaseToFlange , which can be obtained directly from the robotic arm system.
[0069] F camera To F object The transformation relationship is T CameraToObject , which can be obtained directly from the camera system.
[0070] The steps of hand-eye calibration of the robot arm eye on the hand are described as follows: 1) By controlling the end of the robotic arm to move to {P1, P2, P3, P4} positions, the three-dimensional structured light camera coordinate system F is obtained when the robotic arm has different positions. camera To reference array coordinate system F object The transformation matrix T CameraToObject (can be obtained directly from the robot system), and the robot base coordinate system F base To the end coordinate system F of the robot arm flange The transformation matrix T BaseToFlange(can be obtained directly from the camera system), recorded as T BaseToFlange1 、T CameraToObject1 , T BaseToFlange2 、T CameraToObject2 , T BaseToFlange3 、T CameraToObject3 and T BaseToFlange4 、T CameraToObject4 ; 2) Construct multiple sets of AX=XB equations based on the coordinate system transformation of the robot arm, 3D structured light camera and reference array. A and B are both known matrices that can be solved, so the whole problem is simplified to solving A i X=XB i The equation of .
[0071] 3) The coordinate system F of the end of the robot arm can be solved flange To the 3D structured light camera coordinate system F camera The transformation matrix T FlangeToCamera .
[0072] ⅢObject Detection
[0073] Based on the previous surgical registration, the hand-eye calibration of the robot arm was completed, and the coordinate system F flange To the three-dimensional structured light camera coordinate system F camera The transformation relationship T FlangeToCamera , through the results of hand-eye calibration, combined with the data sent back by the robot itself T BaseToFlange With T FlangeToCamera Two transformation relationships can map the object data scanned by the 3D structured light camera to the robot base coordinate system F base Down.
[0074] In the process of building the visual model of the robotic arm, the intrinsic base coordinate system F base and the image coordinate system F of the virtual space image By coinciding, the 3D data scanned by the structured light camera can be mapped into the image coordinate system. In this way, we have completed the mapping of the instruments and surgical area in the surgical space in the image coordinate system, such as Figure 6 shown.
[0075] In this way, we achieve the conversion and mapping of three-dimensional data between different coordinate systems, providing solid technical support for navigation and assistance during surgery.
[0076] Ⅳ Collision Detection
[0077] During the operation, the target position of the end-of-arm instrument is planned according to the actual condition of the patient. The system will calculate the angle change of each joint axis during the movement of the robot arm to the target position through inverse kinematics analysis. When the robot arm is in a certain axis angle combination in the motion path, the following method is used to determine whether the robot arm will collide, so as to decide whether to adopt the motion path: 1) Using the method in step S1, update the posture of the robot model at the specified axis angle combination, and merge the sub-part models of the robot at this time into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as robot; 2) Use a 3D structured light camera to scan the patient's surgical surface and other surgical instruments, and record the surface data model of the patient's surgical site. , each line represents a point, which is merged into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as obstacle; 3) Decentralize surface data: , where P is the surface data model of the patient's surgical site, μ is the mean vector of the surface data; 4) Calculate the covariance matrix , where N is the number of surface data model points of the patient’s surgical site; 5) Use principal component analysis to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues and corresponding eigenvalue vectors: , Λ is a diagonal matrix, the elements on its diagonal are eigenvalues, V is an eigenvector matrix, and its columns are eigenvectors; 6) According to the results of PCA calculation, determine The minimum bounding box Oriented BoundingBox is denoted as OBB, and the center of OBB is the mean vector μ of the surface data; 7) Calculate the projection of each point in the main direction , i is the number of the point, j is the number of the eigenvector; calculate the minimum and maximum values in each main direction
[0078] Where n is the number of points; 8) Construct the eight vertices of OBB, recorded as: ,
[0079] 9) Through image registration, the conversion relationship between the camera coordinate system and the image coordinate system can be obtained , the OBB generated by the above PCA method is transformed into , get the coordinates of the eight vertices in the image coordinate system; let the OBB formed by these eight vertices be , let its voxel size be a; calculate Dimensions in the three coordinate axes:
[0080] ,
[0081] Among them, ceil means rounding up; The value of each voxel point is set to 0, and a copy is recorded as ;
[0082] 10) For For each voxel point, if the coordinate value of the voxel point falls on, set its value to 1, otherwise keep its value 0; Step 10) The specific steps are as follows: a) Use obstacle to initialize a VTK stencil tool vtkPolyDataToImageStencil, named obstacleStencil; b) Use the vtkImageStencil filter, use it as the filter input, use obstacleStencil as the template tool used by the filter, use the ReverseStencilOn() method to set the filter mode to reverse template extraction, and use the SetBackgroundValue() method to set the background voxel value to 1; c) Assign the output of the filter to .
[0083] As a preferred technical solution, the specific steps of step 11) are as follows: d) Use robot to initialize a VTK template tool vtkPolyDataToImageStencil, named robotStencil; e) Use the vtkImageStencil filter to As the input of the filter, robotStencil is used as the template tool used by the filter. The filter mode is set to reverse template extraction using the ReverseStencilOn() method, and the background voxel value is set to 1 using the SetBackgroundValue() method; f) Assign the output of the filter to .
[0084] 11) For For each voxel point, if the coordinate value of the voxel point falls inside the robot, its value is set to 1, otherwise it is kept as 0; specifically: Step 1): Obtain the three-dimensional model of each sub-part of the robot arm, establish a local coordinate system on each model, ensure that the origin of each local coordinate system is located at the center of the rotation axis plane, and the z-axis direction is the same as the rotation axis direction; Step 2) According to the hardware parameters of each sub-component of the robot arm, the DH parameter table can be written; Step 3) For a robot arm posture with known axis angles, calculate the relative position relationship between each sub-part of the robot arm and the base, and move the model accordingly to achieve visual simulation: Combined with the DH parameter table, the relative transformation matrix of each sub-part of the robot arm is obtained.
[0085] 12) Yes and , perform multiplication, that is, for For each voxel in The grayscale value of the voxel at the corresponding position in the multiplication operation is recorded as ;
[0086] 13) Traversal If there is a voxel with a grayscale value of 1, it means that the robot has collided in this posture. If all voxel grayscale values are 0, it means that the robot has not collided in this posture.
[0087] The above method can effectively detect whether the various parts of the robotic arm collide, reduce the possibility of injury to the patient caused by unexpected movement of the robotic arm as a whole, and combine with the visual simulation of the robotic arm's movement to help the surgical operator understand the movement path of the robotic arm in advance, and on this basis optimize the placement of instruments in the operating room.
[0088] Of course, the above technology is also applicable to the fields of industrial production and scientific experiment technology that require precise positioning of the robotic arm.
[0089] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims
1. A robot arm collision detection method, characterized in that: The following steps are involved: Step S1: Building a visual model of the robotic arm; Obtain the three-dimensional model of each sub-part of the robot arm and establish a local coordinate system on each model; Step S2: device registration; A 3D structured light camera is installed at the end of the robot arm to capture the precise relative position relationship between the working area and other equipment and the robot arm, and then complete the registration of other equipment, the hand-eye calibration of the robot arm, and the image registration of the image in sequence; specifically, it includes: The intrinsic base coordinate system of the robot is denoted as F base , the intrinsic TCP coordinate system at the end of the robot is denoted as F flange , the reference array coordinate system is denoted as F object , the three-dimensional structured light camera coordinate system is recorded as F camera; F object With F base Keep relatively still during the entire calibration process; The intrinsic TCP coordinate system at the end of the robot arm is rigidly connected to the camera coordinate system, F flange To F camera The transformation relationship is the unknown constant to be solved; Get F from the robot system base To F flange The transformation relationship T BaseToFlange ; Get F from the camera system camera To F object The transformation relationship T CameraToObject ; Step S3: object detection; Based on the surgical registration in step S2, the hand-eye calibration of the robot arm is completed, and the coordinate system F flange To the three-dimensional structured light camera coordinate system F camera The transformation relationship T FlangeToCamera , through the results of hand-eye calibration, combined with the data sent back by the robot itself T BaseToFlange With T FlangeToCamera Two transformation relationships are used to map the object data scanned by the 3D structured light camera to the robot base coordinate system F base Down; In the process of building the visual model of the robotic arm, the intrinsic base coordinate system F base and the image coordinate system F of the virtual space image Overlap, mapping the three-dimensional data scanned by the structured light camera into the image coordinate system, and then completing the mapping of the instruments and surgical area in the surgical space in the image coordinate system; Step S4: setting collision detection; Plan the target position of the end device of the robot arm, and calculate the angle change of each joint axis during the movement of the robot arm to the target position through inverse kinematics analysis. When the robot arm is in a certain axis angle combination in the motion path, the following method is used to determine whether the robot arm will collide, so as to decide whether to adopt the motion path: 1) Using the method in step S1, update the posture of the robot model at the specified axis angle combination, and merge the sub-part models of the robot at this time into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as robot; 2) Use a 3D structured light camera to scan the patient's surgical surface and other surgical instruments, and record the surface data model of the patient's surgical site. , each line represents a point, which is merged into a vtkPolyData surface data using the vtkAppendPolyData method, recorded as obstacle; 3) Decentralize surface data: , where P is the surface data model of the patient's surgical site, μ is the mean vector of the surface data; 4) Calculate the covariance matrix , where N is the number of surface data model points of the patient’s surgical site; 5) Use principal component analysis to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues and corresponding eigenvalue vectors: , Λ is a diagonal matrix, the elements on its diagonal are eigenvalues, V is an eigenvector matrix, and its columns are eigenvectors; 6) According to the results of PCA calculation, determine The minimum bounding box Oriented Bounding Box is denoted as OBB, and the center of OBB is the mean vector μ of the surface data; 7) Calculate the projection of each point in the main direction , i is the number of the point, j is the number of the eigenvector; calculate the minimum and maximum values in each main direction Where n is the number of points; 8) Construct the eight vertices of OBB, recorded as: , ; 9) Through image registration, the conversion relationship between the camera coordinate system and the image coordinate system can be obtained , the OBB generated by the above PCA method is transformed into , get the coordinates of the eight vertices in the image coordinate system; let the OBB formed by these eight vertices be , let its voxel size be a; calculate Dimensions in the three coordinate axes: , Among them, ceil means rounding up; The value of each voxel point is set to 0, and a copy is recorded as ; 10) For For each voxel point of , if the coordinate value of the voxel point falls on , set its value to 1, otherwise keep its value 0; 11) For For each voxel point, if the coordinate value of the voxel point falls inside the robot, its value is set to 1, otherwise it is kept as 0; 12) Yes and , perform multiplication, that is, for For each voxel in The grayscale value of the voxel at the corresponding position in the multiplication operation is recorded as ; 13) Traversal If there is a voxel with a grayscale value of 1, it means that the robot has collided in this posture. If all voxel grayscale values are 0, it means that the robot has not collided in this posture.
2. A robot arm collision detection method as claimed in claim 1, characterized in that: The specific steps of step 10) are as follows: a) Use obstacle to initialize a VTK stencil tool vtkPolyDataToImageStencil, named obstacleStencil; b) Use the vtkImageStencil filter, use it as the filter input, use obstacleStencil as the template tool used by the filter, use the ReverseStencilOn() method to set the filter mode to reverse template extraction, and use the SetBackgroundValue() method to set the background voxel value to 1; c) Assign the output of the filter to .
3. A robot arm collision detection method as claimed in claim 1, characterized in that: The specific steps of step 11) are as follows: d) Use robot to initialize a VTK stencil tool vtkPolyDataToImageStencil, named robotStencil; e) Use the vtkImageStencil filter to As the input of the filter, robotStencil is used as the template tool used by the filter. The filter mode is set to reverse template extraction using the ReverseStencilOn() method, and the background voxel value is set to 1 using the SetBackgroundValue() method. f) Assign the output of the filter to .
4. A robot arm collision detection method as claimed in claim 1, characterized in that: The step S1 specifically includes the following steps: Step 1): Obtain the three-dimensional model of each sub-part of the robot arm, establish a local coordinate system on each model, ensure that the origin of each local coordinate system is located at the center of the rotation axis plane, and the z-axis direction is the same as the rotation axis direction; Step 2) According to the hardware parameters of each sub-component of the robot arm, the following DH parameter table can be written; Step 3) For a robot arm posture with known axis angles, calculate the relative position relationship between each sub-part of the robot arm and the base, and move the model accordingly to achieve visual simulation: Combined with the DH parameter table, the relative transformation matrix of each sub-part of the robot arm is obtained.
5. A robot arm collision detection method as claimed in claim 1, characterized in that: The steps of hand-eye calibration of the robot arm eye on the hand include: Step 1) By controlling the end of the robotic arm to move to {P1, P2, P3, P4} positions, the three-dimensional structured light camera coordinate system F is obtained when the robotic arm is in different positions. camera To reference array coordinate system F object The transformation matrix T CameraToObject , and the robot base coordinate system F base To the end coordinate system F of the robot arm flange The transformation matrix T BaseToFlange , recorded as T BaseToFlange1 , T CameraToObject1 , T BaseToFlange2 , T CameraToObject2 , T BaseToFlange3 , T CameraToObject3 and T BaseToFlange4 , T CameraToObject4 ; Step 2) Construct multiple sets of AX=XB equations based on the coordinate system transformation of the robot arm, 3D structured light camera, and reference array. A and B are both known matrices that can be solved, so the whole problem is simplified to solving A. i X=XB i The equation of Step 3) Solve the robot end coordinate system F flange To the 3D structured light camera coordinate system F camera The transformation matrix T FlangeToCamera .
Citation Information
Patent Citations
Autonomous path planning method of live-line working robot based on point cloud
CN110587600A
Point cloud collision detection method applied to robot grabbing scene
CN112060087A
Method and system for predicting collision-free pose of motion system
CN115330011A
Mechanical arm collision detection system and method, mechanical arm, robot and chip
CN115351818A
Mechanical arm collision detection method and device, electronic equipment and storage medium
CN117598783A
