Method for measuring pose of robotic end tool

Abstract

A method for measuring a pose of a robotic end tool, including: obtaining a three-dimensional feature of a flange and a three-dimensional feature of the end tool, establishing a first coordinate system and a second coordinate system at the center of the flange and the center of the end tool respectively, calculating a positional offset of the second coordinate system relative to the first coordinate system, and calculating a rotation offset of the second coordinate system relative to the first coordinate system according to each unit vector of the second coordinate system, so as to obtain a pose of the end tool relative to the flange. The method provided in the present application, compared with the manual observation method, the precision and stability of the pose measurement method are high.

Claims

1. A method for measuring a pose of a robotic end tool, comprising: obtaining a three-dimensional feature of a flange for clamping the end tool and a three-dimensional feature of the end tool; determining a center of the flange according to the three-dimensional feature of the flange, determining a center of the end tool according to the three-dimensional feature of the end tool; establishing a first coordinate system and a second coordinate system based on the center of the flange and the center of the end tool acting as the origin of first coordinate system and the origin of the second coordinate system respectively; calculating a positional offset of the origin of the second coordinate system relative to the origin of the first coordinate system; in the first coordinate system, calculating a unit vector in a positive direction of the X-axis, a unit vector in a positive direction of the Y-axis, and a unit vector in a positive direction of the Z-axis of the second coordinate system; and collectively forming a posture transformation matrix of the second coordinate system by the unit vector of the positive direction of the X-axis, the unit vector of the positive direction of the Y-axis, and the unit vector of the positive direction of the Z-axis of the second coordinate system, and calculating a rotation offset of the second coordinate system relative to the first coordinate system by the posture transformation matrix.

2. The method of claim 1, wherein the step of calculating the positional offset of the origin of the second coordinate system relative to the origin of the first coordinate system particularly comprising: defining the origin of the first coordinate system as 0.sub.1, defining the origin of the second coordinate system as 0.sub.2, and defining a coordinate value of 0.sub.1 in the first coordinate system as (0, 0, 0), and defining a coordinate value of 0.sub.2 in the first coordinate system as (x.sub.0, y.sub.0, z.sub.0); and calculating the positional offset as Δx=x.sub.0, Δy=y.sub.0, Δz=z.sub.0, wherein the Δx is the positional offset of the second coordinate system relative to the first coordinate system in the X direction, the Δy is the positional offset of the second coordinate system relative to the first coordinate system in the Y direction, and the Δz is the positional offset of the second coordinate system relative to the first coordinate system in the Z direction.

3. The method of claim 2, wherein the step that in the first coordinate system, calculating a unit vector in a positive direction of the X-axis of the second coordinate system, a unit vector of in positive direction of the Y-axis, and a unit vector of in positive direction of the Z-axis specifically comprising: taking points P.sub.1 and P.sub.2 on the X-axis and the Y-axis of the second coordinate system respectively, and defining coordinate values of P.sub.1 and P.sub.2 in the first coordinate system as (x.sub.1, y.sub.1, z.sub.1) and (x.sub.2, y.sub.2, z.sub.2) respectively; calculating the vector {right arrow over (O.sub.2P.sub.1)}=(x.sub.1−x.sub.0, y.sub.1−y.sub.0, z.sub.1−z.sub.0), and calculating the vector {right arrow over (O.sub.2P.sub.2)}=(x.sub.2−x.sub.0, y.sub.2−y.sub.0, z.sub.2−z.sub.0); calculating a unit vector {right arrow over (X)} of the {right arrow over (O.sub.2P.sub.1)} as $\left(\frac{x_1-x_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},\frac{y_1-y_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},\frac{z_1-z_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}}\right)$ wherein the {right arrow over (X)} is a unit vector of the second coordinate system in the positive direction of the X-axis; calculating a unit vector {right arrow over (Y)} of the {right arrow over (O.sub.2P.sub.2)} as $\left(\frac{x_2-x_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},\frac{y_2-y_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},\frac{z_2-z_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_2-z_0\right)}^2}}\right)$ wherein the {right arrow over (Y)} is a unit vector of the second coordinate system in the positive direction of the Y-axis; and calculating a unit vector {right arrow over (Z)} of the second coordinate system in the positive direction of the Z-axis as {right arrow over (X)}×{right arrow over (Y)}.

