Cooperative Localization using angular measures

COOPERATIVE LOCALIZATION USING ANGULAR MEASURES

Sorawish DhanapanichkulAdvisor : Dr. Attawith Sudsang

Our problem

Localization

Multi-robot localization

Our problem

Our problem

Our problem Input

Angular measurements

Output A Positional pattern

Cooperative LocalizationAlgorithm

A11,…,A14 A21,…,A24

A31,…,A34 A41,…,A44

A51,…,A54

(X1,Y1)

(X2,Y2)

(X3,Y3)

(X4,Y4)

(X5,Y5)

Our problemLine Of Sight(LOS)

Introduction to problem

Unknown correspondence between measurements and robots’ ID

Introduction to problem

Matching Problem Between measurements and robots’

name

Naïve O(NN)

Proposed algorithms

Geometric based algorithm Based on triangulation Sensitive to noise

Convex optimization based algorithm Transform the problem to convex

optimization problem More flexibility

Scope

2D planar space Fully visible Include Uncertainty (measurement’s

noise)

Geometric based algorithm

Use property of convex hull Reduce matching complexity Based on triangulation

Triangulation

Example 3 robots 2 known coordinates

and 1 LOS

12

3

Triangulation : Ghost node > 3 robots (ex. 4 robots)

12

3

4

Ghost node elimination

1 more known coordinate + 2 LOSs

12

3

4

5

3 coordinates3 LOSs

Geometric based algorithm

Compute the position of 3 robots Triangulation using angular measures

of 3 robots with known position

Our matching algorithm

Convexity of LOS Graph

12

3

4

5Boundary points

Boundary lines

Define set of boundary point A boundary point

have 2 measurements which all the others reside between these two measurements

These 2 measurements are called leftmost(LM) and rightmost(RM)

RM LM <= 180 degrees

1 2

4

5

LM1

RM1

LM2

RM2

LM4RM4

LM5

RM5

Find the boundary points which connected with that reference

Choose reference point From set of boundary points By using LM and RM Set the distance between one of

them to be 1

1

(0,0)

RM1

LM5

LM2

RM5

12

5

Try to find the last LOS To find the last coordinate

12

51

Find last LOS

(0,0)

These 3 robots are forming a triangle Assuming that there are S robots

inside this triangle

1

Find last LOS

(0,0)

S

12

5

S+1Convex!!!

Example: S = 3 After sorting, compare 1st

measurement of robot 1 and 2

Find last LOS

1

(0,0)

3

12

5

Example: S = 3 After sorting, compare 2nd

measurement of robot 1 and 2

Find last LOS

1

(0,0)

3

12

5

Example: S = 3 After sorting, compare 3rd

measurement of robot 1 and 2

Find last LOS

1

(0,0)

3

12

5

Example: S = 3 After sorting, compare 4th

measurement of robot 1 and 2

Find last LOS

3+1

1

(0,0)

3

12

5

Algorithm summary

1. Use our matching algorithm to find 3 LOS

2. Calculate all intersection from 2 robots

3. Use 3rd robot’s measurements to eliminate ghost node

1

52

Time complexity : O(N2) + O(N2) + O(N2

lg(N))

Measurement noise

Due to our comparing method (opposite direction)

Change comparing method

Ghost returns

Special case

Ghost nodeEx. 6 robots

Experimental resultW

rong

rate

Number of robot

Convex optimization based algorithm

Convex optimization based algorithm Propose iterative method :: try to

minimize error Reduce problem to convex

optimization problem

Iterative method - flow

Random the answer

Update new answer by step vector

Meet termination condition

NoYes

End

Iterative method - Example

C

B

A

Actual

C

B

A

Answer0th step -Random

Iterative method - Example

C

B

A

Actual

CB

A

Answer1st step

Iterative method - Example

C

B

A

Actual

C B

A

Answer2nd step

Iterative method - Example

C

B

A

Actual

CB

A

Answer3rd step

Iterative method - Example

C

B

A

Actual

C

B

A

AnswerTermination condition

Step vector

Difference between “Sum vector” of actual and answer

Ex.

Actual Answer

0th step

C

B

AC

B

A

A A

Sum vector

A

Step vector

Example

C#

Actual Answer

Example

C#

Actual Answer

Error

Total error = Σ errorij

