UNIVERSITร DEGLI STUDI DI PADOVA
D
IPARTIMENTO DI
I
NGEGNERIA DELL
โI
NFORMAZIONE
T
ESI DI
L
AUREA
M
AGISTRALE IN
I
NGEGNERIA
I
NFORMATICA
An Autonomous System for the Evaluation of
a Robot Tool Center Point using Computer
Abstract
The main purpose of this thesis is to study a method for finding the geometric relationship between the TCP (tool center point) of a tool attached to the robot flange (the last axis) and the robot itself using a camera.
The proposed method requires the rotation matrix between the reference systems related to the robot and the camera and an initial tool positioning moving the robot using the manual control system in order to make the TCP detectable by the camera.
The image processing algorithms were designed to find the TCP on the image of any symmetric tools. Once identified the TCP on the image, the method to find the geometric relationship between robot and the three-dimensional TCP can be applied to any tool.
From a theoretical point of view the TCP computed is not an approximation. Any error is due to intrinsic system errors as camera discretization, robot pose
accuracy and image processing algorithms accuracy.
Contents
1 Introduction ... 1
Background ... 1
Objectives ... 1
State of the art ... 2
1.3.1 Electronic calibration systems ... 2
1.3.2 Camera based calibration systems ... 3
System setup ... 3
Organisation of the document ... 5
2 Robotics ... 7
KUKA Agilus1100 sixx ... 7
2.1.1 Description ... 7
2.1.2 Technical data ... 8
Euclid Labs Simulator ... 10
Coordinate Systems ... 11
2.3.1 Tool Center Point ... 11
2.3.2 Robot Base frame ... 11
2.3.3 Wrist frame ... 12
2.3.4 Tool frame ... 13
3 Image processing ... 15
Pinhole camera model ... 15
Image processing algorithms ... 17
3.2.1 Grayscale histogram ... 17
3.2.2 Binary image ... 17
3.2.3 Gaussian filter ... 18
3.2.4 Sobel operator ... 18
3.2.5 Cannyโs Edge Detection ... 19
3.2.6 Hough transform ... 21
3.2.7 Thinning ... 23
3.2.8 Homotopic skeleton detection ... 23
Key point detection ... 25
3.3.1 Tool direction ... 25
3.3.2 TCP localization ... 27
3.3.3 TCP optimisation ... 28
4 TCP calibration ... 29
Minimum number of equations ... 29
Camera Base calibration ... 34
4.3.1 Kuka base calibration ... 35
4.3.2 Base calibration using camera images ... 35
TCP positioning with camera ... 36
TCP computation ... 37
4.5.1 Least squares method ... 38
4.5.2 Analytic solution ... 38 Error analysis ... 44 5 Results ... 47 Image resolution ... 47 TCP accuracy... 47 TCP precision ... 55 6 Conclusions... 61 Results ... 61 Future Works ... 62 A. Appendix ... 63
A.1 Least squares method with normal equations ... 63
List of Figures
Figure 1.1 โ Euclid Labs logo... 2
Figure 1.2 โ Leoni TCP calibration system ... 2
Figure 2.1 โ Kuka Agiuls1100 sixx ... 7
Figure 2.2 โ Kuka Agiuls1100 sixx size and joint values ... 10
Figure 2.3 โ Euclid Labs simulator ... 11
Figure 2.4 โ Robot base frame ... 12
Figure 2.5 โ Wrist frame ... 12
Figure 2.6 โ Tool frame ... 13
Figure 3.1 โ Pinhole camera model ... 15
Figure 3.2 โ Point projection on image plane ... 16
Figure 3.3 - Histogram ... 17
Figure 3.4 โ Binary image ... 20
Figure 3.5 โ Edge found with Cannyโs edge detection ... 20
Figure 3.6 - Hough transform line representation ... 21
Figure 3.7 โ Edge image ... 23
Figure 3.8 โ Lines found with Hough transform ... 23
Figure 3.9 โ Source image ... 24
Figure 3.10 โ Homotopic skeleton ... 24
Figure 3.11 โ Source image ... 25
Figure 3.12 โ Wrong skeleton detection ... 25
Figure 3.13 โ Correct skeleton detection ... 25
Figure 3.14 - Line direction ... 25
Figure 3.15 โ Source image ... 26
Figure 3.16 โ Wrong tool directon ... 26
Figure 3.17 โ Source image ... 27
Figure 3.18 โ Skeleton direction found ... 27
Figure 3.19 โ Keypoint without tcp optimisation ... 28
Figure 3.20 โ Keypoint with tcp optimisation ... 28
Figure 4.1 โ Axis-angle representation ... 30
Figure 4.2 โ Rotation between two coordinate system ... 31
Figure 5.1 โ Accuracy x y axis ... 48
Figure 5.2 โ Accuracy y z axis ... 48
Figure 5.3 โ Accuracy x z axis ... 49
Figure 5.4 โ Accuracy x y z axis ... 49
Figure 5.6 โ Accuracy y axis... 50
Figure 5.7 โ Accuracy z axis ... 50
Figure 5.8 โ Accuracy - Distance of each ๐๐ถ๐๐ฅ found from ๐๐ถ๐๐๐๐๐๐ฅ ... 51
Figure 5.9 โ Accuracy - Distance of each ๐๐ถ๐๐ฆ found from ๐๐ถ๐๐๐๐๐๐ฆ ... 51
Figure 5.10 โ Accuracy - Distance of each ๐๐ถ๐๐ง found from ๐๐ถ๐๐๐๐๐๐ง ... 51
Figure 5.11 โ Accuracy - Distances histogram from the mean point ๐๐ถ๐๐๐๐๐ ... 55
Figure 5.12 โ Distance of each ๐๐ถ๐๐ฆ found from ๐๐ถ๐๐๐๐๐๐ฆ... 55
Figure 5.13 โ Precision x y axis ... 56
Figure 5.14 โ Precision y z axis ... 56
Figure 5.15 โ Precision x z axis ... 56
Figure 5.16 โ Precision x y z axis ... 56
Figure 5.17 โ Precision x axis ... 56
Figure 5.18 โ Precision y axis ... 56
Figure 5.19 โ Precision z axis ... 57
Figure 5.20 โ Precision - Distance of each ๐๐ถ๐๐ฅ found from ๐๐ถ๐๐๐๐๐๐ฅ ... 57
Figure 5.21 โ Precision - Distance of each ๐๐ถ๐๐ฆ found from ๐๐ถ๐๐๐๐๐๐ฆ ... 57
Figure 5.22 โ Precision - Distance of each ๐๐ถ๐๐ง found from ๐๐ถ๐๐๐๐๐๐ง ... 58
Figure 5.23 โ Accuracy - Distances histogram from the mean point ๐๐ถ๐๐๐๐๐ ... 60
1
1 I
NTRODUCTION
B
ACKGROUNDUsually robots delivered by the manufacturer are calibrated during production. The calibration procedure allows to know position and orientation of each axis during a movement.
On last axis (tool ๏ฌange) can be attached any tool. A point called TCP (tool center point) has to be calibrated to move the robot with respect to the tool assembled: it is the origin of a reference system representing the tool position and orientation. For each tool can be defined a different TCP, meaning that changing a tool the relative calibration has to be performed. Furthermore, while the robot is moving, it could collide with something thus requiring a new TCP calibration.
Typically each robot has a manual procedure to calibrate the TCP. It can be computed using manual move controls to define four different positions keeping the TCP on the same point: for each combination of three of the four flange poses, a sphere whose center should be the TCP is computed. Then the TCP estimation will be the point that minimize the distance from each sphere center found. The result of this method depends on the operator skill to positioning the robot on the different poses keeping the TCP on the same position. Usually the accuracy achievable is around 0,5๐๐.
O
BJECTIVESThe company Euclid Labs requires an autonomous method to calibrate the TCP of symmetric tools using a camera. The Objective is to improve the accuracy of the manual method.
2
Figure 1.1 โ Euclid Labs logo
The final demonstration software has to be a window in which every robot movements have to be simulated before the real execution.
S
TATE OF THE ART1.3.1 Electronic calibration systems
Some manufacturers develop tool calibration system for robots that uses rotation-symmetric tools. These systems measure the toolโs position electronically across all six dimensions and automatically adjusts the toolโs position using an in-line sensor system. On Figure 1.2 is shown advintec TCP-3D designed by Leoni that is an istance of these kind of systems.
3
There are several problems regarding these kind of systems that lead to search for others solutions:
๏ท the workspace is restricted
๏ท there is no user interface
๏ท โblack boxโ system
1.3.2 Camera based calibration systems
There are some patents with several solutions to solve this problem using a camera but even in this case there are some restrictions. The common basic idea is to move the flange in at least three different poses keeping the TCP on the same point and then compute the relationship between TCP and flange defining and solving a set of constraints related to these flange poses.
The easier solution computes the TCP without approximations from a theoretical point of view but it requires two robot flange rotations of 90 degree around two perpendicular axis [ 3 ] [ 4 ]. The main problem of this solution is that there could not be enough space to perform this kind of rotations and an initial TCP approximation is required.
Another solution to compute the TCP makes use of mean squares error algorithm [ 2 ] [ 8 ]. In this case there is no restrictions about the rotation angles amplitude and it can be applied even without initial TCP approximations but to reach a reasonable accuracy many flange poses has to be used to define the system of equations.
A third solution computes the TCP defining a set of equations using several flange poses and keeping the TCP on the camera optical axis [ 2 ]. This method requires at least four flange poses and allows to define system of second order equations. This method has no approximations from a theoretical point of view but second order constraints lead to high intrinsic system errors amplification as camera discretization and robot pose inaccuracy.
S
YSTEM SETUP4
๏ท Manipulator: KUKA AGILUS1100 SIXX
๏ท Camera: Blaser Ace acA2040-25gm (2048x2048 pixels)
๏ท Lens: Goyo GMTHR35028MCN (F/2.8 50mm)
๏ท Led backlight: PHLOX SLLUB (200 X 200 mm, IP-65)
๏ท PC: Asus asus n550jk (Intel Core i7-4700HQ (2.40 GHz))
Manipulator, camera and PC are connected to a local Ethernet network.
Manipulator and PC communicate reading and writing two shared variables stored on the robot control unit:
๏ท flag: a Boolean variable that will be set to ๐ก๐๐ข๐ when a new movement has to be performed. Once the movement has been performed it will be set to ๐๐๐๐ ๐.
๏ท pos: contains the manipulator position to reach. It can be expressed in different coordinate systems.
The PC handles the main software. It elaborates camera images and computes manipulator positions.
The following routine is used to command the manipulator from the PC: 1. set the new position on the variable pos
2. set to true the variable flag
3. suspend execution until the manipulator will set the variable flag to false. While the manipulator executes the following loop:
1. check the variable flag
2. if flag value is ๐ก๐๐ข๐ move the active TCP to the new position pos 3. set flag to ๐๐๐๐ ๐.
5
O
RGANISATION OF THE DOCUMENT0 Errore. Il risultato non
รจ valido per una tabella.
Describes the purpose of this thesis and the configuration of the devices used
1 Errore. Il risultato non
รจ valido per una tabella.
Detailed information about the manipulator and its reference systems and a brief description of the simulator used
3 Image processing All the theory regarding image processing algorithms used and the explanation of the method used to detect the TCP with the camera
4 TCP calibration Detailed analysis of the method used to calibrate the TCP including the interaction between software and camera
5 Results The results obtained with several test to analyse the precision and the accuracy of the method proposed
6 Conclusions Final discussion of the results achieved and possible future developments
A Appendix Explanation of the least squares method with normal equations
7
2 R
OBOTICS
KUKA
A
GILUS1100
SIXXThis small industrial robot is a 6-axis arm and it have been designed to achieve high velocity, at the expense of a reduced payload.
2.1.1 Description
The robot is composed of the following components visible in Figure 2.1: 1. In-line wrist A4, A5, A6
2. Arm A3 3. Link arm A2
4. Rotating column A1 5. Electrical installations 6. Base frame
Figure 2.1 โ Kuka Agiuls1100 sixx
In-line wrist A4, A5, A6 The robot is fitted with a 3-axis line wrist. The
8
Arm A3 The arm is the link between the in-line wrist and the link arm. The arm is driven by the motor of axis 3.
Link arm A2 The link arm is the assembly located between the arm and the rotating column. It houses the motor and gear unit of axis 2.
Rotating column A1 The rotating column houses the motors of axes 1 and 2. The rotational motion of axis 1 is performed by the rotating column.
Electrical installations The electrical installations include all the motor and control cables for the motors of axes 1 to 6. The electrical installations also include the RDC box, which is integrated into the robot.
Base frame The base frame is the base of the robot. Interface A1 is located at the rear of the base frame.
Table 2.1 - Kuka Agiuls1100 sixx components
2.1.2 Technical data
The robot joints are mechanically and software constrained. The maximum values are shown in Table 2.2, with the maximum speed that each axis can achieve.
Axis Range of motion,
software limited
Speed with rated payload 1 +/-170ยฐ 360 ยฐ/s 2 +45ยฐ to -190ยฐ 360 ยฐ/s 3 +156ยฐ to -120ยฐ 360 ยฐ/s 4 +/-185ยฐ 360 ยฐ/s 5 +/-120ยฐ 360 ยฐ/s 6 +/-350ยฐ 360 ยฐ/s
9
The KUKA Agilus Sixx 1100 technical data are listed in Table 2.3. The working area is defined by the size of the robot and the values which can assume the joints represented in Figure 2.2.
Max. reach 1,101 mm
Max. payload 10 kg
Pose repeatability ยฑ0.03 mm
Number of axes 6
Mounting position Floor, ceiling, wall
Robot footprint 209 mm ร 207 mm
Weight (excluding
controller), approx 54 kg
10
Figure 2.2 โ Kuka Agiuls1100 sixx size and joint values
E
UCLIDL
ABSS
IMULATOR11
Figure 2.3 โ Euclid Labs simulator
C
OORDINATES
YSTEMS2.3.1 Tool Center Point
The TCP (tool center point) is the reference point of the tool attached to the robot flange. Usually the robot movements are programmed defining a path with
respect to the TCP that can be represented in different coordinate systems. It could be any point but typically corresponds to the tool tip, e.g. the muzzle of a welding torch. Furthermore, there could be several TCP, but only one at time can be active.
2.3.2 Robot Base frame
12
Figure 2.4 โ Robot base frame
In this work, the robot base coordinate system corresponds to the world coordinate system.
2.3.3 Wrist frame
The wrist frame corresponds to the robot flange (the last axis of the robot where the tools have to be attached). The origin of the wrist frame is the center of the visible ๏ฌange surface. The red, green and blue axis on Figure 2.5 represent the ๐ฅ, ๐ฆ and ๐ง axis.
13
2.3.4 Tool frame
The tool frame is the coordinate system in which the origin corresponds to the tool center point. The z-axis has usually the same direction of the tool tip and defines its work direction. The red, green and blue axis on Figure 2.6 represent the ๐ฅ, ๐ฆ and ๐ง axis.
15
3 I
MAGE PROCESSING
P
INHOLE CAMERA MODELPinhole camera model is a projective model that approximate the image acquisition process of a digital camera through simple geometric relationships. The purpose is to find the relation between image points and their position in the world. It performs well as long as the lens is thin and no wide-angle lens is used.
In Figure 3.1 are shown the relationships between the following coordinate systems:
1. World coordinate system has origin at point ๐ and its orientation is given by ๐๐ฅ, ๐๐ฆ and ๐๐ง.
2. Camera coordinate system has the origin at point C that is also the focal point. Axis ๐๐งโฒ is aligned with camera optical axis and its direction points the
image plane. Plane given by ๐๐ฅโฒ and ๐
๐ฆโฒ is parallel to the camera image
plane.
3. Image coordinate system has axis ๐ข and ๐ฃ aligned with ๐๐ฅโฒ and ๐๐ฆโฒ of
camera coordinate system. The origin is the top left corner and the optical
axis intersect image plane center.
Figure 3.1 โ Pinhole camera model
16 ๐ง๐[ ๐ข๐๐ ๐ฃ๐๐ 1 ] = [ ๐ฅ๐ ๐ฆ๐ ๐ง๐] = [ ๐ ๐๐ถ ๐๐๐ถ 0 1 ] [ ๐๐ ๐๐ ๐๐ 1 ] ( 3.1 )
Where [๐๐, ๐๐, ๐๐]๐ represents a point expressed in world coordinate system,
[๐ฅ๐, ๐ฆ๐, ๐ง๐]๐ is the same point but expressed in camera coordinate system and
[๐ข๐๐, ๐ฃ๐๐]๐ is the corresponding normalized image coordinate. ๐ โ โ3๐ฅ3 and ๐ก โ โ3 are the extrinsic parameters and represent rotation and translation from the world
coordinate system to the camera coordinate system.
Real camera image plane is usually represented with the following transformation from the normalized image coordinate:
[ ๐ข๐ ๐ฃ๐ 1 ] = [ ๐ผ ๐พ ๐ฃ0 0 ๐ฝ ๐ข0 0 0 1 ] [ ๐ข๐๐ ๐ฃ๐๐ 1 ] ( 3.2 )
Where ๐ผ โ โ and ๐ฝ โ โ are scaling factors from normalized image plane to real image plane along ๐ข๐ and ๐ฃ๐ directions, ๐พ โ โ represents the skew coefficient between ๐ข and ๐ฃ axis and [๐ข0, ๐ฃ0] โ โ2 represent the intersection of real image
plane with optical axis.
A simplified schema about how the image is acquired by the sensor is shown in
Figure 3.2. The point (๐ฅ๐, ๐ฆ๐, ๐ง๐) expressed in camera coordinate system,
Figure 3.2 โ Point projection on image plane
17
I
MAGE PROCESSING ALGORITHMS3.2.1 Grayscale histogram
The histogram of a grayscale image with 256 levels is a function โ๐(๐ฅ) where the domain is all the integers contained into the interval [0,255] that for each pixel represents the possible values of the grayscale image while the codomain is the number of pixels with the corresponding value found: ๐ =๐๐
๐ , ๐ โ โ, ๐ โฅ 1
Algorithm 3.1 - Computing the histogram
Figure 3.3 - Histogram
3.2.2 Binary image
Since the tool is in backlight, a threshold value can divide the light background from the darker tool. To avoid external light inference, the threshold is not fixed: computing the image histogram the threshold ๐ can be defined dynamically finding the minimum value between the two higher local maximum values, this should corresponds to a value between the mean intensity of the background and the mean intensity of the tool.
0 2000 4000 6000 8000 10000 12000 14000 16000 18000 20000 1 255 Fre q u en cy Grayscale
1. Set all the elements of the array โ๐(๐ฅ) to zero
18
Once found the threshold ๐ using the histogram, the binary image ๐๐(๐ฅ, ๐ฆ) can be
computed as follows:
๐๐(๐ฅ, ๐ฆ) = {
0 ๐๐ ๐(๐ฅ, ๐ฆ) > ๐
1 ๐๐ ๐(๐ฅ, ๐ฆ) โค ๐ ( 3.3 )
3.2.3 Gaussian filter
This filter smooth the image convolving a Gaussian kernel ๐ป with the image itself. ๐ป is a matrix of 2๐ + 1 ร 2๐ + 1 elements, with ๐ โ โ, where values are defined as follows: ๐ป๐๐ = 1 2๐๐2 ๐ โ(๐โ๐โ1)2+(๐โ๐โ1)2 2๐2 ( 3.4 )
Where ๐ is the standard deviation of the probability distribution.
3.2.4 Sobel operator
This operator can be used to approximate the image gradient convolving the image with two kernels ๐๐ฅโ โค2 and ๐
๐ฆ โ โค2: ๐๐ฅ = [ โ1 0 1 โ1 0 2 โ1 0 1 ] ๐๐ฆ = [ โ1 โ2 โ1 0 0 0 1 2 1 ] ( 3.5 )
19
3.2.5 Cannyโs Edge Detection
John F. Canny developed this algorithm in 1986 to detect a wide range of edges in images. This edge detector has three main criteria to solve the problem:
๏ท The detection: false detection should be avoided and important edge should not be missed
๏ท The localization: minimize the distance between the edge detected and the edge center
๏ท The one response: the algorithm should minimize multiple response to a single edge
The basic idea of Cannyยดs edge detection algorithm is to consider a point in the image as an edge if the first derivatives has local maxima along its gradient direction.
The algorithm can be broken down to five different steps:
1. To remove the noise and obtain better results, the first step of the algorithm is to smooth the image with a Gaussian filter. This can be done convolving the image with a Gaussian filter.
2. Compute the image gradient to estimate local edge magnitude ๐บ๐๐ and
direction ๐๐๐ for each image pixel (๐, ๐). Sobel operator can be used for this
purpose.
3. Find the edges location using Non-maximum suppression Algorithm:
a. Round the direction to the nearest multiple of 45ยฐ
b. Compare the gradient magnitude with the one related to the next pixel along the gradient direction
c. Suppress the current pixel value if its gradient magnitude is lower The purpose is to find all local maxima in the gradient magnitudes along the related gradient directions.
4. At this point, the image contains the edge detected but there could be some spurious response. A way to keep only the true edges is to threshold the related gradient magnitude with hysteresis:
20
b. If the gradient magnitude is under a low threshold then the corresponding pixel is a false edge
c. If the gradient magnitude is between the low and high thresholds then the corresponding pixel is a true edge only if there is at least one other true edge among its neighbours
5. Feature synthesis approach: The general principle is to start from the smallest scale, then synthesise the larger scale outputs that would occur if they were the only edges present. Then comparing the large scale output with the synthetized edge response, additional edges are marked only if the large scale output is significantly greater than the synthetic prediction. Different scale for Cannyโs edge detector are represented by different standard deviation ๐ of the Gaussian filter.
Algorithm 3.2 โ Canny edge detector
Figure 3.4 โ Binary image Figure 3.5 โ Edge found with Cannyโs edge detection
1. Convolve the image with a Gaussian filter with standard deviation ๐ 2. Compute the image gradient
3. Find the location of the edges using Non-maximum suppression Algorithm 4. Threshold edges with hysteresis
21
3.2.6 Hough transform
Conceived by Hough in 1962, in its first version, the purpose was the recognition of straight lines. The main idea is that any point on the entire image gives a contribution to the overall recognition: given a point, can be defined a sheaf of lines and for each couple of points there is only one common line. Computing a discrete sheaf of lines for each image point, if a specific line appears ๐ times, then there is a line on the image with ๐ points.
A bidimensional line can be represented with two parameters, then defining a bidimensional matrix where columns and rows corresponds to the first and second line parameter, the lines found can be counted incrementing the corresponding cell. The result is an accumulation function defined on the lines parameter space. Changing the parameter space, this algorithm can be used to find various geometric shape.
A line can be represented in several ways, the most common equation used is ๐ฆ = ๐๐ฅ + ๐ where ๐ is the gradient of the line, ๐ is the y-intercept of the line and (๐ฅ, ๐ฆ) represent a line point. Changing ๐ and ๐ all the lines that through the point (๐ฅ, ๐ฆ) can be defined. With this kind of representation, the parameter ๐ increases immeasurably when the line become vertical and has small variations when the line become horizontal then this representation is not well suited to define a discrete parametric space. A suitable line representation could be ๐ = ๐ฅ sin ๐ + ๐ฆ cos ๐ where ๐ โ โ is the line distance from the center and ๐ โ โ is the orientation of ๐ with respect to the abscissas axis.
22
With this representation, for each image point (๐ฅ, ๐ฆ) โ ๐ก๐๐๐ ๐ โ โค2 , the corresponding lines ๐๐๐ฅ๐ฆ, with ๐ โ [0,179] can be computed varying the orientation
๐๐ from 0 to 179 and computing the corresponding distance ๐๐ from the center as a function of ๐ฅ, ๐ฆ and ๐๐
Once all the pixels were analysed, the points in the parameter space that have accumulated the most number of "votes" represents the lines that have high probability of being present in the image.
A further step is to keep the lines that have a minimum number of points, the decision threshold ๐ could be a fraction of the number of points ๐๐ of the largest line found: ๐ =๐๐
๐ , ๐ โ โ, ๐ โฅ 1
Algorithm 3.3 โ Hough transform algorithm
1. Define the parameter space (๐, ๐) with an accumulation matrix M 2. Initialize all the M element to zero
3. For each pixel (๐ฅ, ๐ฆ) compute ๐๐ = ๐ฅ sin ๐๐ + ๐ฆ cos ๐๐ with ๐๐ โ [0,179] and increment by a unit ๐(๐๐, ๐๐)
23
Figure 3.7 โ Edge image Figure 3.8 โ Lines found with Hough transform
3.2.7 Thinning
This is a morphological operator used to reduce the area of an element contained into a binary image ๐ โ โค2. The resulting image ๐โฒโ โค2 is:
๐โฒ= ๐ โ ๐ต = ๐ / ๐ โ ๐ต
( 3.7 )
Where ๐ต = (๐ต1, ๐ต2), ๐ต1 โ โค2, ๐ต2โ โค2, ๐ต1โฉ ๐ต2 = โ is called composite structuring
element and ๐ โ ๐ต = {๐ฅ: ๐ต1 โ ๐ ๐๐๐ ๐ต2 โ ๐๐} is the hit-or-miss transformation.
In other words, once the thinning operator has been applied, a pixel ๐ฅ equal to one of the initial image will be zero on the resulting image if the set of surroundings pixels ๐ต1 of ๐ฅ are equal to one and the set of surroundings pixels ๐ต2 of ๐ฅ are equal to zero.
This operator has been implemented convolving the image with eight composite
structuring elements taken from the Golay alphabet:
๐ต1 = [ 0 0 0 โ 1 โ 1 1 1 ] ๐ต2 = [ โ 0 0 1 1 0 โ 1 โ ] ๐ต3 = [ 1 โ 0 1 1 0 1 โ 0 ] ๐ต4 = [ โ 1 โ 1 1 0 โ 0 0 ] ๐ต5 = [ 1 1 1 โ 1 โ 0 0 0 ] ๐ต6 = [ โ 1 โ 0 1 1 0 0 โ ] ๐ต7 = [ 0 0 0 โ 1 โ 1 1 1 ] ๐ต8 = [ 0 0 โ 0 1 1 โ 1 โ ] ( 3.8 )
3.2.8 Homotopic skeleton detection
To define what is an image skeleton, maximal ball needs to be specified: A ball ๐ต(๐, ๐) โ โ๐ with radius ๐ โ โ and center ๐ โ โ๐ is said to be maximal in a set
๐ โ โ๐ if and only if there exists no other ball included in ๐ and containing ๐ต: ๐๐๐ก ๐ โ โ๐๐๐๐ ๐ต ๐๐๐๐ โ โ๐, ๐กโ๐๐ ๐ต ๐๐ ๐๐๐ฅ๐๐๐๐ ๐๐๐๐ ๐๐ ๐ ๐๐
24
The skeleton ๐(๐) โ โ๐ by maximal balls of a set ๐ โ โ๐ is the set of the centers of its maximal balls:
๐(๐) = {๐ โ ๐ โถ โ ๐ โฅ 0, ๐ต(๐, ๐) ๐๐ ๐๐๐ฅ๐๐๐๐ ๐๐๐๐ ๐๐ ๐ }
( 3.9 )
A sequential thinning with the eight composite structuring elements defined in section 3.2.7 applied until idempotency is reached, produces a homotopic substitute in โค2 of the skeleton defined by maximal balls.
Figure 3.9 โ Source image Figure 3.10 โ Homotopic skeleton
In some unfortunate cases this algorithm performs a sequential pixel elimination: with only one iteration a tool portion become a line due to the tool erosion from only one side (
Figure 3.12) leading to a false skeleton detection. The problem can be avoided adding a flag matrix ๐ reinitialized at the beginning of each cycle with false values and set to true a cell ๐(๐ฅ๐, ๐ฆ๐) when the corresponding image pixel ๐ผ(๐ฅ๐, ๐ฆ๐) is going
25
Figure 3.11 โ Source image Figure 3.12 โ Wrong skeleton detection Figure 3.13 โ Correct skeleton detection
K
EY POINT DETECTIONThe TCP projection on the image can be detected finding tool main direction and then searching the minimum point along the found direction. With symmetric tools, this point should correspond to the tool tip.
3.3.1 Tool direction
The Hough transform 3.2.6 can be used for this purpose. Applying this algorithm on the image, the resulting accumulation matrix contains the number of points belonging to each line found. A line is represented by the line distance from the image center ๐๐ and the rotation angle ๐๐ of ๐๐ with respect to ๐ axis (see section 3.2.6 for more details).
The direction of a line can be represented by the angle between the ๐ axis and the line itself as shown in Figure 3.14
26
With this representation the line direction can be obtained as ๐๐โฒ= ๐๐ +๐
2 since it is
perpendicular to the direction of ๐๐.
Now the problem is which line consider as main tool direction.
3.3.1.1 Sum of lines direction
The first idea was to apply the Hough transform on the image containing the edges found with Cannyโs algorithm and then to find main direction with the following method:
1. Define a new vector of directions ๐ท that represents all the possible directions ๐๐, where each cell contains the counting of the points of all the lines with the corresponding direction.
2. Then the main tool direction can be found identifying the cell of ๐ท with the maximum value and thereafter calculating the line direction ๐๐โฒ.
In some cases, this method cannot performs well. For instance applying this method on a triangular tip of a tool, the direction will be not accurate Figure 3.16.
Figure 3.15 โ Source image Figure 3.16 โ Wrong tool directon
3.3.1.2 Largest line
27
In this case, to identify the line representing main tool direction, could be easier to find the absolute maximum on the accumulation matrix found through the Hough transform since it represents the line with more points found on the image that should correspond to the main skeleton part.
Figure 3.17 โ Source image Figure 3.18 โ Skeleton direction found
3.3.2 TCP localization
Once found the tool direction ๐โฒ, rotating the image coordinate space ๐
๐ฅ๐ฆ of ๐โฒ
anticlockwise, the TCP projection can be found among the tool points of local extrema along the new abscissas axis ๐ฅโฒ.
Let ๐๐ฅ๐ฆ the initial image coordinate space with origin on image centre, the new
coordinate space ๐๐ฅโฒ๐ฆโฒ with the abscissas axis parallel to tool direction has
coordinates: [๐ฅ โฒ ๐ฆโฒ] = [ cos ๐โฒ โ sin ๐โฒ sin ๐โฒ cos ๐โฒ ] [ ๐ฅ ๐ฆ] ( 3.10 )
The maximum and minimum points representing the tool along the ๐ฅโฒ axis
correspond to a point on the tool tip and a point of image borders. Now, evaluating these points found expressed in coordinate system ๐๐ฅ๐ฆ, understand which is the
28
3.3.3 TCP optimisation
There could be more than one point of extrema that can represent the TCP ( Figure 3.19). A way to solve this ambiguity is to calculate the mean of the surrounding points along the axis perpendicular to the tool direction: let ๐โฒโ โค2 the set of the points representing the tool with the coordinate system ๐๐ฅโฒ๐ฆโฒ ( 3.10 ),
(๐ฅ๐๐ฅ๐กโฒ , ๐ฆ๐๐ฅ๐กโฒ ) โ ๐โฒ the point found with TCP localisation (section 3.3.2) and ๐ =
{(๐ฅโฒ, ๐ฆโฒ) โ ๐โฒ โถ |๐ฅโฒโ ๐ฅ
๐๐ฅ๐กโฒ | < ๐, ๐ โ โ} the set of its surrounding points, then the
TCP is: (๐ฅ๐ก๐๐, ๐ฆ๐ก๐๐) = [cos โ๐โฒ โ sin โ๐โฒ sin โ๐โฒ cos โ๐โฒ ] [ ๐ฅ๐๐ฅ๐กโฒ ๐ฆ๐๐๐๐โฒ ] ๐คโ๐๐๐ ๐ฆ๐๐๐๐โฒ = โ(๐ฅโฒ,๐ฆโฒ)โ๐๐ฆโฒ โ๐โ ( 3.11 )
A result of this optimisation can be seen in
Figure 3.20 where the set ๐ is represented by purple pixels while the TCP by grey pixels.
Figure 3.19 โ Keypoint without tcp optimisation
29
4 TCP
CALIBRATION
T
OOL CENTER POINT REPRESENTATIONLet ๐น๐ the ๐๐กโ position of the robot flange represented with the homogeneous
transformation matrix ( 4.1 ) expressed in robot base coordinate system. ๐น๐ = [๐ ๐ ๐๐
0 1, ] , ๐ โ [1, ๐] ( 4.1 )
Where ๐ ๐ is the three-dimensional rotation matrix from the robot base fame ๐๐ค and ๐๐ is the three-dimensional translation vector from the robot base frame ๐๐ค. ๐น๐ can
be obtained with the robot forward kinematics.
The tool center point can be represented with a further homogeneous transformation matrix ๐ป๐ก๐๐ with respect to the robot flange coordinate system.
๐ป๐ก๐๐ = [๐ ๐ก๐๐ ๐๐ก๐๐
0 1 ] ( 4.2 )
Where ๐ ๐ก๐๐ is the three-dimensional rotation matrix from the robot flange frame ๐น๐
and ๐๐ก๐๐ = (๐๐ก๐๐๐ฅ, ๐๐ก๐๐๐ฆ, ๐๐ก๐๐๐ง)๐ is the three-dimensional translation vector from the robot flange frame ๐น๐.
M
INIMUM NUMBER OF EQUATIONSA position ๐๐ โ โ3 of the TCP expressed in robot base coordinate system can be computed as follows:
๐๐ = ๐น๐ โ ๐ก๐ก๐๐, ๐ โ [1, ๐] ( 4.3 )
Where the frame ๐น๐ represents a robot flange pose and ๐ก๐ก๐๐ = (๐๐ก๐๐, 1)๐ is the homogeneous vector representing the TCP with respect to ๐น๐.
For each couple of different robot flange poses ๐น๐ and ๐น๐, with the TCP constrained
30
๐น๐โ ๐ก๐ก๐๐โ ๐น๐โ ๐ก๐ก๐๐ = ๐๐ โ ๐๐, ๐, ๐ โ [1, ๐], ๐ โ ๐ (๐๐ = ๐๐) โ [๐น๐ โ ๐น๐] โ ๐ก๐ก๐๐ = ๐ท๐๐ โ ๐ก๐ก๐๐ = 0
( 4.4 )
The dimension of ๐ท๐๐ is 4x4. Since ๐น๐ and ๐น๐ are homogeneous matrix, all the
values of last row of ๐ท๐๐ are zero.
The rank of ๐ท๐๐ ( 4.4 ) is two, this can be demonstrated but some concepts have to be defined.
Let R a three-dimensional rotation matrix:
1 Orthonormal basis: in โ3 can be represented with a 3x3 matrix in which the
columns represent three unit vector where each of them is parallel to one and only one axis of the three-dimensional space. ๐ may be seen as an orthonormal basis since it is an orthogonal matrix with determinant equal to one.
2 Axis-angle representation: ๐ can be represented with the axis-angle
representation ๐ = (๐ฬ, ๐) where ๐ is the rotation around the axis of rotation represented by the three-dimensional vector ๐ฬ. As the rotation ๐ grows, any point rotates around the rotation axis ๐ฬ (Figure 4.1).
3 Rotation plane: it is a plane defined in โ3 in relation with ๐ , it contains the origin of the coordinate system and has the normal perpendicular to the axis of rotation ๐ฬ (Figure 4.1).
31
4 Difference vector: let ๐1 a three-dimensional point and ๐2 = ๐ โ ๐1 the point ๐1
rotated with the matrix ๐ . A difference vector of ๐ is ๐ฃ12 = ๐1โ ๐2. All the
difference vectors of ๐ are contained into its rotation plane since they are perpendicular to the axis of rotation ๐ฬ of ๐ .
5 Correlated rotation matrix: since a rotation matrix is an orthonormal basis, for
each couple of rotations matrix ๐ ๐ and ๐ ๐ exists a correlated rotation matrix ๐ ๐๐ that rotates the three unit vectors represented by the columns of ๐ ๐ (Figure 3.2 ๐ฅ, ๐ฆ, ๐ง) to a new position defined by the three columns of ๐ ๐ (Figure 3.2 ๐ฅโฒ, ๐ฆโฒ, ๐งโฒ).
6
Figure 4.2 โ Rotation between two coordinate system
Statement 4.1: Let ๐ ๐ and ๐ ๐ two rotation matrix such that ๐ ๐ โ ๐ ๐. The matrix
๐ ๐ โ ๐ ๐ has rank equal to two.
Proof: The columns of ๐ ๐ โ ๐ ๐ can be seen as three difference vectors contained
into the rotation plane of the correlated rotation matrix ๐ ๐๐ of ๐ ๐ and ๐ ๐ Then the rank of ๐ ๐ โ ๐ ๐ must be less or equal to two [1].
32
Where ๐ ๐1โ ๐ ๐1and ๐ ๐2โ ๐ ๐2 are the first and the second column of ๐ ๐โ ๐ ๐, but since ๐ ๐ and ๐ ๐ are orthonormal basis:
๐ ๐1โฅ ๐ ๐2 โ ๐ ๐๐1 โ ๐ ๐2 = 0
๐ ๐1 โฅ ๐ ๐2โ ๐ ๐๐1โ ๐ ๐ 2 = 0
( 4.6 )
Where ๐ ๐1 โ 0, ๐ ๐2 โ 0, ๐ ๐1 โ 0 and ๐ ๐2โ 0.
Then if ( 4.5 ) is true, even the following equations are true: (๐ ๐1โ ๐ ๐1)๐ ๐2= ๐ผ(๐ ๐2โ ๐ ๐2)๐ ๐2 ๐ ๐๐1๐ ๐2โ ๐ ๐๐1๐ ๐2 = ๐ผ๐ ๐๐2๐ ๐2โ ๐ผ๐ ๐๐2๐ ๐2 โ โ๐ ๐๐1๐ ๐2 = ๐ผ๐ ๐๐2๐ ๐2โ ๐ผ๐ ๐๐2๐ ๐2 โ โ๐ ๐1 = ๐ผ๐ ๐2โ ๐ผ๐ ๐2 โ โ๐ ๐๐1๐ ๐2 = ๐ผ๐ ๐๐2๐ ๐2โ ๐ผ๐ ๐๐2๐ ๐2 โ 0 = ๐ผ๐ ๐๐2๐ ๐2โ ๐ผ๐ ๐๐2๐ ๐2 โ ๐ผ(๐ ๐2โ ๐ ๐2) = 0 ( 4.7 )
Then equation ( 4.5 ) is true if:
๐ ๐2โ ๐ ๐2 = ๐ ๐1โ ๐ ๐1 = 0
( 4.8 )
But since ๐ ๐ โ ๐ ๐, at most one column of ๐ ๐ can be equal to the same column of ๐ ๐ then ( 4.8 ) cannot be true, meaning that even ( 4.5 ) cannot be true then the
difference vectors cannot be parallel. Since at least two columns of ๐ ๐โ ๐ ๐ are not parallel, ๐ ๐ โ ๐ ๐ has rank grater or equal to two [2].
[1] โฉ [2] โ ๐ ๐ โ ๐ ๐ has rank equal to two.
33
Statement 4.2: If ๐ ๐ โ ๐ ๐, the last column of ๐ท๐๐ ( 4.4 ) ๐๐ โ ๐๐ is a linear
combination of the submatrix columns ๐ ๐ โ ๐ ๐ of ๐ท๐๐.
Proof: ๐๐ and ๐๐ can be seen as two vectors ๐ฃ๐ and ๐ฃ๐ฝ expressed in TCP coordinate system:
๐๐ = TCPw+ ๐ฃ๐
๐๐ = TCPw+ ๐ฃ๐
( 4.9 )
Where TCPw is the TCP expressed in robot base coordinate system.
๐๐ โ ๐๐ = TCPw+ ๐ฃ๐ โ (TCPw+ ๐ฃ๐) = ๐ฃ๐โ ๐ฃ๐ ( 4.10 )
Since ๐ฃ๐ = ๐ ๐๐โ ๐ฃ๐, where ๐ ๐๐ is the correlated rotation matrix of ๐ ๐ and ๐ ๐, then
๐ฃ๐โ ๐ฃ๐ is a difference vector of ๐ ๐๐. This means that also ๐๐โ ๐๐ is a difference
vector of ๐ ๐๐ then it is contained into the rotation plane of ๐ ๐๐ as the columns of ๐ ๐ โ ๐ ๐. Then if ๐ ๐ โ ๐ ๐, ๐ ๐โ ๐ ๐ has rank equal to two then ๐๐ โ ๐๐ can be expressed as a linear combination of ๐ ๐ โ ๐ ๐.
Qed
Then if ๐ ๐ โ ๐ ๐, ๐ ๐โ ๐ ๐ has rank equal to two, ๐๐โ ๐๐ is a linear combination of ๐ ๐ โ
๐ ๐ and since the last row ๐ท๐๐ ( 4.4 ) is zero then ๐ท๐๐ has rank equal to two.
This means that two flange poses are not enough to solve the system of equations ๐ท๐๐ โ ๐ก๐ก๐๐ = 0, at least another pose ๐น๐ is needed.
[๐น๐ โ ๐น๐
๐น๐โ ๐น๐] โ ๐ก๐ก๐๐ = [
๐ ๐ โ ๐ ๐ ๐๐ โ ๐๐
๐ ๐ โ ๐ ๐ ๐๐ โ ๐๐] โ ๐ก๐ก๐๐ = 0 ( 4.11 )
If ๐ ๐ โ ๐ ๐ and ๐ ๐ โ ๐ ๐ and if ๐ฬ๐๐ โ ๐ฬ๐๐, where ๐ฬ is the axis of rotation of the respective
correlated rotation matrix, then the difference vectors represented by the difference
matrix ๐ ๐ โ ๐ ๐ and ๐ ๐ โ ๐ ๐ in ( 4.11 ) are contained in at least two different rotation
34
difference vectors linearly independent or, in other words, the system of equations
has unique solution.
C
AMERAB
ASE CALIBRATIONA binding to solve equation ( 4.11 ) is that the TCP must be in the same point for each flange pose, but using a camera the TCP can be constrained to lie only on a line.
The optical axis ๐๐ in the three-dimensional space is a line whose projection corresponds to the image center. Then, for each flange pose, two coordinates can be constrained with fairly accuracy moving the flange on a plane parallel to the camera plane in such a way to constraint the TCP to be on the image center. Let ๐๐ the camera base that is a coordinate system with the origin corresponding to the center of the camera image plane, with the plane ๐ฅ๐ฆ๐ aligned with the
camera plane ๐ฅ๐ฆ๐ and the z-axis aligned with the camera optical axis: ๐๐ = [๐ ๐ ๐๐
0 1] = ๐๐[
๐ ๐๐ ๐๐๐
0 1 ] ( 4.12 )
Where ๐ ๐๐ โ โ3๐ฅ3 and ๐
๐๐ โ โ3 are the rotation matrix and the translation vector
from the robot base coordinate system to the camera plane.
Since the purpose is to move the tool with respect to the current position (then without setting absolute positions), only the rotation matrix ๐ ๐ is required because it allows to define the direction ๐๐๐ โ โ
3 with respect to ๐
๐, along which to move
the flange to constraint the TCP projection to be on the image center keeping the flange on the plane ๐ฅ๐ฆ๐. Let ๐๐๐ โ โ2 the direction given by the vector that connect the TCP projection (๐ฅ๐, ๐ฆ๐) on the image plane to the image center (๐ฅ๐, ๐ฆ๐), then:
๐๐๐ =
(๐ฅ๐โ ๐ฅ๐ , ๐ฆ๐ โ ๐ฆ๐, 0) |(๐ฅ๐ โ ๐ฅ๐ , ๐ฆ๐ โ ๐ฆ๐)|
( 4.13 )
Note that last value of ( 4.13 ) is zero since the coordinate system ๐๐ has the plane
35
flange along direction ๐๐๐ in such a way to constraint the TCP projection to be on the image center without changing the z-value.
There are several methods to find the camera rotation matrix ๐ ๐, two of which are
the following:
4.3.1 Kuka base calibration
Kuka provides a procedure to define a new reference system called base. This
procedure requires the identification of three points ๐1, ๐2, ๐3 โ โ3 moving the robot using the manual control system:
๏ท ๐1: origin of the new coordinate system
๏ท ๐2: point on x-axis of the new coordinate system
๏ท ๐3: point on xy plane of the new coordinate system The z-axis direction is defined according to left-hand rule.
Note that to define the origin with fairly accuracy a pointed tool already calibrated have to be used. Here only the rotation ๐ ๐ is useful then the base can be defined
moving a pointed tool on the three points but considering the corresponding flange position to set the values ๐1, ๐2 and ๐3, making sure that during the movements
the flange doesnโt change its orientation. The resulting rotation matrix is the same since the flange position can be seen as a constant translation of the tool tip. Then the reference system computed will have another origin but the same orientation.
4.3.2 Base calibration using camera images
Another way to compute the rotation matrix ๐ ๐ is to move the tool on different
positions contained into a plane of the 3D space expressed in robot base coordinate system and recording the TCP position found with the camera.
Let (๐๐ค, ๐๐ค, ๐๐ค) a flange position expressed in the robot base coordinate system, (๐ข, ๐ฃ) the pixel coordinate representing the TCP, ๐ ๐ โ โ3๐ฅ3 a rotation matrix, ๐ผ and
36
Equation ( 4.14 ) represents the relation between flange positions and corresponding TCP found on the image plane. Then the camera rotation matrix can be found recording these planar correspondences and then computing ๐ ๐.
TCP
POSITIONING WITH CAMERALet ๐๐๐ โ โ3 the current flange position expressed in camera base frame ๐๐ ( 4.12
), (๐ฅ๐, ๐ฆ๐) the corresponding TCP projection on the camera plane and (๐ฅ๐, ๐ฆ๐) the image plane center. The flange position expressed in ๐๐ such that the TCP projection on the camera image plane corresponds to the image center is:
๐๐๐โ๐ = ๐๐๐+ [ ๐ผ(๐ฅ๐ โ ๐ฅ๐) ๐ฝ(๐ฆ๐ โ ๐ฆ๐) 0 ] ( 4.15 )
Where ๐ผ and ๐ฝ are the scaling factors from pixel to robot coordinate system unit along ๐ฅ and ๐ฆ directions.
The scaling factor ๐ผ and ๐ฝ can be defined considering a flange position (๐๐ฅ, ๐๐ฅ, ๐๐ง) expressed in camera coordinate system ๐๐, the corresponding TCP projection on the camera plane (๐ฅ1, ๐ฆ1), a second flange position (๐๐ฅ+ ๐, ๐๐ฆ+ ๐, ๐๐ง) (that is a translation of ๐๐1 along the plane ๐ฅ๐ฆ) and the corresponding TCP projection on the camera plane (๐ฅ2, ๐ฆ2): ๐ผ = ๐ ๐ฅ2โ ๐ฅ1 ๐ฝ = ๐ ๐ฆ2โ ๐ฆ1 ( 4.16 )
Note that the plane ๐ฅ๐ฆ is parallel to the camera image plane.
37
Algorithm 4.1 โ TCP positioning with camera
Algorithm 4.1 changes dynamically the scale factor if the flange movements to position the TCP are too high or too low.
TCP
COMPUTATIONRegardless of the method used to compute TCP translation vector, there are two main problems:
1. During early stages no information are known about the tool, then to change a flange pose keeping the TCP detectable by the camera, only small flange rotation are suitable. The maximum rotation suitable for this purpose is related to the tool geometry.
2. As shown above at least three flange poses are required to solve the system of equations ( 4.11 ) and the rotation axis between flange poses must be distinct. Assuming that first rotation has the rotation axis equal to the z-axis with respect to the reference system ๐๐ ( 4.12 ), using Algorithm 4.1 the TCP can be positioned on the same three-dimensional point of the first flange pose. Now the next flange pose must have a different rotation axis, 1. Get flange position ๐๐1 and the corresponding TCP projection on the
camera plane (๐ฅ1, ๐ฆ1)
2. ๐๐ฅ = (๐ฅ๐โ ๐ฅ1), ๐๐ฆ = (๐ฆ๐โ ๐ฆ1) 3. Set the flange position ๐๐2 = ๐๐1+ [
๐ผ๐๐ฅ ๐ฝ๐๐ฆ
0
] and get the corresponding TCP projection on the camera plane (๐ฅ2, ๐ฆ2)
4. If (๐ฅ2, ๐ฆ2) corresponds to the image center then stop.
38
this means, applying Algorithm 4.1 to move the TCP, itโs final position will have a different z-coordinate, then an approximation of TCP can be computed using ( 4.11 ).
The first idea was to estimate a solution with the method of the least squares that compute a result minimizing the error of each TCP position related to the flange poses used. Studying the detail of this approach, to overcome the problem of the third TCP position that have a different z-coordinate, I found another way to solve the problem without approximations from a theoretical point of view using three flange poses.
4.5.1 Least squares method
The least squares method with normal equations A.1 can be used to compute the TCP:
๐๐ก๐๐ = (๐ด๐๐ด)โ1๐ด๐๐ฆ ( 4.17 )
Where ๐ด is the submatrix of ( 4.11 ) containing the difference ๐ ๐โ ๐ ๐ and ๐ ๐ โ ๐ ๐ and ๐ฆ represents the last column of ( 6.1 ) containing the difference of the translation vectors with opposite sign:
๐ด๐ฅ โ ๐ฆ = [ ๐ ๐ โ ๐ ๐ ๐ ๐ โ ๐ ๐ ] ๐๐ก๐๐โ [ ๐๐โ ๐๐ ๐๐โ ๐๐ ] ( 4.18 )
As shown above, with ๐ ๐ = ๐ ๐ง๐ ๐ and ๐ ๐ = ๐ ๐ฅ๐ ๐, A has rank three then even ๐ด๐๐ด
has rank three. This means ๐ด๐๐ด is invertible then the system of equations ( 4.11 ) can be solved and has unique solution.
Other flange pose could be used to add equations at ( 4.18 ), but positioning the flange after a rotation with Algorithm 4.1 there is no guarantee that the TCP will be on the same three-dimensional position. Then the TCP computed will be an approximation.
4.5.2 Analytic solution
Let ๐๐ถ๐๐๐๐โ๐ โ โค2 the image center point, ๐๐ถ๐
๐โ๐ โ โ3 the point used to constraint
39
must be equal to ๐๐ถ๐๐๐๐โ๐ and ๐น1๐ the representation of the first flange position
with the TCP placed on the point ๐๐1= ๐๐ถ๐๐โ๐ with a rotation matrix ๐ 1๐ and a
translation vector ๐1๐.
A rotation ๐ ๐ง around the z-axis applied on ๐น1๐ moves the TCP to a new 3D position without changing the z-value expressed in coordinate syestem ๐๐. Now using
Algorithm 4.1 the flange can be moved along ๐ฅ and ๐ฆ axis in such a way to place the TCP detected with the camera on the image point ๐๐ถ๐๐๐๐โ๐. The new TCP position ๐๐2 โ โ3 will be equal to ๐๐1, let ๐น2๐ the corresponding flange pose with rotation matrix ๐ 2๐ = ๐ ๐งโ ๐ 1๐ and translation vector ๐2๐.
Note that using the image center point as TCP position constraint, independently in which z-coordinate is placed the TCP, if its projection on the image plane is equal to ๐๐ถ๐๐๐๐โ๐ then x-coordinate and y-coordinate will be equal to those of ๐๐ถ๐๐โ๐. To found a solution of ( 4.11 ), the next flange rotation has to have a different rotation-axis and the TCP has to be placed on ๐๐ถ๐๐โ๐. But using another rotation axis the 3D TCP z-value will be not equal to the z-value of ๐๐1 and ๐๐2 and the information obtained from camera are not enough to move the flange on a third pose ๐น3๐ such that the related TCP point ๐๐3โ โ3 is equal to ๐๐1 and ๐๐2, only the x-component and the y-component will be equal. This means that the z-component of the translation vector ๐3๐ of ๐น3๐ will be affected by error. The following method
overcome this problem leading to a solution without errors from a theoretical point of view:
Consider now only the first and the second flange poses:
๐๐1= ๐๐2 โ [๐น1๐โ ๐น2๐] โ ๐ก๐ก๐๐ = [๐ 1๐โ ๐ ๐งโ ๐ 1๐ ๐1๐ โ ๐2๐] โ ๐ก๐ก๐๐ = 0 ( 4.19 )
Where ๐ก๐ก๐๐ = (๐๐ก๐๐, 1)๐ is the homogeneous vector representing the TCP expressed in robot flange coordinate system, then ( 4.19 ) can be rewritten as follows:
40
As shown in Statement 4.1 [๐ 1๐โ ๐ ๐งโ ๐ 1๐] has rank equal to two then a solution of ๐๐ก๐๐ = (๐๐ก๐๐๐ฅ, ๐๐ก๐๐๐ฆ, ๐๐ก๐๐๐ง) cannot be found, however a partial solution can be computed.
If [๐ 1๐โ ๐ ๐งโ ๐ 1๐] would have the last column and the last row equal to zero then
๐๐ก๐๐๐ฅ and ๐๐ก๐๐๐ฆcould be computed using the first two rows of the system of equations ( 4.20 ) but ๐ 1๐ could be any rotation matrix.
Statement 4.3: [๐ 1๐โ ๐ ๐งโ ๐ 1๐]๐๐ก๐๐+ ๐1๐ โ ๐2๐ = 0 ( 4.21 ) โ [๐ผ โ ๐ ๐ง] โ ๐๐ก๐๐โฒ + ๐ 1๐ โ ๐2๐ = 0 ( 4.22 ) Where ๐๐ก๐๐โฒ = ๐ 1๐โ ๐๐ก๐๐. Proof:
Let ๐ด, ๐ต, ๐ถ โ โ3๐ฅ3 and ๐ผ โ โ3๐ฅ3 the identity matrix then:
(๐ด + ๐ต)๐ถ = ๐ด๐ถ + ๐ต๐ถ ( 4.23 )
(๐ด๐ต)๐ถ = ๐ด(๐ต๐ถ) ( 4.24 )
๐ผ๐ด = ๐ด๐ผ = ๐ด ( 4.25 )
๐ 1๐ is a rotation matrix then it is invertible:
๐ 1๐โ ๐ 1๐โ1= ๐ผ ( 4.26 )
Exploiting properties ( 4.25 ) and ( 4.26 ), ( 4.21 ) can be rewritten as:
[๐ 1๐โ ๐ ๐งโ ๐ 1๐]๐ 1๐โ1โ ๐ 1๐โ ๐๐ก๐๐+ ๐1๐ โ ๐2๐ = 0 ( 4.27 )
41
There is a bi-unique relation between ๐๐ก๐๐โฒ and ๐๐ก๐๐, then finding the solution of
๐๐ก๐๐โฒ , even ๐๐ก๐๐ can be computed.
๐๐ก๐๐= ๐ 1๐โ1โ ๐๐ก๐๐โฒ ( 4.29 )
[๐ผ โ ๐ ๐ง] of ( 4.22 ) has the last row and the last column equal to zero:
[๐ผ โ ๐ ๐ง] โ ๐๐ก๐๐โฒ + ๐1๐ โ ๐2๐ = [ 1 โcos ๐ผ๐ง sin ๐ผ๐ง 0 โ sin ๐ผ๐ง 1 โ cos ๐ผ๐ง 0 0 0 0][ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ๐๐ก๐๐โฒ ๐ง] + [ ๐1๐๐ฅ โ ๐2๐๐ฅ ๐1๐๐ฆ โ ๐2๐๐ฆ ๐1๐๐ง โ ๐2๐๐ง] = 0 ( 4.30 )
Where ๐ผ๐ง โ (0, ๐] is the rotation angle around the z-axis and ๐๐ก๐๐โฒ =
(๐๐ก๐๐โฒ ๐ฅ, ๐๐ก๐๐ โฒ ๐ฆ, ๐๐ก๐๐ โฒ ๐ง).
Then from ( 4.30 ), the following system of equations can be extracted:
[ 1 โ cos ๐ผ๐ง sin ๐ผ๐ง โ sin ๐ผ๐ง 1 โ cos ๐ผ๐ง ] [ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ] + [ ๐1๐๐ฅ โ ๐2๐๐ฅ ๐1๐๐ฆ โ ๐2๐๐ฆ ] = 0 ( 4.31 )
If ๐ผ๐ง โ 0, ( 4.31 ) can be solved since the matrix determinant is grater then zero:
๐ท๐๐ก [
1 โ cos ๐ผ๐ง sin ๐ผ๐ง
โ sin ๐ผ๐ง 1 โ cos ๐ผ๐ง
] = (1 โ cos ๐ผ๐ง)2+ (sin ๐ผ๐ง)2 =
1 + cos2๐ผ
๐งโ 2cos ๐ผ๐ง+ sin2๐ผ = 2(1 โ cos ๐ผ๐ง) > 0
( 4.32 ) [ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ] =1 2 [ 1 โ sin ๐ผ๐ง 1 โ cos ๐ผ๐ง sin ๐ผ๐ง 1 โ cos ๐ผ๐ง 1 ] [ ๐2๐๐ฅ โ ๐1๐๐ฅ ๐2๐๐ฆ โ ๐1๐๐ฆ ] ( 4.33 )
42
Considering the TCP constraint resulting from the first and the third flange pose: [๐ 1๐โ ๐ ๐ฅโ ๐ 1๐]๐๐ก๐๐+ ๐1๐ โ ๐3๐= 0 ( 4.34 )
Using Statement 4.3, even ( 4.34 ) can be rewritten as follows: [๐ผ โ ๐ ๐ฅ] โ ๐๐ก๐๐โฒ + ๐ 1๐ โ ๐2๐ = [ 0 0 0 0 1 โ cos ๐ผ๐ฅ sin ๐ผ๐ฅ 0 โ sin ๐ผ๐ฅ 1 โcos ๐ผ๐ฅ][ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ๐๐ก๐๐โฒ ๐ง] + [ ๐1๐๐ฅ โ ๐3๐๐ฅ ๐1๐๐ฆ โ ๐3๐๐ฆ ๐1๐๐ง โ๐3๐๐ง] = 0 ( 4.35 )
Where ๐ผ๐ฅโ (0, ๐] is the rotation angle around the x-axis.
As mentioned before, any rotation with a rotation-axis not equal to ๐ง, leads to a
๐3๐๐ง value affected by error, because using Algorithm 4.1 the TCP related to the two flange poses cannot be constrained to lie on the same point. But knowing ๐๐ก๐๐โฒ
๐ฆ
from ( 4.33 ), the second row of ( 4.35 ) can be used to compute ๐๐ก๐๐โฒ ๐ง: (1 โ cos ๐ผ๐ฅ)๐๐ก๐๐โฒ ๐ฆ+ (sin ๐ผ๐ฅ)๐๐ก๐๐โฒ ๐ง+ ๐1๐๐ฆ โ ๐3๐๐ฆ = 0 ( 4.36 ) ๐๐ก๐๐โฒ ๐ง = ๐3๐๐ฆโ ๐1๐๐ฆ โ (1 โ cos ๐ผ๐ฅ)๐๐ก๐๐โฒ ๐ฆ sin ๐ผ๐ฅ ( 4.37 )
Note that in this case ๐ผ๐ฅ must be less then ๐ to find a solution.
43 [ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ๐๐ก๐๐โฒ ๐ง] = [ 1 2 โ sin ๐ผ๐ง 2(1 โ cos ๐ผ๐ง) 0 sin ๐ผ๐ง 2(1 โ cos ๐ผ๐ง) 1 2 0 โ(1 โ cos ๐ผ๐ฅ) 2 sin ๐ผ๐ฅ sin ๐ผ๐ง (1 โ cos ๐ผ๐ง) โ(1 โ cos ๐ผ๐ฅ) 2 sin ๐ผ๐ฅ 1 sin ๐ผ๐ฅ][ ๐2๐๐ฅ โ ๐1๐๐ฅ ๐2๐๐ฆ โ ๐1๐๐ฆ ๐3๐๐ฆโ ๐1๐๐ฆ] ( 4.38 ) (1 โ cos ๐ผ) sin ๐ผ = 2 sin2๐ผ2
2 sin๐ผ2 cos๐ผ2 = tan ๐ผ 2 ( 4.39 ) [ ๐๐ก๐๐โฒ ๐ฅ ๐๐ก๐๐โฒ ๐ฆ ๐๐ก๐๐โฒ ๐ง] = [ 1 2 โ 1 2 tan๐ผ2๐ง 0 1 2tan๐ผ2๐ง 1 2 0 โ1 2 tan๐ผ2๐ฅ tan๐ผ2๐ง โ 1 2tan ๐ผ๐ฅ 2 1 sin ๐ผ๐ฅ ] [ ๐2๐๐ฅ โ ๐1๐๐ฅ ๐2๐๐ฆ โ ๐1๐๐ฆ ๐3๐๐ฆโ ๐1๐๐ฆ] ( 4.40 )
All the terms of ( 4.40 ) are known then, as shown in Statement 4.3, the solution of ๐๐ก๐๐ can be computed as follows:
๐๐ก๐๐= ๐ 1๐โ1โ ๐๐ก๐๐โฒ ( 4.41 )
44
E
RROR ANALYSISBecause of the translations performed by the robot and the camera discretization, the analytic solution 4.5.2 to compute the TCP has some unavoidable errors. Suppose that for each translations difference term of equation ( 4.40 ) there is an error ๐๐ โ (โ๐๐๐๐ฅ, ๐๐๐๐ฅ) where ๐ โ [1,2,3] is the corresponding row index:
[ ๐๐ก๐๐โฒ ๐ฅ+ ๐๐ฅ ๐๐ก๐๐โฒ ๐ฆ+ ๐๐ฆ ๐๐ก๐๐โฒ ๐ง+ ๐๐ง] = [ 1 2 โ 1 2 tan๐ผ๐ง 2 0 1 2tan๐ผ2๐ง 1 2 0 โ1 2 tan๐ผ2๐ฅ tan๐ผ2๐ง โ 1 2tan ๐ผ๐ฅ 2 1 sin ๐ผ๐ฅ] [ ๐2๐๐ฅ โ ๐1๐๐ฅ+ ๐1 ๐2๐๐ฆ โ ๐1๐๐ฆ+ ๐2 ๐3๐๐ฆโ ๐1๐๐ฆ + ๐3] ( 4.42 )
(๐๐ฅ, ๐๐ฆ, ๐๐ง) is the distance vector from the real solution. This error can be separated
from equation ( 4.42 ): [ ๐๐ฅ ๐๐ฆ ๐๐ง] = [ 1 2 โ 1 2 tan๐ผ2๐ง 0 1 2tan๐ผ2๐ง 1 2 0 โ1 2 tan๐ผ2๐ฅ tan๐ผ2๐ง โ 1 2tan ๐ผ๐ฅ 2 1 sin ๐ผ๐ฅ] [ ๐1 ๐2 ๐3] ( 4.43 )
Matrix on equation ( 4.43 ) represents the error amplification of the method used to compute the TCP as a function of the rotation angle ๐ผ๐ฅ and ๐ผ๐ง. Note that with these values tending to zero the errors will tend to infinity.
The minimum amplification for each error corresponds to the optimal values ๐ผ๐ง = ๐ and ๐ผ๐ฅ= ๐
45 [ ๐๐ฅ ๐๐ฆ ๐๐ง] = [ 1 2 0 0 0 1 2 0 0 โ1 2 1] [ ๐1 ๐2 ๐3] ( 4.44 )
Since the rotations are centered on the flange, to keep the TCP detectable by the camera, the rotation angles must be small, like one or two degrees, but moving ๐ผ๐ง and ๐ผ๐ฅ away from optimal values, errors will be amplified. Then applying the
method described in section 4.5.2 with this kind of rotations, the ๐๐ก๐๐ computed will be inaccurate, however the result can be used to set a new rotation center to compute the ๐๐ก๐๐ a second time using widest rotations and keeping the tool detectable by the camera.
If the errors amplification were still too high, the error ๐๐ง could be reduced with another rotation: after the second rotation around the x-axis with ๐ผ๐ฅ= ๐
2, the plane
๐ฅ๐ง will be parallel to the camera image plane. Now the same method used to compute ๐๐ก๐๐โฒ
๐ฅ and ๐๐ก๐๐ โฒ
๐ฆ ( 4.30 ) can be used to compute ๐๐ก๐๐ โฒ ๐ฅ and ๐๐ก๐๐ โฒ ๐ง replacing ๐๐ก๐๐โฒ ๐ฆ with ๐๐ก๐๐ โฒ
๐ง, performing a the rotation around the y-axis instead of z-axis and
moving the flange with Algorithm 4.1 along z-axis instead of y-axis. Then replacing only the resulting row related to ๐๐ก๐๐โฒ
๐ง, ( 4.42 ) can be rewrite as follows:
[ ๐๐ก๐๐โฒ ๐ฅ+ ๐๐ฅ ๐๐ก๐๐โฒ ๐ฆ+ ๐๐ฆ ๐๐ก๐๐โฒ ๐ง+ ๐๐ง] = [ 1 2 โ 1 2 tan๐ผ2๐ง 0 1 2tan๐ผ2๐ง 1 2 0 1 2tan๐ผ2๐ฆ 0 1 2 ] [ ๐2๐๐ฅ โ ๐1๐๐ฅ+ ๐1 ๐2๐๐ฆ โ ๐1๐๐ฆ+ ๐2 ๐4๐๐งโ ๐1๐๐ง + ๐3] ( 4.45 )
Where ๐ผ๐ฆ is the rotation around the y-axis.
46 [ ๐๐ฅ ๐๐ฆ ๐๐ง] = [ 1 2 0 0 0 1 2 0 0 0 1 2][ ๐1 ๐2 ๐3] ( 4.46 )
However there could be not enough space to perform this kind of rotations. Then the first method (only two rotation around z-axis and x-axis without constraints on rotations angles) could be more suitable for the purpose.
For instance ๐ผ๐ง = ๐ผ๐ฅ= ๐
4 on equation ( 4.42 ) could be the right compromise
between precision and performance:
[ ๐๐ฅ ๐๐ฆ ๐๐ง] = [ 1 2 โ โ2 4 โ 2โ2 0 โ2 4 โ 2โ2 1 2 0 โ1 2 โ 2 โ โ2 2โ2 โ2 2 ] [ ๐1 ๐2 ๐3] ( 4.47 ) [ ๐๐ฅ ๐๐ฆ ๐๐ง] = [ 0,5 โ1,21 0 1,21 0,5 0 โ0,5 โ0,21 1,41][ ๐1 ๐2 ๐3] ( 4.48 )