4. The method of claim 1, wherein the unit vector in the positive direction of the X-axis of the second coordinate system is defined as (a.sub.1, b.sub.1, c.sub.1), and the unit vector in the positive direction of the Y-axis of the second coordinate system is defined as (a.sub.2, b.sub.2, c.sub.2), the unit vector in the positive direction of the Z-axis of the second coordinate system is defined as (a.sub.3, b.sub.3, c.sub.3), and the posture transformation matrix is calculated to be $\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right].$

5. The method of claim 4, wherein the rotation offsets are $R_x={\tan }^{-1}\frac{b_1}{a_1},R_y={\tan }^{-1}\frac{-c_1}{\sqrt{a_1^2+b_1^2}},R_z={\tan }^{-1}\frac{c_2}{c_3},$ wherein the R.sub.x is a rotational offset of the second coordinate system relative to the first coordinate system on the X-axis, and the R.sub.y is a rotational offset of the second coordinate system relative to the first coordinate system on the Y-axis, the R.sub.z is a rotational offset of the second coordinate system relative to the first coordinate system on the Z-axis.

6. The method of claim 1, wherein the three-dimensional features of the flange and the end tool are obtained by a binocular three-dimensional scanner.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In order to make the technical solutions in the embodiments of the present application clearer, the accompanying drawings to be used in the embodiments and the description of the prior art will be briefly introduced below, it is apparent that the drawings in the following description are merely some embodiments of the present application and that other drawings may be obtained by those skilled in the field without departing from the inventive nature of the application.

(2) FIG. 1 is a flowchart of implement of a method for measuring the pose of a robotic end tool according to an embodiment of the present application;

(3) FIG. 2 is a schematic diagram of a method for measuring the pose of a robotic end too according to an embodiment of the present application;

(4) FIG. 3 is a schematic diagram of a first coordinate system and a second coordinate system used in the embodiment of the present application.

(5) In the drawings, the following reference numerals are used: 1—binocular three-dimensional scanner; 2—robot; 21—flange; 210—first coordinate system; 22—end tool; and 220—second coordinate system.

DETAILED DESCRIPTION

(6) In order to make the technical problems to be solved, technical solutions, and beneficial effects of the present application clearer and more understandable, the present application will be further described in detail hereinafter with reference to the accompanying drawings and embodiments. It should be understood that the embodiments described herein are only intended to illustrate but not to limit the present application.

(7) It should be noted that when a component is referred to as being “fixed to” or “disposed on” another component, it can be directly or indirectly on another component. When a component is referred to as being “connected to” another component, it can be directly or indirectly connected to another component.

(8) It should be understood that, “length”, “width”, “upper”, “lower”, “front”, “back”, “left” and “right”, “vertical”, “horizontal”, “top”, “bottom”, “inside”, “outside” and other terms indicating the orientation or positional relationship are based on orientation or positional relationship shown in the drawings, and are only for the purpose of facilitating the description of the application and simplifying the description, instead of indicating or implying that the indicated device or component must have a specific orientation and constructed and operated in a particular orientation, and therefore it cannot be construed as limitation of the application.

(9) In addition, the terms “first” and “second” are for illustrative purposes only and should not be construed as indicating or implying a relative importance or implicitly indicating the quantity of technical features indicated. Therefore, a feature that defines “first” and “second” may expressly or implicitly include one or more of the features. In the description of the present application, “multiple” means two or more than two, unless otherwise specifically defined.

(10) Please refer to FIG. 1 and FIG. 2, the method for measuring the pose of the robotic end tool provided by the present application will now be described. The robot 2 includes a robot arm, a flange 21 fixed to the robot arm, and an end tool 22 clamped at the flange 21, and the end tool 22 comprises a welding gun, a cutter, and the like. The pose of the robotic end tool refers to the position and posture of the end tool 22 in the specified coordinate system, and the position and posture of the end tool 22 can be used to represent as the position offset and the rotational offset of the end tool 22 relative to the specified coordinate system respectively. The method for measuring the pose comprises the following steps:

(11) Step S101, which is specifically as follows:

(12) a three-dimensional feature of the flange 21 for clamping the end tool 22 and a three-dimensional feature of the end tool 22 are obtained, that is, by obtaining an external contour lattice of the flange 21 and the end tool 22, the external contour lattice of the flange 21 are combined to form the three-dimensional feature of the flange 21, the external contour lattices of the end tool 22 are combined to form a three-dimensional feature of the end tool 22.

(13) Further, in step S101, the three-dimensional feature of the flange 21 and the end tool 22 are obtained by the binocular three-dimensional scanner 1. When the binocular three-dimensional scanner 1 is used, the binocular three-dimensional scanner 1 is placed in the vicinity of the robotic end tool 22, and the external contour features of the end tool 22 and the flange 21 are scanned to obtain the external contour lattice of the end tool 22 and the flange 21, then upload the data of the external contour lattice to a reverse engineering software, the unneeded data are deleted using the reverse engineering software, and then the line fitting and surface fitting of the remaining lattice data are performed, and finally the three-dimensional feature is formed through the fitted line and surface.

(14) Step S102, which is specifically as follows:

(15) a center of the flange 21 is determined according to the three-dimensional feature of the flange 21, a center of the end tool 22 is determined according to the three-dimensional feature of the end tool 22. When determining the center of the flange 21 and the center of the end tool 22, the external contour lattices are firstly processed by reverse engineering software such as Germanic, Image ware or the like, the unnecessary lattice data are deleted, and then line fitting and surface fitting of the remaining lattice data are performed, and finally the center of the end tools 22 and the center of the flange 21 are determined through the fitted line and surface.

(16) Step S103, which is specifically as follows:

(17) a first coordinate system 210 and a second coordinate system 220 are established based on the center of the flange 21 and the center of the end tool 22 respectively, and the first coordinate system 210 and the second coordinate system 220 are Cartesian rectangular coordinate systems including mutually perpendicular X-axis, Y-axis and Z-axis;

(18) Step S104, which is specifically as follows:

(19) a positional offset of an origin of the second coordinate system 220 relative to an origin of the first coordinate system 210 is calculated, herein, the positional offset refers to positional offsets of the origin of the second coordinate system 220 relative to the origin of the first coordinate system 210 on the X-axis, the Y-axis, and the Z-axis, respectively.

(20) Further, in step S104, as shown in FIG. 3, the origin of the first coordinate system 210 is defined as 0.sub.1, the origin of the second coordinate system 220 is defined as 0.sub.2, the coordinate value of 0.sub.1 in the first coordinate system 210 is defined as (0, 0, 0), and the coordinate value of 0.sub.2 in the first coordinate system 210 is defined as (x.sub.0, y.sub.0, z.sub.0), the positional offset of the origin of the second coordinate system 220 relative to the origin of the first coordinate system 210 is defined as (Δx, Δy, Δz), wherein the Δx is the positional offset of the second coordinate system 220 relative to the first coordinate system 210 in the X direction, the Δy is the positional offset of the second coordinate system 220 relative to the first coordinate system 210 in the Y direction, and the Δz is the positional offset of the second coordinate system 220 relative to the first coordinate system 210 in the Z direction. So that Δx=x.sub.0−0=x.sub.0, Δy=y.sub.0−0=y.sub.0, Δz=z.sub.0−0=z.sub.0, it can be concluded that the positional offset of the end tool 22 relative to the flange 21 is (x.sub.0, y.sub.0, z.sub.0).

(21) Step S105, which is specifically as follows:

(22) In the first coordinate system 210, a unit vector of the positive direction of the X-axis of the second coordinate system 220, a unit vector of the positive direction of the Y-axis, and a unit vector of the positive direction of the Z-axis are calculated, the first coordinate system 210 is used as a reference coordinate system, and the straight lines of the X-axis, the Y-axis, and the Z-axis of the second coordinate system 220 all have a certain direction. The respective directions of the X-axis, the Y-axis, and the Z-axis of the second coordinate system 220 can be represented by direction vectors, and the unit vectors of the X-axis, the Y-axis, and the Z-axis can be obtained by dividing each direction vector by the respective modular length.

