A Coupled Oscillators-based Control Architecture for Locomotory - - PowerPoint PPT Presentation

A Coupled Oscillators-based Control Architecture for Locomotory Gaits

Presented at the Conference on Decision and Control Los Angeles, CA December 15-17, 2014

Amirhossein Taghvaei

Joint work with: S. A. Hutchinson, and P. G. Mehta

• Dept. of Mechanical Science and Engineering

and the Coordinated Science Laboratory University of Illinois at Urbana-Champaign

Dec 16, 2014

Research supported by NSF grant EECS-0925534 and AFOSR grant FA9550-09-1-0190

Outline

1 Problem overview 2 Literature survey 3 Proposed approach 4 Example with simulation result 5 Summary Control of Locomtory Gaits Amirhossein Taghvaei 2 / 19 Amirhossein

Locomotion

Animal locomotion

Tadpole, San Diego zoo Snake, BBC News Usain Bolt, theconsultant.eu

Locomotion gait

Swimming gait, Journal of Experimental Biology Snake locomotion, Biokids, Univ of Michigan Walking Gait, Science Direct

• P. Holmes, R. J. Full, D. Koditschek, and J. Guckenheimer. The dynamics of legged locomotion, 2006

Control of Locomtory Gaits Amirhossein Taghvaei 3 / 19 Amirhossein

Bio-Inspired Robots

RHex robot, Boston Dynamics Snakelike Robot, Biorobotics CMU http://groups.csail.mit.edu/locomotion/ Wind up toy robot

• D. Xinyan, L. Schenato, and S. S. Sastry, 2006
• Z. G. Zhang, N. Yamashita, M. Gondo, A. Yamamoto, and T. Higuchi, 2008
• R. L. Hatton and H. Choset, 2010

Control of Locomtory Gaits Amirhossein Taghvaei 4 / 19 Amirhossein

Bio-Inspired Robots Periodic actuation of internal degree of freedom → global displacement

RHex robot, Boston Dynamics Snakelike Robot, Biorobotics CMU http://groups.csail.mit.edu/locomotion/ Wind up toy robot

• D. Xinyan, L. Schenato, and S. S. Sastry, 2006
• Z. G. Zhang, N. Yamashita, M. Gondo, A. Yamamoto, and T. Higuchi, 2008
• R. L. Hatton and H. Choset, 2010

Control of Locomtory Gaits Amirhossein Taghvaei 4 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

Shape and Group Variable

Shape variable x ∈ M x = (x1,x2) ∈ T2 Internal dynamics ¨ x = F(x, ˙ x,τ) I(x)¨ x = C(x, ˙ x)˙ x−kx+τ Group variable g ∈ G g = (

• r,ψ) ∈ SE(2)

Group dynamics g−1˙ g = A(x)˙ x ˙ ψ = A1(x1,x2)˙ x1 +A2(x1,x2)˙ x2 The dynamics does not dependen on the group variable

Figure : 3-link system

• S. D. Kelly, and R. M. Murray, Geometric Phase and Robotic Locomotion, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 5 / 19 Amirhossein

General Approach

Control input Shape dynamics Group dynamics

Approach

1 Gait design: Choose a periodic orbit in the shape space to induce the desired change in

group variable.

2 Gait generation: Implement a law for the control input, that leads to the desired periodic

• rbit in the shape space.
• R. W .Brockett. Pattern generation and the control of nonlinear systems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Juan B Melli, Clarence W Rowley, and Dzhelil S Rufat. Motion planning for an articulated body in a perfect planar fluid, 2006

• R. M. Murray, and S. S. Sastry, Nonholonomic motion planning: Steering using sinusoids. 1993.
• J. Blair, and T. Iwasaki, Optimal gaits for mechanical rectifier systems. 2011.
• R. L. Hatton and H. Choset, Generating gaits for snake robots: Annealed chain fitting and keyframe wave
• extraction. 2010.

. . .

Control of Locomtory Gaits Amirhossein Taghvaei 6 / 19 Amirhossein

General Approach

Control input Shape dynamics Group dynamics

Approach

1 Gait design: Choose a periodic orbit in the shape space to induce the desired change in

group variable.

2 Gait generation: Implement a law for the control input, that leads to the desired periodic

