A.D. MCCXXIV Position-Based Visual Servoing - PRISMA Lab · 2004. 10. 26. · Robot Visual Servoing...
Transcript of A.D. MCCXXIV Position-Based Visual Servoing - PRISMA Lab · 2004. 10. 26. · Robot Visual Servoing...
1
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
A.D. MCCXXIV
Position-Based Visual Servoing
Luigi VILLANI
Dipartimento di Informatica e SistemisticaUniversità degli Studi di Napoli Federico II
Robot Visual Servoing
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Outline
n Position-Based Visual Servoingn Pose control problemn Pose estimation problem
n Pose control n Relevant framesn Control objectiven Sensitivity to calibration errorsn Control schemes
n Pose estimationn 3D position of single pointsn Object posen EKF-based pose tracking
n References
A.D. MCCXXIV
2
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Position-Based Visual ServoingA.D. MCCXXIV
Features extraction
Posecontrol
CameraRobot
Pose estimation
Desired
pose
two separateproblems
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
PBVS (cont’d)A.D. MCCXXIV
n Pose control problem
n Pose estimation problem
Control the relative pose (position and orientation) of the robot end effector with respect to the pose of the target object
Estimate the pose of the target object with respect to the robot end-effector or with respect to the robot base
3
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose controlA.D. MCCXXIV
n Relevant frames
x
z y
O
Baseframe
Objectframe
OO
Ox
Oz
Oy
Cx
Cy
Cz
CO
Cameraframe
EO
Ey
Ez
ExEnd-effector
frame
Desiredframe
: dE
OdEO
OdEO
O RrT ,
: EEE RpT ,
: OOO RpT ,
CCC RpT ,:
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Control objectiven Eye-in-hand
n Eye-out-hand
dE
OE
OdEO
OEO
OdE
OE
O RRrrTT ==⇔= ,
dEE
dEE
dEE RRppTT ==⇔= ,
dE
OO
dE
dEOO
dE
d?
OO
dE RRRrppTTT =+=⇔= ,
desired (absolute) end-effector pose
desired (relative) end-effector pose
4
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Sensitivity to calibration errors n Eye-in-hand estimated position:
at equilibrium:
n Eye-out-hand estimated desired position:
estimated robot position:
at equilibrium:
CEOO
EOO drr +=ˆ camera calibration error
Cd
EOO
EOOd
EOO
EOO drrrr −=⇒= ˆ
CdECO
dEO
dE dpdprp +=++=ˆ
REE dpp +=ˆ robot calibration error
RCdEE
dEE ddpp pp −+=⇒= ˆˆ
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Endpoint Closed-Loop configuration (eye-out-hand)
estimated desired position:
estimated robot position:
at equilibrium:
n Robust to robot and camera calibration errors!
CdECO
dEO
dE dpdprp +=++=ˆ
CEE dpp ′+=ˆdECC
dEE
dEE pddpp pp ≈′−+=⇒= ˆˆ {
0≈
5
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Control schemes (eye-in-hand)n Regulation
n Position control
n Orientation control angle/axis error exctracted from
direct visualservoing
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Fixed or slowly moving target objectsn During the motion the object may leave from the camera field
of view or follow a non-predictable path in Cartesian space
n Direct visual servoing requires high rate pose estimation
assign an end-effector trajectory in task space
indirect visual servoing
6
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Resolved rate Vision-basedcontrol Motion control Robot
CameraVisual feedback
Joint feedback
= 20 Hz
= 100 Hz
closed loop
velocity loop
position loop
vision-based control
linear velocity
angular velocity
straight line path withfirst order
exponential time law
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose control (cont’d)A.D. MCCXXIV
n Task-space resolved acceleration
Vision-basedcontrol Motion control Robot
CameraVisual feedback
Joint feedback
= 20 Hz
= 100 Hz
vision-based control
linear acceleration
angular acceleration
closed loop
straight line path withsecond order
exponential time law
7
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimationA.D. MCCXXIV
n 3D position of single pointsn single camera additional information requiredn multiple cameras triangulation
n Object posen multiple feature points + CAD model n analytic solutions (at least 3 noncollinear points or 4 coplanar
points)n least squares solutions (measurement noise robustness, more
than 3 points, iterative optimization techniques)n Extended Kalman Filter (EKF) based on an approximate
object dynamic model
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n Coordinate vector of feature point
nonlinear equations ( feature points)known quantities: 6 unknown quantities:
( )( )jCTCj
C pfRppRp 0000 −−=
A.D. MCCXXIV
T) ( jC
jC
jC
jC zyx=p
perspectivetransformation
=
jC
jC
jC
e
j
j
yx
zf
vu
n
+
n2jjjCC,e v,u,,,f pRp 0
00 fp ,
=
8
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)A.D. MCCXXIV
n EKF-based pose trackingn recursive implementation n noise and disturbance rejection
n use of redundant measurementsn on-line change of the measurement setn dynamic windowing technique (saving of processing time)
However ...n computation time increases with the number of feature
points n a number of 5-6 (optimally selected) feature points is
sufficient to achieve the best estimation accuracy
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n ... effective approach
A.D. MCCXXIV
Binary Space Partitioning tree: efficient representation of 3D
object geometry
pre-selection of the visibile feature points
Complexity:
optimal selectionon a minimal set
of alternatives
)(NO
9
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n Approximate object dynamic model
n output equation
n is a nonlinear function to be linearized (EKF)
kkk γ+= −1Aww
= input disturbancekγ
=
101
101
diagΤΤ
LA
A.D. MCCXXIV
= measurement noise( ) kkk ?wg? += k?Τ
ke
n
e
n
eek f
v,
fu
,,fv
,fu
= K11?
( ).g
[ ]Tzzyyxx
00000000000 ψψααφφ &&&&&& 0=w
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n To encrease the estimation accuracy and reduce the EKF computational time, an optimal subset of feature points can be selected at each sample time n however: the complexity of the selection algorithm grows
exponentially with the number of feature points
n If a CAD model of the object is available, an efficient pre-selection technique of the feature points can be set upn selection of all feature points that are visible to the camera on
the basis of EKF prediction of object pose
n The pre-selection algorithm is based on Binary Space Partitioning (BSP) tree representation of the object geometry (modelled as a set of planar surfaces)
A.D. MCCXXIV
10
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n A BSP tree is a data structure that allows representing the geometry of a 3D object through a recursive and hierarchicalpartition of the set of object surfacesn each node of the tree is the root of a front (back) subtree
corresponding to the subset of all the object surfaces lying tothe front (back) side of a given partition plane
n the feature points of the object that are visible from a givencamera pose are found by implementing a recursive visitalgorithm of the nodes of the tree
A.D. MCCXXIV
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
A BSP tree example
A.D. MCCXXIV
11
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n Windowing is then applied to find the effective visible points that are also robust for the feature extraction process (known as localizable points)
A.D. MCCXXIV
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n A classical selection algorithm is applied to find an optimal set of feature points
Quality indices
n spatial distribution
n angular distribution
n anti-chattering index (hysteresis effects)
n …
{ }∑=
∈−=
n
iijnjsd pp
nQ
1,,1
min1
K
∑=
−−=n
i
iad n
Q1
12
1π
α
A.D. MCCXXIV
12
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
objects with non-interposing parts
A.D. MCCXXIV
objects with interposing parts
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
n Visual system n MATROX GENESIS boardn SONY 8500ce b/w camera
n Objectn 40 feature points (object
corners)
n Robotn COMAU Smart 3-S
A.D. MCCXXIV
13
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
First experiment
A.D. MCCXXIV
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
Second experiment
A.D. MCCXXIV
14
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)
Two cameras
A.D. MCCXXIV
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
Pose estimation (cont’d)A.D. MCCXXIV
15
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
References
n Position-based control
n W.J. Wilson, C.C. Williams Hulls, G.S. Bell, “Relative end-effector control using cartesian position based visual servoing”, IEEE Transactions on Robotics and Automation, 12, pp. 684-696, 1996
n P. Martinet, J. Gallice, “Position based visual servoing using a non-linear approach”, Proc. of 1999 IEEE/RSJ Int. Conf. On Intelligent Robots and Systems, pp. 531-536, 1999
n E. Malis, F. Chaumette, “Theoretical improvements in the stability of new class of model-free visual servoing methods”, IEEE Transactionson Robotics and Automation, 18, pp. 176-186, 2002
A.D. MCCXXIV
VII Scuola Nazionale di Dottorato CIRA Bertinoro (FC)Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione 14–16 Luglio 2003
References (cont’d)
n Pose trackingn D.F. DeMenthon, L.S. Davis, “Model-based object pose in 25 lines of
code”, Int. Journal of Computer Vision, 15, pp. 123-141, 1995n F. Janabi-Sharifi and W.J. Wilson, “Automatic selection of image
features for visual servoing,'' IEEE Transactions on Robotics and Automation, 13, pp. 890-903, 1997
n V. Lippiello, B. Siciliano, L. Villani, “Position and orientation estimation based on Kalman filtering of stereo images”, 2001 IEEE International Conference on Control Applications, pp. 702-707, 2001
n T. Drummond, R. Cipolla, “Real-time visual tracking of complexstructures”, IEEE Transactions on Pattern Analysis and MachineIntelligence, 24, pp. 932-946, 2002
n V. Lippiello, B. Siciliano, and L. Villani, “Objects motion estimation via BSP tree modeling and Kalman filtering of stereo images,'' 2002 IEEE Int. Conf. on Robotics and Automation, pp. 2968-2973, 2002
A.D. MCCXXIV