Dynamic Path Tracking of Industrial Robots With High Accuracy Using Photogrammetry Sensor

IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
1159
Dynamic Path Tracking of Industrial Robots With
High Accuracy Using Photogrammetry Sensor
Tingting Shu
, Sepehr Gharaaty, WenFang Xie , Senior Member, IEEE, Ahmed Joubair,
and Ilian A. Bonev , Senior Member, IEEE
Abstract—In this paper, a practical dynamic path tracking (DPT) scheme for industrial robots is presented. The
DPT scheme is a position-based visual servoing to realize three-dimensional dynamic path tracking by correcting
the robot movement in real time. In the traditional taskimplementation mode for industrial robots, the task planning and implementation are taught manually and hence
the task accuracy largely depends on the repeatability of
industrial robots. The proposed DPT scheme can realize automatic preplanned task and improve the tracking accuracy
with eye-to-hand photogrammetry measurement feedback.
Moreover, an adaptive Kalman filter is proposed to obtain
smooth pose estimation and reduce the influence caused by
image noise, vibration, and other uncertain disturbances.
Due to high repeatability of the photogrammetry sensor,
the proposed DPT scheme can achieve a high path tracking
accuracy. The developed DPT scheme can be seamlessly integrated with the industrial robot controller and improve the
robot’s accuracy without retrofitting with high-end encoder.
By using C-track 780 from Creaform as the photogrammetry sensor, the experimental tests on Fanuc M20-iA with
the developed DPT scheme demonstrate the tracking accuracy is significantly improved (±0.20 mm for position and
±0.10 deg for orientation).
Index Terms—Dynamic path tracking (DPT), industrial
robots, robot accuracy, visual servoing.
I. INTRODUCTION
N INDUSTRIAL manufacturing fields, many tasks, such as
cutting, milling, and lathing, are expected to use robots to
implement operation automatically. According to the standard
I
Manuscript received March 21, 2017; revised October 18, 2017 and
February 5, 2018; accepted March 13, 2018. Date of publication March
30, 2018; date of current version June 12, 2018. Recommended by Technical Editor Prof. Hong Qiao. This work was supported in part by the Natural Sciences and Engineering Research Council of Canada (NSERC),
in part by the Collaborative Research and Development (CRD) program,
and in part by the Consortium for Research and Innovation in Aerospace
in Quebec (CRIAQ). (Corresponding author: WenFang Xie.)
T. Shu is with the Department of Mechanical, Industrial & Aerospace
Engineering, Faculty of Engineering and Computer Science, the
Concordia University, Montreal, QC H3G 1M8, Canada (e-mail:
[email protected]).
S. Gharaaty and W. Xie are with the Concordia University, Montreal,
QC H3G 1M8, Canada (e-mail: [email protected]; wfxie@
encs.concordia.ca).
A. Joubair and I. A. Bonev are with the École de technologie
suprieure, Montreal, QC H3C 1K3, Canada (e-mail: ahmed.joubair.
[email protected]; [email protected]).
Color versions of one or more of the figures in this paper are available
online at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TMECH.2018.2821600
process specifications in the aerospace industry (e.g, Airbus [1]),
the desired accuracy of robot manipulation for manufacturing is
around ±0.20 mm. However, due to the mechanical tolerances
and deflection in the robot structure, the typical difference of a
virtual robot in simulation and a real robot can be 8–15 mm [2],
which is inadequate to meet the precision requirements of many
potential applications. Therefore, the relatively low accuracy
of current robots is the main problem for the industrial manufacturing applications. Especially, it poses a critical obstacle to
use advanced task planning techniques which integrate offline
simulation and computer aided design-based methods.
In the past decades, many researchers have tried to improve
the accuracy of industrial robots. One direct method is to install
secondary high-accuracy encoders at each robot joint [3] and
[4]. Since the encoder installation for each robot needs to be
customized and the price of high-accuracy encoder tends to be
high, this solution is costly and sometimes infeasible for all
the joints. The other solutions may be classified into two main
categories: 1) static calibration [5], [6] and [7]; and 2) dynamic
position or pose (i.e., position and orientation) adjustment [8],
[9] and [10]. The static calibration is mostly based on kinematic
models. Sometimes a partial dynamic model may be involved
as well, such as joint or link compliance, or backlash. Because
the static calibration considers factors that are time invariant, it
can only increase the robot accuracy to a limited extent under
static or quasi-static conditions. In a robot application where
a high-precision tracking of various trajectories is demanded,
dynamic continuous strategies for enhancing real-time tracking
accuracy become necessary. Hence, visual servoing is the main
approach to meet the demand.
Visual servoing for industrial robots is based on the visual
measurement feedback of the end-effector or reference objects.
Visual servoing is a multidisciplinary research topic involving computer vision, robotic kinematics, nonlinear dynamics,
control theory, as well as real-time systems. According to the
features extracted as feedback of controller, visual servoing can
be classified into three categories:
1) position-based visual servoing (PBVS) [8] and [11];
2) image-based visual servoing [12];
3) hybrid visual servoing [13].
According to the location of the vision system, the configuration of visual seroving is classified as either eye-to-hand
or eye-in-hand. In PBVS, the actual pose of the related object
in Cartesian space is estimated according to the feature points
on the image plane. Correspondingly, the control input for the
1083-4435 © 2018 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications standards/publications/rights/index.html for more information.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1160
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
robots is based on the errors between the current pose and desired pose of the end-effector. In this paper, since dynamic path
tracking (DPT) aims at correcting the end-effector’s pose in the
industrial applications by using a photogrammetry sensor, an
eye-to-hand PBVS scheme is adopted.
In PBVS, the pose measurements are computed from the
current image information of the feature points, which tend to
include distortion, blur, and other uncertain noise. In the recent decades, Kalman filter has been widely applied in robot
vision for optimal or suboptimal estimation [14] and [15], and
has started to be used in coordinate measuring machine (CMM)
[16]. Normally, a linear dynamic process model is required, and
a priori statistical information of the process and measurement
noise are assumed to be known. However, industrial robots’
mechanism is highly nonlinear and the pose estimation from
image information captured by the eye-to-hand cameras is a
nonlinear computation. In order to apply Kalman filter to estimate the pose of industrial robots, an extended Kalman filter
(EKF) was employed by many researchers [8], [17]. A comprehensive comparison on various Kalman filter schemes for vision
system is carried out in [17]. However, the optimality of EKF
depends on the accuracy of dynamic process model, which is
hard to obtain. Recently, the adaptive Kalman filter (AKF) has
attracted more and more attention from researchers attempting
to overcome the problems and adapting to complicated situations without knowing the certain system model and precise
priori information about the noises [14], [18]. For industrial
robots, the process and measurement noises are time variant as
the robots move at different speeds, with various configurations,
and along different trajectories. The Kalman filter method for
metrology tool with application to CMM in [16] has not considered this time-varying effect. In this paper, an effective AKF
is proposed to achieve the smooth pose estimation considering
both the noise features from photogrammetry sensor and the
vibration during robot movement. The proposed AKF can optimize the measurement-noise covariance according to the current
velocity of the industrial robot and the process-noise covariance
by using an adaptive method.
A lot of research work has been dedicated to using visual
information to improve the robot accuracy. For example, an offline compensation method is presented in [19], which measures
the sources of robot error and applies them to predict reference
path off-line by using Nikon K-Series optimal CMM. Although
it is reported that the robot accuracy can be increased within
0.2 mm, this method needs experts to carry out extensive tests
to predict the reference path for each different path and robot.
An on-line compensation method is demanded, which does not
need off-line reference path prediction. In [20], a feed-forward
torque control cascaded with an iterative learning control (ILC)
is proposed to enhance the path accuracy of industrial robots.
The experimental results on a KUKA KR 6-2 are provided only
in position errors and the maximum error on single axis can
be up to 2.0 mm, which apparently cannot satisfy the aforementioned accuracy for manufacturing in aerospace industry.
Moreover, both position and orientation accuracy, i.e., pose accuracy, are sought after by industrial manufacturers. In [21], an
ILC combined with a PD feedback control is applied to improve
the tracking performance of an industrial robot. The accuracy
Fig. 1. Definition and relation of the coordinate reference frames in the
workspace.
is demonstrated in joint space and maximum error of the joint
angle is larger than 0.1 deg. In addition, pose accuracy is not
equivalent to the joint angle accuracy, which is based on the
kinematic model with both geometric and nongeometric errors.
In this paper, motivated by the industrial need for high accuracy, we propose the DPT scheme to improve the accuracy of
industrial robots up to 0.2 mm in distance error and 0.05 deg in
orientation error on-line, and adapt to different paths automatically. The DPT scheme can be seamlessly integrated with robot
controller and the low-cost CMM photogrammetry sensor, i.e.,
C-track 780 from Creaform. The stability of the DPT scheme
is proved by using Lyapunov stability theory. The successful
application on industrial Fanuc M20-iA using C-track 780 as
visual measurement sensor demonstrates the effectiveness of
the proposed DPT scheme.
The rest of this paper is organized as the following. In
Section II, the workspace description and problem statement are
provided. Section III presents the complete process to obtain the
pose estimation of the end-effector from the dual-cameras image
information. Then, Section IV describes an adaptive Kalman
filter to reduce the noise influence from the pose estimation.
Section V introduces the basic control configuration of the DPT
scheme, tracking control-law design and stability analysis. The
simulation results on Puma 560 are provided to prove the robustness of the DPT scheme. The experimental results on Fanuc
M20-iA are presented in Section VI. Finally, the concluding
remarks and future work are summarized in Section VII.
II. WORKSPACE DESCRIPTION AND PROBLEM STATEMENT
A. Workspace Description
Consider a 6-DOF industrial robot with an end-effector in
Fig. 1. Generally, the default tool frame is defined at the center
of the flange of Joint 6, P0 . A tool frame FE with origin at
the tool center point (TCP) can be defined by offsetting the
default tool frame to the TCP with the known relative position
information. Assume that the end-effector can reach anywhere
in the reachable workspace WR of the robot. In the workspace,
a user frame FU R for the robot can be defined with respect to
the base frame FB .
On another note, when a task is given, the task space WT
(WT ⊂ WR ) can be defined. In the task space, a user frame
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
SHU et al.: DYNAMIC PATH TRACKING OF INDUSTRIAL ROBOTS WITH HIGH ACCURACY USING PHOTOGRAMMETRY SENSOR
FU T for the task can be defined. Correspondingly, the path
planned for this task can be represented in FU T . The task path
is used as the desired trajectory which the TCP should follow
precisely.
Moreover, in terms of the three-dimensional (3-D) visual
measurement instrument (VMI), there is default sensor frame
FS whose origin is at the center of VMI. User frame FU T can
be calibrated by using VMI to be represented in FS . In order to
locate TCP during any movement, the end-effector should be in
the field-of-view (FOV) of VMI. The pose of TCP in FS is represented by the pose of the tool frame FE in FS . Therefore, the
S
homogeneous transform matrix UST H from FU T to FS and EH
from FS to FE at any time can be obtained. The homogeneous
UT
transformation matrix E H from FU T to FE can be obtained as
follows:
UT
E
H = UST H EH.
S
achieved by first mapping FE in FS and then projecting them
onto the image plane. Since a dual-camera sensor C-track 780
is employed as the visual tacking instrument, the pose estimation principle about binocular vision is presented in this section. C-track 780 can provide continuous image acquisition and
transmission in real time. Moreover, C-track 780 can track the
reference model, including a rigid set of reflectors, which work
as the feature points. It is assumed that there are n feature
points, n > 3, on the rigid end-effector, and the homogeneous
coordinates of each feature point in sensor frame are denoted as
S
Pi = (xi , yi , zi , 1), i = 1 . . . n, while the projection coordinates of each feature point on the image plane of the jth camera
is represented as C Pij = (uij , vij , 1), i = 1 . . . n and j = 1, 2,
where j is the number of dual cameras. The perspective projection can be given as follows [22]:
C
(1)
In order to set up a relation between FU R and FU T , another equivalent user frame FU E q is introduced. Ideally, there
is only translation and no rotation between FU E q and FU R in
workspace. The main difference is that FU R is represented in
FB while FU E q in FS . FU E q in FS can be defined by using these
three points. The definition and relation of the abovementioned
coordinate frames are illustrated in Fig. 1.
B. Problem Statement
When the industrial robots are designated for cutting, painting, and fiber placement in the aerospace industry, the corresponding finite task path with a start point and end point should
be planned in advance. The robots are expected to move from
the start point to end point by strictly following the task path
with high accuracy.
Traditionally, the task path is planned in robot user frame
FU R by using teach pendant to record the point nodes or to
input the point nodes manually. Between the position nodes, the
path is produced by using certain interpolation of several existing interpolation methods in the robot controller. In this paper,
the task path defined in task user frame FU T is continuously
differentiable and independent of the robot controller so that the
task path can be preplanned in task space WT . Any task path P
in FU T can be described as follows:
px = x(η),
py = y(η),
pz = z(η)
pγ = γ(η),
pβ = β(η),
pα = α(η)
(2)
where η ∈ [0, 1] is a normalized variable. η = 0 corresponds to
the start point on P while η = 1 corresponds to the end point on
P. During 0 < η < 1, continuous position (px , py , pz ) forms the
geometry profile of P while (pγ , pβ , pα ) is the orientation of FE
in FU T . P defined in (2) is served as the desired path. In order
to carry out the task, the control objective is to control the TCP
to track (px , py , pz ) along the desired path with (pγ , pβ , pα )
satisfying the error tolerance.
III. POSE ESTIMATION OF THE END-EFFECTOR
The end-effector pose estimation is the localization of the
tool frame FE with respect to the sensor frame FS . This is
1161
Pij = C Xj S Pi ,
C
Xj = Bj SHj
C
(3)
where j = 1, 2; C Xj , is the projection matrix of each camera;
Bj is the camera matrix, integrating the intrinsic parameters of
C
the jth camera; SHj is the homogeneous transformation matrix
from the sensor frame FS to the jth camera frame. When the
C
dual-cameras’ sensor is calibrated, Bj and SHj are known.
C
Therefore, when Pij is obtained in the image plane, S Pi can
be computed.
Ideally, S Pi computed from each camera should be the same.
However, due to the distortion, calibration errors, and other
noises, there is a difference between two results of S Pi from
dual cameras. The triangulation is the main way to balance the
difference in the results [23]. In order to ensure a matching
pair of points, C Pi1 and C Pi2 , meet in space, the following
constraint should be satisfied:
C
PTi1 GC Pi2 = 0
(4)
where G is the fundamental matrix that can be computed when
dual camera projection matrices, C M1 and C M2 , are given.
Due to the uncertainty of image processing, (4) may not be
satisfied accurately. According to optimal correction principle
of Kanatani [24], the objective function is
ˆ i1 + d C Pi2 , C P
ˆ i2
d C Pi1 , C P
(5)
min
C
ˆ i 2 =0
ˆ i 1T G C P
P
where min(.) represents the minimization function subject to the
ˆ i1 T GC P
ˆ i2 = 0, d(∗, ∗) denotes Euclidean disconstraint C P
ˆ i2 are the estimated points of C Pi1 and
ˆ i1 and C P
tance, C P
C
ˆ i1 and C P
ˆ i2 can be obtained
Pi2 , respectively. As a result, C P
by the following formulas:
C
C
C
ˆ i1 = C Pi1 − Pi1 , G Pi2 VG Pi2
CP
ω
C
C
TC
ˆ i2 = C Pi2 − Pi1 , G Pi2 VG Pi1
CP
ω
C
TC
C
ω = G Pi2 , VG Pi2 + G Pi1 , VGT C Pi1 (6)
where in this paper, the inner product of two vectors a and b
is denoted as (a, b); projection matrix V ≡ diag(1, 1, 0). Then,
by (3), the coordinate of ith feature point in sensor frame, S Pi
can be obtained.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1162
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
After the position information of all the feature points on
the end-effector is prepared, the pose estimation of the endeffector can be developed. Suppose n feature points on the rigid
end-effector are fixed and known from the definition of the
tool frame FE , whose homogeneous coordinates are denoted
as E Pi = (E xi , E yi , E zi , 1). It is assumed the current pose
of FE in FS is denoted as (xc , yc , zc , γc , βc , αc ). (xc , yc , zc )
represents the origin position of FE in FS , while the orientation (γc , βc , αc ) represents the Euler-angle rotation from FE
in FS . With the pose (xc , yc , zc , γc , βc , αc ) of FE in FS , the
S
homogeneous transformation matrix EH from FE to FS can be
formulated as follows:
R(γc , βc , αc ) (xc , yc , zc )T
S
(7)
EH =
000
1
k − 1 is the previous time instant. Suppose that the current state
vector of the industrial robot is represented as follows:
where R(γc , βc , αc ) is rotation matrix from FE to FS . Correspondingly, the transformation equation of ith feature point can
be written as
where A is 12 × 12 state transition matrix which is applied to
the previous state ρ̂k −1,k −1 to obtain the current predicted state
ρ̂k ,k −1 ; Wk ,k −1 is the current prediction of the error covariance
matrix, which is a measure of the accuracy of the state estimate
while Wk −1,k −1 are the previous error covariance matrix; Qk −1
is the process noise covariance computed from the information
at time instant k − 1. All the diagonal elements of A are 1
and Ai,i+6 (i = 1 . . . 6) are equal to the sampling interval Ts
since these items will multiply the covariance of velocity in
Wk −1,k −1 to predict the states. ρ̂0,0 and W0,0 are initialized by
using the static samples before path tracking movement. Second,
the Kalman filter gain Dk is computed as follows:
S
Pi = EH E Pi
S
(8)
which can be unfolded into three nonlinear equations with six
unknown variables (xc , yc , zc , γc , βc , αc ) as follows:
xi = xc + Cγ c Cβ c E xi + (Cγ c Sβ c Sα c − Sγ c Cα c )E yi
+ (Cγ c Sβ c Cα c + Sγ c Sα c )E zi
yi = yc + Sγ c Cβ c E xi + (Sγ c Sβ c Sα c + Sγ c Cα c )E yi
+ (Sγ c Sβ c Cα c − Cγ c Sα c )E zi
zi = zc − Sβ c xi + Cβ c Sα c yi + Cβ c Cα c zi
E
E
E
ρk ,k = (x(k), y(k), z(k), γ(k), β(k), α(k),
ẋ(k), ẏ(k), ż(k), γ̇(k), β̇(k), α̇(k))T
i.e., the pose and velocity of the end-effector at the current
time instant. The current measurement vector obtained from
Section III is denoted as ok = (xc , yc , zc , γc , βc , αc )T . Then, an
adaptive Kalman filter method is given as the following recursive
equations.
First, the prediction equations are given as follows:
ρ̂k ,k −1 = Aρ̂k −1,k −1
Wk ,k −1 = AWk −1,k −1 AT + Qk −1
Dk = Wk ,k −1 (Wk ,k −1 + Ωk −1 )−1
(9)
where in this paper, Ca = cos(a) and Sa = sin(a). In order to
solve (9) for (xc , yc , zc , γc , βc , αc ), at least three noncollinear
feature points are required [8]. However, as indicated in [25],
at least four coplanar feature points are necessary for a unique
solution while the additional noncoplanar feature points can
be used to improve the estimation accuracy with measurement
noise. Since the number of the feature points on the end-effector
n is more than three, (xc , yc , zc , γc , βc , αc ) can be determined
uniquely. By using the proprietary software VXelements provided by Creaform, the end-effector is defined as the tracking
model built based on the selected reflectors on the surface of the
end-effector. The positional and rotational information of the
tracking model with respect to sensor frame can be acquired,
recorded, or displayed simultaneously. Therefore, the computation to obtain the pose of the end-effector is carried out by
VXelements.
IV. ADAPTIVE KALMAN FILTER FOR SMOOTHING ESTIMATED
POSE OF THE END-EFFECTOR
The presence of noise is inevitable in the image information
from the 3-D VMI. Moreover, the movement of the end-effector
is highly likely to cause vibration, blur, and distortion to the
images. Therefore, in this Section, an adaptive Kalman filter is
presented to smooth the estimated pose data of the end-effector.
The sampling interval of the VMI is denoted as Ts . For convenience, k instead of kTs represents the current time instant and
(10)
(11)
(12)
where Ωk −1 is the previous measurement noise covariance.
Third, the estimation updating is formulated as follows:
ρ̂k ,k = ρ̂k ,k −1 + Dk (ok − ρ̂k ,k −1 )
Wk ,k = Wk ,k −1 − Dk Wk ,k −1
(13)
where ρ̂k ,k is produced as the optimal pose of the end-effector
at time instant k.
The proper selection or updating of Qk and Ωk are critical
for the accuracy of filtered pose. Qk and Ωk are symmetric
positive definite matrices. When the robot is stationary, Ωk can
be easily obtained by using the root-mean-square error of the
static measurements from VMI. However, Ωk is a time-varying
matrix when the robot is moving because the higher velocity
will make the delay, blur, vibration, and other uncertain factors
worse.
A new effective method is proposed to update matrix Ωk
based on the velocity of the TCP as follows:
ΔΩk = diag μ1 ẋ(k), μ2 ẏ(k), μ3 ż(k), μ4 γ̇(k),
μ5 β̇(k), μ6 α̇(k) , Ωk = Ω0 + ΔΩk
(14)
where diag[ ] represents a diagonal matrix whose elements are
shown in the bracket; Ω0 is the measurement noise covariance
matrix when the TCP is stationary before starting from the start
point; μi , i = 1, . . . , 6 are the constant weights determining the
influence of variable velocity, which is estimated by analyzing
the stationary measurements of C-track in workspace.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
SHU et al.: DYNAMIC PATH TRACKING OF INDUSTRIAL ROBOTS WITH HIGH ACCURACY USING PHOTOGRAMMETRY SENSOR
Fig. 3.
Fig. 2.
Basic configuration of dynamic path tracking control.
An adaptive recursive method is presented to optimize Qk
to compensate the prediction error and uncertain dynamic disturbance [18], [26]. The predictor error ˆk can be estimated
adaptively as follows:
ˆk = ρ̂k ,k − Aρ̂k −1,k −1 .
(15)
Additionally, let ΔWk = AWk −1,k −1 AT − Wk ,k . Then, Qk
can be computed recursively as follows:
1
¯k = ¯k −1 + (ˆ
k − ˆk −N )
N
1
((ˆ
k − ¯k )(ˆ
Qk = Qk −1 +
k − ¯k )T
N −1
− (ˆ
k −N − ¯k ) (ˆ
k −N − ¯k )T
+
1
(ˆ
k − ˆk −N )(ˆ
k − ˆk −N )T
N (N − 1)
1
(ΔWk −N − ΔWk )
(16)
N
where N is the length of the past measurements memory for
updating Qk . Moreover, in order to ensure that Qk is a positive
definite, the diagonal elements need to be reset to their absolute
values.
+
V. DYNAMIC PATH TRACKING CONTROL
Since the task path P is not planned in FU R or FB , it is
not compatible in robot controller. Besides, only finite separate
points, not consecutive geometry curves, can be imported into
the robot controller. Most importantly, the low accuracy and
repeatability of the robot cannot guarantee that the TCP will
track the task path accurately with the robot controller.
A. Basic Control Structure
In this paper, a PBVS scheme is presented to realize task path
3-D tracking by correcting the movement of the end-effector in
real time. The basic control configuration consists of four parts
as shown in Fig. 2. The first part includes robot controller and
robot. Normally, each industrial robot comes with its proprietary robot controller. Only through the robot controller can the
robot parameters be accessed and robot movement command be
realized. The second part is the visual measurement part, which
1163
Path analyzing method of dynamic path tracking control.
includes 3-D visual measurement instrument (VMI) and signal
filtering that estimates the pose feedback information. The third
part is the path analyzer, which computes the pose of the closest point on the desired task path according to the current TCP
pose, and decides the next path step. The fourth part is the path
tracking control, which includes the control algorithm to produce the control input for robot controller based on the current
pose error. In Fig. 2, all the functions in the left dashed-line box
are implemented as software modules running on the computer
that is connected to the physical devices in the right dashed-line
box.
B. Pose Error Computation in Equivalent User Frame
When the end-effector is in the FOV of the 3-D VMI, the
current TCP pose information in FS can be obtained. Assume
η1 and η2 are both in (0, 1) and η2 > η1 . if Δη = η2 − η1 is
small enough, the curve segment on the task path P, described
in (2), can be regarded as a straight line segment approximately. During each path segment Δη, the pose information
(x(η2 ), y(η2 ), z(η2 ), γ(η2 ), β(η2 ), α(η2 )) of P at η2 is the current desired pose. A path analyzing method shown in Fig. 3 can
calculate the pose error and decide compensation input of the
next step. In this paper, a fixed control interval is used, which is
denoted as Tc and Tc = Δη, where is positive and relevant
to the tracking speed. At the end of each Tc , the current TCP
pose and velocity estimation, denoted as ρ̂k ,k , is obtained from
the output of Kalman filter.
Given the segment line between (η1 , η2 ) and the current
pose, it is simple to obtain the pose pd (k) of the closest
point from current point to the line. Moreover, pc (k) is used
to denote the pose part of ρ̂k ,k . Therefore, the pose error
ν(k) = (νx (k), νy (k), νz (k), νγ (k), νβ (k), να (k)) can be computed as follows:
ν(k) = pd (k) − pc (k).
(17)
Since the pose error ν(k) is represented in FU T in task
space, it is completely different from the robot user frame
FU R . In Section II-A, an equivalent user frame FU E q is introduced. Both FU T and FU E q are defined in FS . If ν(k) is
transformed into FU E q as a vector ς(k) = (ςx (k), ςy (k), ςz (k),
ςγ (k), ςβ (k), ςα (k)), the translational and rotational errors in
FU E q are the same as those in FU R . Suppose the homogeneous
transformation matrix of FU T and FU E q in FS is denoted as
S
S
U T H and U E q H, respectively. The homogeneous transformation matrix of FU T in FU E q is
UE q
UT
H. The translational part of
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1164
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
ς(k) can be computed from ν(k) as follows:
UEq
UT
H = UE q
S
H−1 US T H,
[ςx (k) ςy (k) ςz (k) 1]T = UUETq H [νx (k) νy (k) νz (k) 1]T .
(18)
For the rotational part of ς(k), the equivalent angle-axis
method in [27] is used. Generally, the rotational change is slower
than translational change. From the closest point pd (k) in Fig. 3,
the corresponding homogeneous matrix in FU T is UdT H while
the homogeneous matrix UrT H represents the current TCP pose
pc (k) in FU T . Therefore, the homogeneous matrix rd H that represents the transformation from the current TCP pose to the
closest point can be obtained as follows:
r
dH
= U Tr H−1
UT
dH
(19)
where rd R (3 × 3 matrix) is the left-upper part of rd H. Based on
the equivalent angle φ and axis r K can be derived. The
equivalent axis U E q K defined in FU E q is obtained as follows:
where Δp(k) denotes the small change of the pose of the endeffector in FU R , and Δq(k) is the corresponding change of the
joint position. In this paper, singularities are not considered.
Consequently, the current joint error q̃(k) = qd (k) − q(k),
where qd (k) is the desired joint position, can be obtained by
q̃(k) = J−1 (q(k))ς(k).
sd (k) = [qd (k − 1)T , qd (k)T ]T denotes the desired state at
time instant k. Since the task path P is continuously differentiable, an assumption can be made as
sd (k + 1) = sd (k) + f (k)
=
UE q
UT
U T H r H,
UE q
K=
UE q
r
rR K
where 3 × 3 rotation matrix
is the left-upper part of hoU
mogeneous matrix E qr H.
Corresponding to rotation operation rd R, the equivalent angle
in FU E q is the same as φ. Therefore, the rotation matrix U E q (rd R)
in FU E q can be derived. On the other hand, rotation operation
UE q r
(d R) can be realized by using fixed angles method [27] as
follows:
UE q r
(d R)
= Rotz (ςα (k)) Roty (ςβ (k)) Rotx (ςγ (k))
e(k) = Z(k) ς(k − 1)T , ς(k)T
Z(k) =
(20)
UE q
rR
(21)
where Rotx , Roty , and Rotz are the rotation matrix around xaxis, y-axis, and z-axis of FU E q with fixed angles ςγ (k), ςβ (k),
and ςα (k), respectively [27]. Since U E q (rd R) is known, ςγ (k),
ςβ (k), and ςα (k) can be computed through (21). ςγ (k), ςβ (k),
and ςα (k) are the rotational part of ς(k).
06×6
.
J−1 (q(k))
τ (k) = h(k) + Γe(k) + Λ(e(k) − e(k − 1))
(22)
where ṗ(k) is represented in FU R , and q(k) is the current
joint position. When the changes of the joint angles are very
small, based on (22), the following equations can be deduced
as follows:
(23)
(26)
(27)
where Λ = [diag[Λ1 , Λ2 , Λ3 , Λ4 , Λ5 , Λ6 ], 06×6 ] and Γ = [diag
[Γ1 , Γ2 , Γ3 , Γ4 , Γ5 , Γ6 ], 06×6 ] are control gain matrices; O(k)
h(k) = Y(k). Λi and Γi , i = 1, . . . , 6, are positive constants.
Therefore, according to (38), (25), and (27), e(k + 1) can be
obtained as follows:
e(k + 1) = sd (k + 1) − s(k + 1)
= (I − F(k))sd (k) + f (k) + (F(k) − O(k)Γ
− O(k)Λ)e(k) + O(k)Λe(k − 1)
= ξ1 + ξ2 e(k)
Both the translational and the rotational parts of pose error
ς(k) are prepared for path-tracking control block in Fig. 2. The
objective of path-tracking control block is to produce control
input for the robot controller so that all the six elements of ς(k)
converge to a certain boundary to meet the precision requirement. It is required that the TCP movement along the task path
P is smooth, and the control input is compatible with robot
controller and satisfies the saturation conditions.
According to [27], the Jacobian matrix J(q(k)) can be used
to relate the velocities of the end-effector ṗ(k) and robot joints
q̇(k) as follows:
Δp(k) = J(q(k))Δq(k)
J−1 (q(k − 1))
06×6
T
A control law is designed to stabilize closed-loop system as
follows:
C. Tracking Control Law Design
ṗ(k) = J(q(k))q̇(k)
(25)
where each element of f (k) ∈ R6 is a bounded function.
e(k) denotes the state error at time k as e(k) = sd (k) −
s(k) = [q̃(k − 1)T , q̃(k)T ]T . According to (24), e(k) can be
derived as
r
d R,
UE q
rH
(24)
(28)
where
ξ1 = (I − F(k))sd (k) + f (k) + O(k)Λe(k − 1)
ξ2 = F(k) − O(k)Γ − O(k)Λ.
(29)
Assume that at previous time instant k − 1, the state error
e(k − 1) is bounded. Moreover, sd (k) and f (k) are known to
be bounded. Therefore, ξ1 is bounded. ξ2 is mainly related to
control matrices Λ and Γ.
D. Stability Analysis
In this section, the stability of the proposed control law is
analyzed by using Lyapunov funcion. It is assumed the task
path P is reachable for the robot and visible in the FOV of
the VMI. Since only 6-DOF industrial robot is considered in
this paper, the robot is kinematically nonredundant so that the
Jacobian matrix J(q(k)) can satisfy nonsingularity along P.
Theorem 1: Consider the industrial robot (35) and a photogrammetry sensor C-track with eye-to-hand configuration.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
SHU et al.: DYNAMIC PATH TRACKING OF INDUSTRIAL ROBOTS WITH HIGH ACCURACY USING PHOTOGRAMMETRY SENSOR
Under the DPT control law (27), the robot can track the path in
the workspace with bounded pose error.
Proof: The Lyapunov function candidate is defined as
V (k) = e(k)T e(k)
1165
TABLE I
SIMULATION RESULTS OF DPT SCHEME ON PUMA 560 WITH SOME
DEVIATIONS ON J(q(k))
(30)
where V (k) ≥ 0 and V (k) = 0 only if e(k) = 0.
The time difference of (30) can be formed as
ΔV (k + 1) = V (k + 1) − V (k)
= e(k + 1)T e(k + 1) − e(k)T e(k)
= (ξ1 + ξ2 e(k))T (ξ1 + ξ2 e(k)) − e(k)T e(k)
≤ 2ξ1T ξ1 + 2(ξ2 e(k))T (ξ2 e(k)) − e(k)T e(k)
(31)
= 2ξ1T ξ1 + e(k)T 2ξ2T ξ2 − I e(k)
where by tuning the parameters of Λ and Γ, 2ξ2T ξ2 − I can be
negative definite. Therefore, ΔV (k + 1) ≤ 0 is satisfied as long
as the following inequality is satisfied:
2ξ1T ξ1 ≤ e(k) I − 2ξ2T ξ2 e(k) ≤ I − 2ξ2T ξ2 e(k)2 .
(32)
Correspondingly
e(k)2 ≥
2ξ1 2
2ξ1T ξ1
=
.
I − 2ξ2T ξ2 I − 2ξ2T ξ2 (33)
Fig. 4.
Then
√
e(k) ≥ 2ξ1 I − 2ξ2T ξ2 .
(34)
From (34), it is clear that e(k) is bounded by √
√
2ξ 1 I−2ξ 2T
ξ2
.
Also, by choosing suitable control parameters of Λ and Γ such
that ξ2T ξ2 is small, then e(k) can be minimized.
VI. SIMULATION AND EXPERIMENTAL RESULTS
In this section, the simulation of the DPT scheme based on
Puma 560 is carried out. Then, the DPT scheme is implemented
on the 6-DOF industrial manipulator, Fanuc M20-iA by using
the C-track 780 as dynamic pose-measuring instrument.
A. Simulation Results
Puma 560 is the first industrial robot, whose parameters are
well known and very similar to those of most of the modern
6-axis industrial robots [28]. The developed DPT scheme has
been implemented on Puma 560 in Matlab and with the help of
the Robotic Toolbox provided by Peter Corke [28]. Small deviations, e.g., 5% error, are imposed on Jacobian matrix J(q(k)).
The simulation results on the line tracking without path correction and with the proposed DPT scheme are presented in
Table I. It is noticed that after applying DPT scheme, the tracking errors resulted from the inaccurate J(q(k)) can be reduced
to ±0.05 mm for position and ±0.05 deg for orientation. Therefore, the simulation results demonstrate that the DPT scheme is
robust to some deviations on J(q(k)).
Experimental setup.
B. Experimental Preparation
The Fanuc M20-iA, shown in Fig. 4, is a 6-axis hollow-wrist
robot that has six revolute joints. The repeatability of Fanuc
M20-iA is 0.08 mm. The Fanuc M20-iA comes with the Fanuc
R30iB controller, which provides dynamic path modification
(DPM) function through Ethernet connection or an analog signal
input or digital input. 3-D pose modification with respect to robot
user frame FU R or tool frame FE can be realized when Fanuc
M20-iA is working in DPM mode. The Fanuc company provides
software package and PCDK toolkit to support the connected
computer to communicate with the robot controller by Ethernet.
The C-track 780 from Creaform Company, shown in Fig. 4, is
a type of dual-camera sensor with dynamic measurement capabilities, which can track the reference model, including a rigid
set of reflectors, and provide continuous image acquisition (3-D
measurements), which are transmitted to the control system in
real time. The reference model can be built by using some simple
passive reflectors that are cheap, adhesive, and magnetic. The
reflectors work as the feature points. The volumetric accuracy of
the C-track 780 is 0.065 mm and the repeatability of the C-track
780 is 0.0025 mm, both of which are given by root-mean-square
value.
In the experiment, an artificial tool with needle tip, as shown
in Fig. 4, is mounted at the center of flange on Joint 6 of Fanuc
M20-iA and is treated as the end effector. Fanuc provides a
built-in 6-points method to define a new tool frame on the tip
of the new tool, which is regarded as the TCP. By using this
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1166
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
TABLE II
POSE INFORMATION OF Pd 1 FOR THE END-EFFECTOR IN SENSOR FRAME
FS FOR EXPERIMENTS
method together with Vxelements functions, all the frames in
Fig. 1 can be defined.
The DPT scheme in Section V is implemented by the software
of the DPT module. The DPT module integrates Funuc PCDK
and Vxelements to realize communication with Fanuc R30iB
controller and C-track. The task path P is given as a straight
line in the task space. Since the maximum updating frequency of
C-track tracking data is 29 Hz, the control interval Tc should be
bigger than 34.48 ms(1/29 Hz). The basic interpolation interval
(ITP) of Fanuc M20-iA is 8 ms, the cycle at which the destination
is computed and sent to the motion environment. In DPM mode,
the maximum applicable offset value at each axis is 0.02 mm at
each ITP. In the experiments, the DPT module is running under
the Window 7.0 system on a high-performance computer with
Intel Xeon Processor E5-1650 v3 3.5GHz and NVIDIA Quadro
K2200 professional graphics board. Simultaneously, one teachpendant (TP) program in robot controller is running for actuating
the robot to move along the tool path P . When the robot starts
following the path it will send a digital output signal to the DPT
module so that the DPT module begins dynamic path correction.
When the robot reaches the end point of the path, it will send a
digital output signal to the DPT module as the stop sign for the
dynamic path correction.
C. Error Analysis for Fanuc M20-iA
Before implementing DPT, the error analysis for the Fanuc
M20-iA moving along straight line without using DPT for
path correction has been conducted. The distance error between
the current position and the desired path is defined as
νx (k)2 + νy (k)2 + νz (k)2 , where νx (k), νy (k), and νz (k)
are the position error in (17). The tests are carried out to compare the distance errors with and without pose correction when
the TCP is moving along Pd1 , as described in Table II. Fanuc
provides teach-pendant programming function to perform line
movement of TCP without pose correction. By programming
in teach pendant of Fanuc M20-iA, the two points in Table II
can be set as the start and end point of linear movement command, respectively. Then, the program is run in teach pendant.
The TCP of the end-effector will move in linear motion. The
forward speed and the reverse speed are 25 and 15 mm/s, respectively. Extensive experimental tests on Fanuc M20-iA path
tracking have been carried out. The following summary on the
error patterns during path tracking is drawn. The robot performance is configuration dependent. The distance errors exhibit
different patterns when the robot tracks the same line along
different directions and in different orientations [see Figs. (5)
Fig. 5. Distance error when Fanuc M20-iA moves forward along Pd 1 at
speed 15 mm/s without path correction.
Fig. 6. Distance error when Fanuc M20-iA moves backward along Pd 1
at speed 25 mm/s without path correction.
and (6)]. And when the robot tracks different paths, the distance
errors are different as well. The tracking speed also affects the
distance error. The vibration is observed in the distance error
due to the elasticity and backlash of the joint actuators [20].
Besides, high-frequency noise caused by the noisy measurements from C-Track appears on the distance error, which means
that the measurement from C-Track cannot be directly used as
the feedback in dynamic path tracking control. The distance
error is caused by three major factors: 1) robot nonlinearity,
2) low-frequency vibration of robot movement; and 3) highfrequency noise of C-Track measurement. The AKF aims to
remove the high-frequency noise and the PBVS aims to reduce
the distance error due to the nonlinearity and low-frequency
vibration. Therefore, in order to enhance the accuracy of path
tracking of Fanuc M20-iA, the proposed control scheme with a
proper noise filtering addresses all the above-mentioned factors.
D. Kalman Filter Initialization
According to (10)–(16) proposed in Section IV, the following
variables and parameters should be initialized properly to start
the Kalman filter.
The initial estimation ρ̂0,0 is initialized by using the current
pose estimation and setting velocity zero. The initial covariance
matrix W0,0 is set as a 12 × 12 identity matrix, which affects
the transient convergent speed of the Kalman filter.
The diagonal elements of A are 1 and Ai,i+6 (i = 1, . . . , 6)
is taken as Ts . Ideally, the current pose of the end-effector is
equal to the former six elements of the current state vector
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
SHU et al.: DYNAMIC PATH TRACKING OF INDUSTRIAL ROBOTS WITH HIGH ACCURACY USING PHOTOGRAMMETRY SENSOR
Fig. 7. Distance error in sensor frame FS when Fanuc M20-iA moves
forward along Pd 1 at speed 25 mm/s with path correction.
Fig. 8. Position error of the TCP in sensor frame FS when Fanuc M20iA moves forward along Pd 1 at speed 25 mm/s with path correction.
ρ̂k ,k . Ω0 is 0.001diag(0.15, 0.15, 0.8, 0.158, 0.143, 0.0219),
which are obtained by calculating the root-mean-square error of static measurements for fixed point in WR . The constant weights μi , i = 1, . . . , 6 for updating Ωk with velocity
changes are set as 1.5e − 6. Compared to Ω0 , Qk is initialized
as (1e − 6)diag(1, 1, 10, 1, 1, 1, 1, 1, 10, 1, 1, 2) and the length
N in (16) is 20.
1167
Fig. 9. Orientation error of the TCP in sensor frame FS when Fanuc
M20-iA moves forward along Pd 1 at speed 25 mm/s with path correction.
Fig. 10. Distance error in sensor frame FS when Fanuc M20-iA moves
along Pd 1 backward at speed 25 mm/s with path correction.
TABLE III
RESULTS COMPARISON IN SENSOR FRAME FS WHEN FANUC M20-IA
MOVES FORWARD ALONG Pd 1 AT SPEED 25 MM/ S AFTER 9 s
E. Control Parameters Initialization
The sampling interval Ts for C-Track is 1/29 sec, while 29 Hz
is the maximum frequency of C-track 780. Once the image
information from C-Track 780 is updated, the pose estimation
and AKF will work sequentially to obtain the pose information
with minimum delay. Therefore, the pose updating rate is equal
to 29 Hz. Additionally, the control period Tc for DPT module
is determined by considering Ts , ITP of Fanuc M20-iA and
maximum control step limit of Fanuc R30iB controller. For
the experiments in this research, Tc is set as 300 ms. Γ is set
as [diag(0.3, 0.3, 0.3, 0.15, 0.15, 0.15), 06×6 ], and Λ is set as
[diag(0.04, 0.04, 0.04, 0.02, 0.02, 0.02), 06×6 ].
F. Experimental Results
1) Line Tracking: Two experiments are carried out to obtain
the results of using DPT and AKF when Fanuc robot moves forward and backward along Pd1 at speed 25 mm/s, respectively.
The pose-updating rate of AKF is equal to the sampling rate of
C-track. Figs. (7)–(10) show the results by using DPT for path
correction. For tracking Pd1 , the TCP of Fanuc M20-iA starts at
TABLE IV
RESULTS COMPARISON IN SENSOR FRAME FS WHEN FANUC M20-IA
MOVES ALONG Pd 1 BACKWARD AT SPEED 25 MM/ S AFTER 9 s
the starting point and stops at the end point. Correspondingly,
the TCP accelerates at the beginning segment of the path and
decelerates at the final segment of the path. Due to the influence
of the acceleration, AKF can converge and follow the measured
data after around 3 sec. Then, after around 6 sec, the distance
error will keep less than ±0.2 mm. Tables III and IV show the
comparison results after 9 sec. According to Tables III and IV,
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1168
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
TABLE V
CIRCLE CENTER IN SENSOR FRAME FS FOR EXPERIMENTS
Fig. 13. Distance error in sensor frame FS when Fanuc M20-iA moves
along Pd 2 at speed 25 mm/s with path correction.
TABLE VI
RESULTS COMPARISON IN SENSOR FRAME FS WHEN FANUC M20-IA
MOVES ALONG Pd 2 AND Pd 3 AT SPEED 25 MM/ S
Fig. 11. Distance error in sensor frame FS when Fanuc M20-iA moves
along Pd 2 at speed 25 mm/s without path correction.
in terms of distance error and pose error between the TCP pose
and desired one. The experimental tests demonstrate the proposed DPT scheme can compensate for the calibration error,
vibration error, and uncertain model error in real time.
VII. CONCLUSION
Fig. 12. Trajectory of the TCP in sensor frame FS when Fanuc M20-iA
moves along Pd 2 at speed 25 mm/s with path correction.
the line-tracking accuracy is improved obviously by using DPT.
Therefore, the results demonstrate DPT scheme can improve the
line-tracking accuracy up to ±0.1 mm for position accuracy and
±0.05 deg for rotational accuracy, respectively.
2) Circle Tracking: Two groups of experiments are carried
out to compare the tracking accuracy when the TCP of the Fanuc
M20-iA is moving along two half circles at speed 25 mm/s, Pd2
and Pd3 , which have the common circle center in Table V and
different radii, 300 and 400 mm, respectively. Fig. 11 shows the
distance errors when Fanuc robot is moving along Pd2 at speed
25 mm/s without path correction, while Figs. (12) and (13) show
the results of using DPT for path correction. The comparison of
the results is summarized in Table VI where the distance error is
less than ±0.2 mm with path correction. During the circle tracking movement, the orientation of the end-effector is controlled
with respect to the circle center and the rotational accuracy is
up to ±0.1 deg. From the experimental results of both line and
circle tracking, one can see that the developed DPT scheme can
guide the TCP to follow the preplanned path with a high accuracy up to ±0.2 mm for position and ±0.1 deg for orientation.
Compared with the current robot controller without path correction, the path-tracking accuracy has been significantly improved
In this paper, a practical DPT scheme is proposed to command the robot to follow the desired path through the robot
controller. The DPT scheme adopts PBVS strategy and eyeto-hand configuration where a photogrammetry sensor–C-track
can measure the pose of the end-effector in real time. Moreover, in order to reduce the influence from the image noise, blur,
and distortion, an adaptive Kalman filter is proposed to process the pose estimation of the end-effector. The stability of the
DPT scheme is proved by using Lyapunov stability theory. The
experimental tests demonstrate the proposed DPT scheme can
improve the line-tracking accuracy to ±0.1 mm for position and
±0.05 deg for orientation, and the circle-tracking accuracy to
±0.2 mm for position and ±0.1 deg for orientation. The developed DPT scheme renders the industrial high-end robots without
retrofitting the robot with expensive encoders. The future work
includes applying the DPT scheme to other types of path tracking, such as S-shape curve, square, etc., and testing on the other
types of industrial robots, such as ABB and KUKA.
APPENDIX
DISCRETE DYNAMIC MODEL OF 6-LINK MANIPULATORS
The general dynamic model of 6-link manipulators [29] is
described by
M(q(t))q̈(t) + C(q(t), q̇(t))q̇(t) + g(q(t)) = τ (t)
(35)
where t denotes the time; q(t), q̇(t), and q̈(t) ∈ R are the
robot joint position, velocity, and acceleration vectors, respectively; M(q(t)) ∈ R6×6 is a positive-definite, symmetric inertia
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
6
SHU et al.: DYNAMIC PATH TRACKING OF INDUSTRIAL ROBOTS WITH HIGH ACCURACY USING PHOTOGRAMMETRY SENSOR
matrix; C(q(t), q̇(t))q̇(t)) ∈ R6 denotes the vector corresponding to centrifugal and Coriolis forces; g(q(t)) ∈ R6 is
the vector of the gravitational torques; τ (t) ∈ R6 denotes the
control input vector.
Assume that t = kTc . By using forward finite-difference for
discretization, substitute q̇i (t) = (qi (k) − qi (k − 1))/Tc and
q̈i (t) = (qi (k + 1) − 2qi (k) + qi (k − 1))/Tc2 (i = 1, . . . , 6)
into (35), the discrete state equation is formulated as
q(k)
06×6 I6×6
q(k − 1)
=
q(k)
δ(k) ξ(k)
q(k + 1)
+Tc
06×6
ζ(q(k))
τ (k) +
06×6
−ζ(q(k))
g(q(k))
(36)
where
δ(k) = Tc M−1 (q(k))C(q(k), q(k − 1)) − I
ξ(k) = 2I − M−1 (q(k))C(q(k), q(k − 1))
ζ(k) = Tc2 M−1 (q(k))
(37)
I is 6 × 6 identity matrix.
Let s(k) = [q(k − 1)T , q(k)T ]T . Then according to (36), the
state equation can be written as
s(k + 1) = F(k)s(k) − Y(k) + O(k)τ (k)
where
F(k) =
06×6
I6×6
δ(k) ξ(k)
06×6
O(k) = Tc
.
ζ(q(k))
, Y(k) =
06×6
ζ(q(k))
(38)
g(q(k)),
(39)
REFERENCES
[1] N. Holt, “Automotive robots reach for the sky,” 2007. [Online]. Available:
http://www.automotivelogisticsmagazine.com/uncategorized/automotive
-robots-r each-for-the-sky, Accessed on: Sep. 25, 2017.
[2] A. Robotics, “Absolute accuracy industrial robot option,” 2011.
[Online]. Available: https://library.e.abb.com/public/e69d8dd25cd7d36bc
125794400374679/AbsAc cPR10072EN_R6.pdf, Accessed on: Sep. 25,
2017.
[3] R. C. DeVlieg, “Robotic manufacturing system with accurate control,”
US Patent 8 989 898, Mar. 24, 2015.
[4] R. Devlieg and T. Szallay, “Applied accurate robotic drilling for aircraft
fuselage,” SAE Int. J. Aerosp., vol. 3, pp. 180–186, 2010.
[5] A. Joubair and I. A. Bonev, “Nonkinematic calibration of a six-axis serial
robot using planar constraints,” Precis. Eng., vol. 40, pp. 325–333, 2015.
[6] A. Nubiola and I. A. Bonev, “Absolute calibration of an ABB IRB 1600
robot using a laser tracker,” Robot. Comput.-Integr. Manuf., vol. 29, no. 1,
pp. 236–245, 2013.
[7] Y. M. Zhao, Y. Lin, F. Xi, and S. Guo, “Calibration-based iterative learning
control for path tracking of industrial robots,” IEEE Trans. Ind. Electron.,
vol. 62, no. 5, pp. 2921–2929, May 2015.
[8] W. J. Wilson, C. C. W. Hulls, and G. S. Bell, “Relative end-effector control using cartesian position-based visual servoing,” IEEE Trans. Robot.
Autom., vol. 12, no. 5, pp. 684–696, Oct. 1996.
[9] F. Janabi-Sharifi, L. Deng, and W. J. Wilson, “Comparison of basic visual servoing methods,” IEEE/ASME Trans. Mechatronics, vol. 16, no. 5,
pp. 967–983, Oct. 2011.
1169
[10] M. Keshmiri and W.-F. Xie, “Image-based visual servoing using an optimized trajectory planning technique,” IEEE/ASME Trans. Mechatronics,
vol. 22, no. 1, pp. 359–370, Feb. 2017.
[11] D.-H. Park, J.-H. Kwon, and I.-J. Ha, “Novel position-based visual servoing approach to robust global stability under field-of-view constraint,”
IEEE Trans. Ind. Electron., vol. 59, no. 12, pp. 4735–4752, Dec. 2012.
[12] A. Hajiloo, M. Keshmiri, W.-F. Xie, and T.-T. Wang, “Robust online model
predictive control for a constrained image-based visual servoing,” IEEE
Trans. Ind. Electron., vol. 63, no. 4, pp. 2242–2250, Apr. 2016.
[13] L. Deng, F. Janabi-Sharifi, and W. J. Wilson, “Hybrid motion control
and planning strategies for visual servoing,” IEEE Trans. Ind. Electron.,
vol. 52, no. 4, pp. 1024–1040, Aug. 2005.
[14] S. Y. Chen, “Kalman filter for robot vision: A survey,” IEEE Trans. Ind.
Electron., vol. 59, no. 11, pp. 4409–4420, Nov. 2012.
[15] E. V. Cuevas, D. Zaldivar, and R. Rojas, “Kalman filter for vision tracking.”
Freie Universitt Berlin, Inst. Informatik, Berlin, Germany, Tech. Rep. B
05-12, 2005.
[16] G. E. D’Errico, “A la Kalman filtering for metrology tool with application
to coordinate measuring machines,” IEEE Trans. Ind. Electron., vol. 59,
no. 11, pp. 4377–4382, Nov. 2012.
[17] F. Janabi-Sharifi, “Visual servoing: Theory and applications,” Optomechatronic Systems Handbook. Cleveland, OH, USA: CRC Press, 2003.
[18] M. Ficocelli and F. Janabi-Sharifi, “Adaptive filtering for pose estimation
in visual servoing,” in Proc. 2001 IEEE/RSJ Int. Conf. Intell. Robots Syst.,
2001, vol. 1, pp. 19–24.
[19] Nikon, “High accuracy robotics.” 2016. [Online]. Available:
http://www.nikonmetrology.com/en_US/Products/In-processmeasurement/Ada ptive-Robot-Control Accessed on: Sep. 25, 2017.
[20] P. Stückelmaier, M. Grotjahn, and C. Fräger, “Iterative improvement of
path accuracy of industrial robots using external measurements,” in Proc.
2017 IEEE Int. Conf. Adv. Intell. Mechatronics, 2017, pp. 688–693.
[21] T. Hsiao and P.-H. Huang, “Iterative learning control for trajectory tracking of robot manipulators,” Int. J. Autom. Smart Technol., vol. 7, no. 3,
pp. 133–139, 2017.
[22] Y. Shirai, Three-Dimensional Computer Vision. New York, NY, USA:
Springer-Verlag, 2012.
[23] R. I. Hartley and P. Sturm, “Triangulation,” Comput. Vis. Image Understanding, vol. 68, no. 2, pp. 146–157, 1997.
[24] K. Kanatani, Y. Sugaya, and H. Niitsuma, “Triangulation from two views
revisited: Hartley-Sturm versus optimal correction,” in Proc. Brit. Mach.
Conf., vol. 4, 2008, pp. 18.1–18.10.
[25] J. S.-C. Yuan, “A general photogrammetric method for determining object position and orientation,” IEEE Trans. Robot. Autom., vol. 5, no. 2,
pp. 129–142, Apr. 1989.
[26] F. Janabi-Sharifi and M. Marey, “A Kalman-filter-based method for pose
estimation in visual servoing,” IEEE Trans. Robot., vol. 26, no. 5, pp. 939–
947, Oct. 2010.
[27] J. J. Craig, Introduction to Robotics: Mechanics and Control. Upper Saddle
River, NJ, USA: Prentice-Hall, 2005.
[28] P. Corke, Robotics, Vision and Control: Fundamental Algorithms in MATLAB. New York, NY, USA: Springer-Verlag, 2011.
[29] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and
Control. Hoboken, NJ, USA: Wiley, 2006.
Tingting Shu received the B.Sc. and M.Sc. degrees in computer engineering from the Central China Normal University, Wuhan, China,
in 1998 and 2001, respectively. She has been
working toward the Ph.D. degree in mechatronics from 2014 at Concordia University, Montreal,
QC, Canada.
She was a Researcher with the Flight Control & Management and Visual Reality, Beihang
University, Beijing, China, from 2001 to 2014.
Her research interests include intelligent control,
robotic control, visual servoing, and iterative learning control.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.
1170
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 23, NO. 3, JUNE 2018
Sepehr Gharaaty received the B.Sc degree in
mechanical engineering from Isfahan University
of Technology, Isfahan, Iran, and the M.Sc. degree in mechatronics from Concordia University,
Montreal, QC, Canada, in 2013 and 2016, respectively.
He is currently a Robotic Developer with
Omnirobotic, Montreal, QC, Canada. His research interests include robotics and automation, control, precision, and visual servoing.
Ahmed Joubair received the B.Sc degree in automated manufacturing engineering (AME) from
ÉTS, Montreal, Canada, QC, the M.Sc. degree in
industrial engineering from Montreal University,
Montreal, QC, Canada, and the Ph.D. degree in
AME from ÉTS, in 2005, 2007, and 2012, respectively.
He is a Researcher with the Control and
Robotic Laboratory, ÉTS and a Lecturer with
the Department of Mechanical Engineering and
Automated Manufacturing engineering, ÉTS,
Canada. His research interests include robotics metrology and calibration, collaborative robotics, and advanced manufacturing.
WenFang Xie (SM’12) received the Ph.D. degree in electrical engineering from The Hong
Kong Polytechnic University, Hung Hom, Hong
Kong, in 1999, and the M.Sc. degree in automatic control from Beihang University, Beijing,
China, in 1991.
She is a Professor with the Department
of Mechanical, Industrial, and Aerospace Engineering, Concordia University, Montreal, QC,
Canada. Her research interests include nonlinear control and identification in mechatronics,
visual servoing, model predictive control, neural network, and system
identification.
Ilian Bonev (SM’10) received the Ph.D. and
a postdoctoral fellowship degrees in parallel
robotics from the Université Laval, Quebec City,
QC, Canada, and the M.Sc. degree in mechatronics from Gwangju Institute of Science and
Technology, Gwangju, South Korea.
Since 2004, he has been a Professor with
ÉTS, Montreal, QB, Canada, and a holder of the
Canada Research Chair in Precision Robotics.
He is also Head of the Control and Robotics
Lab, ÉTS (CoRo). His research interests mainly
include the areas of parallel robots, precision robotics, and collaborative
robots.
Authorized licensed use limited to: University of Nantes. Downloaded on April 06,2022 at 09:00:14 UTC from IEEE Xplore. Restrictions apply.