5.3.2 Sampling Algorithm RobotMotion

Transcription

1 122 5 RobotMotion (a) (b) (c) Figure 5.3 The velocity motion model, for different noise parameter settings. The function prob(x, b 2 ) models the motion error. It computes the probability of its parameter x under a zero-centered random variable with variance b 2.TwopossibleimplementationsareshowninTable5.2,forerror variables with normal distribution and triangular distribution, respectively. Figure 5.3 shows graphical examples of the velocity motion model, projected into x-y-space. In all three cases, the robot sets the same translational and angular velocity. Figure 5.3a shows the resulting distribution with moderateerrorparameters α 1 to α 6. ThedistributionshowninFigure5.3bis obtainedwithsmallerangularerror(parameters α 3 and α 4 )butlargertranslationalerror(parameters α 1 and α 2 ). Figure5.3cshowsthedistribution under large angular and small translational error Sampling Algorithm For particle filters(c.f. Chapter 4.3), it suffices to sample from the motion model p(x t u t,x t 1 ),insteadofcomputingtheposteriorforarbitrary x t, u t and x t 1. Samplingfromaconditionaldensityisdifferentthancalculating thedensity: Insampling,oneisgiven u t and x t 1 andseekstogenerate arandom x t drawnaccordingtothemotionmodel p(x t u t,x t 1 ). When calculatingthedensity,oneisalsogiven x t generatedthroughothermeans, andoneseekstocomputetheprobabilityof x t under p(x t u t,x t 1 ). The algorithm sample_motion_model_velocity in Table 5.3 generates randomsamplesfrom p(x t u t,x t 1 )forafixedcontrol u t andpose x t 1. It accepts x t 1 and u t asinputandgeneratesarandompose x t accordingto thedistribution p(x t u t,x t 1 ).Line2through4 perturb thecommanded control parameters by noise, drawn from the error parameters of the kinematic motion model. The noise values are then used to generate the sample s

2 5.3 Velocity Motion Model 123 1: Algorithmmotion_model_velocity(x t,u t,x t 1 ): 2: µ = 1 2 (x x )cos θ + (y y )sin θ (y y )cos θ (x x )sin θ 3: x = x + x + µ(y y ) 2 4: y = y + y + µ(x x) 2 5: r = (x x ) 2 + (y y ) 2 6: θ = atan2(y y,x x ) atan2(y y,x x ) 7: ˆv = θ t r 8: ˆω = θ t 9: ˆγ = θ θ t ˆω 10: return prob( v ˆv,α 1 v 2 + α 2 ω 2 ) prob( ω ˆω,α 3 v 2 + α 4 ω 2 ) prob( ˆγ,α 5 v 2 + α 6 ω 2 ) Table5.1 Algorithmforcomputing p(x t u t, x t 1)basedonvelocityinformation. Hereweassume x t 1isrepresentedbythevector (x y θ) T ; x tisrepresentedby (x y θ ) T ;and u tisrepresentedbythevelocityvector (v ω) T. Thefunction prob(a, b 2 )computestheprobabilityofitsargument aunderazero-centereddistributionwithvariance b 2.Itmaybeimplementedusinganyofthealgorithmsin Table : Algorithmprob_normal_distribution(a, b 2 ): { 1 2: return exp 1 a 2 } 2π b 2 2 b 2 3: Algorithmprob_triangular_distribution(a, b 2 ): { 1 4: return max 0, a } 6 b 6 b 2 Table 5.2 Algorithms for computing densities of a zero-centered normal distributionandatriangulardistributionwith variance b 2.

