Mechanical arm optical positioning method and device

Abstract

The present application relates to a kind of mechanical arm optical positioning method and device.The method and device mainly include closed loop matrix calculation and mechanical arm DH parameter identification correction part.First part closed loop matrix calculation is to obtain the rotation and translation relationship of marker and mechanical arm end coordinate system, mechanical arm base coordinate system and infrared binocular device coordinate system;Second part is corrected by the conversion matrix obtained from the first part, using the method based on dual quaternion to carry out mechanical arm DH parameter, update the internal parameter of the above conversion matrix.In the present application, mechanical arm data is automatically measured, compared with manual, and higher precision is more stable.At the same time, the present application only needs to run a calibration program to quickly complete corresponding position calibration, and the efficiency is higher, reduces the influence of optical equipment movement on mechanical arm calibration.

Description

Technical Field

[0001] This invention relates to an optical positioning method and apparatus for a robotic arm. Background Technology

[0002] Currently, ultrasound-guided percutaneous biopsy is an important diagnostic method. This procedure currently relies on the surgeon manually inserting the needle, followed by confirmation by medical ultrasound imaging. This results in low surgical precision, high risk, long procedure time, and a high likelihood of complications. To address these issues, robotic-assisted needle insertion has been researched for many years. The key technology lies in solving the high-precision positioning problem among multiple systems, including the robot, puncture instruments, medical imaging equipment, and external measuring devices, to ensure the positional and orientation accuracy of the puncture needle.

[0003] A similar approach has been proposed in a paper titled "Research on Optical Registration Method for Thoracentesis Surgical Robots," see [link to paper]. Figure 1 The marker is mounted on the needle holder at the end of the robotic arm. The marker has four small balls coated with an infrared reflective coating that can strongly reflect the infrared light emitted by the infrared binocular device, so that the position and attitude information of the marker can be stably tracked by the infrared device.

[0004] For the registration of the marker at the end of the robotic arm with the puncture needle, this scheme uses a rotation method to register the needle tip and the marker, such as... Figure 2 The specific method is as follows: keep the needle tip position at a fixed point, and then rotate the end effector, that is, use the needle tip as the calibration point, keep the needle tip position fixed under different postures of the puncture needle, and find the translational relationship between the marker and the needle tip.

[0005] The rotational relationship can be solved by actively measuring the guide tube of the end effector. Since the needle direction is the same as the guide tube direction, once the guide tube direction is measured, the needle direction can also be solved, such as... Figure 3 The end of the active probe tool is a sphere that can fit perfectly into the inlet A and outlet B of the guide tube. The binocular positioning device can directly obtain the position of the end of the active probe. By placing the end at the inlet and outlet, the position of the catheter inlet and outlet can be determined, thereby further determining the direction of the surgical needle.

[0006] Another Chinese patent publication, CN111956329A, entitled "A Calibration Method, System, Terminal, and Storage Medium for a Dual-Arm Robot," discloses a calibration method and system for a dual-arm robot. This method involves designing and manufacturing a high-precision center, which is then mounted on the end flange of either arm of the dual-arm robot. The robot arm controls the center to reach a preset reference point, and the position of the reference point within the robot's base coordinate system is calculated. The calculation formula is as follows:

[0007] R PRtip = R T ER · ER P Mtip

[0008] In the formula ER P Mtip The coordinates from the tip to the end flange can be obtained from a preset tip reference number; RTER represents the pose of the end flange in the robot's base coordinate system, which can be directly read from the robot's operating system. Remove the tip and install the puncture tube end effector onto any flange of the dual-arm robot. Control the puncture needle to reach the preset reference point via the robotic arm, and record the pose matrix of that end flange in the robot coordinate system. Establish the expression:

[0009] ER P Ntip =inv( R T ER )· R P Rtip

[0010] ER P Ntip The coordinates of the puncture needle in the coordinate system of the distal flange can be obtained by inverting the above equation. See [reference]. Figure 4 For attitude acquisition, the puncture needle can be moved to any point, which is set as the origin of the needle tip coordinate system, and its coordinates in the robot's base coordinate system can be calculated. The robotic arm can then control the needle tip to reach any second point, which is defined as a point on the Y-axis, and the Y-axis direction vector can be calculated. The Z-axis direction vector can then be calculated in the same way. The X-axis direction vector is obtained by cross product of these two vectors, and the X, Y, and Z-axis direction vectors are combined sequentially to obtain the puncture needle attitude matrix.

[0011] In summary, in existing technologies, when the needle tip touches an object, the contact force causes the needle to deform, and the repetitive movements of the robotic arm have a certain degree of error due to its rated DH parameter. Manual operation also introduces certain errors. Summary of the Invention

[0012] This invention provides a method and apparatus for optical positioning of a robotic arm, which at least solves the technical problem of errors caused by needle tip deformation during high-precision positioning of existing guided robots.

