Tool Position Estimation of a Flexible IndustrialRobot using Recursive Bayesian Methods
Patrik Axelsson, Rickard Karlsson, andMikael Norrlöf
Division of Automatic ControlDepartment of Electrical EngineeringLinköping University, Sweden
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Outline 2(12)
1. Introduction• Problem Formulation• Bayesian Estimation
2. Modelling• Robot Model• Accelerometer Model• Estimation Model
3. Experimental Results
4. Conclusions and Future Work
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 3(12)
Controller Robot
Observer
qrefm u
d
X
qmXref
Can control
Want to measure
Want to control the TCP. Can only measure the motor angles qm.
Feedback of qm does not work sufficiently due to flexible joints.
If the TCP can be measured it is natural to feedback it.
What can we do instead of measuring the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 3(12)
Controller Robot
Observer
qrefm u
d
X
qmXref
Want to control
Can measure
Want to control the TCP. Can only measure the motor angles qm.
Feedback of qm does not work sufficiently due to flexible joints.
If the TCP can be measured it is natural to feedback it.
What can we do instead of measuring the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 3(12)
Controller Robot
Observer
qrefm u
d
X
qmXref
Want to control
Can measure
Want to control the TCP. Can only measure the motor angles qm.
Feedback of qm does not work sufficiently due to flexible joints.
If the TCP can be measured it is natural to feedback it.
What can we do instead of measuring the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 3(12)
Controller Robot
Observer
Xref u
d
X
qm
Want to control the TCP. Can only measure the motor angles qm.
Feedback of qm does not work sufficiently due to flexible joints.
If the TCP can be measured it is natural to feedback it.
What can we do instead of measuring the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 3(12)
Controller Robot
Observer
Xref u
d
X
qm
Want to control the TCP. Can only measure the motor angles qm.
Feedback of qm does not work sufficiently due to flexible joints.
If the TCP can be measured it is natural to feedback it.
What can we do instead of measuring the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 4(12)
Controller Robot
Observer
Xref u
d
X
qm
Let the acceleration of the tool be a measurement.
Use an observer to estimate the TCP.
How can we estimate the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 4(12)
Controller Robot
Observer
Xref u
d
X
qmX
Let the acceleration of the tool be a measurement.
Use an observer to estimate the TCP.
How can we estimate the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 4(12)
Controller Robot
Observer
Xref u
d
X
qmX
X
Let the acceleration of the tool be a measurement.
Use an observer to estimate the TCP.
How can we estimate the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Problem Formulation 4(12)
Controller Robot
Observer
Xref u
d
X
qmX
X
Let the acceleration of the tool be a measurement.
Use an observer to estimate the TCP.
How can we estimate the TCP?
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Bayesian State-space Estimation 5(12)
Model:
xt+1 = f (xt, ut, wt),
yt = h(xt) + et.
Bayesian inference:
p(xt+1|Yt) =∫
Rnp(xt+1|xt)p(xt|Yt)dxt,
p(xt|Yt) =p(yt|xt)p(xt|Yt−1)
p(yt|Yt−1).
The Kalman filter is the optimal choice for linear models.Approximative filters have to be used for nonlinear models.
In this work
• Extended Kalman filter (EKF)– Approximate the system with a linearisation of the nonlinear
equations.– Assume additive Gaussian noise.
• Particle filter (PF)– Approximate the posterior distribution with a large number of
particles.– The optimal proposal distribution approximated using an EKF.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Bayesian State-space Estimation 5(12)
Model:
xt+1 = f (xt, ut, wt),
yt = h(xt) + et.
Bayesian inference:
p(xt+1|Yt) =∫
Rnp(xt+1|xt)p(xt|Yt)dxt,
p(xt|Yt) =p(yt|xt)p(xt|Yt−1)
p(yt|Yt−1).
The Kalman filter is the optimal choice for linear models.Approximative filters have to be used for nonlinear models.In this work• Extended Kalman filter (EKF)
– Approximate the system with a linearisation of the nonlinearequations.
– Assume additive Gaussian noise.
• Particle filter (PF)– Approximate the posterior distribution with a large number of
particles.– The optimal proposal distribution approximated using an EKF.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Bayesian State-space Estimation 5(12)
Model:
xt+1 = f (xt, ut, wt),
yt = h(xt) + et.
Bayesian inference:
p(xt+1|Yt) =∫
Rnp(xt+1|xt)p(xt|Yt)dxt,
p(xt|Yt) =p(yt|xt)p(xt|Yt−1)
p(yt|Yt−1).
The Kalman filter is the optimal choice for linear models.Approximative filters have to be used for nonlinear models.In this work• Extended Kalman filter (EKF)
– Approximate the system with a linearisation of the nonlinearequations.
– Assume additive Gaussian noise.• Particle filter (PF)
– Approximate the posterior distribution with a large number ofparticles.
– The optimal proposal distribution approximated using an EKF.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Robot Model 6(12)
Serial robot with 2 DOF.
Linear stiffness.
No friction.
qa =(qa1 qa2
)T ,
qam =
(qm1/η1 qm2/η2
)T ,
τam =
(τm1η1 τm2η2
)T ,
bx
bz
1aq
2aq
SxSz
bρ
Ma(qa)qa + C(qa, qa) + G(qa) + T(qa − qam) + D(qa − qa
m) = 0,Mmqa
m − T(qa − qam)−D(qa − qa
m) = τam.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Robot Model 6(12)
Serial robot with 2 DOF.
Linear stiffness.
No friction.
qa =(qa1 qa2
)T ,
qam =
(qm1/η1 qm2/η2
)T ,
τam =
(τm1η1 τm2η2
)T ,
bx
bz
1aq
2aq
SxSz
bρ
Ma(qa)qa + C(qa, qa) + G(qa) + T(qa − qam) + D(qa − qa
m) = 0,Mmqa
m − T(qa − qam)−D(qa − qa
m) = τam.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Accelerometer Model 7(12)
Acceleration from the motion.
Acceleration due to thegravity.
Rotation matrix from Oxbzb toOxszs.
Bias parameter. bx
bz
1aq
2aq
SxSz
bρ
ρs(qa) = Rb/s (qa) (ρb(qa) + Gb) + bACC
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Estimation Model 8(12)
State space vector:
x =(xT
1 xT2 xT
3)T
=(qT
a qTa qT
a)T
Linear dynamic model (Double integrator in discrete time):
xk+1 = Fxk + Guuk + Gvvk
The input signal is the jerk of the arm angle reference.
Measurement equation:
yk =
(x1,k + K−1 (Ma(x1,k)x3,k + C(x1,k, x2,k) + G(x1,k))
Rb/s(x1,k)(
JACC(x1,k)x3,k +(
ddt JACC(x1,k)
)x2,k + Gb
))+ ek
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Estimation Model 8(12)
State space vector:
x =(xT
1 xT2 xT
3)T
=(qT
a qTa qT
a)T
Linear dynamic model (Double integrator in discrete time):
xk+1 = Fxk + Guuk + Gvvk
The input signal is the jerk of the arm angle reference.
Measurement equation:
yk =
(x1,k + K−1 (Ma(x1,k)x3,k + C(x1,k, x2,k) + G(x1,k))
Rb/s(x1,k)(
JACC(x1,k)x3,k +(
ddt JACC(x1,k)
)x2,k + Gb
))+ ek
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Setup 9(12)
Experiments performed on an ABB IRB4600.
Only joints 2 and 3 are used.
The true path is measured by a laser system from Leica.
Synchronization errors and kinematic errors are present.
EKF and PF are compared to the forward kinematic usingmeasured motor angles, TTCP(qm).
1.1 1.2 1.3 1.40.75
0.85
0.95
1.05
x [m]
z[m
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Setup 9(12)
Experiments performed on an ABB IRB4600.
Only joints 2 and 3 are used.
The true path is measured by a laser system from Leica.
Synchronization errors and kinematic errors are present.
EKF and PF are compared to the forward kinematic usingmeasured motor angles, TTCP(qm).
1.1 1.2 1.3 1.40.75
0.85
0.95
1.05
x [m]
z[m
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Setup 9(12)
Experiments performed on an ABB IRB4600.
Only joints 2 and 3 are used.
The true path is measured by a laser system from Leica.
Synchronization errors and kinematic errors are present.
EKF and PF are compared to the forward kinematic usingmeasured motor angles, TTCP(qm).
1.1 1.2 1.3 1.40.75
0.85
0.95
1.05
x [m]
z[m
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Setup 9(12)
Experiments performed on an ABB IRB4600.
Only joints 2 and 3 are used.
The true path is measured by a laser system from Leica.
Synchronization errors and kinematic errors are present.
EKF and PF are compared to the forward kinematic usingmeasured motor angles, TTCP(qm).
1.1 1.2 1.3 1.40.75
0.85
0.95
1.05
x [m]
z[m
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Results 10(12)
Both filters follow the true path.
The EKF passes in the corners.
TTCP(qm) out of phase.
Bias states for both motor andaccelerometer measurements arerequired to get good results.Root Mean Square Error
EKF PF TTCP(qm)
RMSE 0.124 0.083 0.167
The current MATLAB implementationnot in real time for the EKF and PF.
• True path • EKF • PF • TTCP(qm)
0.998
1.002
1.006
z[m
]
1.15 1.2 1.25 1.3 1.350.795
0.8
0.805
x [m]
z[m
]
0 0.2 0.4 0.6 0.8 1.0 1.2 1.40
1
2
3
4
5
6
Time [s]
RM
SE
[mm
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Experimental Results 10(12)
Both filters follow the true path.
The EKF passes in the corners.
TTCP(qm) out of phase.
Bias states for both motor andaccelerometer measurements arerequired to get good results.Root Mean Square Error
EKF PF TTCP(qm)
RMSE 0.124 0.083 0.167
The current MATLAB implementationnot in real time for the EKF and PF.
• True path • EKF • PF • TTCP(qm)
0.998
1.002
1.006
z[m
]
1.15 1.2 1.25 1.3 1.350.795
0.8
0.805
x [m]
z[m
]
0 0.2 0.4 0.6 0.8 1.0 1.2 1.40
1
2
3
4
5
6
Time [s]
RM
SE
[mm
]
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Conclusions and Future Work 11(12)
Conclusions
Estimation of the robot’s TCP using Bayesian methods and anaccelerometer.
Experimental evaluation of an EKF and PF on an ABB IRB4600.
The Bayesian methods show significant improvement.
Future Work
Sensitivity analysis of model parameters.
Time varying covariance matrices for the EKF to handle sharpturns.
Extend to 6 DOF. More sensors? Where to place the sensors?
Control using the estimated position.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Conclusions and Future Work 11(12)
Conclusions
Estimation of the robot’s TCP using Bayesian methods and anaccelerometer.
Experimental evaluation of an EKF and PF on an ABB IRB4600.
The Bayesian methods show significant improvement.
Future Work
Sensitivity analysis of model parameters.
Time varying covariance matrices for the EKF to handle sharpturns.
Extend to 6 DOF. More sensors? Where to place the sensors?
Control using the estimated position.
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET
Tool Position Estimation of a Flexible IndustrialRobot using Recursive Bayesian Methods
Patrik Axelsson, Rickard Karlsson, andMikael Norrlöf
Division of Automatic ControlDepartment of Electrical EngineeringLinköping University, Sweden
Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf
Tool Position Estimation of a Flexible Industrial Robot using Recursive Bayesian Methods
AUTOMATIC CONTROLREGLERTEKNIK
LINKÖPINGS UNIVERSITET