Distributed localization and control of underwater robots

Transcription

1 Distributed localization and control of underwater robots L. Jaulin ENSTA Bretagne, LabSTICC Methods and Tools for Distributed Hybrid Systems DHS 2018, June 4, Palaiseau

2 Interval analysis

3 Problem. Given f : R n R and a box [x] R n, prove that x [x],f (x) 0. Interval arithmetic can solve eciently this problem.

4 Example. Is the function f (x) = x 1 x 2 (x 1 + x 2 )cosx 2 + sinx 1 sinx always positive for x 1,x 2 [ 1,1]?

5 Interval arithmetic [ 1,3] + [2,5] =?, [ 1,3] [2,5] =?, abs([ 7,1]) =?

6 Interval arithmetic [ 1,3] + [2,5] = [1,8], [ 1,3] [2,5] = [ 5,15], abs([ 7,1]) = [0,7]

7 The interval extension of f (x 1,x 2 ) = x 1 x 2 (x 1 + x 2 ) cosx 2 + sinx 1 sinx is [f ]([x 1 ],[x 2 ]) = [x 1 ] [x 2 ] ([x 1 ] + [x 2 ]) cos[x 2 ] +sin[x 1 ] sin[x 2 ] + 2.

8 Theorem (Moore, 1970) [f ]([x]) R + x [x],f (x) 0.

9 Set Inversion

10 A subpaving of R n is a set of non-overlapping boxes of R n. Compact sets X can be bracketed between inner and outer subpavings: X X X +.

11 Example. X = {(x 1,x 2 ) x x 2 2 [1,2]}.

12 Let f : R n R m and let Y be a subset of R m. Set inversion is the characterization of X = {x R n f(x) Y} = f 1 (Y).

13 We shall use the following tests. (i) [f]([x]) Y [x] X (ii) [f]([x]) Y = /0 [x] X = /0. Boxes for which these tests failed, will be bisected, except if they are too small.

14

15 Contractors