[0013] According to an embodiment of the present invention, an optical positioning method for a robotic arm is provided, comprising the following steps:

[0014] Multiple initial points are set based on the base coordinates and end-effector coordinates of the robotic arm. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and the end-effector coordinate system. The displacement and rotational attitude data of the marker under the coordinates of the infrared binocular device are recorded, and the displacement and rotational attitude data of the end-effector are also recorded.

[0015] The average unit vectors along the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end are calculated at different initial points. The rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained by vector concatenation.

[0016] A closed-loop transfer matrix formula is established using the coordinates of the robotic arm base, the infrared binocular device, the marker, and the robotic arm end effector. By obtaining the generalized inverse matrix, the displacement relationship between the robotic arm base, the infrared binocular device, and the marker to the robotic arm end effector is obtained.

[0017] By obtaining the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself, the displacement and attitude values ​​of the marker are calculated. The calculated values ​​are then compared with the reference values ​​of the infrared binocular optical device.

[0018] By using the dual quaternion method, the difference is iteratively reduced to the global minimum, thus obtaining the optimal transformation relationship.

[0019] Furthermore, the method also includes:

[0020] A puncture needle mechanism is installed on the robotic arm. Based on the mechanical drawing parameters of the puncture needle mechanism and the obtained positive kinematic relationship, the displacement and attitude of the puncture needle in the coordinate system of the robotic arm are calculated.

[0021] Furthermore, the average unit vectors along the X, Y, and Z axes based on the robot arm's base coordinate system and the robot arm's end effector coordinate system are calculated for different initial points. By concatenating these vectors, the rotational attitude relationship between the robot arm's base coordinate system and the infrared binocular device's coordinate system is obtained, including:

[0022]

[0023]

[0024] In the above formula, Rbox, Rboy, and Rboz are the unit normal vectors of the robotic arm moving along its own base coordinates along the three axes under the infrared binocular device, respectively. Rbo is the rotation matrix between the robotic arm's own base coordinates and the coordinates of the infrared binocular device, and the rotation matrix relationship Ret between the robotic arm's end effector and the marker is obtained for different initial point states.

[0025] Furthermore, the average unit vectors along the X, Y, and Z axes based on the robot arm's base coordinate system and the robot arm's end effector coordinate system are calculated for different initial points. By concatenating these vectors, the rotational attitude relationship between the robot arm's base coordinate system and the infrared binocular device's coordinate system is obtained, including:

[0026] The transfer matrix between the robotic arm's end effector and the marker is determined by establishing a closed-loop relationship, as shown in the following formula:

[0027] Pe = Vbe·Vob·Vto·Vet·Pe

[0028] Pe is a random point based on the coordinate system of the robotic arm's end effector; Vbe represents the transition matrix from the base coordinate system Cb of the robotic arm to the end effector coordinate system Ce of the robotic arm; Vob represents the transition matrix from the base coordinate system Co of the robotic arm to the end effector coordinate system Cb of the robotic arm; Vto represents the transition matrix from the base coordinate system Ct of the robotic arm to the end effector coordinate system Co of the robotic arm; and Vet represents the transition matrix from the base coordinate system Ce of the robotic arm to the end effector coordinate system Ct of the robotic arm.

[0029] By using the above closed-loop equation, the displacement relationship between the robot arm base coordinates and the infrared binocular optical device, and between the robot arm end effector and the marker is obtained by finding the pseudo-inverse matrix, thereby obtaining the transition matrices Vob and Vet.

[0030] Furthermore, by obtaining the rotational attitude relationship between the robotic arm base coordinates and the infrared binocular device coordinates, the displacement relationship between the robotic arm base coordinates and the infrared binocular device, and the displacement relationship between the marker and the robotic arm end effector, and the robotic arm's own DH parameters, the displacement and attitude values ​​of the marker are calculated. These calculated values ​​include:

[0031] The Vet and the robot arm's own DH parameters, along with the collected and recorded joint angle data, are used as inputs to the correction system. By substituting the DH parameters, joint angles, and the transfer matrix Ve between the robot arm's end-effector coordinate system and the marker's own coordinate system into the robot arm's forward kinematics, the displacement and rotational attitude of the marker in the robot arm's base coordinate system are calculated. Then, using the calculated transfer matrix Vbo between the robot arm's base coordinate system and the infrared binocular optical device's own coordinate system, the displacement and rotational attitude of the marker in the infrared binocular device's coordinate reference system are calculated.

[0032] Infrared binocular devices capture the posture and displacement of markers using their own coordinate reference, and these values ​​are used as reference values.

[0033] Furthermore, the DH parameter is a system parameter determined by the mathematical model and coordinate system of a robotic arm, which uses four parameters to express the positional and angular relationship between two pairs of joint links.

[0034] Furthermore, by using the dual quaternion method, the difference is iteratively reduced to the global minimum, and the optimal transformation relation is obtained, including:

[0035] By using dual quaternions, multiple rounds of data sets are iterated to optimize the error, continuously updating the value of the DH parameter to make its error reach the global minimum, minimizing the error between the calculated value and the reference value, and obtaining the optimized transfer matrix Vob between the robot arm base coordinate system and the infrared binocular device coordinate system.

[0036] According to another embodiment of the present invention, an optical positioning device for a robotic arm is provided, comprising:

[0037] The basic data acquisition unit is used to set multiple initial points based on the base coordinates of the robotic arm and the coordinates of the robotic arm end effector. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and the coordinate system of the robotic arm end effector. The data of the displacement and rotational attitude of the marker under the coordinates of the infrared binocular device are recorded, and the data of the displacement and rotational attitude of the robotic arm end effector are also recorded.

[0038] The rotational attitude relationship acquisition unit is used to calculate the average unit vectors of the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end effector at different initial points. By concatenating the vectors, the rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained.

[0039] The displacement relationship acquisition unit is used to establish a closed-loop transfer matrix formula using the robot arm base coordinates, infrared binocular device, marker, and robot arm end coordinates. By obtaining the generalized inverse matrix, the displacement relationship between the robot arm base coordinates, the infrared binocular device, and the marker to the robot arm end is obtained.

[0040] The difference acquisition unit is used to calculate the displacement and attitude values ​​of the marker by using the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself. The calculated value is then compared with the reference value of the infrared binocular optical device.

[0041] The optimal transformation relation acquisition unit is used to obtain the optimal transformation relation by iteratively reducing the difference to the global minimum using the dual quaternion method.

[0042] A storage medium storing program files capable of implementing any of the above-described robotic arm optical positioning methods.

[0043] A processor for running a program, wherein the program executes any of the above-mentioned robotic arm optical positioning methods during runtime.

[0044] The robotic arm optical positioning method and apparatus in this invention mainly include a closed-loop matrix calculation and a robotic arm DH parameter identification and correction part. The first part, the closed-loop matrix calculation, is to obtain the rotation and translation relationships between the marker and the coordinate system of the robotic arm end effector, and between the robotic arm base coordinate system and the coordinate system of the infrared binocular device. The second part uses the transformation matrix obtained in the first part to perform robotic arm DH parameter correction using a method based on dual quaternions, updating the internal parameters of the aforementioned transformation matrix. In this invention, the robotic arm measurement data is automatically acquired, which is more stable and accurate than manual measurement. Furthermore, this invention only needs to run the calibration program once to quickly complete the corresponding position calibration, resulting in high efficiency and reducing the impact of optical device movement on robotic arm calibration. Attached Figure Description

[0045] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings:

[0046] Figure 1 This is a real-world image of an existing robotic system for thoracentesis.

[0047] Figure 2 This is a schematic diagram of the rotation of a rotating robotic arm in a current thoracentesis surgical robot system.

[0048] Figure 3 A rotational relationship diagram of active markers in an existing robotic system for thoracentesis.

[0049] Figure 4 This invention relates to a calibration method for a dual-arm robot and a calibration diagram of the puncture needle in the system.

[0050] Figure 5 This is a real-world image of the robotic arm optical positioning system of the present invention;

[0051] Figure 6 This is a schematic diagram showing the calculated position and reference position as the initial position in the optical positioning system of the robotic arm of the present invention;

[0052] Figure 7 This is a flowchart of the optical positioning method for the robotic arm of the present invention. Detailed Implementation

[0053] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0054] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0055] Example 1

[0056] According to an embodiment of the present invention, an optical positioning method for a robotic arm is provided, comprising the following steps:

[0057] Please see Figure 4-5 Multiple initial points are set based on the base coordinates and end-effector coordinates of the robotic arm. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and end-effector coordinate system. The displacement and rotational attitude data of the marker under the coordinates of the infrared binocular device are recorded, and the displacement and rotational attitude data of the end-effector are also recorded.

[0058] The average unit vectors along the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end are calculated at different initial points. The rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained by vector concatenation.

[0059] A closed-loop transfer matrix formula is established using the coordinates of the robotic arm base, the infrared binocular device, the marker, and the robotic arm end effector. By obtaining the generalized inverse matrix, the displacement relationship between the robotic arm base, the infrared binocular device, and the marker to the robotic arm end effector is obtained.

[0060] By obtaining the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself, the displacement and attitude values ​​of the marker are calculated. The calculated values ​​are then compared with the reference values ​​of the infrared binocular optical device.

[0061] By using the dual quaternion method, the difference is iteratively reduced to the global minimum, thus obtaining the optimal transformation relationship.

