Mechanical Engineering and Materials ScienceDuke University

Information Driven Path Planning and Control for Collaborative Aerial Robotic Sensor Using Artificial Potential Functions

W. Lu and S. Ferrari

1

A.C. Bellini and R.NaldiComputer Science and EngineeringUniverisity of Bologna, ItalyAmerican Control ConferencePortland (OR), June 3-6, 2014

Outline

• Introduction and Motivation

• Problem Formulation : Aerial Robotic Sensor Path Planning

• Methodology: Hybrid Controller• Measurement Utility, Information Value• Information Potential Field Method• High Level and Low Level Controller

• Results• Signal Sensor • Multiple Sensors

• Conclusion and Recommendations

Introduction and Motivation

• Modern Sensor Systems – multiple sensors installed on mobile platforms

- landmine detection and identification

- ambient intelligence, monitoring of urban environments, search & rescue

• Traditional paradigm: sensor information is used as feedback to sensors in order to support the sensor navigation.

• New paradigm: sensors’ motion is planned considering the expected utility of future measurement process, to support one or more sensing objectives

• Research Emphasis: Geometric aerial robotic sensor path planning

-- Address couplings between sensor measurements and sensor dynamics

-- Optimize sensing objectives (e.g., detection, classification, tracking.)

Motivation: Applications of Sensor Path Planning

• Applications: landmine detection, sensor networks for monitoring endangered species

C. Cai and S. Ferrari, “Information-Driven Sensor Path Planning by Approximate Cell Decomposition,” IEEE Transactions on Systems, Man, and Cybernetics - Part B, Vol. 39, No. 2, 2009.

M. Qian and S. Ferrari, “Probabilistic deployment for multiple sensor systems,” Proc. SPIE, 2005

Problem Formulation

5

Problem Formulation: Aerial Robotic Sensor Path Planning