3 124 5 RobotMotion 1: Algorithmsample_motion_model_velocity(u t,x t 1 ): 2: ˆv = v + sample( α 1 v 2 + α 2 ω 2 ) 3: ˆω = ω + sample( α 3 v 2 + α 4 ω 2 ) 4: ˆγ = sample( α 5 v 2 + α 6 ω 2 ) 5: x = x ˆvˆω sinθ + ˆvˆω sin(θ + ˆω t) 6: y = y + ˆvˆω cos θ ˆvˆω cos(θ + ˆω t) 7: θ = θ + ˆω t + ˆγ t 8: return x t = (x,y,θ ) T Table5.3 Algorithmforsamplingposes x t = (x y θ ) T fromapose x t 1 = (x y θ) T andacontrol u t = (v ω) T.Notethatweareperturbingthefinalorientation byanadditionalrandomterm, ˆγ. Thevariables α 1through α 6aretheparameters ofthemotionnoise. Thefunctionsample( b 2 )generatesarandomsamplefroma zero-centereddistributionwith variance b 2. Itmay,forexample,beimplemented using the algorithms in Table : Algorithmsample_normal_distribution( b 2 ): 2: return i=1 rand( b, b) 3: Algorithmsample_triangular_distribution( b 2 ): 6 4: return [rand( b,b) + rand( b,b)] 2 Table 5.4 Algorithm for sampling from(approximate) normal and triangular distributionswithzeromeanand variance b 2 ;seewinkler(1995:p293).thefunction rand(x, y) is assumed to be a pseudo random number generator with uniform distributionin [x, y].

4 5.3 Velocity Motion Model 125 (a) (b) (c) Figure 5.4 Sampling from the velocity motion model, using the same parameters as in Figure 5.3. Each diagram shows 500 samples. new pose, in lines 5 through 7. Thus, the sampling procedure implements a simple physical robot motion model that incorporates control noise in its prediction, in just about the most straightforward way. Figure 5.4 illustrates the outcome of this sampling routine. It depicts 500 samples generated by sample_motion_model_velocity. The reader might want to compare this figure with the density depicted in Figure 5.3. Wenotethatinmanycases,itiseasiertosample x t thancalculatethedensityofagiven x t.thisisbecausesamplesrequireonlyaforwardsimulation of the physical motion model. To compute the probability of a hypothetical pose amounts to retro-guessing of the error parameters, which requires us to calculate the inverse of the physical motion model. The fact that particle filters rely on sampling makes them specifically attractive from an implementation point of view Mathematical Derivation of the Velocity Motion Model We will now derive the algorithms motion_model_velocity and sample_motion_model_velocity. As usual, the reader not interested in the mathematical details is invited to skip this section at first reading, and continue in Chapter 5.4(page 132). The derivation begins with a generative model of robot motion, and then derives formulae for sampling and computing p(x t u t,x t 1 )forarbitrary x t, u t,and x t 1. Exact Motion

5 126 5 RobotMotion y <x c,y c> r θ 90 x θ <x,y> Figure 5.5 Motion carried out by a noise-free robot moving with constant velocities vand ωandstartingat (x y θ) T. (5.5) Before turning to the probabilistic case, let us begin by stating the kinematics foranideal,noise-freerobot.let u t = (v ω) T denotethecontrolattime t.if bothvelocitiesarekeptatafixedvaluefortheentiretimeinterval (t 1,t], therobotmovesonacirclewithradius r = v ω This follows from the general relationship between the translational and rotational velocities v and ω for an arbitrary object moving on a circular trajectory with radius r: (5.6) v = ω r Equation(5.5) encompasses the case where the robot does not turn at all(i.e., ω = 0),inwhichcasetherobotmovesonastraightline. Astraightline correspondstoacirclewithinfiniteradius,hencewenotethat rmaybe infinite. Let x t 1 = (x,y,θ) T betheinitialposeoftherobot,andsupposewekeep thevelocityconstantat (v ω) T forsometime t.asoneeasilyshows,the centerofthecircleisat (5.7) (5.8) x c = x v ω sin θ y c = y + v ω cos θ