[0062] The robotic arm optical positioning method in this embodiment mainly includes closed-loop matrix calculation and robotic arm DH parameter identification and correction. The first part, closed-loop matrix calculation, is to obtain the rotation and translation relationships between the marker and the robotic arm end-effector coordinate system, and between the robotic arm base coordinate system and the infrared binocular device coordinate system. The second part uses the transformation matrix obtained in the first part to perform robotic arm DH parameter correction using a dual quaternion-based method, updating the internal parameters of the aforementioned transformation matrix. In this invention, the robotic arm measurement data is automatically acquired, which is more stable and accurate than manual measurement. Furthermore, this invention only requires running the calibration program once to quickly complete the corresponding position calibration, resulting in high efficiency and reducing the impact of optical device movement on robotic arm calibration.

[0063] The methods also include:

[0064] A puncture needle mechanism is installed on the robotic arm. Based on the mechanical drawing parameters of the puncture needle mechanism and the obtained positive kinematic relationship, the displacement and attitude of the puncture needle in the coordinate system of the robotic arm are calculated.

[0065] The calculation of the average unit vectors along the X, Y, and Z axes based on the robot arm's base coordinate system and the robot arm's end effector coordinate system at different initial points, and the obtaining of the rotational attitude relationship between the robot arm's base coordinate system and the infrared binocular device's coordinate system through vector concatenation, includes:

[0066]

[0067]

[0068] In the above formula, Rbox, Rboy, and Rboz are the unit normal vectors of the robotic arm moving along its own base coordinates along the three axes under the infrared binocular device, respectively. Rbo is the rotation matrix between the robotic arm's own base coordinates and the coordinates of the infrared binocular device, and the rotation matrix relationship Ret between the robotic arm's end effector and the marker is obtained for different initial point states.

[0069] The calculation of the average unit vectors along the X, Y, and Z axes based on the robot arm's base coordinate system and the robot arm's end effector coordinate system at different initial points, and the obtaining of the rotational attitude relationship between the robot arm's base coordinate system and the infrared binocular device's coordinate system through vector concatenation, includes:

[0070] The transfer matrix between the robotic arm's end effector and the marker is determined by establishing a closed-loop relationship, as shown in the following formula:

[0071] Pe = Vbe·Vob·Vto·Vet·Pe

[0072] Pe is a random point based on the coordinate system of the robotic arm's end effector; Vbe represents the transition matrix from the base coordinate system Cb of the robotic arm to the end effector coordinate system Ce of the robotic arm; Vob represents the transition matrix from the base coordinate system Co of the robotic arm to the end effector coordinate system Cb of the robotic arm; Vto represents the transition matrix from the base coordinate system Ct of the robotic arm to the end effector coordinate system Co of the robotic arm; and Vet represents the transition matrix from the base coordinate system Ce of the robotic arm to the end effector coordinate system Ct of the robotic arm.

[0073] By using the above closed-loop equation, the displacement relationship between the robot arm base coordinates and the infrared binocular optical device, and between the robot arm end effector and the marker is obtained by finding the pseudo-inverse matrix, thereby obtaining the transition matrices Vob and Vet.

[0074] Specifically, by obtaining the rotational attitude relationship between the robotic arm base coordinates and the infrared binocular device coordinates, the displacement relationship between the robotic arm base coordinates and the infrared binocular device, and the marker to the robotic arm end effector, along with the robotic arm's own DH parameters, the displacement and attitude values ​​of the marker are calculated. These calculated values ​​include:

[0075] The Vet and the robot arm's own DH parameters, along with the collected and recorded joint angle data, are used as inputs to the correction system. By substituting the DH parameters, joint angles, and the transfer matrix Ve between the robot arm's end-effector coordinate system and the marker's own coordinate system into the robot arm's forward kinematics, the displacement and rotational attitude of the marker in the robot arm's base coordinate system are calculated. Then, using the calculated transfer matrix Vbo between the robot arm's base coordinate system and the infrared binocular optical device's own coordinate system, the displacement and rotational attitude of the marker in the infrared binocular device's coordinate reference system are calculated.

[0076] Infrared binocular devices capture the posture and displacement of markers using their own coordinate reference, and these values ​​are used as reference values.

[0077] Among them, the DH parameter is a system parameter determined by the mathematical model and coordinate system of a robotic arm, which uses four parameters to express the positional and angular relationship between two pairs of joint links.

[0078] Among them, the optimal transformation relation is obtained by iteratively reducing the difference to the global minimum using the dual quaternion method, including:

[0079] By using dual quaternions, multiple rounds of data sets are iterated to optimize the error, continuously updating the value of the DH parameter to make its error reach the global minimum, minimizing the error between the calculated value and the reference value, and obtaining the optimized transfer matrix Vob between the robot arm base coordinate system and the infrared binocular device coordinate system.

[0080] The robotic arm optical positioning method of the present invention will be described in detail below with reference to specific embodiments:

[0081] To reduce the errors caused by manually operating robotic arms in existing technologies, this invention not only calculates the transformation matrix between the world coordinate system and the robot coordinate system, but also updates the DH parameters, thereby further improving the spatial accuracy of the robot.

