ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 3 MOTION MODELING. Prof. Steven Waslander

Transcription

1 ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 3 MOTION MODELING Prof. Steven Waslander

2 COMPONENTS Mission Planning Mission Mapping Mission Autonomy Path Planning Mapping Environmental Autonomy Control Estimation Vehicle Autonomy Actuators Vehicle Sensors Hardware 2

3 OUTLINE Example Videos Wheeled, Legged, Aerial, Aquatic Motion Modeling Definitions Kinematics and Dynamics Standard models and disturbances 3

4 SWEDISH WHEELS IN ACTION 4

5 LEGGED ROBOTS IN ACTION 5

6 LEGGED ROBOTS IN ACTION 6

7 AERIAL ROBOTS IN ACTION 7

8 AERIAL ROBOTS IN ACTION 8

9 AERIAL ROBOTS IN ACTION 9

10 AQUATIC ROBOTS IN ACTION 10

11 AQUATIC ROBOTS IN ACTION 11

12 AQUATIC ROBOTS IN ACTION 12

13 OUTLINE Example Videos Wheeled, Legged, Aerial, Aquatic Motion Modeling Definitions Kinematics and Dynamics Standard models and disturbances 13

14 MOTION MODELING A motion model seeks to describe how system motion can occur Given inputs (T), what will the system do (Ɵ)? Define a set of constraints between states and inputs Define unknown disturbances as distributions (ɛ) l T 2 ml mgl sin T mg sin mg 14

15 STATE Recall: The state of a system is a vector of system variables that entirely defines the system at a specific instance in time. The state may include variables describing only the vehicle, or the vehicle and it s environment Common Vehicle States: Position, Velocity, Attitude, Attitude Rates, Motor Speeds, Battery/Fuel Level Common Environment States: Feature Locations, Surface Polygons and Normals, Wind Conditions, Ocean Currents 15

16 COMPLETE STATE Definition: A state vector is complete if it is the best predictor of the future. Known as the Markov Assumption Ensures that past and future states are independent if the current state is known Very useful for estimation and control No need to store excessive amounts of data, only the current state Must balance size of model (number of states) with violations of Markov assumption Ignoring the state of wind on an aircraft leads to significant errors in velocity control However, integral control can accommodate 16

17 INPUTS Definition: The inputs of a system are the set of variables that drive the system that can be controlled. The same notion as in classical and state space controls Common vehicle inputs: Motor throttle, voltage, servo pwm command, steering angle, elevator angle Referred to as control actions in Thrun et al. 17

18 DISTURBANCES Definition: The disturbances of a system are the set of variables that drive the system that cannot be controlled. Disturbances are why we need a control system (and stability), cause uncertainty in state The better the model of disturbances, the better their effect can be rejected. Most common model used is additive Gaussian Often then augmented with linear, nonlinear mapping 18

19 MOTION MODEL The discrete time motion model is defined as x f ( x, u, ) t t1 t t x t is the state vector at time t u t is the input vector at time t Ɛ t is the disturbance to the system at time t e.g. ~ N(0, R ) f(x t-1,u t, Ɛ t ) is the motion model t Can be linear, nonlinear, discontinuous, hybrid t 19

20 MOTION MODEL The Markov assumption implies the following dependencies The motion model captures a single transition from one time period to the next Motion Model x 0 x 1 x t-1 x t u 1 u t-1 u t 20

21 MOTION MODEL TYPES Linear models with additive Gaussian disturbances Linear models satisfy superposition Can always be written in standard form x Ax Bu t t1 t t Nonlinear models with additive Gaussian disturbances x f ( x, u ) t t1 t t Nonlinear models with nonlinear disturbances x f ( x, u, ) t t1 t t 21

22 MOTION MODEL TYPES Motion models can be viewed in other ways Probabilistically, the probability of ending in state x t given input u t and prior state x t-1 px ( x, u) t t1 t Continuous time, the rate of change of the states is governed by a nonlinear function of state, input and disturbances x f ( xu,, ) 22

