Control de robots y sistemas multi-robot basado en visión

Transcription

1 Control de robots y sistemas multi-robot basado en visión Universidad de Zaragoza Ciclo de conferencias Master y Programa de Doctorado en Ingeniería de Sistemas y de Control UNED ES Ingeniería Informática April -014 Colaboradores: Gonzalo López Nicolás Héctor Manuel Becerra Rosario Aragüés Eduardo Montijano Miguel Aranda Universidad de Zaragoza 1

2 Motivation

3 Index Features. FM, H, (Fundamental Matriz, Homography and rifocal ensor) Visual mobile robot control FM based H based based Long term navigation Control of Multi-robot systems Data association Coordinated motion with epipoles Central decision with flying camera on scene - Homography 3

4 Features Harris corner extractor Puntos extraidos de la primera imagen Lines SIF SURF 4

5 FM: Fundamental Matriz p X Image 1 Image C x Fx x l l e e Base Line C 5

6 Fundamental Matrix FM: Matriz Fundamental Matrix 3x3 satisfying: x Fx=0 Independent of scene structure As a dot product: (x x, x y, x, y x, y y, y, x, y, 1) f = 0 With 8 points we have: A f=0 8 points=> Solution to scale factor SVD(A) => Singular vector of smallest singular value 6

7 H: Homography Projective trasformation between two planes X π X? x x H π l C e e C 7

8 H: Homography 8 x = Hx = ' x x x h h h h h h h h h x x x H

9 : rifocal tensor (1D) 9

10 : rifocal tensor he tensor 1D has xx elements, wl < 3w 3+l 1, 5 features needed he D tensor has 3x3x3 elements 10

11 Nonholonomic Epipolar Visual Servoing FM based arget image Desired epipole trajectories Features extraction Robot Matching Epipolar geometry Control law Features extraction Current image 11

12 Nonholonomic Epipolar Visual Servoing FM based arget image arget Current image C t Planar motion C t Current C c C c with 1

13 Nonholonomic Epipolar Visual Servoing FM based Desired epipole trajectories Epipoles evolution Invertible if det#0 det( L) Singularidad ecx = 0 ( θ + ψ )/ d sin ( ψ ) sin ( θ ψ ) = α x cos + ( θ +ψ ) = 90º 13

14 Nonholonomic Epipolar Visual Servoing FM based Matches Epipoles and epipolar lines 14

15 Nonholonomic Epipolar Visual Servoing FM based arget position arget image Current image 15

16 Nonholonomic Epipolar Visual Servoing FM based 16

17 e 13 = e 3 e, e 1 1 Nonholonomic Epipolar Visual Servoing FM based Sliding mode control to avoid singularity he control task is carried out in two steps: y e = 31 e 3 C 3 x First step Alignment with the target e 13 e 1 = y 0 e 3 e 31 e 3 = 0 e 1 C C = C 1 C 1 C 1 Initial configuration Intermediate configuration Final configuration C 3 e 1 x ( e3 = 0) &( e3 = 0) ( x = 0,φ = 0) Second step Depth correction e 1 = e 13 ξ e, e 3 3 y e = 1 e 31 x C = C 3 1 = e1 e13 ξ1 = 0 y = 0 17

18 Nonholonomic Epipolar Visual Servoing FM based Control goal of the step Solve the stabilization problem in the following error d d system, where ξ 3 = e3 e3(t), ξ3 = e3 e3(t). Desired trajectories α x sin( φ ψ ) α x d 3 d cos ( ) cos ( ) e 3 υ e φ ψ φ ψ 3 d e = ( ) L(, ) u e x sin = d α φ ψ 0 φ ψ d 3( 0) π ( t) = σ 1+ cos t 3 τ 3 d ( ) e cos ω 3 3 ψ e e d 3( 0) π ( t) = 1+ cos t 3 τ where L( φ, ψ ) is the so-called decoupling matrix. ξ ξ Sliding mode control with sliding surfaces d sc ξ 3 e 3 e3 s = = = = 0. d st ξ 3 e3 e3 Decoupling-based controller. u υ ω cos cos ( ψ ) ( φ ψ ) d3e db 1 0 sin db = = db α xe ( φ ψ ) cos ( ψ ) u u c t Singularity if φ ψ = nπ where u c u t = e = e d 3 λcsc κc d 3 λtst κt sign( s ), sign c ( s ) t 18

