flying and swimming robots - princeton universitystengel/mae345lecture4.pdf · uninhabited...

Flying and Swimming Robots!Robert Stengel!

Robotics and Intelligent Systems MAE 345, Princeton University, 2017

Copyright 2017 by Robert Stengel. All rights reserved. For educational use only.http://www.princeton.edu/~stengel/MAE345.html

•! Aircraft•! Aquatic robots•! Space robots•! Quaternions•! Simulink/Simscape/

SimMechanics

1

Bio-Inspiration for Flying

2

Hummingbirdhttp://www.youtube.com/watch?v=D8vjYTXgIJw&feature=related Eagle vs. Eagle

http://www.youtube.com/watch?v=tufnqWNP9AA&feature=video_res

ponse

Birds Flyinghttp://

www.youtube.com/watch?v=I5GbFgk-

EPw

Lady Bughttp://www.youtube.com/watch?v=fjZobEZJYBc

Moth Flyinghttps://

www.youtube.com/watch?v=hD2BjAsvIbI

Biomimetic UAVs

Festo Air Ray Dirigiblehttp://www.youtube.com/watch?

v=UxPzodKQays

Harvard Robo-Flieshttp://www.youtube.com/watch?

v=2lQcKr0A_7c

3

Aerovironment Nano Hummingbirdhttps://www.youtube.com/watch?

v=xolH02Zba04

Markus Fisher at TEDhttp://www.youtube.com/watch?

v=Fg_JcKSHUtQ

Uninhabited Air Vehicles (UAV)

4

Uninhabited Aircraft!Tad McGeer, ‘79

Aerosonde First UAV Transatlantic Crossing,

1998

Boeing (InSitu) ScanEagle

Aerovel Flexrotor

5

Mars Aerial Regional-Scale Environmental Survey (ARES)

Research Airplane Concept, ~2008

6 https://www.youtube.com/watch?v=wAOTOmGFs5M

https://www.youtube.com/watch?v=8YutbpJuFiI

Uninhabited Aircraft

MQ-9 Reaperhttp://www.youtube.com/watch?v=kSpOYZR0klA

Aggressive Quadrotor UAV Maneuvershttp://www.youtube.com/watch?v=MvRTALJp8DM

Sikorsky Cypher II UAV

7

Multi-Copters

8

Adam Yabroudi, ‘15, Dual-Quad Submersible

Autonomous Air Taxi!Volocopter

9

18 rotors 30-min flying time

Space Robots!

10

•! Reusable experimental/operational vehicle

•! Unmanned “mini-Space Shuttle”

•! Orbital maneuvering

•! Highly classified project

•! 1st 4 missions: 224, 469, 675, & 717 days in orbit

•! 5th mission on-going

X-37B

11

Expendable (Rocket) Launch Vehicles

Current space launch vehicles are largely autonomous

Atlas V http://www.youtube.com/watch?

v=KxQbex7LJwg

Delta 4

12

Reusable Launch/Reentry Vehicles!Falcon 9/Dragon

13

Uninhabited Spacecraft

Mariner 4, with Solar Control Vanes

GiottoCubeSats

GPS III Satellite

14

Uninhabited Spacecraft

15

Manned Re-Entry Vehicles Largely Autonomous

16

Undersea Robots!

17

Swimming Gaits

Anguilliform locomotion Long, slender fish, e.g., lamprey Amplitude of flexion wave along body ~ constant Sub-carangiform locomotion Increase in wave amplitude along the body Most work done by rear half of fish body Higher speed, reduced maneuverability Carangiform locomotion Stiffer and faster-moving, e.g., trout Majority of movement rear of body and tail Rapidly oscillating tails Thunniform locomotion High-speed long-distance swimmers, e.g. tuna, shark Virtually all lateral movement in the tail Tail itself is large and crescent-shaped

18

Swimming

Human Swimming http://www.youtube.com/watch?

v=CIzBaSiWdRA

Fish Swimming http://www.youtube.com/watch?

v=U_VJ_0wORbM

•! Lift, drag, and vorticity•! Schooling behavior

19

Autonomous Underwater Vehicles

RoboTuna(Olin/MIT)

RoboLobster(Northeastern)

20

https://www.youtube.com/watch?v=pDitxrXeYnA

Autonomous SubmarinesAutonomous Benthic

Explorer VPI concept Oberon (U Sydney)

AQUAhttp://www.youtube.com/watch?v=9Vm-gQ9_H9I&feature=related

21 AQUA encounters a lobster

https://www.youtube.com/watch?v=FCOZFwzMiU8

Autonomous Underwater Gliders

22 https://en.wikipedia.org/wiki/Underwater_glider

•! Slocum Glider–! Variable ballast for climb/dive

http://www.youtube.com/watch?v=aRyEDzaogPc

Avoiding the Euler Angle Singularity!

23

Inverse Transformation for Euler-Angle Rates

!!!"!#

$

%

&&&

'

(

)))=

1 sin! tan" cos! tan"0 cos! * sin!0 sin! sec" cos! sec"

$

%

&&&

'

(

)))

pqr

$

%

&&&

'

(

)))= LB

I ++ B