WAAW r13 ⊂=⊂ },, { plaftormssensor robotic where,RpaceGiven work Ar

WBB W,SS n1r1 ⊂=⊂= },, { obstacles fixed },, { FOVsensor with BnS

.)(Z)]([benefitnobservatioexpextedsomeoptimizeto

},],1,0[, )(|Z{)(tsmeasuremenofsequenceaFind

:Purpose

)(Zi∑

∈

=

∈∈=≠∩=

τ

τ

τφτ

Z

Tiiii

i

VZV

Iis(s),Z qqST

State 𝐪𝐪𝑖𝑖 = 𝑥𝑥𝑖𝑖 𝑦𝑦𝑖𝑖 𝑧𝑧𝑖𝑖 𝜃𝜃𝑖𝑖 𝑇𝑇

1A

2A

2S

WTT m1 ⊂= },, { targetsfixed Tm

1B2B

3B

1T

2T

3T

1SW

Problem Formulation: Aerial Robotic Sensor Dynamics

1A

1S

𝑀𝑀�̈�𝐏 = −𝜇𝜇𝑓𝑓𝐑𝐑𝐞𝐞𝟑𝟑 + 𝑀𝑀𝑀𝑀𝐞𝐞3

�̇�𝐑 = 𝐑𝐑𝐑𝐑 𝐰𝐰

𝐉𝐉�̇�𝐰 = 𝐑𝐑 𝐉𝐉𝐰𝐰 𝐰𝐰 + 𝛍𝛍𝜏𝜏

𝐏𝐏 = 𝑥𝑥𝑖𝑖 𝑦𝑦𝑖𝑖 𝑧𝑧𝑖𝑖

𝐰𝐰 = 𝐰𝐰𝑥𝑥 𝐰𝐰𝑦𝑦 𝐰𝐰𝑧𝑧

𝐞𝐞𝟑𝟑 = 𝟎𝟎 𝟎𝟎 𝟏𝟏 𝑇𝑇

𝑺𝑺 𝑥𝑥1 𝑥𝑥2 𝑥𝑥3 𝑇𝑇

=0 −𝑥𝑥3 𝑥𝑥2𝑥𝑥3 0 −𝑥𝑥1−𝑥𝑥2 𝑥𝑥1 0

• Motion dynamics

P ∈ ℝ3: position of center gravity

w ∈ ℝ3: angular speed (body frame)

R ∈ ℝ3×3: rotation matrix(body→inertial)

M ∈ ℝ>0: mass

J ∈ ℝ3×3: inertia matrix

𝜇𝜇𝑓𝑓 ∈ ℝ≥0: control force

𝛍𝛍𝜏𝜏 ∈ ℝ3: control torque vector

Methodology

11

Information Value

Conditional mutual information

Conditional entropy

Information Benefit to have Zi I(X;Y|z)H(X|Y,z) H(Y|X,z)

H(X|z) H(Y|z)

( ) ( ) log ( )x

H X p x p x∈

= −∑X

Entropy

M. Cover and J. Thomas, Elements of Information Theory, 1991

𝐻𝐻 𝑋𝑋 𝑍𝑍𝑖𝑖 = �𝑧𝑧𝑖𝑖∈𝒵𝒵

𝑝𝑝 𝑧𝑧𝑖𝑖 𝐻𝐻 𝑋𝑋 𝑧𝑧𝑖𝑖

𝐼𝐼 𝑋𝑋;𝑍𝑍𝑖𝑖 𝜆𝜆 = 𝐻𝐻 𝑋𝑋 𝜆𝜆 − 𝐻𝐻 𝑋𝑋 𝑍𝑍𝑖𝑖 , 𝜆𝜆

𝑉𝑉 𝑍𝑍𝑖𝑖 = 𝐼𝐼(𝑋𝑋;𝑍𝑍𝑖𝑖|𝜆𝜆)

𝐼𝐼(𝑋𝑋;𝑍𝑍𝑖𝑖|𝜆𝜆)𝐻𝐻 𝑋𝑋 𝑍𝑍𝑖𝑖 , 𝜆𝜆 𝐻𝐻 𝑍𝑍𝑖𝑖 𝑋𝑋, 𝜆𝜆

𝐻𝐻 𝑋𝑋 𝜆𝜆 𝐻𝐻 𝑍𝑍𝑖𝑖 𝜆𝜆

Hybrid Controller

Cascade control**

Information potential*

**R. Naldi, M. Furci, “Global Trajectory Tracking for Underactuated VTOL Aerial Vehicles using a Cascade Control Paradigm”, IEEE Conference on Decision and Control, 2013

*W. Lu, G. Zhang, and S. Ferrari, “An Information Potential Approach to Integrated Sensor Path Planning and Control” IEEE Transaction on Robotics, to appear

Low level controller

Mode plannerHigh level controller

ReferencesControl input

ConfigurationReferences

UAV simulatorControl inputConfiguration

Information Potential Field Construction

Novel attractive potentialthe ith target )1()( 2

)(

2

2

ai

ti

Vaiatti eVU σ

ρ

σηq

q−

−=

Potential at q attrep UUU )()()( qqq +=

Total attractive potential ∏==

m

i attiatt UU1

)()( qq

Repulsive potentialof the ith obstacle

>

≤

−=

0

0

2

01

)(if0

)(if)(1)(

121

)(ρρ

ρρρρ

η

q

qqqq

bi

biattb

irepiUU

∑=

=n

irepirep UU

1)()( qqTotal repulsive potential

Potential between tworobotic sensors

>

≤

−=

0

0

2

03

)(if0

)(if)(1)(

121

)(

ρρ

ρρρρ

η

q

qqqq

rjk

rjkattr

jkj

rkUU

*W. Lu, G. Zhang, and S. Ferrari, “An Information Potential Approach to Integrated Sensor Path Planning and Control” IEEE Transaction on Robotics, to appear

Connection Between IRD and Potential Field Methods

When the robotic sensor is at a local minimum, randomly generate milestones in surrounding subspace

Milestones distribution

A function of the potential at q, , is used to measure the probability

∉

∈= ∫ −

−

A

Ade

e

fA

U

U

q

qqq q

q

if0

if)( )(

)(

)(qUe−

of sampling a milestone at q.

Escaping Local Minima by IRD method

The milestones are connected to the local minimum to construct the roadmap

A path from the local minimum to a milestone with lower potential than the potential at the local minimum is found.

High Level Control and Low Level Control

High Level Control:1. Fix time step as 𝑑𝑑𝑑𝑑,

Low Level Cascade Control*:1. Position Control Law2. Attitude Control Law

*R. Naldi, M. Furci, “Global Trajectory Tracking for Underactuated VTOL Aerial Vehicles using a Cascade Control Paradigm”, IEEE Conference on Decision and Control, 2013

2. Information Potential

21111)(21

)()()(

dtIMMM

U

dtttdttT

j

jjr

⊗∇−

+=+

q

qqq

attrep UUU )()()( qqq +=

Results

18

Result: One Sensor

One robotic sensor𝑛𝑛 targets with same 𝑉𝑉𝑖𝑖𝑚𝑚 fixed obstacles 𝑟𝑟 moving obstacles

𝜃𝜃(𝑟𝑟𝑟𝑟𝑑𝑑)

𝑥𝑥(𝑚𝑚)

𝑧𝑧(𝑚𝑚)

𝑦𝑦(𝑚𝑚)

0 100 200 300 400 500 600 700Iteration

80

70

60

50

40

Entro

py

0 100 200 300 400 500 600Iteration

8

7

6

5

4

3

2

1

Result: Two Sensors

𝑉𝑉 𝑍𝑍𝑖𝑖

Rwide

Rpricise

𝑉𝑉𝑤𝑤𝑖𝑖𝑤𝑤𝑤𝑤 = 3.67𝑉𝑉𝑝𝑝𝑝𝑝𝑤𝑤𝑝𝑝𝑖𝑖𝑝𝑝𝑤𝑤 = 0.35

0 50 100 150 200 250 300𝑥𝑥𝑖𝑖

8

7

6

5

4

3

First sensor ( Rwide ): Range [1, 256], white Gaussian noise of 𝜎𝜎 = 5

Second sensor ( Rprecise ): Range [5, 25], white Gaussian noise of 𝜎𝜎 = 0.1

One target:𝑃𝑃 𝑋𝑋 = 𝑥𝑥𝑖𝑖 = 1

256, 𝑥𝑥𝑖𝑖 ∈ 1,2,⋯ , 256

Result: Two Sensors

𝑃𝑃 𝑋𝑋 = 𝑥𝑥𝑖𝑖

After Rwide senses the target,

𝑉𝑉𝑤𝑤𝑖𝑖𝑤𝑤𝑤𝑤 = 0,𝑉𝑉𝑝𝑝𝑝𝑝𝑤𝑤𝑝𝑝𝑖𝑖𝑝𝑝𝑤𝑤 > 0

𝑥𝑥𝑖𝑖

𝑃𝑃 𝑥𝑥𝑖𝑖

0 50 100 150 200 250 300𝑥𝑥𝑖𝑖

8

7

6

5

4

3

2

1

0

Conclusions

• Hybrid controller for aerial robotic sensor path planning • Information potential and reference model are integrated to design high

level controller• Cascade controller navigates sensor along reference trajectories• Maximizing classification performance.

Acknowledgement• This work was supported by the National Science Foundation under

grant• ECCS 1028506 and the FP7 European project SHERPA (IP 600958).