• rbit in the shape space.
• R. W .Brockett. Pattern generation and the control of nonlinear systems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Juan B Melli, Clarence W Rowley, and Dzhelil S Rufat. Motion planning for an articulated body in a perfect planar fluid, 2006

• R. M. Murray, and S. S. Sastry, Nonholonomic motion planning: Steering using sinusoids. 1993.
• J. Blair, and T. Iwasaki, Optimal gaits for mechanical rectifier systems. 2011.
• R. L. Hatton and H. Choset, Generating gaits for snake robots: Annealed chain fitting and keyframe wave
• extraction. 2010.

. . .

Control of Locomtory Gaits Amirhossein Taghvaei 6 / 19 Amirhossein

General Approach

Control input Shape dynamics Group dynamics

Approach

1 Gait design: Choose a periodic orbit in the shape space to induce the desired change in

group variable.

2 Gait generation: Implement a law for the control input, that leads to the desired periodic

• rbit in the shape space.
• R. W .Brockett. Pattern generation and the control of nonlinear systems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Juan B Melli, Clarence W Rowley, and Dzhelil S Rufat. Motion planning for an articulated body in a perfect planar fluid, 2006

• R. M. Murray, and S. S. Sastry, Nonholonomic motion planning: Steering using sinusoids. 1993.
• J. Blair, and T. Iwasaki, Optimal gaits for mechanical rectifier systems. 2011.
• R. L. Hatton and H. Choset, Generating gaits for snake robots: Annealed chain fitting and keyframe wave
• extraction. 2010.

. . .

Control of Locomtory Gaits Amirhossein Taghvaei 6 / 19 Amirhossein

Proposed Approach Approach

1 Periodic input, to have shape variable oscillate in periodic manner. 2 Noisy sensory measurements of the shape variables. 3 Control actuation via manipulation of parameters of the system. 4 Find optimal control law, to achieve maneuver about nominal gait, based on noisy sensory

measurements

Shape dynamics Group dynamics

Periodic input

Control of Locomtory Gaits Amirhossein Taghvaei 7 / 19 Amirhossein

Proposed Approach Approach

1 Periodic input, to have shape variable oscillate in periodic manner. 2 Noisy sensory measurements of the shape variables. 3 Control actuation via manipulation of parameters of the system. 4 Find optimal control law, to achieve maneuver about nominal gait, based on noisy sensory

measurements

Shape dynamics Group dynamics

Periodic input

Control of Locomtory Gaits Amirhossein Taghvaei 7 / 19 Amirhossein

Proposed Approach Approach

1 Periodic input, to have shape variable oscillate in periodic manner. 2 Noisy sensory measurements of the shape variables. 3 Control actuation via manipulation of parameters of the system. 4 Find optimal control law, to achieve maneuver about nominal gait, based on noisy sensory

measurements

Shape dynamics Group dynamics

Periodic input

Control of Locomtory Gaits Amirhossein Taghvaei 7 / 19 Amirhossein

Proposed Approach Approach

1 Periodic input, to have shape variable oscillate in periodic manner. 2 Noisy sensory measurements of the shape variables. 3 Control actuation via manipulation of parameters of the system. 4 Find optimal control law, to achieve maneuver about nominal gait, based on noisy sensory

measurements

Shape dynamics Group dynamics

Periodic input

Control of Locomtory Gaits Amirhossein Taghvaei 7 / 19 Amirhossein

Proposed Approach Approach

1 Periodic input, to have shape variable oscillate in periodic manner. 2 Noisy sensory measurements of the shape variables. 3 Control actuation via manipulation of parameters of the system. 4 Find optimal control law, to achieve maneuver about nominal gait, based on noisy sensory

measurements

Shape dynamics Group dynamics

Periodic input

?

Control of Locomtory Gaits Amirhossein Taghvaei 7 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x)˙ x

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x)˙ x Periodic control input: τ(t) = τ0 sin(ω0t)

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x)˙ x Periodic control input: τ(t) = τ0 sin(ω0t)

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x)˙ x Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Control actuation: d(t) = ¯ d(1+u) → ˙ ψ = A(x,u)˙ x

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Geometric phase