LIB( )!1 =

1 0 !sin"0 cos# sin# cos"0 !sin# cos# cos"

$

%

&&&

'

(

)))

!1

24

=1 sin! tan" cos! tan"0 cos! #sin!0 sin! sec" cos! sec"

$

%

&&&

'

(

)))

Euler-angle rates from body-axis rates

Avoiding the Euler Angle Singularity at ! = ±90°

Alternatives to Euler angles 1) Direction cosine (rotation) matrix

2) Quaternions Propagation of direction cosine matrix (9 parameters)

d H IB t( )!" #$dt

= % !&& B t( )H IB t( ) = %

0 %r t( ) q t( )r t( ) 0 % p t( )%q t( ) p t( ) 0 t( )

!

"

''''

#

$

((((B

H IB t( )

25 H I

B 0( ) = H IB !0,"0,# 0( )

Initialize with Euler Angles

Avoiding the Euler Angle Singularity at ! = ±90°

Propagation of quaternion vector: single rotation from inertial to body frame (4 parameters)

26

!! Rotation from one axis system, I, to another, B, represented by !! Orientation of axis vector

about which the rotation occurs (3 parameters of a unit vector, a: a1, a2, and a3)

!! Magnitude of the rotation angle, !, rad

27

Begin with Euler Rotation of a Vector

a : Direction cosines of rotation axisa = 1 [a is a unit vector]!: Rotation angle

Rotation about axis, a, of a vector, rI, to a new orientation, rB

28

Development of Theorem

aTrI( )arI ! a

TrI( )a"# $%cos&

sin& rI ' a( )

Transformation involves addition of 3 vectors

rB = rI

Defined vector is given a different orientation

rB = H IBrI

Along axis of rotation! to a and through rI! to a and rI

Scaled by rotation angle, !, to produce rB

29

Development of TheoremrB = H I

BrI= aTrI( )a + rI ! a

TrI( )a"# $%cos& + sin& rI ' a( )= cos& rI + 1! cos&( ) aTrI( )a ! sin& a ' rI( )

Combine termsReverse cross-product order

30

Rotation Matrix Derived from Euler’s Formula

rB = H I

BrI = cos! rI + 1" cos!( ) aTrI( )a " sin! !arI( )

aTrI( )a = aaT( )rI

H IB = cos! I3 + 1" cos!( )aaT " sin! !a

Identity

Rotation matrix H I

B!" #$rI = cos% + 1& cos%( ) aaT( )& sin% !a!" #$rI

Cancel like terms

Quaternion Derived from Euler Rotation Angle and Orientation

q =

q1q2q3q4

!

"

#####

$

%

&&&&&

!q3q4

!

"##

$

%&&=

sin ' 2( )acos ' 2( )

!

"

##

$

%

&&=

sin ' 2( )a1a2a3

(

)

***

+

,

---

cos ' 2( )

!

"

######

$

%

&&&&&&

4-parameter representation of 3 parameters; hence, a constraint must be satisfied

31

qT q = q12 + q2

2 + q32 + q4

2

= sin2 ! 2( ) a12 + a22 + a32( ) + cos2 ! 2( )= sin2 ! 2( ) + cos2 ! 2( ) = 1

Quaternion vector 4 parameters based on Euler’s rotation formula

4 !1( )

Rotation Matrix Expressed with Quaternion

32

H I

B = q42 ! q3

T q3( )"# $%I3 + 2q3q3T ! 2q4 !q3

H IB =

q12 ! q2

2 ! q32 + q4

2 2 q1q2 + q3q4( ) 2 q1q3 ! q2q4( )2 q1q2 ! q3q4( ) !q1

2 + q22 ! q3

2 + q42 2 q2q3 + q1q4( )

2 q1q3 + q2q4( ) 2 q2q3 ! q1q4( ) !q12 ! q2

2 + q32 + q4

2

"

#

$$$$

%

&

''''

Euler’s formula expressed with quaternion

Terms of rotation matrix from quaternion elements

Initial Quaternion Expressed from Elements of Rotation Matrix

33

Assuming that q4 ! 0

H IB 0( ) =

h11 =cos!11( ) h12 h13h21 h22 h23h31 h32 h33

"

#

$$$

%

&

