Flying and Swimming Robots!

Transcription

1 Flying and Swimming Robots! Robert Stengel! Robotics and Intelligent Systems MAE 345, Princeton University, 2017! Aircraft! Aquatic robots! Space robots! Quaternions! Simulink/Simscape/ SimMechanics Copyright 2017 by Robert Stengel. All rights reserved. For educational use only. 1 Bio-Inspiration for Flying Hummingbird v=d8vjytxgijwfeature=related Eagle vs. Eagle v=tufnqwnp9aafeature=video_res ponse Moth Flying watch?v=hd2bjasvibi Birds Flying watch?v=i5gbfgk- EPw Lady Bug watch?v=fjzobezjybc 2

2 Biomimetic UAVs Markus Fisher at TED v=fg_jckshutq Aerovironment Nano Hummingbird v=xolh02zba04 Festo Air Ray Dirigible v=uxpzodkqays Harvard Robo-Flies v=2lqckr0a_7c 3 Uninhabited Air Vehicles (UAV) 4

3 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, ~

4 Uninhabited Aircraft Sikorsky Cypher II UAV MQ-9 Reaper Aggressive Quadrotor UAV Maneuvers 7 Multi-Copters Adam Yabroudi, 15, Dual-Quad Submersible 8

5 18 rotors 30-min flying time Autonomous Air Taxi! Volocopter 9 Space Robots! 1 0

6 !!!!!! X-37B Reusable experimental/ operational vehicle Unmanned mini- Space Shuttle Orbital maneuvering Highly classified project 1 st 4 missions: 224, 469, 675, 717 days in orbit 5 th mission ongoing 11 Delta 4 Expendable (Rocket) Launch Vehicles Current space launch vehicles are largely autonomous Atlas V v=kxqbex7ljwg 1 2

7 Reusable Launch/Reentry Vehicles! Falcon 9/Dragon 1 3 Uninhabited Spacecraft Giotto CubeSats Mariner 4, with Solar Control Vanes GPS III Satellite 1 4

8 Uninhabited Spacecraft 1 5 Manned Re-Entry Vehicles Largely Autonomous 1 6

9 Undersea Robots! 1 7 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 1 8

10 Swimming! Lift, drag, and vorticity! Schooling behavior Human Swimming v=cizbasiwdra Fish Swimming v=u_vj_0worbm 1 9 Autonomous Underwater Vehicles RoboTuna (Olin/MIT) RoboLobster (Northeastern) watch?v=pditxrxeyna 2 0

11 Autonomous Submarines Autonomous Benthic Explorer VPI concept Oberon (U Sydney) AQUA AQUA encounters a lobster Autonomous Underwater Gliders! Slocum Glider! Variable ballast for climb/dive

12 Avoiding the Euler Angle Singularity! 2 3 Inverse Transformation for Euler-Angle Rates B ( L I )!1 = % 1 0!sin" ) 0 cos sin cos" ) 0! sin cos cos" ) (!1 = % 1 sin! tan" cos! tan" ) 0 cos! sin! ) 0 sin! sec" cos! sec" ) ( Euler-angle rates from body-axis rates %!!"! ) ) ) = ( % 1 sin! tan" cos! tan" 0 cos! *sin! 0 sin! sec" cos! sec" ) ) ) ( % p q r ) ) ) ( = L I B + B 2 4

13 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 B I t dt = %! B ( t)h B I t = %! " 0 %r( t) q( t) 0 % p( t) p( t) 0( t) r t %q t ( ( H I ( ( B B ( t) Initialize with Euler Angles H B I ( 0) = H B I (! 0," 0, 0 ) 2 5 Avoiding the Euler Angle Singularity at! = ±90 Propagation of quaternion vector: single rotation from inertial to body frame (4 parameters)!! 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: a 1, a 2, and a 3 )!! Magnitude of the rotation angle,!, rad 2 6

