## I Introduction

Models are always abstractions in that they capture some pertinent aspects of the system under consideration whereas they neglect others. But models only have value inasmuch as they allow for valid predictions or as generators of design strategies. For example, in a significant portion of the many recent, multi-agent robotics algorithms for achieving coordinated objectives, single-integrator models are employed (e.g., [1, 2, 3, 4]). Arguably, such simple models have enabled complex control strategies to be developed, yet, at the end of the day, they have to be deployed on actual physical robots. This paper formally investigates how to strike a balance between performance and maneuverability when mapping single-integrator controllers onto differential-drive mobile robots.

Due to the single-integrator model’s prevalence as a design tool, a number of methods have been developed for mapping from single-integrator models to more complex, non-holonomic models. For example, the authors of [5] achieve a map from single integrator to unicycle by leveraging a control structure introduced in [6]. However, this map does not come with formal guarantees about the degree to which the unicycle system approximates the single-integrator system. One effective solution to this problem is to utilize a so-called Near-Identity Diffeomorphism (NID) between single-integrator and unicycle systems, as in [7], [8], where the basic idea is to perturb the original system ever-so-slightly (the near-identity part) and then show that there exists a diffeomorphism between a lower-dimensional version of the perturbed system’s dynamics and the single-integrator dynamics. As the size of the perturbation is given as a design parameter, a bound on how far the original system may deviate from the single-integrator system follows automatically.

A concept similar to NIDs from single-integrator to unicycle dynamics appears in the literature in different formats. For example, [9] utilizes this technique from a kinematics viewpoint to stabilize a differential-drive-like system. This "look-ahead" technique also arises in feedback linearization methods 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 that the NID incurs an abstraction cost, in terms of precision and maneuverability, that is based on the physical geometry of the differential-drive robots; in particular, the precision cost focuses on increasing the degree to which the single-integrator system matches the unicycle-modeled system, and the maneuverability cost utilizes physical properties of the differential-drive systems to limit the maneuverability requirements imposed by the transformation. By striking a balance between these two costs, a one-parameter family of abstractions arises. However, the maneuverability cost shows that only selecting the parameter once over the course of an experiment may cause a loss of precision.

A potential solution to this issue is to repeatedly optimize the parameter based on the system’s model and a suitable cost metric. Model Predictive Control (MPC) represents one such method. In particular, MPC approaches solve an optimal control problem over a time interval, utilize a portion of the controller, and re-solve the problem over the next time interval, effectively producing a state- and time-based controller. The authors of [12, 13] produce such a Parametric Model Predictive Control (PMPC) formulation. However, this formulation does not permit the cost metric to influence the time interval, which has practical performance implications. Using the formulated precision and maneuverability costs, this work formulates an appropriate PMPC cost metric and extends the work in [12, 13] to integrate a sampling horizon cost directly into the PMPC program.

This paper is organized as follows: Sec. II presents the system of interest and introduces the inherent trade-off contained in the NID. Sec. III discusses the PMPC formulation. Sec. IV formulates the cost functions that allow a balanced selection of the NID’s parameters, with respect to the generated cost functions. To demonstrate and verify the main results of this work, Sec. V shows data from simulations and physical experiments, with Sec. VI concluding the paper.

## Ii From Unicycles to Single Integrators

This article uses the following mathematical notation. The expression is the usual Euclidean norm. The symbol represents the partial derivative of the function with respect to the variable , assuming the convention that . The symbol refers to the real numbers that are greater than or equal to zero.

As the focus of the paper is effective abstractions for controlling differential-drive robots, this section establishes the Near-Identity Diffeomorphism (NID) that provides a relationship between single-integrator and unicycle models. That is, systems whose pose is given by planar positions and orientations , with the full state given by . The associated unicycle dynamics are given by (dropping the dependence on time )

(1) |

where the control inputs are the linear and rotational velocities, respectively,

is a zero-vector of the appropriate dimension, and

(2) |

Letting

(3) |

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 developments in [7]. Let be given by

(4) |

where is a constant. The map is, in fact, the NID, as defined in [7]. Geometrically, the point is simply given by a point at a distance directly in front of the unicycle with pose .

Now, assume that the dynamics of are given by a controller

(5) |

where is continuously differentiable, and compare this system to the time-derivative of (4), which yields

(6) |

Note that the NID maps from three degrees of freedom to two degrees of freedom. As a consequence, the resulting unicycle controller cannot explicitly affect the orientation

of the unicycle model.By [7], is invertible, yielding a relationship between and . Consequently, (6) allows the transformation of linear, 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 over the PMPC time intervals (i.e., is a constant value).