'''= H I

B (0,)0,* 0( )

Initialize q(0) from Direction Cosine Matrix or Euler Angles

q4 0( ) = 121+ h11 0( ) + h22 0( ) + h33 0( )

q3 0( ) !q1 0( )q2 0( )q3 0( )

!

"

####

$

%

&&&&

= 14q4 0( )

h23 0( )' h32 0( )!" $%h31 0( )' h13 0( )!" $%h12 0( )' h21 0( )!" $%

!

"

####

$

%

&&&&

Quaternion Vector Kinematics

34

!q = ddt

q3q4

!

"##

$

%&&= 12

q4'' B ( "'' Bq3('' B

Tq3

!

"##

$

%&& 4 !1( )

dq t( )dt

=

!q1 t( )!q2 t( )!q3 t( )!q4 t( )

!

"

######

$

%

&&&&&&

= 12

0 r t( ) 'q t( ) p t( )'r t( ) 0 p t( ) q t( )q t( ) ' p t( ) 0 r t( )' p t( ) 'q t( ) 'r t( ) 0

!

"

######

$

%

&&&&&&B

q1 t( )q2 t( )q3 t( )q4 t( )

!

"

######

$

%

&&&&&&

(4 x 4) skew-symmetric matrix

qint tk( ) = q tk!1( ) + dq "( )dt

d"tk!1

tk

#

Digital integration to compute q(tk)

Rigid Body Equations of Motion Using Quaternion

!rI t( ) = HBI q t( )!" #$vB t( )

!vB t( ) = 1mfB t( )% "&& B t( )vB t( )

!q t( ) = 12

0 r t( ) %q t( ) p t( )%r t( ) 0 p t( ) q t( )q t( ) % p t( ) 0 r t( )% p t( ) %q t( ) %r t( ) 0

!

"

''''''

#

$

((((((

q t( )

!&& B t( ) = IB%1 mB t( )% "&& B t( )IB&& B t( )!" #$

35 http://www.princeton.edu/~stengel/FDcodeB.html

Euler Angles Derived from Quaternion

36

!"#

$

%

&&&

'

(

)))=

atan2 2 q1q4 + q2q3( ), 1* 2 q12 + q2

2( )$% '({ }sin*1 2 q2q4 * q1q3( )$% '(

atan2 2 q3q4 + q1q2( ), 1* 2 q22 + q3

2( )$% '({ }

$

%

&&&&&

'

(

)))))

•! atan2: generalized arctangent algorithm, 2 arguments–! returns angle in proper quadrant–! avoids dividing by zero–! has various definitions, e.g., (MATLAB)

atan2 y, x( ) =

tan!1 yx

"#$

%&' if x > 0

( + tan!1 yx

"#$

%&' , !( + tan!1 y

x"#$

%&' if x < 0 and y ) 0, < 0

( 2 , !( 2 if x = 0 and y > 0, < 00 if x = 0 and y = 0

*

+

,,,,

-

,,,,

37

Task : Calculate x1 t( ) and x2 t( )for t = 1 to 10 sec

!x1 t( )!x2 t( )

!

"##

$

%&&=

x2 t( )'x1 t( )' x2 t( )

!

"##

$

%&&

with initial conditions

x1 0( )x2 0( )

!

"##

$

%&&= 0

10!

"#

$

%&

Solving Math Problems Computationally!

MATLAB Models of Dynamic Systems

Systems are described by instructions

38

Main Script

% Linear 2nd-Order Example clear tspan = [0 10]; xo = [0, 10]; [t,x] = ode23('Lin',tspan,xo); subplot(2,1,1) plot(t,x(:,1)) ylabel('Position'), grid subplot(2,1,2) plot(t,x(:,2)) xlabel('Time'), ylabel('Rate'), grid

function xdot = Lin(t,x)% Linear Ordinary Differential Equation% x(1) = Position% x(2) = Ratexdot = [x(2) -x(1) - x(2)];

Function

MATLAB Initial-Condition Output

39

Simulink Models of Dynamic Systems

Systems are described by block diagrams

40

d 2x(t)dt 2

= !!x(t) = !x(t)! !x(t)+ u t( )

!!x t( ) !x t( ) x t( )

Accessing Simulink from MATLAB

41

Accessing Simulink from MATLAB

42

Open Simulink Blank Model

43

Open Simulink Library Browser for Function Blocks

44

Simulink Step Response

45

Alternative Simulink Models of 2nd-Order Systems

Single 2nd-order model, with step input and

damping

State-space model (two 1st-order equations), with step input and

damping

d 2x(t)dt 2

= !!x(t) = !x(t)! !x(t)+ u t( )

!x1(t) = 0( )x1(t)+ 1( )x2 (t)!x2 (t) = ! 1( )x1(t)! Kx2 (t)+ u t( )

46

Simulink Autocoding

47

Pires, NASA Ames

•! Graphic modeling of dynamic systems •! Library of functions •! Generation of C and C++ code

SimMechanics is Mechanical Subset of SimScape Library

48

SimMechanics Library - 1

49

SimMechanics Library - 2

50

Conveyer-Loader Demonstration

51

Conveyer-Loader Demonstration

52

Controller specified within box

Position Controller for Conveyor-Loader Demonstration

(Simulink)

53

Simulink Demonstration of 1-Inch Robot (MAE 345 Mid-Term Project, 2009)

MAE345Lecture2Demo2013.mdl

54

Next Time:!Articulated Robots!

55

Supplemental Material!

56

Simulink Library of blocks, sources, and sinks

57

Simulink Blocks

58

SimMechanics Called from Simulink

59

Simple Pendulum

Specifying Body Coordinate System

60

Simple Pendulum with Scope and XY Graph

Phase-Plane Plot (Rate vs. Displacement)

61

62

SimScape Mechanism Models

63

SimMechanics Body Actuator

64

SimMechanics Body Sensor

65

SimMechanics, Simulink 3D Animation ‘Product Help’ Demos

http://www.mathworks.com/products/simmechanics/demos.html

Robotic Manipulator Vehicle Dynamics

66