6 5.3 Velocity Motion Model 127 (5.9) (5.10) Thevariables (x c y c ) T denotethiscoordinate.after ttimeofmotion,our idealrobotwillbeat x t = (x,y,θ ) T with x x c + v y ω sin(θ + ω t) = y c v θ ω cos(θ + ω t) θ + ω t x v ω sinθ + v ω sin(θ + ω t) = y + v ω cos θ v ω cos(θ + ω t) θ ω t The derivation of this expression follows from simple trigonometry: After tunitsoftime,thenoise-freerobothasprogressed v talongthecircle, whichcauseditsheadingdirectiontoturnby ω t.atthesametime,its x and ycoordinateisgivenbytheintersectionofthecircleabout (x c y c ) T,and theraystartingat (x c y c ) T attheangleperpendicularto ω t.thesecond transformation simply substitutes(5.8) into the resulting motion equations. Of course, real robots cannot jump from one velocity to another, and keep velocity constant in each time interval. To compute the kinematics with nonconstant velocities, it is therefore common practice to use small values for t, and to approximate the actual velocity by a constant within each time interval. The(approximate) final pose is then obtained by concatenating the corresponding cyclic trajectories using the mathematical equations just stated. Real Motion In reality, robot motion is subject to noise. The actual velocities differ from the commanded ones(or measured ones, if the robot possesses a sensor for measuring velocity). We will model this difference by a zero-centered random variable with finite variance. More precisely, let us assume the actual velocities are given by ( ˆv ˆω ) = ( v ω ) + ( εα1v 2 +α 2ω 2 ε α3v 2 +α 4ω 2 ) Here ε b 2 isazero-meanerrorvariablewith variance b 2. Thus,thetrue velocity equals the commanded velocity plus some small, additive error (noise). In our model, the standard deviation of the error is proportional tothecommandedvelocity. Theparameters α 1 to α 4 (with α i 0for i = 1,...,4)arerobot-specificerrorparameters. Theymodeltheaccuracy of the robot. The less accurate a robot, the larger these parameters.

7 128 5 RobotMotion (a) (b) -b b -b b Figure5.6 Probabilitydensityfunctionswith variance b 2 :(a)normaldistribution, (b) triangular distribution. NORMAL DISTRIBUTION (5.11) Twocommonchoicesfortheerror ε b 2 arethenormalandthetriangular distribution. Thenormaldistributionwithzeromeanand variance b 2 isgivenbythe density function ε b 2 (a) = 1 1 a 2 2π b 2 e 2 b 2 Figure 5.6a shows the density function of a normal distribution with variance b 2. Normaldistributionsarecommonlyusedtomodelnoisein continuous stochastic processes. Its support, which is the set of points a with p(a) > 0,is R. TRIANGULAR Thedensityofatriangulardistributionwithzeromeanand variance b 2 is DISTRIBUTION given by (5.12) ε b 2 (a) = { } 1 max 0, 6 b a 6 b 2 (5.13) whichisnon-zeroonlyin ( 6b; 6b).AsFigure5.6bsuggests,thedensity resembles the shape of a symmetric triangle hence the name. Abettermodeloftheactualpose x t = (x y θ ) T afterexecutingthe motioncommand u t = (v ω) T at x t 1 = (x y θ) T isthus x y θ = x y θ + ˆvˆω sin θ + ˆvˆω sin(θ + ˆω t) ˆv ˆω cos θ ˆvˆω cos(θ + ˆω t) ˆω t Thisequationisobtainedbysubstitutingthecommandedvelocity u t = (v ω) T withthenoisymotion (ˆv ˆω) T in(5.9).however,thismodelisstillnot very realistic, for reasons discussed in turn.