(23) Further, in step S105, please refer to FIG. 3, points P.sub.1 and P.sub.2 are respectively taken from the X-axis and the Y-axis of the second coordinate system 220 respectively, and the coordinate values of P.sub.1 and P.sub.2 in the first coordinate system 210 are defined as (x.sub.1, y.sub.1, z.sub.1) and (x.sub.2, y.sub.2, z.sub.2) respectively; P.sub.1 and P.sub.2 are any points on the X-axis and the Y-axis respectively; the vector direction of the directed line segment O.sub.2P.sub.1 is the same as the vector direction of the positive direction of the X-axis of the second coordinate system 220, and the vector direction of the directed line segment O.sub.2P.sub.2 is the same as the vector direction of the positive direction of the Y-axis of the second coordinate system 220,

(24) So calculating the vector {right arrow over (O.sub.2P.sub.1)}=(x.sub.1−x.sub.0, y.sub.1−y.sub.=0, z.sub.1−z.sub.0), the vector {right arrow over (O.sub.2P.sub.2)}=(x.sub.2−x.sub.0, y.sub.2−y.sub.0, z.sub.2−z.sub.0); and a unit vector {right arrow over (X)} of the {right arrow over (O.sub.2P.sub.1)} as

(25) $\stackrel{.fwdarw.}{X}=\left(\frac{x_1-x_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},\frac{y_1-y_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},\frac{z_1-z_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}}\right)$
a unit vector {right arrow over (Y)} of the {right arrow over (O.sub.2P.sub.2)} as

(26) $\stackrel{.fwdarw.}{Y}=\left(\frac{x_2-x_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},\frac{y_2-y_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},\frac{z_2-z_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}}\right)$
wherein the {right arrow over (X)} is the unit vector of the second coordinate system 220 in the positive direction of the X-axis, the {right arrow over (Y)} is the unit vector of the second coordinate system 220 in the positive direction of the Y-axis; the unit vector {right arrow over (X)} and the unit vector {right arrow over (Y)} are perpendicular to each other, according to the expressions of the {right arrow over (X)} vector and the {right arrow over (Y)} vector, a {right arrow over (Z)} vector perpendicular to the plane in which the {right arrow over (X)} vector and the {right arrow over (Y)} vector are located can be calculated, and the {right arrow over (Z)} vector is a unit vector in the positive direction of the Z-axis in the second coordinate system 220, supposing {right arrow over (X)}=(a.sub.1,b.sub.1,c.sub.1), Y=(a.sub.2,b.sub.2,c.sub.2), wherein:

(27) $a_1=\frac{x_1-x_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},b_1=\frac{y_1-y_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},$