[0082] To achieve the above objectives, this invention provides an optical positioning method for a robotic arm, which mainly includes closed-loop matrix calculation and robotic arm DH parameter identification and correction. The first part, closed-loop matrix calculation, is to obtain the rotation and translation relationships between the marker and the coordinate system of the robotic arm end effector, and between the robotic arm base coordinate system and the coordinate system of the infrared binocular device. The second part uses the transformation matrix obtained in the first part to perform robotic arm DH parameter correction using a method based on dual quaternions, and updates the internal parameters of the aforementioned transformation matrix.

[0083] Step 1: Please refer to Figure 4-5 The program controls the robotic arm to be in multiple different initial positions, and then translates it several times along the x, y, and z axes of the robotic arm end coordinate system and the robotic arm base coordinate system. It collects the posture data of the marker recorded by the infrared binocular device, as well as the posture and joint angle data of the robotic arm system itself. By calculating the average value of the unit vectors of the three axes and arranging them, the corresponding rotation matrix can be obtained.

[0084]

[0085]

[0086] In the above formula, Rbox, Rboy, and Rboz are the unit normal vectors of the robotic arm's movement along its own base coordinates towards the three axes under the infrared binocular device, respectively. Rbo is the rotation matrix between the robotic arm's own base coordinates and the coordinates of the infrared binocular device. Similarly, the rotation matrix Ret between the robotic arm's end effector and the marker can be obtained for different initial point states. The transfer matrix between the robotic arm's end effector and the marker is determined by establishing a closed-loop relationship formula, as shown in the following equation:

[0087] Pe = Vbe·Vob·Vto·Vet·Pe

[0088] Pe is a random point based on the coordinate system of the robotic arm's end effector. Vbe represents the transition matrix from the base coordinate system Cb of the robotic arm to the end effector coordinate system Ce of the robotic arm. The same applies to Vob, Vto, and Vet. Through the above closed-loop equations, the displacement relationship between the base coordinate system of the robotic arm and the infrared binocular optical device, and between the end effector of the robotic arm and the marker can be obtained by finding the pseudo-inverse matrix, thereby obtaining the transition matrices Vob and Vet.

[0089] Step 2: Based on the results of Step 1, Vet and the robotic arm's own DH parameters (DH parameters are system parameters determined by the robotic arm's mathematical model and coordinate system, expressing the positional and angular relationship between two pairs of joint links using four parameters), along with the joint angle data collected and recorded in Step 1, are used as inputs to the correction system. Substituting the DH parameters, joint angles, and the transfer matrix Ve between the robotic arm's end-effector coordinate system and the marker's own coordinate system into the robotic arm's forward kinematics, the displacement and rotational attitude of the marker in the robotic arm's base coordinates are calculated. Then, using the transfer matrix Vbo between the robotic arm's base coordinates calculated in Step 1 and the infrared binocular optical device's own coordinates, the displacement and rotational attitude of the marker in the infrared binocular device's coordinate reference system are calculated. Meanwhile, the infrared binocular device, under its own coordinate reference, can capture the marker's attitude and displacement. Due to the high accuracy of the infrared binocular device, its values ​​are used as reference values. Figure 6 The diagram shows the calculated position and the reference position calculated as the initial position in the optical positioning system of the robotic arm of the present invention.

[0090] Step 3: See Figure 7 The difference between the calculated value and the reference value is calculated. By using dual quaternions, multiple rounds of data sets are iterated to optimize the error. The value of the DH parameter is continuously updated to make the error reach the global minimum. This minimizes the error between the calculated result and the reference value, and the optimized transfer matrix Vob between the robot arm base coordinate system and the infrared binocular device coordinate system is obtained.

[0091] When installing the puncture needle mechanism, the displacement and attitude of the puncture needle in the coordinate system of the robotic arm can be calculated based on the mechanical drawing parameters of the puncture needle mechanism and the above-mentioned positive kinematic relationship.

[0092] The innovative technical points and beneficial effects of the method of this invention are at least as follows:

[0093] In this invention, the data acquired by the robotic arm is automatically measured, which is more stable and accurate than manual measurement. Furthermore, this invention only requires running the calibration program once to quickly complete the calibration of the corresponding position, resulting in high efficiency and reducing the impact of optical equipment movement on the robotic arm calibration.

[0094] The feasibility of this invention has been proven through physical robotic arm experiments. These experiments compared the reference values ​​obtained from the measured markers with the calculated values ​​of this invention. The single-axis errors of X, Y, and Z were within 1 mm, with an average error within 0.7 mm. When various types of actuators are added to the robotic arm, the coordinate transformation relationship of this invention can be used to move the end effector to the target position via the robotic arm, serving as an extension of this invention.

[0095] Example 2