8 5.3 Velocity Motion Model 129 Final Orientation (5.14) The two equations given above exactly describe the final location of the robot given that the robot actually moves on an exact circular trajectory with radius r = ˆvˆω. Whiletheradiusofthiscircularsegmentandthedistance traveled is influenced by the control noise, the very fact that the trajectory is circular is not. The assumption of circular motion leads to an important degeneracy.inparticular,thesupportofthedensity p(x t u t,x t 1 )istwodimensional, within a three-dimensional embedding pose space. The fact that all posterior poses are located on a two-dimensional manifold within the three-dimensional pose space is a direct consequence of the fact that we used only two noise variables, one for v and one for ω. Unfortunately, this degeneracy has important ramifications when applying Bayes filters for state estimation. In reality, any meaningful posterior distribution is of course not degenerate, and poses can be found within a three-dimensional space of variations in x, y,and θ.togeneralizeourmotionmodelaccordingly,wewillassume thattherobotperformsarotation ˆγwhenitarrivesatitsfinalpose. Thus, insteadofcomputing θ accordingto(5.13),wemodelthefinalorientation by θ = θ + ˆω t + ˆγ t with (5.15) ˆγ = ε α5v 2 +α 6ω 2 Here α5and α6areadditionalrobot-specificparametersthatdeterminethe variance of the additional rotational noise. Thus, the resulting motion model is as follows: (5.16) x y θ = x y θ + ˆvˆω sinθ + ˆvˆω sin(θ + ˆω t) ˆv ˆω Computationof p(x t u t, x t 1 ) cos θ ˆvˆω cos(θ + ˆω t) ˆω t + ˆγ t The algorithm motion_model_velocity in Table 5.1 implements the computationof p(x t u t,x t 1 )forgivenvaluesof x t 1 = (x y θ) T, u t = (v ω) T, and x t = (x y θ ) T.Thederivationofthisalgorithmissomewhatinvolved, as it effectively implements an inverse motion model. In particular, motion_model_velocitydeterminesmotionparameters û t = (ˆv ˆω) T fromthe

9 130 5 RobotMotion (5.17) (5.18) (5.19) (5.20) poses x t 1 and x t,alongwithanappropriatefinalrotation ˆγ.Ourderivation makesitobviousastowhyafinalrotationisneeded:foralmostallvalues of x t 1, u t,and x t,themotionprobabilitywouldsimplybezerowithout allowing for a final rotation. Letuscalculatetheprobability p(x t u t,x t 1 )ofcontrolaction u t = (v ω) T carryingtherobotfromthepose x t 1 = (x y θ) T tothepose x t = (x y θ ) T within ttimeunits.todoso,wewillfirstdeterminethecontrol û = (ˆv ˆω) T requiredtocarrytherobotfrom x t 1 toposition (x y ),regardlessofthe robot s final orientation. Subsequently, we will determine the final rotation ˆγnecessaryfortherobottoattaintheorientation θ.basedonthesecalculations,wecantheneasilycalculatethedesiredprobability p(x t u t,x t 1 ). Thereadermayrecallthatourmodelassumesthattherobottravelswith a fixed velocity during t, resulting in a circular trajectory. For a robot that movedfrom x t 1 = (x y θ) T to x t = (x y ) T,thecenterofthecircleis definedas (x y ) T andgivenby ( x y ) = ( x y ) ( λ sin θ + λ cos θ ) = ( x+x 2 + µ(y y ) y+y 2 + µ(x x) forsomeunknown λ,µ R. Thefirstequalityistheresultofthefactthat the circle s center is orthogonal to the initial heading direction of the robot; the second is a straightforward constraint that the center of the circle lies on araythatliesonthehalf-waypointbetween (x y) T and (x y ) T andis orthogonal to the line between these coordinates. Usually, Equation(5.17) has a unique solution except in the degenerate caseof ω = 0,inwhichthecenterofthecircleliesatinfinity.Asthereader mightwanttoverify,thesolutionisgivenby µ = 1 2 and hence ( ) x = y (x x )cos θ + (y y )sin θ (y y )cos θ (x x )sin θ ( x+x y+y (x x ) cos θ+(y y ) sin θ (y y ) cos θ (x x ) sin θ (y y ) (x x ) cos θ+(y y ) sin θ (y y ) cos θ (x x ) sin θ (x x) TheradiusofthecircleisnowgivenbytheEuclideandistance r = (x x ) 2 + (y y ) 2 = (x x ) 2 + (y y ) 2 ) ) Furthermore, we can now calculate the change of heading direction (5.21) θ = atan2(y y,x x ) atan2(y y,x x )

