NishaRaniMahato,LucJaulin,SnehashishChakravertySwim-Smart2017,Manchester LocalizationforGroupofRobotsusingMatrixContractors

(1)

Localization

Localization for Group of Robots using Matrix Contractors

Nisha Rani Mahato, Luc Jaulin, Snehashish Chakraverty Swim-Smart 2017, Manchester

Nisha Rani Mahato, Luc Jaulin, Snehashish Chakraverty Swim-Smart 2017, ManchesterLocalization for Group of Robots using Matrix Contractors

(2)

Localization

Classical contractors

(3)

Localization

A (classical) contractor associated with a set X ⊂ R ⁿ is an operator C : IR ⁿ → IR ⁿ

such that for all boxes [x] ∈ IR ⁿ : Contraction: C ([x]) ⊂ [x],

Completeness: C ([x]) ∩ X = [x] ∩ X

(4)

Localization

Matrix contractors

(5)

Localization

A matrix contractor of a constraint Γ(A,B, . . .) is an operator C : IR ^m

^a

^×n

^a

× IR ^m

^b

^×n

^b

× · · · → IR ^m

^a

^×n

^a

× IR ^m

^b

^×n

^b

× . . . such that ([A ₁ ],[B ₁ ] ,. . . ) = C ([A] ,[B], . . . ) satisfies :

Contraction: [A ₁ ] ⊂ [A], [B ₁ ] ⊂ [B] , . . .

Completeness: Γ(A,B, . . . ), A ∈ [A] , B ∈ [B] , . . . implies that A ∈ [A ₁ ] , B ∈ [B ₁ ] , . . .

(6)

Localization

Example 1. The minimal contractor for the unary constraint

“S = S ^T ” is

C ([S]) = [S] ∩ [S] ^T . where [S]∈ IR ^n×n . For instance

C

[1, 3] [2,4]

[−1,3] [−1, 1]

=

[1, 3] [2,4]

[−1,3] [−1, 1]

∩

[1,3] [−1,3]

[2,4] [−1,1]

=

[1, 3] [2,3]

[2, 3] [−1,1]

.

(7)

Localization

Example 2. The minimal contractor for the ternary constraint

“A + B = C” is

C plus



 [A]

[B]

[C]



 =





[C] − [B]

[C] − [A]

[A] + [B]



 .

(8)

Localization

Example 3. For “ A · B = C” or for “R ^T = R ⁻¹ ”, no minimal contractor exists in the literature, to our knowledge.

(9)

Localization

Example 4. The minimal contractor C associated to “ P ∈ R ^m×m is a positive semi-definite matrix” has been proposed in [3]. For instance:

C









[−7, 3] [−1,4] [−5,4]

[−1, 4] [−8,3] [2, 9]

[−5, 4] [2,9] [4, 9]









=





[0,3] [−1, 2] [−4,4]

[−1,2] [0.4, 3] [2, 5.2]

[−4,4] [2,5.2] [4,9]





(10)

Localization

Interval angles?

(11)

Localization

The set of angles A is not a lattice. Thus, we cannot define intervals of angles.

(12)

Localization

Bisectable abstract domains

(13)

Localization

Generalize interval algorithms with bisections on a Riemannian manifold M such a R , R ⁿ , a sphere, the Klein bottle, etc.

(14)

Localization

Is such a paving always possible? How to define ’intervals’ of M ?

(15)

Localization

From the distance d (a,b) between a and b, we define the diameter w (X), X ⊂ M .

A bad family IM is a family of subsets of M which satisfies [2]:

(16)

Localization

1) IM is a Moore family (containing M ), i.e.,

∀i, [a] (i) ∈ IM ⇒ ^\

i

[a] (i ) ∈ IM

2) IM is equipped with a bisector, i.e., a function β : IM → IM × IM such that β ([x]) = {[a] ,[b]} : (i) [a] and [b] do not overlap,

(ii) [a] and [b] cover [x]

(iii) β minimizes max{w ([a]) , w ([b])}.

(17)

Localization

(18)

Localization

Embedding

(19)

Localization

The equation

cos (α) = 1

where α is an angle has a unique solution: α = 2k π, k ∈ Z . Angles form a Riemannian manifold.

(20)

Localization

Embedding. To avoid the ambiguity, we perform an embedding:

α 7→

cos α sin α

Or equivalenltly, using complex numbers:

α 7→ cos α + i · sin α

Now, we introduce a pessimism (Embedding effect)

(21)

Localization

The ’interval angle’ [ ^π ₂ ,π] is represented by the box [−1,0] + i · [0,1].

