Optical Tracking-Based Robot Navigation Method

Abstract

The present invention discloses a robot navigation method based on optical tracking, comprising the steps of: (1) determining the initial target pose of the end effector and taking it as the current target pose; (2) judging whether the included angle between the normal direction of a certain group of end effectors in the current target pose and the normal direction of the optical tracking system is within the effective included angle range and whether the inverse kinematic solution of the robotic arm has a solution; (3) if the aforementioned included angle is within the effective included angle range and the inverse kinematic solution of the robotic arm has a solution, taking the current target pose as the target pose; if there is no inverse solution or no included angle is within the effective included angle range, rotating the current target pose by a set step length with the execution direction as the rotation axis and updating the current target pose, and repeating steps (2) to (3) until the target pose is obtained or the set angle is traversed. The present invention simplifies the process of solving the target pose of the end effector, reduces the solution time while ensuring the navigation accuracy.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a robot navigation method based on optical tracking. Background Art

[0002] For a robot navigated by an optical tracking system, the planning information of a given end tool in the usage scenario usually only has one position coordinate and one direction vector. However, the movement of the robot to the target position requires a complete posture information containing 6 degrees of freedom to obtain the transformation matrix from the robot base coordinate system to the posture to be achieved by the end tool, thereby inversely solving the motion of each joint of the robot.

[0003] The common practice nowadays is to first determine the position and posture of the end tool in the robot base coordinate system and in the optical tracking system coordinate system, and then adjust the end tool so that the origin of the end tool coordinate system reaches the position coordinate, and the axis of the end tool coincides with the direction vector, while ensuring that the angle between the plane where the optical tracker is located and the plane of the optical tracking system is within the recognizable range of the optical tracking system. In this case, the motion of each joint of the robot is inversely solved. If there is no inverse solution, adjust the end tool until the end tool is adjusted to a position and posture that satisfies the planning information, the recognition accuracy of the optical tracking system, and the robot kinematics has an inverse solution. This method has a complex calculation process, especially when multiple sets of trackers are equipped at the end of the robot in order to expand the recognizable angle of the end, the calculation process becomes more complicated and time-consuming. Summary of the invention

[0004] Purpose of the invention: In view of the above-mentioned shortcomings, the present invention proposes a robot navigation method based on optical tracking, which can quickly solve the target position of the end tool when there are multiple sets of trackers at the end, thereby ensuring the accuracy of navigation while reducing the solution time.

[0005] Technical solution:

[0006] A robot navigation method based on optical tracking, comprising the steps of:

[0007] (1) Determine the initial target pose of the end tool according to the planned position and execution direction that the end tool needs to reach, and use it as the current target pose;

[0008] (2) Determine whether the angle between the normal of a set of end trackers and the normal of the optical tracking system is within the valid angle range when the end tool is in the current target posture and whether the inverse solution of the robot arm kinematics is solvable;

[0009] (3) If the aforementioned included angle is within the effective included angle range and the inverse kinematic solution of the robotic arm has a solution, use the current target pose as the target pose; if there is no inverse solution or no included angle is within the effective included angle range, rotate the current target pose around the execution direction by a set step length and update the current target pose, and repeat steps (2) - (3) until the target pose is obtained or the set angle range is traversed.

[0010] Based on the planned path and the reserved safety distance, obtain the planned position and execution direction that the end effector needs to reach.

[0011] The determination of the initial target pose is specifically as follows:

[0012] Combining the planned path, the conversion relationship between the image coordinate system and the robot coordinate system, and the reserved safety distance, obtain the planned position and execution direction that the end effector needs to reach in the robot coordinate system. Use the pose where the end effector satisfies its planned position and execution direction and the normal included angle between a certain end tracker on it and the optical tracking system is less than the set range as its initial target pose.

[0013] The determination of the initial target pose is specifically as follows:

[0014] Transform the planned position and execution direction that the end effector needs to reach into the optical tracking system coordinate system to obtain the position coordinate P1 and direction vector R1 of the end effector in the optical tracking system coordinate system;

[0015] Construct a vector V0 with the position coordinate P1 and the center point of the binocular camera;

[0016] Construct a vector V1 with the position coordinate P1 and the direction vector R1;

[0017] There are two vectors perpendicular to the vector V1 and passing through the position coordinate P1 on the plane formed by the vector V0 and the vector V1. Select the vector with a smaller included angle with the vector V0 from these two vectors as the vector V2;