Mean angular error = total error / no. of input

B

A A

BerrorAB

Actual

Result

Experimental resultM

ean a

ng

ula

r err

or

(Radia

n)

Number of robot

Mathematical explanation

Compare iterative method with gradient descent

Proof of correctness

Update eqn <-> Gradient descent Update equation

Gradient descent

Optimization problem !!!

Integration of gradient

Gradient of function

After integration

Convex function

Property One lowest value Locally optimal point = Globally optimal

point

Convex!!

Proof of correctness

Rewrite the equation (no error)

Lowest value1. All robots in the result are at the same

place2. vector from robot ith to jth of the result =

vector from robot ith to jth of the real robot

Experimental resultM

ean a

ng

ula

r err

or

(Radia

n)

Number of robot

3.437 degrees

Experimental resultN

um

ber

of

itera

tion s

tep

Number of robot

Algorithm summary

Reduce problem Convex optimization

Tolerate to measurement noise

Conclusion

Summary

2 algorithms Localize group of robots using only

angular measures 1st algorithm

Reduce matching complexity 2nd algorithm

Reduce problem

Key Convexity – convex set & convex

function

Future works

Handle obstruction tested by using 2nd algorithm and seems

to work > 2D

Both algorithms can be effortlessly adopt to be used with higher dimensional environment

END

Q&A

Experimental result

Real world experiment 5 robots Using Omni-directional

camera Mean angular error =

2.31652 degrees

Our matching algorithm - flow1. Define set of boundary points2. Choose one of boundary point to be

a reference point3. Find the boundary points which

connected with that reference 4. Find last LOS

Gradient descent step bounding Depends on how good the initial

point is & the final require accuracy

Nu

mb

er

of

itera

tion

ste

p

Number of robots

Mathematical explanation

Robots’ position

Measurement without noise

Measurement with noise

Key idea

Try to find “answer” that match with input the most

System overview

Multi-robot system Equipped with omni-directional &

compass Tagged with color marker

Our problem Input

Angular measurements

Output A Positional pattern

A11,…,A14 A21,…,A24

A31,…,A34 A41,…,A44

A51,…,A54

Our problem Input

Angular measurements

Output A Positional pattern

Angular measurements from each robot

Image from Omni-directional camera

Compass

x

y

N

x

y

Our problem Input

Angular measurements

Output A Positional pattern

A11,…,A14 A21,…,A24

A31,…,A34 A41,…,A44

A51,…,A54

(X1,Y1)

(X2,Y2)

(X3,Y3)

(X4,Y4)

(X5,Y5)

Our problem Input

Angular measurements

Output A Positional pattern

Positional pattern

y

x

Key idea

C

B

x1

y1

x3

x2

y3

y2

A

x0

y0

Same reference

Key idea

C

B

x1

y1

x3

x2

y3

y2

A

x0

y0

Matching

Line of sight (LOS)

Normal of boundary

Property :: we can find a pair of nodes which connected to it.

2

5

Naïve MMM

Try all possible solution (exhaustive search)

Verify configuration > O(NN)1

23

4 2

2 1

1 1

1

2

3

Related works

Measurement

Absolute position

Range

Angular Identity

Wireless sensor network

Related works

Measurement

Absolute position

Range

Angular Identity

Multi-robot system

Our work

Measurement

Absolute position

Range

Angular Identity

Naïve

Try all possible solution (exhaustive search)

1

23

Naïve

Try all possible solution (exhaustive search)

Verify configuration > O(NN)

1

23

Elimination - Prerequisite 3 known coordinates 3 LOS

12

5

Elimination - Prerequisite 3 known coordinates 3 LOS

12

5

+ Not collinear

Elimination - Prerequisite 3 known coordinates 3 correspondences among them

12

5

Special case

3 collinear coordinates Unable to locate points which lie on this

line

12

5

Coordinates LOSs

12

5

(0,0)

1

(x2,y2)

(x5,y5)

Update equation

State vector (The answer)

Update equation

Step vector

Sum vector (actual) of ith robot

Step vector

Iterative method Minimize error in every step

Mathematical explanation of the above method Shows that this problem is convex

optimization problem

Choose one of boundary point to be reference point Choose one from set of boundary

point and set its coordinate to be (0,0)

5(0,0)