Kinematic and Dynamic Models

Transcription

1 Kinematic and Dynamic Models b Advanced Robotics: Navigation and Perception 3/29/ Reasoning about mobility is important. Image: RedTeamRacing.com 2

2 ecture Outline Nomenclature Kinematic Constraints and Models Dynamic Constraints and Models Applications Current and Active Research 3 Nomenclature State = r o v ω... T State Space X Configuration Space C Configuration q Action u Action Space State/Configuration Transition Equation Trajectory U() (state-space) ẋ = f(, u) (state-space) (t) = (t 0 ) (t 1 ) (t 2 )... T U(q) (configuration-space) q = f(q, u) (configuration-space) (state-space) 4

3 Actions U is the set of all possible actions over all states Eample: Moving on a 2D grid = y T U = X U() + = f(, u) = + u U = {(0, 1), (0, 1), (1, 0), ( 1, 0)} 5 Actions (cont.) Eample: Moving on a 3D grid = y z T U = X U() = f(, u) = + u (same as before, just a higher dimensional state space) U = {(0, 0, 1), (0, 0, 1), (0, 1, 0), (0, 1, 0), (1, 0, 0), ( 1, 0, 0)} + +z 6

4 Actions (cont.) In continuous spaces the net state is determined by a integrating the state transition equation Eample: One-dimensional particle =[] T ẋ = v U = {v } =[ 1, 1] U = X U() + 7 Implicit Kinematic Constraints g(q, q ) 0 Parametric q = f(q, u) Pfaffian g 1 (q) q 1 + g 2 (q) q g n (q) q n =0 8