Control cost Small control penalty parameter

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Geometric phase

Control cost Small control penalty parameter

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Geometric phase

Control cost Small control penalty parameter

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Geometric phase

Control cost Small control penalty parameter

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Example: 2-body System

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ0 sin(ω0t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Geometric phase

Control cost Small control penalty parameter

Head

Tail

Shape variable: x Group variable: ψ

Control of Locomtory Gaits Amirhossein Taghvaei 8 / 19 Amirhossein

Numerical Result, Open Loop

Shape dynamics Group dynamics

Periodic input

T 2T 3T 4T t −1.0 1.0 τ Torque input: τ(t) T 2T 3T 4T t

−π

2 π 2

y Observation: y(t) ˜ h(x, ˙ x) = x(t) T 2T 3T 4T t

−π

6 π 6

x State: x(t) T 2T 3T 4T t −0.2 0.0 0.2 q1 Head Orientation q1(t)

Control of Locomtory Gaits Amirhossein Taghvaei 9 / 19 Amirhossein

Solution: Phase reduction

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Limit cycle solution: XLC(θ(t)) = (x(t), ˙ x(t)) θ(t) = (ω0t +θ0) mod 2π Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 10 / 19 Amirhossein

Solution: Phase reduction

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Limit cycle solution: XLC(θ(t)) = (x(t), ˙ x(t)) θ(t) = (ω0t +θ0) mod 2π Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 10 / 19 Amirhossein

Solution: Phase reduction

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Limit cycle solution: XLC(θ(t)) = (x(t), ˙ x(t)) θ(t) = (ω0t +θ0) mod 2π Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 10 / 19 Amirhossein

Solution: Phase reduction

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = ˜ h(x, ˙ x)dt + dWt W(t) : Wiener process Limit cycle solution: XLC(θ(t)) = (x(t), ˙ x(t)) θ(t) = (ω0t +θ0) mod 2π Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 10 / 19 Amirhossein

Solution: Phase reduction

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = ˜ A(x,u)˙ x = A(θ,u) Periodic control input: τ(t) = τ0 sin(ω0t) Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Limit cycle solution: XLC(θ(t)) = (x(t), ˙ x(t)) θ(t) = (ω0t +θ0) mod 2π Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 10 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) ⇒ ψ(T)−ψ(0) =

T

0 A(θ(t),u(t))dt

Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 11 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) ⇒ ψ(T)−ψ(0) =

T

0 A(θ(t),u(t))dt

Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 11 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) ⇒ ψ(T)−ψ(0) =

T

0 A(θ(t),u(t))dt

Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• =

min

u[0,T]

E T

• A(θ(t),u(t))+ 1

2ε u(t)2

• dt
• Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 11 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) ⇒ ψ(T)−ψ(0) =

T

0 A(θ(t),u(t))dt

Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• min

u[0,T]

E T

• A(θ(t),u(t))+ 1

2ε u(t)2

• dt
• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 11 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) ⇒ ψ(T)−ψ(0) =

T

0 A(θ(t),u(t))dt

Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: Turning the head, min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• min

u[0,T]

E T

• A(θ(t),u(t))+ 1

2ε u(t)2

• dt
• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• Construct filter to evaluate the average

Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 11 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

FPF: dθ i

t = ωi dt + dBi(t) + K(θ i,t)◦

• dZt − 1

2(h(θ i t )+ ˆ

ht)dt

• i = 1,...N,

mod 2π

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

FPF: dθ i

t = ωi dt + dBi(t) + K(θ i,t)◦

• dZt − 1

2(h(θ i t )+ ˆ

ht)dt

• i = 1,...N,

mod 2π ith oscillator’s frequency

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

FPF: dθ i

t = ωi dt + dBi(t) + K(θ i,t)◦

• dZt − 1

2(h(θ i t )+ ˆ

ht)dt

• i = 1,...N,

mod 2π ith oscillator’s noise

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

FPF: dθ i

t = ωi dt + dBi(t) + K(θ i,t)◦

• dZt − 1

2(h(θ i t )+ ˆ

ht)dt

• i = 1,...N,

mod 2π ith oscillator’s control

Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Sloution: Coupled Oscillator Feedback Particle Filter

Signal: dθt = ω0 dt + dBt, mod 2π Observations: dZt = h(θt)dt + dWt

