A&R Project Group: Time-optimal large view visual servoing for the navigation of a mobile robot using moments of SIFT-features

Transkript

1 Universität Dortmund A&R Project Group: Time-optimal large view visual servoing for the navigation of a mobile robot using moments of SIFT-features Introduction Slides Department of Electrical Engineering and Information Technology, Chair of Control and Systems Engineering Univ.-Prof. Dr.-Ing. Prof. h.c. Torsten Bertram

2 Motivation Goal pose Goal pose Current pose Current pose Robot orientation Camera orientation

3 Outline Introduction Camera gaze control Virtual camera plane Navigation control Visual Servoing using SIFT-Features

4 Navigation using visual servoing Goal pose v Δf = f ref Δu 1 Δu1 Δv1. f =. ΔuN ΔvN Δv 1 f ref + - f Current pose Δf J + feature extraction ( x z θ ) T y robot u 1 u 2 1+ u x u Z Z z v = v 0 uv θ Z y =>Couplings =>Limited field of view

5 Suboptimal robot trajectory Start Charging station

6 Desirable robot trajectory Start Charging station

7 Outline Introduction Camera gaze control Virtual camera plane Navigation control Visual Servoing using SIFT-Features

8 Camera gaze control Image plane Goal: Keep image features centered in the image u M : u-coordinate of the centroid u ref : Desired coordinate of the centroid (image center) uref um Problems: Delay time Velocity of the robot Solution: Prediction of pixel displacement based on known movement of the robot

9 Camera gaze control u The known displacement of u M is eliminated by a appropriate counter rotation of the camera : θ M K u M = (1 + u 2 M Camera control scheme: 1 um 2 vx vz + (1 + um = ) ωy Z Z ) ( v ω ) Z v X Y T J M u M 1 2 (1 + u M ) + + θ K camera θ K u ref P-Control + - u M

10 Outline Introduction Camera gaze control Virtual camera plane Navigation control Visual Servoing using SIFT-Features

11 Virtual camera plane Θ V α Θ V : Viewing direction of virtual camera : Angle between virtual camera plane and driving direction of the robot Image plane α : Rotation angle between the camera and the virtual plane Virtual camera plane

12 Determination of coordinates in a virtual camera plane P R ˆ u V = tan( β α) g + d ( g + d sin β ) sin( β α) B R Assumption: d<<g = 1 B V uˆ V tan(arctan( uˆ = R ) α) ^ u R ^ u V 2 uˆ V vˆ = ˆ V vr 2 uˆ R

13 Outline Introduction Camera gaze control Virtual camera plane Navigation control Visual Servoing using SIFT-Features

14 Decoupling of translation and rotation Camera retreat problem Angular criterium Δϕ ϕ ϕ ref Image distortion for proper orientation alignment Determination of the compensated feature error: ω ϕ = K Δϕ f ϕ = J ϕ ω ϕ

15 Control design Θ Θ Θ Θ = ω ϕ ω ϕ V V V V V V R R R x z x z cos sin 0 sin cos, ( ) f ϕ f f J x z ref xz V V = + Translational velocity: Transformation from virtual camera frame to robot coordinate system Control law for non-holonomic robot (Hager98): κ x R K + = 1 1 ω z R x R ω R ( ) R R R R x K z v + = = η ω ω ϕ

16 Experimental results (1) Lateral error in cm Lateral Error in cm Switch to landmark 4 Switch to Landmark Center of the floor Center of the floor Iterations Control cycles

17 Experimental results (2)

18 Visual Servoing using SIFT-Features Control Scheme SIFT Generic Moments of SIFT Features Decoupled Visual Servoing

19 Visual Servoing Control Loop f goal + - Δ f = fgoal f controller Regler q q soll goal Achsen axis q f axis control image processing

20 Motivation IBVS Challenges Artificial markers Coupling between degrees of freedom Why SIFT? Natural texture Frequent in natural objects Invariant to scale and orientation Solution of correspondence problem

21 Visual Servoing using SIFT-Features Control Scheme SIFT Generic Moments of SIFT Features Decoupled visual servoing

22 SIFT-Merkmale SIFT = Scale Invariant Feature Transform (Lowe 2004) Das Verfahren basiert auf der Extraktion von markanten Bildpunkten durch sukzessive Bandpassfilterung (DOGs) mit stabilitätsbasierter Extremwertsuche und Zuweisung eines Merkmalsvektors Scale Invariant Feature Transform

23 Robustheitseigenschaften von SIFT-Merkmalen Skalierungsinvariant (links: aktuelle Ansicht, rechts: Lernbild) Rotationsinvariant

24 Robustheitseigenschaften von SIFT-Merkmalen Lichtverhältnisse (links: aktuelle Ansicht, rechts: Lernbild) Teilweise robust gegenüber perspektivischer Verzerrung

25 Edges / Corners Homogene Bildregion (konstante Intensität bedeutet kleine Intensitätsänderung bei Verschiebung der Maske) Kantensegment (Verschiebung orthogonal zum Kantensegment verursacht eine große Intensitätsänderung) Ecksegment (alle Verschiebungen verursachen eine große Intensitätsänderung)