The sum can be done using the relation:

cos(a + b) = cos a · cosb − sin a · sin b sin(a + b) = cos a · sin b + sin a · cosb Other operations on angles can also be defined.

A classical interval resolution can thus be done.

(22)

Localization

Example. Solve

a + b = 0, a ∈ [ π

2 , π],b ∈ [ π 2 ,π ] We take [a] = [b] = [ ^π ₂ ,π].

(23)

Localization

The forward contraction is

cos(a + b) ∈ cos [a] ·cos [b] − sin [a] ·sin [b]

= cos [ ^π ₂ ,π]

· cos [ ^π ₂ , π]

− sin [ ^π ₂ ,π ]

· sin [ ^π ₂ ,π]

= [−1,0] · [−1,0] − [0,1] · [0,1]

sin(a + b) ∈ cos [a] ·sin [b] + sin [a] · cos [b]

= [0, 1] − [0,1] = [−1,1]

= [−1,0] · [0,1] + [0, 1] ·[−1,0]

= [−1, 0] + [−1,0] = [−2, 0]

Since 0 7→ (1,0), the backward step yields

(cos[a] ,sin [a]) = (cos [b] ,sin [b]) = (−1,0) .

(24)

Localization

Consider the equivalence relation α ∼ β ⇔ β − α

2π ∈ Z ⇔ cos(α − β ) = 1 The set A of all angles is

A = R

∼ = R 2π Z . Sets A , A × A , A ^m×n are Riemannian manifolds.

(25)

Localization

If α and β are angles and if ρ ∈ R , we can define α +β , α − β and ρ · α .

(26)

Localization

Arcs

(27)

Localization

An arc hα i is a connected subset of A . We have hα i = hα , α e i with α ∈ A and α e ∈ [0, π].

The set of all arcs is denoted by IA .

We may define an arc arithmetic. For instance 1

2 · 2 · D 0, π

2 E

= 1

2 · h0,πi = h0,π i .

(28)

Localization

IA is not a Moore family.

The smallest Moore family which contains IA is the unions of arcs.

(29)

Localization

(30)

Localization

Note. It may be dangerous to deal with union of arcs.

Example of Chabert[1]. With initial domains [x] = [y] = [1,9],

y = x

9 (x − 5) ² = 16y

an explosion of the number of intervals during the propagation occurs.

(31)

Localization

(32)

Localization

Absolute. Estimate the pose of robots (x _i ,y _i , θ _i ) ^T ,i ∈ {1, 2, . . .}.

Relative. Estimate the azimuth, the distances or the bearings. No absolute frame is needed.

(33)

Localization

Azimuth angles for three robots

Azimuth is measured using a compass and goniometric sensors.

(34)

Localization

Azimuth contractor

(35)

Localization

Azimuth matrix is the matrix of azimuth angles α _ij between i ^th robot and j ^th robot:

A =







0 α 12 · · · α 1n

α 21 0 . .. .. .

.. . . .. . .. α (n−1)n

α _n1 · · · α _n(n−1) 0





 .

By convention, α _ii = 0.

Note that A ∈ _2πZ ^R n

²

which is a Riemannian manifold.

(36)

Localization

This uncertainty can be represented under the form of an interval matrix.

[A] =







0 [α 12 ] · · · [α 1n ]

[α 21 ] 0 . .. .. .

.. . . .. . ..

α (n−1)n)

[α n1 ] · · ·

α _n(n−1)

0 



 .

where [α _ij ] ∈ IA . Recall that IA is not a Moore family.

(37)

Localization

For i, j ,k i 6= j 6= k ,

azimuth (A) ⇒







α ij − α ji ∼ π

(α ij − α _ik ) + (α _jk − α ji ) + (α _ki − α _kj ) ∼ π (α _ij − α _ki ) ∼ (α _ji − α _jk ) + (α _kj − α _ki )

This can used to build an azimuth contractor C az ([A]).

(38)

Localization

Distance contractor

(39)

Localization

The distance matrix associated with n points is

D =







0 d 12 · · · d 1n

d 21 0 . .. .. .

.. . . .. . .. d _(n−1)n

d n1 · · · d _n(n−1) 0







To build distance contractor consistent with the constraint “D is a distance matrix”, we consider constraints such as:

distance (D) ⇒

d ij = d ji

d _ij ≤ d _ik + d _kj

(40)

Localization

Distance-Azimuth contractor

(41)

Localization

The localization in terms of azimuth angles are done with respect to composition of azimuth C az ([A]) and distance C d ([D]) contractors.