14 Begin with Euler Rotation of a Vector Rotation about axis, a, of a vector, r I, to a new orientation, r B a : Direction cosines of rotation axis a = 1 [a is a unit vector]!: Rotation angle 2 7 Development of Theorem Defined vector is given a different orientation r B = r I r B = H I B r I Transformation involves addition of 3 vectors ( a T r I )a Along axis of rotation " r I! ( a T r I )a % cos! to a and through r I sin r I a! to a and r I Scaled by rotation angle,!, to produce r B 2 8

15 Development of Theorem r B = H B I r I = ( a T r I )a + " r I! ( a T r I )a % cos + sin r a I = cos r I + ( 1! cos )( a T r I )a! sin ( a r I ) Combine terms Reverse cross-product order 2 9 Rotation Matrix Derived from Euler s Formula r B = H B I r I = cos! r I + ( 1" cos! ) a T r I Identity ( a T r I )a = ( aa T )r I Cancel like terms aa T B!" H I r I =! " cos% + 1 cos% Rotation matrix a " sin!!ar I sin%!a H B I = cos! I 3 + ( 1" cos! )aa T " sin!!a r I 3 0

16 Quaternion Derived from Euler Rotation Angle and Orientation Quaternion vector 4 parameters based on Euler s rotation formula! q = " q 1 q 2 q 3 q 4!! " % q 3 q 4! % = sin ( 2)a cos ( 2) "! ( * = sin ( 2) * * ) % cos 2 " a 1 a 2 a , % ( 4!1) 4-parameter representation of 3 parameters; hence, a constraint must be satisfied = sin 2! 2 q T q = q q q q cos 2! 2 a a a 3 2 = sin 2! 2 + cos 2 (! 2) = " Rotation Matrix Expressed with Quaternion Euler s formula expressed with quaternion H B I = " q 2 4! q T 3 q 3 q 1 2! q 2 2! q q 4 2 % I 3 + 2q 3 q T 3! 2q 4!q 3 Terms of rotation matrix from quaternion elements H I B = 2( q 1 q 2 + q 3 q 4 ) 2( q 1 q 3! q 2 q 4 )!q q 2 2! q q 4 2( q 2 q 3 + q 1 q 4 ) 2( q 2 q 3! q 1 q 4 )!q 2 1! q q q 4 2 q 1 q 2! q 3 q 4 2 q 1 q 3 + q 2 q %