(28) $c_1=\frac{z_1-z_0}{\sqrt{{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2+{\left(z_1-z_0\right)}^2}},a_2=\frac{x_2-x_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},b_2=\frac{y_2-y_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},c_2=\frac{z_2-z_0}{\sqrt{{\left(x_2-x_0\right)}^2+{\left(y_2-y_0\right)}^2+{\left(z_2-z_0\right)}^2}},$
so {right arrow over (Z)}={right arrow over (X)}×{right arrow over (Y)}=(b.sub.1,c.sub.2−b.sub.2c.sub.1,a.sub.1c.sub.2−a.sub.2c.sub.1,a.sub.1b.sub.2−a.sub.2b.sub.1), according to the above steps, the unit vector of the X-axis positive direction, the Y-axis positive direction, and the Z-axis positive direction in the second coordinate system 220 can be obtained.

(29) Step S106 specifically is:

(30) The unit vector of the positive direction of the X-axis, the unit vector of the positive direction of the Y-axis, and the unit vector of the positive direction of the Z-axis of the second coordinate system 220 together form a posture transformation matrix of the second coordinate system 220, and a rotation offset of the second coordinate system 220 relative to the first coordinate system 210 is calculated by the posture transformation matrix.

(31) Further, in step S106, the unit vector of the positive direction of the X-axis of the second coordinate system 220 is defined as {right arrow over (X)}=(a.sub.1,b.sub.1,c.sub.1), and the unit vector of the positive direction of the Y-axis of the second coordinate system 220 is defined as Y=(a.sub.2,b.sub.2,c.sub.2), the unit vector of the positive direction of the Z-axis of the second coordinate system 220 is defined as {right arrow over (Z)}=(a.sub.3,b.sub.3,c.sub.3), wherein a.sub.3=b.sub.1c.sub.2−b.sub.2c.sub.1, b.sub.3=a.sub.1c.sub.2−a.sub.2c.sub.1, c.sub.3=a.sub.1b.sub.2−a.sub.2b.sub.1, and the posture transformation matrix is calculated as

(32) 0$\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]=\left[\begin{array}{ccc}{a}_1& a_2& b_1{c}_2-b_2{c}_1\\ b_1& b_2& a_1{c}_2-a_2{c}_1\\ c_1& c_2& a_1{b}_2-a_2{b}_1\end{array}\right],$
the posture transformation matrix may represent the posture transformation of the second coordinate system 220 relative to the first coordinate system 210, that is, the posture transformation of the end tool 22 relative to the flange 21 may be indicated.

(33) Further, in step S106, according to the posture transformation matrix can obtain, the rotation offset of the second coordinate system 220 relative to the first coordinate system 210, the rotation offset are

(34) $R_x={\tan }^{-1}\frac{b_1}{a_1},R_y={\tan }^{-1}\frac{-c_1}{\sqrt{a_1^2+b_1^2}},R_z={\tan }^{-1}\frac{c_2}{c_3},$
wherein the R.sub.x is a rotational offset of the second coordinate system 220 relative to the first coordinate system 210 on the X-axis, and the R.sub.y is a rotational offset of the second coordinate system 220 relative to the first coordinate system 210 on the Y-axis, the R.sub.z is a rotational offset of the second coordinate system 220 relative to the first coordinate system 210 on the Z-axis, and then substituting a.sub.3=b.sub.1c.sub.2−b.sub.2c.sub.1, b.sub.3=a.sub.1c.sub.2−a.sub.2c.sub.1, c.sub.3=a.sub.1b.sub.2−a.sub.2b.sub.1 into the expression of the rotation offset, which can obtain

(35) $R_x={\tan }^{-1}\frac{b_1}{a_1},R_y={\tan }^{-1}\frac{-c_1}{\sqrt{a_1^2+b_1^2}},R_z={\tan }^{-1}\frac{c_2}{a_1{b}_2-a_2{b}_1},$
wherein a.sub.1, b.sub.1, c.sub.1, a.sub.2, b.sub.2, c.sub.2 are obtained when solving the unit vector of the positive direction of the X-axis and the positive direction of the Y-axis in the second coordinate system 220, and substituting the value of a.sub.1, b.sub.1, c.sub.1, a.sub.2, b.sub.2 and c.sub.2 into the expression of the rotation offset defined as get the result of the final rotation offset.

(36) The method for measuring the pose of the robotic end tool provided by the present application, the method establishes a first coordinate system 210 and a second coordinate system 220 at the center of the flange 21 and the center of the end tool 22 respectively, calculates a positional offset of the origin of the second coordinate system 220 relative to the origin of the first coordinate system 210, and calculates a rotational offset of the second coordinate system 220 relative to the first coordinate system 210 by the posture transformation matrix of the second coordinate system 220, therefore obtains the pose of the end tool 22 relative to the flange 21, the method for measuring the pose obtains the pose of the end tool 22 by calculating the relative position and relative posture of the end tool 22 and the flange 21, and the precision and stability of the pose measurement method are high.

(37) It's obvious that the aforementioned embodiments are only preferred embodiments of the present application, and are not intended to limit the present application. Any modification, equivalent replacement, improvement, and so on, which are made within the spirit and the principle of the present application, should be comprised in the scope of the present application.