Mobile Robot Kinematics PDF

3. MOBILE ROBOT KINEMATICS 1 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematics - definition Definition and origin: Greek kinein = to move Kinematics is a branch of mechanics Mathematical modeling of motio...

3. MOBILE ROBOT KINEMATICS 1 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematics - definition Definition and origin: Greek kinein = to move Kinematics is a branch of mechanics Mathematical modeling of motion without considering the causing forces is called kinematics. Mathematical modeling of motion considering the causing forces is called dynamics. 2 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematics - goal Design and control Mobile robot kinematics is similar to manipulator kinematics Mobile robots can move freely in space One cannot measure directly position of a MR Position must be integrated over time – errors in determining the position MR motion is a product of wheel motion and constraints thereof 3 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematics - goal T Compute robot velocity q =  x y   as a funciton of wheel velocity  i , wheel angles  i , heading rate i and robot geometric parameters. yI Direct kinematics v(t) s(t)  x  q =  y  = f ( 1 , n , 1 ,  m , 1 , m )   xI Inverse kinematics  1  n 1  m 1  m  = f ( x, y,)   T 4 Autorska prava © Ivan Petrović. Sva prava pridržana.  Robot configuration - pose Robot position in arbitrary coo. frame YI Global coo. System (GCS): X I ,YI  YR Local robot coo. System: X R ,YR  XR  qI =  x y  T Robot pose in GCS: P XI Transformation between the two systems qR = R ( ) qI , qI = R −1 ( ) qR Y I XR  cos sin  0 cos  − sin  0 R( ) = − sin  cos 0 R −1 ( ) =  sin  cos  0    0 0 1  0 0 1  YR XI 5 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematic wheel constraints Assumptions: Wheel plane is always perpendicular to the ground plane Only a single point of contact between wheel and ground No slipage, only rolling and rotation about the vertical axis through point of contact is possible Two constraints for each wheel type: The concept of rolling The concept of no lateral slipage 6 Autorska prava © Ivan Petrović. Sva prava pridržana. Standard wheel 7 Autorska prava © Ivan Petrović. Sva prava pridržana.  Standard wheel Rolling – projection of all motion along the direction of motion – first equation xR sin( +  ) − yR cos( +  ) − R l cos  = r Lateral motion constraint– projection of all motion along the wheel axis – second eq. xR cos( +  ) + yR sin( +  ) + R l sin  = 0 Matrix form: sin( +  ) − cos( +  ) −l cos   R( )qI = r cos( +  ) sin( +  ) l sin   R( )qI = 0 8 Autorska prava © Ivan Petrović. Sva prava pridržana. Steered standard wheel 9 Autorska prava © Ivan Petrović. Sva prava pridržana.  Steered standard wheel Heading rate  affects the platform mobility – an added degree of freedom  has no contribution to motion along the motion direction (zero contribution in contact point A) Equations equal to the standard wheel 10 Autorska prava © Ivan Petrović. Sva prava pridržana.  Castor wheel 11 Autorska prava © Ivan Petrović. Sva prava pridržana.  Castor wheel Lenght AB along the wheel rolling direction Effect of robot angular velocity  R can be analysed in point A sin( +  ) − cos( +  ) −l cos   R( )qI = r cos( +  ) sin( +  ) d + l sin   R( )qI + d  = 0   For   xR T yR  R     so that constraints apply   Omnidirectional motion 12 Autorska prava © Ivan Petrović. Sva prava pridržana.  Swedish wheel 13 Autorska prava © Ivan Petrović. Sva prava pridržana.  Swedish wheel We look at the roll direction of small rollers sin( +  +  ) − cos( +  +  ) −l cos( +  ) R( )qI = r cos  cos( +  +  ) sin( +  +  ) l sin( +  ) R( )qI = rrr − r sin  For  = 0 Swedish 90 – first constraint equal to other wheel constraints For  = 90 equal to standard wheel   For   xR yR  R     so that constraints T r  apply Omnidirectional motion 14 Autorska prava © Ivan Petrović. Sva prava pridržana. Spherical wheel 15 Autorska prava © Ivan Petrović. Sva prava pridržana.  Spherical wheel sin( +  ) − cos( +  ) −l cos   R( )qI = r cos( +  ) sin( +  ) l sin   R( )qI = 0 For arbitrary  xR yR R  we choose momentarily direction  so that it fullfils constraint 2 Omnidirectional motion 16 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematic robot constraints Robot with M wheels Each wheel brings 0 or more motion constraints Only standard and standard directed wheel have constraints Combine constraints so that you get the kinematic model of the form:  x q =  y  = f ( 1 , n , 1 ,  m , 1 , m )   17 Autorska prava © Ivan Petrović. Sva prava pridržana.  Differential drive MR Left wheel: Right wheel: 1 = 90, 1 = 0  2 = −90,  2 = 180 xR −  R l = rL L (1) xR +  R l = rR R (2) yR = 0 yR = 0 rR R + rL L (1)+(2) -> xR = =v YR 2 r  − rL L (2)-(1) ->  = R R = vL R 2l GCS: qI = R −1 ( ) qR l  x  cos  − sin  0  v  x = v cos  l XR  y  =  sin     cos  0   0  => y = v sin  vR    0 0 1     = 18 Autorska prava © Ivan Petrović. Sva prava pridržana.  Omnidirectional MR Wheel 1: Wheel 2: Wheel 3: 1 = 60, 1 = 0,  1 = 0  2 = 180,  2 = 0,  2 = 0  3 = −60, 3 = 0,  3 = 0 xR sin 60 − yR cos 60 −  R l = r11 yR −  R l = r22 − xR sin 60 − yR cos 60 −  R l = r33 xR cos 60 + yR sin 60 = rr1r1 − xR = rr 2r 2 xR cos 60 − yR sin 60 = rr 3r 3 matrix: AqR = b YR l = 1, r1,2,3 = 1, rr1,2,3 = 1 , 1 = 4, 2 = 1, 3 = 2 exercise: 4 xR , y R ,  R ,  r 1 ,  r 2 ,  r 3 = ?  3 3  2   3 1   0 −   3 V2  − −1   l V1  3 3  l  2 2   1 2 1   4 A= 0 1 −1 −1 A = − −  qR =  −    3 3 3 3     − 3 1 −1 l XR  2 − 2  −  3 1 − 1 3 − 1  3  −  7  3   3 3 3  r1 = − , r 2 = − , r 3 == 12 6 4 V3 20 Autorska prava © Ivan Petrović. Sva prava pridržana.  Nonholonomic constraints Configuration space: q C  n Holonomic constraints Reduce configuration space Generally result of mechanical connections between various robot parts Vehicle with trailer (not all trailer-vehicle angles are possible) Nonholonomic constraints Do not reduce the configuration space, but they reduce local mobility (e.g. no lateral motion) Nonholonomic system can access the whole configuration space, although with a limited set of velocities 21 Autorska prava © Ivan Petrović. Sva prava pridržana.  Nonholonomic constraints A holonomic robot has 0 nonholonomic constraints E.g. MR has 3 DoF with the configuration space defined as q = x y  C  3 If nonholonomic, then can can access any q, but not freely in C (has limited velocities) Nonholonomic robots – differential drive, Ackermann drive Holonomic robots – drives with swedish and/or spherical wheels A robot is holonomic when the number of controlable DoF is equal to the total number of DoF 22 Autorska prava © Ivan Petrović. Sva prava pridržana.  Instantaneous Centre of Curvature (ICC) Point around which all the wheels make circular trajectory ICC must lie on a line which coincides with the wheel rotation axis so that roll is possible: Rolling All wheel motion can produce cannot be rolling achieved motion 26 Autorska prava © Ivan Petrović. Sva prava pridržana.  Determinig kinematic model using ICC VL VR Algorithm: 1. Pick the coordinate system. 2. Determine ICC, i.e., radius of robot rotation R about ICC. 3. Determine angular velocity  about ICC, and the linear velocity V. 4. Integrate robot velocities to determine the robot pose. 27 Autorska prava © Ivan Petrović. Sva prava pridržana.  Determining kinematic model using ICC ICC Determining ICC:  In order to minimize slipage, ICC R must lie at the intersection of wheel axes All wheels must have the same  angular velocity about ICC y VL Determining angular velocity , radius R 2d and linear velocity V: VR (R+d) = VR (R-d) = VL x Which gives:  = ( VR - VL ) / 2d R = VR /  - d V = R = ( VL + VR ) / 2 28 Autorska prava © Ivan Petrović. Sva prava pridržana.  Determining kinematic model using ICC Determining robot pose: ICC Vx = V(t) cos((t)) Vy = V(t) sin((t))  By integrating velocities Vx(t), Vy(t) and  R (t): x(t) = ∫ V(t) cos((t)) dt  y(t) = ∫ V(t) sin((t)) dt y VL (t) = ∫ (t) dt 2d VR Dir. kinematics of MR-a with diff. drive Where:  = ( VR - VL ) / 2d x R = VR /  - d What do we need to change in order to change ICC V = R = ( VL + VR ) / 2 We need to change the wheel velocities! 29 Autorska prava © Ivan Petrović. Sva prava pridržana.  Kinematic model calibration Historical background in sailing – “dead reckoning” - odometry Simplest method of MR localization Current pose is determined based on MR kinematic model and measured relative displacement Affected by systematic and non-systematic errors Systematic mitigated by calibration Final MR pose in case of real trajectory Real trajectory with Position Starting MR pose systematics errors error Ideal trajectory without systematic errors Final MR pose in case of ideal trajectory 30 Autorska prava © Ivan Petrović. Sva prava pridržana.  Odometry – kinematic model Mobile robot kinematic model MR kinematics x(k + 1) = x(k ) + D  cos (k + 1), lijevi kotač y(k + 1) = y(k ) + D  sin (k + 1), VL (k + 1) = (k ) + (k ). Y središte osovine kotača b y − Traversed path and rotation D(k ) = vt (k )  T , desni kotač VR Sampling (k ) =  (k )  T. x X time Translation and rotation vel. Exact values vL ( k ) + v R ( k )  L ( k ) R +  R ( k ) R unknown → vt (k ) = = , 2 2 systematic error→ v (k ) − vL (k ) R (k ) R − L (k ) R needs calibration!  (k ) = R =. b b b=2d 31 Autorska prava © Ivan Petrović. Sva prava pridržana. Odometry – extended kinematic model We use the extended model for calibration with 3 parameters Compensating unknown wheel radius k1  vL (k ) + k2  vR (k ) vt (k ) = , 2 Compensating unknown axle k1  vR (k ) − k2  vL (k ) length  (k ) =. sa 2 parametra k3  b Traversed distance of wheel in a single time step k D(k ) = vt ( k )  T +  trans d ,  ( k ) =  ( k )  T +  rot d. Compensation of translation error Compensation of rotation error 32 Autorska prava © Ivan Petrović. Sva prava pridržana.  Odometry – calibration based on optimization Possible to apply 2 and 3 parameter model Special trajectories First exp.: straight motion Second exp.: in-place rotation What we need for calibration Exact starting and final MR pose MR trajectory:  Wheel velocities Final MR pose in case of real In-place trajectory rotation to left  Sampling time Final MR Starting MR orientation orientation Real trajectory with Position Starting MR pose systematics errors error In-place rotation to right Ideal trajectory without systematic errors Final MR pose in case of ideal trajectory 33 Autorska prava © Ivan Petrović. Sva prava pridržana.  Odometry – calibration based on optimization We use “Optimization Tolboox” in Matlab Optimization computes parameter values that minimize the pose error Optimization pseudocode Get experimental data Get calibration parameters While unused data exist  Pose change = f (k, vt, ω, current orientation)  New pose = current pose + pose change Return current optimization function value Optimization criterium 2 parameter model MIN  ( x − xtrue ) + ( y − ytrue )  2 2    3 parameter model MIN (orientation – true_orientation)  We use iterative optimization 34 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics (IK) - 1 IK: for given pose (x, y, ) or velocity (V,) find the required wheel velocities (VL i VR)! y We need to solve: x= ∫ V(t) cos((t)) dt VL (t) x y= ∫ V(t) sin((t)) dt VR(t) = ∫ (t) dt  = ( VR - VL ) / 2d Final pose R = VR /  - d Starting pose V = R = ( VL + VR ) / 2 for VL (t) i VR(t). Multiple solutions exist… 36 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics- 2 Key question: Which solution is the best and how to find it? y Finding a solution is not difficult, but the best solution depends on the criterium: VL (t) x shortest time, VR(t) minimal energy, smoothest wheel velocity,... Starting poes Final pose VL (t) VR (t) t 37 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics- 3 Most common way to solve the problem of IK: We decompose the problem so that we control a smaller number of degrees of freedom at the same time. y VL (t) x VR(t) Procedure Starting pose Final pose 38 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics- 4 (1) Turn MR in-place so that wheel are parallel to line connecting starting and final position of the origin of the MR coordinate system. y -VL (t) = VR (t) = -Vmax VL (t) x VR(t) Starting pose Final pose 39 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics- 5 (2) Drive straight until origin of the MR coordinate system is at the final position. y VL (t) = VR (t) = Vmax VL (t) x VR(t) Starting pose Final pose 40 Autorska prava © Ivan Petrović. Sva prava pridržana.  DD: Inverse kinematics - 6 (3) Rotate MR again until the final rotation is met. y -VL (t) = VR (t) = Vmax VL (t) x Wheel vel. diagram: VR(t) (1) (2) (3) VL (t) Starting pose Final pose VR (t) t 41 Autorska prava © Ivan Petrović. Sva prava pridržana.  Motion control 1/4 Pose error e =  x y  T Find a control matrix K such that control of v   drives the error e to zero  x  v(t )   k11 k12 k13     (t )  = K  e =  k   y    21 k22 k23    42 Autorska prava © Ivan Petrović. Sva prava pridržana.  Motion control 2/4 Kinematic model of  x  cos  0 a differential drive  y  =  sin   v    0    mobile robot    0  1 Coordinate  = x 2 + y 2 transformation into  = − + a tan 2 ( y, x ) polar coordinates  = − −    New system  − cos  0     description   =  sin  v −1               sin   −  0   43 Autorska prava © Ivan Petrović. Sva prava pridržana.  Motion control 3/4 Switching between forward and backward motion    − 2 , 2  → v = −v Coordinate transformation is not defined for x=y=0 Control law v = k   = k  + k  Closed loop    −k   cos     =  k sin  − k  − k              −k  sin    44 Autorska prava © Ivan Petrović. Sva prava pridržana.  Motion control 4/4 Local stability – linearized around the origin (cos x = 1,sin x = x )     −k  0 0      =  0 − ( k − k )   −k           0 −k   0      Locally exponentially stable if the eigenvalues have a negative real part k   0, −k  0, k − k   0 k = ( k  , k , k ) = ( 3,8, −1.5 ) 45 Autorska prava © Ivan Petrović. Sva prava pridržana.