Problem: Estimate the phase θt from noisy observations.

FPF: dθ i

t = ωi dt + dBi(t) + K(θ i,t)◦

• dZt − 1

2(h(θ i t )+ ˆ

ht)dt

• i = 1,...N,

mod 2π

hidden Yang, Mehta, Meyn. Feedback Particle Filter. IEEE Trans. Automatic Control (Oct 2013). Laugesen, Mehta, Meyn, Raginsky. Poisson’s Equation in Nonlinear Filtering. SIAM J. Opt. Control (2014).

Control of Locomtory Gaits Amirhossein Taghvaei 12 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 13 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• ≈

−ε 1 N

N

∑

i=1

∂A ∂u (θ i(t),u∗(t)) Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 13 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• ≈

−ε 1 N

N

∑

i=1

∂A ∂u (θ i(t),u∗(t)) = −ε 1 N

N

∑

i=1

∂A ∂u (θ i(t),0)+O(ε2) Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 13 / 19 Amirhossein

Solution: Optimal Control

Dynamics: M(x)¨ x = C(x)˙ x2 +B(x)(τ(t)−κx−b˙ x) ˙ ψ = A(θ,u) Measurement: dZt = h(θ)dt + dWt W(t) : Wiener process Objective: min

u[0,T]

E

• (ψ(T)−ψ(0))+ 1

2ε

T

0 u(t)2dt

• ⇒

u∗(t) = −εE ∂A ∂u (θ(t),u∗(t)) | Zt

• ≈

−ε 1 N

N

∑

i=1

∂A ∂u (θ i(t),u∗(t)) = −ε 1 N

N

∑

i=1

∂A ∂u (θ i(t),0)+O(ε2) ≈ −ε 1 N

N

∑

i=1

φ(θ i) Head

Tail

Control of Locomtory Gaits Amirhossein Taghvaei 13 / 19 Amirhossein

2-body system, Simulation Result

Particles True Phase t −π

3 π 3

x x(t) Observation: y(t)

10 20 30 40 50 60 70 80

t

−1.0 −0.8 −0.6 −0.4 −0.2 0.0 0.2

q1

• pen loop: q1(t)

close loop: q1(t)

↑ → → ↓

[Click to play the movie]

Control of Locomtory Gaits Amirhossein Taghvaei 14 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

In Summary

Shape dynamics Group dynamics

Periodic input

FPF

Control law

Periodic input Noisy sensory measurements of the shape Coupled oscillator Feedback Particle Filter to estimate the shape Optimal control law based on oscillators Maneuver around a nominal gait

Control of Locomtory Gaits Amirhossein Taghvaei 15 / 19 Amirhossein

Thank You

Questions?

Control of Locomtory Gaits Amirhossein Taghvaei 16 / 19 Amirhossein

Rectification, Geometric Phase

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Rectification, Geometric Phase

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Rectification, Geometric Phase

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Rectification, Geometric Phase

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Rectification, Geometric Phase

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Rectification, Geometric Phase Geometric Phase: Net change in group variable over one cycle

• R. W .Brockett. Patterngenerationandthecontrolofnonlinearsystems. 2004
• P. S. Krishnaprasad. Geometric phases, and optimal reconfiguration for multibody systems, 1994

Control of Locomtory Gaits Amirhossein Taghvaei 17 / 19 Amirhossein

Numerical Result, Estimation

Shape dynamics Group dynamics

Periodic input

FPF

T 2T 3T t2 t1 t

π 2

π

3π 2

2π θ ±2σ θ(t) θi(t) 0.1 ρ(θ)

π 2

π

3π 2

θ ρ(θ, t2) ρ(θ, t1) Control of Locomtory Gaits Amirhossein Taghvaei 18 / 19 Amirhossein

Numerical Result, Close loop

Shape dynamics Group dynamics

Periodic input

FPF

Control law

2T 4T 6T 8T t

−0.1 0.1

u Control Input: u(t) 2T 4T 6T 8T t −1.0 −0.5 0.0 0.5 q1 Close loop q1(t) Open loop q1(t) Control of Locomtory Gaits Amirhossein Taghvaei 19 / 19 Amirhossein