23 MOTION MODELS Degrees of freedom vs. states Degrees of freedom are axes of independent motion, whereas states may also include derivatives and other terms. A quadrotor has 6 degrees of freedom (X, Y, Z, roll, pitch, yaw) but at least 12 states. Constraints on the motion Holonomic vs. nonholonomic constraints Holonomic constraints depend only on the position of the vehicle On the states that define the degrees of freedom Nonholonomic constraints also depend on velocity (or the derivatives of the position) On all states, on how a vehicle moves 23

24 KINEMATIC AND DYNAMIC MODELS At low speeds, it is often sufficient to look only at kinematic models of vehicles Two wheeled robot Bicycle model However, when forces vary with the state, more precise modeling can be beneficial Dynamic modeling of cars for cruise control Quadrotor dynamics In this course, models will mostly be supplied, assume you already know how to define them These notes now cover the basic models we use 24

25 MOTION MODELS Five basic models to be used throughout course Linear dynamic model AUV Simple 2D Nonholonomic model Two-wheel robot, speed and rotation rate inputs 2D Nonholonomic model Two-wheel robot, left and right wheel speed inputs Bicycle model Two wheel model with speed and steering angle Valid for four wheel cars as well Quadrotor dynamic model Example of 6DOF model with four thrust inputs 25

26 LINEAR DYNAMIC MODEL Linear Example Simple AUV 3D Linear motion model for three thruster AUV (attitude held constant) State pn v n pe x v e pd v d Input Continuous dynamics for x () t v () t n n T u T T n e d mv () t bv () t T () t n n n 26

27 LINEAR DYNAMIC MODEL Example Linear AUV Should always perform discretization through zeroorder hold, first-order hold, Tustins Simple alternative (for exams, proof of concept): Approximating left hand side derivatives with finite differences, holding right hand side at previous values x nt, nt, 1 Solving for current state v x dt v v nt, 1 nt, nt, 1 m bvnt, 1 Tnt, dt x x v dt nt, nt, 1 nt, 1 nt, vnt, vnt, 1 vnt, 1dt dt b T m m 27

28 LINEAR DYNAMIC MODEL Example Linear AUV Discrete Dynamics 1 dt bdt n / m dt / m dt x x u bdt m dt bdt d / m 0 0 dt / m t t1 t e / dt / m 0 Comes from approximation to matrix exponential At e I Adt More accurate approaches (which rely on actually calculating matrix exponential) can be used by taking advantage of built-in Matlab tools. 28

29 LINEAR DYNAMIC MODEL Example Linear AUV Discrete Dynamics from zero order hold of continuous model (only N,E directions for plotting purposes) xt xt 1 ut t Comparison to above solution xt xt 1 ut t

30 LINEAR DYNAMIC MODEL Example Linear AUV in X,Y plane t 10 dt 0.1 sin( t) u cos( t) 30

31 TWO-WHEELED KINEMATIC MODEL Two-wheeled robot Vehicle State, Inputs: Y I x1 x x y 2 x 3 u1 v u 2 y Motion Model O x X I x1, t x1, t1 u1, tcos x3, t1dt x gx (, u) x u sinx dt 2, t t1 t 2, t1 1, t 3, t1 x3, t x3, t1 u2, tdt 31

32 TWO-WHEELED KINEMATIC MODEL Two-wheeled robot with additive Gaussian disturbances Disturbance model ~ N(0, R ) t R t diagonal Independent disturbances t Y I y Motion Model O x x1, t x1, t1 u1, tcos x3, t1dt x 2, t g( xt 1, ut, t) x2, t1 u1, tsinx3, t1dt t x x u dt 3, t 3, t1 2, t X I 32

33 TWO-WHEELED KINEMATIC MODEL Example: Two-wheeled robot with additive Gaussian disturbances R