26 SIFT-Algorithmus 1. Extrahiere skalierungsinvariante Keypoints basierend auf Extrema im Skalenraum 2. Stabilitätsanalyse der Keypoints anhand der Eigenwerte der Hessematrix

27 Extraktion stabiler Merkmale Faltung des Bildes I(x,y) mit einem Gausskernel: L( x, y, σ ) = G( x, y, σ ) I( x, y) 2 2 ( x + y ) σ G ( x, y, σ ) = e 2 2πσ Berechnung der DoG (Laplacepyramide) anhand der Differenz zweier benachbarter Skalierungen: ( ) D( x, y, σ ) = G( x, y, kσ) G( x, y, σ) I( x, y) = Lx (, y, kσ) Lx (, y, σ)

28 Extraktion stabiler Merkmale Extraktion stabiler Merkmale Skalierung (nächste Oktave) Difference of Gaussians Skalierung (erste Oktave)

29 Extraktion stabiler Merkmale Extraktion stabiler Merkmale Skalierung Lokale Extremwertdetektion: Detektiere Maxima und Minima der DoG im Skalenraum

30 SIFT-Algorithmus 3. Zuweisung einer Hauptorientierung zu jedem Keypoint (Rotationsinvarianz) 4. Berechne für jede Hauptorientierung eines Keypoints einen Merkmalsvektor

31 Orientierungszuweisung Jedem Merkmalspunkt wird anhand seiner Orientierungen der Gradienten der Lokalen Umgebung eine Hauptorientierung zugewiesen Die Beschreibung der Lokalen Nachbarschaft wird relativ zu der Hauptorientierung durchgeführt =>Orientierungsinvarianz ( ) ( ) 2 2 m( x, y) = L( x + 1, y) L( x 1, y) + L( x, y + 1) L( x, y 1) ( Lx y Lx y ) ( Lx ( + 1, y) Lx ( 1, y) ) (, + 1) (, 1) 1 θ( x, y) = tan

32 Orientierungszuweisung Berechne sowohl für die Hauptorientierung als auch für Nebenmaxima, die über 80 % des Hauptmaxima liegen, einen Merkmalsvektor Amplitude Amplitude a b Winkel Orientierungshistogramm Winkel

33 Merkmalsvektor Der Merkmalsvektor hat Einträge In der Praxis wählen wir den Merkmalsvektor mit 4 4 8=128 Einträgen Durch Normierung der Länge dieses Vektors wird Invarianz gegenüber Kontraständerungen erreicht.

34 Visual Servoing using SIFT-Features Control Scheme SIFT Generic Moments of SIFT Features Decoupled visual servoing

35 SIFT moments f z and f γ SIFT moment f z : f z n i=1 i = n σ σ SIFT moment f γ : f n i=1 i γ = n ϕ Δφ distance rotation

36 SIFT moments f x,f y,f α and f β SIFT moments f x and f y : f x wu n i=1 i i = n f y wv n i=1 i i = n SIFT moments f α and f β : v 1 v u u f α = 3 ( vˆ ˆ i vj) ( pj pi ) n n 1 i= 1 j= i+ 1 n n k= 1 l= k+ 1 ( pk pl ) f β = 3 ( uˆ ˆ i uj) ( pj pi ) n n 1 i= 1 j= i+ 1 n n k= 1 l= k+ 1 ( pk pl )

37 Visual Servoing using SIFT-Features Control Scheme SIFT Generic Moments of SIFT Features Decoupled visual servoing

38 Decoupled Visual Servoing in 4DOF Image jacobian v n n λ -u i z 0 wi z 0 f x i=1 i=1 T n n f y -v T λ i = 0 z wi z 0. f z i=1 i= 1 T f 0 0 Kz 0 γ T Kγ x y z γ 1 1, , , , 4 4 u n n 1 2 i n 1 i i i=1 i=1 F= (w - ) +λ wu cost constraint w= - i 1 n uu i n i=1 u 2 i

39 Decoupled Visual Servoing in 4DOF Image jacobian v n n λ -u i z 0 wi z 0 f x i=1 i=1 T n n f y -v T λ i = 0 z wi z 0. f z i=1 i= 1 T f 0 0 Kz 0 γ T Kγ x y z γ u n n 1 2 i n 1 i i i=1 i=1 F= (w - ) +λ wu cost constraint w= - i 1 n uu i n i=1 u 2 i

40 Experimental Results

41 Experimental Results Decoupled visual servoing for 4DOF coupled decoupled task space error Δ x [mm] Δ y [mm] -30 Δ z [mm] Δ γ [ ] iter Decoupled visual servoing for 6DOF 20 coupled task space error Δ x [mm] Δ y [mm] -30 Δ z [mm] Δ γ [ ] iter largely decoupled taskspace error 0 Δ x -10 Δ y Δ z -20 Δ α *100 Δ β *100 Δ γ * iter taskspace error 0 Δ x -10 Δ y Δ z -20 Δ α *100 Δ β *100 Δ γ * iter