The unicycle model in (1) is not directly realizable on a differential-drive mobile robot. However, the relationship between the control inputs to the unicycle model and the differential-drive model is given by

(7) |

where and are the right and left wheel velocities, respectively. The wheel radius and base length encode the geometric properties of the robot.

In the discussion above, the parameter (i.e., the distance off the wheel axis to the new point) is not canonical. Moreover, it plays an important role since

(8) |

The above equation seems to indicate that one should simply choose to be as small as possible. However, the following sections show that small values of induce high maneuverability costs.

In order to strike a balance between precision and maneuverability, we will, for the remainder of this paper, assume that the control input to the unicycle model is given by

(9) |

where is the control input supplied by a single-integrator algorithm. Sec. IV contains the further investigation of the effects of the parameter on the precision and maneuverability implications of the transformation in (4).

## Iii A Parametric MPC Formulation

Having introduced the system of interest, this section contains a derivation of a Parametric Model Predictive Control (PMPC) method with a variable sampling interval for general, nonlinear systems. Later, Sec. V utilizes a specific case of these results. In general, MPC methods solve an optimal control problem over a time interval and use only a portion of the obtained controller (for a small amount of time) before resolving the problem, producing a time- and state-based controller. In this case, PMPC optimizes the parameters of a system. That is, this method finds the optimal, constant parameters of a system, rather than a time-varying control input, over a time interval. For clarity, this section specifies dependencies on time . Let

(10) |

where , , and is continuously differentiable in , measurable in . The program

(11) | |||

(12) | |||

(13) |

expresses the PMPC problem of interest, where is continuously differentiable in and . Note that, in this case, both and are decision variables determined by the PMPC program.

### Iii-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, the derivation proceeds by calculus of variations.

###### Proposition 1.

The augmented cost derivatives , are

where the augmented cost (i.e., augmented with the dynamics constraint) is given by

(14) |

###### Proof.

The proof proceeds by calculus of variations. Perturb and as and , where , . The perturbed augmented cost is

(15) |

Performing a Taylor expansion yields that

(16) |

The proof now proceeds with multiple steps. First, the application of integration by parts to the quantity . Second, the subtraction of the costs . Note that, to subtract the costs properly, the integral in must be broken up into two intervals: and . Furthermore, the costate assumes the usual definition: with the boundary condition . Applying the mean value theorem and taking the limit as shows that

(17) |

which is linear in and , and provides the final expressions

(18) | |||

(19) |

completing the proof. ∎

Interestingly, both of the usual conditions for free parameters and final time still hold, and the first-order, necessary optimality conditions for candidate solutions and are that

(20) |

Furthermore, this formulation becomes amenable to solution by numerical methods for the optimal parameters and . In such cases, the expression for can also be expressed as a costate-like variable with dynamics

(21) | |||

(22) |

where is defined as

(23) |

In this case, the necessary optimality condition is that

(24) |

### Iii-B Numerical Methods

The above expressions allow for applications of typical gradient descent methods. Many such methods could apply, and this article presents one simple method in Alg. 1. Note that this algorithm procures the decision variables over one sampling interval . In practice, one typically applies this algorithm repeatedly.

For example, the experiments in Sec. V-C consecutively apply this algorithm to solve the PMPC problem.

## Iv Precision vs. Maneuverability

As already noted in Sec. II, the parameter is a design parameter. This section discusses the importance and effects of selecting and proposes precision and maneuverability costs that elucidate the selection of this parameter and its impact on the differential-drive system. These derivations influence the PMPC cost metric in Sec. V and, for comparison, an optimal, static parameterization.

### Iv-a Precision Cost

Seeking to select , we initially present a cost that incorporates the degree to which the transformed system in (4) represents the original system over an arbitrary time duration . As such, we model the precision cost by the averaged tracking error

(25) |

It immediately follows from (8) that can be directly written as a function of , given by

(26) | ||||

(27) |

This immediate result states that the smaller is, the better the unicycle model tracks the single-integrator model.

### Iv-B Maneuverability Cost

In this section, we derive a geometrically-influenced maneuverability cost that models the degree to which the selection of influences the maneuverability requirements of the unicycle-modeled system, with respect to the map defined in (4). That is, we wish to elucidate how the parameter affects the expressions for the differential-drive agent’s forward velocity, wheel difference, and exerted control effort.

To this end, we utilize the differential-drive model in (7). Initially, note that the magnitude of the wheel-velocity difference represents a measure of the complexity of a maneuver that the differential-drive system performs. Using this definition as guidance, we state the following proposition.