10 5.3 Velocity Motion Model 131 (5.22) (5.23) (5.24) Here atan2 is the common extension of the arcus tangens of y/x extended tothe R 2 (mostprogramminglanguagesprovideanimplementationofthis function): atan2(y, x) = atan(y/x) if x > 0 sign(y) (π atan( y/x )) if x < 0 0 if x = y = 0 sign(y) π/2 if x = 0,y 0 Since we assume that the robot follows a circular trajectory, the translational distancebetween x t and x t 1 alongthiscircleis dist = r θ From distand θ,itisnoweasytocomputethevelocities ˆvand ˆω: ( ) ( ) ˆv dist û t = = t 1 ˆω θ Therotationalvelocity ˆγneededtoachievethefinalheading θ oftherobotin (x y )within t canbedeterminedaccordingto(5.14)as: (5.25) ˆγ = t 1 (θ θ) ˆω Themotionerroristhedeviationof û t and ˆγfromthecommandedvelocity u t = (v ω) T and γ = 0,asdefinedinEquations(5.24)and(5.25). (5.26) (5.27) (5.28) v err = v ˆv ω err = ω ˆω γ err = ˆγ Under our error model, specified in Equations(5.10), and(5.15), these errors have the following probabilities: (5.29) (5.30) (5.31) ε α1v 2 +α 2ω 2 (v err) ε α3v 2 +α 4ω 2 (ω err) ε α5v 2 +α 6ω 2 (γ err) where ε b 2 denotesazero-meanerrorvariablewith variance b 2,asbefore. Since we assume independence between the different sources of error, the desiredprobability p(x t u t,x t 1 )istheproductoftheseindividualerrors: (5.32) p(x t u t,x t 1 ) = ε α1v 2 +α 2ω 2 (v err) ε α3v 2 +α 4ω 2 (ω err) ε α5v 2 +α 6ω 2 (γ err)

11 132 5 RobotMotion To see the correctness of the algorithm motion_model_velocity in Table 5.1, the reader may notice that this algorithm implements this expression. More specifically, lines 2 to 9 are equivalent to Equations(5.18),(5.19),(5.20),(5.21), (5.24), and(5.25). Line 10 implements(5.32), substituting the error terms as specified in Equations(5.29) to(5.31). Samplingfrom p(x u, x) The sampling algorithm sample_motion_model_velocity in Table 5.3 implementsaforwardmodel,asdiscussedearlierinthissection.lines5through7 correspond to Equation(5.16). The noisy values calculated in lines 2 through 4 correspond to Equations(5.10) and(5.15). The algorithm sample_normal_distribution in Table 5.4 implements a common approximation to sampling from a normal distribution. This approximation exploits the central limit theorem, which states that any average of non-degenerate random variables converges to a normal distribution. By averaging 12 uniform distributions, sample_normal_distribution generates values that are approximately normal distributed; though technically the resulting values lie always in [ 2b, 2b]. Finally, sample_triangular_distribution in Table 5.4 implements a sampler for triangular distributions. 5.4 Odometry Motion Model The velocity motion model discussed thus far uses the robot s velocity to compute posteriors over poses. Alternatively, one might want to use the odometry measurements as the basis for calculating the robot s motion over time. Odometry is commonly obtained by integrating wheel encoder information; most commercial robots make such integrated pose estimation availableinperiodictimeintervals(e.g.,everytenthofasecond). Thisleadsto a second motion model discussed in this chapter, the odometry motion model. The odometry motion model uses odometry measurements in lieu of controls. Practical experience suggests that odometry, while still erroneous, is usually more accurate than velocity. Both suffer from drift and slippage, but velocity additionally suffers from the mismatch between the actual motion controllers and its(crude) mathematical model. However, odometry is only available in retrospect, after the robot moved. This poses no problem for fil-