5 Kinematic Constraints (cont.) Pfaffian g 1 (q) q 1 + g 2 (q) q g n (q) q n =0 Configuration transition equation q 1 = u 1 q 2 = u 2 q n k = u n k q n k+1 = f n k+1 (q, u) q n k+2 = f n k+2 (q, u) q n = f n (q, u) (these are determined by solving for the remaining variables) 9 Kinematic Constraints (cont.) A quick eample... C = R 3 (three dimensional state space, n=3) 7 q 1 3 q 2 + q 3 =0 (one Pfaffian constraint, k=1) (two control inputs (n - k = 2) q 1 = u 1 q 1 = u 1 q 2 = u 2 q 2 = u 2 q 3 = 7u 1 +3u 2 (substituting for configuration rates) 7u 1 3u 2 + q 3 =0 (configuration transition equation) 10

6 Simple Car Model Rigid body that moves in a twodimensional configuration space C = R 2 S 1 q = y T ẋ = f 1 (, y,,v body, φ) ẏ = f 2 (, y,,v body, φ) = f 3 (, y,,v body, φ) φ v body + ρ (, y) 11 Simple Car Model dy d Rigid body that moves in a twodimensional configuration space = tan() =sin() cos() = ẏ ẋ as t 0 ẋsin()+ẏcos() =0 C = R 2 S 1 q = y T φ v body + (rewritten as a Pfaffian constraint) ẋ = cos() =v body cos() ẏ = sin() =v body sin() ρ (, y) 12

7 Simple Car Model How are these constraints found? ẋsin()+ẏcos() =0 C = R 2 S 1 q = y T (n=2, k=1) ẋ = u 1 ẏ = u 1 tan() φ v body + (substituting for u 1) ẋ = cos() ẏ = cos() sin() cos() = sin() ρ (, y) 13 Simple Car Model Rigid body that moves in a twodimensional configuration space C = R 2 S 1 q = y T (distance traveled) (divide by dt) dω = ρd d = tan(φ) dω = v body tan(φ) φ ρ v body (, y) + 14

8 Simple Car Model With control inputs of the form u =(u v body,u φ ) the configuration transition equation is ẋ = u v body cos() ẏ = u v body sin() C = R 2 S 1 q = y T φ v body + = u v body tan(u φ ) ρ (, y) 15 Variations of the Simple Car Model Simple Car U =[ 1, 1] ( φ ma, φ ma ) (cannot turn in place) Tricycle U =[ 1, 1] ( π 2, π 2 ) (can turn in place) Reed-Shepp Car U = { 1, 0, 1} ( φ ma, φ ma ) Dubins Car U = {0, 1} ( φ ma, φ ma ) (can drive forwards and backwards) (cannot drive backwards) 16

9 Differential Drive Model With inputs of the form u =(u ωr,u ωl ) The configuration transition equation is ẋ = r 2 (u ω l + u ωr )cos() ẏ = r 2 (u ω l + u ωr )sin() = r (u ω r u ωl ) ω wheel C = R 2 S 1 q = y T r v wheel v r v l (, y) + 17 N-Trailers A variation of the simple car model involves one that is pulling N passively controlled trailers φ v body + C = R 2 S 1 S 1 S 1 q = y T 1 2 ρ (, y) 0 18

10 Dynamic Constraints Implicit g i ( q, q, q) =0 similar to the kinematic constraints Parametric q = f( q, q, u) 19 Phase Space Increase the dimension to remove higher-order derivatives a single constraint 4ÿ 5ẏ + y =0 define a phase vector =( 1, 2 ) set the values of the phase vector 1 = y 2 =ẏ creates new constraints 2 =ÿ 1 = 2 system is converted into two first--order differential equations 1 = 2 2 =

11 Improved Car Model Use a double integrator to ensure a continuously varying steering angle = y φ T u =(u v body,u ω ) ẋ = v body cos() ẏ = v body sin() φ v body + = v body tan(φ) φ = u ω ρ (, y) 21 Improved Differential Drive Model This can similarly be applied for the differential drive model = y ω l ω r T u =(u αl,u αr ) ẋ = r 2 (ω l + ω r )cos() ẏ = r 2 (ω l + ω r )sin() = r (ω r ω l ) ω wheel ω l = u αl ω r = u αr r v wheel v r v l (, y) + 22

12 Predictive Model Applications Predictive Terrain Compensation [Howard07a] Eploited a motion model that simulated the effects of terrain geometry to plan paths that considered rough terrain and wheel slip models Image: [Howard07a] Image: [Howard07a] 23 Predictive Model Applications (cont.) Autonomous automobile navigation [Ferguson08a] Modeled curvature, curvature rate, safety controller constraints Image: [Howard09a] Image: [Howard09a] 24

13 Current and Active Research Online Slip Identification [Rogers- Marcovitz10a] Etended Kalman filter to calibrate a parameterized predictive motion model in real-time Planar slip disturbances modeled as second-order functions of velocity and curvature Demonstrated on multiple platforms Image: [Rogers-Marcovitz10a] Image: [Rogers-Marcovitz10a] 25 Net ecture 2: Motion simulation (3/31) Newton-Euler Mechanics Motion of Particles Motion Simulation (inear, Euler, Runge-Kutta) Homework #1 Assigned 26

14 Document/Image References [Ferguson08a] David Ferguson, Thomas Howard, and Maim ikhachev, "Motion Planning in Urban Environments: Part I," Proceedings of the IEEE/RSJ 2008 International Conference on Intelligent Robots and Systems, September, [Howard07a] Thomas Howard and Alonzo Kelly, "Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots," International Journal of Robotics Research, Vol. 26, No. 2, March, 2007, pp [avalle 06a] S. avalle, Planning Algorithms. Cambridge: Cambridge University Press. ISBN [Rogers-Marcovitz10a] Forrest Rogers- Marcovitz and Alonzo Kelly, "On-line Mobile Robot Model Identification using Integrated Perturbative Dynamics," 12th International Symposium on Eperimental Robotics, December, [Howard 09a] T.M. Howard, Adaptive Model-Predictive Motion Planning for Navigation in Comple Environments. Ph.D. Thesis, Carnegie Mellon University, August