[0018] Obtain a vector V3 by taking the cross product of the vector V1 and the vector V2;

[0019] Establish an end effector coordinate system with the position coordinate P1 as the origin, the vector V1 as the z-axis, the vector V2 as the y-axis, and the vector V3 as the x-axis. Use the pose of the end effector corresponding to the establishment of this coordinate system as the initial pose of the end effector in the optical tracking system coordinate system, and transform the initial pose into the robot coordinate system to obtain the initial target pose.

[0020] The specific effective included angle range is as follows:

[0021] The maximum visible half-angle β between the optical tracking system and the end effector tracker is calculated based on the positions and installation parameters of the optical tracking system and the end effector tracker. The relationship between it and the maximum effective angle α is α = β – atan(A / Z), where A is half of the distance between the two cameras in the optical tracking system, and Z is the distance between the center point of the optical tracking system and the center point of the end effector tracker. The range of the effective angle is [-α, α].

[0022] The range of the effective angle is [-30°, 30°].

[0023] The set step size is 1°.

[0024] The specific operation of rotating the current target pose by the set step size with the execution direction as the rotation axis is as follows:

[0025] Based on the initial target pose, rotate the current target pose to traverse the angles ±(α + θ), where α is the maximum effective angle and θ is the normal angle between the planes where two adjacent end effector trackers are located.

[0026] The specific operation of rotating the current target pose by the set step size with the execution direction as the rotation axis is as follows:

[0027] Based on the initial target pose, first rotate the current target pose one by one with the iterative step size to traverse the angles ±α;

[0028] Based on the initial target pose, rotate θ forward or backward at one time. Taking the pose after rotating θ as the new reference, rotate with the iterative step size to traverse the angles ±α, where α is the maximum effective angle and θ is the normal angle between the planes where two adjacent end effector trackers are located.

[0029] The specific operation of rotating the current target pose by the iterative step size to traverse the angles ±α is as follows:

[0030] Rotate one by one in one direction with the iterative step size until α is reached, and then return to the reference and rotate in the reverse direction one by one with the iterative step size until -α is reached; or rotate alternately in each direction with the iterative step size until the angles ±α are traversed.

[0031] The end effector tracker is set to three groups, and each group of end effector trackers is respectively arranged on different planes. The three planes are arranged adjacent to each other in the circumferential direction, and the normal angle between two adjacent planes is 90°.

[0032] Beneficial effects: In the robot navigation method based on optical tracking of the present invention, during the process of solving the target pose of the forging tool, it can ensure that the angle between the normal direction of a certain plane in the plane where the end tracer is located and the normal direction of the optical tracking system is within the range of the effective angle, simplifies the process of solving the target pose of the end tool, and reduces the solving time while ensuring the navigation accuracy. Brief Description of the Drawings

[0033] Figure 1 Schematic diagrams of the end effector and the end tracer;

[0034] Figure 2 Flowchart of the present invention for determining the initial target pose of the end tool;

[0035] Figure 3 Flowchart of the present invention for solving the target pose of the end tool;

[0036] Figure 4 Schematic diagram of the maximum effective angle between the optical tracking system and the end tracer;

[0037] Among them, 10 is the end effector, 11 is the end tracer, and 20 is the optical tracking system. Detailed implementation manners

[0038] The present invention will be further illustrated below in conjunction with the accompanying drawings and specific embodiments.

[0039] As Figure 1 shown, in the embodiment disclosed by the present invention, an end effector 10 is provided at the end of the robot. The end effector 10 holds an end tool. A plurality of groups of end tracers 11 are installed on the end effector 10. The pose of the end tracer 11 is obtained in real time through the optical tracking system 20 for pose navigation of the end tool. In the present invention, the end tracer 11 is set to three groups. Each group of end tracers 11 is composed of at least three tracers. Each group of end tracers 11 is respectively arranged on different planes. In the embodiment disclosed by the present invention, they are the A plane, the B plane and the C plane respectively. The three planes are arranged adjacent to each other along the circumferential direction of the robot end. Among them, the normal angle between two adjacent planes is θ. In this embodiment, θ = 90°.

[0040] The robot navigation method based on optical tracking of the present invention includes the following steps:

[0041] (1) Determine the initial target pose of the end tool;

[0042] As Figure 2 shown, it includes the following steps:

[0043] (11) Transform the planned position and execution direction of the end tool into the coordinate system of the optical tracking system to obtain the corresponding position coordinates and direction vectors;