19 Nonholonomic Epipolar Visual Servoing FM based A singular pose is shown in the figure φ ψ = arctan( e3 / α x ) = 0 e 3 C 3 e 3 = 0 Bounded controller. hese inputs don t use the decoupling matrix ( ). υb kυ sign st sin( φ ψ ) ub = = ( ) ωb kω sign sc his is a local control law for the error system. By switching between controllers accordingly, robust global stabilization of the error system is achieved. d d Robust Control Law e Epipolar Geometry e, e 3 3 ψ φ ψ e3, 3 Decoupling Based control Bounded control u db u b C e 3 If < h, hen u b Else u db u 19

20 Nonholonomic Epipolar Visual Servoing FM based he epipoles are computed from synthetic images of size 640x480 pixels. arget location is (0,0,0º). Virtual scene: 0

21 Nonholonomic Epipolar Visual Servoing FM based 0. υ (m/s) ω (rad/s) ime (s) e 3 (pixels) Desired trajectory Real evolution e 3 (pixels) e 1 (pixels) ime (s) 1

22 Nonholonomic Homography based H based wo images can be geometrically linked by a homography he homography is generated by a plane of the scene he homography can be computed from point matches Goal: H = I p p p 1 C C 1 H p = H p 1 π

23 Nonholonomic Homography based H based he homography is related to camera motion: Planar motion: with: Non-linear relation of H with state system: 3

24 Nonholonomic Homography based VS dof system wo elements of the homography are enough to define the control Derivatives of the output functions: State space form with: State vector: Input vector: Output vector: Linear relation between the input and output with: 4

25 Nonholonomic Homography based H based racking of the desired trajectories of the homography elements Input of the control: Exponentially stable error dynamics Desired trajectories: 5

26 Nonholonomic Homography based H based 6

27 7

28 Combination of Epipoles/Homographies for VS Path Epipoles Homography Epipolar-based control: 8

29 Combination of Epipoles/Homographies for VS Epipoles Homography Path Homography-based control: 9

30 Combination of Epipoles/Homographies for VS arget image Extraction of features Matching δ 1-δ 1-δ Robot δ Extraction of features Current image 30

31 Combination of Epipoles/Homographies for VS 31

32 Visual control based he trifocal tensor is the intrinsic geometry between three views. It only depends on the camera internal parameters and relative pose. he trifocal tensor encapsulates this intrinsic geometry. Matrix notation Seven correspondences needed 3

33 Visual control based arget image Initial image Extraction of features Extraction of features Feature matching and estimation of the trifocal tensor Control law Mobile robot Extraction of features Current image 33

34 Visual control based Particularly the 1D trifocal tensor allows: Exploit the bearing information. Reduce the camera calibration parameters required for control (center of projection and vertical alignment). he trifocal tensor is a more general geometric constraint than epipolar geometry. Epipolar geometry is ill-conditioned with short baseline and with planar scenes. Five corresponding points X = [ X Y Z] i= 1 j= 1 k = 1 ijk u v i j w k = 0 v = [ v 1 v ] w = [ w 1 w ] x0 x x0 θv u = [ u 1 u] θ u θ w θ 34

35 Visual control based Initial location C1 = ( x1, y1, φ1 ). arget location C = 3 ( 0, 0, 0). Current location (moving camera) = ( x, y, ). y 1 φ 1 θ i1 C 1 X x 1 y C φ θ i x θ i3 φ 1 C 1 X t y1 1 x 1 C φ t x1 φ y t y t x y 3 C 3 C y x x 3 8 elements of the tensor: m ijk = m 111 m 11 m 11 m 1 m 11 m 1 m 1 m t y sinφ t 1 y sinφ 1 t y cosφ + t 1 y cosφ 1 t + y cosφ t 1 x sinφ 1 t y sinφ t 1 x cosφ 1 = t x sinφ 1 t y cosφ 1 tx cosφ t φ 1 y sin 1 t cosφ + x1 tx cosφ 1 tx sinφ + t sinφ1 1 x where the relative locations between cameras are given as tx cosφi sinφi = x i i t y sinφi cosφi y i i for i =1,. his is an over-constrained measurement 35

36 Visual control based Values of the trifocal tensor in particular locations When C =C 1 When C =C 3 ( t x = t, ) x t 1 y = t y 1 ( t x, t 0) = 0 y = = 0, = 0, = 0, = 0, 11 1 = 0, = 0, = 0, = 0, = 0, = 0. = 0, = 0. φ 1 C 1 t y1 1 x 1 t x1 t x C 3 φ t y y y 3 C y x x 3 ime-derivatives of the elements of the tensor = sinφ = 1 m N υ + cosφ 1 m N 11 υ + ω, 1 ω, 11 1 = = cosφ 1 m N sinφ 1 m N υ + υ + 1 ω, ω, 11 1 = = ω, ω, 1 = = 11 1 ω, ω. Useful only for orientation control 36