34 KINEMATIC MODEL Two-wheeled robot with nonlinear disturbances Speed, heading affected Y I v t t v ~ N(0, R ) ~ N(0, R ) t t y Motion Model O x v cos x 1, t 1 1, t t 3, t 1 t 1, t x u x dt v x 2, t gx ( t1, ut, t) x2, t1 u1, t t sin x3, t1 t dt x 3, t x3, t1 t u2, tdt X I 34

35 KINEMATIC MODEL Example: Two-wheeled robot with nonlinear disturbances (accurate steering and velocity control) R

36 TWO-WHEELED KINEMATIC MODEL Example: Two-wheeled robot with nonlinear disturbances (poor steering) R

37 TWO-WHEELED KINEMATIC MODEL Example: Two-wheeled robot with nonlinear disturbances (poor velocity control) R

38 TWO-WHEELED KINEMATIC MODEL If the control inputs are wheel speeds, can augment the model as follows: Center: p Wheel to center: l Wheel radius: r Wheel rotation rates: w1, w2 w 2, v p r w l

39 TWO-WHEELED KINEMATIC MODEL Kinematic constraint v i rw i w i r v i v i 39

40 TWO-WHEELED KINEMATIC MODEL Velocity is the average of the two wheel velocities We can use the instantaneous centre of rotation (ICR) v 2 v v rw rw 2 2 v v v 1 40 ICR

41 TWO-WHEELED KINEMATIC MODEL Equivalent triangles give the angular rate equation v 2 2l v2 ( v2 v1) 2l v 1 rw rw 2l 1 2 ICR 41

42 TWO-WHEELED KINEMATIC MODEL So we now have a pair of equality constraints that relate the wheel rotation rates to the speed and rotation rate of the vehicle. Return to the standard 2 wheel robot, but change the inputs Vehicle State: Y I x1 x x y 2 x 3 y Inputs: ut,1 w1 u t,2 w 2 O x X I 42

43 TWO-WHEELED KINEMATIC MODEL Summarizing the kinematic model in body coordinates rw rw 1 2 v 2 rw rw 2l 1 2 () t Finally, the full dynamics of the vehicle are rw1 rw2cos cos 2 x v rw rw y vsin sin 2 rw1 rw 2 2l 1 2 vt () () t 43

44 BICYCLE KINEMATIC MODEL Bicycle model Front wheel steering Track motion of rear wheel Rear x, y dynamics same as before Front wheel Y x x v cos dt t t1 t t1 y t yt 1 vtsint 1dt xy, f x f, t x f, t1vt cos t 1t dt f y f, t yf, t1vt sin t 1t dt x xlcos, y ylsin f v f x f, y f L v f X 44

45 BICYCLE KINEMATIC MODEL For rotation, we rely on the Instantaneous Center of Rotation (ICR) again 1. v R ICR tan L R v vtan R L R v L v f 4. t t 1 v t tant dt L 45

46 BICYCLE KINEMATIC MODEL Standard bicycle model xt xt 1 vtcost 1dt y t yt 1 vtsint 1dt t vt tant t1 dt L Y xy, v x f, y f L Good model for cars, Ackermann robots Add drivetrain dynamics which affect speed control X 46

47 DYNAMIC MODELS For some robots, kinematics are insufficient to describe relationship between inputs and vehicle state Dynamics include forces and moments acting on robot in motion model Process: Draw Free Body Diagram Define equations of motion Model forces and moments acting on vehicle 47

48 DYNAMIC MODELS Example: Quadrotor helicopter Free Body Diagram T 1 Q 1 D 1 mg D b 48

49 DYNAMIC MODELS -STANDARD FORMS (EULER) Inertial frame Mixed frame Body frame p I v mv F I I R J I E R B I I B B M I I I p I v mv F I I R I E B J J M B B B B p v I v I R v I I B B mv mv F R B B B B E B J J M B B B B Common matrix form mi 0v b b mvb FB 0 J b b J b M B 49