[0044] In the image coordinate system, the doctor plans the surgical channel to obtain the planned channel. Combining the conversion relationship between the planned channel, the image coordinate system and the robot coordinate system and the reserved safety distance, the planned position P0 and execution direction R0 that the end tool needs to reach in the robot coordinate system are obtained;

[0045] According to the transformation relationship between the robot coordinate system and the optical tracking system coordinate system, the planned position P0 and execution direction R0 of the end effector are transformed into the optical tracking system coordinate system, obtaining the position coordinate P1 and direction vector R1 of the end effector in the optical tracking system coordinate system;

[0046] (12) Establish an end effector coordinate system in the optical tracking system coordinate system and determine the initial pose of the end effector in the optical tracking system coordinate system;

[0047] In the optical tracking system coordinate system, a vector V0 is constructed with the position coordinate P1 and the center point of the binocular camera;

[0048] A vector V1 is constructed with the position coordinate P1 and the direction vector R1;

[0049] There are two vectors perpendicular to the vector V1 and passing through the position coordinate P1 on the plane formed by the vector V0 and the vector V1. Among these two vectors, the vector with a smaller included angle with the vector V0 is selected as the vector V2;

[0050] The vector V3 is obtained by cross-multiplying the vector V1 and the vector V2;

[0051] An end effector coordinate system is established with the position coordinate P1 as the origin, the vector V1 as the z-axis, the vector V2 as the y-axis, and the vector V3 as the x-axis. The pose of the end effector corresponding to the establishment of this coordinate system is used as the initial pose of the end effector in the optical tracking system coordinate system;

[0052] Furthermore, the present invention can establish the aforementioned end effector coordinate system when the end effector is in an arbitrary pose that satisfies the planned position P0 and execution direction R0, and use this pose as its initial pose;

[0053] Furthermore, in order to make the calculation process faster and reduce the solution time, when initially installing the end effector, the normal angle between a certain surface of the end effector and the optical tracking system is made less than a set range, such as [-30°, 30°]. Therefore, the initial pose of the end effector at this time is the pose when the normal angle between the end effector corresponding to this surface and the optical tracking system is less than the set range;

[0054] Furthermore, when initially installing the end effector, a certain surface of the end effector is made to face the optical tracking system directly. Therefore, the initial pose of the end effector at this time is the pose when the end effector corresponding to this surface faces the optical tracking system directly;

[0055] In the present invention, the end effector coordinate system is established in the optical tracking system coordinate system, but the present invention can also directly establish the end effector coordinate system in the robot coordinate system;

[0056] (13) Transform the aforementioned initial pose to the robot coordinate system according to the transformation relationship between the optical tracking system coordinate system and the robot coordinate system, and use the pose of the initial pose in the robot coordinate system as the initial target pose T of the end effector. r0 ;

[0057] In the embodiment disclosed in the present invention, the planned position P0 and the execution direction R0 that the end effector needs to reach in the robot coordinate system are transformed into the optical tracking system coordinate system, and combined with the pose of the optical tracking system to determine the above initial pose, and then use the pose obtained by transforming the initial pose into the robot coordinate system as the initial target pose; in the present invention, the optical tracking system is a binocular camera.

[0058] (2) Solve the target pose of the end effector;

[0059] In the robot coordinate system, the planned position P0 and the execution direction R0 that the end effector needs to reach are fixed, but the poses corresponding to the planned position P0 and the execution direction R0 are diverse, that is, in the robot coordinate system, the planned position P0 and the execution direction R0 are fixed, but the end effector can rotate around its own central axis (parallel to the execution direction R0, that is, the z-axis of the aforementioned established end effector coordinate system) to obtain countless poses that satisfy the planned position P0 and the execution direction R0. Due to the problems of robotic arm motion planning and the recognition accuracy of the end effector tracker, when the end effector needs to be driven to some of the above poses, there may be a situation where a certain joint on the robotic arm is in a singular pose and cannot reach, there may be a collision between the robotic arm and the patient, and the recognition accuracy of the tracker cannot meet the usage requirements. Then, these poses are considered invalid poses. Before performing the operation, it is necessary to rotate the end effector around the central axis of the end effector to update the current pose of the end effector and simultaneously update the end effector coordinate system while ensuring that the planned position P0 and the execution direction R0 are fixed until the robotic arm can drive the end effector to reach the current target pose and the recognition accuracy of the tracker meets the usage requirements, and then define this current pose as the target pose of the end effector;