16 The operator C : IR n IR n is a contractor [4] for the equation f (x) = 0, if { C ([x]) [x] (contractance) x [x] and f (x) = 0 x C ([x]) (consistence)

17 Building contractors Consider the primitive equation x 1 + x 2 = x 3 with x 1 [x 1 ], x 2 [x 2 ], x 3 [x 3 ].

18 We have x 3 = x 1 + x 2 x 3 [x 3 ] ([x 1 ] + [x 2 ]) x 1 = x 3 x 2 x 1 [x 1 ] ([x 3 ] [x 2 ]) x 2 = x 3 x 1 x 2 [x 2 ] ([x 3 ] [x 1 ])

19 The contractor associated with x 1 + x 2 = x 3 is thus [x 1 ] [x 1 ] ([x 3 ] [x 2 ]) C [x 2 ] = [x 2 ] ([x 3 ] [x 1 ]) [x 3 ] [x 3 ] ([x 1 ] + [x 2 ])

20 Tubes

21 A trajectory is a function f : R R n. [6, 5]. For instance ( ) cost f (t) = sint is a trajectory.

22 Order relation f g t, i,f i (t) g i (t).

23 We have h = f g t, i,h i (t) = min(f i (t),g i (t)), h = f g t, i,h i (t) = max(f i (t),g i (t)).

24 The set of trajectories is a lattice. Interval of trajectories (tubes) can be dened.

25 Example. ( [ ] cost + 0,t 2 [f](t) = sint + [ 1,1] is an interval trajectory (or tube). )

26 Tube arithmetics

27 If [x] and [y] are two scalar tubes [1], we have [z] = [x] + [y] [z](t) = [x](t) + [y](t) (sum) [z] = shift a ([x]) [z](t) = [x](t + a) (shift) [z] = [x] [y] [z](t) = [x]([y](t)) (composition) [z] = [x] [z](t) = [ t 0 x (τ)dτ, t 0 x + (τ)dτ ] (integral)

28 Tube Contractors

29 Tube arithmetic allows us to build contractors [3].

30 Consider for instance the dierential constraint ẋ (t) = x (t + τ) u (t), x (t) [x](t),ẋ (t) [ẋ](t),u (t) [u](t),τ [τ] We decompose as follows x (t) = x (0) + t 0 y (τ)dτ y (t) = a(t) u (t). a(t) = x (t + τ)

31 Possible contractors are [x](t) = [x](t) ( [x](0) + t 0 [y](τ)dτ) [y](t) = [y](t) [a](t) [u](t) [u](t) = [u](t) [y](t) [a](t) [a](t) = [a](t) [y](t) [u](t) [a](t) = [a](t) [x](t + [τ]) [x](t) = [x](t) [a](t [τ]) [τ] = [τ](t)...

32 Example. Consider x (t) [x](t) with the constraint Contract the tube [x](t). t, x (t) = x (t + 1)

33 We rst decompose into primitive trajectory constraints x (t) = a(t + 1) x (t) = a(t).

34 Contractors [x](t) : = [x](t) [a](t + 1) [a](t) : = [a](t) [x](t 1) [x](t) : = [x](t) [a](t) [a](t) : = [a](t) [x](t)

35

36

37

38

39

40

41

42 [6]

43 Time-space estimation

44 Classical state estimation { ẋ(t) = f (x(t),u(t)) t R 0 = g (x(t),t) t T R. Space constraint g (x(t),t) = 0.

45 Example. ẋ 1 = x 3 cosx 4 ẋ 2 = x 3 cosx 4 ẋ 3 = u 1 ẋ 4 = u 2 (x 1 (5) 1) 2 + (x 2 (5) 2) 2 4 = 0 (x 1 (7) 1) 2 + (x 2 (7) 2) 2 9 = 0

46 With time-space constraints { ẋ(t) = f (x(t),u(t)) t R 0 = g (x(t),x(t ),t,t ) (t,t ) T R R.

47 Example. An ultrasonic underwater robot with state x = (x 1,x 2,...) = (x,y,θ,v,...) At time t the robot emits an omnidirectional sound. At time t it receives it ( ) x 1 x 2 ( 1 + x 2 x 2) 2 ( c t t )2 = 0.

48 Mass spring problem

49 The mass spring satises ẍ + ẋ + x x 3 = 0 i.e. { ẋ1 = x 2 The initial state is unknown. ẋ 2 = x 2 x 1 + x 3 1

50

51 ẋ 1 = x 2 ẋ 2 = x 2 x 1 + x 3 1 L x 1 (t 1 ) + L x 1 (t 2 ) = c (t 2 t 1 ).

52

53

54 Consider n robots R 1,...,R n described by ẋ i = f (x i, u i ),u i [u i ].

55 Omnidirectional sounds are emitted and received. A ping is a 4-uple (a,b,i,j) where a is the emission time, b is the reception time, i is the emitting robot and j the receiver.

56

57 With the time space constraint ẋ i = f (x i, u i ),u i [u i ]. g ( x i(k) (a(k)),x j(k) (b (k)),a(k),b (k) ) = 0 where g (x i, x j,a,b) = x 1 x 2 c (b a).

58 Clocks are uncertain. We only have measurements ã(k), b (k) of a(k),b (k) thanks to clocks h i. Thus ẋ i = f (x i, u i ),u i [u i ]. g ( x i(k) (a(k)),x j(k) (b (k)),a(k),b (k) ) = 0 ã(k) = h i(k) (a(k)) b (k) = h j(k) (b (k))

59 The drift of the clocks is bounded ẋ i = f (x i, u i ),u i [u i ]. g ( x i(k) (a(k)),x j(k) (b (k)),a(k),b (k) ) = 0 ã(k) = h i(k) (a(k)) b (k) = h j(k) (b (k)) ḣ i = 1 + n h, n h [n h ]

60 [2]

61

62

63

64 F. Le Bars, J. Sliwka, O. Reynet, and L. Jaulin. State estimation with eeting data. Automatica, 48(2):381387, A. Bethencourt and L. Jaulin. Cooperative localization of underwater robots with unsynchronized clocks. Journal of Behavioral Robotics, 4(4):233244, A. Bethencourt and L. Jaulin. Solving non-linear constraint satisfaction problems involving time-dependant functions. Mathematics in Computer Science, 8(3), G. Chabert and L. Jaulin. Contractor Programming. Articial Intelligence, 173: , S. Rohou.

65 Reliable robot localization: a constraint programming approach over dynamical systems. PhD dissertation, Université de Bretagne Occidentale, ENSTA-Bretagne, France, december S. Rohou, L. Jaulin, M. Mihaylova, F. Le Bars, and S. Veres. Guaranteed Computation of Robots Trajectories. Robotics and Autonomous Systems, 93:7684, 2017.