50 DYNAMIC MODELS Equations of motion Standard 6 DOF motion model p I mv I v R I E F I B J M J B B B B I p I v I Can be used for any rigid body that translates and rotates in 3D Naturally aligns with inertial (GPS) position and body (gyro) angular rate measurement 50

51 DYNAMICS Forces acting on vehicle 4 I F ˆ ˆ ˆ ˆ I RB( Tiz Dieh) DBe mgd i1 Moments acting on vehicle 4 i ˆ ˆ ˆ B iz i ri z i ri eh i1 M ( 1) Q T( ) D( ) T 1 Q 1 D 1 mg D b 51

52 RESULTING MOTION Trajectory Control in Windy Conditions 52 Carlos Wang, University of Waterloo, 2009

53 EXTRA SLIDES 53

54 WHEEL CONFIGURATIONS Two-wheeled Bicycle Segway Three-wheeled Dolley Tricycle Big Wheel Omni-directional Four-wheeled Rear/Front/4 WD Crazy vehicles 54

55 WHEELED MOTION Kinematics governed by wheel number, type, geometry Standard 2DOF Caster 3DOF Chair Swedish 3DOF Robocup Spherical 3DOF, Impractical 55

56 WHEEL CHOICE Main Issues: Stability Maneuverability Controllability Mechanical Complexity Image courtesy of Payam Sabzmeydani Image courtesy of Segway Image courtesy of Segway Passive stability is guaranteed with 3 wheels, improved with 4 wheels Active stability required with less (bicycle, Segway) Maneuverability/Controllability/Complexity Combining steering and drive on one wheel difficult to realize but great for control (front wheel drive) 56

57 UNEVEN TERRAIN Suspension required to maintain contact, smooth sensor motion Bigger wheels effective for overcoming obstacles, but require more torque 57 Image courtesy of T. Barfoot Image courtesy of EPFL

58 LEGGED MOTION Inspired by nature, less efficient, more maneuverable than wheeled motion Number of legs determines stability Number of joints determines maneuverability 58 Image courtesy of Lego

59 LEGGED MOTION Usually requires predefined gait Sequence of motions that achieves forward mobility Dynamics quite complex, specialized Free Fly 59 Changeover Walking Galloping

60 LEGGED MOTION Once gait is defined, motion can be approximated by rolling polygon Leg angle Polygon sides Hip height Forward speed 1 d /2 sin l n h lcos h mdt d m steps per second, δt elapsed time 60 l h

61 LEGGED MOTION Red circle = hip height Leg length = step distance = 1 Steps per second =

62 AERIAL MOTION Fixed Wing Longitudinal Forces and Moments Elevator causes moment about cg Tail resists rotation about cg (damping) Total lift and weight approximately balance Drag increases with elevator deflection Wing Lift Drag Tail Lift Weight Elevator Force 62

63 AERIAL MOTION Rotary wing: Quadrotors have two pairs of counter-rotating blades allow for fixed pitch rotors and independent actuation of roll, pitch, yaw and altitude Yaw Torque Roll/Pitch Torque Total Thrust for Motion 63 63

64 AERIAL MOTION Flapping Wing Biologically inspired, difficult to achieve comparable efficiency to fixed, rotary wing Similar approach to walking Produce cyclic model of forces and moments Dr. James DeLaurier, 2006 University of Toronto Aerospace Institute 64

65 UWMAV IN ACTION 65

66 AQUATIC MOTION Ship motion, close to planar ground robot Propulsion and steering at rear Sideslip possible Augmented model may include roll, pitch Submersible motion, analogous to fixed wing aircraft Buoancy replaces lift to counteract gravity Swimming motion Complex cyclic behaviour, can be modeled in a similar manner to walking, flapping 66

67 AQUATIC ROBOTS IN ACTION World Autonomous Sailing Competition

68 SWEDISH WHEEL ROBOT x v 1 1, y 120 v 3 l 2 3 p r v 2 68