L ECTURE 8: K INEMATICS E QUATIONS O DOMETRY , D EAD R ECKONING I - PowerPoint PPT Presentation

16-311-Q I NTRODUCTION TO R OBOTICS L ECTURE 8: K INEMATICS E QUATIONS O DOMETRY , D EAD R ECKONING I NSTRUCTOR : G IANNI A. D I C ARO

F O R WA R D K I N E M AT I C S E Q U AT I O N S 2 3 2 3 2 3 ⇣ ⌘ ⇣ ⌘ x ( t + δ t ) � � � � x ( t ) + R ( t ) sin θ ( t ) + ωδ t − sin( θ ( t )) x ( t ) + R ( t ) sin θ ( t ) + ∆ θ ( t + δ t ) − sin( θ ( t )) W 6 7 6 7 6 7 y ( t + δ t ) = 5 = ⇣ ⌘ ⇣ ⌘ 6 7 6 7 6 7 � � � � y ( t ) − R ( t ) cos θ ( t ) + ωδ t − cos( θ ( t )) y ( t ) − R ( t ) cos θ ( t ) + ∆ θ ( t + δ t ) − cos( θ ( t )) 6 7 6 7 6 7 4 5 4 4 5 θ ( t + δ t ) θ ( t ) + ωδ t θ ( t ) + ∆ θ ( t + δ t ) 2 3 x ( t ) + v ( t ) ⇣ ⌘ sin( θ ( t ) + ∆ θ ( t + δ t )) − sin( θ ( t )) Function of the ICR 6 7 ω ( t ) 6 7 6 7 = y ( t ) − v ( t ) 6 7 ⇣ ⌘ Function of the 6 cos( θ ( t ) + ∆ θ ( t + δ t )) − cos( θ ( t )) 7 6 7 ω ( t ) 6 7 issued velocities 4 5 θ ( t ) + ω ( t ) δ t To obtain future poses over time-extended intervals, it is necessary to provide initial conditions, specify geometry parameters, assign the linear and angular velocity profiles v(t) and ω (t), and integrate over time (which might not be obvious/easy) In the specific case of a two-wheeled ! P ( t ) = r ˙ ' R − r ˙ = v R ( t ) − v L ( t ) ' L di ff erential robot , v(t) and ω (t) at 2 ` 2 ` the reference point P on the chassis are functions of the Left and Right v P ( t ) = r ˙ ϕ R + r ˙ = v R ( t ) + v L ( t ) ϕ L speeds issued to the Left and Right 2 2 wheel, respectively: 2

F O R WA R D K I N E M AT I C S F O R D I F F E R E N T I A L D R I V E Y R In the robot frame , given the spinning velocity controls for wheels Y W       ICR(t) r ˙ ' R (t) + r ˙ ' L (t) v R (t) + v L (t) x ˙ ω (t) 2 2 X R             y ˙ 0 0 = =       V L (t)             R ˙    r ˙ ' R (t) – r ˙ ' L (t)   v R (t) – v L (t)  θ V(t) 2 ` 2 ` R l 𝜾     y P l x ˙ v(t) V R (t) Two control inputs,         y ˙ 0 linear and angular =         velocity X W     x ˙     ω (t) θ R In the world reference frame , given the local inputs v(t), ω (t)       x ˙ cos( θ (t)) – sin( θ (t)) 0 v(t) x = v(t) cos( θ (t)) ˙             y ˙ sin( θ (t)) cos( θ (t)) 0 0 = y = v(t) sin( θ (t)) ˙                   ˙ ˙       0 0 1 ω (t) θ θ = ω (t) W R 3

A G E N E R A L M O D E L : T H E U N I C Y C L E M O D E L x = v R (t) + v L (t) Y R Y W ˙ cos( ✓ (t)) 2 ICR(t) ω (t) y = v R (t) + v L (t) X R ˙ sin( ✓ (t)) 2 V L (t) x = v(t) cos( θ (t)) ˙ R V(t) ✓ = v R (t) – v L (t) ˙ y = v(t) sin( θ (t)) ˙ l 2 ` 𝜾 y P V R (t) l ˙ θ = ω (t) x = r ˙ ' R (t) + r ˙ ' L (t) ˙ cos( ✓ (t)) 2 X W x y = r ˙ ' R (t) + r ˙ ' L (t) ˙ sin( ✓ (t)) 2 Unicyle model (non holonomic) ✓ = r ˙ ' R (t) – r ˙ ' L (t) ˙ 2 ` Abstract model! ω (t) V(t) 𝜾 X y x 4

F U L L F O R WA R D K I N E M AT I C S : T I M E T R A J E C T O RY x = v(t) cos( θ (t)) ˙ For a generic robot, given v(t), ω (t) as local inputs, the velocity of pose change in the world reference frame: y = v(t) sin( θ (t)) ˙ ˙ θ = ω (t) ω (t) V(t) If the time-profiles of the velocities are known, the equations can be integrated over time to predict the time trajectory: 𝜾 y x For a 2-wheeled di ff erential robot 5

C O M P U TAT I O N O F R AT E O F C H A N G E I N T H E P O S E V E C T O R A robot is positioned at an angle of 60 degrees with respect to the global reference frame and has wheels with a radius of 1 cm. The wheels are 2 cm from the center of the chassis. If the speeds of wheels 1 and 2, are 4 cm/s and 2 cm/s, respectively, what is the robot velocity with respect to the global reference frame? 6