12 5.4 Odometry Motion Model 133 δ rot2 δ trans δ rot1 Figure5.7 Odometrymodel:Therobotmotioninthetimeinterval (t 1, t]isapproximatedbyarotation δ rot1,followedbyatranslation δ transandasecondrotation δ rot2.theturnsandtranslationsarenoisy. ter algorithms, such as the localization and mapping algorithms discussed in later chapters. But it makes this information unusable for accurate motion planning and control Closed Form Calculation Technically, odometric information are sensor measurements, not controls. To model odometry as measurements, the resulting Bayes filter would have to include the actual velocity as state variables which increases the dimensionofthestatespace.tokeepthestatespacesmall,itisthereforecommon toconsiderodometrydataasifitwerecontrolsignals. Inthissection,we will treat odometry measurements just like controls. The resulting model is at the core of many of today s best probabilistic robot systems. Letusdefinetheformatofourcontrolinformation.Attime t,thecorrect poseoftherobotismodeledbytherandomvariable x t.therobotodometryestimatesthispose;however,duetodriftandslippagethereisnofixed coordinate transformation between the coordinates used by the robot s internal odometry and the physical world coordinates. In fact, knowing this transformation would solve the robot localization problem! The odometry model uses the relative motion information, as measured by the robot s internal odometry. More specifically, in the time interval (t 1, t], therobotadvancesfromapose x t 1 topose x t.theodometryreportsback tousarelatedadvancefrom x t 1 = ( x ȳ θ) T to x t = ( x ȳ θ ) T.Herethe

13 134 5 RobotMotion 1: Algorithmmotion_model_odometry(x t,u t,x t 1 ): 2: δ rot1 = atan2(ȳ ȳ, x x) θ 3: δ trans = ( x x ) 2 + (ȳ ȳ ) 2 4: δ rot2 = θ θ δ rot1 5: ˆδrot1 = atan2(y y,x x) θ 6: ˆδtrans = (x x ) 2 + (y y ) 2 7: ˆδrot2 = θ θ ˆδ rot1 8: p 1 = prob(δ rot1 ˆδ rot1,α 1 ˆδ2 rot1 + α 2 ˆδ2 trans ) 9: p 2 = prob(δ trans ˆδ trans,α 3 ˆδ2 trans + α 4 ˆδ2 rot1 + α 4 ˆδ2 rot2 ) 10: p 3 = prob(δ rot2 ˆδ rot2,α 1 ˆδ2 rot2 + α 2 ˆδ2 trans ) 11: return p 1 p 2 p 3 Table5.5 Algorithmforcomputing p(x t u t, x t 1)basedonodometryinformation. Herethecontrol u tisgivenby ( x t 1 x t) T,with x t 1 = ( x ȳ θ)and x t = ( x ȳ θ ). (5.33) bar indicates that these are odometry measurements embedded in a robotinternal coordinate whose relation to the global world coordinates is unknown. The key insight for utilizing this information in state estimation is thattherelativedifferencebetween x t 1 and x t,underanappropriatedefinitionoftheterm difference, isagoodestimatorforthedifferenceofthe trueposes x t 1 and x t.themotioninformation u t is,thus,givenbythepair ( ) xt 1 u t = x t Toextractrelativeodometry, u t istransformedintoasequenceofthreesteps: a rotation, followed by a straight line motion(translation), and another rotation. Figure 5.7 illustrates this decomposition: the initial turn is called δ rot1,thetranslation δ trans,andthesecondrotation δ rot2. Asthereader easilyverifies,eachpairofpositions ( s s )hasauniqueparametervector