###### Proposition 2.

Given that the control-input magnitude is upper-bounded by , the magnitude of the wheel-velocity difference, , is upper bounded by

(28) |

###### Proof.

Thus, Prop. 2 yields an upper bound on the magnitude of the wheel-velocity difference

(38) |

Prop. 3 shows a similar result for the forward velocity of the differential-drive agent.

###### Proposition 3.

Given that the control-input magnitude is upper-bounded by , the magnitude of the forward velocity, , is upper bounded by

(39) |

###### Proof.

So Prop. 3 reveals that the forward velocity of the differential-drive system remains independent of the selection of the parameter .

To elucidate an appropriate maneuverability cost in terms of , define the average control effort exerted by the differential-drive system over an arbitrary time duration as

(49) |

Directly applying Props. 2,3 reveals that the above expression is bounded above by

(50) |

The expression in (50) demonstrates an interesting quality of the system. As grows large, the forward velocity dominates the control effort exerted by the differential-drive system. However, if becomes small, then the choice of affects the potentially exerted control effort.

Thus, (50) reveals how affects the maneuverability requirements imposed by the abstraction. The fact that we always pay the forward-velocity price, regardless of the selection of , naturally excludes the forward velocity from the soon-to-be-formulated cost, because any selection of results in the same cost bound; but the choice of directly affects the cost associated with the wheel difference. Accordingly, the wheel difference must play a role in the final PMPC cost metric. With this conclusion in mind, we define the static maneuverability cost as

(51) |

which the static parameterization in the following section utilizes.

### Iv-C An Optimal, One-Time Selection

Sec. V utilizes the results in Sec. IV to formulate an appropriate cost metric for a PMPC program. To have a baseline comparison, this section formulates an optimal, one-time selection for the parameter . That is, the selection occurs once over the experiment’s duration. This selection should strike a balance between precision and maneuverability. Eqns. (27) and (51) represent each of these facets, respectively, and introduce an inherent trade-off in selecting . Making smaller directly reduces the cost in (27). However, consider the relationship in (51); as decreases, the differential-drive system accumulates a higher maneuverability cost.

As such, the convex combination of (25) and (51) yields a precision and maneuverability cost in terms of as

(52) |

where . Now, we seek the optimal such that (52) is minimized. That is,

###### Proposition 4.

The optimal is given by

(54) |

###### Proof.

We have that

(55) | ||||

(56) |

Setting this equation equal to zero directly yields the minimizer

(57) |

∎

### Iv-D PMPC Cost

This section formulates a PMPC cost based on the analysis in Sec. IV. To increase precision, the parameter must be minimized. However, (51) in Sec. IV-B indicates that the wheel velocity difference must be managed. Thus, precision and maneuverability are balanced with the cost

(58) | |||

(59) |

where .

With this cost metric, the PMPC program becomes

(60) | |||

(61) | |||

(62) | |||

(63) |

Note that the sampling cost and single-integrator control input have yet to be specified.

## V Numerical Results

To demonstrate the findings in Sec. IV, we conduct two separate tests: in simulation and on real hardware. The simulation portion shows the effects of a one-time parameter selection on the angular velocity versus the PMPC method. The experimental section contains the same implementation on a real, physical system: the Robotarium (www.robotarium.org). In particular, the experimental results highlight the practical differences between using a PMPC approach and a one-time selection.

### V-a Experiment Setup

This section proposes cost functions based on the results in Sec. IV and expresses the PMPC problem to be solved in simulation and on the Robotarium. Furthermore, this section also statically parameterizes the NID to provide a baseline comparison to the PMPC strategy. In this case, the particular setup involves a mobile robot tracking an ellipsoidal reference signal

(64) |

For a single-integrator system, the controller

(65) |

drives the single-integrator system to the reference exponentially quickly. Utilizing the transformation in Sec. IV yields the controller

(66) | ||||

(67) |

The GRITSbots of the Robotarium (shown in Fig. 1) have a wheel radius and base length of

(68) |

Furthermore, their maximum forward velocity is

(69) |

For this problem, we also consider the sampling cost

(70) |

which prevents the time horizon from becoming too small (i.e., the cost penalizes small time horizons).

Substituting these values into (62), the particular PMPC problem to be solved is

(71) | |||

(72) | |||

(73) | |||

(74) |

where and are the decision variables and . Both simulation and experimental results utilize Alg. 1 to solve for the optimal parameters and time horizon online with the step-size values

(75) |