37 Visual control based hree variables to desired values but we choose to make a Square control system. υ y ω Visual measurements Error Function y 1 By using two outputs, the tensor provides three possibilities: First part of the control Second part Correcting DOF Drawback ( φ, y) 1 Orientation and depth Lateral error (x) Non-holonomic constraint does not allow to correct the remainder lateral error. ( φ, x) Orientation and lateral error Depth ( y) Unknown final values of the tensor elements to define the control objective. ( x, y) 3 Lateral error and depth Orientation (φ) Differential-drive allows to correct the remainder orientation error. 37

38 Position correction with two selected outputs: When Zero dynamics: Control goal of the step Stabilize the following error system, where and he initial orientation introduces uncertainty in this system and a robust control law is required. 1 φ [ ] { } [ ] { }.,, R y x Z = = φ φ ξ ξ φ., = + = ξ ξ. sin cos cos sin = = y x t t φ φ φ φ ξ ξ, d e ξ = ξ d e ξ = ξ ( ). ξ u M, sin cos d d d m N m N e e = = φ ξ ξ ω υ φ φ. cos cos + + = + + = t t ini ini d ini ini d τ π ξ τ π ξ Desired trajectories 38 Visual control based

39 Visual control based Position correction: It is carried out by two controllers, because the first one has a singularity problem when the robot is reaching the target location. Sliding mode control with sliding surfaces: d s1 e1 ξ 1 ξ1 s = = = = 0. d s e ξ1 ξ Decoupling-based controller υdb 1 = = u u sinφ1 cosφ db 1 ω db det M m m N N 1 m m where det( M) = [( ) ( ) ] m sinφ cosφ1, N = 11 N d d u1 = ξ1 λ1s1 κ1sign( s1 ), u = ξ λs κ sign( s ). Bounded controller u b υb = = ωb k 1 1 ( ) u ω k υ sign( s sign ( ( s ) )) Singularity if det( M) = 0. At the final location Robust global stabilization of the error system is achieved by commuting from the decoupling controller to the bounded one if det( M) <. h 39

40 Visual control based Correction orientation: We can use any single tensor element whose dynamics depends on and its final value being zero. ω Control goal of the step Stabilization of the following dynamics ω 1 1 = 11 ω. A suitable input that yields exponentially stable is 1 ω = k ω, t >τ 11 When position correction has been reached, and consequently, 1 = t y1 cosφ if = 0 then φ = nπ 1 with n, and the orientation is corrected. Z Although only a rotation is needed, the same bounded translational velocity is used to maintain the longitudinal position under closed loop control. υ = k υ sign( s 1 ). 40

41 Visual control based he 1D- is computed from synthetic images of size 104x768 pixels. he desired pose is (0,0,0º). Virtual scene: 41

42 Visual control with FoV constraints Scene Scene φ z x arget φ z x arget Initial Inicial arget 4

43 Visual control with FoV constraints Observed target Initial positions II O II Goal I IV G V III I III 43

44 Visual control with FoV constraints he homography between two views is related to camera motion: Planar motion: With: arget: Plane of the scene Goal: H = I Subgoals: H = Observed target: π Initial H Goal 44

45 Visual control with FoV constraints Particular homographies in particular positions 45

46 Visual control with FoV constraints Switched control: hree sequential steps Step 1: Step : Step 3: 46

47 Visual control with FoV constraints Switched control: Five sequential steps Subgoals G 1 : Pure rotation until reaching the first -curve G : Follow the first -curve forward G 3 : Pure rotation until reaching the second -curve G 4 : Follow the second -curve backward G 5 : Pure rotation until reaching desired Goal 47

48 Visual control with FoV constraints Switched control: Five sequential steps Step 1: Step 4: Step : Step 5: Step 3: Subgoals: Defined in terms of homography parameters Decomposition of the homography 48

49 Visual control with FoV constraints 49

50 Visual control with FoV constraints 50

51 Long term navigation ask: reach a desired position associated with a target image, which belongs to a visual memory acquired in a teaching phase. A visual path of n key images is extracted from the visual memory, which must be followed autonomously in order to reach the target. arget Home n Key images Initial Issues in previous work in the literature: 1) Constrained field of view of conventional cameras. ) Change of velocities when change of image. 3) Information about velocity in the visual path. 51