14 5.4 Odometry Motion Model 135 (a) (b) (c) Figure 5.8 The odometry motion model, for different noise parameter settings. (δ rot1 δ trans δ rot2 ) T,andtheseparametersaresufficienttoreconstructthe relativemotionbetween sand s.thus, δ rot1,δ trans,δ rot2 formtogetherasufficient statistics of the relative motion encoded by the odometry. The probabilistic motion model assumes that these three parameters are corrupted by independent noise. The reader may note that odometry motion uses one more parameter than the velocity vector defined in the previous section,forwhichreasonwewillnotfacethesamedegeneracythatledto the definition of a final rotation. Before delving into mathematical detail, let us state the basic algorithm for calculating this density in closed form. Table 5.5 depicts the algorithm forcomputing p(x t u t,x t 1 )fromodometry.thisalgorithmacceptsasan inputaninitialpose x t 1,apairofposes u t = ( x t 1 x t ) T obtainedfromthe robot sodometry,andahypothesizedfinalpose x t.itoutputsthenumerical probability p(x t u t,x t 1 ). Lines 2 to 4 in Table 5.5 recover relative motion parameters (δ rot1 δ trans δ rot2 ) T fromtheodometryreadings. Asbefore, theyimplement an inverse motion model. The corresponding relative motion parameters (ˆδ rot1 ˆδtrans ˆδrot2 ) T forthegivenposes x t 1 and x t arecalculatedinlines5through7ofthisalgorithm. Lines8to10computethe error probabilities for the individual motion parameters. As above, the functionprob(a, b 2 )implementsanerrordistributionover awithzero meanand variance b 2.Heretheimplementermustobservethatallangular differencesmustliein [ π,π].hencetheoutcomeof δ rot2 δ rot2 hastobe truncated correspondingly a common error that tends to be difficult to debug. Finally, line 11 returns the combined error probability, obtained by multiplyingtheindividualerrorprobabilities p 1, p 2,and p 3. Thislaststep

15 136 5 RobotMotion 1: Algorithmsample_motion_model_odometry(u t,x t 1 ): 2: δ rot1 = atan2(ȳ ȳ, x x) θ 3: δ trans = ( x x ) 2 + (ȳ ȳ ) 2 4: δ rot2 = θ θ δ rot1 5: ˆδrot1 = δ rot1 sample(α 1 δ 2 rot1 + α 2 δ 2 trans ) 6: ˆδtrans = δ trans sample(α 3 δ 2 trans + α 4 δ 2 rot1 + α 4 δ 2 rot2 ) 7: ˆδrot2 = δ rot2 sample(α 1 δ 2 rot2 + α 2 δ 2 trans ) 8: x = x + ˆδ trans cos(θ + ˆδ rot1 ) 9: y = y + ˆδ trans sin(θ + ˆδ rot1 ) 10: θ = θ + ˆδ rot1 + ˆδ rot2 11: return x t = (x,y,θ ) T Table5.6 Algorithmforsamplingfrom p(x t u t, x t 1)basedonodometryinformation.Heretheposeattime tisrepresentedby x t 1 = (x y θ) T.Thecontrolisadifferentiablesetoftwoposeestimatesobtainedbytherobot sodometer, u t = ( x t 1 x t) T, with x t 1 = ( x ȳ θ)and x t = ( x ȳ θ ). assumes independence between the different error sources. The variables α 1 through α 4 arerobot-specificparametersthatspecifythenoiseinrobot motion. Figure 5.8 shows examples of our odometry motion model for different valuesoftheerrorparameters α 1 to α 4.ThedistributioninFigure5.8aisa typical one, whereas the ones shown in Figures 5.8b and 5.8c indicate unusually large translational and rotational errors, respectively. The reader maywanttocarefullycomparethesediagramswiththoseinfigure5.3on page 122. The smaller the time between two consecutive measurements, the more similar those different motion models. Thus, if the belief is updated frequently e.g., every tenth of a second for a conventional indoor robot, the difference between these motion models is not very significant.

16 5.4 Odometry Motion Model 137 (a) (b) (c) Figure 5.9 Sampling from the odometry motion model, using the same parameters as in Figure 5.8. Each diagram shows 500 samples Sampling Algorithm Ifparticlefiltersareusedforlocalization,wewouldalsoliketohaveanalgorithmforsamplingfrom p(x t u t,x t 1 ).Recallthatparticlefilters(Chapter4.3)requiresamplesof p(x t u t,x t 1 ),ratherthanaclosed-formexpressionforcomputing p(x t u t,x t 1 )forany x t 1, u t,and x t. Thealgorithm sample_motion_model_odometry, shown in Table 5.6, implements the samplingapproach.itacceptsaninitialpose x t 1 andanodometryreading u t asinput,andoutputsarandom x t distributedaccordingto p(x t u t,x t 1 ). Itdiffersfromthepreviousalgorithminthatitrandomlyguessesapose x t (lines5-10),insteadofcomputingtheprobabilityofagiven x t. Asbefore, the sampling algorithm sample_motion_model_odometry is somewhat easier to implement than the closed-form algorithm motion_model_odometry, since it side-steps the need for an inverse model. Figure 5.9 shows examples of sample sets generated by sample_motion_model_odometry, using the same parameters as in the model shown in Figure 5.8. Figure 5.10 illustrates the motion model in action by superimposing sample sets from multiple time steps. This data has been generated using the motion update equations of the algorithm particle_filter (Table 4.3), assuming the robot s odometry follows the path indicated by the solid line. The figure illustrates how the uncertainty grows as the robot moves. The samples are spread across an increasingly large space Mathematical Derivation of the Odometry Motion Model The derivation of the algorithms is relatively straightforward, and once again may be skipped at first reading. To derive a probabilistic motion model using