F O R WA R D K I N E M AT I C S : E A S Y ( B U T U S E F U L ) C A S E S Equal (constant) forward speed for both wheels Also for interval-wise changes v L = v R = v in the common velocity x ( t ) = x 0 + vt cos( θ ) The robot moves along a y ( t ) = y 0 + vt sin( θ ) straight trajectory θ ( t ) = θ 0 Equal but opposite wheel speeds v L = − v R Constant (di ff erent) speeds for both wheels v L ( t ) = v L , v R ( t ) = v R , v L 6 = v R x ( t ) = x 0 The robot moves along a circular y ( t ) = y 0 v R + v L x ( t ) = x 0 + ` � t � trajectory sin ` ( v R � v L ) ✓ ( t ) = ✓ 0 + 2 vt 2 v R � v L of constant radius R 2 ` v R + v L y ( t ) = y 0 � ` � t � cos ` ( v R � v L ) R = ` v R + v L The robot 2 v R � v L v R − v L rotates in place ✓ ( t ) = ✓ 0 + t � � v R � v L ` 7

K I N E M AT I C S E Q U AT I O N S F O R T H E B I C Y C L E M O D E L x = v(t) cos( θ (t)) ˙ y = v(t) sin( θ (t)) ˙ ω ( t ) = v ( t ) L ˙ θ = ω (t) R ( t ) = R 1 ( t ) = tan( γ ( t )) , R ( t ) x = v(t) cos( θ (t)) ˙ y = v(t) sin( θ (t)) ˙ θ = v(t) ˙ tan( γ (t)) L The max value of 𝛅 limits maneuverability: parking problem, complex inverse kinematics 8

A C K E R M A N N S T E E R I N G The front wheel must follow a longer path, R 2 ( t ) > R 1 ( t ) and therefore must rotate faster than the rear wheel. With two front wheels a di ff erential gear is necessary to implement this di ff erence R L 𝓂 𝓂 L R ( t ) − ` = Once set the steering for the left wheel, the right wheel tan( ↵ l ( t )) is constrained by rolling motion to steer a specific angle L R ( t ) + ` = which is coherent with the vehicle’s ICR tan( ↵ r ( t )) 9

F O R WA R D V S . I N V E R S E K I N E M AT I C S Posture prediction: Forward Kinematics Posture regulation: Inverse Kinematics Controls R Controls ? Pose o b ? (v, 𝞉 ) o Robot Robot t Robot Goal pose Feasible? Control Robot Controls ? Path Planning Path following R Robot Robot o (geometry) b Path( s ) o t Feasible? Robot Controls ? Trajectory Planning Trajectory following R Robot Robot o (kinematics, time) b Path(s, t ) o t Feasible? 10

I N V E R S E K I N E M AT I C S ( F O R D I F F E R E N T I A L R O B O T S ) Given an initial and a goal pose, what are the velocity profiles to provide to the wheels to achieve the desired pose transition? {W} In the general case, a very hard problem, the presence of the non-holonomic sliding constraints makes computations di ffi cult ? Given a time t and a goal pose, the equations solve for v L and v R but do not provide an v L ( t ) = v L , v R ( t ) = v R , v L 6 = v R independent control for 𝜄 ( 𝜺 m =2) v R + v L x ( t ) = x 0 + ` � t � sin ` ( v R � v L ) The same final pose(t) 2 v R � v L can be achieved in many/infinite ways v R + v L y ( t ) = y 0 � ` � t � cos ` ( v R � v L ) Goal 2 v R � v L Di ff erent radii, and/or ✓ ( t ) = ✓ 0 + t � � v R � v L multiple iterations over ` the same circular path to Start meet time requirements 11

S I M P L I F I E D I N V E R S E K I N E M AT I C S A P P R O A C H In general, a forward solution is already quite complex, a direct approach to inversion would be problematic for general relations between the two control velocities v L = − v R v L = v R = v Easy forward kinematics cases Rotate Move x ( t ) = x 0 x ( t ) = x 0 + vt cos( θ ) in place straight y ( t ) = y 0 y ( t ) = y 0 + vt sin( θ ) ✓ ( t ) = ✓ 0 + 2 vt θ ( t ) = θ 0 2 ` Solve the problem by decomposing the trajectory in primitive motion segments (very easy in open space): • Straight lines • Segments of a circle or rotation in place Easier but not easy: a lot of issues to guarantee smoothness (and other quality constraints) and to deal with robot and environments’ constraints Other (better) ways to do it, later on …. 12

C O M P U T I N G T H E S TAT E / P O S E What is robot’s pose in {W} {W} after moving at a velocity (v, 𝞉 ), for 1 minute? Actual trajectory ξ (t) = [x(t) y(t) 𝜄 (t)] Δξ (t) ? Target trajectory ξ d (t) = [x d (t) y d (t) 𝜄 d (t)] Where am I? / What is my pose? With respect to an initial reference point, a coordinate system, a map …. Δξ wind 13

W H AT I S T H E T Y P E O F A P O S E ? Pose / Position should be thought in a quite general sense. The required form of a position depends on the type of the reference system that is of interest / available Geographical map, landmarks Cartesian, 2D 3D earth coordinates 14 T oplogical map Metric map Sensor map