52 Long term navigation ψ he omnidirectional cameras can be virtually represented as conventional cameras when working with points on the sphere. y φ e c C c x + φ d y Each one of the key images is used as target image accordingly. e t C t x arget location Current location Epipoles e e c t x = α x. y Interaction with the robot velocities: α x sin e c d cos e t C = t C c = α x sin = d cos ( 0, 0, 0) ( ) x, y,φ xcosφ + ysinφ = α x, y cosφ xsinφ ( ) α x + ( ) cos ( φ ψ ) φ ψ υ φ ψ ( φ ψ ) ( ψ ) υ = ω, 5

53 he current epipole gives information of the translation direction and it is directly related to the required robot rotation to be aligned with the target. Use of the x-coordinate of the current epipole as feedback information to control the robot heading and so, to correct the lateral deviation. Non-null translational velocity Long term navigation υ 0 k ce ce ω = tωrt + ω ce. First component of the rotational velocity i C c i e c = 0 φ i i C t x i e c i+1 0 y i i+1 C t i+1 = 0 e c x i+1 y i+1 ω f ( e c ) Second component of the rotational velocity x i 1 y i 1 i 1 C t ki e, c i 1 = 0 i C t x i y i ki i e, c 0 i+1 C t x i+1 y i+1 ω f ( e ki c ) 53

54 Long term navigation Let us define a tracking error to drive the epipole smoothly to zero for every segment between key images d ζ ce = ec ec ( t) = 0. where k c e e d c d c > 0. ( t) = ec ( 0) π 1+ cos t, 0 t τ τ ( t) = 0, t > τ ( φ ψ ) α x υ + ( φ ψ ) cos ( φ ψ ) α x sin = ω rt d cos ζ ce ω ce rt with Control goal Stabilization of the error system: ce e ( φ ψ ) cos ( φ ψ ) d υ + ( e k ζ ). sin = d α x dmin τ =. υ Considering that the translational velocity is known, the following rotational velocity, referred as reference tracking (R) control, stabilizes the error system with c d c. c ce 54

55 A varying translational velocity according to the shape of the path can be computed depending on the epipoles between key images. υ ce = υ max + υ min υ + ce ω = max k υ kmυ d min ce min e ki c ce ce ω = tωrt + tanh 1 We propose the following nominal rotational velocity, which is computed from the epipoles between key images: So that, the complete rotational velocity (R+ control) is given as: Switching and stop condition Long term navigation he switching condition to the next key image or to stop the task is given when the image error starts to increase, which is defined as follows: r 1 ε = r j=1 p j p i, j. ω. ce. e ki c / d σ min. 55

56 Long term navigation 56

57 Index Features. FM, H, (Fundamental Matriz, Homography and rifocal ensor) Visual mobile robot control FM based H based based Long term navigation Control of Multi-robot systems Data association Coordinated motion with epipoles Central decision with flying camera on scene - Homography 57

58 Robots communication is limited Wireless network. Range-limited. Visibility (Comm.) Communication graphs Nodes: the robots Edges: link between robots that can exchange data Multi-Robot Systems Each robot exchange data with its one-hop neighbors Robots are moving: new edges may appear / previous links disappear Communication graphs with switching topology 58

59 Distributed Data Association Limited communication: Locally associate features with neighbors Propagate local associations through the network Inconsistent global associations Distributed algorithms: propagate local associations detect inconsistencies resolve them Additionally, establish global labels for the features 59

60 Distributed Data Association Each robot in the team has a set of features. It has executed a local association method to match its features and its neighbors ones, for his information can be represented with graph, where the nodes are the features of all the robots, and there is a link between two features if they have been locally matched by. he adjacency matrix of this graph is with 60

61 Distributed Data Association Goal (robot i). Discover for each the features, all the other features which are connected to through a path. Idea. If there is a link between features and, then the features connected to and to through a path are the same. Formal. Distributed computation of the powers of the adjacency matrix, Each robot maintains the rows of the adjacency matrix power associated to its own features, and updates them using data from its neighbors For each of this features, each robot i obtains all connected to through a path, and detects the inconsistent ones. 61