17 138 5 RobotMotion Start location 10 meters Figure 5.10 Sampling approximation of the position belief for a non-sensing robot. The solid line displays the actions, and the samples represent the robot s belief at different points in time. odometry, we recall that the relative difference between any two poses is represented by a concatenation of three basic motions: a rotation, a straight-line motion(translation), and another rotation. The following equations show howtocalculatethevaluesofthetworotationsandthetranslationfromthe odometryreading u t = ( x t 1 x t ) T,with x t 1 = ( x ȳ θ)and x t = ( x ȳ θ ): (5.34) (5.35) (5.36) δ rot1 = atan2(ȳ ȳ, x x) θ δ trans = ( x x ) 2 + (ȳ ȳ ) 2 δ rot2 = θ θ δ rot1 Tomodelthemotionerror,weassumethatthe true valuesoftherotation and translation are obtained from the measured ones by subtracting inde-

18 5.4 Odometry Motion Model 139 pendentnoise ε b 2 withzeromeanand variance b 2 : (5.37) (5.38) (5.39) (5.40) ˆδ rot1 = δ rot1 ε α1δ 2 rot1 +α2δ2 trans ˆδ trans = δ trans ε α3 δ 2 trans +α4 δ2 rot1 +α4 δ2 rot2 ˆδ rot2 = δ rot2 ε α1δ 2 rot2 +α2δ2 trans As in the previous section, ε b 2 is a zero-mean noise variable with variance b 2. Theparameters α 1 to α 4 arerobot-specificerrorparameters, which specify the error accrued with motion. Consequently,thetrueposition, x t,isobtainedfrom x t 1 byaninitialrotationwithangle ˆδ rot1,followedbyatranslationwithdistance ˆδ trans,followed byanotherrotationwithangle ˆδ rot2.thus, x y θ = x y θ + ˆδ trans cos(θ + ˆδ rot1 ) ˆδ trans sin(θ + ˆδ rot1 ) ˆδ rot1 + ˆδ rot2 Notice that algorithm sample_motion_model_odometry implements Equations(5.34) through(5.40). The algorithm motion_model_odometry is obtained by noticing that lines 5-7computethemotionparameters ˆδ rot1, ˆδ trans,and ˆδ rot2 forthehypothesizedpose x t,relativetotheinitialpose x t 1.Thedifferenceofboth, (5.41) (5.42) (5.43) (5.44) (5.45) (5.46) δ rot1 ˆδ rot1 δ trans ˆδ trans δ rot2 ˆδ rot2 istheerrorinodometry,assumingofcoursethat x t isthetruefinalpose.the error model(5.37) to(5.39) implies that the probability of these errors is given by p 1 = ε α1δ 2 rot1 +α2δ2 trans (δ rot1 ˆδ rot1 ) p 2 = ε α3 δ 2 trans +α4 δ2 rot1 +α4 δ2 rot2 (δ trans ˆδ trans ) p 3 = ε α1δ 2 rot2 +α2δ2 trans (δ rot2 ˆδ rot2 ) with the distributions ε defined as above. These probabilities are computed in lines 8-10 of our algorithm motion_model_odometry, and since the errors areassumedtobeindependent,thejointerrorprobabilityistheproduct p 1 p 2 p 3 (c.f.,line11).