17 Initial Quaternion Expressed from Elements of Rotation Matrix Initialize q(0) from Direction Cosine Matrix or Euler Angles H B I ( 0) = " h 11 ( = cos! 11 ) h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 % Assuming that q 4! 0 = H B I (( 0,) 0,* 0 ) q 4 ( 0) = h 11( 0) + h 22 ( 0) + h 33 ( 0) q 3 ( 0)!! " q 1 0 q 2 0 q 3 0 = % 1 4q 4 0! " h 32 ( 0) h 13 ( 0) h 21 ( 0)!" h 23 0 %!" h 31 0 %!" h 12 0 % % 3 3 Quaternion Vector Kinematics dq( t) = dt! "!q = d! dt "!q 1 t!q 2 t!q 3 t!q 4 t q 3 q 4! = 1 r t 2 q t % p t " % = 1! 2 " q 4 B ( " B q 3 ( B T q 3 (4 x 4) skew-symmetric matrix 0 r( t) q( t) p( t) 0 p( t) q( t) p( t) 0 r( t) q( t) r( t) 0 B %! % " Digital integration to compute q(t k ) q int ( t k ) = q( t k!1 ) + t k t k!1 dq (" ) d" dt 4!1 q 1 q 2 q 3 q 4 ( t) ( t) ( t) ( t) % 3 4

18 Rigid Body Equations of Motion Using Quaternion I!r I ( t) = H B!" q( t) v B ( t)!v B ( t) = 1 m f ( t) % B " B ( t)v B ( t)!q ( t) = 1 2! " 0 r( t) %q( t) p( t) 0 p( t) q( t) % p( t) 0 r( t) %q( t) %r( t) 0 %r t q t % p t %1! B ( t) = I B!" m B ( t) % " B ( t)i B B ( t) ( ( ( ( ( ( q( t) Euler Angles Derived from Quaternion! atan2: generalized arctangent algorithm, 2 arguments! returns angle in proper quadrant! avoids dividing by zero! has various definitions, e.g., (MATLAB) atan2( y, x) = % *,,, +,,, -, y tan!1 " % x if x > 0 y ( + tan!1 " % y x,! ( + tan!1 " % x if x < 0 and y ) 0, < 0 ( 2,!( 2 if x = 0 and y > 0, < 0 0 if x = 0 and y = 0 { }! atan2 2( q 1 q 4 + q 2 q 3 ), 1* 2 q 2 2 % 1 + q 2 ( ) " ) = sin *1 % 2( q 2 q 4 * q 1 q 3 ) ( ) ( atan2 2( q 3 q 4 + q 1 q 2 ), 1* 2 q 2 2 % ( 2 + q 3 ) % ( { } ) ) ) ) ) ( 3 6

19 Solving Math Problems Computationally! Task : Calculate x 1 ( t) and x 2 ( t)for t = 1 to 10 sec! " x 2 ( t)!x 1 t!!x 2 t = x 2 t x % 1 t " with initial conditions! " x 1 0 x 2 0 =! 0 10 % " % % 3 7 MATLAB Models of Dynamic Systems Systems are described by instructions Main Script % Linear 2 nd -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 function xdot = Lin(t,x) % Linear Ordinary Differential Equation % x(1) = Position % x(2) = Rate xdot = [x(2) -x(1) - x(2)]; 3 8

20 MATLAB Initial-Condition Output 3 9 Simulink Models of Dynamic Systems Systems are described by block diagrams d 2 x(t) dt 2 =!!x(t) =!x(t)!!x(t) + u( t)!!x ( t)!x ( t) x( t) 4 0

21 Accessing Simulink from MATLAB 4 1 Accessing Simulink from MATLAB 4 2

22 Open Simulink Blank Model 4 3 Open Simulink Library Browser for Function Blocks 4 4

23 Simulink Step Response 4 5 Alternative Simulink Models of 2 nd -Order Systems d 2 x(t) dt 2 Single 2 nd -order model, with step input and damping =!!x(t) =!x(t)!!x(t) + u( t) State-space model (two 1 st -order equations), with step input and damping!x 1 (t) = ( 0)x 1 (t) + ( 1)x 2 (t)!x 2 (t) =!( 1)x 1 (t)! Kx 2 (t) + u t 4 6

24 Simulink Autocoding Pires, NASA Ames! Graphic modeling of dynamic systems! Library of functions! Generation of C and C++ code 4 7 SimMechanics is Mechanical Subset of SimScape Library 4 8

25 SimMechanics Library SimMechanics Library

26 Conveyer-Loader Demonstration 5 1 Conveyer-Loader Demonstration Controller specified within box 5 2

27 Position Controller for Conveyor- Loader Demonstration (Simulink) 5 3 Simulink Demonstration of 1-Inch Robot (MAE 345 Mid-Term Project, 2009) MAE345Lecture2Demo2013.mdl 5 4

28 Next Time:! Articulated Robots! 5 5 Supplemental Material! 5 6

29 Simulink Library of blocks, sources, and sinks 5 7 Simulink Blocks 5 8

30 SimMechanics Called from Simulink 5 9 Simple Pendulum Specifying Body Coordinate System 6 0

31 Simple Pendulum with Scope and XY Graph Phase-Plane Plot (Rate vs. Displacement)

32 SimScape Mechanism Models 6 3 SimMechanics Body Actuator 6 4

33 SimMechanics Body Sensor 6 5 SimMechanics, Simulink 3D Animation Product Help Demos Robotic Manipulator Vehicle Dynamics 6 6