Root Position from Wheel Odometry Christopher Marshall 26 Fe 2008 Astract This document develops equations of motion for root position as a function of the distance traveled y each wheel as a function of time (the odometry). The result is a set of equations to update the root position and heading (angle) with incremental changes in the wheel rotation. 1 The Prolem We wish to develop a method to determine the root position given the encoder tick distances for each wheel as a function of time. The inputs will e the linear distance traveled y each wheel or equivalently the encoder ticks traveled y each wheel. The output will e the position x= (x, y) and direction θ after each wheel position update. 2 Analysis 2.1 Coordinates and Parameters y L n x unit direction vector R Figure 1: Odometry Coordinates Figure 1 indicates the root position model eing used for this analysis. The eld coordinates are represented as a normal right handed axis system with positive x to the right and positive y to the left shown in gray. Some key parameters are 1
L,R linear distance of travel of the appropriate wheel (left or right) l, r the corresponding tick counts measured y the encoders aseline distance etween wheel centers n unit normal in the direction of the root heading = (cosθ, sin θ) Given the measured caliration parameters of distance per encoder tick count, δ L and δ R, we have and L = δ L l (1) R = δ R r (2) Since we expect the distance per tick to e equal for the left and right wheels, lets make a new representation of the relationship etween tick count and distance using parameters dened as δ = (δ R + δ L )/2 is the mean distance per tick γ = (δ R δ L )/2 is the asymmetry of the wheels Solving for δ L and δ R in terms of δ and γ we have δ R = δ + γ δ L = δ γ 2.2 Root Direction/Angle Update The general expression for the root direction (i.e. the heading angle with respect to the eld coordinate system) is θ = θ 0 + R L where θ is the current heading of the root, θ 0 is the initial heading of the root at the start of the match, L and R are the distance traveled y the left and right wheel, and is the aseline separation of the wheels on the root. This can e derived for general types of wheel motion ut can e most clearly seen y considering the specic case where L and R are the result of constant velocity wheel motion (linear functions of time). For these special cases, the motion is seen to e circular and the aove expression is immediately determined. Sustituting in the values for L and R from equations 1 and 2 we get: θ = θ 0 + δ Rr δ L l (δ + γ)r (δ γ)l = θ 0 + = θ 0 + δ (r l) + γ (r + l) (3) 2
We expect that γ δ since the wheel drive mechanisms and wheel diameters are the same. The natural units for θ are in terms of (δ/) so we dene a reduced variale y the relationship (δ/) θ = θ with which sustitution the update for heading ecomes ( δ ) θ = ( δ ) [ θ0 + (r l) + γ δ (r + l) ] θ = θ 0 + (r l) + γ (r + l) (4) δ θ is thus the direction angle in units of encoder ticks and, if we assume γ = 0, then the total heading change in these units is just the dierence etween the left and right tick counts. 2.3 Position Update Now consider the general formula for a position update for the root where the wheels have only moved a small amount. In this case, the change in angle is very small and the motion may e approximated y a straight line move of the root center y the mean change in wheel counts in the current direction of the root followed y an update of the heading from the aove expression. In the limit of innitesimal motion steps, this is exact. Let x= (x, y) e the root center position and θ (or equivalently, θ) the heading. The unit normal in the direction of the root heading is n = (cos θ, sin θ) = (cos δ θ, sin δ θ) (5) where x n, θ n are the root position and heading at the start of the small motion, and x n+1, θ n+1 are the nal position and heading. n n and n n+1 are the unit direction vectors corresponding to θ n and θ n+1 respectively. Dening r = r n+1 r n and l = l n+1 l n, we can write the position update as: ( ) x n+1 = x n δr r + δ L l + n n 2 = x n + 1 [δ( r + l) + γ( r l)] nn 2 The appearance of δ and γ as multipliers on r and l suggests a change to a reduced variale dened y δ x = x. The aove equation now ecomes x n+1 = x n + 1 [( r + l) + ( γ ] 2 δ )( r l) n n (6) which completes the equations required for position and heading update. From the equation 4 for θ we calculate the exact heading for all values of r, l. At the start of each step we know x n and θ n and can calculate the unit direction vector n n (cos θ n, sin θ n ). Now use the position update formula of equation 6 to get the new position, x n+1, and nish y updating the nal heading from the exact formula of equation 4. Iterating this process for each motion step will generate the position and heading of the root as a function of time. 3
2.4 Direction Vector Update We complete our analysis y determining an incremental update formula for the unit direction vector n n+1 as a function of r, l, and n n. The idea is to have an approximate solution that is accurate enough for our purposes ut does not required heavy oating point calculations or expensive transcendental function evaluations. Writing the exact vector expression for n n+1 we have where n n+1 = = = cos θn+1 n+1 sin θ cos{θ n + (δ/)( r l) + (γ/)( r + l)} sin{θ n + (δ/)( r l) + (γ/)( r + l)} cos sin n n sin cos = δ ( r l) + γ ( r + l) Now let δ = δ/ and γ = γ/ and we have = δ( r l) + γ( r + l) (7) where with the parameters of the 2008 root, we have δ < 1 5 1 20 1 100 γ δ NOTE: for the measured values of δ and we expect to e 1 for small changes in the tick counts. For straight line motion or gentle turns, this condition will continue to hold through a larger range of tick count delta values. Therefore, we can use the small angle approximations for sin and cos : cos. = 1 2 /2 sin. = and sustituting into the exact expression for n n+1 we have the desired update formula: n n+1. 1 2 /2 = 1 2 n n /2 n n+1. (1 2 /2) n n = x n n y n n x + (1 2 /2) n n (8) y where n = (n x, n y ) and we may need to renormalize the direction vector so that (n x ) 2 + (n y ) 2 = 1 to correct for round-o or other numerical errors. For constant increments of the angle and direction vector, i.e. l = 0, ±1 and/or r = 0, ±1 we can precalculate the exact values for cos and sin and avoid the transcendental function call. The values of for these values are given in Figure 2. 4
l -1 0 1-1 0 1 r γ = 0-1 2 γ δ γ 2 δ 0 δ 2 δ 0 δ γ 0 δ + γ δ 0 δ 1 2 δ δ + γ 2 γ 2 δ δ 0 Figure 2: values for single tick increments 2.5 Update Approximation Error Let us riey consider one type of error that may occur during the incremental position update. First, the approximation is exact in the limit of innitesimal position increments since the error terms are higher order in the step size than the update terms and so go to zero in the small step limit. What type of motion is generated y the single wheel update increment for constant velocity motion (i.e. oth wheels rotating at the same speed)? Assume, without loss of generality, that θ 0 = 0. In this case, ticks from the two sides will increment together in an alternating fashion L, R, L, R, L, R.... As a result, the position will alternately e moved straight ahead y δ, the angle will e changed y δ, then a move of δ at heading θ = δ, with the angle changed y + δ to the original heading at θ = 0. The result is the heading is correct after each pair of L, R increments ut the forward increment is less than the full amount y δ 2 /2 and there is a perpendicular shift to the left y δ. BUT, if the encoder ticks alternate starting on the right, the same shortening of the forward motion is otained ut now the perpendicular shift is to the right! What is the origin of this peculiar ehavior and what can e done to x it? The drift in the update position when driving straight comes from the nonphysical nature of the single encoder update steps. While in the limit as δ 0, the update approximation error does goes to zero, for nite steps, it amounts to a jerky, alternating wheel, step motion. In fact, the two cases of drifting to the left and drifting to the right represent the ounding, worst case errors for straight line motion. The x is to notice that although the ticks may increment discretely, the underlying wheel rotations, L(t) and R(t) are continuous and generally smooth functions. Rather than assuming the other wheel has a tick delta of zero, we should assume that it continues in motion with the current velocity. The result is that the forward motion will e larger due to the contriution from the alternate wheel and the direction error from one wheel will e reduced or canceled y the motion of the other wheel. As a nal point, the magnitude of the position error per step is less than 0.4% for each coordinate direction. This is small enough that if we could somehow remove the ias to a given side, that would resolve the lateral shift. The decrease in advance rate could e addressed y a scale factor. A simple approach would e to comine alternating encoder increments into a single, doule increment, straight ahead. A more sophisticated approach might e a predictor-corrector 5
algorithm where you do your est at each step, an on successive steps use the additional information to correct the error in the previous update. 3 Conclusions We have developed step method to update the current root position as the wheel tick counts change. By expressing position in units of tick length, δ, and heading angle in units of tick length over aseline length, δ = δ/, position an angle updates ecome integer additions and sutractions (exactly when wheels are the same size, γ = 0). The unit vector update is the only oating point required and it consists of constant rotations y small angles which should e amenale to xed point analysis. 3.1 Integer Parameter Sizes Given the use of appropriate tick ased units, what type of variales do we need to represent the odometer position and angle parameters? 3.1.1 Position If we update the position after each encoder interrupt (in the handler itself) then the result that the propagation equations are all for tick deltas in { 1, 0, 1} so that the values for sin and cos may e precalculated. Since we are in the interrupt handler, only one of r or l is non-zero. For that tick delta, the expression for is an odd function of the delta count so we need only one value precalculated for each wheel and can get the rest y symmetry. For γ 0, we need to precalculate for two values, one for each wheel. Given the eld dimensions of 54 x 27 feet, and a measured δ = 0.1795 inches, the largest possile eld position is 57 12/0.1795 = 3811 which needs 12 inary digits to represent. If γ < δ/16 then another 4 inary digits are needed for 1 it of signicance. Therefore at least a 16 it integer is needed for eld position. To avoid round-o errors from the calculation, a 24 it representation should e used. 32 it integers should not e required ut an error analysis should e done. Note: this looks like a 24-it add and a 24-it for each component, since all tick deltas are ±1. 3.1.2 Heading With the dimensionless units δ = δ/, the numer of counts in a 2π rotation is 2π/(δ/) or 6.28/(0.1795/22.5) = 787 which needs 10 inary digits for full representation. As aove, if γ < δ/16, than at least another 4 its resolution are required. A 16 it representation would give 6 its of precision in the update which might e sucient. We'll start with a 24 it representation which allows for update precision more than the angular integer position accuracy. A full error analysis needs to e performed. 6
3.1.3 Unit Direction Vector To match a 24 it representation of eld position (which implies a 12 it integer part and a 12 it fractional part) the coordinate values of n must have at least a 12 it representation. To avoid round-o error in the unit vector update step, we notice that 2 /2 is approximately 0.00003 which requires at least a 15 it fractional part to represent. A 24 it representation seems adequate to start. The update step then consists of 2 24-it additions, 2 24-it multiplies and a shift (maye) for each coordinate or a total of 4 additions and 4 multiplications per encoder interrupt. At the maximum encoder rate of 300 RPM with 128 counts/revolution and 2 encoders, we have a total of 1280 wheel updates per second maximum. From the PIC18 reference manual (and assuming that our model has the hardware multiply commandneed to check the assemly output), a rough estimate for a 24x24 signed integer multiply is aout 100 cycles. With a 10 MHz clock, that amounts to 7800 cycles/call. 3.1.4 Cycle Count From the maximum interrupt rate, we have aout 7800 CPU cycles to process each update. The position update takes aout 2 24-it adds and 2 24-it multiplies. The heading update consists of a single 24-it add. The direction vector update takes 4 24-it adds and 4 24-it multiplies. If we call an add 1/5 of the multiply cycle time, we have a total of 7.4 multiplies or roughly 740 CPU cycles per interrupt. This works out to a duty cycle for position interrupt handling of aout 10% with a handler latency of < 0.08 msec/interrupt so it should e OK in terms of system response. This is the peak update load when driving at maximum speed (aout 10 ft/sec). For medium speed maneuvering, the load drops elow 5% or < 0.04 msec/interrupt. If the interrupt update rate ends up aecting the root responsiveness and other interrupt handling, it is possile to pack a it mask to with update encoder IDs and directions (the sign of the tick deltas). Then the it mask could e processed outside of the interrupt loops to update the position information. Additionally, since straight ahead motion is a common pattern (alternating left and right counts in the same direction), a single precalculated update could take care of multiple ticks at once. 3.2 Simple Odometry Update (γ = 0) From the aove analysis, we show an iterative position update process that may e used to track the root eld position in an incremental fashion. The steps are as follows: 1. Initialize motion parameters: x 0, θ 0, n 0 from the initial position and orientation 2. Each tick of the left or right wheel encoder, update the position, x,heading, θ,and unit direction vector, n,using equations 6, 4, and 8 7
By taking advantage of the appropriate units for position (δ) and angle ( δ) and computing the updates each tick change on each wheel separately, we get a very simple set of update equations for γ = 0: x n+1 = x n + n/2 θ n+1 = θ n + r l [ n n+1 = n n /2 1 + 1 /2 ] n n where r or l = ±1 and the other is zero. The left side of the tale of gure 2 shows the values for = ± δ and the appropriate sign depending on the wheel and direction. How could this e improved to handle the side shift error as discussed in section 2.5? 8