[0060] As Figure 3 shown, the steps to solve the target pose are specifically as follows:

[0061] (21) Use the initial target pose T obtained in step (1) r0 as the current target pose T of the end effector r ;

[0062] (22) Calculate the angle between the normal direction of each group of end effectors and the normal direction of the optical tracking system when the end effector is in the current target pose T r and determine whether one of the angles is within the effective angle range. If so, go to step (23); otherwise, go to step (24);

[0063] In the embodiments disclosed by the present invention, the optical tracking system is a binocular camera. As Figure 4 shown, for the maximum visible half-angle β between the optical tracking system and a set of end-effectors facing it directly, the relationship between it and the maximum effective angle α is α = β - atan(A / Z), where A is half of the binocular camera spacing, and Z is the distance between the center point of the binocular camera and the center point of this set of end-effectors. When the angle between the normal direction of the end-effectors and the normal direction of the binocular camera is less than the maximum effective angle α, the recognition accuracy of the end-effectors meets the usage requirements;

[0064] In the embodiments disclosed by the present invention, when the end-effector is in the initial target pose, the end-effectors set on the B surface face the binocular camera directly. The maximum visible half-angle β of the binocular camera is 60°, A = 250 mm, and Z = 1900 mm. Therefore, the maximum effective angle α = 52.5° can be calculated, and the maximum effective angle range is [-52.5°, 52.5°]. Considering that the smaller the effective angle range, the higher the accuracy, in the specific embodiments of the present invention, the effective angle range is set within 30°, that is, the effective angle range is [-30°, 30°];

[0065] (23) Perform inverse kinematics solution for the robot manipulator with the current target pose T of the end-effector r ; if the inverse kinematics solution of the robot manipulator has a solution, then determine that the robot manipulator can drive the end-effector to reach the current target pose T r , and determine this current target pose T r as the target pose of the end-effector, and accordingly control the robot manipulator to execute the motion; if there is no inverse solution, then go to step (24);

[0066] Specifically, the inverse kinematics solution of the robot manipulator is as follows:

[0067] Establish a robot base coordinate system with the robot base joint, then the pose of the end-effector can be converted into the transformation relationship between the end-effector coordinate system and the robot base coordinate system. And according to the current target pose of the end-effector, the current end-effector coordinate system can be obtained, that is, calculate the transformation matrix M from the robot base coordinate system to the current end-effector coordinate system. Through the transformation matrix M, it can be determined whether the robot manipulator has an inverse solution;

[0068] (24) Rotate the z-axis of the current end-effector coordinate system (that is, the current target pose of the end-effector rotates around the rotation axis determined by the foregoing planned position P0 and execution direction R0) with a set rotation step to update the current end-effector coordinate system, and return to step (22);

[0069] In the present invention, the set rotation step is 1°;

[0070] The iterative process of rotating the current target pose around its rotation axis can be carried out in the following manner:

[0071] First, taking the initial target pose as a reference, first rotate forward by α + θ one by one around its z-axis with a set iterative step size, and then return to the reference and rotate backward by -(α + θ) one by one with the iterative step size;

[0072] Second, taking the initial target pose as a reference, first rotate unidirectionally around its rotation axis to α one by one with the iterative step size, and then return to the reference and rotate backward around its rotation axis to -α one by one with the iterative step size; then, taking the initial target pose as a reference, rotate forward or backward around its rotation axis by θ at one time, and taking the pose after rotating by θ as a reference, rotate forward around its rotation axis to α one by one with the iterative step size, and then return to the reference and rotate backward around its rotation axis to -α one by one;

[0073] In the first and second methods, the angle is traversed by rotating unidirectionally one by one with the iterative step size. In other embodiments, the angle can be traversed by rotating with the iterative step size and alternating directions each time;

[0074] In the present invention, there are 3 end effectors; further, α = 30°, θ = 90°;

[0075] (25) Repeat steps (22) to (24) until the target pose of the end effector is obtained or all valid included angles are traversed. If all valid included angles are traversed and there is no solution to the inverse kinematics of the robotic arm, it is determined that there is no reachable target pose for the end effector.