62 Distributed Data Association Idea: break local associations so that there are no two features from the same robot related by a path. Note that each inconsistency is motivated by, at least, one spurious local link (false positives). All local links are equal Resol. algorithm based on rees For each conflictive feature belonging to the same robot, use it as root of its tree and incrementally add features linked to it. If a feature already belongs to a tree, or receives requests from more than a tree, it selects one of the trees and erases links to the others. Links with quality information Maximum Error Cut For each pair of inconsistent features belonging to same robot, select and erase the link with the largest error that breaks the inconsistency. 6

63 Distributed Data Association Compute the robot positions in a common reference frame Each robot measures the relative position of its neighbors Distributed map merging scenario Local maps aligned before merging It only needs to be computed once (a) Merged map after 5 iterations (b) Merged map after 0 iterations 63

64 Distributed Data Association 64

65 Multi robot control based on epipoles Coordinated control for attitude sincronization Modeled with an undirected graph Non holonomic motion on the plane Polar coordinates 65

66 Multi robot control based on epipoles he robots exchange the visual features Correspondences satisfy the epipolar constraint he epipoles are the null space of he attitude consensus implies the epipoles to be equal Note that the opposite is not necessarily true 66

67 Multi robot control based on epipoles Define he geodesic in the epipole domain If the calibration is known, then choosing the exact relative orientation can be computed and we have a standard consensus problem 67

68 Multi robot control based on epipoles he distributed controller used by the robots is Properties of the controller 68

69 Multi robot control based on epipoles 69

70 Multi robot control with flying camera (H) What? Visual control of mobile robots Desired configuration defined by an image ask: Navigate to the desired configuration Image of desired configuration Initial configuration Desired configuration 70

71 Multi robot control with flying camera (H) What? Visual control of mobile robots Who? Set of nonholonomic vehicles Nonholonomic kinematics Cartesian coordinates Polar coordinates 71

72 Multi robot control with flying camera (H) What? Visual control of mobile robots Who? Set of nonholonomic vehicles How? Flying camera Flying camera looking downward Camera motion unknown Intrinsic camera parameters known Homography: Only visual information 7

73 Multi robot control with flying camera (H) What? Visual control of mobile robots Who? Set of nonholonomic vehicles How? Flying camera Where? Motion occurs in a planar floor his gives additional constraints on the homography Only the set of robots may remain common in the scene Image of desired configuration: Desired configuration Actual configuration 73

74 Multi robot control with flying camera (H) he homography in our framework: Multi-robot motion in a planar floor Points = Robots => Homography Camera flies parallel to the floor C hen, the homography is constrained: p p π his homography can be computed from a minimal set of two points/robots 74

75 Multi robot control with flying camera (H) If the robots are in the desired configuration: he homography is conjugate to a planar Euclidean transformation he homography is not the identity matrix Extracting Motion parameters Which is coherent with a rigid motion. So, the robots are in the desired formation Desired configuration Desired configuration Current image Current configuration 75

76 Multi robot control with flying camera (H) If the robots are NO in the desired configuration: he homography is a similarity transformation with isotropic scaling s he H computation with the -point method Extracting Motion parameters Which is NO coherent with a rigid motion. So, the robots are not in formation Desired configuration Desired configuration Current image Current configuration 76

77 Multi robot control with flying camera (H) We have controller Robots not in formation Nonrigid homography Each pair of robots induces a different Homography, valid but not coherent We want Robots in formation Rigid homography Every pair of robots induce the same Homography We define a desired homography Like the nonrigid homography but being induced by keeping the motion constraints he task is to drive the robots to the desired homography he desired homography is not constant and depends on the robots and camera motion 77

78 Multi robot control with flying camera (H) Image of desired configuration Current image Flying camera arget homography Control law Set of robots: 78

79 Multi robot control with flying camera (H) Image-based control law Control error: Current state of the robots on the image vs desired states given by the desired homography Switched control consisting of three sequential steps: Image plane 79

80 Multi robot control with flying camera (H) Steps 1- orientate and drive the robots toward their target locations. In practice, they are carried out simultaneously: Step 3 rotates the robots until they are in the required relative orientation within the formation Image of desired configuration Steps 1- Step 3 80

81 Multi robot control with flying camera (H) op view Linear velocity: v op view Homography entries Angular velocity: ω Desired configuration: Desired configuration: 81

82 8

83 Control de robots y sistemas multi-robot basado en visión Universidad de Zaragoza Ciclo de conferencias Master y Programa de Doctorado en Ingeniería de Sistemas y de Control UNED ES Ingeniería Informática April -014 Colaboradores: Gonzalo López Nicolás Héctor Manuel Becerra Rosario Aragüés Eduardo Montijano Miguel Aranda Universidad de Zaragoza 83