[0096] According to another embodiment of the present invention, an optical positioning device for a robotic arm is provided, comprising:

[0097] The basic data acquisition unit is used to set multiple initial points based on the base coordinates of the robotic arm and the coordinates of the robotic arm end effector. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and the coordinate system of the robotic arm end effector. The data of the displacement and rotational attitude of the marker under the coordinates of the infrared binocular device are recorded, and the data of the displacement and rotational attitude of the robotic arm end effector are also recorded.

[0098] The rotational attitude relationship acquisition unit is used to calculate the average unit vectors of the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end effector at different initial points. By concatenating the vectors, the rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained.

[0099] The displacement relationship acquisition unit is used to establish a closed-loop transfer matrix formula using the robot arm base coordinates, infrared binocular device, marker, and robot arm end coordinates. By obtaining the generalized inverse matrix, the displacement relationship between the robot arm base coordinates, the infrared binocular device, and the marker to the robot arm end is obtained.

[0100] The difference acquisition unit is used to calculate the displacement and attitude values ​​of the marker by using the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself. The calculated value is then compared with the reference value of the infrared binocular optical device.

[0101] The optimal transformation relation acquisition unit is used to obtain the optimal transformation relation by iteratively reducing the difference to the global minimum using the dual quaternion method.

[0102] The robotic arm optical positioning device in this embodiment mainly includes a closed-loop matrix calculation and a robotic arm DH parameter identification and correction part. The first part, closed-loop matrix calculation, is to obtain the rotation and translation relationships between the marker and the coordinate system of the robotic arm end effector, and between the robotic arm base coordinate system and the coordinate system of the infrared binocular device. The second part uses the transformation matrix obtained in the first part to perform robotic arm DH parameter correction using a method based on dual quaternions, updating the internal parameters of the aforementioned transformation matrix. In this invention, the robotic arm measurement data is automatically acquired, which is more stable and accurate than manual measurement. Furthermore, this invention only needs to run the calibration program once to quickly complete the corresponding position calibration, resulting in high efficiency and reducing the impact of optical device movement on robotic arm calibration.

[0103] The optical positioning device for the robotic arm of the present invention will be described in detail below with reference to specific embodiments:

[0104] To reduce the errors caused by manually operating robotic arms in existing technologies, this invention not only calculates the transformation matrix between the world coordinate system and the robot coordinate system, but also updates the DH parameters, thereby further improving the spatial accuracy of the robot.

[0105] To achieve the above objectives, this invention provides an optical positioning device for a robotic arm, which mainly includes a closed-loop matrix calculation and a robotic arm DH parameter identification and correction section. The first part, the closed-loop matrix calculation, is used to obtain the rotation and translation relationships between the marker and the coordinate system of the robotic arm end effector, and between the robotic arm base coordinate system and the coordinate system of the infrared binocular device. The second part uses the transformation matrix obtained in the first part to perform robotic arm DH parameter correction using a method based on dual quaternions, and updates the internal parameters of the aforementioned transformation matrix.

[0106] Step 1: Through the program, control the robotic arm to be in multiple different initial positions, and then translate it several times in the x, y, and z axes of the robotic arm end coordinate system and the robotic arm base coordinate system. Collect the posture data of the marker recorded by the infrared binocular device, as well as the posture and joint angle data of the robotic arm system itself. By calculating the average value of the unit vectors of the three axes and arranging them, the corresponding rotation matrix can be obtained.

[0107]

[0108]

[0109] In the above formula, Rbox, Rboy, and Rboz are the unit normal vectors of the robotic arm's movement along its own base coordinates towards the three axes under the infrared binocular device, respectively. Rbo is the rotation matrix between the robotic arm's own base coordinates and the coordinates of the infrared binocular device. Similarly, the rotation matrix Ret between the robotic arm's end effector and the marker can be obtained for different initial point states. The transfer matrix between the robotic arm's end effector and the marker is determined by establishing a closed-loop relationship formula, as shown in the following equation:

[0110] Pe = Vbe·Vob·Vto·Vet·Pe

[0111] Pe is a random point based on the coordinate system of the robotic arm's end effector. Vbe represents the transition matrix from the base coordinate system Cb of the robotic arm to the end effector coordinate system Ce of the robotic arm. The same applies to Vob, Vto, and Vet. Through the above closed-loop equations, the displacement relationship between the base coordinate system of the robotic arm and the infrared binocular optical device, and between the end effector of the robotic arm and the marker can be obtained by finding the pseudo-inverse matrix, thereby obtaining the transition matrices Vob and Vet.

