A Parametric MPC Approach to Balancing the Cost of Abstraction for Differential-Drive Mobile Robots
AA Parametric MPC Approach to Balancing the Cost of Abstraction forDifferential-Drive Mobile Robots
Paul Glotfelter and Magnus Egerstedt
Abstract — When designing control strategies for differential-drive mobile robots, one standard tool is the consideration of apoint at a fixed distance along a line orthogonal to the wheel axisinstead of the full pose of the vehicle. This abstraction supportsreplacing the non-holonomic, three-state unicycle model with amuch simpler two-state single-integrator model (i.e., a velocity-controlled point). Yet this transformation comes at a perfor-mance cost, through the robot’s precision and maneuverability.This work contains derivations for expressions of these precisionand maneuverability costs in terms of the transformation’sparameters. Furthermore, these costs show that only selectingthe parameter once over the course of an application maycause an undue loss of precision. Model Predictive Control(MPC) represents one such method to ameliorate this condition.However, MPC typically realizes a control signal, rather thana parameter, so this work also proposes a Parametric ModelPredictive Control (PMPC) method for parameter and samplinghorizon optimization. Experimental results are presented thatdemonstrate the effects of the parameterization on the deploy-ment of algorithms developed for the single-integrator modelon actual differential-drive mobile robots.
I. I
NTRODUCTION
Models are always abstractions in that they capture somepertinent aspects of the system under consideration whereasthey neglect others. But models only have value inasmuchas they allow for valid predictions or as generators of designstrategies. For example, in a significant portion of the manyrecent, multi-agent robotics algorithms for achieving coor-dinated objectives, single-integrator models are employed(e.g., [1], [2], [3], [4]). Arguably, such simple models haveenabled complex control strategies to be developed, yet, atthe end of the day, they have to be deployed on actualphysical robots. This paper formally investigates how tostrike a balance between performance and maneuverabilitywhen mapping single-integrator controllers onto differential-drive mobile robots.Due to the single-integrator model’s prevalence as adesign tool, a number of methods have been developedfor mapping from single-integrator models to more com-plex, non-holonomic models. For example, the authors of[5] achieve a map from single integrator to unicycle byleveraging a control structure introduced in [6]. However,this map does not come with formal guarantees about thedegree to which the unicycle system approximates the single-integrator system. One effective solution to this problem isto utilize a so-called Near-Identity Diffeomorphism (NID)
This research was sponsored by Grants No. 1531195 from the U.S.National Science Foundation.The authors are with the Institute for Robotics and Intelligent Ma-chines, Georgia Institute of Technology, Atlanta, GA 30332, USA,{paul.glotfelter,magnus}@gatech.edu. between single-integrator and unicycle systems, as in [7],[8], where the basic idea is to perturb the original systemever-so-slightly (the near-identity part) and then show thatthere exists a diffeomorphism between a lower-dimensionalversion of the perturbed system’s dynamics and the single-integrator dynamics. As the size of the perturbation is givenas a design parameter, a bound on how far the originalsystem may deviate from the single-integrator system followsautomatically.A concept similar to NIDs from single-integrator to uni-cycle dynamics appears in the literature in different formats.For example, [9] utilizes this technique from a kinematicsviewpoint to stabilize a differential-drive-like system. This"look-ahead" technique also arises in feedback linearizationmethods as a mathematical tool to ensure that the differential-drive system is feedback linearizable (e.g., [10], [11]).This paper utilizes the ideas in [7], [8] to show thatthe NID incurs an abstraction cost, in terms of precisionand maneuverability, that is based on the physical geometryof the differential-drive robots; in particular, the precisioncost focuses on increasing the degree to which the single-integrator system matches the unicycle-modeled system, andthe maneuverability cost utilizes physical properties of thedifferential-drive systems to limit the maneuverability re-quirements imposed by the transformation. By striking abalance between these two costs, a one-parameter family ofabstractions arises. However, the maneuverability cost showsthat only selecting the parameter once over the course of anexperiment may cause a loss of precision.A potential solution to this issue is to repeatedly optimizethe parameter based on the system’s model and a suitablecost metric. Model Predictive Control (MPC) represents onesuch method. In particular, MPC approaches solve an optimalcontrol problem over a time interval, utilize a portion of thecontroller, and re-solve the problem over the next time inter-val, effectively producing a state- and time-based controller.The authors of [12], [13] produce such a Parametric ModelPredictive Control (PMPC) formulation. However, this for-mulation does not permit the cost metric to influence thetime interval, which has practical performance implications.Using the formulated precision and maneuverability costs,this work formulates an appropriate PMPC cost metric andextends the work in [12], [13] to integrate a sampling horizoncost directly into the PMPC program.This paper is organized as follows: Sec. II presents thesystem of interest and introduces the inherent trade-off con-tained in the NID. Sec. III discusses the PMPC formulation.Sec. IV formulates the cost functions that allow a balanced a r X i v : . [ c s . R O ] F e b election of the NID’s parameters, with respect to the gen-erated cost functions. To demonstrate and verify the mainresults of this work, Sec. V shows data from simulations andphysical experiments, with Sec. VI concluding the paper.II. F ROM U NICYCLES TO S INGLE I NTEGRATORS
This article uses the following mathematical notation. Theexpression (cid:107) · (cid:107) is the usual Euclidean norm. The symbol ∂ x f ( x ) represents the partial derivative of the function f : R n → R m with respect to the variable x , assuming theconvention that ∂ x f ( x ) ∈ R m × n . The symbol R ≥ refersto the real numbers that are greater than or equal to zero.As the focus of the paper is effective abstractions forcontrolling differential-drive robots, this section establishesthe Near-Identity Diffeomorphism (NID) that provides arelationship between single-integrator and unicycle models.That is, systems whose pose is given by planar positions ¯ x = [ x x ] T and orientations θ , with the full state givenby x = (cid:2) ¯ x T θ (cid:3) T = [ x x θ ] T . The associated unicycledynamics are given by (dropping the dependence on time t ) ˙ x = (cid:20) R ( θ ) e (cid:21) (cid:20) vω (cid:21) , (1)where the control inputs v, ω ∈ R are the linear androtational velocities, respectively, is a zero-vector of theappropriate dimension, and e = (cid:2) (cid:3) T , R ( θ ) = (cid:20) cos( θ ) − sin( θ )sin( θ ) cos( θ ) (cid:21) . Letting u x = (cid:2) v ω (cid:3) T be the collective control input to the unicycle-modeled agent,the objective becomes to turn this model into a single-integrator model. To this end, we here recall the develop-ments in [7]. Let x si ∈ R be given by x si = Φ( x, l ) = ¯ x + lR ( θ ) e , (2)where l ∈ (0 , ∞ ) is a constant. The map Φ( x, l ) is, in fact,the NID, as defined in [7]. Geometrically, the point x si issimply given by a point at a distance l directly in front ofthe unicycle with pose x .Now, assume that the dynamics of x si are given by acontroller ˙ x si = u si , where u si ∈ R is continuously differentiable, and comparethis system to the time-derivative of (2), which yields ˙ x si = u si = (cid:20) cos( θ ) − l sin( θ )sin( θ ) l cos( θ ) (cid:21) u x = R l ( θ ) u x . (3)Note that the NID maps from three degrees of freedom totwo degrees of freedom. As a consequence, the resultingunicycle controller cannot explicitly affect the orientation θ of the unicycle model.By [7], R l ( θ ) is invertible, yielding a relationship between u si and u x . Consequently, (3) allows the transformation oflinear, single-integrator algorithms into algorithms in terms of the non-linear, unicycle dynamics. Note that in this paper,which is different from [7], we let ˙ l = 0 over the PMPCtime intervals (i.e., l is a constant value).The unicycle model in (1) is not directly realizable ona differential-drive mobile robot. However, the relationshipbetween the control inputs to the unicycle model and thedifferential-drive model is given by v = r w ω r + ω l ) , ω = r w l w ( ω r − ω l ) , (4)where ω r and ω l are the right and left wheel velocities,respectively. The wheel radius r w and base length l w encodethe geometric properties of the robot.In the discussion above, the parameter l (i.e., the distanceoff the wheel axis to the new point) is not canonical.Moreover, it plays an important role since (cid:107) ¯ x − x si (cid:107) = l. (5)The above equation seems to indicate that one should simplychoose l ∈ (0 , ∞ ) to be as small as possible. However, thefollowing sections show that small values of l induce highmaneuverability costs.In order to strike a balance between precision and maneu-verability, we will, for the remainder of this paper, assumethat the control input to the unicycle model is given by u x = R l ( θ ) − u si , where u si is the control input supplied by a single-integratoralgorithm. Sec. IV contains the further investigation of theeffects of the parameter l on the precision and maneuver-ability implications of the transformation in (2).III. A P ARAMETRIC
MPC F
ORMULATION
Having introduced the system of interest, this sectioncontains a derivation of a Parametric Model Predictive Con-trol (PMPC) method with a variable sampling interval forgeneral, nonlinear systems. Later, Sec. V utilizes a specificcase of these results. In general, MPC methods solve anoptimal control problem over a time interval and use onlya portion of the obtained controller (for a small amountof time) before resolving the problem, producing a time-and state-based controller. In this case, PMPC optimizesthe parameters of a system. That is, this method finds theoptimal, constant parameters of a system, rather than a time-varying control input, over a time interval. For clarity, thissection specifies dependencies on time t . Let ˙ x ( t ) = f ( x ( t ) , p, t ) , x t = x ( t ) , where x ( t ) ∈ R n , p ∈ R m , and f ( · ) is continuouslydifferentiable in x , measurable in t . The program arg min p ∈ R m , ∆ t ∈ R ≥ J ( p, ∆ t ) = t +∆ t (cid:90) t L ( x ( s ) , p, s ) ds + C (∆ t ) s.t. ˙ x ( t ) = f ( x ( t ) , p, t ) x ( t ) = x t , xpresses the PMPC problem of interest, where L ( · ) iscontinuously differentiable in x and p . Note that, in thiscase, both ∆ t and p are decision variables determined bythe PMPC program. A. Optimality Conditions
This section contains the derivation of the necessary,first-order optimality conditions for the PMPC formulation,realizing gradients for the proposed cost. In particular, thederivation proceeds by calculus of variations.
Proposition 1.
The augmented cost derivatives ∂ p ˜ J ( p, ∆ t ) , ∂ ∆ t ˜ J ( p, ∆ t ) are ∂ p ˜ J ( p, ∆ t ) = t +∆ t (cid:90) t ∂ p L ( x ( s ) , p, s ) + λ ( s ) T ∂ p f ( x ( s ) , p, s ) ds∂ ∆ t ˜ J ( p, ∆ t ) = L ( x ( t + ∆ t ) , p, t + ∆ t ) + ∂ ∆ t C (∆ t ) , where the augmented cost ˜ J ( p, ∆ t ) (i.e., J ( p, ∆ t ) augmentedwith the dynamics constraint) is given by ˜ J ( p, ∆ t ) = t +∆ t (cid:90) t L ( x ( s ) , p, s )+ λ ( s ) T ( f ( x ( s ) , p, s ) − ˙ x ( s )) ds + C (∆ t ) . Proof.
The proof proceeds by calculus of variations. Perturb p and ∆ t as p (cid:55)→ p + (cid:15)γ and ∆ t (cid:55)→ ∆ t + (cid:15)τ , where γ ∈ R m , τ ∈ R . The perturbed augmented cost is ˜ J ( p + (cid:15)γ, ∆ t + (cid:15)τ ) = t +∆ t + (cid:15)τ (cid:90) t L ( x ( s )+ (cid:15)η ( s ) , p + (cid:15)γ, s ) λ ( s ) T ( f ( x ( s )+ (cid:15)η ( s ) , p + (cid:15)γ, s ) − ˙ x ( s ) − (cid:15) ˙ η ( s )) ds + C (∆ t + (cid:15)τ )+ o ( (cid:15) ) . Performing a Taylor expansion yields that ˜ J ( p + (cid:15)γ, ∆ t + (cid:15)τ ) = t +∆ t + (cid:15)τ (cid:90) t L ( x ( s ) , p, s )+ (cid:15)∂ x L ( x ( s ) , p, s ) η ( s )+ (cid:15)∂ p L ( x ( s ) , p, s ) γ + λ T ( f ( x ( s ) , p, s )+ (cid:15)∂ x f ( x ( s ) , p, s ) η ( s )+ (cid:15)∂ p f ( x ( s ) , p, s ) γ − ˙ x ( s ) − (cid:15) ˙ η ( s )) ds + C (∆ t ) + (cid:15)∂ ∆ t C (∆ t ) τ + o ( (cid:15) ) . The proof now proceeds with multiple steps. First, theapplication of integration by parts to the quantity λ ( t ) T ˙ η ( t ) .Second, the subtraction of the costs ˜ J ( p + (cid:15)γ, ∆ t + (cid:15)τ ) − ˜ J ( p, ∆ t ) . Note that, to subtract the costs properly, the inte-gral in ˜ J ( p + (cid:15)γ, t + (cid:15)τ ) must be broken up into two intervals: [ t, t + ∆ t ] and [ t + ∆ t, t + ∆ t + τ ] . Furthermore, the costateassumes the usual definition: ˙ λ ( t ) = − ∂ x L ( x ( t ) , p, t ) T − ∂ x f ( x ( t ) , p, t ) T λ ( t ) with the boundary condition λ ( t + ∆ t ) = 0 . Applying the mean value theorem and taking thelimit as (cid:15) → shows that lim (cid:15) → ˜ J ( p + (cid:15)γ, ∆ t + (cid:15)τ ) − ˜ J ( p, ∆ t ) (cid:15) = t +∆ t (cid:90) t ∂ p L ( x ( s ) , p, s )+ λ ( s ) T ∂ p f ( x ( s ) , p, s ) ds γ +[ ∂ ∆ t C (∆ t )+ L ( x ( t +∆ t ) , p, t +∆ t )] τ, which is linear in τ and γ , and provides the final expressions ∂ p ˜ J ( p, ∆ t ) = t +∆ t (cid:90) t ∂ p L ( x ( s ) , p, s )+ λ ( t ) T ∂ p f ( x ( s ) , p, s ) ds∂ ∆ t ˜ J ( p, ∆ t ) = ∂ ∆ t C (∆ t )+ L ( x ( t + ∆ t ) , p, t +∆ t ) , completing the proof.Interestingly, both of the usual conditions for free param-eters and final time still hold, and the first-order, necessaryoptimality conditions for candidate solutions p ∗ and ∆ t ∗ arethat ∂ p ˜ J ( p ∗ , ∆ t ∗ ) = 0 , ∂ ∆ t ˜ J ( p ∗ , ∆ t ∗ ) = 0 . Furthermore, this formulation becomes amenable to solu-tion by numerical methods for the optimal parameters p ∗ and ∆ t ∗ . In such cases, the expression for ∂ p ˜ J ( p, ∆ t ) can alsobe expressed as a costate-like variable ξ : [ t , t +∆ t ] → R m with dynamics ˙ ξ ( t ) = − ∂ p L ( x ( t ) , p, t ) T − ∂ p f ( x ( t ) , p, t ) T λ ( t ) ξ ( t + ∆ t ) = 0 , where ξ ( · ) is defined as ξ ( t ) = t +∆ t (cid:90) t ∂ p L ( x ( s ) , p, s ) T + ∂ p f ( x ( s ) , p, s ) T λ ( s ) ds. In this case, the necessary optimality condition is that ξ ( t ) = 0 . B. Numerical Methods
The above expressions allow for applications of typicalgradient descent methods. Many such methods could apply,and this article presents one simple method in Alg. 1.Note that this algorithm procures the decision variablesover one sampling interval [ t , t + ∆ t ] . In practice, onetypically applies this algorithm repeatedly. For example, theexperiments in Sec. V-C consecutively apply this algorithmto solve the PMPC problem.IV. P RECISION VS . M
ANEUVERABILITY
As already noted in Sec. II, the parameter l is a design pa-rameter. This section discusses the importance and effects ofselecting l and proposes precision and maneuverability coststhat elucidate the selection of this parameter and its impacton the differential-drive system. These derivations influencethe PMPC cost metric in Sec. V and, for comparison, anoptimal, static parameterization. lgorithm 1 Gradient Descent Algorithm for PMPC k ← p k ← initial guess ∆ t k ← initial guess while (cid:13)(cid:13)(cid:13) ∂ p ˜ J ( p k , ∆ t ) (cid:13)(cid:13)(cid:13) + (cid:13)(cid:13)(cid:13) ∂ ∆ t ˜ J ( p k , ∆ t ) (cid:13)(cid:13)(cid:13) > (cid:15) do Solve forward for x ( · ) from x t using p k and ∆ t k Solve backward for λ ( · ) , ξ ( · ) using x ( · ) Compute gradients ∂ p ˜ J ( p k , ∆ t ) and ∂ ∆ t ˜ J ( p k , ∆ t ) p k +1 ← p k − γ ∂ p ˜ J ( p k , ∆ t ) T ∆ t k +1 ← ∆ t k − γ ∂ ∆ t ˜ J ( p k , ∆ t ) k ← k + 1 A. Precision Cost
Seeking to select l , we initially present a cost that in-corporates the degree to which the transformed system in(2) represents the original system x si over an arbitrary timeduration T ≥ . As such, we model the precision cost bythe averaged tracking error D (¯ x, x si ) = 1 T (cid:90) T (cid:107) ¯ x − x si (cid:107) dt. (6)It immediately follows from (5) that D (¯ x, p ) can be directlywritten as a function of l , given by D (¯ x, x si ) = 1 T (cid:90) T (cid:107) ¯ x − x si (cid:107) dt = 1 T (cid:90) T l dt = l. (7)This immediate result states that the smaller l is, the betterthe unicycle model tracks the single-integrator model. B. Maneuverability Cost
In this section, we derive a geometrically-influenced ma-neuverability cost that models the degree to which theselection of l influences the maneuverability requirements ofthe unicycle-modeled system, with respect to the map definedin (2). That is, we wish to elucidate how the parameter l affects the expressions for the differential-drive agent’sforward velocity, wheel difference, and exerted control effort.To this end, we utilize the differential-drive model in(4). Initially, note that the magnitude of the wheel-velocitydifference | ω r − ω l | represents a measure of the complexityof a maneuver that the differential-drive system performs.Using this definition as guidance, we state the followingproposition. Proposition 2.
Given that the control-input magnitude (cid:107) u si (cid:107) is upper-bounded by ¯ v , the magnitude of the wheel-velocitydifference, | ω r − ω l | , is upper bounded by | ω r − ω l | ≤ l w ¯ vr w λ . Proof.
Let e = (cid:2) (cid:3) T , T − = (cid:34) l (cid:35) , ¯ u si = (cid:2) (cid:107) u si (cid:107) (cid:3) T and let θ si be the angle of the vector u si . From (3),(4) we canretrieve the magnitude of the difference in angular velocities, | ω r − ω l | , as | ω r − ω l | = (cid:12)(cid:12)(cid:12)(cid:12) l w r w ω (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) lr e T R l ( θ ) − u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) l w r w e T T − R ( − θ ) R ( θ si )¯ u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) l w r w e T T − R ( θ si − θ )¯ u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) l w r w (cid:20) l (cid:21) (cid:20) cos( θ si − θ ) − sin( θ si − θ )sin( θ si − θ ) cos( θ si − θ ) (cid:21) (cid:20) (cid:107) u si (cid:107) (cid:21)(cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) l w r w (cid:20) sin( θ si − θ ) l cos( θ si − θ ) l (cid:21) (cid:20) (cid:107) u si (cid:107) (cid:21)(cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) l w (cid:107) u si (cid:107) r w l sin( θ si − θ ) (cid:12)(cid:12)(cid:12)(cid:12) ≤ l w ¯ vr w l . Thus, Prop. 2 yields an upper bound on the magnitude ofthe wheel-velocity difference | ω r − ω l | ≤ l w ¯ vr w l . Prop. 3 shows a similar result for the forward velocity of thedifferential-drive agent.
Proposition 3.
Given that the control-input magnitude (cid:107) u si (cid:107) is upper-bounded by ¯ v , the magnitude of the forward velocity, | ω r + ω l | , is upper bounded by | ω r + ω l | ≤ vr w . Proof.
Let e = (cid:2) (cid:3) T and T − , ¯ u si , θ si be defined as in the proof of Prop. 2. Then, we have, through(4), that | ω r + ω l | = (cid:12)(cid:12)(cid:12)(cid:12) r w v (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) r w e T R l ( θ ) − u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) r w e T T − R ( − θ ) R ( θ si )¯ u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) r w [1 0] R ( θ si − θ )¯ u si (cid:12)(cid:12)(cid:12)(cid:12) = (cid:12)(cid:12)(cid:12)(cid:12) r w [cos( θ si − θ ) − sin( θ si − θ )] (cid:20) (cid:107) u si (cid:107) (cid:21)(cid:12)(cid:12)(cid:12)(cid:12) (cid:12)(cid:12)(cid:12)(cid:12) r w (cid:107) u si (cid:107) cos( θ si − θ ) (cid:12)(cid:12)(cid:12)(cid:12) ≤ vr w . So Prop. 3 reveals that the forward velocity of thedifferential-drive system remains independent of the selec-tion of the parameter l .To elucidate an appropriate maneuverability cost in termsof l , define the average control effort exerted by thedifferential-drive system over an arbitrary time duration T > = 0 as T (cid:90) T | ω r − ω l | + | ω r + ω l | dt. Directly applying Props. 2,3 reveals that the above expressionis bounded above by l w ¯ vr w l + 2¯ vr . (8)The expression in (8) demonstrates an interesting qual-ity of the system. As l grows large, the forward velocitydominates the control effort exerted by the differential-drivesystem. However, if l becomes small, then the choice of l affects the potentially exerted control effort.Thus, (8) reveals how l affects the maneuverability re-quirements imposed by the abstraction. The fact that wealways pay the forward-velocity price, regardless of the se-lection of l , naturally excludes the forward velocity from thesoon-to-be-formulated cost, because any selection of l resultsin the same cost bound; but the choice of l directly affectsthe cost associated with the wheel difference. Accordingly,the wheel difference must play a role in the final PMPC costmetric. With this conclusion in mind, we define the staticmaneuverability cost as D ( l ) = l w ¯ vr w l , (9)which the static parameterization in the following sectionutilizes. C. An Optimal, One-Time Selection
Sec. V utilizes the results in Sec. IV to formulate anappropriate cost metric for a PMPC program. To have a base-line comparison, this section formulates an optimal, one-timeselection for the parameter l . That is, the selection occursonce over the experiment’s duration. This selection shouldstrike a balance between precision and maneuverability.Eqns. (7) and (9) represent each of these facets, respectively,and introduce an inherent trade-off in selecting l . Making l smaller directly reduces the cost in (7). However, considerthe relationship in (9); as l decreases, the differential-drivesystem accumulates a higher maneuverability cost.As such, the convex combination of (6) and (9) yields aprecision and maneuverability cost in terms of l as D ( l ) = αD ( l ) + (1 − α ) D ( l )= αl + (1 − α ) l w ¯ vr w l , (10) where α ∈ (0 , . Now, we seek the optimal l such that (10)is minimized. That is, l ∗ = arg min l D ( l ) . (11)(11) leads to Prop. 4. Proposition 4.
The optimal l ∗ is given by l ∗ = (cid:114) − αα l w ¯ vr w . Proof.
We have that ∂∂l D ( l ) = α − (1 − α ) l w ¯ vr w l = α − (1 − α ) l w ¯ vr w l . Setting this equation equal to zero directly yields the mini-mizer l ∗ = (cid:114) − αα l w ¯ vr w . (12)Note that the above result utilizes (9), which is an upperbound on the wheel velocity difference. Thus, the PMPCmethod should outperform this static selection, a suspicionthat Sec. V investigates. D. PMPC Cost
This section formulates a PMPC cost based on the analysisin Sec. IV. To increase precision, the parameter l must beminimized. However, (9) in Sec. IV-B indicates that thewheel velocity difference must be managed. Thus, precisionand maneuverability are balanced with the cost L ( x, l, t ) = (1 − β )( ω r − ω l ) + βl = (1 − β )(( l w /r w ) e R l ( θ ) − ( u si )) + βl , where β ∈ (0 , .With this cost metric, the PMPC program becomes arg min l ∈ R , ∆ t ∈ R ≥ t +∆ t (cid:90) t (1 − β )(( l w /r w ) e R l ( θ ) − ( u si )) + βl + C (∆ t ) s.t. ˙ x = (cid:20) R ( θ ) e (cid:21) R l ( θ ) − u si (13) x ( t ) = x t , Note that the sampling cost C (∆ t ) and single-integratorcontrol input u si have yet to be specified.ig. 1: The GRITSbot, which is a small, differential-drivemobile robot used in the Robotarium. This figure displaysthe base length and wheel radius of the GRITSbots.V. N UMERICAL R ESULTS
To demonstrate the findings in Sec. IV, we conduct twoseparate tests: in simulation and on real hardware. Thesimulation portion shows the effects of a one-time parameterselection on the angular velocity versus the PMPC method.The experimental section contains the same implementa-tion on a real, physical system: the Robotarium ( ). In particular, the experimental resultshighlight the practical differences between using a PMPCapproach and a one-time selection.
A. Experiment Setup
This section proposes cost functions based on the resultsin Sec. IV and expresses the PMPC problem to be solvedin simulation and on the Robotarium. Furthermore, thissection also statically parameterizes the NID to provide abaseline comparison to the PMPC strategy. In this case,the particular setup involves a mobile robot tracking anellipsoidal reference signal
Time (s) P a r a m e t e r ( l ) Fig. 2: Parameter (left) and sampling horizon (right) fromPMPC simulation, which oscillate because of the ellipsoidalreference trajectory in (14). Due to the sharp maneuvers re-quired, the time horizon shortens and the parameter increaseson the left and right sides of the ellipse. On flatter regions,the PMPC reduces the parameter and increases the samplingtime. The zoomed portion displays the discrete nature of thePMPC solution.
Time (s) -0.3-0.2-0.100.1 A ngu l a r V e l o c i t y () StaticPMPC
Fig. 3: Angular velocity ( ω ) during the simulation. Thesimulation shows that the static selection (solid line) andPMPC method (dashed line) both generate similar angularvelocity values. r ( t ) = (cid:20) . / t )0 . / t ) (cid:21) . (14)For a single-integrator system, the controller u si = x si − r + ˙ r drives the single-integrator system to the reference exponen-tially quickly. Utilizing the transformation in Sec. IV yieldsthe controller u x = R l ( θ ) − ( r − x si + ˙ r )= R l ( θ ) − ( r − (¯ x + lR ( θ ) e ) + ˙ r ) . The GRITSbots of the Robotarium (shown in Fig. 1) have awheel radius and base length of r w = 0 . m, l w = 0 . m. Furthermore, their maximum forward velocity is ¯ v = 0 . m/s. For this problem, we also consider the sampling cost C (∆ t ) = 1∆ t , which prevents the time horizon from becoming too small(i.e., the cost penalizes small time horizons).Substituting these values into (13), the particular PMPCproblem to be solved is arg min l ∈ R , ∆ t ∈ R ≥ t +∆ t (cid:90) t ( β − l w /r w ) e R l ( θ ) − ( r − x si + ˙ r )) + βl ds + (1 / ∆ t ) s.t. ˙ x = (cid:20) R ( θ ) e (cid:21) R l ( θ ) − ( r − x si + ˙ r ) x ( t ) = x t , where l and ∆ t are the decision variables and β = 0 . .Both simulation and experimental results utilize Alg. 1 to =0.030937 t =3.3119 l =0.043572 t =3.2757 l =0.027048 t =3.4262 Fig. 4: Robot during the PMPC experiment. This figure shows that the parameter grows and sampling horizon shrinks whenthe robot must perform more complex maneuvers (i.e., on the left and right sides of the ellipse). Over the flatter portionsof the ellipse, the parameter increases and sampling horizon (solid line) reduces, allowing the robot to track the reference(solid circle) more closely.
Time (s) -1-0.500.51 A ngu l a r V e l o c i t y () StaticPMPC
Fig. 5: Angular velocity of robots for PMPC method (dashedline) versus static parameterization (solid line). In this case,both methods generate similar angular velocities, but thePMPC method produces better tracking.solve for the optimal parameters and time horizon onlinewith the step-size values γ = 0 . , γ = 0 . . Each experiment initially executes Alg. 1 to termination;then, steps are performed each iteration to ensure that thecurrent values stays close to the locally optimal solutionrealized by Alg. 1. In particular, each iteration takes . s ,which is the Robotarium’s sampling interval.For comparison, the one-time selection method stemsdirectly from the abstraction cost formulated in Sec. IV-C Time (s) P a r a m e t e r ( l ) Fig. 6: Parameter (left) and sampling horizon (right) fromPMPC experiment on the Robotarium. The PMPC programreduces the sampling horizon and increases the parameter tocope with the sharp maneuvers required at the left and rightsides of the ellipse. On flatter regions, the PMPC decreasesthe parameter and increases the time horizon, providingbetter reference tracking. The zoomed portion illustrates thediscrete nature of the PMPC solution. with α = 0 . . This assignment to α in (12) implies that l ∗ = 0 . . Note that this value of l ∗ is only for the one-time selection.The PMPC method induces different parameter values every . s . B. Simulation Results
This section contains the simulation results for the methoddescribed in Sec. V-A. In particular, the simulation comparesthe proposed PMPC method to the one-time selection processin Sec. IV-C, showing that the PMPC method can outperformthe one-time selection. Fig. 3 shows the simulated angularvelocities, and Fig. 2 shows the parameter and samplinghorizon evolution. Both methods generate similar controlinputs. However, Fig. 2 demonstrates that the PMPC methodselects smaller parameter values, implying that this methodprovides better reference tracking.Additionally, Fig. 2 also shows that the sampling horizonshortens and the parameter increases around the left andright portions of the ellipse, because these regions requiresharper maneuvers and incur a higher maneuverability cost.Furthermore, the ellipsoidal reference trajectory induces theoscillations in Fig. 2. Overall, these simulated results showthat the PMPC method can outperform a static parameteri-zation.
C. Experimental Comparison
This section contains the experimental results of the imple-mentation described in Sec. V-A. The physical experimentsfor this paper were deployed on the Robotarium and serveto highlight the efficacy and validity of applying the PMPCapproach on a real system. Additionally, the experimentsdisplay the propriety of the maneuverability cost outlinedin Sec. IV-B.Figs. 5-6 display the angular velocity of the mobile robot,the sampling horizon, and the parameter selection, respec-tively. As in the simulated results, Fig. 5 shows that the staticparameterization and PMPC method produce similar angularvelocities, and Fig. 6 shows that the PMPC method is ableto adaptively adjust the parameter and sampling horizon tohandle variations in the reference signal.oreover, on a physical system, the PMPC method stilladjusts the time horizon and parameter to account for ma-neuverability requirements. For example, on the left andright sides of the ellipse, the maneuverability cost rises,because the reference turns sharply. Thus, the parameterincreases and the sampling horizon decreases. Over flatportions of the ellipse, the maneuverability cost decreases,permitting the extension of the time horizon and reductionof the parameter (i.e., better tracking). That is, reductionsof the maneuverability cost permit decreasing the parameter l , allowing the PMPC strategy to outperform the staticparameterization. Furthermore, the decrease of the samplinghorizon during high-maneuverability regions accelerates theexecution of Alg. 1, which is useful in a practical sense.VI. C ONCLUSION
This work presented a variable-sampling-horizon Paramet-ric Model Predictive Control (PMPC) method that allows foroptimal parameter and sampling horizon selection with theapplication of controlling differential-drive mobile robots. Toformulate an appropriate cost for the PMPC strategy, thisarticle discussed a class of Near-Identity Diffeomorphisms(NIDs) that allow the transformation of single-integratoralgorithms to unicycle-modeled systems. Additionally, thiswork showed an inherent trade-off induced by the NID andformulated precision and maneuverability costs that allowfor the optimal parameterization of the NID via a PMPCprogram. Furthermore, simulation and experimental resultswere produced that illustrated the validity of the proposedcosts and the efficacy of the PMPC method.R
EFERENCES [1] M. Ji and M. Egerstedt, “Distributed formation control while preserv-ing connectedness,” in
Proceedings of the IEEE CDC , pp. 5962–5967,Dec 2006.[2] A. Y. Yazicio˘glu and M. Egerstedt, “Leader selection and networkassembly for controllability of leader-follower networks,” in , pp. 3802–3807, June 2013.[3] Y. Hong, J. Hu, and L. Gao, “Tracking control for multi-agentconsensus with an active leader and variable topology,”
Automatica ,vol. 42, no. 7, pp. 1177 – 1182, 2006.[4] W. Ni and D. Cheng, “Leader-following consensus of multi-agentsystems under fixed and switching topologies,”
Systems & ControlLetters , vol. 59, no. 3–4, pp. 209 – 217, 2010.[5] J. Cortes et al. , “Coverage control for mobile sensing networks:Variations on a theme,” in
Proceedings of the MCCA , July 2002.[6] A. Astolfi, “Exponential stabilization of a wheeled mobile robot viadiscontinuous control.,”
Journal of dynamic systems, measurement,and control , vol. 121, no. 1, pp. 121–126, 1999.[7] R. Olfati-Saber, “Near-identity diffeomorphisms and exponential (cid:15) -tracking and (cid:15) -stabilization of first-order nonholonomic se(2) vehi-cles,” in
Proceeding of the 2002 ACC , May 2002.[8] R. Olfati-Saber, “Exponential (cid:15) -tracking and (cid:15) -stabilization of second-order nonholonomic se (2) vehicles using dynamic state feedback,”in Proceedings of the 2002 ACC (IEEE Cat. No.CH37301) , vol. 5,pp. 3961–3967 vol.5, May 2002.[9] P. Ogren, M. Egerstedt, and X. Hu, “A control Lyapunov functionapproach to multi-agent coordination,” in
Decision and Control, 2001.Proceedings of the IEEE Conference on , vol. 2, 2001.[10] G. Oriolo, A. D. Luca, and M. Vendittelli, “Wmr control via dynamicfeedback linearization: design, implementation, and experimental val-idation,”
IEEE Transactions on Control Systems Technology , vol. 10,Nov 2002.[11] E. Yang, D. Gu, and H. Hu, “Nonsingular formation control of coop-erative mobile robots via feedback linearization,” in , pp. 826–831, Aug 2005.[12] G. Droge and M. Egerstedt, “Adaptive look-ahead for robotic nav-igation in unknown environments,” in , pp. 1134–1139, Sept2011.[13] G. Droge and M. Egerstedt, “Adaptive time horizon optimizationin model predictive control,” in