[0076] In the embodiments disclosed in the present invention, first execute step (22) to determine whether one of the included angles between the normal direction of each group of end effectors and the normal direction of the optical tracking system is within the valid included angle range when the end effector is in the current target pose, and then execute step (23) to determine whether there is a solution to the inverse kinematics of the robotic arm when the end effector is in the current target pose on the premise that a certain included angle is within the valid included angle range; in other embodiments, step (23) can be executed first, and then step (22).

[0077] In the robotic navigation method based on optical tracking of the present invention, in the process of solving the target pose of the end effector, it can always be ensured that the included angle between the normal direction of a certain plane in the plane where the end tracer is located and the normal direction of the optical tracking system is within the valid included angle range, which simplifies the process of solving the target pose of the end effector.

[0078] The preferred embodiments of the present invention have been described in detail above, but the present invention is not limited to the specific details in the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solutions of the present invention, and these equivalent transformations all belong to the protection scope of the present invention.

Claims

1. A robot navigation method based on optical tracking, characterized in that: An end effector is provided at the end of the robot. The end effector holds an end tool, and several groups of end trackers are installed on the end effector, including the steps: (1) According to the planned path and the reserved safety distance, obtain the planned position and execution direction that the end tool needs to reach; Transform the planned position and execution direction that the end tool needs to reach into the coordinate system of the optical tracking system to obtain the position coordinate P1 and direction vector R1 of the end tool in the coordinate system of the optical tracking system; Construct a vector V0 with the position coordinate P1 and the center point of the binocular camera; Construct a vector V1 with the position coordinate P1 and the direction vector R1; There are two vectors perpendicular to the vector V1 and passing through the position coordinate P1 on the plane formed by the vector V0 and the vector V1. Select the vector with a smaller included angle with the vector V0 as the vector V2; Obtain a vector V3 by cross-multiplying the vector V1 and the vector V2; Establish an end tool coordinate system with the position coordinate P1 as the origin, the vector V1 as the z-axis, the vector V2 as the y-axis, and the vector V3 as the x-axis. Take the pose of the end tool corresponding to when the end tool coordinate system is established as the initial pose of the end tool in the coordinate system of the optical tracking system, transform the initial pose into the robot coordinate system to obtain the initial target pose of the end tool, and use it as the current target pose; (2) When it is judged that the end tool is in the current target pose, determine whether the included angle between the normal direction of a certain group of end trackers and the normal direction of the binocular camera is within the set effective included angle range and whether the inverse kinematic solution of the robotic arm has a solution; (3) If the aforementioned included angle is within the effective included angle range and the inverse kinematic solution of the robotic arm has a solution, take the current target pose as the target pose; if there is no inverse solution or no included angle is within the effective included angle range, rotate the current target pose with the execution direction as the rotation axis by the set step size and update the current target pose, and repeat steps (2) to (3) until the target pose is obtained or the set angle is traversed.

2. The robot navigation method according to claim 1, wherein: The specific set effective included angle range is: Calculate the maximum visible half-angle β between the binocular camera and the end tracker according to the positions and installation parameters of the binocular camera and the end tracker. The relationship between it and the maximum effective included angle α is α = β – atan(A / Z), where A is half of the distance between the binocular cameras, and Z is the distance between the center point of the binocular camera and the center point of the end tracker. The effective included angle range is [-α, α].

3. The robot navigation method according to claim 1, wherein: The set effective included angle range is [-30°, 30°].

4. The robot navigation method according to claim 1, wherein: The set step size is 1°.

5. The robot navigation method according to claim 1, characterized in that: The rotation of the current target pose with the execution direction as the rotation axis by the set step size is specifically: Based on the initial target pose, rotate the current target pose to traverse the set angles ±(α + θ), where α is the maximum effective included angle and θ is the included angle between the normal directions of the planes where two adjacent groups of end trackers are located.

6. The robot navigation method according to claim 1, wherein: The rotation of the current target pose with the execution direction as the rotation axis by the set step size is specifically: Based on the initial target pose, first rotate the current target pose one by one with the iterative step size to traverse the set angles ±α; Then, taking the initial target pose as a reference, rotate θ forward or backward in one go. Taking the pose after rotating θ as a new reference, rotate with an iterative step size to traverse the angle ±α, where α is the maximum effective included angle and θ is the normal included angle between the planes where two adjacent sets of end effectors are located.

7. The robot navigation method according to claim 1, wherein: Three sets of end effectors are provided, and each set of end effectors is respectively arranged on different planes. The three planes are arranged adjacent to each other circumferentially, and the normal included angle between two adjacent planes is 90°.

Images