[0112] Step 2: Based on the results of Step 1, Vet and the robotic arm's own DH parameters (DH parameters are system parameters determined by the robotic arm's mathematical model and coordinate system, expressing the positional and angular relationship between two pairs of joint links using four parameters), along with the joint angle data collected and recorded in Step 1, are used as inputs to the correction system. Substituting the DH parameters, joint angles, and the transfer matrix Ve between the robotic arm's end-effector coordinate system and the marker's own coordinate system into the robotic arm's forward kinematics, the displacement and rotational attitude of the marker in the robotic arm's base coordinates are calculated. Then, using the transfer matrix Vbo between the robotic arm's base coordinates calculated in Step 1 and the infrared binocular optical device's own coordinates, the displacement and rotational attitude of the marker in the infrared binocular device's coordinate reference system are calculated. Meanwhile, the infrared binocular device, under its own coordinate reference, can capture the marker's attitude and displacement. Due to the high accuracy of the infrared binocular device, its values ​​are used as reference values. Figure 6 The diagram shows the calculated position and the reference position calculated as the initial position in the optical positioning system of the robotic arm of the present invention.

[0113] Step 3: See Figure 7 The difference between the calculated value and the reference value is calculated. By using dual quaternions, multiple rounds of data sets are iterated to optimize the error. The value of the DH parameter is continuously updated to make the error reach the global minimum. This minimizes the error between the calculated result and the reference value, and the optimized transfer matrix Vob between the robot arm base coordinate system and the infrared binocular device coordinate system is obtained.

[0114] When installing the puncture needle mechanism, the displacement and attitude of the puncture needle in the coordinate system of the robotic arm can be calculated based on the mechanical drawing parameters of the puncture needle mechanism and the above-mentioned positive kinematic relationship.

[0115] The innovative technical points and beneficial effects of the device of this invention are at least as follows:

[0116] In this invention, the data acquired by the robotic arm is automatically measured, which is more stable and accurate than manual measurement. Furthermore, this invention only requires running the calibration program once to quickly complete the calibration of the corresponding position, resulting in high efficiency and reducing the impact of optical equipment movement on the robotic arm calibration.

[0117] The feasibility of this invention has been proven through physical robotic arm experiments. These experiments compared the reference values ​​obtained from the measured markers with the calculated values ​​of this invention. The single-axis errors of X, Y, and Z were within 1 mm, with an average error within 0.7 mm. When various types of actuators are added to the robotic arm, the coordinate transformation relationship of this invention can be used to move the end effector to the target position via the robotic arm, serving as an extension of this invention.

[0118] Example 3

[0119] A storage medium storing program files capable of implementing any of the above-described robotic arm optical positioning methods.

[0120] Example 4

[0121] A processor for running a program, wherein the program executes any of the above-mentioned robotic arm optical positioning methods during runtime.

[0122] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0123] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0124] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The system embodiments described above are merely illustrative; for example, the division of units can be a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection of units or modules may be electrical or other forms.

[0125] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0126] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0127] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0128] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for optical positioning of a robotic arm, characterized in that, Includes the following steps: Multiple initial points are set based on the base coordinates and end-effector coordinates of the robotic arm. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and the end-effector coordinate system. The displacement and rotational attitude data of the marker under the coordinates of the infrared binocular device are recorded, and the displacement and rotational attitude data of the end-effector are also recorded. The average unit vectors along the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end are calculated at different initial points. The rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained by vector concatenation. A closed-loop transfer matrix formula is established using the coordinates of the robotic arm base, the infrared binocular device, the marker, and the robotic arm end effector. By obtaining the generalized inverse matrix, the displacement relationship between the robotic arm base, the infrared binocular device, and the marker to the robotic arm end effector is obtained. By obtaining the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself, the displacement and attitude values ​​of the marker are calculated. The calculated values ​​are then compared with the reference values ​​of the infrared binocular optical device. By using the dual quaternion method, the difference is iteratively reduced to the global minimum to obtain the optimal transformation relation; where: The displacement and attitude values ​​of the marker are calculated by using the rotational attitude relationship between the robot arm base coordinates and the infrared binocular device coordinates, the displacement relationship between the robot arm base coordinates and the infrared binocular device and the marker to the end effector of the robot arm, and the robot arm's own DH parameters. The calculated values ​​include: The Vet and the robot arm's own DH parameters, along with the collected and recorded joint angle data, are used as inputs to the correction system. By substituting the DH parameters, joint angles, and the transfer matrix Ve between the robot arm's end-effector coordinate system and the marker's own coordinate system into the robot arm's forward kinematics, the displacement and rotational attitude of the marker in the robot arm's base coordinate system are calculated. Then, using the calculated transfer matrix Vbo between the robot arm's base coordinate system and the infrared binocular optical device's own coordinate system, the displacement and rotational attitude of the marker in the infrared binocular device's coordinate reference system are calculated. Infrared binocular devices capture the posture and displacement of markers using their own coordinate reference, and these values ​​are used as reference values.

2. The method of claim 1, wherein, The method further includes: A puncture needle mechanism is installed on the robotic arm. Based on the mechanical drawing parameters of the puncture needle mechanism and the obtained positive kinematic relationship, the displacement and attitude of the puncture needle in the coordinate system of the robotic arm are calculated.