We can also use mixed constraints such as distaz(D,A) ⇒

sin(α _ik − α ij ) ·d _ij = sin(α _ki − α kj ) · d kj

(42)

Localization

With five robots

(43)

Localization

A localization problem with five robots with respect to azimuth and distance is considered.

(44)

Localization

Take:

[A] =







0 [2, 2.1] −[2.8,3] [2.5, 3.3] − [2.1, 2.2]

[5.2, 5.3] 0 −[1.9,2] [3.1, 3.2] − [1.6, 1.7]

[0.1, 0.2] [1.2,1.3] 0 − [2.6, 2.7] − [1.3, 1.5]

[5.5, 6.5] [6.2,6.3] −[0.5,0.6] 0 − [0.7, 0.8]

[0.9, 0.1] [1.4,1.5] [1.7, 1.8] [2.2, 2.4] 0







[D] =







0 [9.9, 10] [8,10] [22,31] [10, 13]

[9.8,10] 0 [9,12] [19,22] [15, 20]

[7, 9] [8,11] 0 [19,21] [7, 10]

[23, 32] [20, 22] [20,21] 0 [26, 28]

[10, 15] [15, 22] [8,10] [25,30] 0







(45)

Localization

The contracted azimuth matrix is







0 [2.0,2.1] −[2.9,3] [2.5, 3.3] − [2.1, 2.2]

[5.2, 5.24] 0 −[1.9,2.0] [3.1, 3.2] − [1.6, 1.7]

[0.1, 0.2] [1.2,1.3] 0 − [2.6, 2.7] − [1.3, 1.4]

[5.6, 6.4] [6.2,6.3] [0.5, 0.6] 0 − [0.7, 0.8]

[0.9, 1.0] [1.4,1.5] [1.7, 1.8] [2.3, 2.4] 0





 .

(46)

Localization

Pose of five robots with azimuth measurements

(47)

Localization

The contractions with respect to azimuth and distance are:

[A] =







0 [2.0,2.1] − [2.94, 3] [2.6, 2.9] − [2.1,2.2]

[5.2, 5.24] 0 −[1.9,1.94] [3.1, 3.2] − [1.6,1.7]

[0.14,0.2] [1.2,1.3] 0 [2.6, 2.7] − [1.3,1.4]

[5.90,6.0] [6.2,6.3] [0.5, 0.54] 0 − [0.7,0.8]

[0.94,1.0] [1.4,1.5] [1.7,1.8] [2.3, 2.4] 0







[D] =







0 [9.9,10.1] [8.1,9] [25,30.6] [10.6, 13]

[9.9,10.1] 0 [10.3,11] [20.4,22] [16.7, 20]

[8.1,9] [10.3, 11] 0 [20, 21] [8,10]

[25.1,30.6] [20.4, 22] [20, 21] 0 [26, 28]

[10.6,13] [16.7, 20] [8, 10] [26, 28] 0







(48)

Localization

Pose of five robots before contraction

(49)

Localization

One contraction with respect to azimuth

(50)

Localization

After two contractions with respect to azimuth

(51)

Localization

Many contractions with respect to azimuth

(52)

Localization

Many contractions with respect to the distance

(53)

Localization

Many contractions with respect to both distance and azimuth

(54)

Localization

With 12 robots

(55)

Localization

A localization problem with 12 robots with respect to azimuth and distance is considered.

(56)

Localization

With 12 robots, before contraction

(57)

Localization

After contractions

(58)

Localization

Gilles Chabert.

Techniques d’intervalles pour la résolution de systèmes d’équations.

PhD dissertation, Nice, France, 2007.

L. Jaulin, B. Desrochers, and D. Massé.

Bisectable Abstract Domains for the resolution of equations involving complex numbers.

Reliable Computing, 23:35–46, 2016.

L. Jaulin and D. Henrion.

Contracting optimally an interval matrix without loosing any positive semi-definite matrix is a tractable problem.

Reliable Computing, 11(1):1–17, 2005.

(59)

Localization

Acknowledgement

(60)

Localization

The authors are thankful to the Science and Engineering Research Board, Department of Science and Technology, New Delhi, India for the funding to carry out the present research. N. R. Mahato is also supported by Raman-Charpak Fellowship 2016, Indo-French Centre for the Promotion of Advanced Research, New Delhi, India.

NishaRaniMahato,LucJaulin,SnehashishChakravertySwim-Smart2017,Manchester LocalizationforGroupofRobotsusingMatrixContractors

Localization for Group of Robots using Matrix Contractors