Each experiment initially executes Alg. 1 to termination; then, steps are performed each iteration to ensure that the current values stays close to the locally optimal solution realized by Alg. 1. In particular, each iteration takes , which is the Robotarium’s sampling interval.

### V-B Simulation Results

This section contains the simulation results for the method described in Sec. V-A. In particular, the simulation compares the proposed PMPC method to the one-time selection process in Sec. IV-C, showing that the PMPC method can outperform the one-time selection. Fig. 3 shows the simulated angular velocities, and Fig. 2 shows the parameter and sampling horizon evolution. Both methods generate similar control inputs. However, Fig. 2 demonstrates that the PMPC method selects smaller parameter values, implying that this method provides better reference tracking.

Additionally, Fig. 2 also shows that the sampling horizon shortens and the parameter increases around the left and right portions of the ellipse, because these regions require sharper maneuvers and incur a higher maneuverability cost. Furthermore, the ellipsoidal reference trajectory induces the oscillations in Fig. 2. Overall, these simulated results show that the PMPC method can outperform a static parameterization.

### V-C Experimental Comparison

This section contains the experimental results of the implementation described in Sec. V-A. The physical experiments for this paper were deployed on the Robotarium and serve to highlight the efficacy and validity of applying the PMPC approach on a real system. Additionally, the experiments display the propriety of the maneuverability cost outlined in Sec. IV-B.

Figs. 5-6 display the angular velocity of the mobile robot, the sampling horizon, and the parameter selection, respectively. As in the simulated results, Fig. 5 shows that the static parameterization and PMPC method produce similar angular velocities, and Fig. 6 shows that the PMPC method is able to adaptively adjust the parameter and sampling horizon to handle variations in the reference signal.

Moreover, on a physical system, the PMPC method still adjusts the time horizon and parameter to account for maneuverability requirements. For example, on the left and right sides of the ellipse, the maneuverability cost rises, because the reference turns sharply. Thus, the parameter increases and the sampling horizon decreases. Over flat portions of the ellipse, the maneuverability cost decreases, permitting the extension of the time horizon and reduction of the parameter (i.e., better tracking). That is, reductions of the maneuverability cost permit decreasing the parameter , allowing the PMPC strategy to outperform the static parameterization. Furthermore, the decrease of the sampling horizon during high-maneuverability regions accelerates the execution of Alg. 1, which is useful in a practical sense.

## Vi Conclusion

This work presented a variable-sampling-horizon Parametric Model Predictive Control (PMPC) method that allows for optimal parameter and sampling horizon selection with the application of controlling differential-drive mobile robots. To formulate an appropriate cost for the PMPC strategy, this article discussed a class of Near-Identity Diffeomorphisms (NIDs) that allow the transformation of single-integrator algorithms to unicycle-modeled systems. Additionally, this work showed an inherent trade-off induced by the NID and formulated precision and maneuverability costs that allow for the optimal parameterization of the NID via a PMPC program. Furthermore, simulation and experimental results were produced that illustrated the validity of the proposed costs and the efficacy of the PMPC method.

## References

- [1] M. Ji and M. Egerstedt, “Distributed formation control while preserving connectedness,” in Proceedings of the IEEE CDC, pp. 5962–5967, Dec 2006.
- [2] A. Y. Yazicioğlu and M. Egerstedt, “Leader selection and network assembly for controllability of leader-follower networks,” in 2013 ACC, pp. 3802–3807, June 2013.
- [3] Y. Hong, J. Hu, and L. Gao, “Tracking control for multi-agent consensus 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-agent systems under fixed and switching topologies,” Systems & Control Letters, 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 via discontinuous 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 -tracking and -stabilization of first-order nonholonomic se(2) vehicles,” in Proceeding of the 2002 ACC, May 2002.
- [8] R. Olfati-Saber, “Exponential -tracking and -stabilization of second-order nonholonomic 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 function approach 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 dynamic feedback linearization: design, implementation, and experimental validation,” IEEE Transactions on Control Systems Technology, vol. 10, Nov 2002.
- [11] E. Yang, D. Gu, and H. Hu, “Nonsingular formation control of cooperative mobile robots via feedback linearization,” in 2005 IEEE/RSJ IROS, pp. 826–831, Aug 2005.
- [12] G. Droge and M. Egerstedt, “Adaptive look-ahead for robotic navigation in unknown environments,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1134–1139, Sept 2011.
- [13] G. Droge and M. Egerstedt, “Adaptive time horizon optimization in model predictive control,” in Proceedings of the 2011 American Control Conference, pp. 1843–1848, June 2011.