3. The robotic arm optical positioning method of claim 1, wherein, The calculation of the average unit vectors along the X, Y, and Z axes based on the robot arm base coordinate system and the robot arm end-effector coordinate system at different initial points, and the obtaining of the rotational attitude relationship between the robot arm base coordinate system and the infrared binocular device coordinate system by vector concatenation, includes: In the above formula, Rbox, Rboy, and Rboz are the unit normal vectors of the robotic arm moving along its own base coordinates along the three axes under the infrared binocular device, respectively. Rbo is the rotation matrix between the robotic arm's own base coordinates and the coordinates of the infrared binocular device, and the rotation matrix relationship Ret between the robotic arm's end effector and the marker is obtained for different initial point states.

4. The optical positioning method for a robotic arm according to claim 3, characterized in that, The calculation of the average unit vectors along the X, Y, and Z axes based on the robot arm base coordinate system and the robot arm end-effector coordinate system at different initial points, and the obtaining of the rotational attitude relationship between the robot arm base coordinate system and the infrared binocular device coordinate system by vector concatenation, includes: The transfer matrix between the robotic arm's end effector and the marker is determined by establishing a closed-loop relationship, as shown in the following formula: Pe is a random point based on the coordinate system of the robotic arm's end effector; Vbe represents the transition matrix from the base coordinate system Cb of the robotic arm to the end effector coordinate system Ce of the robotic arm; Vob represents the transition matrix from the base coordinate system Co of the robotic arm to the end effector coordinate system Cb of the robotic arm; Vto represents the transition matrix from the base coordinate system Ct of the robotic arm to the end effector coordinate system Co of the robotic arm; and Vet represents the transition matrix from the base coordinate system Ce of the robotic arm to the end effector coordinate system Ct of the robotic arm. By using the above closed-loop equation, the displacement relationship between the robot arm base coordinates and the infrared binocular optical device, and between the robot arm end effector and the marker is obtained by finding the pseudo-inverse matrix, thereby obtaining the transition matrices Vob and Vet.

5. The optical positioning method for a robotic arm according to claim 4, characterized in that, The DH parameter is a system parameter determined by the mathematical model and coordinate system of a robotic arm, which uses four parameters to express the positional and angular relationship between two pairs of joint links.

6. The optical positioning method for a robotic arm according to claim 4, characterized in that, The method of iteratively reducing the difference to the global minimum using dual quaternions to obtain the optimal transformation relationship includes: By using dual quaternions, multiple rounds of data sets are iterated to optimize the error, continuously updating the value of the DH parameter to make its error reach the global minimum, minimizing the error between the calculated value and the reference value, and obtaining the optimized transfer matrix Vob between the robot arm base coordinate system and the infrared binocular device coordinate system.

7. A robotic arm optical positioning device utilizing the robotic arm optical positioning method of claim 1, characterized in that, include: The basic data acquisition unit is used to set multiple initial points based on the base coordinates of the robotic arm and the coordinates of the robotic arm end effector. The robotic arm is set at multiple different initial points and moved several times along the X, Y, and Z axes based on the base coordinate system and the coordinate system of the robotic arm end effector. The data of the displacement and rotational attitude of the marker under the coordinates of the infrared binocular device are recorded, and the data of the displacement and rotational attitude of the robotic arm end effector are also recorded. The rotational attitude relationship acquisition unit is used to calculate the average unit vectors of the X, Y, and Z axes based on the coordinate system of the robot arm base and the coordinate system of the robot arm end effector at different initial points. By concatenating the vectors, the rotational attitude relationship between the robot arm base coordinates and the coordinate system of the infrared binocular device is obtained. The displacement relationship acquisition unit is used to establish a closed-loop transfer matrix formula using the robot arm base coordinates, infrared binocular device, marker, and robot arm end coordinates. By obtaining the generalized inverse matrix, the displacement relationship between the robot arm base coordinates, the infrared binocular device, and the marker to the robot arm end is obtained. The difference acquisition unit is used to calculate the displacement and attitude values ​​of the marker by using the rotational attitude relationship between the base coordinates of the robotic arm and the coordinates of the infrared binocular device, the displacement relationship between the base coordinates of the robotic arm and the infrared binocular device and the marker to the end of the robotic arm, and the DH parameters of the robotic arm itself. The calculated value is then compared with the reference value of the infrared binocular optical device. The optimal transformation relation acquisition unit is used to obtain the optimal transformation relation by iteratively reducing the difference to the global minimum using the dual quaternion method.

8. A storage medium, characterized in that, The storage medium stores program files capable of implementing the robotic arm optical positioning method according to any one of claims 1 to 6.

9. A processor, characterized in that, The processor is used to run a program, wherein the program executes the robotic arm optical positioning method according to any one of claims 1 to 6.

Images