Cooperative Localization using angular measures
-
Upload
galvin-montoya -
Category
Documents
-
view
28 -
download
1
description
Transcript of Cooperative Localization using angular measures
![Page 1: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/1.jpg)
COOPERATIVE LOCALIZATION USING ANGULAR MEASURES
Sorawish DhanapanichkulAdvisor : Dr. Attawith Sudsang
![Page 2: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/2.jpg)
Our problem
Localization
Multi-robot localization
![Page 3: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/3.jpg)
Our problem
![Page 4: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/4.jpg)
Our problem
![Page 5: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/5.jpg)
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)
![Page 6: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/6.jpg)
Our problemLine Of Sight(LOS)
![Page 7: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/7.jpg)
Introduction to problem
Unknown correspondence between measurements and robots’ ID
![Page 8: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/8.jpg)
Introduction to problem
Matching Problem Between measurements and robots’
name
Naïve O(NN)
![Page 9: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/9.jpg)
Proposed algorithms
Geometric based algorithm Based on triangulation Sensitive to noise
Convex optimization based algorithm Transform the problem to convex
optimization problem More flexibility
![Page 10: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/10.jpg)
Scope
2D planar space Fully visible Include Uncertainty (measurement’s
noise)
![Page 11: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/11.jpg)
Geometric based algorithm
Use property of convex hull Reduce matching complexity Based on triangulation
![Page 12: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/12.jpg)
Triangulation
Example 3 robots 2 known coordinates
and 1 LOS
12
3
![Page 13: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/13.jpg)
Triangulation : Ghost node > 3 robots (ex. 4 robots)
12
3
4
![Page 14: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/14.jpg)
Ghost node elimination
1 more known coordinate + 2 LOSs
12
3
4
5
3 coordinates3 LOSs
![Page 15: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/15.jpg)
Geometric based algorithm
Compute the position of 3 robots Triangulation using angular measures
of 3 robots with known position
![Page 16: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/16.jpg)
Our matching algorithm
Convexity of LOS Graph
12
3
4
5Boundary points
Boundary lines
![Page 17: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/17.jpg)
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
![Page 18: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/18.jpg)
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
![Page 19: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/19.jpg)
Try to find the last LOS To find the last coordinate
12
51
Find last LOS
(0,0)
![Page 20: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/20.jpg)
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!!!
![Page 21: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/21.jpg)
Example: S = 3 After sorting, compare 1st
measurement of robot 1 and 2
Find last LOS
1
(0,0)
3
12
5
![Page 22: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/22.jpg)
Example: S = 3 After sorting, compare 2nd
measurement of robot 1 and 2
Find last LOS
1
(0,0)
3
12
5
![Page 23: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/23.jpg)
Example: S = 3 After sorting, compare 3rd
measurement of robot 1 and 2
Find last LOS
1
(0,0)
3
12
5
![Page 24: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/24.jpg)
Example: S = 3 After sorting, compare 4th
measurement of robot 1 and 2
Find last LOS
3+1
1
(0,0)
3
12
5
![Page 25: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/25.jpg)
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))
![Page 26: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/26.jpg)
Measurement noise
Due to our comparing method (opposite direction)
Change comparing method
![Page 27: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/27.jpg)
Ghost returns
Special case
Ghost nodeEx. 6 robots
![Page 28: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/28.jpg)
Experimental resultW
rong
rate
Number of robot
![Page 29: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/29.jpg)
Convex optimization based algorithm
![Page 30: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/30.jpg)
Convex optimization based algorithm Propose iterative method :: try to
minimize error Reduce problem to convex
optimization problem
![Page 31: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/31.jpg)
Iterative method - flow
Random the answer
Update new answer by step vector
Meet termination condition
NoYes
End
![Page 32: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/32.jpg)
Iterative method - Example
C
B
A
Actual
C
B
A
Answer0th step -Random
![Page 33: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/33.jpg)
Iterative method - Example
C
B
A
Actual
CB
A
Answer1st step
![Page 34: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/34.jpg)
Iterative method - Example
C
B
A
Actual
C B
A
Answer2nd step
![Page 35: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/35.jpg)
Iterative method - Example
C
B
A
Actual
CB
A
Answer3rd step
![Page 36: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/36.jpg)
Iterative method - Example
C
B
A
Actual
C
B
A
AnswerTermination condition
![Page 37: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/37.jpg)
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
![Page 38: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/38.jpg)
Example
C#
Actual Answer
![Page 39: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/39.jpg)
Example
C#
Actual Answer
![Page 40: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/40.jpg)
Error
Total error = Σ errorij
Mean angular error = total error / no. of input
B
A A
BerrorAB
Actual
Result
![Page 41: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/41.jpg)
Experimental resultM
ean a
ng
ula
r err
or
(Radia
n)
Number of robot
![Page 42: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/42.jpg)
Mathematical explanation
Compare iterative method with gradient descent
Proof of correctness
![Page 43: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/43.jpg)
Update eqn <-> Gradient descent Update equation
Gradient descent
Optimization problem !!!
![Page 44: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/44.jpg)
Integration of gradient
Gradient of function
After integration
![Page 45: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/45.jpg)
Convex function
Property One lowest value Locally optimal point = Globally optimal
point
Convex!!
![Page 46: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/46.jpg)
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
![Page 47: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/47.jpg)
Experimental resultM
ean a
ng
ula
r err
or
(Radia
n)
Number of robot
3.437 degrees
![Page 48: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/48.jpg)
Experimental resultN
um
ber
of
itera
tion s
tep
Number of robot
![Page 49: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/49.jpg)
Algorithm summary
Reduce problem Convex optimization
Tolerate to measurement noise
![Page 50: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/50.jpg)
Conclusion
![Page 51: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/51.jpg)
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
![Page 52: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/52.jpg)
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
![Page 53: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/53.jpg)
END
Q&A
![Page 54: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/54.jpg)
Experimental result
Real world experiment 5 robots Using Omni-directional
camera Mean angular error =
2.31652 degrees
![Page 55: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/55.jpg)
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
![Page 56: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/56.jpg)
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
![Page 57: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/57.jpg)
Mathematical explanation
Robots’ position
Measurement without noise
Measurement with noise
![Page 58: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/58.jpg)
Key idea
Try to find “answer” that match with input the most
![Page 59: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/59.jpg)
System overview
Multi-robot system Equipped with omni-directional &
compass Tagged with color marker
![Page 60: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/60.jpg)
Our problem Input
Angular measurements
Output A Positional pattern
A11,…,A14 A21,…,A24
A31,…,A34 A41,…,A44
A51,…,A54
![Page 61: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/61.jpg)
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
![Page 62: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/62.jpg)
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)
![Page 63: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/63.jpg)
Our problem Input
Angular measurements
Output A Positional pattern
Positional pattern
y
x
![Page 64: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/64.jpg)
Key idea
C
B
x1
y1
x3
x2
y3
y2
A
x0
y0
![Page 65: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/65.jpg)
Same reference
Key idea
C
B
x1
y1
x3
x2
y3
y2
A
x0
y0
Matching
Line of sight (LOS)
![Page 66: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/66.jpg)
Normal of boundary
Property :: we can find a pair of nodes which connected to it.
2
5
![Page 67: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/67.jpg)
Naïve MMM
Try all possible solution (exhaustive search)
Verify configuration > O(NN)1
23
4 2
2 1
1 1
1
2
3
![Page 68: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/68.jpg)
Related works
Measurement
Absolute position
Range
Angular Identity
Wireless sensor network
![Page 69: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/69.jpg)
Related works
Measurement
Absolute position
Range
Angular Identity
Multi-robot system
![Page 70: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/70.jpg)
Our work
Measurement
Absolute position
Range
Angular Identity
![Page 71: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/71.jpg)
Naïve
Try all possible solution (exhaustive search)
1
23
![Page 72: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/72.jpg)
Naïve
Try all possible solution (exhaustive search)
Verify configuration > O(NN)
1
23
![Page 73: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/73.jpg)
Elimination - Prerequisite 3 known coordinates 3 LOS
12
5
![Page 74: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/74.jpg)
Elimination - Prerequisite 3 known coordinates 3 LOS
12
5
+ Not collinear
![Page 75: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/75.jpg)
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
![Page 76: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/76.jpg)
Coordinates LOSs
12
5
(0,0)
1
(x2,y2)
(x5,y5)
![Page 77: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/77.jpg)
Update equation
State vector (The answer)
Update equation
![Page 78: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/78.jpg)
Step vector
Sum vector (actual) of ith robot
Step vector
![Page 79: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/79.jpg)
Iterative method Minimize error in every step
Mathematical explanation of the above method Shows that this problem is convex
optimization problem
![Page 80: Cooperative Localization using angular measures](https://reader030.fdocuments.in/reader030/viewer/2022032604/56812b1d550346895d8f129e/html5/thumbnails/80.jpg)
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)